Solver_Horn.cpp

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.
 * ------------------------------------------------------------------------- */
#include <mp2p_icp/Solver_Horn.h>
#include <mp2p_icp/optimal_tf_horn.h>
#include <mp2p_icp/pt2ln_pl_to_pt2pt.h>
#include <mrpt/core/exceptions.h>
#include <mrpt/math/geometry.h>
 
#include <iostream>
 
IMPLEMENTS_MRPT_OBJECT(Solver_Horn, mp2p_icp::Solver, mp2p_icp)
 
using namespace mp2p_icp;
 
void Solver_Horn::initialize(const mrpt::containers::yaml& p)
{
    Solver::initialize(p);
 
    if (p.has("pairingsWeightParameters"))
        pairingsWeightParameters.load_from(p["pairingsWeightParameters"]);
}
 
bool Solver_Horn::impl_optimal_pose(
    const Pairings& pairings, OptimalTF_Result& out,
    [[maybe_unused]] const SolverContext& sc) const
{
    MRPT_START
 
    out = OptimalTF_Result();
 
    const Pairings*         effectivePairings = &pairings;
    std::optional<Pairings> altPairings;
 
    if (!pairings.paired_pt2ln.empty() || !pairings.paired_pt2pl.empty())
    {
        altPairings       = pt2ln_pl_to_pt2pt(pairings, sc);
        effectivePairings = &altPairings.value();
    }
 
    // Compute the optimal pose, and return its validity:
    return optimal_tf_horn(*effectivePairings, pairingsWeightParameters, out);
 
    MRPT_END
}