在manif项目中使用Ceres求解器进行流形优化-CSDN博客

本文链接：https://blog.csdn.net/gitblog_01159/article/details/148971769

在manif项目中使用Ceres求解器进行流形优化

前言

在机器人、计算机视觉和SLAM等领域，我们经常需要处理存在于特殊流形空间（如旋转矩阵、位姿变换等）的数据。manif项目为这些特殊流形提供了高效的C++实现，而Ceres Solver则是一个广泛使用的非线性优化库。本文将详细介绍如何将manif与Ceres结合使用，实现流形空间的高效优化。

流形优化的特殊性

流形空间与欧氏空间不同，它们具有特殊的拓扑结构和代数性质。例如，三维旋转群SO(3)中的元素不能简单地相加或相减，而需要使用特殊的运算规则。这种特性使得在流形空间上的优化问题需要特殊处理。

manif项目的一个关键特性是它直接计算关于局部切空间扰动的雅可比矩阵，这与Ceres Solver的工作方式有所不同。理解这一点对于正确使用这两个库至关重要。

Ceres Solver的工作机制

Ceres Solver处理流形优化问题时，将雅可比矩阵的计算分为两部分：

代价函数(CostFunction)：计算目标函数关于状态变量的导数
局部参数化(LocalParameterization)：处理状态更新并计算相应的雅可比矩阵

Ceres最终会将这两部分雅可比矩阵相乘，得到关于局部切空间扰动的完整雅可比矩阵。

manif与Ceres的集成方案

虽然manif直接提供了最终的雅可比矩阵，而Ceres需要分开的两部分，但我们仍然可以通过以下方式实现两者的无缝集成：

1. 通用LocalParameterization实现

manif提供了一个模板化的CeresLocalParameterization类，可以适用于任何李群：

template <typename _LieGroup>
class CeresLocalParameterization {
  // 实现状态更新和雅可比计算
  // 适用于SO2、SE2、SO3、SE3等各种流形
};

这个模板类封装了流形上的加法操作（即retraction操作），使得Ceres能够正确地更新流形上的状态变量。

2. 预定义的辅助函数

manif提供了两个方便的辅助函数来简化与Ceres的集成：

make_objective_autodiff()：创建Ceres代价函数
make_manifold_autodiff()：创建Ceres局部参数化

实际应用示例

让我们看一个在SE(2)流形上计算平均值的完整示例：

// 创建Ceres问题
ceres::Problem problem;

// 创建4个SE(2)上的目标点
auto obj1 = manif::make_objective_autodiff<SE2d>(3, 3, M_PI/4.);
auto obj2 = manif::make_objective_autodiff<SE2d>(3, 1, 3.*M_PI/8.);
// ... 其他目标点

// 初始化平均值
SE2d average_state(0,0,0);

// 添加残差块
problem.AddResidualBlock(obj1.get(), nullptr, average_state.data());
// ... 添加其他残差块

// 设置局部参数化
auto auto_diff_manifold = manif::make_manifold_autodiff<SE2d>();
problem.SetManifold(average_state.data(), auto_diff_manifold.get());

// 配置并运行求解器
ceres::Solver::Options options;
options.minimizer_progress_to_stdout = true;

ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);

// 输出结果
std::cout << "Average state:\n" << average_state << "\n";

这个例子展示了如何在SE(2)流形上计算多个位姿的平均值，这是SLAM和位姿图优化中的常见操作。