SLAM算法＆技术之Gauss-Newton非线性最小二乘算法

点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

编辑丨点云PCL

前言

很多问题最终归结为一个最小二乘问题，如SLAM算法中的Bundle Adjustment，位姿图优化等等。求解最小二乘的方法有很多，高斯-牛顿法就是其中之一。

推导

对于一个非线性最小二乘问题：

高斯牛顿的思想是把 f(x)利用泰勒展开，取一阶线性项近似。

带入到(1)式：

对上式求导，令导数为0。

令 

式（4）即为

求解式（5），便可以获得调整增量 Delta_x 。这要求H可逆（正定），但实际情况并不一定满足这个条件，因此可能发散，另外步长 Delta_x可能太大，也会导致发散。

综上，高斯牛顿法的步骤为

编程实现

问题：

非线性方程: 

给定n组观测数据 (x,y) ，求系数

分析

令 

N组数据可以组成一个大的非线性方程组

我们可以构建一个最小二乘问题：

要求解这个问题，根据推导部分可知，需要求解雅克比。

使用推导部分所述的步骤就可以进行解算。代码实现：

#include#include#include#include 
#include#include#include#include/* 计时类 */
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(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::vectorob;
            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& 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 <