深蓝学院SLAM课程作业1~4章

最新推荐文章于 2022-12-04 17:04:46 发布

智能之欣

最新推荐文章于 2022-12-04 17:04:46 发布

阅读量589

点赞数

分类专栏： SLAM深蓝学院文章标签： slam

本文链接：https://blog.csdn.net/ZDPZN/article/details/119596371

版权

SLAM深蓝学院专栏收录该内容

1 篇文章 0 订阅

订阅专栏

深蓝SLAM从理论到实践课程–习题

文章目录

深蓝SLAM从理论到实践课程--习题
1 习题一
2 习题二
3 习题三
4 习题四

代码已上传至github:

1 习题一

1.1 熟悉Linux

1.2 SLAM综述文献阅读

1.3 CMake练习

1.4 理解ORB-SLAM2框架

环境搭建

虚拟机中使用必须有4个核心，内存至少分配8GB

按照README的介绍依次安装Pangolin(参考该项目的github主页)、OpenCV、Eigen3、DBoW2和g2o

其中Opencv和eigen3使用下列命令安装：
```
sudo apt-get install libopencv-dev libeigen3-dev libqt4-dev qt4-qmake libqglviewer-dev libsuitesparse-dev
libcxsparse3.1.2 libcholmod-dev
```
opencv通过apt-get安装的是2.4系列的版本，位置在/usr/include/opencv2中

另一种安装opencv的方式是自己通过下载源代码(slambook2使用的opencv3，因此需要自己下载安装)编译，这种编译安装后位置在/usr/local/include/中

DBoW2和g2o不需要自己安装，已经包含在orbslam2的3rdparty文件中，编译orbslam2时会自动编译这两个库。

安装完以上依赖库后就可以编译orbslam2项目了，如下：
```
git clone https://github.com/raulmur/ORB_SLAM2.git ORB_SLAM2
cd ORB_SLAM2
chmod +x build.sh
./build.sh
```

2 习题二

2.1 熟悉Eigen矩阵运算（3分，约2小时）

解答

矩阵 $A$ 和增广矩阵 $B$ 秩为n 或者 $\neq 0$

$R (A) = R (B) = n$

Guass Elimination，也称列消去法。

类似如下过程：

为了避免用小的做除数，衍生了列主元素消去法，通过交换列使大的数做除数
[参考资料](GramSchmidt.pdf (ucla.edu))

