编辑丨点云PCL
前言
很多问题最终归结为一个最小二乘问题，如SLAM算法中的Bundle Adjustment，位姿图优化等等。求解最小二乘的方法有很多，高斯-牛顿法就是其中之一。
推导
对于一个非线性最小二乘问题：
高斯牛顿的思想是把 f(x)利用泰勒展开，取一阶线性项近似。
带入到(1)式：
对上式求导，令导数为0。
令
式（4）即为
求解式（5），便可以获得调整增量 Delta_x 。这要求H可逆（正定），但实际情况并不一定满足这个条件，因此可能发散，另外步长 Delta_x可能太大，也会导致发散。
综上，高斯牛顿法的步骤为
编程实现
问题：
非线性方程:
给定n组观测数据 (x,y) ，求系数
分析
令
N组数据可以组成一个大的非线性方程组
我们可以构建一个最小二乘问题：
要求解这个问题，根据推导部分可知，需要求解雅克比。
使用推导部分所述的步骤就可以进行解算。代码实现：
代码语言：javascript复制#include <iostream>
#include <eigen3/Eigen/Core>
#include <vector>
#include <opencv2/opencv.hpp>
#include <eigen3/Eigen/Cholesky>
#include <eigen3/Eigen/QR>
#include <eigen3/Eigen/SVD>
#include <chrono>
/* 计时类 */
class Runtimer{
public:
inline void start()
{
t_s_ = std::chrono::steady_clock::now();
}
inline void stop()
{
t_e_ = std::chrono::steady_clock::now();
}
inline double duration()
{
return std::chrono::duration_cast<std::chrono::duration<double>>(t_e_ - t_s_).count() * 1000.0;
}
private:
std::chrono::steady_clock::time_point t_s_; //start time ponit
std::chrono::steady_clock::time_point t_e_; //stop time point
};
/* 优化方程 */
class CostFunction{
public:
CostFunction(double* a, double* b, double* c, int max_iter, double min_step, bool is_out):
a_(a), b_(b), c_(c), max_iter_(max_iter), min_step_(min_step), is_out_(is_out)
{}
void addObservation(double x, double y)
{
std::vector<double> ob;
ob.push_back(x);
ob.push_back(y);
obs_.push_back(ob);
}
void calcJ_fx()
{
J_ .resize(obs_.size(), 3);
fx_.resize(obs_.size(), 1);
for ( size_t i = 0; i < obs_.size(); i )
{
std::vector<double>& ob = obs_.at(i);
double& x = ob.at(0);
double& y = ob.at(1);
double j1 = -x*x*exp(*a_ * x*x *b_*x *c_);
double j2 = -x*exp(*a_ * x*x *b_*x *c_);
double j3 = -exp(*a_ * x*x *b_*x *c_);
J_(i, 0 ) = j1;
J_(i, 1) = j2;
J_(i, 2) = j3;
fx_(i, 0) = y - exp( *a_ *x*x *b_*x *c_);
}
}
void calcH_b()
{
H_ = J_.transpose() * J_;
B_ = -J_.transpose() * fx_;
}
void calcDeltax()
{
deltax_ = H_.ldlt().solve(B_);
}
void updateX()
{
*a_ = deltax_(0);
*b_ = deltax_(1);
*c_ = deltax_(2);
}
double getCost()
{
Eigen::MatrixXd cost= fx_.transpose() * fx_;
return cost(0,0);
}
void solveByGaussNewton()
{
double sumt =0;
bool is_conv = false;
for( size_t i = 0; i < max_iter_; i )
{
Runtimer t;
t.start();
calcJ_fx();
calcH_b();
calcDeltax();
double delta = deltax_.transpose() * deltax_;
t.stop();
if( is_out_ )
{
std::cout << "Iter: " << std::left <<std::setw(3) << i << " Result: "<< std::left <<std::setw(10) << *a_ << " " << std::left <<std::setw(10) << *b_ << " " << std::left <<std::setw(10) << *c_ <<
" step: " << std::left <<std::setw(14) << delta << " cost: "<< std::left <<std::setw(14) << getCost() << " time: " << std::left <<std::setw(14) << t.duration() <<
" total_time: "<< std::left <<std::setw(14) << (sumt = t.duration()) << std::endl;
}
if( delta < min_step_)
{
is_conv = true;
break;
}
updateX();
}
if( is_conv == true)
std::cout << "nConvergedn";
else
std::cout << "nDivergednn";
}
Eigen::MatrixXd fx_;
Eigen::MatrixXd J_; // 雅克比矩阵
Eigen::Matrix3d H_; // H矩阵
Eigen::Vector3d B_;
Eigen::Vector3d deltax_;
std::vector< std::vector<double> > obs_; // 观测
double* a_, *b_, *c_;
int max_iter_;
double min_step_;
bool is_out_;
};//class CostFunction
int main(int argc, char **argv) {
const double aa = 0.1, bb = 0.5, cc = 2; // 实际方程的参数
double a =0.0, b=0.0, c=0.0; // 初值
/* 构造问题 */
CostFunction cost_func(&a, &b, &c, 50, 1e-10, true);
/* 制造数据 */
const size_t N = 100; //数据个数
cv::RNG rng(cv::getTickCount());
for( size_t i = 0; i < N; i )
{
/* 生产带有高斯噪声的数据 */
double x = rng.uniform(0.0, 1.0) ;
double y = exp(aa*x*x bb*x cc) rng.gaussian(0.05);
/* 添加到观测中 */
cost_func.addObservation(x, y);
}
/* 用高斯牛顿法求解 */
cost_func.solveByGaussNewton();
return 0;
}基础与细节
（1）最小二乘问题：least squares method，又称最小平方法，是一种数学优化建模方法。它通过最小化误差的平方和寻找数据的最佳函数匹配。
最小平方问题分为两种：线性最小二乘法，和非线性的最小二乘法，取决于在所有未知数中的残差是否为线性。线性的最小平方问题发生在统计回归分析中；它有一个封闭形式的解决方案。非线性的问题通常经由迭代细致化来解决；在每次迭代中，系统由线性近似，因此在这两种情况下核心演算是相同的。
（2）泰勒公式：泰勒公式是用多项式来近似表示函数在某点周围的情况。泰勒公式中的一阶导称为雅克比矩阵（是函数的一阶偏导数以一定方式排列成的矩阵），二阶导称为海塞矩阵（二阶偏导数组成的方块矩阵，由德国数学家奥托·黑塞引入并以其命名）。
（3）由公式（3）到公式（4）的推导过程（涉及到矩阵微积分：https://arxiv.org/pdf/1802.01528.pdf）根据下面的式子理解：
（4）下面的雅克比矩阵是如何得到？是根据 非线性方程组分别对a,bc求偏导而得，与代码也是对应的。
本文仅做学术分享，如有侵权，请联系删文。
