Class NDT
Defined in File NDT.h
Nested Relationships
Nested Types
Inheritance Relationships
Base Types
public mrpt::maps::CMetricMap
public mrpt::maps::NearestNeighborsCapable
public mp2p_icp::NearestPlaneCapable
Class Documentation
-
class NDT : public mrpt::maps::CMetricMap, public mrpt::maps::NearestNeighborsCapable, public mp2p_icp::NearestPlaneCapable
3D NDT maps: Voxels with points approximated by Gaussians. Implementation of magnusson2007scan.
This implements two nearest-neighbor virtual APIs:
mrpt::maps::NearestNeighborsCapable: It will return pairings of local points against global (this map) points in voxels that do not conform planes.
mp2p_icp::NearestPlaneCapable: It will return points-to-plane pairings, if the local point falls within a voxel with points conforming a plane.
Indices and coordinates
-
using global_plain_index_t = uint64_t
collapsed plain unique ID for global indices
-
static constexpr size_t MAX_POINTS_PER_VOXEL = 16
-
static constexpr std::size_t GLOBAL_ID_SUBVOXEL_BITCOUNT = 5
-
inline global_index3d_t coordToGlobalIdx(const mrpt::math::TPoint3Df &pt) const
-
inline mrpt::math::TPoint3Df globalIdxToCoord(const global_index3d_t idx) const
returns the coordinate of the voxel “bottom” corner
-
static inline global_plain_index_t g2plain(const global_index3d_t &g, int subVoxelIndex = 0)
Data structures
-
using grids_map_t = tsl::robin_map<global_index3d_t, VoxelData, index3d_hash<int32_t>>
-
bool ndt_is_plane(const mp2p_icp::PointCloudEigen &ndt) const
Returns true if the fitted Gaussian fulfills the requirements of being planar enough, according to the insertionOptions parameters
Basic API for construction and main parameters
-
NDT(float voxel_size = 5.00f)
Constructor / default ctor.
- Parameters:
voxel_size – Voxel size [meters]
-
~NDT()
-
void setVoxelProperties(float voxel_size)
Reset the main voxel parameters, and clears all current map contents
Data access API
-
inline VoxelData *voxelByGlobalIdxs(const global_index3d_t &idx, bool createIfNew)
returns the voxeldata by global index coordinates, creating it or not if not found depending on createIfNew. Returns nullptr if not found and createIfNew is false
Function defined in the header file so compilers can optimize for literals “createIfNew”
-
inline const VoxelData *voxelByGlobalIdxs(const global_index3d_t &idx) const
-
inline VoxelData *voxelByCoords(const mrpt::math::TPoint3Df &pt, bool createIfNew)
Get a voxeldata by (x,y,z) coordinates, creating the voxel if needed.
-
void insertPoint(const mrpt::math::TPoint3Df &pt, const mrpt::math::TPoint3Df &sensorPose)
Insert one point into the map. The sensor pose is used to estimate the outward surface normal
-
inline const grids_map_t &voxels() const
-
mrpt::math::TBoundingBoxf boundingBox() const override
Computes the bounding box of all the points, or (0,0 ,0,0, 0,0) if there are no points. Results are cached unless the map is somehow modified to avoid repeated calculations.
-
void visitAllPoints(const std::function<void(const mrpt::math::TPoint3Df&)> &f) const
-
void visitAllVoxels(const std::function<void(const global_index3d_t&, const VoxelData&)> &f) const
-
bool saveToTextFile(const std::string &file) const
Save to a text file. Each line contains “X Y Z” point coordinates. Returns false if any error occured, true elsewere.
API of the NearestNeighborsCapable virtual interface
-
bool nn_has_indices_or_ids() const override
-
size_t nn_index_count() const override
-
bool nn_single_search(const mrpt::math::TPoint3Df &query, mrpt::math::TPoint3Df &result, float &out_dist_sqr, uint64_t &resultIndexOrID) const override
-
bool nn_single_search(const mrpt::math::TPoint2Df &query, mrpt::math::TPoint2Df &result, float &out_dist_sqr, uint64_t &resultIndexOrID) const override
-
void nn_multiple_search(const mrpt::math::TPoint3Df &query, const size_t N, std::vector<mrpt::math::TPoint3Df> &results, std::vector<float> &out_dists_sqr, std::vector<uint64_t> &resultIndicesOrIDs) const override
-
void nn_multiple_search(const mrpt::math::TPoint2Df &query, const size_t N, std::vector<mrpt::math::TPoint2Df> &results, std::vector<float> &out_dists_sqr, std::vector<uint64_t> &resultIndicesOrIDs) const override
-
void nn_radius_search(const mrpt::math::TPoint3Df &query, const float search_radius_sqr, std::vector<mrpt::math::TPoint3Df> &results, std::vector<float> &out_dists_sqr, std::vector<uint64_t> &resultIndicesOrIDs, size_t maxPoints) const override
-
void nn_radius_search(const mrpt::math::TPoint2Df &query, const float search_radius_sqr, std::vector<mrpt::math::TPoint2Df> &results, std::vector<float> &out_dists_sqr, std::vector<uint64_t> &resultIndicesOrIDs, size_t maxPoints) const override
-
template<size_t MAX_KNN>
void nn_multiple_search_impl(const mrpt::math::TPoint3Df &query, const size_t N, std::vector<mrpt::math::TPoint3Df> &results, std::vector<float> &out_dists_sqr, std::vector<uint64_t> &resultIndicesOrIDs) const
Public virtual methods implementation for NearestPlaneCapable
-
NearestPlaneResult nn_search_pt2pl(const mrpt::math::TPoint3Df &point, const float max_search_distance) const override
Public virtual methods implementation for CMetricMap
-
std::string asString() const override
Returns a short description of the map.
-
void getVisualizationInto(mrpt::opengl::CSetOfObjects &outObj) const override
-
bool isEmpty() const override
Returns true if the map is empty
-
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
This virtual method saves the map to a file “filNamePrefix”+< some_file_extension >, as an image or in any other applicable way (Notice that other methods to save the map may be implemented in classes implementing this virtual interface).
-
const mrpt::maps::CSimplePointsMap *getAsSimplePointsMap() const override
Returns a cached point cloud view of the hash map. Not efficient at all. Only for MOLA->ROS2 bridge.
Public Members
-
TInsertionOptions insertionOptions
-
TLikelihoodOptions likelihoodOptions
-
TRenderOptions renderOptions
-
float voxel_size = 1.00f
-
mola::NDT::TInsertionOptions insertionOpts
-
mola::NDT::TLikelihoodOptions likelihoodOpts
-
mola::NDT::TRenderOptions renderOpts
Protected Functions
-
void internal_clear() override
-
struct point_vector_t
Public Members
-
std::array<float, MAX_POINTS_PER_VOXEL> xs
-
std::array<float, MAX_POINTS_PER_VOXEL> ys
-
std::array<float, MAX_POINTS_PER_VOXEL> zs
-
std::array<float, MAX_POINTS_PER_VOXEL> xs
-
struct TInsertionOptions : public mrpt::config::CLoadableOptions
Options for insertObservation()
Public Functions
-
TInsertionOptions() = default
-
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion) override
-
void dumpToTextStream(std::ostream &out) const override
-
void writeToStream(mrpt::serialization::CArchive &out) const
-
void readFromStream(mrpt::serialization::CArchive &in)
Public Members
-
uint32_t max_points_per_voxel = 0
Maximum number of points per voxel. 0 means no limit further than the hard compile-time limit.
-
double remove_voxels_farther_than = .0
If !=0, remove the voxels farther (L1 distance) than this distance, in meters.
-
float min_distance_between_points = .0f
If !=0 skip the insertion of points that are closer than this distance to any other already in the voxel.
-
double max_eigen_ratio_for_planes = 0.01
-
TInsertionOptions() = default
-
struct TLikelihoodOptions : public mrpt::config::CLoadableOptions
Options used when evaluating “computeObservationLikelihood” in the derived classes.
See also
CObservation::computeObservationLikelihood
Public Functions
-
TLikelihoodOptions() = default
-
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion) override
-
void dumpToTextStream(std::ostream &out) const override
-
void writeToStream(mrpt::serialization::CArchive &out) const
-
void readFromStream(mrpt::serialization::CArchive &in)
Public Members
-
double sigma_dist = 0.5
Sigma (standard deviation, in meters) of the Gaussian observation model used to model the likelihood (default= 0.5 m)
-
double max_corr_distance = 1.0
Maximum distance in meters to consider for the numerator divided by “sigma_dist”, so that each point has a minimum (but very small) likelihood to avoid underflows (default=1.0 meters)
-
uint32_t decimation = 10
Speed up the likelihood computation by considering only one out of N rays (default=10)
-
TLikelihoodOptions() = default
-
struct TRenderOptions : public mrpt::config::CLoadableOptions
Rendering options, used in getAs3DObject()
Public Functions
-
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion) override
-
void dumpToTextStream(std::ostream &out) const override
-
void writeToStream(mrpt::serialization::CArchive &out) const
Binary dump to stream - used in derived classes’ serialization
-
void readFromStream(mrpt::serialization::CArchive &in)
Binary dump to stream - used in derived classes’ serialization
Public Members
-
bool points_visible = false
-
float point_size = 1.0f
-
mrpt::img::TColorf points_color = {.0f, .0f, 1.0f}
-
bool planes_visible = true
-
mrpt::img::TColorf planes_color = {1.0f, .0f, 1.0f}
-
bool normals_visible = true
-
mrpt::img::TColorf normals_color = {1.0f, 0.0f, 0.0f}
-
mrpt::img::TColormap points_colormap = mrpt::img::cmHOT
Colormap to use. Can be set to cmNone for uniform color
-
mrpt::img::TColormap planes_colormap = mrpt::img::cmHOT
-
uint8_t recolorizeByCoordinateIndex = 2
If colormap!=mrpt::img::cmNONE, use this coordinate as color index: 0=x 1=y 2=z
-
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion) override
-
struct VoxelData
Public Functions
-
VoxelData() = default
-
inline void insertPoint(const mrpt::math::TPoint3Df &p, const mrpt::math::TPoint3Df &sensorPose)
-
const std::optional<mp2p_icp::PointCloudEigen> &ndt() const
Gets cached NDT for this voxel, or computes it right now. If there are no points enough, nullopt is returned
-
inline bool has_ndt() const
-
inline size_t size() const
-
inline const auto &was_seen_from() const
-
VoxelData() = default