Consider the GramSchmidt procedure, with the vectors to be considered in the process as columns of the matrix A. That is,
$A = [a_1|a_2|...|a_n]$
Then,
$u_1 = a_1, e_1= \frac{u_1}{|u_1|} \\ u_2 = a_2-(a_2 \cdot e_1)e_1,e_2=\frac{u_2}{|u_2|} \\ u_{k+1} = a_{k+1}-(a_{k+1}\cdot e_1)e_1-\cdots-(a_{k+1}\cdot e_k)e_k,e_{k+1}=\frac{u_{k+1}}{|u_{k+1}|}$
The resulting QR factorization is
$KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ A &= [a_1|a_2|…$
cholesky分解：[参考资料一：分解及算法方法](矩阵分解----Cholesky分解 - FitzXidian - 博客园 (cnblogs.com))
代码如下

#include<iostream>
#include<Eigen/Core>
#include<Eigen/Dense>

#define MATRIX_SIZE 100
using namespace std;
using namespace Eigen;

int main(int argc, char** argv)
{
    MatrixXd Apre = MatrixXd::Random(100, 100);
    // to make a symmetric matrix to mkaesure Cholesky Decomposition work
    MatrixXd A = Apre.transpose() * Apre;

    Matrix<double, MATRIX_SIZE, 1> v_Nd = MatrixXd::Random( MATRIX_SIZE, 1);
    cout << "b(0) = " << v_Nd(0) << endl;
    // QR decomposition
    Eigen::Matrix<double, MATRIX_SIZE, 1> x1;
    x1 = A.colPivHouseholderQr().solve(v_Nd);
    // b1_ = A * x1
    Eigen::Matrix<double, MATRIX_SIZE, 1> b1_ = A *x1;
    cout << "b1_(0) = " << b1_(0) << endl;

    // Cholesky decomposition
    Eigen::Matrix<double, MATRIX_SIZE, 1> x2;
    x2 = A.llt().solve(v_Nd);
    // b2_ = A * x2
    Eigen::Matrix<double, MATRIX_SIZE, 1> b2_ = A * x2;
    cout << "b2_(0) = " << b2_(0) << endl;
    return 0;
}

2.2 Eigen几何运算练习(2分，约1小时)

解答

#include<iostream>
#include<Eigen/Core>
#include<Eigen/Dense>
#include<Eigen/Geometry>

using namespace std;
using namespace Eigen;

int main(int argc, char **argv)
{
    Quaterniond q1(0.55, 0.3, 0.2, 0.2);
    Quaterniond q2(-0.1, 0.3, -0.7, 0.2);
    // Normalize the Quaternion before using
    q1.normalize();
    q2.normalize();
    Vector3d t1(0.7, 1.1, 0.2);
    Vector3d t2(-0.1, 0.4, 0.8);
    Isometry3d T_c1w = Isometry3d::Identity();
    Isometry3d T_c2w = Isometry3d::Identity();
    T_c1w.rotate( q1 );
    T_c1w.pretranslate(t1);
    T_c2w.rotate( q2 );
    T_c2w.pretranslate(t2);
    Vector3d p1_c1(0.5, -0.1, 0.2);
    Vector3d p1_c2;
    p1_c2 = T_c2w * T_c1w.inverse() * p1_c1;
    cout << p1_c2 << endl;
    return 0;
}

2.3 旋转的表达（2分，约1小时）

解答

设两个坐标系 $S_a$ 和 $S_b$ ，坐标基分别为 $e_a$ 和 $e_b$ ，对于矢量 $\mathbf{u}$ 有：
$\mathbf{u}=e^T_bu_b = e^T_a u_a \\ e_b e^T_b u_b = e_b e_a^T u_a \Rightarrow \\ u_b = e_be_a^Tu_a^T \Rightarrow \\ R=C_{ba} = e_be_a^T$
则 $R^TR$ ：
$KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ R^TR &=e_a e_b…$
由 $R^TR=I$ 有：
$det(R)det(R^T)=[det(R)]^2=1 \\ \Rightarrow det(R)= \pm 1 \\ R^T = adj(R);结合R^{-1}=\frac{adj(R)}{det(R)} \\ \Rightarrow det(R) = 1$
$q = w + x i + y j + z k$ ，则：
$\epsilon = [x,y,z] \\ \eta = w$
故 $\epsilon$ 为3维的， $\eta$ 为一维的
四元素可以表示为一个标量和一个矢量，如下：
$q=(\eta, \epsilon)$
其中， $\eta$ 是一个标量， $\epsilon$ 是一个三维矢量。那么，四元数的乘法就可以表示为：
$KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ q_1q_2 &=(\eta…$
又由：
$KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ q^+_1q_2 &= \…$
则可以看出 $q_1\cdot q_2 = q_1^+ q_2$

同理可以证明： $q_1 \cdot q_2 = q_2^{\oplus}q_1$

四元数参考资料:Quaternion – from Wolfram MathWorld

2.4 罗德里格斯公式证明（2分，约1小时）

解答

Let k be a unit vector defining a rotation axis, and let v be any vector to rotate about k by angle θ (right hand rule, anticlockwise in the figure).

Using the dot and cross products, the vector v can be decomposed into components parallel and perpendicular to the axis k,
$\mathbf{v} = \mathbf{v_{||} + v_{\perp}}, \\$
where the component parallel to k is
$\mathbf{v_{||}} = (\mathbf{v\cdot k})\mathbf{k}$
called the vector projection of v on k, and the component perpendicular to k is
$\mathbf{v_{\perp} = v - v_{||} = v - (k \cdot v)k=-k \times (k \times v)}$
called the vector rejection of v from k.

The vector k × v can be viewed as a copy of v⊥ rotated anticlockwise by 90° about k, so their magnitudes are equal but directions are perpendicular. Likewise the vector k × (k × v) a copy of v⊥ rotated anticlockwise through 180° about k, so that k × (k × v) and v⊥ are equal in magnitude but in opposite directions (i.e. they are negatives of each other, hence the minus sign). Expanding the vector triple product establishes the connection between the parallel and perpendicular components, for reference the formula is a × (b × c) = (a · c)b − (a · b)c given any three vectors a, b, c.

The component parallel to the axis will not change magnitude nor direction under the rotation,
$\mathbf{v_{||rot} = v_{||}}$
only the perpendicular component will change direction but retain its magnitude, according to
$|\mathbf{v_{\perp rot}}| = |\mathbf{v_{\perp}}|, \\ \mathbf{v_{\perp rot}} = \cos(\theta)\mathbf{v_{\perp}} + \sin(\theta)\mathbf{k \times v_{\perp}},$
and since k and$ \mathbf{ v_{||}}$are parallel, their cross product is zero k × $ \mathbf{ v_{||}}$ = 0, so that
$\mathbf{k \times v_{\perp} = k \times (v - v_{||}) = k\times v - k \times v_{||} = k\times v}$
and it follows
$\mathbf{v_{\perp rot} = \cos(\theta) v_{\perp} + \sin(\theta) k \times v}$
This rotation is correct since the vectors v⊥ and k × v have the same length, and k × v is v⊥ rotated anticlockwise through 90° about k. An appropriate scaling of v⊥ and k × v using the trigonometric functions sine and cosine gives the rotated perpendicular component. The form of the rotated component is similar to the radial vector in 2D planar polar coordinates (r, θ) in the Cartesian basis
$\mathbf{r} = r\cos(\theta) \mathbf{e}_x + r \sin(\theta)\mathbf{e}_y$
where ex, ey are unit vectors in their indicated directions.

Now the full rotated vector is
$\mathbf{v}_{rot} = \mathbf{v}_{||rot} + \mathbf{v}_{\perp rot}$
By substituting the definitions of v∥rot and v⊥rot in the equation results in
$KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ \mathbf{v}_{ro…$

2.5 四元数运算性质的验证（1分，约1小时）

解答

证明：

设四元数 $\mathbf{q}=(q,\overrightarrow{q})$ ，由点p的四元数实部为0，则
$KaTeX parse error: No such environment: align* at position 8: \begin{̲a̲l̲i̲g̲n̲*̲}̲ \mathbf{p}' &=…$
上式可得 $\mathbf{p}'$ 的实部为：
${p}'=-\overrightarrow{q} \cdot \overrightarrow{p} q + q \overrightarrow{p} \cdot \overrightarrow{q} + (\overrightarrow{q} \times \overrightarrow{p}) \cdot{\overrightarrow{q}} \\ =0$
因此得证 $\mathbf{p}'$ 的实部为0。

补充证明：

设有四元数 $\mathbf{q} = (w,x,y,z)$ ，则四元数的共轭： $\bar{\mathbf{q}}=(w,-x, -y, -z)$

逆： $\frac{1}{\mathbf{q}} = \frac{ \bar{\mathbf{q}}}{\mathbf{q}\mathbf{\bar{q}}}=\frac{\mathbf{\bar{q}}}{w^2+x^2+y^2+z^2}$

求解Q

2.6 * 熟悉C++11 (2分，约1小时)

解答

vector<A> avec{a1, a2, a3}初始化列表
匿名函数[](const A&a1, const A&a2){return a1.index < a2.index}
auto类型推导和for循环：for(auto & a: avec){...}

3 习题三

李群、李代数参考资料：lie.pdf (ethaneade.org)

3.1 群的性质（2分，约2小时）

解答

$\left\{ \mathbb{Z},+\right\}$ 是群，
- 封闭性： $\forall a_1, a_2 \in \mathbb{Z}, a_1 + a_2 \in \mathbb{Z}$
- 结合律： $\forall a_1, a_2, a_3 \in \mathbb{Z}, (a_1 + a_2)+a_3=a_1 + (a_2 + a_3)$
- 幺元： $\exists a_0=0 \in \mathbb{Z}, s.t. \forall a \in \mathbb{Z}, a_0 + a = a+ a_0=a$
- 逆： $\forall a \in \mathbb{Z}, \exists a^{-1} \in \mathbb{Z}, s.t. a+a^{-1} = a_0$
$\left \{ \mathbb{N},+ \right\}$ 不是群，逆不满足

3.2 验证向量叉乘的李代数性质（2分，约1小时）

解答

封闭性：成立，向量叉乘后仍为向量
双线性：成立
自反性：也成立，向量自身叉乘为0
雅可比等价：

由公式： $\mathbf{a\times (b \times c)=(a \cdot c)b-(a\times b)c}$
$\times (Y\times Z) +Y \times (Z\times X) +Z \times (X\times Y) \\ = (X \cdot Z) Y - (X \cdot Y)Z +(Y \cdot X) Z - (Y \cdot Z)X +(Z \cdot Y) X - (Z \cdot X)Y \\ = 0$

3.3 推导SE(3)的指数映射（2分，约1小时）

解答

3.4 伴随（2分，约1小时）

解答

[1]:

[2] :

3.5 轨迹的描绘(2分，约1小时)

解答

关于代码中Sophus::SE3作为Vector元素定义方式的说明：

Sophus::SE3使用了Eigen::Quaternion作为其成员变量，因此在使用Vector时需要使用Eigen::aligned_allocator<Sophus::SE3>，具体我也不是很明白，可以参考下面两个：

官网解释：Eigen: Using STL Containers with Eigen

论坛解释：Using Sophus::SE3f with std::vector - githubmemory

总之，最终的声明如下：
```
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>
```
编译中报错：#error This file requires compiler and library support for the ISO C++ 2011

原因在于：编译文件和so需要对C++11支持

解决方案：在CMakeLists.txt前添加：

SET( CMAKE_CXX_FLAGS "-std=c++11 -O3")

代码如下：

#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>
#include <Eigen/Core>
#include <Eigen/StdVector>

// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>

using namespace std;

// path to trajectory file
string trajectory_file = "../trajectory.txt";

// function for plotting trajectory, don't edit this code
// start point is red and end point is blue
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>);

vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> ReadTrajectory(const string &path);

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

    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses;

    /// implement pose reading code
    // start your code here (5~10 lines)
    poses = ReadTrajectory(trajectory_file);
    // end your code here

    // draw trajectory in pangolin
    DrawTrajectory(poses);
    return 0;
}

/*******************************************************************************************/
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> ReadTrajectory(const string &path)
{
    ifstream fin(path);
    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> trajectory;
    if(!fin){
        cerr << "trajectory " << path << " not found " << endl;
        return trajectory;
    }
    while(!fin.eof()){
        double time, tx, ty, tz, qx, qy, qz, qw;
        fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
        Sophus::SE3 p1(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz));
        trajectory.push_back(p1);
    }
    return trajectory;
}

