《视觉SLAM十四讲》（第二版）程序

---

CH2

IDE的使用

Clion： 断点设置之后，逐行执行 Step Into ( F7 )，逐函数执行 Step Over ( F8 )。
注意0(0)和O(O)在IDE中的显示区别，0是中间有个点的，O看起来像0，中间没有原点。
Run -> Edit Configurations-> Program arugements可直接加入程序所需的文件等等，便于debug的使用，如ch5的图片。
Run -> Edit Configurations-> Working Directory可直接加入fstream所需的工作目录，便于debug或者run的使用，如ch4的轨迹文件。
TODO 类似与带书签属性的注释。
程序出错时，当出现一大堆error时，可以在error中找到 required from here 有对应的错误代码行提示的。

CMake

文件夹的命名和作用：
library是库文件，include是头文件，src是源文件（该源文件不一定是可执行程序的所需的源文件，也可以是生成所需的内部库的源文件）。
CMakeLists.txt:
library是库的意思，include是包含，directory是目录。
include_directories()添加目录，一般find_package()下紧挨着include_directories()找到外部库之后添加目录到工程中。

find_package(OpenCV 3 REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})

注意：OpenCV大小写和${NAME_INCLUDE_DIRS} 在target_link_libraries( ... ${NAME_LIBS}) DIRS和LIBS。在ORB_SLAM的CMakeLists中，使用${PROJECT_NAME} 变量作为中间量，把ORB_SLAM中所需的内部库和外部库一切连接起来。
set(CMAKE_CXX_STANDARD 11)是高版本，
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")是低版本。
在set(CMAKE_BUILD_TYPE "Release")如果要是用断点，修改"Release"为"Debug"。同时，如果有set(CMAKE_CXX_FLAGS "-O3")也要去掉"-O3"或者改成"-O0"，"-O3"表示优化程度最高，-O0表示不需要优化。
find_package具体用法1

---

CH3

Eigen

sudo apt-get install libeigen3-dev

libeigen3-dev:lib表示库，eigen3就是软件名称和版本，dev表示开发包。安装到/usr/include/目录中。
eigen求矩阵的特征值和特征向量的时候，得是实对称矩阵，才能保证对角化的成功。

---

ch3/useEigen/eigenMatrix.cpp

#include //计时所用
clock_t time_stt = clock(); // 计时
...
cout << "time of normal inverse is "
       << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;

clock()是计时函数，clock_t为其相关的数据类型，clock()表示该程序从启动到函数调用占用CPU的时间，但是函数是通过计算硬件滴答的数目（时钟周期）。CLOCKS_PER_SEC表示一秒钟内CPU运行的时钟周期数，用于将clock()函数的结果转化为以秒为单位的量。

// Eigen 核心部分
#include 
// 稠密矩阵的代数运算（逆，特征值等）
#include 
using namespace Eigen;
Matrix<typeName,a,b> value;//矩阵value类型为typeName,维数为aXb
Matrix<double,2,3> matrix_23;
Matrix<float,1,3> matrix_13;
Matrix3d value;//矩阵value类型为double，维数为3X3
Vector3d value;//向量value类型为double，维数为3X1
Vector3f value;//向量value类型为float，维数为3X1
value << a,b,c,...;//流式操作符，value的数值输入a,b,c等。
Vector3d v;v<< 1,2,3;//三维列向量数值为1,2,3
Matrix3d value = Matrix3d::Zero();   //value初始化为零矩阵
               = Matrix3d::Random(); //value为随机数矩阵
cout << "random matrix: \n" << matrix_33 << endl;
cout << "transpose: \n" << matrix_33.transpose() << endl;   // 转置
cout << "sum: " << matrix_33.sum() << endl;                 // 各元素和
cout << "trace: " << matrix_33.trace() << endl;             // 迹
cout << "times 10: \n" << 10 * matrix_33 << endl;           // 数乘
cout << "inverse: \n" << matrix_33.inverse() << endl;       // 逆
cout << "det: " << matrix_33.determinant() << endl;         // 行列式
// 特征值
// 实对称矩阵可以保证对角化成功
SelfAdjointEigenSolver<Matrix3d> value(matrix_33.transpose() * matrix_33);
cout << "Eigen values = \n" << value.eigenvalues() << endl;
cout << "Eigen vectors = \n" << value.eigenvectors() << endl;
// 解方程
// 我们求解 matrix_NN * x = v_Nd 这个方程
#define MATRIX_SIZE  100;
Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN
      = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);//动态矩阵
