一、 矩陣微分問題

0

1-2：

更多性質見https://blog.csdn.net/crazy_scott/article/details/80557814 和書

二、

//
// Created by 高翔 on 2017/12/15.
//
#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <math.h>
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;                   // 噪聲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));
    }

    // 開始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 ex=exp(ae*xi*xi+be*xi+ce);
            double error = 0;   // 第i個數據點的計算誤差
            error = yi-ex; // 填寫計算error的表達式
            Vector3d J; // 雅可比矩陣
            J[0] = -xi*xi*ex;  // de/da
            J[1] = -xi*ex;  // de/db
            J[2] = -ex;  // de/dc

            H += J * J.transpose(); // GN近似的H
            b += -error * J;
            // end your code here

            cost += error * error;
        }

        // 求解線性方程 Hx=b，建議用ldlt
        // start your code here
        Vector3d dx;
        dx=H.ldlt().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<<"iterate: "<<iter<<endl;
        cout<<"ae="<<ae<<" be="<<be<<" ce="<<ce<<endl;
        cout<<"cost= "<<cost<<endl;
    // end your code here

        cout << "total cost: " << cost << endl;
    }

    cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
    return 0;
}

代碼如上。

自寫部分：

 double ex=exp(ae*xi*xi+be*xi+ce);
 double error = 0;   // 第i個數據點的計算誤差
 error = yi-ex; // 填寫計算error的表達式
 Vector3d J; // 雅可比矩陣
 J[0] = -xi*xi*ex;  // de/da
 J[1] = -xi*ex;  // de/db
 J[2] = -ex;  // de/dc

 H += J * J.transpose(); // GN近似的H
 b += -error * J;
然後解個方程

自己開始推了一大堆後來發現按照提示寫很簡單就完了。

需要註意的地方：

　　每一個點引入一個error，在內層循環中只計算了單個error的J矩陣，並且不考慮外面的平方。內層就是f(x)，所以形式都很簡單，然後在外層把算出來的H和b加起來最後統一求解，這裏的加法原理是講得通的。

　　一開始我解得欲仙欲死就是因為想到這個函數是一百個點誤差平方加起來，之後對這個大函數求一次J和H然後解方程直接得到叠代值，最後出了問題。

　　其實這麽做理論上也是對的，一會再試試。（PS：沒有試出來）

運行結果如下：

高博SLAM第四講——非線性優化