slam14-ch4作业---高斯牛顿法求解非线性拟合

仅自己学习笔记使用。
参考了这篇答案

一、问题描述

二、思路分析

• 思路：
• 掌握的也不熟不知道对不对

1. 首先设定要优化的函数–误差函数f(x),本题中的优化函数是f(x) = error * error 二次范数
2. 其次，利用一次范数error 求出雅阁比矩阵J
3. 利用G-N法的公式 H * x = b ,H = J^T * J ,b = -f(x) * J, J 则是error的一阶倒数矩阵
4. 利用for循环,把100个样本点的 H ，J ，b总和计算出来，再算x 再对参数a b c 进行更新, 迭代100次

三、代码（源代码是slam课提供的）

gaussnewton.cpp

//
// Created by 高翔 on 2017/12/15.
//
#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;
/*
 * 思路：
 * 1. 首先设定要优化的函数--误差函数f(x),本题中的优化函数是f(x) = error * error   二次范数
 * 2. 其次，利用一次范数error 求出雅阁比矩阵J
 * 3. 利用G-N法的公式  H * x  = b  ,H = J^T * J ,b = -f(x) * J,  J 则是error的一阶倒数矩阵
 * 4. 利用for循环,把100个样本点的 H ，J ，b总和计算出来，再算x  再对参数a b c 进行更新, 迭代100次
 * */
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;
        x_data.push_back(x);
        y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma));
    }
    //x_data y_data是生成的数据点，共100组，满足公式y=f(x)

    // 开始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 = 0; // 填写计算error的表达式
            error=yi - (exp(ae * xi * xi + be * xi + ce));
            //f(x)=error*error 是目标函数
            //但是可以从error的雅克比J来用J^T模拟出黑塞矩阵H
            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/db
            //求出雅克比矩阵
//            cout<<"jjjjjjjjj    "<<J<<endl;
            H += J * J.transpose(); // GN近似的H
            b += -error * J;   //如公式中的  H * x = b  , b = -J*f(x) ,H = J^T * J  去解出x，就是可以移动的距离，使得f(x)最小
            // end your code here
            cost += error * error;
        }
        // 求解线性方程 Hx=b，建议用ldlt
 	// start your code here
        Vector3d dx;
        dx = H.colPivHouseholderQr().solve(b);
        // end your code here
        if (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;
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(disparity)

set(CMAKE_CXX_STANDARD 11)

find_package(OpenCV)
find_package(Eigen3)
find_package(Pangolin)
include_directories(${Pangolin_INCLUDE_DIRS})
#include_directories( "/usr/include/eigen3" )
include_directories(${EIGEN3_INCLUDE_DIR})

add_executable(prac gaussnewton.cpp)
target_link_libraries(prac ${OpenCV_LIBS})

四、运行结果