MRPT  1.9.9
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-2018, 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 
19  /** [in,out] The input pointcloud, which will be modified upon return after
20  filtering. */
22  /** [in] The timestamp of the input pointcloud */
23  const mrpt::system::TTimeStamp pc_timestamp,
24  /** [in] If nullptr, the PC is assumed to be given in global coordinates.
25  Otherwise, it will be transformed from local coordinates to global using
26  this transformation. */
27  const mrpt::poses::CPose3D& cur_pc_pose,
28  /** [in,out] additional in/out parameters */
30 {
31  using namespace mrpt::poses;
32  using namespace mrpt::math;
33  using mrpt::square;
34 
35  MRPT_START;
36  ASSERT_(pc_timestamp != INVALID_TIMESTAMP);
37  ASSERT_(pc != nullptr);
38 
39  CSimplePointsMap::Ptr original_pc =
40  mrpt::make_aligned_shared<CSimplePointsMap>();
41  original_pc->copyFrom(*pc);
42 
43  // 1) Filter:
44  // ---------------------
45  const size_t N = pc->size();
46  std::vector<bool> deletion_mask;
47  deletion_mask.assign(N, false);
48  size_t del_count = 0;
49 
50  // get reference, previous PC:
52  bool can_do_filter = true;
53 
54  std::vector<FrameInfo*> prev_pc; // (options.previous_keyframes, nullptr);
55  {
56  auto it = m_last_frames.rbegin();
57  for (int i = 0;
58  i < options.previous_keyframes && it != m_last_frames.rend();
59  ++i, ++it)
60  {
61  prev_pc.push_back(&it->second);
62  }
63  }
64 
65  if (prev_pc.size() < static_cast<size_t>(options.previous_keyframes))
66  {
67  can_do_filter = false;
68  }
69  else
70  {
71  for (int i = 0; can_do_filter && i < options.previous_keyframes; ++i)
72  {
74  m_last_frames.rbegin()->first, pc_timestamp) >
76  {
77  can_do_filter = false; // A required keyframe is too old
78  break;
79  }
80  }
81  }
82 
83  if (can_do_filter)
84  {
85  // Reference poses of each PC:
86  // Previous: prev_pc.pose
88  for (int k = 0; k < options.previous_keyframes; ++k)
89  {
90  const CPose3D rel_pose = cur_pc_pose - prev_pc[k]->pose;
91  rel_poses.push_back(rel_pose);
92  }
93 
94  // The idea is that we can now find matches between pt{i} in time_{k},
95  // composed with rel_pose
96  // with the local points in time_{k-1}.
97 
98  std::vector<TPoint3D> pt_km1(options.previous_keyframes);
99 
100  for (size_t i = 0; i < N; i++)
101  {
102  // get i-th point in time=k:
103  TPoint3Df ptf_k;
104  pc->getPointFast(i, ptf_k.x, ptf_k.y, ptf_k.z);
105  const TPoint3D pt_k = TPoint3D(ptf_k);
106 
107  // Point, referred to time=k-1 frame of reference
108  for (int k = 0; k < options.previous_keyframes; ++k)
109  rel_poses[k].composePoint(pt_k, pt_km1[k]);
110 
111  // Look for neighbors in "time=k"
112  std::vector<TPoint3D> neig_k;
113  std::vector<float> neig_sq_dist_k;
115  pt_k, 2 /*num queries*/, neig_k, neig_sq_dist_k);
116 
117  // Look for neighbors in "time=k-i"
118  std::vector<TPoint3D> neig_kmi(options.previous_keyframes);
119  std::vector<float> neig_sq_dist_kmi(
120  options.previous_keyframes, std::numeric_limits<float>::max());
121 
122  for (int k = 0; k < options.previous_keyframes; ++k)
123  {
124  for (int prev_tim_idx = 0;
125  prev_tim_idx < options.previous_keyframes; prev_tim_idx++)
126  {
127  if (prev_pc[prev_tim_idx]->pc->size() > 0)
128  {
129  prev_pc[prev_tim_idx]->pc->kdTreeClosestPoint3D(
130  pt_km1[prev_tim_idx], neig_kmi[prev_tim_idx],
131  neig_sq_dist_kmi[prev_tim_idx]);
132  }
133  }
134  }
135 
136  // Rule:
137  // we must have at least 1 neighbor in t=k, and 1 neighbor in t=k-i
138  const double max_allowed_dist_sq = square(
140 
141  bool ok_total = true;
142  const bool ok_t =
143  neig_k.size() > 1 && neig_sq_dist_k[1] < max_allowed_dist_sq;
144  ok_total = ok_total && ok_t;
145 
146  for (int k = 0; k < options.previous_keyframes; ++k)
147  {
148  const bool ok_tm1 = neig_sq_dist_kmi[k] < max_allowed_dist_sq;
149  ok_total = ok_total && ok_tm1;
150  }
151 
152  // Delete?
153  const bool do_delete = !(ok_total);
154  deletion_mask[i] = do_delete;
155 
156  if (do_delete) del_count++;
157  }
158 
159  // Remove points:
160  if ((params == nullptr || params->do_not_delete == false) && N > 0 &&
161  del_count / double(N) <
162  options.max_deletion_ratio // If we are deleting too many
163  // points, it may be that the filter
164  // is plainly wrong
165  )
166  {
167  pc->applyDeletionMask(deletion_mask);
168  }
169  } // we can do filter
170 
171  if (params != nullptr && params->out_deletion_mask != nullptr)
172  {
173  *params->out_deletion_mask = deletion_mask;
174  }
175 
176  // 2) Add PC to list
177  // ---------------------
178  {
179  FrameInfo fi;
180  fi.pc = original_pc;
181  fi.pose = cur_pc_pose;
182 
183  m_last_frames[pc_timestamp] = fi;
184  }
185 
186  // 3) Remove too-old PCs.
187  // ---------------------
188  for (auto it = m_last_frames.begin(); it != m_last_frames.end();)
189  {
190  if (mrpt::system::timeDifference(it->first, pc_timestamp) >
192  {
193  it = m_last_frames.erase(it);
194  }
195  else
196  {
197  ++it;
198  }
199  }
200 
201  MRPT_END;
202 }
203 
205  : min_dist(0.10),
206  angle_tolerance(mrpt::DEG2RAD(5)),
207  too_old_seconds(1.0),
208  previous_keyframes(1),
209  max_deletion_ratio(.4)
210 {
211 }
212 
215 {
216  MRPT_LOAD_CONFIG_VAR(min_dist, double, c, s);
217  MRPT_LOAD_CONFIG_VAR_DEGREES(angle_tolerance, c, s);
218  MRPT_LOAD_CONFIG_VAR(too_old_seconds, double, c, s);
219  MRPT_LOAD_CONFIG_VAR(previous_keyframes, int, c, s);
220  MRPT_LOAD_CONFIG_VAR(max_deletion_ratio, double, c, s);
221 }
222 
225 {
226  MRPT_SAVE_CONFIG_VAR_COMMENT(min_dist, "");
228  "angle_tolerance", angle_tolerance, "");
229  MRPT_SAVE_CONFIG_VAR_COMMENT(too_old_seconds, "");
231  previous_keyframes,
232  "(Default: 1) How many previous keyframes will be compared with the "
233  "latest pointcloud.");
235  max_deletion_ratio,
236  "(Default: 0.4) If the ratio [0,1] of points considered invalid "
237  "(`deletion` ) is larger than this ratio, no point will be deleted "
238  "since it'd be too suspicious and may indicate a failure of this "
239  "filter.");
240 }
mrpt::aligned_std_map< mrpt::system::TTimeStamp, FrameInfo > m_last_frames
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...
Definition: CPointsMap.h:450
#define MRPT_START
Definition: exceptions.h:262
double DEG2RAD(const double x)
Degrees to radians.
int previous_keyframes
(Default: 1) How many previous keyframes will be compared with the latest pointcloud.
double norm() const
Point norm.
std::vector< T, mrpt::aligned_allocator_cpp11< T > > aligned_std_vector
GLdouble s
Definition: glext.h:3676
double max_deletion_ratio
(Default: 0.4) If the ratio [0,1] of points considered invalid ("deletion") is larger than this ratio...
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:40
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:64
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
Lightweight 3D point (float version).
const GLubyte * c
Definition: glext.h:6313
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.
GLsizei const GLchar ** string
Definition: glext.h:4101
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
#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...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
#define MRPT_END
Definition: exceptions.h:266
double angle_tolerance
(Default: 2 deg) Stored in rad.
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
#define MRPT_LOAD_CONFIG_VAR_DEGREES( variableName, configFileObject, sectionNameStr)
Loads a double variable, stored as radians but entered in the INI-file as degrees.
double 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.h:122
Lightweight 3D point.
#define MRPT_SAVE_CONFIG_VAR_DEGREES_COMMENT( __entryName, __variable, __comment)
size_t size() const
Returns the number of stored points in the map.
Definition: CPointsMap.h:408
void filter(mrpt::maps::CPointsMap *inout_pointcloud, const mrpt::system::TTimeStamp pc_timestamp, const mrpt::poses::CPose3D &pc_reference_pose, TExtraFilterParams *params=nullptr) override
Apply the filtering algorithm to the pointcloud.
GLenum const GLfloat * params
Definition: glext.h:3534
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43
void applyDeletionMask(const std::vector< bool > &mask)
Remove from the map the points marked in a bool&#39;s array as "true".
void saveToConfigFile(mrpt::config::CConfigFileBase &c, const std::string &section) const override
This method saves the options to a ".ini"-like file or memory-stored string list. ...
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020