metricmap.h

Go to the documentation of this file.

/* -------------------------------------------------------------------------
 *  A repertory of multi primitive-to-primitive (MP2P) ICP algorithms in C++
 * Copyright (C) 2018-2021 Jose Luis Blanco, University of Almeria
 * See LICENSE for license information.
 * ------------------------------------------------------------------------- */
#pragma once

#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/math/TLine3D.h>
#include <mrpt/math/TPoint3D.h>
#include <mrpt/math/geometry.h>
#include <mrpt/serialization/CSerializable.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;

    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_point_layer(
        mrpt::opengl::CSetOfObjects& o, const render_params_point_layer_t& p,
        const mrpt::maps::CPointsMap::Ptr& pts);

    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)
    {
    }
};

struct pointcloud_bitfield_t
{
    pointcloud_bitfield_t()  = default;
    ~pointcloud_bitfield_t() = default;

    std::map<layer_name_t, std::vector<bool>> point_layers;
    std::vector<bool>                         lines;
    std::vector<bool>                         planes;
    void initialize_from(const metric_map_t& pc, bool initBoolValue = false)
    {
        // Points:
        // Done in this way to avoid avoidable memory reallocations.
        for (const auto& kv : pc.layers)
        {
            ASSERT_(kv.second);
            auto pts =
                std::dynamic_pointer_cast<mrpt::maps::CPointsMap>(kv.second);
            if (!pts) continue;
            point_layers[kv.first].assign(pts->size(), initBoolValue);
        }
        std::set<layer_name_t> layersToRemove;
        for (auto& kv : point_layers)
        {
            if (pc.layers.count(kv.first) == 0) layersToRemove.insert(kv.first);
        }
        for (const auto& ly : layersToRemove) point_layers.erase(ly);

        // Lines:
        lines.assign(pc.lines.size(), initBoolValue);

        // planes:
        planes.assign(pc.planes.size(), initBoolValue);
    }
};

}  // namespace mp2p_icp