电子工程师技术服务社区
公告
登录
|
注册
首页
技术问答
厂商活动
正点原子
板卡试用
资源库
下载
文章
社区首页
文章
【7天搞定视觉SLAM】第五天——非线性优化(下)
分 享
扫描二维码分享
【7天搞定视觉SLAM】第五天——非线性优化(下)
slam
hero_chao
关注
发布时间: 2020-04-23
丨
阅读: 778
# 一、高斯牛顿法 ## 1.简介 什么是高斯牛顿法? > 高斯一牛顿迭代法(Gauss-Newton iteration method)是非线性回归模型中求回归参数进行最小二乘的一种迭代方法,该法使用泰勒级数展开式去近似地代替非线性回归模型,然后通过多次迭代,多次修正回归系数,使回归系数不断逼近非线性回归模型的最佳回归系数,最后使原模型的残差平方和达到最小。其直观思想是先选取一个参数向量的参数值β,若函数ft(Xt,β)在β0附近有连续二阶偏导数,则在β0的邻域内可近似地将ft(Xt,β)看作是线性,因而可近似地用线性最小二乘法求解 在上一章中我们讲解了牛顿法、梯度法等解决最小二乘估计的方法,这一章我主要讲解关于非线性优化的实践。让我们用代码加深对这个问题的理解,而不是仅仅明白数学的推导。 ![](https://cf05.ickimg.com/bbsimages/202004/7733fa8522310052195092dc15c0d416.png) ## 2.实践 关于高斯牛顿法,这里列举一个简单的例子帮助大家理解: ```cpp #include <iostream> #include <chrono> #include <opencv2/opencv.hpp> #include <Eigen/Core> #include <Eigen/Dense> using namespace std; using namespace Eigen; 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)); } // 开始Gauss-Newton迭代 int iterations = 100; // 迭代次数 double cost = 0, lastCost = 0; // 本次迭代的cost和上一次迭代的cost chrono::steady_clock::time_point t1 = chrono::steady_clock::now(); for (int iter = 0; iter < iterations; iter++) { Matrix3d H = Matrix3d::Zero(); // Hessian = J^T W^{-1} J in Gauss-Newton Vector3d b = Vector3d::Zero(); // bias cost = 0; for (int i = 0; i < N; i++) { double xi = x_data[i], yi = y_data[i]; // 第i个数据点 double error = yi - exp(ae * xi * xi + be * xi + ce); Vector3d J; // 雅可比矩阵 J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce); // de/da J[1] = -xi * exp(ae * xi * xi + be * xi + ce); // de/db J[2] = -exp(ae * xi * xi + be * xi + ce); // de/dc H += inv_sigma * inv_sigma * J * J.transpose(); b += -inv_sigma * inv_sigma * error * J; cost += error * error; } // 求解线性方程 Hx=b Vector3d dx = H.ldlt().solve(b); if (isnan(dx[0])) { cout << "result is nan!" << endl; break; } if (iter > 0 && cost >= lastCost) { cout << "cost: " << cost << ">=last cost: " << lastCost << ", break." << endl; break; } ae += dx[0]; be += dx[1]; ce += dx[2]; lastCost = cost; cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpose() << "\t\testimated params: " << ae << "," << be << "," << ce << endl; } 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 << "estimated abc=" << ae << ", " << be << ", " << ce << endl; return 0; } ``` # 二、函数拟合 ## 1.CERES库 什么是CERES库? > Ceres Solver是谷歌2010就开始用于解决优化问题的C++库,2014年开源.在Google地图,Tango项目,以及著名的SLAM系统OKVIS和Cartographer的优化模块中均使用了Ceres Solver. 在SLAM领域优化问题还可以使用g2o来求解.不过Ceres提供了自动求导功能,虽然是数值求导,但可以避免复杂的雅克比计算,目前来看Ceres相对于g2o的缺点仅仅是依赖的库多一些(g2o仅依赖Eigen).但是提供了可以直接对数据进行操作的能力,相对于g2o应用在视觉SLAM中,更加倾向于通用的数值优化,最重要的是提供的官方资料比较全(看g2o简直受罪...).详细的介绍可以参考google的http://ceres-solver.org/features.html 首先,我们先安装这个库: ```cpp wget https://github.com/ceres-solver/ceres-solver ``` 安装依赖: ```cpp sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3.1.2 libgflags-dev libgoogle-glog-dev libgtest-dev ``` ![](https://cf05.ickimg.com/bbsimages/202004/9e185487f2d41fd3513f36fd8937a439.png) ## 2.实践 ```cpp // // Created by xiang on 18-11-19. // #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.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; } ``` 这里使用这个库来拟合假设的曲线函数,希望大家可以分析一下代码,这里误差为一个维度。优化为三个维度。 # 小结 这里主要是让大家实践一下这个最小二乘问题的求解,通过代码可以帮助我们的理解,当然,优化的办法有很多种,大家也可以尝试一下使用图优化来求解这个问题,这里就不多做扩展,下一章,我会讲解一下slam问题的前端,视觉里程计的相关内容。 > 参考:《视觉SLAM十四讲》
原创作品,未经权利人授权禁止转载。详情见
转载须知
。
举报文章
点赞
(
0
)
hero_chao
关注
评论
(0)
登录后可评论,请
登录
或
注册
相关文章推荐
MK-米客方德推出工业级存储卡
Beetle ESP32 C3 蓝牙数据收发
Beetle ESP32 C3 wifi联网获取实时天气信息
开箱测评Beetle ESP32-C3 (RISC-V芯片)模块
正点原子数控电源DP100测评
DP100试用评测-----开箱+初体验
Beetle ESP32 C3环境搭建
【花雕体验】16 使用Beetle ESP32 C3控制8X32位WS2812硬屏之二
X
你的打赏是对原创作者最大的认可
请选择打赏IC币的数量,一经提交无法退回 !
100IC币
500IC币
1000IC币
自定义
IC币
确定
X
提交成功 ! 谢谢您的支持
返回
我要举报该内容理由
×
广告及垃圾信息
抄袭或未经授权
其它举报理由
请输入您举报的理由(50字以内)
取消
提交