QualityEvaluator_RangeImageSimilarity.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/QualityEvaluator.h>
#include <mrpt/img/TCamera.h>
#include <mrpt/math/CMatrixDynamic.h>

namespace mp2p_icp
{
class QualityEvaluator_RangeImageSimilarity : public QualityEvaluator
{
    DEFINE_MRPT_OBJECT(QualityEvaluator_RangeImageSimilarity, mp2p_icp)

   public:
    void initialize(const mrpt::containers::yaml& params) override;

    double evaluate(
        const metric_map_t& pcGlobal, const metric_map_t& pcLocal,
        const mrpt::poses::CPose3D& localPose,
        const Pairings&             pairingsFromICP) const override;

    mrpt::img::TCamera rangeCamera;

    double sigma = 0.1;  

    double penalty_not_visible = 2.0;

    bool debug_show_all_in_window = false;
    bool debug_save_all_matrices  = false;

    mrpt::math::CMatrixDouble projectPoints(
        const mrpt::maps::CPointsMap&              pts,
        const std::optional<mrpt::poses::CPose3D>& relativePose =
            std::nullopt) const;

    std::vector<double> scores(
        const mrpt::math::CMatrixDouble& m1,
        const mrpt::math::CMatrixDouble& m2) const;
};

}  // namespace mp2p_icp