class mola::TSDF

Overview

Truncated signed distance field (TSDF) map, stored as a voxel hash map.

Each measured point contributes a signed distance sample to every voxel inside a narrow cylinder around its measurement ray, within truncation_distance of the surface. Samples are combined by a weighted running average, so the surface a query sees is the fused estimate over every measurement of it, rather than any one stored point.

Three consequences make this a distinct map class rather than another point container, and they are the reason it exists:

  • There is no representative point per cell, so the “which point stands for

    this cell” selection disappears entirely.

  • There is no correspondence search: the residual of a query point is the interpolated field value at it, and the surface normal is the field gradient. What the ICP engine consumes is still a point-to-plane pairing, so no new matcher or solver is required.

  • The field is continuous (trilinear) inside the mapped region, so the residual has no jumps as the state moves.

The map exposes mp2p_icp::NearestPlaneCapable and is therefore usable by mp2p_icp::Matcher_Point2Plane with no changes.

For a per-voxel Gaussian instead of a distance field, see mola::NDT.

#include <TSDF.h>

class TSDF:
    public mrpt::maps::CMetricMap,
    public mp2p_icp::NearestPlaneCapable,
    public mola::OptionsCapable
{
public:
    // typedefs

    typedef tsl::robin_map<global_index3d_t, VoxelData, index3d_hash<int32_t>> grids_map_t;
    typedef index3d_t<int32_t> global_index3d_t;

    // structs

    struct FieldQuery;
    struct TCreationOptions;
    struct TInsertionOptions;
    struct TRenderOptions;
    struct VoxelData;

    // fields

    TCreationOptions creationOptions;
    TInsertionOptions insertionOptions;
    TRenderOptions renderOptions;
    float voxel_size = 0.30f;
    mola::TSDF::TInsertionOptions insertionOpts;
    mola::TSDF::TRenderOptions renderOpts;

    // construction

    TSDF(float voxel_size = 0.30f);
    TSDF(const TSDF&);
    TSDF(TSDF&&);

    // methods

    void setVoxelProperties(float voxel_size);
    float voxelSize() const;
    global_index3d_t coordToGlobalIdx(const mrpt::math::TPoint3Df& pt) const;
    mrpt::math::TPoint3Df globalIdxToCenter(const global_index3d_t idx) const;
    const VoxelData* voxelByGlobalIdxs(const global_index3d_t& idx) const;
    void insertPoint(const mrpt::math::TPoint3Df& pt, const mrpt::math::TPoint3Df& sensorPt);
    FieldQuery queryField(const mrpt::math::TPoint3Df& p) const;
    const grids_map_t& voxels() const;
    size_t size() const;
    bool isBootstrapping() const;
    double lastFieldReadyFraction() const;
    mrpt::math::TBoundingBoxf boundingBox() const;
    void visitAllVoxels(const std::function<void(const global_index3d_t&, const VoxelData&)>& f) const;

    NearestPlaneResult nn_search_pt2pl(
        const mrpt::math::TPoint3Df& point,
        const float max_search_distance
        ) const;

    std::string asString() const;
    void getVisualizationInto(mrpt::opengl::CSetOfObjects& outObj) const;
    bool isEmpty() const;
    void saveMetricMapRepresentationToFile(const std::string& filNamePrefix) const;
    const mrpt::maps::CSimplePointsMap* getAsSimplePointsMap() const;
    TSDF& operator = (const TSDF&);
    TSDF& operator = (TSDF&&);
    virtual std::map<std::string, mrpt::config::CLoadableOptions*> optionsByName();

    bool trySetCreationOptions(
        const mrpt::config::CConfigFileBase& cfg,
        const std::string& section
        );
};

Inherited Members

public:
    // methods

    OptionsCapable& operator = (const OptionsCapable&);
    OptionsCapable& operator = (OptionsCapable&&);
    virtual std::map<std::string, mrpt::config::CLoadableOptions*> optionsByName() = 0;

Construction

TSDF(float voxel_size = 0.30f)

Constructor.

Parameters:

voxel_size

Voxel size [meters]

Methods

void setVoxelProperties(float voxel_size)

Resets the voxel size, and clears all current map contents.

global_index3d_t coordToGlobalIdx(const mrpt::math::TPoint3Df& pt) const

Global index of the voxel containing a point.

mrpt::math::TPoint3Df globalIdxToCenter(const global_index3d_t idx) const

Coordinates of the center of a voxel. The field is sampled at centers, so this, and not the corner, is what the interpolation is built on.

const VoxelData* voxelByGlobalIdxs(const global_index3d_t& idx) const

Returns the voxel by global index, or nullptr if it does not exist.

void insertPoint(
    const mrpt::math::TPoint3Df& pt,
    const mrpt::math::TPoint3Df& sensorPt
    )

Fuses one measured point into the field.

Parameters:

pt

Measured point, in map coordinates.

sensorPt

Origin of the measurement ray, in map coordinates.

 
FieldQuery queryField(const mrpt::math::TPoint3Df& p) const

Trilinear interpolation of the field, and its analytical gradient, at an arbitrary point. The query is valid only if all eight surrounding voxels exist and carry at least min_weight_for_query of evidence: a partially observed neighborhood would otherwise extrapolate a surface from one measurement.

bool isBootstrapping() const

True while queries are still answered from the warm-up point cloud instead of from the field.

See also:

TInsertionOptions::bootstrap_with_points

double lastFieldReadyFraction() const

Fraction of the last inserted scan at which the field returned a valid query. This is the quantity that ends the warm-up.

NearestPlaneResult nn_search_pt2pl(
    const mrpt::math::TPoint3Df& point,
    const float max_search_distance
    ) const

Returns the local tangent plane of the zero level set, as seen from the query point: the plane through the point’s own projection onto the surface, with the field gradient as normal. There is no candidate set and no selection, so nn_visit_pt2pl_candidates() reports exactly this one pairing.

const mrpt::maps::CSimplePointsMap* getAsSimplePointsMap() const

Zero-crossing points of the field, for visualization and for the MOLA->ROS2 bridge only. Not efficient.

virtual std::map<std::string, mrpt::config::CLoadableOptions*> optionsByName()

Maps an options-group name (e.g. “insertionOptions”) to a pointer to the corresponding, live CLoadableOptions member. Pointers remain valid as long as this is alive, and point directly to the map’s actual option members (no copies), so writes through them (e.g. via loadFromConfigFile()) take effect immediately.

Implementations should list every CLoadableOptions member they define, INCLUDING “creationOptions” if present (so it can be discovered/exported generically); however, callers must use trySetCreationOptions(), not a direct write through this pointer, to safely modify creation options.