GTSAM 中 LevenbergMarquardtParams详解和综合完整使用示例-CSDN博客

本文链接：https://blog.csdn.net/qq_36812406/article/details/150552987

1. `LevenbergMarquardtParams` 是什么

在 GTSAM 中，LevenbergMarquardtParams 是 Levenberg–Marquardt (LM) 优化器 的参数配置类，用来控制优化过程的收敛、阻尼和终止条件。

LM 是一种 介于高斯-牛顿 (GN) 和梯度下降 之间的优化方法：

当阻尼系数 λ 很小 → 接近 GN，收敛快但可能不稳定
当 λ 很大 → 接近梯度下降，收敛慢但更稳定
λ 会在迭代过程中自动调整（如果优化成功降低了误差 → 减小 λ；否则增加 λ）

2. 主要字段解释

GTSAM 源码位置：gtsam/nonlinear/LevenbergMarquardtParams.h

参数	类型	作用
`lambdaInitial`	double	初始阻尼系数 λ，默认 1e-5
`lambdaFactor`	double	每次失败时 λ 的放大倍数（>1 增大，<1 减小）
`lambdaUpperBound`	double	阻尼系数上限，防止 λ 无限增长
`lambdaLowerBound`	double	阻尼系数下限，防止 λ 过小
`relativeErrorTol`	double	相对误差变化的收敛条件
`absoluteErrorTol`	double	绝对误差的收敛条件
`maxIterations`	int	最大迭代次数
`verbosityLM`	enum VerbosityLM	输出详细程度（SILENT, SUMMARY, DAMPED, TRYlambda, TRYdelta）
`verbosity`	enum Verbosity	打印迭代收敛信息
`ordering`	Ordering	可选，指定变量的消元顺序
`linearSolverType`	enum LinearSolverType	线性求解器类型（CHOLMOD, MULTIFRONTAL_QR, 等）

3. 加入惩罚参数（阻尼系数 λ）

在 LM 中，阻尼参数 λ 是 惩罚项 的权重，相当于在 Hessian 矩阵上加上一个 λI：

$H_{\text{LM}} = H + \lambda I$

这样：

λ 大时 → 更保守，步长更小
λ 小时 → 更接近高斯-牛顿

可以通过：

params.lambdaInitial = 1e-3;     // 初始 λ
params.lambdaFactor = 2.0;       // 每次失败翻倍
params.lambdaUpperBound = 1e6;   // 防止 λ 过大
params.lambdaLowerBound = 1e-7;  // 防止 λ 过小

直接调整惩罚参数。

4. 简单示例

下面的示例：

创建一个简单的非线性优化问题（Pose2 平移旋转）
使用 LevenbergMarquardtOptimizer 优化
设置 LevenbergMarquardtParams，加入阻尼惩罚参数控制

#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtParams.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Pose2.h>

using namespace gtsam;

int main() {
    NonlinearFactorGraph graph;

    // 噪声模型
    auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
    auto odomNoise  = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));

    // 先验约束
    graph.add(PriorFactor<Pose2>(0, Pose2(0, 0, 0), priorNoise));

    // 里程计约束
    graph.add(BetweenFactor<Pose2>(0, 1, Pose2(2, 0, 0), odomNoise));
    graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, M_PI / 2), odomNoise));

    // 初始值（故意偏差大）
    Values initialEstimate;
    initialEstimate.insert(0, Pose2(0.5, 0.0, 0.2));
    initialEstimate.insert(1, Pose2(2.3, 0.1, -0.2));
    initialEstimate.insert(2, Pose2(4.1, 0.1, M_PI/2 - 0.2));

    // 设置 LM 参数
    LevenbergMarquardtParams params;
    params.setVerbosityLM("SUMMARY");   // 输出每步 λ 变化
    params.verbosity = NonlinearOptimizerParams::Verbosity::ERROR;
    params.lambdaInitial = 1e-3;        // 初始阻尼
    params.lambdaFactor = 2.0;          // λ 扩大倍数
    params.lambdaUpperBound = 1e6;      // λ 上限
    params.lambdaLowerBound = 1e-7;     // λ 下限
    params.maxIterations = 50;
    params.relativeErrorTol = 1e-5;
    params.absoluteErrorTol = 1e-7;

    // 优化
    LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params);
    Values result = optimizer.optimize();

    // 输出结果
    result.print("Final Result:\n");

    return 0;
}