void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses) {
    if (poses.empty()) {
        cerr << "Trajectory is empty!" << endl;
        return;
    }

    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Trajectory 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);

        glLineWidth(2);
        for (size_t i = 0; i < poses.size() - 1; i++) {
            glColor3f(1 - (float) i / poses.size(), 0.0f, (float) i / poses.size());
            glBegin(GL_LINES);
            auto p1 = poses[i], p2 = poses[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }

}

CMakeLists.txt

CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
PROJECT( DrawTrajectory )
SET( CMAKE_CXX_FLAGS "-std=c++11 -O3")

FIND_PACKAGE(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})

FIND_PACKAGE(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})

include_directories("/usr/include/eigen3")

add_executable(draw_trajectory draw_trajectory.cpp)
target_link_libraries(draw_trajectory ${Sophus_LIBRARIES})
target_link_libraries(draw_trajectory ${Pangolin_LIBRARIES})

3.6 *轨迹的误差（2分，约1小时）

解答

C++代码:

#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>
#include <Eigen/Core>
#include <Eigen/StdVector>

// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>

using namespace std;

// path to trajectory file
string estimation_file = "../estimated.txt";
string groundtruth_file = "../groundtruth.txt";

// function for plotting trajectory, don't edit this code
// start point is red and end point is blue
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>);
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> ReadTrajectory(const string &path);

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

    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses1; // estimation
    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses2; // truth

    /// implement pose reading code
    poses1 = ReadTrajectory(estimation_file);
    poses2 = ReadTrajectory(groundtruth_file);
    
    // calculate RMSE
    double nRMSE = 0;
    double n = 0;
    for (size_t i = 0; i < poses1.size() - 1; i++) {
        auto p1 = poses1[i], p2 = poses2[i];
        auto temp = p1.inverse() * p2;
        Eigen::Matrix<double, 6, 1> e = temp.log();
        double e_norm = e.norm();
        nRMSE += e_norm * e_norm;
        n = (double)i + 1;
    }
    double RMSE = sqrt(nRMSE / n);
    cout << "RMSE = " << RMSE << endl;
    // draw trajectory in pangolin
    DrawTrajectory(poses1);
    DrawTrajectory(poses2);
    return 0;
}

