這篇博客主要記錄了我在深藍學院視覺slam課程中的課后習題，因為是為了統計知識點來方便自己以后查閱，所以有部分知識可能不太嚴謹，如果給大家造成了困擾請見諒，大家發現了問題也可以私信或者評論給我及時改正，歡迎大家一起探討，其他章的筆記可以在我的首頁文章中查看，

整個作業的代碼和檔案都可以參考我的GitHub存盤庫GitHub - 1481588643/vslam

如果想要了解視覺slam其他章節的內容，也可以查閱以下鏈接視覺slam學習筆記以及課后習題《第三講李群李代數》_m0_61238051的博客-CSDN博客

視覺slam學習筆記以及課后習題《第一講初識slam》_m0_61238051的博客-CSDN博客_slam下載地址

視覺slam學習筆記以及課后習題《第二講三維物體剛體運動》_m0_61238051的博客-CSDN博客

一.影像去畸變 (2 分，約 小時)

現實生活中的影像總存在畸變，原則上來說，針孔透視相機應該將三維世界中的直線投影成直線，但 是當我們使用廣角和魚眼鏡頭時，由于畸變的原因，直線在影像里看起來是扭曲的，本次作業，你將嘗試 如何對一張影像去畸變，得到畸變前的影像，

圖 1: 測驗影像

圖 1 是本次習題的測驗影像（code/test.png），來自 EuRoC 資料集 [1]，可以明顯看到實際的柱子、箱子的直線邊緣在影像中被扭曲成了曲線，這就是由相機畸變造成的，根據我們在課上的介紹，畸變前后的 坐標變換為

xdistorted = x 1 + k1r2 + k2r4 + 2p1xy + p2 r2 + 2x2

ydistorted = y 1 + k1r2 + k2r4 + p1 r2 + 2y2 + 2p2xy

其中 x, y 為去畸變后的坐標，xdistorted, ydistroted 為去畸變前的坐標，現給定引數：

k1 = ?0.28340811, k2 = 0.07395907, p1 = 0.00019359, p2 = 1.76187114e ? 05.

(1)以及相機內參

fx = 458.654, fy = 457.296, cx = 367.215, cy = 248.375.

請根據 undistort_image.cpp 檔案中內容，完成對該影像的去畸變操作，

注：本題不要使用 OpenCV 自帶的去畸變函式，你需要自己理解去畸變的程序，我給你準備的程式中已經有了基本的提示，作為驗證，去畸變后影像如圖 2 所示，如你所見，直線應該是直的，

圖 2: 驗證影像

#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

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.0;                        // 噪聲Sigma值
    double inv_sigma=1/w_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 error = yi-exp(ae*xi*xi+be*xi+ce);   // 第i個資料點的計算誤差
   // 填寫計算error的運算式
            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/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=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 << "total cost: " << cost << endl;
    }

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

二.魚眼模型與去畸變 ( 分，約 小時)

在很多視覺 SLAM 的應用里，我們都會選擇廣角或魚眼相機作為主要的視覺傳感器，與針孔相機不同，魚眼相機的視野往往可以在 150? 以上，甚至超過 180?，普通的畸變模型在魚眼相機下作業的并不好， 幸好魚眼相機有自己定義的畸變模型，

