引言
ch6.task4_5.
1.GNpractice
注意未知数是abc.GN分析:
code:
#include <iostream>
#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值
cv::RNG rng; // OpenCV随机数产生器
vector<double> x_data, y_data; // 数据
for (int i = 0; i < N; i++) {
double x = i / 100.0; // scale to 0--1
x_data.push_back(x);
y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma)); // 产生測量值 y_data
}
// 开始Gauss-Newton迭代
int iterations = 100; // 迭代次数
double cost = 0, lastCost = 0; // 本次迭代的cost和上一次迭代的cost
for (int iter = 0; iter < iterations; iter++) {
Matrix3d H = Matrix3d::Zero(); // Hessian = J^T * 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个数据点
// start your code here
double error = 0; // 第i个数据点的计算误差
error = yi - exp(ae*xi*xi + be*xi + ce) ; // 填写计算error的表达式
Vector3d J; // 雅可比矩阵, 是向量. [de/da, de/db, de/dc]
J[0] = -exp(ae*xi*xi + be*xi + ce)*xi*xi; // de/da
J[1] = -exp(ae*xi*xi + be*xi + ce)*xi; // de/db
J[2] = -exp(ae*xi*xi + be*xi + ce); // de/dc
H += J * J.transpose(); // GN近似的H
b += -error * J; // See book P. 112 , eqt(6.21)
// end your code here
cost += error * error;
}
// 求解线性方程 Hx=-b,建议用ldlt
// start your code here
Vector3d dx;
LLT<MatrixXd> llt;
llt.compute(H);
dx = llt.solve(b); // increment dx
//或者使用
//dx = H.ldlt().solve(b);
// end your code here
if (std::isnan(dx[0])){
cout << "result is nan!" << endl;
break;
}
if (iter > 0 && cost > lastCost) {
// 误差增长了,说明近似的不够好
cout << "cost: " << cost << ", last cost: " << lastCost << endl;
break;
}
// 更新abc估计值
ae += dx[0];
be += dx[1];
ce += dx[2];
lastCost = cost;
cout << "total cost: " << cost << endl;
}
cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
return 0;
}
2.MaximumLikelihoodEstimation
最大似然估计:假设一个袋子里面有两种装球方案:
- 99个白球一个红球
- 99个红球一个白球
从袋子里面随机拿出一个一个球为白色,问装球方案是哪一种?(这个例子是五一前夕高工课上玩手机时听见老师讲的,很贴切!)最大似然就是解决类似问题。SLAM中理解为在什么样的状态下,最可能发生现在观测到的数据.
非线性优化,后端。