运行后会输出：

iter      cost      lambda
0         12.34     1e-3
1         8.21      5e-4
2         4.56      2.5e-4
...
Final Result:
Values with 3 values:
Value 0: (0,0,0)
Value 1: (2,0,0)
Value 2: (4,0,1.5708)

5. 在 ICP + ISAM2 + LM 全局优化中的综合示例

5.1、概述

在用 ISAM2 实时优化 后想再做一次 全局 LM，具体的操作步骤如下：

从 ISAM2 拿出全局因子图 graph 和当前估计 values
配置 LevenbergMarquardtParams（可加 λ 惩罚）
调用 LevenbergMarquardtOptimizer(graph, values, params) 再全局优化一次

这样能 消除 ISAM2 增量更新带来的漂移和局部最优陷阱。

5.2、完成实现代码示例

下面演示 ICP → ISAM2（增量融合）→ Levenberg–Marquardt（全局精修，带 LevenbergMarquardtParams） 的地图拼接融合流程，并把 LevenbergMarquardtParams 明确配置在最后的全局优化中。

说明：示例使用合成点云（便于直接运行），依赖 PCL（用于 ICP / 点云处理）和 GTSAM（用于 ISAM2 与 LM）。把代码保存为 icp_isam2_lm.cpp，并配合提供的 CMakeLists.txt 编译运行即可。

文件：`icp_isam2_lm.cpp`

// icp_isam2_lm.cpp
// Demo: ICP (PCL) -> ISAM2 (GTSAM) incremental fusion -> Final LM (GTSAM) with LevenbergMarquardtParams
// Build: requires PCL, GTSAM, Eigen

#include <iostream>
#include <vector>
#include <cmath>
#include <cstdlib>

#include <Eigen/Core>
#include <Eigen/Geometry>

// PCL
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/registration/icp.h>
#include <pcl/filters/voxel_grid.h>

// GTSAM
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtParams.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/noiseModel/Diagonal.h>

using PointT = pcl::PointXYZ;
using Cloud = pcl::PointCloud<PointT>;
using CloudPtr = pcl::PointCloud<PointT>::Ptr;
using namespace gtsam;

// ------------------- Helpers: conversions -------------------
Pose3 eigen4dToPose3(const Eigen::Matrix4d &M) {
    Eigen::Matrix3d R = M.block<3,3>(0,0);
    Eigen::Vector3d t = M.block<3,1>(0,3);
    return Pose3(Rot3(R), Point3(t));
}
Eigen::Matrix4d pose3ToEigen4d(const Pose3 &p) {
    Eigen::Matrix4d M = Eigen::Matrix4d::Identity();
    M.block<3,3>(0,0) = p.rotation().matrix();
    M.block<3,1>(0,3) = p.translation().vector();
    return M;
}

// ------------------- Simple ICP wrapper -------------------
struct ICPResult {
    Eigen::Matrix4d transform; // source -> target
    double fitness; // lower is better
    bool converged;
};

ICPResult runICP(const CloudPtr& src_raw, const CloudPtr& tgt_raw,
                 const Eigen::Matrix4d& init_guess = Eigen::Matrix4d::Identity(),
                 double leaf = 0.02, int max_iter = 100, double max_corr = 0.05)
{
    // Downsample
    CloudPtr src(new Cloud), tgt(new Cloud);
    pcl::VoxelGrid<PointT> vg;
    vg.setLeafSize(float(leaf), float(leaf), float(leaf));
    vg.setInputCloud(src_raw); vg.filter(*src);
    vg.setInputCloud(tgt_raw); vg.filter(*tgt);

    pcl::IterativeClosestPoint<PointT, PointT> icp;
    icp.setMaximumIterations(max_iter);
    icp.setMaxCorrespondenceDistance(max_corr);
    icp.setTransformationEpsilon(1e-9);
    icp.setInputSource(src);
    icp.setInputTarget(tgt);

    Cloud aligned;
    Eigen::Matrix4f initf = init_guess.cast<float>();
    icp.align(aligned, initf);

    ICPResult r;
    r.converged = icp.hasConverged();
    r.transform = icp.getFinalTransformation().cast<double>();
    r.fitness = icp.getFitnessScore(max_corr);
    return r;
}

