Eigen库：手写高斯-牛顿法

最新推荐文章于 2023-07-19 23:31:20 发布

Jason.Li_0012

最新推荐文章于 2023-07-19 23:31:20 发布

阅读量1.3k

点赞数 1

分类专栏：《视觉SLAM十四讲》笔记 C++\CMake 文章标签：自动驾驶线性代数

本文链接：https://blog.csdn.net/weixin_45929038/article/details/122973583

版权

《视觉SLAM十四讲》笔记同时被 2 个专栏收录

21 篇文章 19 订阅

订阅专栏

C++\CMake

13 篇文章 4 订阅

订阅专栏

问题描述

对一条曲线 $y=exp(ax^2+bx+c)+w$ ，其中 $a 、 b 、 c$ 为曲线参数，噪声项 $w\sim \mathcal{N}(0,\sigma^2)$ 。

假设现有 $N$ 个关于 $x ， y$ 的观测数据点，希望能够求取曲线参数 $a 、 b 、 c$ ，则可构建非线性最小二乘问题用于估计参数：
$\min_{a,b,c}\frac{1}{2}\sum_{i=1}^N\begin{Vmatrix}y_i-exp(ax_i^2+bx_i+c)\end{Vmatrix}^2$
定义误差为： $e_i=y_i-exp(ax_i^2+bx_i+c)$ ，则可求的雅克比矩阵：
$J_i=\begin{bmatrix}\frac{\partial e_i}{\partial a}\\\frac{\partial e_i}{\partial b}\\\frac{\partial e_i}{\partial c}\end{bmatrix}$
其中，偏导数表达式如下：
$\begin{aligned} \frac{\partial e_i}{\partial a}&=-x_i^2\:exp(ax_i^2+bx_i+c)\\ \frac{\partial e_i}{\partial b}&=-x_i\:exp(ax_i^2+bx_i+c)\\ \frac{\partial e_i}{\partial c}&=-\:exp(ax_i^2+bx_i+c)\\ \end{aligned}$
则可构建高斯-牛顿法中增量方程如下：
$\Bigl(\sum_{i=1}^{100}J_i(\sigma^2)^{-1}\:J_i^T\Bigr)\:\Delta x_k=\sum_{i=1}^{100}-J_i(\sigma^2)^{-1}\:e_i$

工程构建

采用Ubuntu20.04+CLion进行开发，主要使用Eigen库进行矩阵计算。

工程结构

├── CMakeLists.txt
├── include
│   └── main.h
└── src
    ├── CMakeLists.txt
    └── main.cpp

CMake配置

主要使用了OpenCV库生成数据点，Eigen库用于矩阵计算。根目录下CMakeLists.txt文件内容如下：

# cmake version
cmake_minimum_required(VERSION 3.21)
# project name
project(Study)
# cpp version
set(CMAKE_CXX_STANDARD 14)
# eigen
include_directories("/usr/include/eigen3")
# opencv
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
# incldue
include_directories(include)
# src
add_subdirectory(src)

src下CMakeLists.txt文件内容如下：

# exec
add_executable(Study main.cpp)
# link opencv
target_link_libraries(Study ${OpenCV_LIBRARIES})

头文件配置

include目录下头文件main.h内容如下：

//
// Created by jasonli on 2022/2/12.
//

#ifndef STUDY_MAIN_H
#define STUDY_MAIN_H

#include <iostream>
#include <cmath>
#include <unistd.h>
//  Eigen
#include <Eigen/Core>
#include <Eigen/Dense>
//  OpenCV
#include <opencv2/opencv.hpp>
//  namespace
using namespace std;
using namespace Eigen;


#endif //STUDY_MAIN_H

源文件配置

src下源文件main.cpp导入头文件：

#include "main.h"

int main()
{

    return 0;
}

代码

整体代码以及注释如下：

#include "main.h"

int main()
{
    /*--------  初始参数配置  --------*/
    //  实际曲线参数
    double ar = 1.0, br = 1.0, cr = 1.0;
    //  估计曲线参数初始值
    double ae = 2.0, be = 1.0, ce = 5.0;
    //  采样观测数据点个数
    int N = 100;
    //  噪声标准差及其倒数
    double w_sigma = 1.0;
    double inv_sigma = 1.0 / w_sigma;
    //  随机数生成器
    cv::RNG rng;

    /*--------  观测数据生成  --------*/
    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;
    //  迭代cost以及上一次迭代cost
    double cost, lastCost = 0;

    //  开始求解时间t1
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    for(int iter=0; iter < iterations; iter++){
        //  增量方程左侧值初始化：Hessian = J^T W^{-1} J in Gauss-Newton
        Matrix3d H = Matrix3d::Zero();
        //  增量方程右侧值初始化：g = - W^{-1} ei J
        Vector3d g = Vector3d::Zero();
        //  估计同真实的差距值，归零化
        cost = 0;

        for(int i=0; i < N; i++){
            //  获取第i个数据点
            double xi = x_data[i], yi = y_data[i];
            //  误差
            double ei = yi - exp(ae * xi * xi + be * xi + ce);
            //  雅克比矩阵
            Vector3d J;
            J[0] = - xi * xi * exp(ae * xi * xi + be * xi + ce);
            J[1] = - xi * exp(ae * xi * xi + be * xi + ce);
            J[2] = - exp(ae * xi * xi + be * xi + ce);
            //  增量方程系数
            H += inv_sigma * inv_sigma * J * J.transpose();
            g += -inv_sigma * inv_sigma * ei * J;
            //  差距
            cost += ei * ei;
        }
        //  求解增量方程Hdx=g
        Vector3d dx = H.ldlt().solve(g);
        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\t update:" << dx.transpose() << "\t\t estimated params:" << ae << "," << be << "," << ce << endl;

    }

    //  结束求解时间t2
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    //  求解花费时间t2 - t1
    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;
}

输出

输出如下：

total cost:9.26353e+07, 		 update:-0.00563613   -0.006202   -0.981441		 estimated params:1.99436,0.993798,4.01856
total cost:1.23782e+07, 		 update:-0.0154356 -0.0158777  -0.950534		 estimated params:1.97893,0.97792,3.06802
total cost:1.61827e+06, 		 update:-0.0425927 -0.0364985   -0.87242		 estimated params:1.93634,0.941422,2.19561
total cost:199423, 		 update:  -0.11697 -0.0600174  -0.697204		 estimated params:1.81937,0.881404,1.4984
total cost:20992.7, 		 update:  -0.287499 -0.00580094   -0.403671		 estimated params:1.53187,0.875603,1.09473
total cost:1519.46, 		 update:-0.437282  0.186207 -0.135058		 estimated params:1.09458,1.06181,0.959671
total cost:132.709, 		 update: -0.205976   0.142068 -0.0272031		 estimated params:0.888608,1.20388,0.932468
total cost:102.041, 		 update: -0.0110366  0.00856986 -0.00121983		 estimated params:0.877572,1.21245,0.931248
total cost:102.004, 		 update: 7.74339e-05 -9.85241e-05  2.39198e-05		 estimated params:0.877649,1.21235,0.931272
total cost:102.004, 		 update:-3.05581e-07  3.32264e-07 -5.91727e-08		 estimated params:0.877649,1.21235,0.931272
total cost:102.004, 		 update: 1.77986e-09 -2.04038e-09  4.27718e-10		 estimated params:0.877649,1.21235,0.931272
cost:102.004>= last cost :102.004, break.
solve time cost = 0.00442306seconds. 
estimated abc = 0.877649 , 1.21235 , 0.931272