40 mrpt::make_aligned_shared<CSimplePointsMap>();
41 original_pc->copyFrom(*pc);
45 const size_t N = pc->
size();
46 std::vector<bool> deletion_mask;
47 deletion_mask.assign(N,
false);
52 bool can_do_filter =
true;
54 std::vector<FrameInfo*> prev_pc;
61 prev_pc.push_back(&it->second);
67 can_do_filter =
false;
77 can_do_filter =
false;
90 const CPose3D rel_pose = cur_pc_pose - prev_pc[k]->pose;
91 rel_poses.push_back(rel_pose);
100 for (
size_t i = 0; i < N; i++)
109 rel_poses[k].composePoint(pt_k, pt_km1[k]);
112 std::vector<TPoint3D> neig_k;
113 std::vector<float> neig_sq_dist_k;
115 pt_k, 2 , neig_k, neig_sq_dist_k);
119 std::vector<float> neig_sq_dist_kmi(
124 for (
int prev_tim_idx = 0;
127 if (prev_pc[prev_tim_idx]->pc->
size() > 0)
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]);
138 const double max_allowed_dist_sq =
square(
141 bool ok_total =
true;
143 neig_k.size() > 1 && neig_sq_dist_k[1] < max_allowed_dist_sq;
144 ok_total = ok_total && ok_t;
148 const bool ok_tm1 = neig_sq_dist_kmi[k] < max_allowed_dist_sq;
149 ok_total = ok_total && ok_tm1;
153 const bool do_delete = !(ok_total);
154 deletion_mask[i] = do_delete;
156 if (do_delete) del_count++;
160 if ((
params ==
nullptr ||
params->do_not_delete ==
false) && N > 0 &&
161 del_count / double(N) <
171 if (
params !=
nullptr &&
params->out_deletion_mask !=
nullptr)
173 *
params->out_deletion_mask = deletion_mask;
181 fi.
pose = cur_pc_pose;
207 too_old_seconds(1.0),
208 previous_keyframes(1),
209 max_deletion_ratio(.4)
228 "angle_tolerance", angle_tolerance,
"");
232 "(Default: 1) How many previous keyframes will be compared with the " 233 "latest pointcloud.");
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 " 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...
double DEG2RAD(const double x)
Degrees to radians.
int previous_keyframes
(Default: 1) How many previous keyframes will be compared with the latest pointcloud.
mrpt::poses::CPose3D pose
double norm() const
Point norm.
std::vector< T, mrpt::aligned_allocator_cpp11< T > > aligned_std_vector
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.
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.
Lightweight 3D point (float version).
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.
GLsizei const GLchar ** string
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).
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)
#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
Returns the number of stored points in the map.
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
#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.