class mrpt::maps::CPointsMap

Python API: mrpt.maps.CPointsMap

Overview

A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors.

This is a virtual class, thus only a derived class can be instantiated by the user. The user most usually wants to use CSimplePointsMap.

This class implements generic version of mrpt::maps::CMetric::insertObservation() accepting these types of sensory data:

• mrpt::obs::CObservation2DRangeScan : 2D range scans

• mrpt::obs::CObservation3DRangeScan : 3D range scans (Kinect, etc…)

• mrpt::obs::CObservationRange : IRs, Sonars, etc.

• mrpt::obs::CObservationVelodyneScan

• mrpt::obs::CObservationPointCloud

Loading and saving in the standard LAS LiDAR point cloud format is supported by installing libLAS and including the header <mrpt/maps/CPointsMaps_liblas.h> in your program. Since MRPT 1.5.0 there is no need to build MRPT against libLAS to use this feature. See LAS functions in mrpt_maps_liblas_grp.

See also:

CMetricMap, CPoint, CSerializable

#include <mrpt/maps/CPointsMap.h>

class CPointsMap:
    public mrpt::maps::CMetricMap,
    public mrpt::math::KDTreeCapable,
    public mrpt::viz::PLY_Importer,
    public mrpt::viz::PLY_Exporter,
    public mrpt::maps::NearestNeighborsCapable,
    public mrpt::config::OptionsCapable
{
public:
    // typedefs

    typedef std::shared_ptr<CPointsMap> Ptr;
    typedef std::shared_ptr<const CPointsMap> ConstPtr;

    // structs

    struct InsertCtx;
    struct TInsertionOptions;
    struct TLaserRange2DInsertContext;
    struct TLaserRange3DInsertContext;
    struct TLikelihoodOptions;
    struct TRenderOptions;

    // fields

    static constexpr static const char* POINT_FIELD_INTENSITY = "intensity";
    static constexpr static const char* POINT_FIELD_RING_ID = "ring";
    static constexpr static const char* POINT_FIELD_TIMESTAMP = "t";
    static constexpr static const char* POINT_FIELD_COLOR_Ru8 = "color_r";
    static constexpr static const char* POINT_FIELD_COLOR_Gu8 = "color_g";
    static constexpr static const char* POINT_FIELD_COLOR_Bu8 = "color_b";
    static constexpr static const char* POINT_FIELD_COLOR_Rf = "color_rf";
    static constexpr static const char* POINT_FIELD_COLOR_Gf = "color_gf";
    static constexpr static const char* POINT_FIELD_COLOR_Bf = "color_bf";
    TInsertionOptions insertionOptions;
    TLikelihoodOptions likelihoodOptions;
    TRenderOptions renderOptions;

    // construction

    CPointsMap();
    CPointsMap(const CPointsMap& o);
    CPointsMap(CPointsMap&& o);

    // methods

    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    virtual bool registerField_float(const std::string& fieldName);
    virtual bool registerField_uint16(] const std::string& fieldName);
    virtual bool registerField_uint8(] const std::string& fieldName);
    virtual bool registerField_uint32(] const std::string& fieldName);
    virtual bool registerField_double(] const std::string& fieldName);
    virtual void reserve(size_t newLength) = 0;
    virtual void resize(size_t newLength) = 0;
    virtual void setSize(size_t newLength) = 0;
    void setPointFast(size_t index, float x, float y, float z);
    void insertPointFast(float x, float y, float z);
    virtual void getPointAllFieldsFast(size_t index, std::vector<float>& point_data) const = 0;
    virtual void setPointAllFieldsFast(size_t index, const std::vector<float>& point_data) = 0;
    bool loadFromKittiVelodyneFile(const std::string& filename);
    bool saveToKittiVelodyneFile(const std::string& filename) const;
    bool load2D_from_text_file(const std::string& file);

    bool load2D_from_text_stream(
        std::istream& in,
        mrpt::optional_ref<std::string> outErrorMsg = std::nullopt
        );

    bool load3D_from_text_file(const std::string& file);

    bool load3D_from_text_stream(
        std::istream& in,
        mrpt::optional_ref<std::string> outErrorMsg = std::nullopt
        );

    bool load2Dor3D_from_text_file(const std::string& file, bool is_3D);

    bool load2Dor3D_from_text_stream(
        std::istream& in,
        mrpt::optional_ref<std::string> outErrorMsg,
        bool is_3D
        );

    bool save2D_to_text_file(const std::string& file) const;
    bool save2D_to_text_stream(std::ostream& out) const;
    bool save3D_to_text_file(const std::string& file) const;
    bool save3D_to_text_stream(std::ostream& out) const;
    virtual void saveMetricMapRepresentationToFile(const std::string& filNamePrefix) const;
    virtual bool hasPointField(const std::string& fieldName) const;
    virtual std::vector<std::string> getPointFieldNames_float() const;
    virtual std::vector<std::string> getPointFieldNames_double() const;
    virtual std::vector<std::string> getPointFieldNames_uint16() const;
    virtual std::vector<std::string> getPointFieldNames_uint8() const;
    virtual std::vector<std::string> getPointFieldNames_uint32() const;
    std::vector<std::string> getPointFieldNames_float_except_xyz() const;
    virtual float getPointField_float(size_t index, const std::string& fieldName) const;
    virtual double getPointField_double(] size_t index, ] const std::string& fieldName) const;
    virtual uint16_t getPointField_uint16(] size_t index, ] const std::string& fieldName) const;
    virtual uint8_t getPointField_uint8(] size_t index, ] const std::string& fieldName) const;
    virtual uint32_t getPointField_uint32(] size_t index, ] const std::string& fieldName) const;
    virtual void setPointField_float(size_t index, const std::string& fieldName, float value);
    virtual void setPointField_double(] size_t index, ] const std::string& fieldName, ] double value);
    virtual void setPointField_uint16(] size_t index, ] const std::string& fieldName, ] uint16_t value);
    virtual void setPointField_uint8(] size_t index, ] const std::string& fieldName, ] uint8_t value);
    virtual void setPointField_uint32(] size_t index, ] const std::string& fieldName, ] uint32_t value);
    virtual void insertPointField_float(] const std::string& fieldName, ] float value);
    virtual void insertPointField_double(] const std::string& fieldName, ] double value);
    virtual void insertPointField_uint16(] const std::string& fieldName, ] uint16_t value);
    virtual void insertPointField_uint8(] const std::string& fieldName, ] uint8_t value);
    virtual void insertPointField_uint32(] const std::string& fieldName, ] uint32_t value);

    virtual void reserveField_float(
        ] const std::string& fieldName,
        ] size_t n
        );

    virtual void reserveField_double(
        ] const std::string& fieldName,
        ] size_t n
        );

    virtual void reserveField_uint16(
        ] const std::string& fieldName,
        ] size_t n
        );

    virtual void reserveField_uint8(
        ] const std::string& fieldName,
        ] size_t n
        );

    virtual void reserveField_uint32(
        ] const std::string& fieldName,
        ] size_t n
        );

    virtual void resizeField_float(
        ] const std::string& fieldName,
        ] size_t n
        );

    virtual void resizeField_double(
        ] const std::string& fieldName,
        ] size_t n
        );

    virtual void resizeField_uint16(
        ] const std::string& fieldName,
        ] size_t n
        );

    virtual void resizeField_uint8(
        ] const std::string& fieldName,
        ] size_t n
        );

    virtual void resizeField_uint32(
        ] const std::string& fieldName,
        ] size_t n
        );

    virtual auto getPointsBufferRef_float_field(const std::string& fieldName) const;
    virtual auto getPointsBufferRef_double_field(] const std::string& fieldName) const;
    virtual auto getPointsBufferRef_uint16_field(] const std::string& fieldName) const;
    virtual auto getPointsBufferRef_uint8_field(] const std::string& fieldName) const;
    virtual auto getPointsBufferRef_uint32_field(] const std::string& fieldName) const;
    virtual auto getPointsBufferRef_float_field(const std::string& fieldName);
    virtual auto getPointsBufferRef_double_field(] const std::string& fieldName);
    virtual auto getPointsBufferRef_uint16_field(] const std::string& fieldName);
    virtual auto getPointsBufferRef_uint8_field(] const std::string& fieldName);
    virtual auto getPointsBufferRef_uint32_field(] const std::string& fieldName);
    void enableFilterByHeight(bool enable = true);
    bool isFilterByHeightEnabled() const;
    void setHeightFilterLevels(const double _z_min, const double _z_max);
    void getHeightFilterLevels(double& _z_min, double& _z_max) const;
    std::pair<double, double> getHeightFilterLevels() const;

    template <class POINTCLOUD>
    void getPCLPointCloud(POINTCLOUD& cloud) const;

    template <class POINTCLOUD>
    void setFromPCLPointCloud(const POINTCLOUD& cloud);

    size_t kdtree_get_point_count() const;
    float kdtree_get_pt(size_t idx, int dim) const;
    float kdtree_distance(const float* p1, size_t idx_p2, size_t size) const;

    template <typename BBOX>
    bool kdtree_get_bbox(BBOX& bb) const;

    virtual void nn_prepare_for_2d_queries() const;
    virtual void nn_prepare_for_3d_queries() const;
    virtual bool nn_has_indices_or_ids() const;
    virtual size_t nn_index_count() const;

    virtual bool nn_single_search(
        const mrpt::math::TPoint3Df& query,
        mrpt::math::TPoint3Df& result,
        float& out_dist_sqr,
        uint64_t& resultIndexOrIDOrID
        ) const;

    virtual bool nn_single_search(
        const mrpt::math::TPoint2Df& query,
        mrpt::math::TPoint2Df& result,
        float& out_dist_sqr,
        uint64_t& resultIndexOrID
        ) const;

    virtual void nn_multiple_search(
        const mrpt::math::TPoint3Df& query,
        size_t N,
        std::vector<mrpt::math::TPoint3Df>& results,
        std::vector<float>& out_dists_sqr,
        std::vector<uint64_t>& resultIndicesOrIDs
        ) const;

    virtual void nn_multiple_search(
        const mrpt::math::TPoint2Df& query,
        size_t N,
        std::vector<mrpt::math::TPoint2Df>& results,
        std::vector<float>& out_dists_sqr,
        std::vector<uint64_t>& resultIndicesOrIDs
        ) const;

    virtual void nn_radius_search(
        const mrpt::math::TPoint3Df& query,
        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;

    virtual void nn_radius_search(
        const mrpt::math::TPoint2Df& query,
        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;

    CPointsMap& operator = (const CPointsMap& o);
    CPointsMap& operator = (CPointsMap&& o);
    virtual float squareDistanceToClosestCorrespondence(float x0, float y0) const;
    float squareDistanceToClosestCorrespondenceT(const mrpt::math::TPoint2D& p0) const;
    virtual std::map<std::string, mrpt::config::CLoadableOptions*> optionsByName();

    void insertAnotherMap(
        const CPointsMap* otherMap,
        const mrpt::poses::CPose3D& otherPose,
        bool filterOutPointsAtZero = false,
        bool autoRegisterAllSourceFields = true
        );

    void operator += (const CPointsMap& anotherMap);
    size_t size() const;
    void getPoint(size_t index, float& x, float& y, float& z) const;
    void getPoint(size_t index, float& x, float& y) const;
    void getPoint(size_t index, double& x, double& y, double& z) const;
    void getPoint(size_t index, double& x, double& y) const;
    void getPoint(size_t index, mrpt::math::TPoint2D& p) const;
    void getPoint(size_t index, mrpt::math::TPoint3D& p) const;

    void getPoint(
        size_t index,
        mrpt::math::TPoint3Df& p
        ) const;

    void getPointFast(size_t index, float& x, float& y, float& z) const;
    bool hasColor_u8() const;
    bool hasColor_f() const;
    void setPoint(size_t index, float x, float y, float z);
    void setPoint(size_t index, const mrpt::math::TPoint2D& p);
    void setPoint(size_t index, const mrpt::math::TPoint3D& p);
    void setPoint(size_t index, float x, float y);
    const mrpt::aligned_std_vector<float>& getPointsBufferRef_x() const;
    const mrpt::aligned_std_vector<float>& getPointsBufferRef_y() const;
    const mrpt::aligned_std_vector<float>& getPointsBufferRef_z() const;

    template <class VECTOR>
    void getAllPoints(
        VECTOR& xs,
        VECTOR& ys,
        VECTOR& zs,
        size_t decimation = 1
        ) const;

    void getAllPoints(std::vector<float>& xs, std::vector<float>& ys, size_t decimation = 1) const;

    void getAllPoints(
        std::vector<mrpt::math::TPoint2D>& ps,
        size_t decimation = 1
        ) const;

    void insertPoint(float x, float y, float z = 0);
    void insertPoint(const mrpt::math::TPoint3D& p);
    bool registerPointFieldsFrom(const mrpt::maps::CPointsMap& source);
    InsertCtx prepareForInsertPointsFrom(const CPointsMap& source);
    void insertPointFrom(size_t sourcePointIndex, const InsertCtx& ctx);

    template <typename VECTOR>
    void setAllPointsTemplate(
        const VECTOR& X,
        const VECTOR& Y,
        const VECTOR& Z = VECTOR()
        );

    void setAllPoints(
        const std::vector<float>& X,
        const std::vector<float>& Y,
        const std::vector<float>& Z
        );

    void setAllPoints(const std::vector<float>& X, const std::vector<float>& Y);
    void getPointAllFields(size_t index, std::vector<float>& point_data) const;
    void setPointAllFields(size_t index, const std::vector<float>& point_data);
    void clipOutOfRangeInZ(float zMin, float zMax, mrpt::maps::CPointsMap& result) const;
    void clipOutOfRange(const mrpt::math::TPoint2D& point, float maxRange, mrpt::maps::CPointsMap& result) const;

    virtual void determineMatching2D(
        const mrpt::maps::CMetricMap* otherMap,
        const mrpt::poses::CPose2D& otherMapPose,
        mrpt::tfest::TMatchingPairList& correspondences,
        const TMatchingParams& params,
        TMatchingExtraResults& extraResults
        ) const;

    virtual void determineMatching3D(
        const mrpt::maps::CMetricMap* otherMap,
        const mrpt::poses::CPose3D& otherMapPose,
        mrpt::tfest::TMatchingPairList& correspondences,
        const TMatchingParams& params,
        TMatchingExtraResults& extraResults
        ) const;

    virtual float compute3DMatchingRatio(const mrpt::maps::CMetricMap* otherMap, const mrpt::poses::CPose3D& otherMapPose, const TMatchingRatioParams& params) const;

    void compute3DDistanceToMesh(
        const mrpt::maps::CMetricMap* otherMap2,
        const mrpt::poses::CPose3D& otherMapPose,
        float maxDistForCorrespondence,
        mrpt::tfest::TMatchingPairList& correspondences,
        float& correspondencesRatio
        );

    virtual void loadFromRangeScan(const mrpt::obs::CObservation2DRangeScan& rangeScan, const std::optional<const mrpt::poses::CPose3D>& robotPose) = 0;
    virtual void loadFromRangeScan(const mrpt::obs::CObservation3DRangeScan& rangeScan, const std::optional<const mrpt::poses::CPose3D>& robotPose) = 0;
    void loadFromVelodyneScan(const mrpt::obs::CObservationVelodyneScan& scan, const std::optional<const mrpt::poses::CPose3D>& robotPose = std::nullopt);

    void fuseWith(
        CPointsMap* anotherMap,
        float minDistForFuse = 0.02f,
        std::vector<bool>* notFusedPoints = nullptr
        );

    void changeCoordinatesReference(const mrpt::poses::CPose2D& b);
    void changeCoordinatesReference(const mrpt::poses::CPose3D& b);
    void changeCoordinatesReference(const CPointsMap& other, const mrpt::poses::CPose3D& b);
    virtual bool isEmpty() const;
    bool empty() const;
    virtual void getVisualizationInto(mrpt::viz::CSetOfObjects& outObj) const;
    virtual mrpt::math::TBoundingBoxf boundingBox() const;

    void extractCylinder(
        const mrpt::math::TPoint2D& center,
        double radius,
        double zmin,
        double zmax,
        CPointsMap& outMap
        );

    void extractPoints(const mrpt::math::TBoundingBoxf& bbox, CPointsMap& outMap);
    virtual double internal_computeObservationLikelihood(const mrpt::obs::CObservation& obs, const mrpt::poses::CPose3D& takenFrom) const;

    double internal_computeObservationLikelihoodPointCloud3D(
        const mrpt::poses::CPose3D& pc_in_map,
        const float* xs,
        const float* ys,
        const float* zs,
        std::size_t num_pts
        ) const;

    void mark_as_modified() const;
    virtual std::string asString() const;
};

// direct descendants

class CGenericPointsMap;
class CSimplePointsMap;

Inherited Members

public:
    // typedefs

    typedef std::shared_ptr<CObject> Ptr;
    typedef std::shared_ptr<const CObject> ConstPtr;
    typedef std::unique_ptr<CObject> UniquePtr;
    typedef std::unique_ptr<const CObject> ConstUniquePtr;
    typedef std::shared_ptr<CSerializable> Ptr;
    typedef std::shared_ptr<const CSerializable> ConstPtr;
    typedef std::shared_ptr<CMetricMap> Ptr;
    typedef std::shared_ptr<const CMetricMap> ConstPtr;
    typedef KDTreeCapable<Derived, num_t, metric_t> self_t;

    // structs

    template <int _DIM = -1>
    struct TKDTreeDataHolder;

    struct TKDTreeSearchParams;

    // fields

    TMapGenericParams genericMapParams;
    TKDTreeSearchParams kdtree_search_params;

    // methods

    mrpt::rtti::CObject::Ptr duplicateGetSmartPtr() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    virtual CObject* clone() const = 0;
    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    virtual std::string asString() const = 0;
    Visualizable& operator = (const Visualizable&);
    Visualizable& operator = (Visualizable&&);
    virtual void getVisualizationInto(mrpt::viz::CSetOfObjects& o) const = 0;
    std::shared_ptr<mrpt::viz::CSetOfObjects> getVisualization() const;
    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    void clear();
    virtual bool isEmpty() const = 0;
    virtual auto boundingBox() const;
    void loadFromSimpleMap(const mrpt::maps::CSimpleMap& Map);
    bool insertObservation(const mrpt::obs::CObservation& obs, const std::optional<const mrpt::poses::CPose3D>& robotPose = std::nullopt);
    bool insertObservationPtr(const mrpt::obs::CObservation::Ptr& obs, const std::optional<const mrpt::poses::CPose3D>& robotPose = std::nullopt);
    double computeObservationLikelihood(const mrpt::obs::CObservation& obs, const mrpt::poses::CPose3D& takenFrom) const;
    virtual bool canComputeObservationLikelihood(const mrpt::obs::CObservation& obs) const;
    double computeObservationsLikelihood(const mrpt::obs::CSensoryFrame& sf, const mrpt::poses::CPose3D& takenFrom);
    bool canComputeObservationsLikelihood(const mrpt::obs::CSensoryFrame& sf) const;

    virtual void determineMatching2D(
        const mrpt::maps::CMetricMap* otherMap,
        const mrpt::poses::CPose2D& otherMapPose,
        mrpt::tfest::TMatchingPairList& correspondences,
        const TMatchingParams& params,
        TMatchingExtraResults& extraResults
        ) const;

    virtual void determineMatching3D(
        const mrpt::maps::CMetricMap* otherMap,
        const mrpt::poses::CPose3D& otherMapPose,
        mrpt::tfest::TMatchingPairList& correspondences,
        const TMatchingParams& params,
        TMatchingExtraResults& extraResults
        ) const;

    virtual float compute3DMatchingRatio(const mrpt::maps::CMetricMap* otherMap, const mrpt::poses::CPose3D& otherMapPose, const TMatchingRatioParams& params) const;
    virtual void saveMetricMapRepresentationToFile(const std::string& filNamePrefix) const = 0;
    virtual void auxParticleFilterCleanUp();
    virtual float squareDistanceToClosestCorrespondence(float x0, float y0) const;
    size_t kdTreeClosestPoint2D(float x0, float y0, float& out_x, float& out_y, float& out_dist_sqr) const;
    size_t kdTreeClosestPoint2D(float x0, float y0, float& out_dist_sqr) const;
    size_t kdTreeClosestPoint2D(const TPoint2D& p0, TPoint2D& pOut, float& outDistSqr) const;
    float kdTreeClosestPoint2DsqrError(float x0, float y0) const;
    float kdTreeClosestPoint2DsqrError(const TPoint2D& p0) const;

    void kdTreeTwoClosestPoint2D(
        float x0,
        float y0,
        float& out_x1,
        float& out_y1,
        float& out_x2,
        float& out_y2,
        float& out_dist_sqr1,
        float& out_dist_sqr2
        ) const;

    void kdTreeTwoClosestPoint2D(const TPoint2D& p0, TPoint2D& pOut1, TPoint2D& pOut2, float& outDistSqr1, float& outDistSqr2) const;

    std::vector<size_t> kdTreeNClosestPoint2D(
        float x0,
        float y0,
        size_t knn,
        std::vector<float>& out_x,
        std::vector<float>& out_y,
        std::vector<float>& out_dist_sqr,
        const std::optional<float>& maximumSearchDistanceSqr = std::nullopt
        ) const;

    std::vector<size_t> kdTreeNClosestPoint2D(
        const TPoint2D& p0,
        size_t N,
        std::vector<TPoint2D>& pOut,
        std::vector<float>& outDistSqr,
        const std::optional<float>& maximumSearchDistanceSqr = std::nullopt
        ) const;

    void kdTreeNClosestPoint2DIdx(
        float x0,
        float y0,
        size_t knn,
        std::vector<size_t>& out_idx,
        std::vector<float>& out_dist_sqr,
        const std::optional<float>& maximumSearchDistanceSqr = std::nullopt
        ) const;

    void kdTreeNClosestPoint2DIdx(
        const TPoint2D& p0,
        size_t N,
        std::vector<size_t>& outIdx,
        std::vector<float>& outDistSqr
        ) const;

    size_t kdTreeClosestPoint3D(
        float x0,
        float y0,
        float z0,
        float& out_x,
        float& out_y,
        float& out_z,
        float& out_dist_sqr
        ) const;

    size_t kdTreeClosestPoint3D(float x0, float y0, float z0, float& out_dist_sqr) const;
    size_t kdTreeClosestPoint3D(const TPoint3D& p0, TPoint3D& pOut, float& outDistSqr) const;

    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 std::optional<float>& maximumSearchDistanceSqr = std::nullopt
        ) const;

    void kdTreeNClosestPoint3DWithIdx(
        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<size_t>& out_idx,
        std::vector<float>& out_dist_sqr,
        const std::optional<float>& maximumSearchDistanceSqr = std::nullopt
        ) const;

    void kdTreeNClosestPoint3D(
        const TPoint3D& p0,
        size_t N,
        std::vector<TPoint3D>& pOut,
        std::vector<float>& outDistSqr,
        const std::optional<float>& maximumSearchDistanceSqr = std::nullopt
        ) const;

    size_t kdTreeRadiusSearch3D(
        const num_t x0,
        const num_t y0,
        const num_t z0,
        const num_t maxRadiusSqr,
        std::vector<nanoflann::ResultItem<size_t, num_t>>& out_indices_dist
        ) const;

    size_t kdTreeRadiusSearch2D(
        const num_t x0,
        const num_t y0,
        const num_t maxRadiusSqr,
        std::vector<nanoflann::ResultItem<size_t, num_t>>& out_indices_dist
        ) const;

    void kdTreeNClosestPoint3DIdx(
        float x0,
        float y0,
        float z0,
        size_t knn,
        std::vector<size_t>& out_idx,
        std::vector<float>& out_dist_sqr,
        const std::optional<float>& maximumSearchDistanceSqr = std::nullopt
        ) const;

    void kdTreeNClosestPoint3DIdx(
        const TPoint3D& p0,
        size_t N,
        std::vector<size_t>& outIdx,
        std::vector<float>& outDistSqr,
        const std::optional<float>& maximumSearchDistanceSqr = std::nullopt
        ) const;

    void kdTreeEnsureIndexBuilt3D();
    void kdTreeEnsureIndexBuilt2D();
    bool kdtree_save_index_3D(] std::ostream& out) const;
    void kdtree_load_index_3D(] std::istream& in) const;
    bool kdtree_save_index_2D(] std::ostream& out) const;
    void kdtree_load_index_2D(] std::istream& in) const;
    KDTreeCapable& operator = (const KDTreeCapable& o);
    const Derived& derived() const;
    Derived& derived();

    bool loadFromPlyFile(
        const std::string& filename,
        std::vector<std::string>* file_comments = nullptr,
        std::vector<std::string>* file_obj_info = nullptr
        );

    std::string getLoadPLYErrorString() const;

    bool saveToPlyFile(
        const std::string& filename,
        bool save_in_binary = false,
        const std::vector<std::string>& file_comments = std::vector<std::string>(),
        const std::vector<std::string>& file_obj_info = std::vector<std::string>()
        ) const;

    std::string getSavePLYErrorString() const;
    virtual bool nn_has_indices_or_ids() const = 0;
    virtual void nn_prepare_for_2d_queries() const;
    virtual void nn_prepare_for_3d_queries() const;
    virtual size_t nn_index_count() const = 0;

    virtual bool nn_single_search(
        const mrpt::math::TPoint3Df& query,
        mrpt::math::TPoint3Df& result,
        float& out_dist_sqr,
        uint64_t& resultIndexOrIDOrID
        ) const = 0;

    virtual bool nn_single_search(
        const mrpt::math::TPoint2Df& query,
        mrpt::math::TPoint2Df& result,
        float& out_dist_sqr,
        uint64_t& resultIndexOrIDOrID
        ) const = 0;

    virtual void nn_multiple_search(
        const mrpt::math::TPoint3Df& query,
        size_t N,
        std::vector<mrpt::math::TPoint3Df>& results,
        std::vector<float>& out_dists_sqr,
        std::vector<uint64_t>& resultIndicesOrIDs
        ) const = 0;

    virtual void nn_multiple_search(
        const mrpt::math::TPoint2Df& query,
        size_t N,
        std::vector<mrpt::math::TPoint2Df>& results,
        std::vector<float>& out_dists_sqr,
        std::vector<uint64_t>& resultIndicesOrIDs
        ) const = 0;

    virtual void nn_radius_search(
        const mrpt::math::TPoint3Df& query,
        float search_radius_sqr,
        std::vector<mrpt::math::TPoint3Df>& results,
        std::vector<float>& out_dists_sqr,
        std::vector<uint64_t>& resultIndicesOrIDs,
        size_t maxPoints = 0
        ) const = 0;

    virtual void nn_radius_search(
        const mrpt::math::TPoint2Df& query,
        float search_radius_sqr,
        std::vector<mrpt::math::TPoint2Df>& results,
        std::vector<float>& out_dists_sqr,
        std::vector<uint64_t>& resultIndicesOrIDs,
        size_t maxPoints = 0
        ) const = 0;

    NearestNeighborsCapable& operator = (const NearestNeighborsCapable&);
    NearestNeighborsCapable& operator = (NearestNeighborsCapable&&);
    OptionsCapable& operator = (const OptionsCapable&);
    OptionsCapable& operator = (OptionsCapable&&);
    virtual std::map<std::string, mrpt::config::CLoadableOptions*> optionsByName() = 0;
    virtual bool trySetCreationOptions(] const mrpt::config::CConfigFileBase& cfg, ] const std::string& section);