/*******************************************************************************************/
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> ReadTrajectory(const string &path)
{
    ifstream fin(path);
    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> trajectory;
    if(!fin){
        cerr << "trajectory " << path << " not found " << endl;
        return trajectory;
    }
    while(!fin.eof()){
        double time, tx, ty, tz, qx, qy, qz, qw;
        fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
        Sophus::SE3 p1(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz));
        trajectory.push_back(p1);
    }
    return trajectory;
}

void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses) {
    if (poses.empty()) {
        cerr << "Trajectory is empty!" << endl;
        return;
    }

    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Trajectory 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);

        glLineWidth(2);
        for (size_t i = 0; i < poses.size() - 1; i++) {
            glColor3f(1 - (float) i / poses.size(), 0.0f, (float) i / poses.size());
            glBegin(GL_LINES);
            auto p1 = poses[i], p2 = poses[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }

}

CMakeLists.txt

CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
PROJECT( DrawTrajectory )
SET( CMAKE_CXX_FLAGS "-std=c++11 -O3")

FIND_PACKAGE(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})

FIND_PACKAGE(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})

include_directories("/usr/include/eigen3")

add_executable(draw_trajectory draw_trajectory.cpp)
target_link_libraries(draw_trajectory ${Sophus_LIBRARIES})
target_link_libraries(draw_trajectory ${Pangolin_LIBRARIES})