matrix_NN = matrix_NN * matrix_NN.transpose();  // 保证半正定
Matrix<double, MATRIX_SIZE, 1> v_Nd = MatrixXd::Random(MATRIX_SIZE, 1);
// 直接求逆自然是最直接的，但是求逆运算量大
Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd;
// 通常用矩阵分解来求，例如QR分解，速度会快很多
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
cout << "x = " << x.transpose() << endl;
// 对于正定矩阵，还可以用cholesky分解来解方程
x = matrix_NN.ldlt().solve(v_Nd);
cout << "x = " << x.transpose() << endl;

---

ch3/useGeometry/useGeometry.cpp

#include //用于表示math.h中的M_PI
#include //旋转、平移的表示
Matrix3d value = Matrix3d::Identity();//矩阵value单位化
AngleAxisd value(angle,axis);//旋转向量方向为axis所在方向，大小为angle（弧度制）所表示
AngleAxisd rotation_vector (M_PI/4,Vector3d(0,0,1))//沿z轴旋转45度
cout.precision(n);//结果保留n位有效数字
cout << "rotation matrix =\n" << rotation_vector.matrix() << endl
	 <<rotation_vector.toRotationMatrix();//将旋转向量转换成对应的旋转矩阵
Vector3d v(1, 0, 0);
Vector3d v_rotated = rotation_vector * v;
		 v_rotated = rotation_matrix * v;
Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); //(2, 1, 0)表示ZYX顺序，即旋转顺序为yaw pitch roll
cout << "yaw pitch roll = " << euler_angles.transpose() << endl;//euler_angles为绕对应顺序旋转了所得结果的角度（弧度制）
Isometry3d value = Isometry3d::Identity();//变换矩阵T，看上去为3维，实际上是4X4
value.rotate(_rotate);//按照_rotate旋转，_rotate可以是旋转矩阵、旋转向量、四元数
value.pretranslate(Vector3d(a,b,c));//按照三维列向量a,b,c进行平移
cout << "Transform matrix = \n" << T.matrix() << endl;
// 用变换矩阵进行坐标变换
Vector3d v_transformed = T * v;                              // 相当于R*v+t
cout << "v tranformed = " << v_transformed.transpose() << endl;
Quaterniond q = Quaterniond(_rotate);//_rotate可以是旋转向量也可以是旋转矩阵。
cout << "quaternion from _rotate = " << q.coeffs().transpose()
       << endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
v_rotated = q * v; // 注意数学上是qvq^{-1}
cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
  // 用常规向量乘法表示，则应该如下计算
cout << "should be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;

---

ch3/examples/coordinateTranform.cpp

  Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);
  q1.normalize();//单位四元数表示旋转，需要单位化
  q2.normalize();
  Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);
  Vector3d p1(0.5, 0, 0.2);

  Isometry3d T1w(q1), T2w(q2);//T1w.rotate(q1)也可表示为变换矩阵中旋转矩阵的部分
  T1w.pretranslate(t1);
  T2w.pretranslate(t2);

  Vector3d p2 = T2w * T1w.inverse() * p1;
  cout << endl << p2.transpose() << endl;

---

ch3/examples/plotTrajectory.cpp

void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses) //

如果STL容器中的元素是Eigen库数据结构，例如这里定义一个vector容器，元素是Isometry3d，则为vector，但此结构编译不会出错，只有在运行的时候出错。解决的方法很简单，定义改为如下vector>;其实上述的这段代码才是标准的定义容器方法，只是我们一般情况下定义容器的元素都是C++中的类型，所以可以省略，这是因为在C++11标准中，aligned_allocator管理C++中的各种数据类型的内存方法是一样的，可以不需要着重写出来。但是在Eigen管理内存和C++11中的方法是不一样的，所以需要单独强调元素的内存分配和管理。