Fields

static constexpr static const char* POINT_FIELD_COLOR_Ru8 = "color_r"

uint8_t RGB.r

static constexpr static const char* POINT_FIELD_COLOR_Gu8 = "color_g"

uint8_t RGB.g

static constexpr static const char* POINT_FIELD_COLOR_Bu8 = "color_b"

uint8_t RGB.b

static constexpr static const char* POINT_FIELD_COLOR_Rf = "color_rf"

float RGB.r

static constexpr static const char* POINT_FIELD_COLOR_Gf = "color_gf"

float RGB.g

static constexpr static const char* POINT_FIELD_COLOR_Bf = "color_bf"

float RGB.b

TInsertionOptions insertionOptions

The options used when inserting observations in the map.

Construction

CPointsMap()

Ctor.

CPointsMap(const CPointsMap& o)

Don’t define this one as we cannot call the virtual method impl_copyFrom() during copy ctors.

Redefine in derived classes as needed instead.

Methods

virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const

Returns information about the class of an object in runtime.

virtual bool registerField_float(const std::string& fieldName)

Registers a new data channel of type float.

If the map is not empty, the new channel is filled with default values (0) to match the current point count.

Returns:

true if the field could effectively be added to the underlying point map class.

See also:

hasPointField(), getPointFieldNames_float()