4 习题四

4.1 图像去畸变（3分，约1小时）

解答

CMAKE_MINIMUM_REQUIRED( VERSION 2.8)
PROJECT( UndistortImage )

set(CMAKE_CXX_FLAGS "-std=c++11")

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
add_executable(undistort_image undistort_image.cpp)
target_link_libraries(undistort_image ${OpenCV_LIBS})

#include <opencv2/opencv.hpp>
#include <string>
#include <cmath>

using namespace std;

string image_file = "../test.png";   // 请确保路径正确

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

    // 本程序需要你自己实现去畸变部分的代码。尽管我们可以调用OpenCV的去畸变，但自己实现一遍有助于理解。
    // 畸变参数
    double k1 = -0.28340811, k2 = 0.07395907, p1 = 0.00019359, p2 = 1.76187114e-05;
    // 内参
    double fx = 458.654, fy = 457.296, cx = 367.215, cy = 248.375;

    cv::Mat image = cv::imread(image_file,0);   // 图像是灰度图，CV_8UC1
    int rows = image.rows, cols = image.cols;
    cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC1);   // 去畸变以后的图

    // 计算去畸变后图像的内容
    for (int v = 0; v < rows; v++)
        for (int u = 0; u < cols; u++) {

            double u_distorted = 0, v_distorted = 0;
            // TODO 按照公式，计算点(u,v)对应到畸变图像中的坐标(u_distorted, v_distorted) (~6 lines)
            // start your code here
            double x = (u - cx)/fx;
            double y = (v - cy)/fy;
            double r = sqrt(x*x + y*y);
            double x_distorted = x*(1 + k1*pow(r,2) + k2*pow(r,4)) + 2*p1*x*y + p2*(r*r + 2*x*x);
            double y_distorted = y*(1 + k1*pow(r,2) + k2*pow(r,4)) + p1*(r*r + 2*y*y) + 2*p2*x*y;
            u_distorted = fx*x_distorted + cx;
            v_distorted = fy*y_distorted + cy;
            // end your code here

            // 赋值 (最近邻插值)
            if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows) {
                image_undistort.at<uchar>(v, u) = image.at<uchar>((int) v_distorted, (int) u_distorted);
            } else {
                image_undistort.at<uchar>(v, u) = 0;
            }
        }

    // 画图去畸变后图像
    cv::imshow("image undistorted", image_undistort);
    cv::waitKey();

    return 0;
}

