Pairings.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/plane_patch.h>
#include <mp2p_icp/render_params.h>
#include <mrpt/containers/yaml.h>
#include <mrpt/math/TLine3D.h>
#include <mrpt/serialization/CSerializable.h>
#include <mrpt/tfest/TMatchingPair.h>
#include <mrpt/typemeta/TTypeName.h>

namespace mp2p_icp
{
struct matched_plane_t
{
    plane_patch_t p_global, p_local;

    matched_plane_t() = default;
    matched_plane_t(const plane_patch_t& pl_this, const plane_patch_t& pl_other)
        : p_global(pl_this), p_local(pl_other)
    {
    }

    DECLARE_TTYPENAME_CLASSNAME(mp2p_icp::matched_plane_t)
};
using MatchedPlaneList = std::vector<matched_plane_t>;

struct matched_line_t
{
    mrpt::math::TLine3D ln_global, ln_local;

    DECLARE_TTYPENAME_CLASSNAME(mp2p_icp::matched_line_t)
};
using MatchedLineList = std::vector<matched_line_t>;

struct point_plane_pair_t
{
    plane_patch_t         pl_global;
    mrpt::math::TPoint3Df pt_local;

    point_plane_pair_t() = default;
    point_plane_pair_t(
        const plane_patch_t& p_global, const mrpt::math::TPoint3Df& p_local)
        : pl_global(p_global), pt_local(p_local)
    {
    }

    DECLARE_TTYPENAME_CLASSNAME(mp2p_icp::point_plane_pair_t)
};
using MatchedPointPlaneList = std::vector<point_plane_pair_t>;

struct point_line_pair_t
{
    mrpt::math::TLine3D  ln_global;
    mrpt::math::TPoint3D pt_local;

    point_line_pair_t() = default;
    point_line_pair_t(
        const mrpt::math::TLine3D&  l_global,
        const mrpt::math::TPoint3D& p_local)
        : ln_global(l_global), pt_local(p_local)
    {
    }

    DECLARE_TTYPENAME_CLASSNAME(mp2p_icp::point_line_pair_t)
};

using MatchedPointLineList = std::vector<point_line_pair_t>;

struct Pairings
{
    mrpt::tfest::TMatchingPairList paired_pt2pt;
    MatchedPointLineList           paired_pt2ln;
    MatchedPointPlaneList          paired_pt2pl;
    MatchedLineList                paired_ln2ln;
    MatchedPlaneList               paired_pl2pl;

    std::vector<std::pair<std::size_t, double>> point_weights;

    virtual bool empty() const
    {
        return paired_pt2pt.empty() && paired_pl2pl.empty() &&
               paired_ln2ln.empty() && paired_pt2ln.empty() &&
               paired_pt2pl.empty();
    }

    virtual size_t size() const;

    virtual std::string contents_summary() const;

    virtual void push_back(const Pairings& o);

    virtual void push_back(Pairings&& o);

    virtual void serializeTo(mrpt::serialization::CArchive& out) const;
    virtual void serializeFrom(mrpt::serialization::CArchive& in);

    virtual auto get_visualization(
        const mrpt::poses::CPose3D&     localWrtGlobal,
        const pairings_render_params_t& p = pairings_render_params_t()) const
        -> std::shared_ptr<mrpt::opengl::CSetOfObjects>;

    virtual void get_visualization_pt2pt(
        mrpt::opengl::CSetOfObjects&          o,
        const mrpt::poses::CPose3D&           localWrtGlobal,
        const render_params_pairings_pt2pt_t& p) const;

    virtual void get_visualization_pt2pl(
        mrpt::opengl::CSetOfObjects&          o,
        const mrpt::poses::CPose3D&           localWrtGlobal,
        const render_params_pairings_pt2pl_t& p) const;

    virtual void get_visualization_pt2ln(
        mrpt::opengl::CSetOfObjects&          o,
        const mrpt::poses::CPose3D&           localWrtGlobal,
        const render_params_pairings_pt2ln_t& p) const;

    DECLARE_TTYPENAME_CLASSNAME(mp2p_icp::Pairings)
};

mrpt::serialization::CArchive& operator<<(
    mrpt::serialization::CArchive& out, const Pairings& obj);

mrpt::serialization::CArchive& operator>>(
    mrpt::serialization::CArchive& in, Pairings& obj);

struct OutlierIndices
{
    std::vector<std::size_t> point2point;
    std::vector<std::size_t> line2line;
    std::vector<std::size_t> plane2plane;

    inline bool empty() const
    {
        return point2point.empty() && line2line.empty() && plane2plane.empty();
    }
    inline std::size_t size() const
    {
        return point2point.size() + line2line.size() + plane2plane.size();
    }
};

std::tuple<mrpt::math::TPoint3D, mrpt::math::TPoint3D> eval_centroids_robust(
    const Pairings& in, const OutlierIndices& outliers);

}  // namespace mp2p_icp

namespace mrpt::serialization
{
CArchive& operator<<(CArchive& out, const mp2p_icp::point_line_pair_t& obj);
CArchive& operator>>(CArchive& in, mp2p_icp::point_line_pair_t& obj);

CArchive& operator<<(CArchive& out, const mp2p_icp::point_plane_pair_t& obj);
CArchive& operator>>(CArchive& in, mp2p_icp::point_plane_pair_t& obj);

CArchive& operator<<(CArchive& out, const mp2p_icp::matched_line_t& obj);
CArchive& operator>>(CArchive& in, mp2p_icp::matched_line_t& obj);

CArchive& operator<<(CArchive& out, const mp2p_icp::matched_plane_t& obj);
CArchive& operator>>(CArchive& in, mp2p_icp::matched_plane_t& obj);

}  // namespace mrpt::serialization