virtual bool registerField_uint16(] const std::string& fieldName)

Registers a new data channel of type uint16_t.

If the map is not empty, the new channel is filled with default values (0) to match the current point count.

Returns:

true if the field could effectively be added to the underlying point map class.

See also:

hasPointField(), getPointFieldNames_uint16()

virtual bool registerField_uint8(] const std::string& fieldName)

Registers a new data channel of type uint8_t.

If the map is not empty, the new channel is filled with default values (0) to match the current point count.

Returns:

true if the field could effectively be added to the underlying point map class.

See also:

hasPointField(), getPointFieldNames_uint8()

virtual bool registerField_uint32(] const std::string& fieldName)

Registers a new data channel of type uint32_t.

If the map is not empty, the new channel is filled with default values (0) to match the current point count. (New in MRPT 3.0.0)

Returns:

true if the field could effectively be added to the underlying point map class.

See also:

hasPointField(), getPointFieldNames_uint32()

virtual bool registerField_double(] const std::string& fieldName)

Registers a new data channel of type double.

If the map is not empty, the new channel is filled with default values (0) to match the current point count.

Returns:

true if the field could effectively be added to the underlying point map class.

See also:

hasPointField(), getPointFieldNames_double()

virtual void reserve(size_t newLength) = 0

Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.