請參閱 OpenCV 檔案 (https://docs.opencv.org/3.4/db/d58/group calib3d fisheye.html)， 完成以下問題：

1. 請說明魚眼相機相比于普通針孔相機在 SLAM 方面的優勢，

大多數視覺SLAM系統都受到普通針孔攝像機視場有限的影響,這很容易由于視角的突然變化或捕捉無紋理場景而導致跟蹤程序失敗.在用于測繪的移動測繪系統(MMS)中,由于成像和存盤器訪問速度有限,大尺寸傳感器(例如與計算機視覺中使用的640×480傳感器相比,8000×4000像素)需要寬基線拍攝模式,這一問題會加劇.因此,寬FoV的光學傳感器,如魚眼或全景/全向相機,成為一個很好的選擇.全景相機可以一次拍攝360°的場景,越來越多地應用于街景地圖收集、交通監控、虛擬現實和機器人導航.

2.請整理并描述 OpenCV 中使用的魚眼畸變模型（等距投影）是如何定義的，它與上題的畸變模型有何不同，

魚眼相機和單目相機的畸變模型可以參考以下兩篇博文：

魚眼相機成像模型_m0_61238051的博客-CSDN博客

機器視覺——單目相機模型（坐標標定以及去畸變）_m0_61238051的博客-CSDN博客

3.完成 fisheye.cpp 檔案中的內容，針對給定的影像，實作它的畸變校正，要求：通過手寫方式實作， 不允許呼叫 OpenCV 的 API，

#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

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.0;                        // 噪聲Sigma值
    double inv_sigma=1/w_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 error = yi-exp(ae*xi*xi+be*xi+ce);   // 第i個資料點的計算誤差
   // 填寫計算error的運算式
            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/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=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 << "total cost: " << cost << endl;
    }

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

1. 什么在這張影像中，我們令畸變引數 k1, . . . , k4 = 0，依然可以產生去畸變的效果？

答：在k1;k2;k3;k4都為0時，theta_d就等于theta，此時也可以使魚眼相機達到去畸變的效果

5.在魚眼模型中，去畸變是否帶來了影像內容的損失？如何避免這種影像內容上的損失呢？

答：在魚眼模型中，去畸變帶來了影像內容的損失，

三.雙目視差的使用 (2 分，約 1 小時)

雙目相機的一大好處是可以通過左右目的視差來恢復深度，課程中我們介紹了由視差計算深度的程序， 本題，你需要根據視差計算深度，進而生成點云資料，本題的資料來自 Kitti 資料集 [2]，

Kitti 中的相機部分使用了一個雙目模型，雙目采集到左圖和右圖，然后我們可以通過左右視圖恢復出深度，經典雙目恢復深度的演算法有 BM(Block Matching), SGBM(Semi-Global Block Matching)[3, 4] 等， 但本題不探討立體視覺內容（那是一個大問題），我們假設雙目計算的視差已經給定，請你根據雙目模型， 畫出影像對應的點云，并顯示到 Pangolin 中，

理論部分：

1.推導雙目相機模型下，視差與 XY Z 坐標的關系式，請給出由像素坐標加視差 u, v, d 推導 XY Z與已知 XY Z 推導 u, v, d 兩個關系，

雙目相機模型可以參考此博文：

機器視覺——雙目視覺的基礎知識（視差深度、標定、立體匹配）_m0_61238051的博客-CSDN博客

2.推導在右目相機下該模型將發生什么改變，

單目相機模型可以參考此博文：

機器視覺——單目相機模型（坐標標定以及去畸變）_m0_61238051的博客-CSDN博客

編程部分：

本題給定的左右圖見 code/left.png 和 code/right.png，視差圖亦給定，見 code/right.png，雙目的引數如下：

fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157.

且雙目左右間距（即基線）為：

d = 0.573 m.

請根據以上引數，計算相機資料對應的點云，并顯示到 Pangolin 中，程式請參考 code/disparity.cpp 檔案，

圖 4: 雙目影像的左圖、右圖與視差

作為驗證，生成點云應如圖 5 所示，

圖 5: 雙目生成點云結果

#include <opencv2/opencv.hpp>
#include <string>
#include <Eigen/Core>
#include <pangolin/pangolin.h>
#include <unistd.h>

using namespace std;
using namespace Eigen;

// 檔案路徑，如果不對，請調整
string left_file = "/home/wang/桌面/L4/code/left.png";
string right_file = "/home/wang/桌面/L4/code/right.png";
string disparity_file = "/home/wang/桌面/L4/code/disparity.png";

