Main MRPT website > C++ reference for MRPT 1.5.6
CPointCloudFilterByDistance.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "maps-precomp.h" // Precomp header
11 
15 
16 using namespace mrpt::maps;
17 using namespace mrpt::utils;
18 
20  mrpt::maps::CPointsMap * pc, //!< [in,out] The input pointcloud, which will be modified upon return after filtering.
21  const mrpt::system::TTimeStamp pc_timestamp, //!< [in] The timestamp of the input pointcloud
22  const mrpt::poses::CPose3D & cur_pc_pose, //!< [in] If NULL, the PC is assumed to be given in global coordinates. Otherwise, it will be transformed from local coordinates to global using this transformation.
23  TExtraFilterParams * params //!< [in,out] additional in/out parameters
24 )
25 {
26  using namespace mrpt::poses;
27  using namespace mrpt::math;
28  using mrpt::math::square;
29 
30  MRPT_START;
31  ASSERT_(pc_timestamp!=INVALID_TIMESTAMP);
32  ASSERT_(pc != nullptr);
33 
34  CSimplePointsMapPtr original_pc = CSimplePointsMap::Create();
35  original_pc->copyFrom(*pc);
36 
37  // 1) Filter:
38  // ---------------------
39  const size_t N = pc->size();
40  std::vector<bool> deletion_mask;
41  deletion_mask.assign(N, false);
42  size_t del_count = 0;
43 
44  // get reference, previous PC:
45  ASSERT_(options.previous_keyframes >= 1);
46  bool can_do_filter = true;
47 
48  std::vector<FrameInfo*> prev_pc; // (options.previous_keyframes, nullptr);
49  {
50  auto it = m_last_frames.rbegin();
51  for (int i = 0; i < options.previous_keyframes && it!=m_last_frames.rend(); ++i, ++it)
52  {
53  prev_pc.push_back(&it->second);
54  }
55  }
56 
57  if (prev_pc.size() < static_cast<size_t>(options.previous_keyframes) ) {
58  can_do_filter = false;
59  }
60  else {
61  for (int i = 0; can_do_filter && i < options.previous_keyframes; ++i)
62  {
63  if (mrpt::system::timeDifference(m_last_frames.rbegin()->first, pc_timestamp) > options.too_old_seconds)
64  {
65  can_do_filter = false; // A required keyframe is too old
66  break;
67  }
68  }
69  }
70 
71  if (can_do_filter)
72  {
73  // Reference poses of each PC:
74  // Previous: prev_pc.pose
76  for (int k = 0; k < options.previous_keyframes; ++k) {
77  const CPose3D rel_pose = cur_pc_pose - prev_pc[k]->pose;
78  rel_poses.push_back(rel_pose);
79  }
80 
81  // The idea is that we can now find matches between pt{i} in time_{k}, composed with rel_pose
82  // with the local points in time_{k-1}.
83 
84  std::vector<TPoint3D> pt_km1(options.previous_keyframes);
85 
86  for (size_t i = 0; i < N; i++)
87  {
88  // get i-th point in time=k:
89  TPoint3Df ptf_k;
90  pc->getPointFast(i, ptf_k.x, ptf_k.y, ptf_k.z);
91  const TPoint3D pt_k = TPoint3D(ptf_k);
92 
93  // Point, referred to time=k-1 frame of reference
94  for (int k = 0; k < options.previous_keyframes; ++k)
95  rel_poses[k].composePoint(pt_k, pt_km1[k]);
96 
97  // Look for neighbors in "time=k"
98  std::vector<TPoint3D> neig_k;
99  std::vector<float> neig_sq_dist_k;
100  pc->kdTreeNClosestPoint3D(pt_k, 2 /*num queries*/, neig_k, neig_sq_dist_k);
101 
102  // Look for neighbors in "time=k-i"
103  std::vector<TPoint3D> neig_kmi(options.previous_keyframes);
104  std::vector<float> neig_sq_dist_kmi(options.previous_keyframes, std::numeric_limits<float>::max() );
105 
106  for (int k = 0; k < options.previous_keyframes; ++k) {
107  for (int prev_tim_idx = 0; prev_tim_idx < options.previous_keyframes; prev_tim_idx++) {
108  if (prev_pc[prev_tim_idx]->pc->size() > 0) {
109  prev_pc[prev_tim_idx]->pc->kdTreeClosestPoint3D(pt_km1[prev_tim_idx], neig_kmi[prev_tim_idx], neig_sq_dist_kmi[prev_tim_idx]);
110  }
111  }
112  }
113 
114  // Rule:
115  // we must have at least 1 neighbor in t=k, and 1 neighbor in t=k-i
116  const double max_allowed_dist_sq = square(options.min_dist + options.angle_tolerance * pt_k.norm());
117 
118  bool ok_total = true;
119  const bool ok_t = neig_k.size() > 1 && neig_sq_dist_k[1] < max_allowed_dist_sq;
120  ok_total = ok_total && ok_t;
121 
122  for (int k = 0; k < options.previous_keyframes; ++k)
123  {
124  const bool ok_tm1 = neig_sq_dist_kmi[k] < max_allowed_dist_sq;
125  ok_total = ok_total && ok_tm1;
126  }
127 
128  // Delete?
129  const bool do_delete = !(ok_total);
130  deletion_mask[i] = do_delete;
131 
132  if (do_delete) del_count++;
133  }
134 
135  // Remove points:
136  if ( (params == nullptr || params->do_not_delete == false) &&
137  N>0 &&
138  del_count/double(N) < options.max_deletion_ratio // If we are deleting too many points, it may be that the filter is plainly wrong
139  )
140  {
141  pc->applyDeletionMask(deletion_mask);
142  }
143  } // we can do filter
144 
145  if (params != nullptr && params->out_deletion_mask != nullptr) {
146  *params->out_deletion_mask = deletion_mask;
147  }
148 
149  // 2) Add PC to list
150  // ---------------------
151  {
152  FrameInfo fi;
153  fi.pc = original_pc;
154  fi.pose = cur_pc_pose;
155 
156  m_last_frames[pc_timestamp] = fi;
157  }
158 
159  // 3) Remove too-old PCs.
160  // ---------------------
161  for (auto it = m_last_frames.begin(); it != m_last_frames.end(); )
162  {
163  if (mrpt::system::timeDifference(it->first, pc_timestamp) > options.too_old_seconds) {
164  it = m_last_frames.erase(it);
165  }
166  else {
167  ++it;
168  }
169  }
170 
171  MRPT_END;
172 }
173 
175  min_dist(0.10),
176  angle_tolerance( mrpt::utils::DEG2RAD(5)),
177  too_old_seconds(1.0),
178  previous_keyframes(1),
179  max_deletion_ratio(.4)
180 {
181 }
182 
184 {
185  MRPT_LOAD_CONFIG_VAR(min_dist, double, c, s);
186  MRPT_LOAD_CONFIG_VAR_DEGREES(angle_tolerance, c, s);
187  MRPT_LOAD_CONFIG_VAR(too_old_seconds, double, c, s);
188  MRPT_LOAD_CONFIG_VAR(previous_keyframes, int, c, s);
189  MRPT_LOAD_CONFIG_VAR(max_deletion_ratio, double, c, s);
190 }
191 
193 {
194  MRPT_SAVE_CONFIG_VAR_COMMENT(min_dist, "");
195  MRPT_SAVE_CONFIG_VAR_DEGREES_COMMENT("angle_tolerance",angle_tolerance, "");
196  MRPT_SAVE_CONFIG_VAR_COMMENT(too_old_seconds, "");
197  MRPT_SAVE_CONFIG_VAR_COMMENT(previous_keyframes, "(Default: 1) How many previous keyframes will be compared with the latest pointcloud.");
198  MRPT_SAVE_CONFIG_VAR_COMMENT(max_deletion_ratio, "(Default: 0.4) If the ratio [0,1] of points considered invalid (`deletion` ) is larger than this ratio, no point will be deleted since it'd be too suspicious and may indicate a failure of this filter.");
199 }
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
std::vector< bool > * out_deletion_mask
If a pointer is provided to a user-given container, the list of points to be deleted will be marked h...
double norm() const
Point norm.
bool do_not_delete
(Default:false) If true, only out_deletion_mask is filled in, but the filtered-out points will be not...
const GLfloat * c
Definition: glew.h:10088
static CSimplePointsMapPtr Create()
This class allows loading and storing values and vectors of different types from a configuration text...
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
GLdouble s
Definition: glew.h:1295
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
Lightweight 3D point (float version).
#define MRPT_END
#define DEG2RAD
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:17
#define MRPT_START
#define MRPT_LOAD_CONFIG_VAR(variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
void kdTreeNClosestPoint3D(float x0, float y0, float z0, size_t knn, std::vector< float > &out_x, std::vector< float > &out_y, std::vector< float > &out_z, std::vector< float > &out_dist_sqr) const
KD Tree-based search for the N closest points to some given 3D coordinates.
void filter(mrpt::maps::CPointsMap *inout_pointcloud, const mrpt::system::TTimeStamp pc_timestamp, const mrpt::poses::CPose3D &pc_reference_pose, TExtraFilterParams *params=nullptr) MRPT_OVERRIDE
Apply the filtering algorithm to the pointcloud.
void getPointFast(size_t index, float &x, float &y, float &z) const
Just like getPoint() but without checking out-of-bound index and without returning the point weight...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
#define MRPT_LOAD_CONFIG_VAR_DEGREES(variableName, configFileObject, sectionNameStr)
Loads a double variable, stored as radians but entered in the INI-file as degrees.
T square(const T x)
Inline function for the square of a number.
size_t size() const
Returns the number of stored points in the map.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
#define ASSERT_(f)
GLfloat * params
Definition: glew.h:1436
double BASE_IMPEXP timeDifference(const mrpt::system::TTimeStamp t_first, const mrpt::system::TTimeStamp t_later)
Returns the time difference from t1 to t2 (positive if t2 is posterior to t1), in seconds...
Definition: datetime.cpp:205
Lightweight 3D point.
#define MRPT_SAVE_CONFIG_VAR_DEGREES_COMMENT(__entryName, __variable, __comment)
void saveToConfigFile(mrpt::utils::CConfigFileBase &c, const std::string &section) const MRPT_OVERRIDE
This method saves the options to a ".ini"-like file or memory-stored string list. ...
std::vector< TYPE1, Eigen::aligned_allocator< TYPE1 > > vector_t
void applyDeletionMask(const std::vector< bool > &mask)
Remove from the map the points marked in a bool's array as "true".



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018