This is useful for situations where it is approximately known the final size of the map. This method is more efficient than constantly increasing the size of the buffers. Refer to the STL C++ library’s “reserve” methods. Implementations apply a growth factor, so the resulting capacity may exceed newLength; subclasses that need the exact figure should use resize() followed by shrink_to_fit()-style compacting instead.

virtual void resize(size_t newLength) = 0

Resizes all point buffers so they can hold the given number of points: newly created points are set to default values, and old contents are not changed.

See also:

reserve, setPoint, setPointFast, setSize

virtual void setSize(size_t newLength) = 0

Resizes all point buffers so they can hold the given number of points, erasing all previous contents and leaving all points to default values.

See also:

reserve, setPoint, setPointFast, setSize

void setPointFast(size_t index, float x, float y, float z)

Changes the coordinates of the given point (0-based index), without checking for out-of-bounds and without calling mark_as_modified().

Also, color, intensity, or other data is left unchanged.

See also:

setPoint

void insertPointFast(float x, float y, float z)

Low-level method for insertPoint() without calling mark_as_modified().

Note that for derived classes having more per-point fields, you must ensure adding those fields after this to keep the length of all vectors consistent.

virtual void getPointAllFieldsFast(size_t index, std::vector<float>& point_data) const = 0

Get all the data fields for one point as a vector: depending on the implementation class this can be [X Y Z] or [X Y Z R G B], etc…