// 在panglin中畫圖，已寫好，無需調整
void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud);

int main(int argc, char **argv) {

    // 內參
    double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
    // 間距
    double d = 0.573;
    // 讀取影像
    cv::Mat left = cv::imread(left_file, 0);
    cv::Mat right = cv::imread(right_file, 0);
    cv::Mat disparity = cv::imread(disparity_file, 0); // disparty 為CV_8U,單位為像素
    // 生成點云
    vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;
    
    for (int v = 0; v < left.rows; v++)
        for (int u = 0; u < left.cols; u++) {

            Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); // 前三維為xyz,第四維為顏色
           
            // 根據雙目模型計算 point 的位置
             unsigned int depth = disparity.ptr<unsigned short>(v)[u]; //深度值
            // if(depth == 0)continue; //深度為0表示未測量到
          
            point[2]=(fx*d*1000)/depth;
            point[1]=(v-cy)*point[2]/fy;
            point[0]=(u-cx)*point[2]/fx;
          
            pointcloud.push_back(point);
        }

    // 畫出點云
    showPointCloud(pointcloud);
    return 0;
}



void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud) {

    if (pointcloud.empty()) {
        cerr << "Point cloud is empty!" << endl;
        return;
    }

    pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
            pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
            .SetHandler(new pangolin::Handler3D(s_cam));

    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

        glPointSize(2);
        glBegin(GL_POINTS);
        for (auto &p: pointcloud) {
            glColor3f(p[3], p[3], p[3]);
            glVertex3d(p[0], p[1], p[2]);
        }
        glEnd();
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
    return;
}

四.矩陣運算微分 (2 分，約 1.5 小時)

在優化中經常會遇到矩陣微分的問題，例如，當自變數為向量 x，求標量函式 u(x) 對 x 的導數時，即為矩陣微分，通常線性代數教材不會深入探討此事，這往往是矩陣論的內容，我在 ppt/目錄下為你準備了一份清華研究生課的矩陣論課件（僅矩陣微分部分），閱讀此 ppt，回答下列問題：

設變數為 x ∈ RN ，那么：

1. 矩陣 A ∈ RN ×N ，那么 d(Ax)/dx 是什么1？
2. 矩陣 A ∈ RN ×N ，那么 d(xTAx)/dx 是什么？
3. 證明：xTAx = tr(AxxT). (2)

1嚴格的寫法必須對行向量求導，所以應該寫成 d(Ax)/dxT，但有些時候我們為了公式簡潔，也會省略這個 T，

五.高斯牛頓法的曲線擬合實驗 (2 分，約 1 小時)

六.* 批量最大似然估計 (2 分，約 2 小時)

考慮離散時間系統：

xk = xk?1 + vk + wk, w ～ N (0, Q)

yk = xk + nk, nk ～ N (0, R)

這可以表達一輛沿 x 軸前進或后退的汽車，第一個公式為運動方程，vk 為輸入，wk 為噪聲；第二個公式為觀測方程，yk 為路標點，取時間 k = 1, . . . , 3，現希望根據已有的 v, y 進行狀態估計，設初始狀態 x0 已知，

請根據本題題設，推導批量（batch）最大似然估計，首先，令批量狀態變數為 x = [x0, x1, x2, x3]T， 令批量觀測為 z = [v1, v2, v3, y1, y2, y3]T，那么：

1. 可以定義矩陣 H，使得批量誤差為 e = z ? Hx，請給出此處 H 的具體形式，
2. 據上問，最大似然估計可轉換為最小二乘問題：

x? = arg min 1 (z ? Hx)TW?1 (z ? Hx) , (5)

其中 W 為此問題的資訊矩陣，可以從最大似然的概率定義給出，請給出此問題下 W 的具體取值，

1. 假設所有噪聲相互無關，該問題存在唯一的解嗎？若有，唯一解是什么？若沒有，說明理由，