// ------------------- Main demo -------------------
int main(int argc, char** argv) {
    std::cout << "ICP + ISAM2 + LM demo (with LevenbergMarquardtParams)\n";

    // --- 1. create N synthetic submaps (simple line-like point clouds) ---
    const int N = 6;
    std::vector<CloudPtr> submaps;
    for (int i = 0; i < N; ++i) {
        CloudPtr cloud(new Cloud);
        // generate points along x with small y variation, shifted by i * spacing
        double spacing = 2.0;
        for (int p = 0; p < 300; ++p) {
            float x = float(p) / 150.0f + float(i) * spacing;
            float y = std::sin(float(p)/20.0f) * 0.05f + 0.02f * ((std::rand() % 100) / 100.0f);
            float z = 0.01f * ((std::rand() % 100) / 100.0f);
            cloud->push_back(PointT(x, y, z));
        }
        submaps.push_back(cloud);
    }

    // --- 2. build fullGraph (odom edges + will hold loop closures) and initial values ---
    NonlinearFactorGraph fullGraph;
    Values fullInitial;

    // Noise models
    auto priorNoise = noiseModel::Diagonal::Sigmas((Vector(6) << 1e-6,1e-6,1e-6,1e-3,1e-3,1e-3).finished());
    auto odomNoise   = noiseModel::Diagonal::Sigmas((Vector(6) << 0.05,0.05,0.05,0.1,0.1,0.1).finished());
    auto loopNoise   = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1,0.1,0.1,0.2,0.2,0.2).finished());

    // Add nodes and odom factors
    for (int i = 0; i < N; ++i) {
        Key k = Symbol('X', i);
        Pose3 initPose(Rot3::identity(), Point3(i * 2.0, 0.0, 0.0)); // coarse
        fullInitial.insert(k, initPose);
        if (i == 0) fullGraph.add(PriorFactor<Pose3>(k, initPose, priorNoise));
        if (i > 0) {
            Key kp = Symbol('X', i-1);
            Pose3 odomRel(Rot3::identity(), Point3(2.0, 0.0, 0.0));
            fullGraph.add(BetweenFactor<Pose3>(kp, k, odomRel, odomNoise));
        }
    }

    // --- 3. initialize ISAM2 with odometry graph (incremental) ---
    ISAM2Params params;
    params.relinearizeThreshold = 0.01;
    params.relinearizeSkip = 1;
    ISAM2 isam(params);

    isam.update(fullGraph, fullInitial); // initial graph (odom + prior)

    std::cout << "[ISAM2] initial update done.\n";

    // --- 4. online: attempt ICP-based loops (non-adjacent submap pairs)
    // For every pair (i, j) with j >= i+2 try ICP (could parallelize in real system)
    for (int i = 0; i < N; ++i) {
        for (int j = i + 2; j < N; ++j) {
            // Get current estimated poses to build ICP init guess
            Pose3 pi = isam.calculateEstimate<Pose3>(Symbol('X', i));
            Pose3 pj = isam.calculateEstimate<Pose3>(Symbol('X', j));
            Eigen::Matrix4d init_guess = pose3ToEigen4d(pi).inverse() * pose3ToEigen4d(pj);

            // run ICP: align submap j -> submap i
            ICPResult icp = runICP(submaps[j], submaps[i], init_guess, 0.03, 100, 0.08);
            std::cout << "ICP " << j << "->" << i << " converged=" << icp.converged
                      << " fitness=" << icp.fitness << std::endl;

            // heuristic acceptance: converged + fitness small + reasonable translation
            if (icp.converged && icp.fitness < 0.01) {
                Pose3 between = eigen4dToPose3(icp.transform);

                // Add the loop to fullGraph (so LM later has it)
                fullGraph.add(BetweenFactor<Pose3>(Symbol('X', i), Symbol('X', j), between, loopNoise));

                // Also add incrementally to ISAM2 (online fusion)
                NonlinearFactorGraph newFactor;
                newFactor.add(BetweenFactor<Pose3>(Symbol('X', i), Symbol('X', j), between, loopNoise));
                Values emptyInit;
                isam.update(newFactor, emptyInit);

                std::cout << "[ISAM2] added loop factor " << i << " <-> " << j << "\n";
            }
        }
    }

    // ISAM2 estimate after incremental loops
    Values isamEstimate = isam.calculateEstimate();
    std::cout << "\nISAM2 estimate (before final LM):\n";
    for (int i = 0; i < N; ++i) {
        Pose3 p = isamEstimate.at<Pose3>(Symbol('X', i));
        std::cout << "X" << i << ": " << p.translation().transpose() << "\n";
    }

    // --- 5. Final batch Levenberg-Marquardt optimization (global refinement) ---
    // Use ISAM2 estimate as initial guess for LM (good practice)
    LevenbergMarquardtParams lmParams;
    // ---- Configure LM params (lambda / damping) ----
    lmParams.setVerbosityLM("SUMMARY");     // print lambda summary
    lmParams.verbosity = NonlinearOptimizerParams::Verbosity::ERROR;
    lmParams.lambdaInitial = 1e-3;         // initial damping
    lmParams.lambdaFactor = 2.0;           // factor to multiply/divide lambda
    lmParams.lambdaUpperBound = 1e6;       // upper bound
    lmParams.lambdaLowerBound = 1e-10;     // lower bound
    lmParams.maxIterations = 200;
    lmParams.relativeErrorTol = 1e-6;
    lmParams.absoluteErrorTol = 1e-8;
    // -----------------------------------------------

    // Build initial values for LM from isam estimate
    Values lmInit;
    for (int i = 0; i < N; ++i) {
        lmInit.insert(Symbol('X', i), isamEstimate.at<Pose3>(Symbol('X', i)));
    }

    std::cout << "\nRunning Levenberg-Marquardt (global) ...\n";
    LevenbergMarquardtOptimizer optimizer(fullGraph, lmInit, lmParams);
    Values lmResult = optimizer.optimize();

    std::cout << "\nLM optimized poses (final):\n";
    for (int i = 0; i < N; ++i) {
        Pose3 p = lmResult.at<Pose3>(Symbol('X', i));
        std::cout << "X" << i << ": " << p.translation().transpose() << "\n";
    }

    std::cout << "Demo finished.\n";
    return 0;
}