Unlike getPointAllFields(), this method does not check for index out of bounds

See also:

getPointAllFields, setPointAllFields, setPointAllFieldsFast

virtual void setPointAllFieldsFast(
    size_t index,
    const std::vector<float>& point_data
    ) = 0

Set all the data fields for one point as a vector: depending on the implementation class this can be [X Y Z] or [X Y Z R G B], etc…

Unlike setPointAllFields(), this method does not check for index out of bounds

See also:

setPointAllFields, getPointAllFields, getPointAllFieldsFast

bool loadFromKittiVelodyneFile(const std::string& filename)

Loads from a Kitti dataset Velodyne scan binary file with an XYZI point cloud.

The file can be gz compressed (only enabled if the filename ends in “.gz” to prevent spurious false autodetection of gzip files).

Returns:

true on success

bool saveToKittiVelodyneFile(const std::string& filename) const

Saves in a binary file compatible with the Kitti dataset, including XYZI fields.

Returns:

true on success

bool load2D_from_text_file(const std::string& file)

Load from a text file.

Each line should contain an “X Y” coordinate pair, separated by whitespaces. Returns false if any error occurred, true elsewere.

bool load3D_from_text_file(const std::string& file)

Load from a text file.

Each line should contain an “X Y Z” coordinate tuple, separated by whitespaces. Returns false if any error occurred, true elsewere.

bool load2Dor3D_from_text_file(const std::string& file, bool is_3D)

2D or 3D generic implementation of load2D_from_text_file and load3D_from_text_file

bool save2D_to_text_file(const std::string& file) const

Save to a text file.

Each line will contain “X Y” point coordinates. Returns false if any error occurred, true elsewere.

bool save3D_to_text_file(const std::string& file) const

Save to a text file.

Each line will contain “X Y Z” point coordinates. Returns false if any error occurred, true elsewere.

virtual void saveMetricMapRepresentationToFile(const std::string& filNamePrefix) const

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)

virtual bool hasPointField(const std::string& fieldName) const

Returns true if the map has a data channel with the given name.

See also:

getPointField_float, getPointField_double, getPointField_uint16

virtual std::vector<std::string> getPointFieldNames_float() const

Get list of all float channel names.

virtual std::vector<std::string> getPointFieldNames_double() const

Get list of all double channel names.

virtual std::vector<std::string> getPointFieldNames_uint16() const

Get list of all uint16_t channel names.

virtual std::vector<std::string> getPointFieldNames_uint8() const

Get list of all uint8_t channel names.

virtual std::vector<std::string> getPointFieldNames_uint32() const

Get list of all uint32_t channel names.

(New in MRPT 3.0.0)

std::vector<std::string> getPointFieldNames_float_except_xyz() const

Get list of all float channel names, except x,y,z.

virtual float getPointField_float(size_t index, const std::string& fieldName) const

Read the value of a float channel for a given point.

Returns 0 if field does not exist.

Parameters:

std::exception

on index out of bounds or if field exists but is not float.

virtual double getPointField_double(
    ] size_t index,
    ] const std::string& fieldName
    ) const

Read the value of a double channel for a given point.

Returns 0 if field does not exist.

Parameters:

std::exception

on index out of bounds or if field exists but is not double.

virtual uint16_t getPointField_uint16(
    ] size_t index,
    ] const std::string& fieldName
    ) const

Read the value of a uint16_t channel for a given point.

Returns 0 if field does not exist.

Parameters:

std::exception

on index out of bounds or if field exists but is not uint16_t.

virtual uint8_t getPointField_uint8(
    ] size_t index,
    ] const std::string& fieldName
    ) const

Read the value of a uint8_t channel for a given point.

Returns 0 if field does not exist.

Parameters:

std::exception

on index out of bounds or if field exists but is not uint16_t.

virtual uint32_t getPointField_uint32(
    ] size_t index,
    ] const std::string& fieldName
    ) const

Read the value of a uint32_t channel for a given point.

Returns 0 if field does not exist.

Parameters:

std::exception

on index out of bounds or if field exists but is not uint32_t.

virtual void setPointField_float(
    size_t index,
    const std::string& fieldName,
    float value
    )

Sets the value of a float channel for a given point.

Parameters:

std::exception

