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-2024 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;
 
    Result 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