Ceres库与位姿图优化
文章目录
-
- 前言
- Ceres库理论与实践
- 位姿图优化
- SLAM中的优化问题
- 小结
前言
SLAM中后端优化求解上ceres库位姿图优化有非常多的应用，这里记录一下自己的学习内容，主要参考B站的视频和CSDN的博客，推荐以下资料：
【非线性优化器ceres的使用20221125】 https://www.bilibili.com/video/BV1p24y1y7BL/?share_source=copy_web&vd_source=24db73a73cddacddda48febd1ffc28ef
【SLAM】Ceres优化库超详细解析，https://blog.csdn.net/qq_38410730/article/details/131439027
[代码实践] Ceres 学习记录（一）：2D 位姿图优化，https://xiaotaoguo.com/p/ceres-usage/
ceres位姿图优化，https://blog.csdn.net/weixin_42099090/article/details/106907271
slam14(1) v3_3 后端优化 BA位姿图优化，https://www.cnblogs.com/gooutlook/p/16646439.html
Ceres库理论与实践
Ceres是由Google开发的开源C++通用非线性优化库，与g2o并列为目前视觉SLAM中应用最广泛的优化算法库。Ceres库主要用于求解无约束或者有界约束的最小二乘问题。其数学形式如下：
使用 Ceres Solver求解非线性优化问题，主要包括以下几部分：
1、构建代价函数(cost function)或残差(residual)
2、构建优化问题(ceres::Problem)：通过 AddResidualBlock 添加代价函数(cost function)、损失函数(loss function核函数) 和 待优化状态量
3、配置求解器(ceres::Solver::Options)
4、运行求解器(ceres::Solve(options, &problem, &summary))
例子1：求解根号2的值是多少
#include 位姿图优化
BA优化时间跟特征点数量有关，特征点数量越多BA消耗时间越长。折衷做法是，在进行几次优化后，将特征点位置固定，不再优化特征点，只优化相机位姿，即位姿图优化。
为了降低问题的计算复杂度，位姿图方法将原始测量值抽象出来。具体来说，它创建了一个表示机器人姿态的节点图，以及表示两个节点之间的相对变换（增量位置和方向）的边。
通过李群李代数的知识，我们知道优化问题的相关数学表达如下：
相对的位姿关系：
通过对积几何（平面投影）得到位姿变化1，又通过上式得到位姿变化2，我们要做的，就是减少位姿变化1与位姿变化1的误差，这是一个最小二乘问题。
为了优化两个位姿，求其偏导数：
所有的位姿顶点和位姿边构成了一个图优化，本质上是一个最小二乘问题，优化变量为各个顶点的位姿，边来自于位姿观测约束，则总体的目标函数为
信息矩阵用来代表边的不确定度，信息矩阵越大代表这条边在优化的过程中越重要，图优化中每一条边的信息矩阵为测量协方差矩阵的逆。
例子2：slambook2/ch10g2o位姿图优化实践例子改成使用ceres进行位姿图优化。
#include CMakeLists.txt：
cmake_minimum_required(VERSION 3.0)
project(ceres_use1)
set(CMAKE_CXX_FLAGS "-std=c++11")
set(CMAKE_BUILD_TYPE Debug)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
find_package(Ceres REQUIRED)
find_package(Sophus REQUIRED)
include_directories(${CERES_INCLUDE_DIRS} ${Sophus_INCLUDE_DIRS})
#Eigen库只需要添加头文件
include_directories("/usr/include/eigen3")
#add_executable要在target_link_libraries
add_executable(ceres_use1 ceres_use1.cpp)
add_executable(pose_graph pose_graph_ceres.cpp)
#target_link_libraries中只需要添加ceres和opencv
target_link_libraries(ceres_use1 ${CERES_LIBRARIES})
target_link_libraries(pose_graph ${CERES_LIBRARIES})
target_link_libraries(pose_graph ${Sophus_LIBS} fmt)
SLAM中的优化问题
在 优化位姿 时，其思想是构造一个关于位姿变化的误差函数，当这个误差函数最小时，认为此时估计的位姿最优。视觉SLAM主要分为 直接法 和 特征点法，但无论是直接法还是特征点法，位姿的迭代优化都是求解一个最小二乘问题。
直接法最小化光度误差，即前后帧像素的灰度误差，特征点法最小化重投影误差，即地图点到当前图像投影点与匹配点的坐标误差。
基于李代数进行视觉SLAM位姿优化时，可以得到：
残差（预测值 - 观测值）
雅克比矩阵
代价函数：
// 重投影误差，残差2维的，优化变量是6维度
class BAGNCostFunctor : public ceres::SizedCostFunction<2, 6> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW //对齐
// 第一个是像素观测值，第二个是准备用来重投影的三维点
BAGNCostFunctor(Eigen::Vector2d observed_p, Eigen::Vector3d observed_P) :
observed_p_(observed_p), observed_P_(observed_P) {}
virtual ~BAGNCostFunctor() {}
// 输入参数块、残差、雅可比矩阵
virtual bool Evaluate(double const* const* parameters,
double *residuals, double **jacobians) const {
// 获取位姿，并将三维点进行重投影到成像平面
Eigen::Map<const Eigen::Matrix<double, 6, 1> > T_se3(*parameters);
Sophus::SE3 T_SE3 = Sophus::SE3::exp(T_se3);
Eigen::Vector3d Pc = T_SE3 * observed_P_;
// 内参矩阵
Eigen::Matrix3d K;
double fx = 520.9, fy = 521.0, cx = 325.1, cy = 249.7;
K << fx, 0, cx, 0, fy, cy, 0, 0, 1;
// 计算残差
Eigen::Vector2d residual = observed_p_ - (K * Pc).hnormalized();
residuals[0] = residual[0];
residuals[1] = residual[1];
if(jacobians != NULL) {
if(jacobians[0] != NULL) {
// 2*6的雅克比矩阵
Eigen::Map<Eigen::Matrix<double, 2, 6, Eigen::RowMajor> > J(jacobians[0]);
double x = Pc[0];
double y = Pc[1];
double z = Pc[2];
double x2 = x * x;
double y2 = y * y;
double z2 = z * z;
J(0,0) = fx / z;
J(0,1) = 0;
J(0,2) = -fx * x / z2;
J(0,3) = -fx * x * y / z2;
J(0,4) = fx + fx * x2 / z2;
J(0,5) = -fx * y / z;
J(1,0) = 0;
J(1,1) = fy / z;
J(1,2) = -fy * y / z2;
J(1,3) = -fy - fy * y2 / z2;
J(1,4) = fy * x * y / z2;
J(1,5) = fy * x / z;
}
}
return true;
}
private:
const Eigen::Vector2d observed_p_;
const Eigen::Vector3d observed_P_;
};
优化问题：
Sophus::Vector6d se3;
// 在当前problem中添加代价函数残差块，损失函数为NULL采用默认的最小二乘误差即L2范数，优化变量为 se3
ceres::Problem problem;
for(int i = 0; i < n_points; ++i) {
ceres::CostFunction *cost_function;
cost_function = new BAGNCostFunctor(p2d[i], p3d[i]);
problem.AddResidualBlock(cost_function, NULL, se3.data());
}
ceres::Solver::Options options;
options.dynamic_sparsity = true;
options.max_num_iterations = 100; // 迭代100次
options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE;
options.minimizer_type = ceres::TRUST_REGION;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.trust_region_strategy_type = ceres::DOGLEG;
options.minimizer_progress_to_stdout = true;
options.dogleg_type = ceres::SUBSPACE_DOGLEG;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << "\n";
std::cout << "estimated pose: \n" << Sophus::SE3::exp(se3).matrix() << std::endl;
小结
Cere库在SLAM中的使用非常广泛，需要更多地结合代码来看，还需要补充图优化的相关理论知识，进行理论实践。目前我对于位姿图优化中的信息矩阵还有点迷糊，下周要完成一次图优化知识学习的博客！