4.2 双目视差的使用（2分，约1小时）

解答

CMAKE_MINIMUM_REQUIRED( VERSION 2.8 )
PROJECT( Disparity )
SET(CMAKE_CXX_FLAGS "-std=c++11 -O3")

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})
include_directories("/usr/include/eigen3")

add_executable(disparity disparity.cpp)

target_link_libraries(disparity ${OpenCV_LIBS})
target_link_libraries(disparity ${Pangolin_LIBRARIES})

#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 = "../left.png";
string right_file = "../right.png";
string disparity_file = "../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 b = 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;

    // TODO 根据双目模型计算点云
    // 如果你的机器慢，请把后面的v++和u++改成v+=2, u+=2
    for (int v = 0; v < left.rows; v+=2)
        for (int u = 0; u < left.cols; u+=2) {
            Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); // 前三维为xyz,第四维为颜色

            // start your code here (~6 lines)
            // 根据双目模型计算 point 的位置
            unsigned short disp = disparity.at<unsigned short>(v, u);
            if (disp==0)
            {
                continue;
            }
            double depth = (fx*b*1000)/disp;
            point(0) = depth*(u - cx)/fx;
            point(1) = depth*(v - cy)/fy;
            point(2) = depth;
            pointcloud.push_back(point);
            // end your code here
        }

    // 画出点云
    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;
}

4.3 矩阵微分（2分，约1.5小时）

解答

4.4 高斯牛顿法的曲线拟合实验（3分，约2小时）

解答

CMAKE_MINIMUM_REQUIRED( VERSION 2.8 )
PROJECT( GAUSSNEWTON )

find_package(OpenCV REQUIRED)
include_directories( ${OpenCV_INCLUDE_DIRS} )
include_directories( "/usr/include/eigen3" )

add_executable(gaussnewton gaussnewton.cpp)

target_link_libraries(gaussnewton ${OpenCV_LIBRARIES})

#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值
    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 = 0;   // 第i个数据点的计算误差
            error = yi - exp(ae*xi*xi + be*xi + ce); // 填写计算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;
        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;
}