Program Listing for File optimal_tf_gauss_newton.h

↰ Return to documentation for file (mp2p_icp/include/mp2p_icp/optimal_tf_gauss_newton.h)

/*               _
 _ __ ___   ___ | | __ _
| '_ ` _ \ / _ \| |/ _` | Modular Optimization framework for
| | | | | | (_) | | (_| | Localization and mApping (MOLA)
|_| |_| |_|\___/|_|\__,_| https://github.com/MOLAorg/mola

 A repertory of multi primitive-to-primitive (MP2P) ICP algorithms
 and map building tools. mp2p_icp is part of MOLA.

 Copyright (C) 2018-2026 Jose Luis Blanco, University of Almeria,
                         and individual contributors.
 SPDX-License-Identifier: BSD-3-Clause
*/

#pragma once

#include <mp2p_icp/OptimalTF_Result.h>
#include <mp2p_icp/PairWeights.h>
#include <mp2p_icp/Pairings.h>
#include <mp2p_icp/robust_kernels.h>
#include <mrpt/poses/CPose3DPDFGaussianInf.h>

namespace mp2p_icp
{
struct OptimalTF_GN_Parameters
{
    OptimalTF_GN_Parameters() = default;

    std::optional<mrpt::poses::CPose3D> linearizationPoint;

    std::optional<mrpt::poses::CPose3DPDFGaussianInf> prior;

    double minDelta = 1e-7;

    double maxCost = 0;

    PairWeights pairWeights;

    uint32_t maxInnerLoopIterations = 6;

    RobustKernel kernel      = RobustKernel::None;
    double       kernelParam = 1.0;

    double cov2cov_alpha = 1.0;

    bool cov2cov_auto_balance_with_prior = true;

    bool verbose = false;
};

[[nodiscard]] bool optimal_tf_gauss_newton(
    const Pairings& in, OptimalTF_Result& result,
    const OptimalTF_GN_Parameters& gnParams = OptimalTF_GN_Parameters());

}  // namespace mp2p_icp