on index out of bounds or if field does not exist or is not float.

virtual void setPointField_double(
    ] size_t index,
    ] const std::string& fieldName,
    ] double value
    )

Sets the value of a double channel for a given point.

Parameters:

std::exception

on index out of bounds or if field does not exist or is not double.

virtual void setPointField_uint16(
    ] size_t index,
    ] const std::string& fieldName,
    ] uint16_t value
    )

Sets the value of a uint16_t channel for a given point.

Parameters:

std::exception

on index out of bounds or if field does not exist or is not uint16_t.

virtual void setPointField_uint8(
    ] size_t index,
    ] const std::string& fieldName,
    ] uint8_t value
    )

Sets the value of a uint8_t channel for a given point.

Parameters:

std::exception

on index out of bounds or if field does not exist or is not uint8_t.

virtual void setPointField_uint32(
    ] size_t index,
    ] const std::string& fieldName,
    ] uint32_t value
    )

Sets the value of a uint32_t channel for a given point.

Parameters:

std::exception

on index out of bounds or if field does not exist or is not uint32_t.

virtual void insertPointField_float(
    ] const std::string& fieldName,
    ] float value
    )

Appends a value to a float channel (for use after insertPointFast())

virtual void insertPointField_double(
    ] const std::string& fieldName,
    ] double value
    )

Appends a value to a double channel (for use after insertPointFast())

virtual void insertPointField_uint16(
    ] const std::string& fieldName,
    ] uint16_t value
    )

Appends a value to a uint16_t channel (for use after insertPointFast())

virtual void insertPointField_uint8(
    ] const std::string& fieldName,
    ] uint8_t value
    )

Appends a value to a uint8_t channel (for use after insertPointFast())

virtual void insertPointField_uint32(
    ] const std::string& fieldName,
    ] uint32_t value
    )

Appends a value to a uint32_t channel (for use after insertPointFast())

void enableFilterByHeight(bool enable = true)

Enable/disable the filter-by-height functionality.

Default upon construction is disabled.

See also:

setHeightFilterLevels

bool isFilterByHeightEnabled() const

Return whether filter-by-height is enabled.

See also:

enableFilterByHeight

void setHeightFilterLevels(const double _z_min, const double _z_max)

Set the min/max Z levels for points to be actually inserted in the map (only if enableFilterByHeight() was called before).

void getHeightFilterLevels(double& _z_min, double& _z_max) const

Get the min/max Z levels for points to be actually inserted in the map.

Deprecated Use getHeightFilterLevels() returning a pair instead.

See also:

enableFilterByHeight, setHeightFilterLevels

std::pair<double, double> getHeightFilterLevels() const

Returns the min/max Z filter levels as a pair {z_min, z_max}.

See also:

enableFilterByHeight, setHeightFilterLevels

template <class POINTCLOUD>
void getPCLPointCloud(POINTCLOUD& cloud) const

Use to convert this MRPT point cloud object into a PCL point cloud object (PointCloud<PointXYZ>).

Usage example:

mrpt::maps::CPointsCloud       pc;
pcl::PointCloud<pcl::PointXYZ> cloud;

pc.getPCLPointCloud(cloud);

See also:

setFromPCLPointCloud, CColouredPointsMap::getPCLPointCloudXYZRGB (for color data)

template <class POINTCLOUD>
void setFromPCLPointCloud(const POINTCLOUD& cloud)

Loads a PCL point cloud into this MRPT class (note: this method ignores potential RGB information, see CColouredPointsMap::setFromPCLPointCloudRGB() ).

Usage example:

pcl::PointCloud<pcl::PointXYZ> cloud;
mrpt::maps::CPointsCloud       pc;

pc.setFromPCLPointCloud(cloud);

See also:

getPCLPointCloud, CColouredPointsMap::setFromPCLPointCloudRGB()

size_t kdtree_get_point_count() const

Must return the number of data points.

float kdtree_get_pt(size_t idx, int dim) const

Returns the dim’th component of the idx’th point in the class:

float kdtree_distance(const float* p1, size_t idx_p2, size_t size) const

Returns the distance between the vector “p1[0:size-1]” and the data point with index “idx_p2” stored in the class:

virtual void nn_prepare_for_2d_queries() const

Must be called before calls to nn_*_search() to ensure the required data structures are ready for queries (e.g.

KD-trees). Useful in multithreading applications.

virtual void nn_prepare_for_3d_queries() const

Must be called before calls to nn_*_search() to ensure the required data structures are ready for queries (e.g.

KD-trees). Useful in multithreading applications.

virtual bool nn_has_indices_or_ids() const

Returns true if the rest of nn_* methods will populate the output indices values with 0-based contiguous indices.

Returns false if indices are actually sparse ID numbers without any expectation of they be contiguous or start near zero.

virtual size_t nn_index_count() const

If nn_has_indices_or_ids() returns true, this must return the number of “points” (or whatever entity) the indices correspond to.

Otherwise, the return value should be ignored.

virtual bool nn_single_search(
    const mrpt::math::TPoint3Df& query,
    mrpt::math::TPoint3Df& result,
    float& out_dist_sqr,
    uint64_t& resultIndexOrIDOrID
    ) const

Search for the closest 3D point to a given one.

Parameters:

query	The query input point.
result	The found closest point.
out_dist_sqr	The square Euclidean distance between the query and the returned point.
resultIndexOrID	The index or ID of the result point in the map.

Returns:

True if successful, false if no point was found.

virtual void nn_multiple_search(
    const mrpt::math::TPoint3Df& query,
    size_t N,
    std::vector<mrpt::math::TPoint3Df>& results,
    std::vector<float>& out_dists_sqr,
    std::vector<uint64_t>& resultIndicesOrIDs
    ) const

Search for the N closest 3D points to a given one.

Parameters:

query	The query input point.
results	The found closest points.
out_dists_sqr	The square Euclidean distances between the query and the returned point.
resultIndicesOrIDs	The indices or IDs of the result points.

