40 (*original_pc) = (*pc);
44 const size_t N = pc->
size();
45 std::vector<bool> deletion_mask;
46 deletion_mask.assign(N,
false);
51 bool can_do_filter =
true;
53 std::vector<FrameInfo*> prev_pc;
60 prev_pc.push_back(&it->second);
66 can_do_filter =
false;
76 can_do_filter =
false;
86 std::vector<CPose3D> rel_poses;
89 const CPose3D rel_pose = cur_pc_pose - prev_pc[k]->pose;
90 rel_poses.push_back(rel_pose);
99 for (
size_t i = 0; i < N; i++)
108 rel_poses[k].composePoint(pt_k, pt_km1[k]);
111 std::vector<TPoint3D> neig_k;
112 std::vector<float> neig_sq_dist_k;
114 pt_k, 2 , neig_k, neig_sq_dist_k);
118 std::vector<float> neig_sq_dist_kmi(
123 for (
int prev_tim_idx = 0;
126 if (prev_pc[prev_tim_idx]->pc->
size() > 0)
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]);
137 const double max_allowed_dist_sq =
square(
140 bool ok_total =
true;
142 neig_k.size() > 1 && neig_sq_dist_k[1] < max_allowed_dist_sq;
143 ok_total = ok_total && ok_t;
147 const bool ok_tm1 = neig_sq_dist_kmi[k] < max_allowed_dist_sq;
148 ok_total = ok_total && ok_tm1;
152 const bool do_delete = !(ok_total);
153 deletion_mask[i] = do_delete;
155 if (do_delete) del_count++;
159 if ((
params ==
nullptr ||
params->do_not_delete ==
false) && N > 0 &&
160 del_count / double(N) <
170 if (
params !=
nullptr &&
params->out_deletion_mask !=
nullptr)
172 *
params->out_deletion_mask = deletion_mask;
180 fi.
pose = cur_pc_pose;
224 "angle_tolerance", angle_tolerance,
"");
228 "(Default: 1) How many previous keyframes will be compared with the " 229 "latest pointcloud.");
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 " 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...
int previous_keyframes
(Default: 1) How many previous keyframes will be compared with the latest pointcloud.
mrpt::poses::CPose3D pose
mrpt::vision::TStereoCalibParams params
double max_deletion_ratio
(Default: 0.4) If the ratio [0,1] of points considered invalid ("deletion") is larger than this ratio...
#define ASSERT_(f)
Defines an assertion mechanism.
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
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.
constexpr double DEG2RAD(const double x)
Degrees to radians.
mrpt::maps::CSimplePointsMap::Ptr pc
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.
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
return_t square(const num_t x)
Inline function for the square of a number.
#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).
double angle_tolerance
(Default: 2 deg) Stored in rad.
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
double min_dist
(Default: 0.05 m)
double too_old_seconds
(Default: 1 s)
T norm() const
Point norm: |v| = sqrt(x^2+y^2+z^2)
#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...
#define MRPT_SAVE_CONFIG_VAR_DEGREES_COMMENT( __entryName, __variable, __comment)
size_t size() const
Save the point cloud as a PCL PCD file, in either ASCII or binary format.
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.
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
void applyDeletionMask(const std::vector< bool > &mask)
Remove from the map the points marked in a bool's array as "true".
void saveToConfigFile(mrpt::config::CConfigFileBase &c, const std::string §ion) 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 §ion) 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