0. 前言
在了解SLAM的原理、流程后，个人经常实时困惑该如何去从零开始去设计编写一套能够符合我们需求的SLAM框架。作者认为Ceres、Eigen、Sophus、G2O这几个函数库无法避免，尤其是Ceres函数库在激光SLAM和V-SLAM的优化中均有着大量的应用。所以作者从Ceres作为开端，来对手写SLAM开个头，来方便各位后续的开发。
这里分享以为博主写的博客，个人看了感觉写的不错，对SLAM的剖析很细致
1.Ceres示例
首先我们需要明确Ceres函数库在SLAM中主要起到的作用是优化。目前Bundle Adjustment 其本质还是离不开最小二乘原理(几乎所有优化问题其本质都是最小二乘),目前Bundle Adjustment 优化框架最为代表的是Ceres solver和G2O(这里主要介绍ceres solver)。Ceres中的优化需要四步，构建优化的残差函数，构建优化问题，在每次获取到数据后添加残差块，总体优化。
构建残差函数：
//构建代价函数结构体，residual为残差。
//last_point_a_为这一帧中的点a，curr_point_b_为点a旋转后和上一帧里最近的点
//curr_point_c_为点b同线或上线号的点，curr_point_d_为点b下线号的点
//b，c，d与a点距离不超过3m
//plane_norm为根据向量bc和bd求出的法向量
struct CURVE_FITTING_COST
{
//类似构造函数
CURVE_FITTING_COST(Eigen::Vector3d _curr_point_a_, Eigen::Vector3d _last_point_b_,
Eigen::Vector3d _last_point_c_, Eigen::Vector3d _last_point_d_):
curr_point_a_(_curr_point_a_),last_point_b_(_last_point_b_),
last_point_c_(_last_point_c_),last_point_d_(_last_point_d_)
{
plane_norm = (last_point_d_ - last_point_b_).cross(last_point_c_ - last_point_b_);
plane_norm.normalize();
}
template <typename T>
//plane_norm点乘向量ab为a点距面bcd的距离，即残差
bool operator()(const T* q,const T* t,T* residual)const
{
Eigen::Matrix<T, 3, 1> p_a_curr{T(curr_point_a_.x()), T(curr_point_a_.y()), T(curr_point_a_.z())};
Eigen::Matrix<T, 3, 1> p_b_last{T(last_point_b_.x()), T(last_point_b_.y()), T(last_point_b_.z())};
Eigen::Quaternion<T> rot_q{q[3], q[0], q[1], q[2]};
Eigen::Matrix<T, 3, 1> rot_t{t[0], t[1], t[2]};
Eigen::Matrix<T, 3, 1> p_a_last;
p_a_last=rot_q * p_a_curr + rot_t;
residual[0]=abs((p_a_last - p_b_last).dot(plane_norm));
return true;
}
const Eigen::Vector3d curr_point_a_,last_point_b_,last_point_c_,last_point_d_;
Eigen::Vector3d plane_norm;
};
构建优化问题：
//优化问题构建
ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
ceres::LocalParameterization *q_parameterization =
new ceres::EigenQuaternionParameterization();
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
problem.AddParameterBlock(para_q, 4, q_parameterization);
problem.AddParameterBlock(para_t, 3);
每次求出abcd点后，将他们的坐标构建成Eigen::Vector3d数据，添加残差块：
Eigen::Vector3d curr_point_a(laserCloudIn_plane.points[i].x,
laserCloudIn_plane.points[i].y,
laserCloudIn_plane.points[i].z);
Eigen::Vector3d last_point_b(laserCloudIn_plane_last.points[closestPointInd].x,laserCloudIn_plane_last.points[closestPointInd].y,
laserCloudIn_plane_last.points[closestPointInd].z);
Eigen::Vector3d last_point_c(laserCloudIn_plane_last.points[minPointInd2].x,
laserCloudIn_plane_last.points[minPointInd2].y,
laserCloudIn_plane_last.points[minPointInd2].z);
Eigen::Vector3d last_point_d(laserCloudIn_plane_last.points[minPointInd3].x,
laserCloudIn_plane_last.points[minPointInd3].y,
laserCloudIn_plane_last.points[minPointInd3].z);
problem.AddResidualBlock(new ceres::AutoDiffCostFunction<CURVE_FITTING_COST,1,4,3>
(new CURVE_FITTING_COST(last_point_a,curr_point_b,
curr_point_c,curr_point_d)),loss_function,para_q,para_t);
遍历过所有的a点后，就可以优化求解了。
//所有前一帧里的点都当a点遍历过后，进行优化
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
//迭代数
options.max_num_iterations = 5;
//进度是否发到STDOUT
options.minimizer_progress_to_stdout = false;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
而在V-SLAM中Ceres则用的更多，我们可以看下下面的例子
double para_Pose[7];
para_Pose[0] = 0.0;
para_Pose[1] = 0.0;
para_Pose[2] = 0.0;
para_Pose[6] = 1.0;
para_Pose[3] = 0.0;
para_Pose[4] = 0.0;
para_Pose[5] = 0.0;
int kNumObservations = cur_pts.size();
double invDepth[kNumObservations][1];
ceres::LossFunction *loss_function;
//loss_function = new ceres::HuberLoss(1.0);
loss_function = new ceres::CauchyLoss(1.0);// 柯西核函数
ceres::LocalParameterization *local_parameterizationP = new PoseLocalParameterization();
problem.AddParameterBlock(para_Pose, 7, local_parameterizationP);//对Pose重新参数化
for (int i = 0; i < kNumObservations; ++i) {
invDepth[i][0] = 1;
problem.AddParameterBlock(invDepth[i], 1); //对深度重新参数化
if (!invdepths.empty()&&invdepths[i]>0){
// cout << "depth observations "<< 1./invdepths[i] <<" "<< invdepths[i] <<endl;
invDepth[i][0] = invdepths[i];
problem.SetParameterBlockConstant(invDepth[i]);//把任何参数块设为常数，并且使用SetParameterBlockVariable()来撤销这一操作
ceres::CostFunction *f_d;
//自动求导方法，AutoDiffCostFunction
f_d = new ceres::AutoDiffCostFunction<autoIvDepthFactor, 1,1>(
new autoIvDepthFactor(invdepths[i]) );
problem.AddResidualBlock(f_d, loss_function, invDepth[i]);
}
ceres::CostFunction *f;
f = new ceres::AutoDiffCostFunction<autoMonoFactor, 3,7,1>(
new autoMonoFactor(Vector3d(un_prev_pts[i].x, un_prev_pts[i].y, 1),Vector3d(un_cur_pts[i].x, un_cur_pts[i].y, 1)) );
problem.AddResidualBlock(f, loss_function, para_Pose,invDepth[i]);
}
ceres::Solver::Options options;
// options.max_num_iterations = 7;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.trust_region_strategy_type = ceres::DOGLEG;
options.minimizer_progress_to_stdout = false;
ceres::Solver::Summary summary;
TicToc solveTime;
ceres::Solve(options, &problem, &summary);
2. Ceres常见函数总结
1、创建优化问题与损失核函数
用于处理参数中含有野值的情况，避免错误量测对估计的影响，常用参数包括HuberLoss、CauchyLoss等；该参数可以取NULL或nullptr，此时损失函数为单位函数。
ceres::Problem problem;
ceres::LossFunction *loss_function; // 损失核函数
//loss_function = new ceres::HuberLoss(1.0);
loss_function = new ceres::CauchyLoss(1.0); // 柯西核函数
2、在创建的problem中添加优化问题的优化变量 problem.AddParameterBlock()，该函数常用的重载有两个
用户在调用 AddResidualBlock( ) 时其实已经隐式地向Problem传递了参数模块，但在一些情况下，需要用户显示地向Problem传入参数模块（通常出现在需要对优化参数进行重新参数化的情况）。Ceres提供了Problem::AddParameterBlock( )函数用于用户显式传递参数模块：
void AddParameterBlock(double* values, int size);
void AddParameterBlock(double* values,
int size,
LocalParameterization* local_parameterization);
注：values表示优化变量，size表示优化变量的维度。
其中，第一种函数原型除了会增加一些额外的参数检查之外，功能上和隐式传递参数并没有太大区别。第二种函数原型则会额外传入LocalParameterization参数，用于重构优化参数的维数。
LocalParameterization是在优化Manifold(流形)上的变量时需要考虑的，Manifold上变量是过参数的，即Manifold上变量的维度大于其自由度。这会导致Manifold上变量各个量之间存在约束，如果直接对这些量求导、优化，那么这就是一个有约束的优化，实现困难。为了解决这个问题，在数学上对Manifold在当前变量值处形成的切空间求导，在切空间上优化，最后投影回Manifold。
对于SLAM问题，广泛遇到的Manifold是旋转，旋转仅需要3个量，但实际运用中涉及到万向锁问题，在更高维空间表达旋转，四元数就是在维度4表达3个自由度的三维空间的旋转。
bool ComputeJacobian()计算得到一个4*3的矩阵(global_to_local)，含义是Manifold上变量对Tangent Space上变量的导数，在ceres::CostFunction处提供residuals对Manifold上变量的倒数，乘以这个矩阵，之后变就变成了对Tangent Space上变量的导数。
problem.AddParameterBlock(quaternion, 4);// 直接传递4维参数
ceres::LocalParameterization* local_param = new ceres::QuaternionParameterization();
problem.AddParameterBlock(quaternion, 4, local_param)//重构参数，优化时实际使用的是3维的等效旋转矢量
3、添加残差块
一个优化问题可以看成通过调整参数将一大堆各种各样的残差降到最小，因此，残差的提供是至关重要的，一个残差的构建离不开残差的数学定义以及关联的参数，ceres添加残差块通过 AddResidualBlock() 完成 ， 有两个重载貌似最为常用
template <typename... Ts>
ResidualBlockId AddResidualBlock(CostFunction* cost_function,
LossFunction* loss_function,
double* x0,
Ts*... xs)
ResidualBlockId AddResidualBlock(
CostFunction* cost_function,
LossFunction* loss_function,
const std::vector<double*>& parameter_blocks);
也就是需要提供三种参数 —— cost_function对象、鲁棒核函数对象、 该残差的关联参数 。
代价函数：包含了参数模块的维度信息，内部使用仿函数定义误差函数的计算方式。AddResidualBlock( )函数会检测传入的参数模块是否和代价函数模块中定义的维数一致，维度不一致时程序会强制退出。代价函数模块的详解参见Ceres详解（二） CostFunction。
损失函数：用于处理参数中含有野值的情况，避免错误量测对估计的影响，常用参数包括HuberLoss、CauchyLoss等（完整的参数列表参见Ceres API文档）；该参数可以取NULL或nullptr，此时损失函数为单位函数。
参数模块：待优化的参数，可一次性传入所有参数的指针容器vector<double*>或依次传入所有参数的指针double*</double*>。
其中重点是cost_function对象的给定，它有三种常见的提供方式：
- 自动导数（AutoDiffCostFunction）：由ceres自行决定导数的计算方式，最常用的求导方式。
数值导数（NumericDiffCostFunction）：由用户手动编写导数的数值求解形式，通常在残差函数的计算使用无法直接 - 调用的库函数，导致调用AutoDiffCostFunction类构建时使用；但手动编写的精度和计算效率不如模板类，因此不到不得已，官方并不建议使用该方法。
- 解析导数（Analytic
Derivatives）：当导数存在闭合解析形式时使用，用于可基于CostFunciton基类自行编写；但由于需要自行管理残差和雅克比矩阵，除非闭合解具有具有明显的精度和效率优势，否则同样不建议使用。
例如loam-livox 的ICP的ceres 自动求导实现：
// p2p with motion deblur 点-点 ICP
template <typename _T>
struct ceres_icp_point2point_mb
{
Eigen::Matrix<_T, 3, 1> m_current_pt; // 当前的点
Eigen::Matrix<_T, 3, 1> m_closest_pt; // 最近的点
_T m_motion_blur_s; // 用于畸变去除
// 上一次变换
Eigen::Matrix<_T, 4, 1> m_q_last;
Eigen::Matrix<_T, 3, 1> m_t_last;
_T m_weigh;
ceres_icp_point2point_mb( const Eigen::Matrix<_T, 3, 1> current_pt,
const Eigen::Matrix<_T, 3, 1> closest_pt,
const _T &motion_blur_s = 1.0,
Eigen::Matrix<_T, 4, 1> q_s = Eigen::Matrix<_T, 4, 1>( 1, 0, 0, 0 ),
Eigen::Matrix<_T, 3, 1> t_s = Eigen::Matrix<_T, 3, 1>( 0, 0, 0 ) ) : m_current_pt( current_pt ),
m_closest_pt( closest_pt ),
m_motion_blur_s( motion_blur_s ),
m_q_last( q_s ),
m_t_last( t_s )
{
m_weigh = 1.0;
};
// operator() 重载计算残差，通过输入的参数，并返回
template <typename T>
bool operator()( const T *_q, const T *_t, T *residual ) const
{
// 上一次的变换
Eigen::Quaternion<T> q_last{ ( T ) m_q_last( 0 ), ( T ) m_q_last( 1 ), ( T ) m_q_last( 2 ), ( T ) m_q_last( 3 ) };
Eigen::Matrix<T, 3, 1> t_last = m_t_last.template cast<T>();
// 畸变去除
Eigen::Quaternion<T> q_incre{ _q[ 3 ], _q[ 0 ], _q[ 1 ], _q[ 2 ] };
Eigen::Matrix<T, 3, 1> t_incre{ _t[ 0 ], _t[ 1 ], _t[ 2 ] };
Eigen::Quaternion<T> q_interpolate = Eigen::Quaternion<T>::Identity().slerp( ( T ) m_motion_blur_s, q_incre );
Eigen::Matrix<T, 3, 1> t_interpolate = t_incre * T( m_motion_blur_s );
// 当前的点
Eigen::Matrix<T, 3, 1> pt{ T( m_current_pt( 0 ) ), T( m_current_pt( 1 ) ), T( m_current_pt( 2 ) ) };
// 当前点经过变换后的位置
Eigen::Matrix<T, 3, 1> pt_transfromed;
pt_transfromed = q_last * ( q_interpolate * pt + t_interpolate ) + t_last;
residual[ 0 ] = ( pt_transfromed( 0 ) - T( m_closest_pt( 0 ) ) ) * T( m_weigh );
residual[ 1 ] = ( pt_transfromed( 1 ) - T( m_closest_pt( 1 ) ) ) * T( m_weigh );
residual[ 2 ] = ( pt_transfromed( 2 ) - T( m_closest_pt( 2 ) ) ) * T( m_weigh );
return true;
};
static ceres::CostFunction *Create( const Eigen::Matrix<_T, 3, 1> current_pt,
const Eigen::Matrix<_T, 3, 1> closest_pt,
const _T motion_blur_s = 1.0,
Eigen::Matrix<_T, 4, 1> q_s = Eigen::Matrix<_T, 4, 1>( 1, 0, 0, 0 ),
Eigen::Matrix<_T, 3, 1> t_s = Eigen::Matrix<_T, 3, 1>( 0, 0, 0 ) )
{
return ( new ceres::AutoDiffCostFunction< // 自动求导
ceres_icp_point2point_mb, 3, 4, 3>( // 对应的是operator中的维度
new ceres_icp_point2point_mb( current_pt, closest_pt, motion_blur_s ) ) );
}
};
其中重载操作符()（必有）
操作符()是一个模板方法，返回值为bool型，接受参数依次为待优化变量和残差变量，且待优化变量的传入方式应和Problem::AddResidualBlock()一致。
4.其他成员函数
// 设定对应的参数模块在优化过程中保持不变
void Problem::SetParameterBlockConstant(double *values)
// 设定对应的参数模块在优化过程中可变
void Problem::SetParameterBlockVariable(double *values)
// 设定优化下界
void Problem::SetParameterLowerBound(double *values, int index, double lower_bound)
// 设定优化上界
void Problem::SetParameterUpperBound(double *values, int index, double upper_bound)
// 该函数紧跟在参数赋值后，在给定的参数位置求解Problem，给出当前位置处的cost、梯度以及Jacobian矩阵；
bool Problem::Evaluate(const Problem::EvaluateOptions &options,
double *cost, vector<double>* residuals,
vector<double> *gradient, CRSMatrix *jacobian)
5.求解
ceres::Solve(options, &problem, &summary);
3. 参考链接
https://blog.csdn.net/wzheng92/article/details/79874724
https://zhuanlan.zhihu.com/p/332237105
https://blog.csdn.net/unlimitedai/article/details/107701861
4. 参考链接
#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>
using namespace std;
// 代价函数的计算模型
struct CURVE_FITTING_COST {
CURVE_FITTING_COST(double x, double y) : _x(x), _y(y) {}
// 残差的计算
template<typename T>
bool operator()(
const T *const abc, // 模型参数，待优化的参数，有3维
T *residual) const {
residual[0] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2]); // y-exp(ax^2+bx+c) //残差，也就是代价函数的输出
return true;
}
const double _x, _y; // x,y数据
};
int main(int argc, char **argv) {
double ar = 1.0, br = 2.0, cr = 1.0; // 真实参数值
double ae = 2.0, be = -1.0, ce = 5.0; // 估计参数值
int N = 100; // 数据点
double w_sigma = 1.0; // 噪声Sigma值
double inv_sigma = 1.0 / w_sigma;
cv::RNG rng; // OpenCV随机数产生器
vector<double> x_data, y_data; // 数据
for (int i = 0; i < N; i++) {
double x = i / 100.0;
x_data.push_back(x);
y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
}
double abc[3] = {ae, be, ce};
// 构建最小二乘问题
ceres::Problem problem;
for (int i = 0; i < N; i++) {
problem.AddResidualBlock( // 向问题中添加误差项
// 使用自动求导，将定义的代价函数结构体传入。模板参数：误差类型，输出维度即残差的维度，输入维度即优化参数的维度，维数要与前面struct中一致
new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(
new CURVE_FITTING_COST(x_data[i], y_data[i])
),
nullptr, // 核函数，这里不使用，为空
abc // 待估计参数
);
}
// 配置求解器
ceres::Solver::Options options; // 这里有很多配置项可以填
options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY; // 增量方程如何求解
//options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = true; // 输出到cout
ceres::Solver::Summary summary; // 优化信息
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
ceres::Solve(options, &problem, &summary); // 开始优化，求解
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
// 输出结果
cout << summary.BriefReport() << endl; //输出优化的简要信息
cout << "estimated a,b,c = ";
for (auto a:abc) cout << a << " ";
cout << endl;
return 0;
}
cmakelists.txt:
cmake_minimum_required(VERSION 2.8)
project(gaussnewton)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIRS})
include_directories("/usr/include/eigen3")
set(SOURCE_FILES main.cpp)
add_executable(gaussnewton ${SOURCE_FILES})
target_link_libraries(gaussnewton ${OpenCV_LIBS})
target_link_libraries(gaussnewton ${CERES_LIBRARIES})
评论（0）
您还未登录，请登录后发表或查看评论