视觉SLAM⼗四讲CH6代码解析及课后习题详解
gaussNewton.cpp
#include <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>//Eigen核⼼模块
#include <Eigen/Den>//Eigen稠密矩阵运算模块
using namespace std;
using namespace Eigen;
//⾼斯⽜顿法拟合曲线y = exp(a * x^2 + b * x + c)
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随机数产⽣器 RNG为OpenCV中⽣成随机数的类，全称是Random Number Generator
vector<double> x_data, y_data; // double数据x_data, y_data
for (int i = 0; i < N; i++) {
double x = i / 100.0;//相当于x范围是0-1
x_data.push_back(x);//x_data存储的数值
y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));//rng.gaussian(w_sigma * w_sigma)为opencv随机数产⽣⾼斯噪声
//rng.gaussian(val)表⽰⽣成⼀个服从均值为0，标准差为val的⾼斯分布的随机数视觉slam⼗四讲p133式6.38上⾯的表达式
}
// 开始Gauss-Newton迭代求ae，be和ce的值，使得代价最⼩
int iterations = 100; // 迭代次数
double cost = 0, lastCost = 0; // 本次迭代的cost和上⼀次迭代的cost cost表⽰本次迭代的代价，lastCost表⽰上次迭代的代价
//cost = error * error，error表⽰测量⽅程的残差
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();//std::chrono是c++11引⼊的⽇期处理库，其中包含三种时钟（system_clock,steady_clock,high_ //t1表⽰steady_clock::time_point类型
for (int iter = 0; iter < iterations; iter++) {
Matrix3d H = Matrix3d::Zero(); // Hessian = J^T W^{-1} J in Gauss-Newton 将矩阵H初始化为3*3零矩阵，表⽰海塞矩阵，H = J * (sigma * sigma).transpo() * J. //（视觉slam⼗四讲p133式6.41左边）
Vector3d b = Vector3d::Zero(); // bias 将b初始化为3*1零向量，b = -J * (sigma * sigma).transpo() * error，error表⽰测量⽅程的残差（视觉slam⼗四讲p133式6 cost = 0;
//遍历所有数据点计算H，b和cost
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);//视觉slam⼗四讲p133式6.39
Vector3d J; // 雅可⽐矩阵
J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce); // de/da 视觉slam⼗四讲p133式6.40 第⼀个
J[1] = -xi * exp(ae * xi * xi + be * xi + ce); // de/db 视觉slam⼗四讲p133式6.40 第⼆个
J[2] = -exp(ae * xi * xi + be * xi + ce); // de/dc 视觉slam⼗四讲p133式6.40 第三个
H += inv_sigma * inv_sigma * J * J.transpo();//视觉slam⼗四讲p133式6.41左边求和
b += -inv_sigma * inv_sigma * error * J;//视觉slam⼗四讲p133式6.41右边求和
cost += error * error;//残差平⽅和
}
// 求解线性⽅程 Hx=b
Vector3d dx = H.ldlt().solve(b); //ldlt()表⽰利⽤Cholesky分解求dx
if (isnan(dx[0]))//isnan()函数判断输⼊是否为⾮数字，是⾮数字返回真，nan全称为not a number
{
cout << "result is nan!" << endl;
break;
}
}
if (iter > 0 && cost >= lastCost) //因为iter要⼤于0，第1次迭代(iter = 0, cost > lastCost)不执⾏！
{
cout << "cost: " << cost << ">= last cost: " << lastCost << ", break." << endl;
break;
}
//更新优化变量ae，be和ce！
ae += dx[0];
be += dx[1];
ce += dx[2];
lastCost = cost; //更新上⼀时刻代价
cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpo() <<
"\t\testimated params: " << ae << "," << be << "," << ce << endl;
}
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_ud = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "solve time cost = " << unt() << " conds. " << endl;
cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
return 0;
}
<
cmake_minimum_required(VERSION 2.8)
project(ch6)
t(CMAKE_BUILD_TYPE Relea)
t(CMAKE_CXX_FLAGS "-std=c++14 -O3")
list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
# OpenCV
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
# Ceres
find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIRS})
# g2o
find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})
# Eigen
include_directories("/usr/include/eigen3")
add_executable(gaussNewton gaussNewton.cpp)
target_link_libraries(gaussNewton ${OpenCV_LIBS})
add_executable(ceresCurveFitting ceresCurveFitting.cpp)
target_link_libraries(ceresCurveFitting ${OpenCV_LIBS} ${CERES_LIBRARIES})
add_executable(g2oCurveFitting g2oCurveFitting.cpp)
target_link_libraries(g2oCurveFitting ${OpenCV_LIBS} ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY}) 执⾏结果：
total cost: 3.19575e+06, update: 0.0455771 0.078164 -0.985329 estimated params: 2.04558,-0.921836,4.01467
total cost: 376785, update: 0.065762 0.224972 -0.962521 estimated params: 2.11134,-0.696864,3.05215
total cost: 35673.6, update: -0.0670241 0.617616 -0.907497 estimated params: 2.04432,-0.0792484,2.14465
total cost: 2195.01, update: -0.522767 1.19192 -0.756452 estimated params: 1.52155,1.11267,1.3882
total cost: 174.853, update: -0.537502 0.909933 -0.386395 estimated params: 0.984045,2.0226,1.00181
total cost: 102.78, update: -0.0919666 0.147331 -0.0573675 estimated params: 0.892079,2.16994,0.944438
total cost: 101.937, update: -0.00117081 0.00196749 -0.00081055 estimated params: 0.890908,2.1719,0.943628
total cost: 101.937, update: 3.4312e-06 -4.28555e-06 1.08348e-06 estimated params: 0.890912,2.1719,0.943629
total cost: 101.937, update: -2.01204e-08 2.68928e-08 -7.86602e-09 estimated params: 0.890912,2.1719,0.943629
cost: 101.937>= last cost: 101.937, break.
solve time cost = 0.00112835 conds.
estimated abc = 0.890912, 2.1719, 0.943629
ceresCurveFitting.cpp
#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>//ceres库头⽂件
#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) residual表⽰残差
return true;
//返回bool类型，计算结果已经存⼊函数外的residual变量中
}
const double _x, _y; // x,y数据结构体CURVE_FITTING_COST中的成员变量
};
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值初始化为1
double inv_sigma = 1.0 / w_sigma; //标准差的逆
cv::RNG rng; // OpenCV随机数产⽣器 RNG为OpenCV中⽣成随机数的类，全称是Random Number Generator
vector<double> x_data, y_data; // 数据 // double数据x_data, y_data
for (int i = 0; i < N; i++) {
double x = i / 100.0;//相当于x范围是0-1
x_data.push_back(x);//x_data存储的数值所给的100个观测点数据
y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
//rng.gaussian(w_sigma * w_sigma)为opencv随机数产⽣⾼斯噪声
//rng.gaussian(val)表⽰⽣成⼀个服从均值为0，标准差为val的⾼斯分布的随机数视觉slam⼗四讲p133式6.38上⾯的表达式
}
double abc[3] = {ae, be, ce};//定义优化变量
// 构建最⼩⼆乘问题
ceres::Problem problem;//定义⼀个优化问题类problem
for (int i = 0; i < N; i++) {
problem.AddResidualBlock( // 向问题中添加误差项
problem.AddResidualBlock( // 向问题中添加误差项
// 使⽤⾃动求导，模板参数：误差类型，输出维度，输⼊维度，维数要与前⾯struct中⼀致
new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(
new CURVE_FITTING_COST(x_data[i], y_data[i])
),
nullptr, // 核函数，这⾥不使⽤，为空添加损失函数(即鲁棒核函数)，这⾥不使⽤，为空
abc // 待估计参数优化变量，3维数组
);
}
// 配置求解器
ceres::Solver::Options options; // 这⾥有很多配置项可以填定义⼀个配置项集合类options
options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
// 增量⽅程如何求解增量⽅程求解⽅式，本质上是稠密矩阵求逆的加速⽅法选择
options.minimizer_progress_to_stdout = true;
// 输出到cout minimizer_progress_to_stdout表⽰是否向终端输出优化过程信息
ceres::Solver::Summary summary; // 优化信息利⽤ceres执⾏优化
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_ud = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "solve time cost = " << unt() << " conds. " << endl;
// 输出结果
cout << summary.BriefReport() << endl;
cout << "estimated a,b,c = ";//输出估计值
for (auto a:abc) cout << a << " ";
cout << endl;
return 0;
}
< 和上⾯是⼀样的。
执⾏结果：
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 1.597873e+06 0.00e+00 3.52e+06 0.00e+00 0.00e+00 1.00e+04 0 4.82e-05 1.25e-04
1 1.884440e+05 1.41e+06 4.86e+05 9.88e-01 8.82e-01 1.81e+04 1 7.30e-05 2.73e-04
2 1.784821e+04 1.71e+05 6.78e+04 9.89e-01 9.06e-01 3.87e+04 1 2.50e-05 3.11e-04
3 1.099631e+03 1.67e+0
4 8.58e+03 1.10e+00 9.41e-01 1.16e+0
5 1 2.41e-05 3.48e-04
4 8.784938e+01 1.01e+03 6.53e+02 1.51e+00 9.67e-01 3.48e+0
5 1 2.38e-05 3.81e-04
5 5.141230e+01 3.64e+01 2.72e+01 1.13e+00 9.90e-01 1.05e+0
6 1 2.41e-05 4.15e-04
6 5.096862e+01 4.44e-01 4.27e-01 1.89e-01 9.98e-01 3.14e+06 1 2.38e-05 4.48e-04
7 5.096851e+01 1.10e-04 9.53e-04 2.84e-03 9.99e-01 9.41e+06 1 2.41e-05 4.81e-04
solve time cost = 0.00053221 conds.
Ceres Solver Report: Iterations: 8, Initial cost: 1.597873e+06, Final cost: 5.096851e+01, Termination: CONVERGENCE estimated a,b,c = 0.890908 2.1719 0.943628
g2oCurveFitting.cpp
#include <iostream>
#include <g2o/core/g2o_core_api.h>
#include <g2o/core/ba_vertex.h>//g2o顶点（Vertex）头⽂件视觉slam⼗四讲p141⽤顶点表⽰优化变量，⽤边表⽰误差项#include <g2o/core/ba_unary_edge.h>//g2o边（edge）头⽂件
#include <g2o/core/block_solver.h>//求解器头⽂件
#include <g2o/core/optimization_algorithm_levenberg.h>//列⽂伯格——马尔夸特算法头⽂件
#include <g2o/core/optimization_algorithm_gauss_newton.h>//⾼斯⽜顿算法头⽂件
#include <g2o/core/optimization_algorithm_dogleg.h>//dogleg算法头⽂件
#include <g2o/solvers/den/linear_solver_den.h>//稠密矩阵求解
#include <g2o/solvers/den/linear_solver_den.h>//稠密矩阵求解
#include <Eigen/Core>//Eigen核⼼模块
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>
using namespace std;
// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex : public g2o::BaVertex<3, Eigen::Vector3d> //：表⽰继承，public表⽰公有继承；CurveFittingVertex是派⽣类，BaVertex<3, Eigen::Vec {
public://以下定义的成员变量和成员函数都是公有的
EIGEN_MAKE_ALIGNED_OPERATOR_NEW//解决Eigen库数据结构内存对齐问题
// 重置
virtual void tToOriginImpl() override //virtual表⽰该函数为虚函数，override保留字表⽰当前函数重写了基类的虚函数
{
_estimate << 0, 0, 0;
}
// 更新
virtual void oplusImpl(const double *update) override
{
_estimate += Eigen::Vector3d(update);//更新量累加
}
// 存盘和读盘：留空
virtual bool read(istream &in) {} //istream类是c++标准输⼊流的⼀个基类
//可参照C++ Primer Plus第六版的6.8节
virtual bool write(ostream &out) const {} //ostream类是c++标准输出流的⼀个基类
//可参照C++ Primer Plus第六版的6.8节
};
/
/ 误差模型模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge : public g2o::BaUnaryEdge<1, double, CurveFittingVertex> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW//解决Eigen库数据结构内存对齐问题
CurveFittingEdge(double x) : BaUnaryEdge(), _x(x) {}//使⽤列表赋初值
// 计算曲线模型误差
virtual void computeError() override //virtual表⽰虚函数，保留字override表⽰当前函数重写了基类的虚函数
{
const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);//创建指针v
const Eigen::Vector3d abc = v->estimate();//将estimate()值赋给abc
_error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));//视觉slam⼗四讲p133式6.39
}
// 计算雅可⽐矩阵
virtual void linearizeOplus() override //virtual表⽰虚函数，保留字override表⽰当前函数重写了基类的虚函数
{
const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
const Eigen::Vector3d abc = v->estimate();
double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);//视觉slam⼗四讲p133式6.38上⾯的式⼦
_jacobianOplusXi[0] = -_x * _x * y;//视觉slam⼗四讲p133式6.40第⼀个
_jacobianOplusXi[1] = -_x * y;//视觉slam⼗四讲p133式6.40第⼆个
_jacobianOplusXi[2] = -y;//视觉slam⼗四讲p133式6.40第三个
}
virtual bool read(istream &in) {}
virtual bool write(ostream &out) const {}
public:
double _x; // x 值， y 值为 _measurement
};