** CMakeLists.txt**

cmake_minimum_required(VERSION 3.10)
project(icp_isam2_lm_demo)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_BUILD_TYPE Release)

find_package(PCL REQUIRED)
find_package(GTSAM REQUIRED)
find_package(Eigen3 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

include_directories(${GTSAM_INCLUDE_DIRS})
link_directories(${GTSAM_LIBRARY_DIRS})

add_executable(icp_isam2_lm icp_isam2_lm.cpp)

target_link_libraries(icp_isam2_lm
    ${PCL_LIBRARIES}
    ${GTSAM_LIBRARIES}
)

编译与运行步骤

安装依赖（示例基于 Ubuntu）：

sudo apt update
sudo apt install -y libpcl-dev libeigen3-dev
# GTSAM: 推荐从源码编译并安装或使用系统包（若可用）

将 icp_isam2_lm.cpp 与 CMakeLists.txt 放在同一目录 project/ 下：
```
mkdir build && cd build
cmake ..
make -j4
```
运行（示例使用内置合成数据）：
```
./icp_isam2_lm
```

说明与调参建议

ICP 阈值：fitness 与 max_correspondence_distance、voxel_leaf 密切相关。真实数据需要多尺度 ICP 与法向量/点到平面方法，并做 RANSAC 验证误匹配。
ISAM2 参数：ISAM2Params 中 relinearizeThreshold、relinearizeSkip 可控制重线性化频率；对实时性/精度平衡有影响。
LM 阻尼（lambda）：示例中 lambdaInitial = 1e-3，若初始估计很差可增大到 1.0 或 10；lambdaFactor 决定失败时放大速度（2 或 5 都常用）。
鲁棒核：若担心错误闭环，给 loop factor 使用鲁棒损失（如 Huber）可以降低错误闭环影响。
性能：对大量子图并行做 ICP，加速配对；对非常大图，用分层/稀疏化策略或稀疏 LM 求解器（CHOLMOD）提高性能。