{
  // create pangolin window and plot the trajectory
  pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);//创建3D窗口
  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, 0.0, 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(); i++) {
      //pose[i]点的三维坐标系
      Vector3d Ow = poses[i].translation();
      Vector3d Xw = poses[i] * (0.01*Vector3d(1, 0, 0));
      Vector3d Yw = poses[i] * (0.02*Vector3d(0, 1, 0));
      Vector3d Zw = poses[i] * (0.03*Vector3d(0, 0, 1));
      glBegin(GL_LINES);
      glColor3f(1.0, 0.0, 0.0);//x轴方向颜色red
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Xw[0], Xw[1], Xw[2]);

      glColor3f(0.0, 1.0, 0.0);//y轴方向颜色green
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Yw[0], Yw[1], Yw[2]);

      glColor3f(0.0, 0.0, 1.0);//z轴方向颜色blue
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Zw[0], Zw[1], Zw[2]);

      glEnd();
    }
    // 画出连线
    for (size_t i = 0; i < poses.size(); i++) {
      glColor3f(0.0, 0.0, 0.0);//各个位置连线的颜色
      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
  }
}

size_t在C语言中就有了。它是一种“整型”类型，里面保存的是一个整数，就像int, long那样。这种整数用来记录一个大小(size)。size_t的全称应该是size type，就是说“一种用来记录大小的数据类型”,便于程序的移植。

string trajectory_file = "./examples/trajectory.txt";

按照git clone的文件，由于在ch3/目录下有CMakeLists.txt直接在ch3/目录下编译，并且不切换到其他目录，直接在该目录下执行: cmake-build-debug/examples/plotTrajectory就会成功编译。是因为trajectory_file的路径显示examples/trajectory.txt。而在编译的时候(无论是build还是cmake-build-debug)都是默认在当前目录下寻找./examples/trajectory.txt。所以无论是在example/目录下还是正确编译之后(cmake-build-debug/examples/plotTrajectory)，只要运行可执行程序时，对应不到./examples/trajectory.txt都会报错。当然，也可以更改trajectory_file的路径。