virtual void nn_radius_search(
    const mrpt::math::TPoint3Df& query,
    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

Radius search for closest 3D points to a given one.

Parameters:

query	The query input point.
search_radius_sqr	The search radius, squared.
results	The found closest points.
out_dists_sqr	The square Euclidean distances between the query and the returned point.
resultIndicesOrIDs	The indices or IDs of the result points.
maxPoints	If !=0, the maximum number of neigbors to return.

virtual float squareDistanceToClosestCorrespondence(float x0, float y0) const

Returns the square distance from the 2D point (x0,y0) to the closest correspondence in the map.

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 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.

void insertAnotherMap(
    const CPointsMap* otherMap,
    const mrpt::poses::CPose3D& otherPose,
    bool filterOutPointsAtZero = false,
    bool autoRegisterAllSourceFields = true
    )

Insert the contents of another map into this one with some geometric transformation, without fusing close points.

Parameters:

otherMap	The other map whose points are to be inserted into this one.
otherPose	The pose of the other map in the coordinates of THIS map
filterOutPointsAtZero	If true, points at (0,0,0) (in the frame of reference of otherMap) will be assumed to be invalid and will not be copied.
autoRegisterAllSourceFields	If true (default) all source map fields will be registered and copied. If false, only those fields already existing in the target (this) map will be copied.

See also:

fuseWith, addFrom

void operator += (const CPointsMap& anotherMap)

Inserts another map into this one.

See also:

insertAnotherMap()

size_t size() const

Save the point cloud as a PCL PCD file, in either ASCII or binary format.

This method requires user code to include PCL before MRPT headers.

This method requires user code to include PCL before MRPT headers.

Returns:

false on any error Load the point cloud from a PCL PCD file.

false on any error Returns the number of stored points in the map.

void getPoint(size_t index, float& x, float& y, float& z) const

Access to a given point from map, as a 2D point.

First index is 0.

Parameters:

Throws

std::exception on index out of bound.

See also:

setPoint, getPointFast

void getPoint(size_t index, float& x, float& y) const

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

void getPoint(size_t index, double& x, double& y, double& z) const

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

void getPoint(size_t index, double& x, double& y) const

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

void getPoint(size_t index, mrpt::math::TPoint2D& p) const

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

void getPoint(size_t index, mrpt::math::TPoint3D& p) const

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

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, just XYZ.

bool hasColor_u8() const

Returns true if the point map has a color field for each point (uint8_t), named “color_{r,g,b}”.

bool hasColor_f() const

Returns true if the point map has a color field for each point (float), named “color_{rf,gf,bf}”.

void setPoint(size_t index, float x, float y, float z)

Changes a given point from map, with Z defaulting to 0 if not provided.

Parameters:

Throws

std::exception on index out of bound.

void setPoint(size_t index, const mrpt::math::TPoint2D& p)

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

void setPoint(size_t index, const mrpt::math::TPoint3D& p)

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

void setPoint(size_t index, float x, float y)

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

const mrpt::aligned_std_vector<float>& getPointsBufferRef_x() const

Provides a direct access to a read-only reference of the internal point buffer.

See also:

getAllPoints

const mrpt::aligned_std_vector<float>& getPointsBufferRef_y() const

Provides a direct access to a read-only reference of the internal point buffer.

See also:

getAllPoints

const mrpt::aligned_std_vector<float>& getPointsBufferRef_z() const

Provides a direct access to a read-only reference of the internal point buffer.

See also:

getAllPoints

template <class VECTOR>
void getAllPoints(
    VECTOR& xs,
    VECTOR& ys,
    VECTOR& zs,
    size_t decimation = 1
    ) const

Returns a copy of the 2D/3D points as a std::vector of float coordinates.

If decimation is greater than 1, only 1 point out of that number will be saved in the output, effectively performing a subsampling of the points.

Parameters:

VECTOR

can be std::vector<float or double> or any row/column Eigen::Array or Eigen::Matrix (this includes mrpt::math::CVectorFloat and mrpt::math::CVectorDouble).

See also:

getPointsBufferRef_x, getPointsBufferRef_y, getPointsBufferRef_z

void getAllPoints(
    std::vector<float>& xs,
    std::vector<float>& ys,
    size_t decimation = 1
    ) const

Returns a copy of the 2D/3D points as a std::vector of float coordinates.

If decimation is greater than 1, only 1 point out of that number will be saved in the output, effectively performing a subsampling of the points.

See also:

setAllPoints

void insertPoint(float x, float y, float z = 0)

Provides a way to insert (append) individual points into the map: the missing fields of child classes (color, weight, etc) are left to their default values.

void insertPoint(const mrpt::math::TPoint3D& p)

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

bool registerPointFieldsFrom(const mrpt::maps::CPointsMap& source)

Must be called before insertPointFrom() to make sure we have the required fields.

Returns:

true if ALL fields could be added, false if some would be missing because the underlying point cloud class cannot hold them.

InsertCtx prepareForInsertPointsFrom(const CPointsMap& source)

Prepare efficient data structures for repeated insertion from another point map with insertPointFrom()

void insertPointFrom(size_t sourcePointIndex, const InsertCtx& ctx)

Generic method to copy all applicable point properties from one map to another, e.g.

timestamp, intensity, etc. Before calling this in a loop, make sure of calling registerPointFieldsFrom()

template <typename VECTOR>
void setAllPointsTemplate(
    const VECTOR& X,
    const VECTOR& Y,
    const VECTOR& Z = VECTOR()
    )

Set all the points at once from vectors with X,Y and Z coordinates (if Z is not provided, it will be set to all zeros).

Parameters:

VECTOR

can be mrpt::math::CVectorFloat or std::vector<float> or any other column or row Eigen::Matrix.

void setAllPoints(
    const std::vector<float>& X,
    const std::vector<float>& Y,
    const std::vector<float>& Z
    )

Set all the points at once from vectors with X,Y and Z coordinates.

See also:

getAllPoints

void setAllPoints(const std::vector<float>& X, const std::vector<float>& Y)

Set all the points at once from vectors with X and Y coordinates (Z=0).

See also:

getAllPoints

void getPointAllFields(size_t index, std::vector<float>& point_data) const

Get all the data fields for one point as a vector: depending on the implementation class this can be [X Y Z] or [X Y Z R G B], etc…

See also:

getPointAllFieldsFast, setPointAllFields, setPointAllFieldsFast

void setPointAllFields(size_t index, const std::vector<float>& point_data)

Set all the data fields for one point as a vector: depending on the implementation class this can be [X Y Z] or [X Y Z R G B], etc…

Unlike setPointAllFields(), this method does not check for index out of bounds

See also:

setPointAllFields, getPointAllFields, getPointAllFieldsFast

void clipOutOfRangeInZ(float zMin, float zMax, mrpt::maps::CPointsMap& result) const

Stores into a new cloud all the points except those out of the given “z” azis range.

void clipOutOfRange(
    const mrpt::math::TPoint2D& point,
    float maxRange,
    mrpt::maps::CPointsMap& result
    ) const

Stores into a new cloud all the points except those farther than “maxRange” away from the given “point”.

virtual void determineMatching2D(
    const mrpt::maps::CMetricMap* otherMap,
    const mrpt::poses::CPose2D& otherMapPose,
    mrpt::tfest::TMatchingPairList& correspondences,
    const TMatchingParams& params,
    TMatchingExtraResults& extraResults
    ) const

Computes the matching between this and another 2D point map, which includes finding:

• The set of points pairs in each map

• The mean squared distance between corresponding pairs.

The algorithm is:

• For each point in “otherMap”:

  • Transform the point according to otherMapPose

  • Search with a KD-TREE the closest correspondences in “this” map.

  • Add to the set of candidate matchings, if it passes all the thresholds in params.

This method is the most time critical one into ICP-like algorithms.

Parameters:

otherMap	[IN] The other map to compute the matching with.
otherMapPose	[IN] The pose of the other map as seen from “this”.
params	[IN] Parameters for the determination of pairings.
correspondences	[OUT] The detected matchings pairs.
extraResults	[OUT] Other results.

See also:

compute3DMatchingRatio

virtual void determineMatching3D(
    const mrpt::maps::CMetricMap* otherMap,
    const mrpt::poses::CPose3D& otherMapPose,
    mrpt::tfest::TMatchingPairList& correspondences,
    const TMatchingParams& params,
    TMatchingExtraResults& extraResults
    ) const

Computes the matchings between this and another 3D points map - method used in 3D-ICP.

This method finds the set of point pairs in each map.

The method is the most time critical one into ICP-like algorithms.

The algorithm is:

• For each point in “otherMap”:

  • Transform the point according to otherMapPose

  • Search with a KD-TREE the closest correspondences in “this” map.

  • Add to the set of candidate matchings, if it passes all the thresholds in params.

Parameters:

otherMap	[IN] The other map to compute the matching with.
otherMapPose	[IN] The pose of the other map as seen from “this”.
params	[IN] Parameters for the determination of pairings.
correspondences	[OUT] The detected matchings pairs.
extraResults	[OUT] Other results.

See also:

compute3DMatchingRatio

virtual float compute3DMatchingRatio(
    const mrpt::maps::CMetricMap* otherMap,
    const mrpt::poses::CPose3D& otherMapPose,
    const TMatchingRatioParams& params
    ) const

Computes the ratio in [0,1] of correspondences between “this” and the “otherMap” map, whose 6D pose relative to “this” is “otherMapPose” In the case of a multi-metric map, this returns the average between the maps.

This method always return 0 for grid maps.

Parameters:

otherMap	[IN] The other map to compute the matching with.
otherMapPose	[IN] The 6D pose of the other map as seen from “this”.
params	[IN] Matching parameters

Returns:

The matching ratio [0,1]

See also:

determineMatching2D

void compute3DDistanceToMesh(
    const mrpt::maps::CMetricMap* otherMap2,
    const mrpt::poses::CPose3D& otherMapPose,
    float maxDistForCorrespondence,
    mrpt::tfest::TMatchingPairList& correspondences,
    float& correspondencesRatio
    )

Computes the matchings between this and another 3D points map.

This method matches each point in the other map with the centroid of the 3 closest points in 3D from this map (if the distance is below a defined threshold).

Parameters:

otherMap	[IN] The other map to compute the matching with.
otherMapPose	[IN] The pose of the other map as seen from “this”.
maxDistForCorrespondence	[IN] Maximum 2D linear distance between two points to be matched.
correspondences	[OUT] The detected matchings pairs.
correspondencesRatio	[OUT] The ratio [0,1] of points in otherMap with at least one correspondence.

See also:

determineMatching3D

virtual void loadFromRangeScan(
    const mrpt::obs::CObservation2DRangeScan& rangeScan,
    const std::optional<const mrpt::poses::CPose3D>& robotPose
    ) = 0

Transform the range scan into a set of cartessian coordinated points.

The options in “insertionOptions” are considered in this method. Only ranges marked as “valid=true” in the observation will be inserted

Each derived class may enrich points in different ways (color, weight, etc..), so please refer to the description of the specific implementation of mrpt::maps::CPointsMap you are using.

The actual generic implementation of this file lives in <src>/CPointsMap_crtp_common.h, but specific instantiations are generated at each derived class.

Parameters:

rangeScan	The scan to be inserted into this map
robotPose	Default to (0,0,0|0deg,0deg,0deg). Changes the frame of reference for the point cloud (i.e. the vehicle/robot pose in world coordinates).

See also:

CObservation2DRangeScan, CObservation3DRangeScan

virtual void loadFromRangeScan(
    const mrpt::obs::CObservation3DRangeScan& rangeScan,
    const std::optional<const mrpt::poses::CPose3D>& robotPose
    ) = 0

Overload of loadFromRangeScan() for 3D range scans (for example, Kinect observations).

Each derived class may enrich points in different ways (color, weight, etc..), so please refer to the description of the specific implementation of mrpt::maps::CPointsMap you are using.

The actual generic implementation of this file lives in <src>/CPointsMap_crtp_common.h, but specific instantiations are generated at each derived class.

Parameters:

rangeScan	The scan to be inserted into this map
robotPose	Default to (0,0,0|0deg,0deg,0deg). Changes the frame of reference for the point cloud (i.e. the vehicle/robot pose in world coordinates).

See also:

loadFromVelodyneScan

void loadFromVelodyneScan(
    const mrpt::obs::CObservationVelodyneScan& scan,
    const std::optional<const mrpt::poses::CPose3D>& robotPose = std::nullopt
    )

Like loadFromRangeScan() for Velodyne 3D scans.

Points are translated and rotated according to the sensorPose field in the observation and, if provided, to the robotPose parameter.

Parameters:

scan	The Raw LIDAR data to be inserted into this map. It MUST contain point cloud data, generated by calling to mrpt::obs::CObservationVelodyneScan::generatePointCloud() prior to insertion in this map.
robotPose	Default to (0,0,0|0deg,0deg,0deg). Changes the frame of reference for the point cloud (i.e. the vehicle/robot pose in world coordinates).

See also:

loadFromRangeScan

void fuseWith(
    CPointsMap* anotherMap,
    float minDistForFuse = 0.02f,
    std::vector<bool>* notFusedPoints = nullptr
    )

Insert the contents of another map into this one, fusing the previous content with the new one.

This means that points very close to existing ones will be “fused”, rather than “added”. This prevents the unbounded increase in size of these class of maps. NOTICE that “otherMap” is neither translated nor rotated here, so if this is desired it must done before calling this method.

Parameters:

otherMap	The other map whose points are to be inserted into this one.
minDistForFuse	Minimum distance (in meters) between two points, each one in a map, to be considered the same one and be fused rather than added.
notFusedPoints	If a pointer is supplied, this list will contain at output a list with a “bool” value per point in “this” map. This will be false/true according to that point having been fused or not.

See also:

loadFromRangeScan, addFrom

void changeCoordinatesReference(const mrpt::poses::CPose2D& b)

Replace each point \(p_i\) by \(p'_i = b \oplus p_i\) (pose compounding operator).

void changeCoordinatesReference(const mrpt::poses::CPose3D& b)

Replace each point \(p_i\) by \(p'_i = b \oplus p_i\) (pose compounding operator).

void changeCoordinatesReference(const CPointsMap& other, const mrpt::poses::CPose3D& b)

Copy all the points from “other” map to “this”, replacing each point \(p_i\) by \(p'_i = b \oplus p_i\) (pose compounding operator).

virtual bool isEmpty() const

Returns true if the map is empty/no observation has been inserted.

bool empty() const

STL-like method to check whether the map is empty:

virtual void getVisualizationInto(mrpt::viz::CSetOfObjects& outObj) const

Returns a 3D object representing the map.

The color of the points is controlled by renderOptions

virtual mrpt::math::TBoundingBoxf boundingBox() const

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 extractCylinder(
    const mrpt::math::TPoint2D& center,
    double radius,
    double zmin,
    double zmax,
    CPointsMap& outMap
    )

Extracts the points in the map within a cylinder in 3D defined the provided radius and zmin/zmax values.

void extractPoints(const mrpt::math::TBoundingBoxf& bbox, CPointsMap& outMap)

Extracts the points in the map within the area defined by two corners.

The points are coloured according the R,G,B input data.

virtual double internal_computeObservationLikelihood(
    const mrpt::obs::CObservation& obs,
    const mrpt::poses::CPose3D& takenFrom
    ) const

Internal method called by computeObservationLikelihood()

void mark_as_modified() const

Users normally don’t need to call this.

Called by this class or children classes, set m_largestDistanceFromOriginIsUpdated=false, invalidates the kd-tree cache, and such.

virtual std::string asString() const

Returns a short description of the map.