class mrpt::maps::CWeightedPointsMap

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

This class stores the coordinates (x,y,z) and a “weight”, or counter of how many times that point has been seen, used only if points fusion is enabled in the options structure.

See also:

CMetricMap, CPoint, mrpt::serialization::CSerializable, CSimplePointsMap

#include <mrpt/maps/CWeightedPointsMap.h>

class CWeightedPointsMap: public mrpt::maps::CPointsMap
{
public:
    //
fields

    TLikelihoodOptions likelihoodOptions;
    TRenderOptions renderOptions;

    // construction

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

    //
methods

    virtual void reserve(size_t newLength);
    virtual void resize(size_t newLength);
    virtual void setSize(size_t newLength);
    virtual void insertPointFast(float x, float y, float z = 0);
    virtual void getPointAllFieldsFast(size_t index, std::vector<float>& point_data) const;
    virtual void setPointAllFieldsFast(size_t index, const std::vector<float>& point_data);

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

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

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

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

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

    bool save2D_to_text_stream(std::ostream& out) const;
    bool save3D_to_text_stream(std::ostream& out) const;

    template <typename BBOX>
    bool kdtree_get_bbox(BBOX& bb) 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::TPoint2Df& query,
        const 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::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;

    CWeightedPointsMap& operator = (const CPointsMap& o);
    CWeightedPointsMap& operator = (const CWeightedPointsMap& o);
    void setPointWeight(size_t index, unsigned long w);
    unsigned int getPointWeight(size_t index) const;
    float squareDistanceToClosestCorrespondenceT(const mrpt::math::TPoint2D& p0) const;

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

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

Inherited Members

public:
    // typedefs

    typedef KDTreeCapable<Derived, num_t, metric_t> self_t;

    // structs

    template <int _DIM = -1>
    struct TKDTreeDataHolder;

    struct TKDTreeSearchParams;
    struct TInsertionOptions;
    struct TLaserRange2DInsertContext;
    struct TLaserRange3DInsertContext;
    struct TLikelihoodOptions;
    struct TRenderOptions;

    //
methods

    virtual bool isEmpty() const = 0;
    virtual void saveMetricMapRepresentationToFile(const std::string& filNamePrefix) const = 0;
    virtual std::string asString() const = 0;
    virtual void getVisualizationInto(mrpt::opengl::CSetOfObjects& o) const = 0;
    float kdTreeClosestPoint2DsqrError(const TPoint2D& p0) const;
    void kdTreeTwoClosestPoint2D(const TPoint2D& p0, TPoint2D& pOut1, TPoint2D& pOut2, float& outDistSqr1, float& outDistSqr2) 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(
        const TPoint2D& p0,
        size_t N,
        std::vector<size_t>& outIdx,
        std::vector<float>& outDistSqr
        ) 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;

    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();
    KDTreeCapable& operator = (const KDTreeCapable&);
    virtual bool nn_has_indices_or_ids() const = 0;
    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,
        const 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,
        const 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,
        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 = 0
        ) const = 0;

    virtual 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 = 0
        ) const = 0;

    virtual void reserve(size_t newLength) = 0;
    virtual void resize(size_t newLength) = 0;
    virtual void setSize(size_t newLength) = 0;
    virtual void insertPointFast(float x, float y, float z = 0) = 0;
    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;
    CPointsMap& operator = (const CPointsMap& o);

    virtual void getPointRGB(
        size_t index,
        float& x,
        float& y,
        float& z,
        float& R,
        float& G,
        float& B
        ) const;

    virtual bool hasColorPoints() const;

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

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

    virtual void getVisualizationInto(mrpt::opengl::CSetOfObjects& outObj) const;

Construction

CWeightedPointsMap()

Default constructor.

Methods

virtual void reserve(size_t newLength)

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.

virtual void resize(size_t newLength)

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)

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

virtual void insertPointFast(float x, float y, float z = 0)

The virtual method for insertPoint() without calling mark_as_modified()

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

Get all the data fields for one point as a vector: [X Y Z WEIGHT] 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
    )

Set all the data fields for one point as a vector: [X Y Z WEIGHT] Unlike setPointAllFields(), this method does not check for index out of bounds.

See also:

setPointAllFields, getPointAllFields, getPointAllFieldsFast

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

See CPointsMap::loadFromRangeScan()

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

See CPointsMap::loadFromRangeScan()

void setPointWeight(size_t index, unsigned long w)

Sets the point weight, which is ignored in all classes but those which actually store that field (Note: No checks are done for out-of-bounds index).

See also:

getPointWeight

unsigned int getPointWeight(size_t index) const

Gets the point weight, which is ignored in all classes (defaults to 1) but in those which actually store that field (Note: No checks are done for out-of-bounds index).

See also:

setPointWeight