本文为视觉 SLAM 学习总结。
在给定观测数据 时,如何估计状态变量 ,即同时定位和建图
本讲内容概要
- 最小二乘法的含义和处理方式
- Gauss-Newton,Levenburg-Marquadt 等下降策略
- Ceres 库和 g2o 库的基本使用方法
状态估计问题
最大后验与最大似然
回顾 SLAM 方程:
其中
- 相机位姿:
- 像素:
- 噪声:
最简单的情况是: 都为线性函数,噪声服从高斯分布,这种情况在 CH10 中会讲到,使用 KEF 可以求得无偏的最优解。如果是非线性系统,和非高斯噪声,情况会变得很复杂,要分情况讨论。
以前通常用滤波器求解状态估计,该方法假设该系统具有马尔可夫性——系统下一时刻状态仅依赖于上一个状态。于是我们可以只维护上一个状态,当有新的输入进入时就更新一次状态估计。
近年来非线性优化已成为主流方法。
状态变量:所有待求解的量,即所有时刻的位姿和路标
状态估计实际上是求解条件分布:
,这里的
是统称。考虑最简单的情况,当只有观测时,该问题类似于求解一个 SfM(Structure from Motion) 问题。
由贝叶斯法则有:
其中
为似然,
为先验。
然而, 条件分布很难求解,但是我们可以求解:
-
最大后验估计(MAP)。因为分母与 无关直接省略。
-
最大似然估计(MLE)。此时先验 未知
其物理意义是“在哪种状态下,最容易产生当前的观测值”。
最小二乘的引出
在某次观测
,由于噪声是高斯的
,于是:
现在我们要求
的值,使得该高斯分布的值最大,即求解这个高斯分布的最大似然。因为高斯分布具有简单的性质,该问题易解。
一般的高斯分布:
观察到形式较为复杂,我们取负对数进行化简,乘法变为加法,指数变成了常数项:
此时,求原分布的最大值问题就转换为了求负对数的最小值问题。观察到第一项与
无关,则我们只需最小化后面的二次型。将二次型带入 SLAM 观测模型中有:
观察上式,我们相当于在最小化噪声项,在这里我们就定义误差:
则误差的平方和为:
直观的解释是,由于噪声的存在,我们估计的轨迹带入 SLAM 的方程中不能很好地成立,我们需要调整状态的估计,使得误差最小化。
该问题的结构:
- 由多个误差的平方和组成
- 虽然整体维度高,但每个误差项简单,仅与一两个状态变量有关
- 如果用李代数表达位姿,是无约束优化问题
至此,我们将一个最大似然问题转换成了非线性最小二乘问题。
非线性最小二乘
我们首先考虑一个最简单的最小二乘问题:
如果
形式十分简单,我们可以考虑对
求导:
得到导数为 0 的点,可能是极值点或鞍点,只需要逐个比较大小即可。而当
不便于求导或求导后不便于解出极值点时,我们通常用迭代的方法逼近最优解:
- 给定某个初始值
- 对于第 次迭代,寻找增量 ,使得 达到极小值
- 若 足够小则停止
- 否则更新 ,令 ,返回 2
其中最关键的问题是 如何确定?
一阶和二阶梯度法
我们将目标函数在
附近进行泰勒展开:
我们可以选择保留一阶项或二阶项,其分别对应了一阶和二阶梯度法。
如果保留一阶项,令增量方程导数为 0,增量的解为:
我们只需要沿着
的反梯度方向前进就可下降,若要下降地最快,还需要计算该方向上的步长
,这种方法称为最速下降法。
如果保留二阶项,令增量方程导数为 0,增量的解为:
该方法称为牛顿法。
一阶和二阶梯度下降法十分直观,但各自都有缺点:
- 最速下降法过于贪心,走出锯齿线路,反而增加了迭代次数
- 牛顿法要计算 矩阵,在问题规模较大时难以求解
高斯牛顿法
高斯牛顿法的思想是对
进行一阶泰勒展开,推导过程比较复杂,这里不做介绍,直接给出增量方程:
将左边系数定义为
,右边残差定义为
,则上式变为:
对比牛顿法,高斯牛顿法将
来近似牛顿法中的
矩阵,省略了计算
。我们在求解增量时只需求解当前的雅可比矩阵
和误差
。但
会出现不可逆的情况。
高斯牛顿法属于线搜索方法:先找方向,后确定步长。
裂纹伯格—马夸尔特方法
裂纹伯格—马夸尔特方法属于信赖区域方法:在信赖区域中,认为近似是有效的。
使用近似模型和实际函数的差异来进行近似程度的描述:
其中分子为实际下降,分母为近似模型的下降。
- 当 接近 1 时,近似是好的
- 太小时,近似较差,要缩小近似范围
- 太大时,实际下降更大,可以放大近似范围
该方法的框架为:
-
给定初始值 以及初始优化半径
-
对于第 次迭代,求解:
其中 为信赖区域半径, 通常取 ,相当于把 约束在球中,也可取非负数对角阵,约束在椭球中。 -
计算
-
若 ,则 ;若 ,则 ;为经验值。
-
如果 大于某阈值,则认为近似可行,更新
-
判断算法是否收敛。如果不收敛返回 2,否则结束
在求解增量方程时,牛顿法是直接对
求导,但这里我们有信赖区域的约束,因此需要用拉格朗日乘子将其转换为一个无约束的问题。我们直接写出增量方程:
在裂纹伯格—马夸尔特方法中,
取
,相当于求解:
通过观察上式发现,
实际为一二阶间的权重,增强了正定性。
小结
非线性优化的主要方法有:最速下降、牛顿、G-N、L-M、DogLeg 等。
与线性规划不同,非线性需要针对具体问题具体分析。
当问题非凸时,即目标函数有多个局部最优解,对初值敏感,如果初值离最优解过远可能会陷入局部最优解。
下面对两个实验进行讲解,两个实验求解同一个问题。
实践:Ceres
安装 Ceres
首先安装 Ceres,安装依赖项(如果缺少依赖项,在 cmake 时会有提示,补上重新 cmake 即可):
sudo apt-get install libcxsparse3.1.4 libsuitesparse-dev libeigen3-dev libgoogle-glog-dev libgtest-dev
因为 Ceres 中有 CMakeLists.txt,因此我们可以直接在新建的 build 文件夹中 cmake:
cmake ..
make -j6 # 加速,6替换为自己的CPU内核数
sudo make install
该过程较慢,耐心等待。安装结束后:
ls /usr/local/include/ceres/ # 头文件在这里
ls /usr/local/lib/libceres.a # 库在这里
Ceres 只有 1 个库。在工程目录下添加 cmake_modules/CeresConfig.cmake.in:
# 添加cmake模块以使用ceres库
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )
还需要在 CMakeLists.txt 中添加以下代码:
# 寻找Ceres库并添加它的头文件
find_package( Ceres REQUIRED )
include_directories( ${CERES_INCLUDE_DIRS} )
# 与Ceres和OpenCV链接
target_link_libraries( curve_fitting ${CERES_LIBRARIES} ${OpenCV_LIBS} )
Ceres 相关文档写的较为全面完善——http://ceres-solver.org/,原理也较为直观,在 Tutorial 中可进行系统学习。
实验:曲线拟合
设曲线方程为 。我们得到一些带噪声的样本数据 ,拟合曲线参数 。
Ceres 是一个规范的最小二乘求解器。只需定义出优化函数和优化变量,再选好求解器,可直接求解。Ceres 会自动求导,得到数值导数,但结果不一定是最好的(大部分情况是好的),想要得到最好的结果还是需要自己推出解析的导数。
编程实现
接下来我们编程实现。首先给出 的真实值,产生 100 个样本点,噪声是 0 均值的高斯噪声。在循环中生成样本点:
double a=1.0, b=2.0, c=1.0; // 真实参数值
int N=100; // 数据点
double w_sigma=1.0; // 噪声Sigma值
cv::RNG rng; // OpenCV随机数产生器
double abc[3] = {0,0,0}; // abc参数的估计值
vector<double> x_data, y_data; // 数据
cout<<"generating data: "<<endl;
for ( int i=0; i<N; i++ ){
double x = i/100.0;
x_data.push_back ( x );
y_data.push_back (
exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma ) // 表达式+噪声
);
cout<<x_data[i]<<" "<<y_data[i]<<endl;
}
然后构建最小二乘问题:
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 // 待估计参数,求导后变为优化后的值
);
}
其中,代价函数的计算模型:
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 // 残差
{ // 给定abc返回误差
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数据
};
接下来配置求解器:
ceres::Solver::Options options; // 这里有很多配置项可以填
options.linear_solver_type = ceres::DENSE_QR; // 增量方程如何求解,这里使用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;
编译运行后得到结果:
优化后代价函数明显减小,最终收敛,得到的结果与实际设定值相差小数点后第一位,我们画出图像:
可见两条曲线几乎重合,估计准确。
实践:g2o
安装 g2o
安装 g2o 是标准的 cmake 安装流程,十分简单。
首先安装依赖项(如果提示无法定位,就把库名写一半然后用 tab 补全):
sudo apt-get install libqt4-dev qt4-qmake libqglviewer-dev libcholmod3.0.6
然后在 github 下载安装包,并解压到本地:
git clone https://github.com/RainerKuemmerle/g2o.git
tar -zxvf g2o-master.zip
g2o 也有 CMakeLists.txt,可直接编译:
cd g2o
mkdir build
cd build
cmake ..
make -j6 # 加速,6改为自己的CPU内核数
sudo make install
安装完毕。
图优化理论
图优化是将优化问题用图表示,其中顶点表示优化变量,边表示误差项。
使用 g2o 拟合曲线时,待估计的参数构成顶点,观测数据构成边。
其中顶点为三维向量,边为一元边,即只连接一个顶点。
编程实现
噪声的产生与 Ceres 中相同。
构建图优化,先设定 g2o。选择优化方式相似,可选择 GN, LM, DogLeg:
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block; // 每个误差项优化变量维度为3,误差值维度为1
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器
//Block* solver_ptr = new Block( linearSolver ); // 矩阵块求解器
Block* solver_ptr = new Block( unique_ptr<Block::LinearSolverType>(linearSolver) );
// 梯度下降方法,从GN, LM, DogLeg 中选
//g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(unique_ptr<Block>(solver_ptr) );
// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
// g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm( solver ); // 设置求解器
optimizer.setVerbose( true ); // 打开调试输出
我们还要定义顶点和边,从 g2o 提供的类型派生,在里面定义顶点和边的行为:
// 曲线模型的顶点,模板参数:优化变量维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// 定义顶点和边的行
virtual void setToOriginImpl() // 重置
{
_estimate << 0,0,0;
}
virtual void oplusImpl( const double* update ) // 更新
{
_estimate += Eigen::Vector3d(update);
}
// 存盘和读盘:留空
virtual bool read( istream& in ) {}
virtual bool write( ostream& out ) const {}
};
然后是误差模型:
// 误差模型 模板参数:观测值维度,类型,连接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
// 计算曲线模型误差
void computeError(){
const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]); // 顶点
const Eigen::Vector3d abc = v->estimate(); // 估计值
// 误差
_error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) );
}
virtual bool read( istream& in ) {}
virtual bool write( ostream& out ) const {}
public:
double _x; // x 值, y 值为 _measurement
};
在图模型中增加顶点和边。共 100 个数据,对应 100 条边,只有一个待优化变量,只有一个顶点:
// 往图中增加顶点
CurveFittingVertex* v = new CurveFittingVertex();
// 添加前设id和估计值
v->setEstimate( Eigen::Vector3d(0,0,0) );
v->setId(0);
optimizer.addVertex( v );
// 往图中增加边
for ( int i=0; i<N; i++ ){
CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
// 添加前设id和估计值
edge->setId(i);
edge->setVertex( 0, v ); // 设置连接的顶点
edge->setMeasurement( y_data[i] ); // 观测数值
edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵:协方差矩阵之逆
optimizer.addEdge( edge );
}
执行优化:
cout<<"start optimization"<<endl;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.initializeOptimization();
optimizer.optimize(100);
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;
// 输出优化值
Eigen::Vector3d abc_estimate = v->estimate();
cout<<"estimated model: "<<abc_estimate.transpose()<<endl;
得到结果:
结果与 Ceres 相差不多。
详细了解两个库还需要参考官方文档进行学习。