int main(int argc, char **argv) {
  vector<Isometry3d, aligned_allocator<Isometry3d>> poses;
  ifstream fin(trajectory_file);//文件输入
  if (!fin) //文件检测
  {
    cout << "cannot find trajectory file at " << trajectory_file << endl;
    return 1;
  }

  while (!fin.eof()) //循环
  {
    double time, tx, ty, tz, qx, qy, qz, qw;
    fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;//按照给定顺序的输入流
    Isometry3d Twr(Quaterniond(qw, qx, qy, qz));
    Twr.pretranslate(Vector3d(tx, ty, tz));
    poses.push_back(Twr);
  }
  cout << "read total " << poses.size() << " pose entries" << endl;

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

---

CH4

Sophus

SLAM十四讲第一版用的sophus是非模板，第二版用的是模板类。模板类是基于eigen3.2.92版本，非模板是基于eigen3.3.7。虽然书上说Sophus不用安装，但是有时程序会找不到头文件，还是需要sudo make install。find_package是寻找SophusConfig.cmake文件，此文件说明了你的Sophus库安装在哪里，如果只是make了，没有make install，那把Sophus的目录就放在当前项目根目录中，这个工程中的find_package会在当前目录中找这个配置文件。
当系统既安装了模板类，也安装了非模板类的Sophus时，在使用模板类时，正常写出CMakelists即可，而使用非模板类时，需将target_link_libraries中的${Sophus_LIBRARIES}改为具体路径，如/usr/local/lib/libSophus.so等（在Sophus非模板的源文件中，找到SophusConfig.cmake中，对应的${Sophus_LIBRARIES}变量即可）

ch4/useSophus.cpp

#include "sophus/se3.hpp"//sophus的模板类头文件
using namespace Eigen;
int main(int argc, char **argv) {
  // 沿Z轴转90度的旋转矩阵
  Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
  // 或者四元数
  Quaterniond q(R);
  Sophus::SO3d SO3_R(R);              // Sophus::SO3d可以直接从旋转矩阵构造
  Sophus::SO3d SO3_q(q);              // 也可以通过四元数构造
  // 二者是等价的
  cout << "SO(3) from matrix:\n" << SO3_R.matrix() << endl;
  cout << "SO(3) from quaternion:\n" << SO3_q.matrix() << endl;

  // 使用对数映射获得它的李代数
  Vector3d so3 = SO3_R.log();//对数映射,求出的为向量\phi,而非矩阵\phi^\wedge
  cout << "so3 = " << so3.transpose() << endl;
  // hat 为向量到反对称矩阵
  cout << "so3 hat=\n" << Sophus::SO3d::hat(so3) << endl;
  // 相对的，vee为反对称到向量
  cout << "so3 hat vee= "<<Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << endl;

  // 增量扰动模型的更新
  Vector3d update_so3(1e-4, 0, 0); //假设更新量为这么多
  Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_R;//exp指数映射也为向量
  cout << "SO3 updated = \n" << SO3_updated.matrix() << endl;

  // 对SE(3)操作大同小异
  Vector3d t(1, 0, 0);           // 沿X轴平移1
  Sophus::SE3d SE3_Rt(R, t);           // 从R,t构造SE(3)
  Sophus::SE3d SE3_qt(q, t);            // 从q,t构造SE(3)
  cout << "SE3 from R,t= \n" << SE3_Rt.matrix() << endl;
  cout << "SE3 from q,t= \n" << SE3_qt.matrix() << endl;
  // 李代数se(3) 是一个六维向量，方便起见先typedef一下
  typedef Eigen::Matrix<double, 6, 1> Vector6d;
  Vector6d se3 = SE3_Rt.log();
  // 观察输出，会发现在Sophus中，se(3)的平移在前，旋转在后.
  cout << "se3 = " << se3.transpose() << endl;
   // 同样的，有hat和vee两个算符
  cout << "se3 hat = \n" << Sophus::SE3d::hat(se3) << endl;
  cout << "se3 hat vee = " << Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose() << endl;

  // 最后，演示一下更新
  Vector6d update_se3; //更新量
  update_se3.setZero();//六维零向量
  update_se3(0, 0) = 1e-4d;//（行，列）
  Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_Rt;
  cout << "SE3 updated = " << endl << SE3_updated.matrix() << endl;
}

---

ch4/example/trajectoryError.cpp

typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;

typedef:
1.定义一种类型的别名：

char* pa, pb; // 这多数不符合我们的意图，它只声明了一个指向字符变量的指针和一个字符变量
typedef char* P; 
P pa,pb;//即可

struct tag{int x;int y;}; 
struct tag p1;
//可以改成
typedef struct tag{int x; int y;}P; 
P p1;

详见https://blog.csdn.net/andrewniu/article/details/80566324#commentBox

assert(!groundtruth.empty() && !estimated.empty());
assert(groundtruth.size() == estimated.size());

assert:预处理宏（所谓预处理宏就是一个预处理变量，行为有点类似与内联函数）。用法：assert(expr)；如果expr的值为0/假，assert输出信息并终止程序的执行。如果expr的值为1/真，assert什么也不做。

void DrawTrajectory(const TrajectoryType &gt, const TrajectoryType &esti) {

  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 < gt.size() - 1; i++) {
      glColor3f(0.0f, 0.0f, 1.0f);  // blue for ground truth
      glBegin(GL_LINES);
      auto p1 = gt[i], p2 = gt[i + 1];//真实轨迹，顶点需要写3个
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }

    for (size_t i = 0; i < esti.size() - 1; i++) {
      glColor3f(1.0f, 0.0f, 0.0f);  // red for estimated
      glBegin(GL_LINES);
      auto p1 = esti[i], p2 = esti[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
  }

// compute rmse
  double rmse = 0;
  for (size_t i = 0; i < estimated.size(); i++) {
    Sophus::SE3d p1 = estimated[i], p2 = groundtruth[i];//p1p2为李代数形式
    double error = (p2.inverse() * p1).log().norm();//norm()为二范数
    rmse += error * error;
  }
  rmse = rmse / double(estimated.size());
  rmse = sqrt(rmse);
  cout << "RMSE = " << rmse << endl;

---

CH5

OpenCV

图像
在程序中，图像的二维数组的第一个下标是数组的行，第二个下标为数组的列，分别对应图像的高度和宽度。一张宽度为640像素，高度为480像素分辨率的灰度图可表示为：

unsigned char image[480][640];

则对应着x,y处灰度值的像素

unsigned char pixel = image[y][x];

ch5/imageBasics/imageBasics.cpp

#include 
#include 

int main(int argc, char **argv) {
  // 读取argv[1]指定的图像
  cv::Mat image;
  image = cv::imread(argv[1]); //cv::imread函数读取指定路径下的图像
  }

带形参的main函数，如 main( int argc, char* argv[], char ** env ) ，是UNIX、Linux以及Mac OS操作系统中C/C++的main函数标准写法，并且是血统最纯正的main函数写法。
第一个参数，int型的argc，用来统计程序运行时发送给main函数的命令行参数的个数；第二个参数，char*型的argv[]，为字符串数组，用来存放指向的字符串参数的指针数组，每一个元素指向一个参数。各成员含义如下： argv[0]指向程序运行的全路径名，argv[1]指向在DOS命令行中执行程序名后的第一个字符串，argv[2]指向执行程序名后的第二个字符串，argv[3]指向执行程序名后的第三个字符串，argv[argc]为NULL；第三个参数，char**型的env，为字符串数组。env[]的每一个元素都包含ENVVAR=value形式的字符串，其中ENVVAR为环境变量，value为其对应的值。平时使用到的比较少。
详见https://blog.csdn.net/dgreh/article/details/80985928

  // 判断图像文件是否正确读取
  if (image.data == nullptr) { //数据不存在,可能是文件不存在
    cerr << "文件" << argv[1] << "不存在." << endl;
    return 0;
  }

  // 文件顺利读取, 首先输出一些基本信息
  cout << "图像宽为" << image.cols << ",高为" << image.rows << ",通道数为" << image.channels() << endl;
  cv::imshow("image", image);      // 用cv::imshow显示图像
  cv::waitKey(0);                  // 暂停程序,等待一个按键输入

  // 判断image的类型
  if (image.type() != CV_8UC1 && image.type() != CV_8UC3) {
    // 图像类型不符合要求
    cout << "请输入一张彩色图或灰度图." << endl;
    return 0;
  }

  // 遍历图像, 请注意以下遍历方式亦可使用于随机像素访问
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  for (size_t y = 0; y < image.rows; y++) {
    // 先用图像的行指针
    unsigned char *row_ptr = image.ptr<unsigned char>(y);  // row_ptr是第y行的头指针
    for (size_t x = 0; x < image.cols; x++) {
      // 再用图像的列指针
      unsigned char *data_ptr = &row_ptr[x * image.channels()]; // data_ptr 指向待访问的像素数据
      // 输出该像素的每个通道,如果是灰度图就只有一个通道
      for (int c = 0; c != image.channels(); c++) {
        unsigned char data = data_ptr[c]; // data为I(x,y)第c个通道的值
      }
    }
  }
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast < chrono::duration < double >> (t2 - t1);
  cout << "遍历图像用时：" << time_used.count() << " 秒。" << endl;

  // 对于图像还有很多基本的操作,如剪切,旋转,缩放等,限于篇幅就不一一介绍了,请参看OpenCV官方文档查询每个函数的调用方法.
  cv::destroyAllWindows();
  return 0;
}

ch5/imageBasics/undistortedImage.cpp

---

atan和atan2
atan(y/x)是正常的反正切函数，数值只与y/x的数值有关，而atan2(x,y)是四象限反正切函数,比前者多了个x,y本身有关。