目录
非线性优化
最小二乘法的引入
1. 几个名词的定义：
1）先验：根据0~k-1时刻的各种数据来推测k时刻的状态
2）后验：结合了k时刻观测数据的k时刻状态
3）似然：在怎么样的位姿下，可能产生怎么样的观测数据
2. 思路
想知道在已知观测数据z（比如在图像中的位置）的情况下，估计系统的状态（比如它的位姿），这就是后验概率，想根据z直接求位姿是比较困难的。但我们可以用似然的思路来解决，就比如不停的试各种状态，看看是在什么样的状态下，最有可能产生现在观测到的数据。找到这个状态了，那就是我要求的位姿了。为了找到一个x和y（位姿和地标），使得利用它们产生的观测数据z‘最接近目前直接观测到的z，那这样的x和y不就是我们需要的吗。
用概率学的观点看，即是已知输入数据u和观测数据z的条件下，求x，y的条件概率分布。 即：P(x,y|z,u)，也是后验
由贝叶斯法则
所以
问题转化为怎么样的x，y使得P(z,u|x,y)P(x,y)最大，相当于最大化似然与先验的乘积。
最大化似然估计的的理解是：“在什么样的状态下最有可能产生现在的数据”
3. 数学描述
因而得到了一个最小二乘的问题
解决最小二乘的方法
1. 梯度法
2. 牛顿法
3. 高斯牛顿法
4. 列文伯格-马夸尔特法
高斯牛顿法的代码实战
求带估计的变量a,b,c使之最吻合
理论思路
1. 转化问题为
2. 定义e=y-exp(ax^2+bx+c)
3. 得到J
4. 解方程
代码思路
已知真正的a,b,c
double ar = 1.0, br = 2.0, cr = 1.0; // 真实参数值1. 将数据真实数据存入数组
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));
}2. 给定初始值
double ae = 2.0, be = -1.0, ce = 5.0; // 估计参数值3. 求每一个点的梯度并叠加求H和g（这里要的是整个曲线的误差）
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;
}4. 解线性方程组
// 求解线性方程 Hx=b
Vector3d dx = H.ldlt().solve(b);5. 判断误差是否大于上一次迭代的误差，若是，跳出循环
if (iter > 0 && cost >= lastCost) {
cout << "cost: " << cost << ">= last cost: " << lastCost << ", break." << endl;
break;
}6. 若否：迭代点变化
ae += dx[0];
be += dx[1];
ce += dx[2];
lastCost = cost;整体代码如下
#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;
}
用ceres库解上述问题
代码思路
1. 定义参数模型的计算方式
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维 当没有必要分类的时候 就用一个数组来存储未知的系数,方便管理，而不是设3个变量，之后在()重载函数的形式参数个数变为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数据
};2. 定义参数块
double abc[3] = {0.8,2.1,0.9}; // abc参数的估计值3. 构建最小二乘问题
problem.AddResidualBlock(代价函数，核函数，参数块)
ceres::Problem problem;
for ( int i=0; i<N; i++ )
{
problem.AddResidualBlock ( // 向问题中添加误差项
/*使用自动求导，模板参数：误差类型，Dimension of residual(输出维度 表示有几类残差，本例程中就一类残差项目，所以为1)，*/
//输入维度，维数要与前面struct中一致
/*这里1 代表*/
new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3> (
new CURVE_FITTING_COST ( x_data[i], y_data[i] )// x_data[i], y_data[i] 代表输入的获得的试验数据
),
nullptr, // 核函数，这里不使用，为空 这里是LossFunction的位置
abc // 待估计参数3维
);
}4. 配置求解器
ceres::Solver::Options options; // 在Options类中可以设置关于求解器的参数
options.linear_solver_type = ceres::DENSE_QR; // 增量方程如何求解
options.minimizer_progress_to_stdout = true; // 为真时 内部错误输出到cout
ceres::Solver::Summary summary; // 优化信息5. 开始优化
ceres::Solve(配置项，问题地址，输出地址)
ceres::Solve ( options, &problem, &summary ); // 开始优化全代码
注释部分来自：视觉SLAM十四讲ch6曲线拟合 代码注释_Night___Raid的博客-CSDN博客
// ceres 版本
#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维 当没有必要分类的时候 就用一个数组来存储未知的系数,方便管理，而不是设3个变量，之后在()重载函数的形式参数个数变为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 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.8,2.1,0.9}; // 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++ )
{
/* 第一个参数 CostFunction* : 描述最小二乘的基本形式即代价函数 例如书上的116页fi(.)的形式
* 第二个参数 LossFunction* : 描述核函数的形式 例如书上的ρi（.）
* 第三个参数 double* : 待估计参数（用数组存储）
* 这里仅仅重载了三个参数的函数，如果上面的double abc[3]改为三个double a=0 ,b=0,c = 0;
* 此时AddResidualBlock函数的参数除了前面的CostFunction LossFunction 外后面就必须加上三个参数 分别输入&a,&b,&c
* 那么此时下面的 ceres::AutoDiffCostFunction<>模板参数就变为了 <CURVE_FITTING_COST,1,1,1,1>后面三个1代表有几类未知参数
* 我们修改为了a b c三个变量，所以这里代表了3类，之后需要在自己写的CURVE_FITTING_COST类中的operator()函数中，
* 把形式参数变为了const T* const a, const T* const b, const T* const c ,T* residual
* 上面修改的方法与本例程实际上一样，只不过修改的这种方式显得乱，实际上我们在用的时候，一般都是残差种类有几个，那么后面的分类 就分几类
* 比如后面讲的重投影误差，此事就分两类 一类是相机9维变量，一类是点的3维变量，然而残差项变为了2维
*
* （1）: 修改后的写法（当然自己定义的代价函数要对应修改重载函数的形式参数，对应修改内部的残差的计算）：
* ceres::CostFunction* cost_function
* = new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 1 ,1 ,1>(
* new CURVE_FITTING_COST ( x_data[i], y_data[i] ) );
* problem.AddResidualBlock(cost_function,nullptr,&a,&b,&c);
* 修改后的代价函数的计算模型:
* struct CURVE_FITTING_COST
* {
* CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}
* // 残差的计算
* template <typename T>
* bool operator() (
* const T* const a,
* const T* const b,
* const T* const c,
* T* residual ) const // 残差
* {
* residual[0] = T ( _y ) - ceres::exp ( a[0]*T ( _x ) *T ( _x ) + b[0]*T ( _x ) + c[0] ); // y-exp(ax^2+bx+c)
* return true;
* }
* const double _x, _y; // x,y数据
* };//代价类结束
*
*
* （2）: 本例程下面的语句通常拆开来写(看起来方便些):
* ceres::CostFunction* cost_function
* = new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(
* new CURVE_FITTING_COST ( x_data[i], y_data[i] ) );
* problem.AddResidualBlock(cost_function,nullptr,abc)
* */
problem.AddResidualBlock ( // 向问题中添加误差项
// 使用自动求导，模板参数：误差类型，Dimension of residual(输出维度 表示有几类残差，本例程中就一类残差项目，所以为1)，输入维度，维数要与前面struct中一致
/*这里1 代表*/
new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3> (
new CURVE_FITTING_COST ( x_data[i], y_data[i] )// x_data[i], y_data[i] 代表输入的获得的试验数据
),
nullptr, // 核函数，这里不使用，为空 这里是LossFunction的位置
abc // 待估计参数3维
);
}
// 配置求解器ceres::Solver (是一个非线性最小二乘的求解器)
ceres::Solver::Options options; // 这里有很多配置项可以填Options类嵌入在Solver类中 ，在Options类中可以设置关于求解器的参数
options.linear_solver_type = ceres::DENSE_QR; // 增量方程如何求解 这里的linear_solver_type 是一个Linear_solver_type的枚举类型的变量
options.minimizer_progress_to_stdout = true; // 为真时 内部错误输出到cout，我们可以看到错误的地方，默认情况下，会输出到日志文件中保存
ceres::Solver::Summary summary; // 优化信息
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();//记录求解时间间隔
//cout<<endl<<"求解前....."<<endl;
/*下面函数需要3个参数：
* 1、 const Solver::Options& options <----> optione
* 2、 Problem* problem <----> &problem
* 3、 Solver::Summary* summary <----> &summart (即使默认的参数也需要定义该变量 )
* 这个函数会输出一些迭代的信息。
* */
ceres::Solve ( options, &problem, &summary ); // 开始优化
//cout<<endl<<"求解后....."<<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;
// 输出结果
// BriefReport() : A brief one line description of the state of the solver after termination.
cout<<summary.BriefReport() <<endl;
cout<<"estimated a,b,c = ";
/*auto a:abc 或者下面的方式都可以*/
for ( auto &a:abc ) cout<<a<<" ";
cout<<endl;
return 0;
}g2o求解上述问题
看不太懂，前面的区域以后再来探索吧
习题答案
1. 证明Ax=b超定时，最小二乘解为：
2. 梯度法、牛顿法、高斯牛顿法、列-马法的优缺点
最速下降法、牛顿法：优：直观方便，求解增量时只需解线性方程即可。
最速下降法：缺：可能增加迭代次数。
牛顿法：缺：需要计算目标函数的海塞矩阵，在问题规模较大时非常困难。
高斯牛顿法：优：用J^TJ作为牛顿法H矩阵的近似，简化了计算。
缺：实际中该近似只有半正定性。
列文伯格-马夸尔特法：优：一定程度上避免系数矩阵的非奇异和病态问题。
缺：收敛速度较慢。
3.
2059
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
