metricmap.h

Go to the documentation of this file.

/* -------------------------------------------------------------------------
 *  A repertory of multi primitive-to-primitive (MP2P) ICP algorithms in C++
 * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria
 * See LICENSE for license information.
 * ------------------------------------------------------------------------- */
#pragma once
 
#include <mp2p_icp/NearestPlaneCapable.h>
#include <mp2p_icp/layer_name_t.h>
#include <mp2p_icp/plane_patch.h>
#include <mp2p_icp/render_params.h>
#include <mrpt/maps/CPointsMap.h>
#include <mrpt/maps/NearestNeighborsCapable.h>
#include <mrpt/math/TLine3D.h>
#include <mrpt/math/TPoint3D.h>
#include <mrpt/math/geometry.h>
#include <mrpt/poses/CPose3DPDFGaussian.h>
#include <mrpt/serialization/CSerializable.h>
#include <mrpt/topography/data_types.h>
 
#include <map>
#include <memory>
#include <optional>
#include <string>
#include <vector>
 
namespace mp2p_icp
{
class metric_map_t : public mrpt::serialization::CSerializable,
                     public std::enable_shared_from_this<metric_map_t>
{
    DEFINE_SERIALIZABLE(metric_map_t, mp2p_icp)
 
   public:
    constexpr static const char* PT_LAYER_RAW             = "raw";
    constexpr static const char* PT_LAYER_PLANE_CENTROIDS = "plane_centroids";
 
    std::map<layer_name_t, mrpt::maps::CMetricMap::Ptr> layers;
 
    std::vector<mrpt::math::TLine3D> lines;
 
    std::vector<plane_patch_t> planes;
 
    std::optional<uint64_t> id;
 
    std::optional<std::string> label;
 
    struct Georeferencing
    {
        mrpt::topography::TGeodeticCoords geo_coord;
 
        mrpt::poses::CPose3DPDFGaussian T_enu_to_map;
    };
 
    std::optional<Georeferencing> georeferencing;
 
    virtual bool empty() const;
 
    virtual size_t size() const;
 
    virtual size_t size_points_only() const;
 
    virtual std::string contents_summary() const;
 
    virtual void clear();
 
    bool save_to_file(const std::string& fileName) const;
 
    bool load_from_file(const std::string& fileName);
 
    mrpt::maps::CPointsMap::Ptr point_layer(const layer_name_t& name) const;
 
    virtual auto get_visualization(const render_params_t& p = render_params_t())
        const -> std::shared_ptr<mrpt::opengl::CSetOfObjects>;
 
    virtual void merge_with(
        const metric_map_t&                       otherPc,
        const std::optional<mrpt::math::TPose3D>& otherRelativePose =
            std::nullopt);
 
    void get_visualization_planes(
        mrpt::opengl::CSetOfObjects& o, const render_params_planes_t& p) const;
 
    void get_visualization_lines(
        mrpt::opengl::CSetOfObjects& o, const render_params_lines_t& p) const;
 
    void get_visualization_points(
        mrpt::opengl::CSetOfObjects& o, const render_params_points_t& p) const;
 
    static void get_visualization_map_layer(
        mrpt::opengl::CSetOfObjects& o, const render_params_point_layer_t& p,
        const mrpt::maps::CMetricMap::Ptr& map);
 
    Ptr get_shared_from_this();
 
    Ptr get_shared_from_this_or_clone();
 
    // const versions:
    ConstPtr get_shared_from_this() const;
    ConstPtr get_shared_from_this_or_clone() const;
 
   protected:
    virtual void derivedSerializeTo(
        [[maybe_unused]] mrpt::serialization::CArchive& out) const
    {
    }
 
    virtual void derivedSerializeFrom(
        [[maybe_unused]] mrpt::serialization::CArchive& in)
    {
    }
};
 
const mrpt::maps::CPointsMap* MapToPointsMap(const mrpt::maps::CMetricMap& map);
 
mrpt::maps::CPointsMap* MapToPointsMap(mrpt::maps::CMetricMap& map);
 
const mrpt::maps::NearestNeighborsCapable* MapToNN(
    const mrpt::maps::CMetricMap& map, bool throwIfNotImplemented);
 
const mp2p_icp::NearestPlaneCapable* MapToNP(
    const mrpt::maps::CMetricMap& map, bool throwIfNotImplemented);
 
// Serialization of geo-reference information:
mrpt::serialization::CArchive& operator>>(
    mrpt::serialization::CArchive&               in,
    std::optional<metric_map_t::Georeferencing>& g);
mrpt::serialization::CArchive& operator<<(
    mrpt::serialization::CArchive&                     out,
    const std::optional<metric_map_t::Georeferencing>& g);
 
}  // namespace mp2p_icp