有两个方程，x是相机位姿，u是位姿改变数据，y是路标点在相机坐标系的坐标，z是路标点在图片上的坐标 状态估计问题：在有一个已知的xk-1时，通过uk我们求出xk; 通过yj,xk我们求出zk；但是问题是求的过程中都会有噪声wk,vk，所以我们只能求出来xk和zk的概率分布，所以构成了状态估计问题
!](https://img-blog.csdnimg.cn/2020070409274871.png) 滤波器：为了处理状态估计问题，我们假设已经有了一个当前时刻的估计状态（有误差），之后用新的数据来更新他，从而减小误差，这种方法称为增量/渐进方法，或者叫做滤波器，滤波器尽管新 批量：为了处理状态估计问题，把0到k时刻的所有uk,zk放在一起，然后进行优化，减小误差 非线性优化方法是批量方法的一种，要求的状态量是，P（x,y|u,z），有下面的关系，后验正比与似然乘以先验； 后验：已知观测数据z（比如在图像中的位置）的情况下，估计系统的状态（比如它的位姿），这就是后验概率 似然：直接求后验比较难，我们可以不停的试各种状态，看看是在什么样的状态下(x,y)，最有可能产生现在观测到的数据。找到这个状态了，那就是我要求的位姿了 最大似然估计：所以非线性优化转为求最大似然估计的问题，估计的方程是，要求的是当P最大时，x,y是多少。 求解最大似然估计：我们把P变成-log( P ),求解该式的最小值，就相当于求P的最大值， P的概率密度函数是 -log( p)的概率密度函数是 密度函数求和之后是 转换为最小二乘问题：把上面的用误差表示，可以写为下面的形式： Eu,k=Xk-f(Xk-1,Uk) Ez,j,k=Zk,j-h(Xk,Yj) 求解最小二乘问题：一般思路是求导数为0处所有点的值，然后比较求最小的，但是有时候方程可能不便于求导，所以有两种方法，line search和Trust Region 1、Line search 在line search具体实施的时候，需要找到增量Δxk,所以又分为两种方法，梯度下降法和高斯牛顿法 梯度下降法：在x附近展开目标函数，H是二阶导数，海塞矩阵。J是雅克比矩阵，表示fx关于x的导数。对于多个数据点u,z,J是多个数据点的fx关于x变量求导之和。这种方式的优点是比较直观，解线性方程组就行了
如果是一阶梯度法，那就不要最后一项，让它最小就完事了。这又叫最速下降法，缺陷是太贪心了，容易走锯齿路线。
如果是二阶梯度法，那么最后一项项还是要的，求右侧关于△x的导数，并令其为0。这种方法又叫牛顿法，缺陷是它得算一个H矩阵，不好算。
两者结果的区别，都是-JT，后者是H△x=-JT，前者直接△x就是-JT（T是转置符号）
高斯牛顿法：把目标函数变为一次方程，然后进行展开。然后求展开之后的目标函数的最小值。 求△x的导数，令其为0。
最后求得的 结果是JT *J △x=-JTfx （这里的fx是误差），近似写成H△x=g
这种方法的限制在于：JT和J构成的近似H矩阵应该是可逆的，正定的（特征值为正，各阶顺序主子式为正）。但实际算出来的是半正定的。如果出现它是奇异矩阵或者是病态矩阵，那算法就不会收敛。
对于这个的改进，有的方法是在找到△x以后，再找一个α，让||f(x+α△x)||2最小，不是简单的让α等于1。
2、Trust Region 这里的ρ如下定义： 分子表明实际函数下降的值，分母表明用雅克比矩阵的近似估计值。可以根据ρ来调整近似范围，太小说明分母较大，说明估计的太大，应该缩小范围；太大说明分母较小，说明的估计的小，可以放大近似范围。
上面算法里的阈值3/4和1/4，还有半径的放缩系数2和0.5都是经验值。 列文伯格-马尔夸特方法又可以叫做阻尼牛顿法。对于上面的D，我认为这个是控制区域的形状。如果区域是球，那就取单位阵E，它也可以把球弄成一个椭球。
注意上面步骤里面，是有个st不等式约束的，可以用拉格朗日乘子把目标函数转化成一个整体的无约束优化问题。 λ较小，H占主要地位，接近上面的高斯牛顿法。
λ较大，更接近一阶梯度下降法。DT*D可以简化为单位矩阵E。
列文伯格-马尔夸特方法优点就是可以避免非奇异与病态问题，缺点是收敛速度会很慢。
y=exp(ax2+bx+c)+w #include #include #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 J in Gauss-Newton Vector3d b = Vector3d::Zero(); // g(x) cost = 0; //使用100个数据点获取J 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; } // // 求解线性方程 H*dx=b获得增量dx 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 time_used = chrono::duration_cast<chrono::duration>(t2 - t1); cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl; return 0; }
// // 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> 声明模板函数，类型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( // 向问题中添加误差项 // 使用自动求导完成de/da,de/db,de/dc，模板参数：误差计算函数，输出维度，输入维度，维数要与前面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 time_used = chrono::duration_cast<chrono::duration>(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; }
定点是待估计参数abc,边是误差项e #include #include <g2o/core/g2o_core_api.h> #include <g2o/core/base_vertex.h> #include <g2o/core/base_unary_edge.h> #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> #include <g2o/solvers/dense/linear_solver_dense.h> #include <Eigen/Core> #include <opencv2/core/core.hpp> #include < cmath> #include < chrono>
using namespace std;
// 顶点Vertex的类型，模板参数：优化变量维度和数据类型 class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// 重置函数，将待优化变量置0 virtual void setToOriginImpl() override { _estimate << 0, 0, 0; }
// 更新函数，定义增量dx如何更新_estimate的公式 virtual void oplusImpl(const double *update) override { _estimate += Eigen::Vector3d(update); }
// 存盘和读盘：留空 virtual bool read(istream &in) {}
virtual bool write(ostream &out) const {} };
// Edge的误差模型 模板参数：观测值维度，类型，连接的顶点类型 class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}观测值的初始化
// 计算曲线模型误差 virtual void computeError() override { const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);vertices是g2o给定值 const Eigen::Vector3d abc = v->estimate();以上两句话可以直接背住，方便下面计算误差 _error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0)); }measurement也是g2o给定值
// 计算雅可比矩阵 virtual void linearizeOplus() 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]); _jacobianOplusXi[0] = -_x * _x * y; _jacobianOplusXi[1] = -_x * y; _jacobianOplusXi[2] = -y; }
virtual bool read(istream &in) {}
virtual bool write(ostream &out) const {}
public: double _x; // x 值， y 值为 _measurement };
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)); }
// 构建图优化，先设定g2o误差项优化变量和观测值维度 typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType; // 每个误差项优化变量维度为3，误差值维度为1 typedef g2o::LinearSolverDense< BlockSolverType::PoseMatrixType> LinearSolverType; // 设定线性求解器类型
// 梯度下降方法，可以从GN, LM, DogLeg 中选 auto solver = new g2o::OptimizationAlgorithmGaussNewton( g2o::make_unique< BlockSolverType>(g2o::make_unique< LinearSolverType>())); g2o::SparseOptimizer optimizer; // 图模型 optimizer.setAlgorithm(solver); // 设置求解器 optimizer.setVerbose(true); // 打开调试输出
// 往图中增加顶点 CurveFittingVertex *v = new CurveFittingVertex(); v->setEstimate(Eigen::Vector3d(ae, be, ce)); v->setId(0); optimizer.addVertex(v);
// 往图中增加边 for (int i = 0; i < N; i++) { CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]); edge->setId(i);设定每个边的ID edge->setVertex(0, v); // 设置边连接的顶点 edge->setMeasurement(y_data[i]); // 设定有噪声的观测数值y传给measurement 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(10);执行优化 chrono::steady_clock::time_point t2 = chrono::steady_clock::now(); chrono::duration time_used = chrono::duration_cast<chrono::duration>(t2 - t1); cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
// 输出优化值 Eigen::Vector3d abc_estimate = v->estimate(); cout << "estimated model: " << abc_estimate.transpose() << endl;
return 0; }
