MRPT  1.9.9
CPointCloudFilterByDistance.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "maps-precomp.h" // Precomp header
11 
14 #include <vector>
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  auto original_pc = CSimplePointsMap::Create();
40  (*original_pc) = (*pc); // make deep copy
41 
42  // 1) Filter:
43  // ---------------------
44  const size_t N = pc->size();
45  std::vector<bool> deletion_mask;
46  deletion_mask.assign(N, false);
47  size_t del_count = 0;
48 
49  // get reference, previous PC:
51  bool can_do_filter = true;
52 
53  std::vector<FrameInfo*> prev_pc; // (options.previous_keyframes, nullptr);
54  {
55  auto it = m_last_frames.rbegin();
56  for (int i = 0;
57  i < options.previous_keyframes && it != m_last_frames.rend();
58  ++i, ++it)
59  {
60  prev_pc.push_back(&it->second);
61  }
62  }
63 
64  if (prev_pc.size() < static_cast<size_t>(options.previous_keyframes))
65  {
66  can_do_filter = false;
67  }
68  else
69  {
70  for (int i = 0; can_do_filter && i < options.previous_keyframes; ++i)
71  {
73  m_last_frames.rbegin()->first, pc_timestamp) >
75  {
76  can_do_filter = false; // A required keyframe is too old
77  break;
78  }
79  }
80  }
81 
82  if (can_do_filter)
83  {
84  // Reference poses of each PC:
85  // Previous: prev_pc.pose
86  std::vector<CPose3D> rel_poses;
87  for (int k = 0; k < options.previous_keyframes; ++k)
88  {
89  const CPose3D rel_pose = cur_pc_pose - prev_pc[k]->pose;
90  rel_poses.push_back(rel_pose);
91  }
92 
93  // The idea is that we can now find matches between pt{i} in time_{k},
94  // composed with rel_pose
95  // with the local points in time_{k-1}.
96 
97  std::vector<TPoint3D> pt_km1(options.previous_keyframes);
98 
99  for (size_t i = 0; i < N; i++)
100  {
101  // get i-th point in time=k:
102  TPoint3Df ptf_k;
103  pc->getPointFast(i, ptf_k.x, ptf_k.y, ptf_k.z);
104  const TPoint3D pt_k = TPoint3D(ptf_k);
105 
106  // Point, referred to time=k-1 frame of reference
107  for (int k = 0; k < options.previous_keyframes; ++k)
108  rel_poses[k].composePoint(pt_k, pt_km1[k]);
109 
110  // Look for neighbors in "time=k"
111  std::vector<TPoint3D> neig_k;
112  std::vector<float> neig_sq_dist_k;
114  pt_k, 2 /*num queries*/, neig_k, neig_sq_dist_k);
115 
116  // Look for neighbors in "time=k-i"
117  std::vector<TPoint3D> neig_kmi(options.previous_keyframes);
118  std::vector<float> neig_sq_dist_kmi(
119  options.previous_keyframes, std::numeric_limits<float>::max());
120 
121  for (int k = 0; k < options.previous_keyframes; ++k)
122  {
123  for (int prev_tim_idx = 0;
124  prev_tim_idx < options.previous_keyframes; prev_tim_idx++)
125  {
126  if (prev_pc[prev_tim_idx]->pc->size() > 0)
127  {
128  prev_pc[prev_tim_idx]->pc->kdTreeClosestPoint3D(
129  pt_km1[prev_tim_idx], neig_kmi[prev_tim_idx],
130  neig_sq_dist_kmi[prev_tim_idx]);
131  }
132  }
133  }
134 
135  // Rule:
136  // we must have at least 1 neighbor in t=k, and 1 neighbor in t=k-i
137  const double max_allowed_dist_sq = square(
139 
140  bool ok_total = true;
141  const bool ok_t =
142  neig_k.size() > 1 && neig_sq_dist_k[1] < max_allowed_dist_sq;
143  ok_total = ok_total && ok_t;
144 
145  for (int k = 0; k < options.previous_keyframes; ++k)
146  {
147  const bool ok_tm1 = neig_sq_dist_kmi[k] < max_allowed_dist_sq;
148  ok_total = ok_total && ok_tm1;
149  }
150 
151  // Delete?
152  const bool do_delete = !(ok_total);
153  deletion_mask[i] = do_delete;
154 
155  if (do_delete) del_count++;
156  }
157 
158  // Remove points:
159  if ((params == nullptr || params->do_not_delete == false) && N > 0 &&
160  del_count / double(N) <
161  options.max_deletion_ratio // If we are deleting too many
162  // points, it may be that the filter
163  // is plainly wrong
164  )
165  {
166  pc->applyDeletionMask(deletion_mask);
167  }
168  } // we can do filter
169 
170  if (params != nullptr && params->out_deletion_mask != nullptr)
171  {
172  *params->out_deletion_mask = deletion_mask;
173  }
174 
175  // 2) Add PC to list
176  // ---------------------
177  {
178  FrameInfo fi;
179  fi.pc = original_pc;
180  fi.pose = cur_pc_pose;
181 
182  m_last_frames[pc_timestamp] = fi;
183  }
184 
185  // 3) Remove too-old PCs.
186  // ---------------------
187  for (auto it = m_last_frames.begin(); it != m_last_frames.end();)
188  {
189  if (mrpt::system::timeDifference(it->first, pc_timestamp) >
191  {
192  it = m_last_frames.erase(it);
193  }
194  else
195  {
196  ++it;
197  }
198  }
199 
200  MRPT_END;
201 }
202 
204  : angle_tolerance(mrpt::DEG2RAD(5))
205 
206 {
207 }
208 
211 {
212  MRPT_LOAD_CONFIG_VAR(min_dist, double, c, s);
213  MRPT_LOAD_CONFIG_VAR_DEGREES(angle_tolerance, c, s);
214  MRPT_LOAD_CONFIG_VAR(too_old_seconds, double, c, s);
215  MRPT_LOAD_CONFIG_VAR(previous_keyframes, int, c, s);
216  MRPT_LOAD_CONFIG_VAR(max_deletion_ratio, double, c, s);
217 }
218 
221 {
222  MRPT_SAVE_CONFIG_VAR_COMMENT(min_dist, "");
224  "angle_tolerance", angle_tolerance, "");
225  MRPT_SAVE_CONFIG_VAR_COMMENT(too_old_seconds, "");
227  previous_keyframes,
228  "(Default: 1) How many previous keyframes will be compared with the "
229  "latest pointcloud.");
231  max_deletion_ratio,
232  "(Default: 0.4) If the ratio [0,1] of points considered invalid "
233  "(`deletion` ) is larger than this ratio, no point will be deleted "
234  "since it'd be too suspicious and may indicate a failure of this "
235  "filter.");
236 }
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:462
#define MRPT_START
Definition: exceptions.h:241
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: |v| = sqrt(x^2+y^2+z^2)
Definition: TPoint3D.h:177
GLdouble s
Definition: glext.h:3682
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:120
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:65
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).
Definition: TPoint3D.h:21
const GLubyte * c
Definition: glext.h:6406
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:4116
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...
static Ptr Create(Args &&... args)
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:84
#define MRPT_END
Definition: exceptions.h:245
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:123
Lightweight 3D point.
Definition: TPoint3D.h:90
#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:422
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:3538
#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.
std::map< mrpt::system::TTimeStamp, FrameInfo > m_last_frames



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 928d0abbd Sun Oct 13 02:28:49 2019 +0200 at dom oct 13 02:30:11 CEST 2019