Solver.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/OptimalTF_Result.h>
#include <mp2p_icp/Pairings.h>
#include <mp2p_icp/Parameterizable.h>
#include <mp2p_icp/WeightParameters.h>
#include <mrpt/containers/yaml.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPose3DPDFGaussianInf.h>
#include <mrpt/rtti/CObject.h>
#include <mrpt/system/COutputLogger.h>
#include <mrpt/version.h>
 
#include <any>
#include <optional>
 
namespace mp2p_icp
{
class Solver;
 
struct SolverContext
{
    SolverContext() = default;
 
    std::optional<mrpt::poses::CPose3D> guessRelativePose;
    std::optional<mrpt::poses::CPose3D> currentCorrectionFromInitialGuess;
    std::optional<mrpt::poses::CPose3D> lastIcpStepIncrement;
 
    std::optional<mrpt::poses::CPose3DPDFGaussianInf> prior;
 
    // room for optional solver-specific context:
    mutable std::map<const Solver*, std::map<std::string, std::any>>
        perSolverPersistentData;
 
    std::optional<uint32_t> icpIteration;
};
 
class Solver : public mrpt::system::COutputLogger,
               public mrpt::rtti::CObject,
               public mp2p_icp::Parameterizable
{
#if MRPT_VERSION < 0x020e00
    DEFINE_VIRTUAL_MRPT_OBJECT(Solver)
#else
    DEFINE_VIRTUAL_MRPT_OBJECT(Solver, mp2p_icp)
#endif
 
   public:
    virtual void initialize(const mrpt::containers::yaml& params);
 
    virtual bool optimal_pose(
        const Pairings& pairings, OptimalTF_Result& out,
        const SolverContext& sc) const;
 
    uint32_t runFromIteration = 0;
    uint32_t runUpToIteration = 0;  
 
    double runUntilTranslationCorrectionSmallerThan = 0;
 
    bool enabled = true;
 
   protected:
    virtual bool impl_optimal_pose(
        const Pairings& pairings, OptimalTF_Result& out,
        const SolverContext& sc) const = 0;
};
 
}  // namespace mp2p_icp