视觉SLAM十四讲学习第一部分（ch1-ch6）

Debug过程中遇到的问题

0、写在前言

题头诗：

初尝视觉SLAM，代码头挠秃
代码好在哪，看完云雾里
度娘google来回切，方知知易行难

这篇Debug的记录是按照章节组织的，内容包括但不限于如下的内容：

1. 环境配置
2. 代码解析
3. 运行报错
4. 其他杂项

每一个内容讲述的要点我都在小标题里有比较详细的说明，可以先查看目录看看是否有你遇到的疑问。

一、ch3

1. 运行报错：cannot find trajectory file at trajectory.txt

这个问题出现的原因在于，cmake过后的文件路径是从cmake文件出发的，可以看我利用以下代码输出的当前路径，路径输出代码如下：

char *buffer;
    //也可以将buffer作为输出参数
    if((buffer = getcwd(NULL, 0)) == NULL)
    {
        perror("getcwd error");
    }
    else
    {
        printf("%s\n", buffer);
        free(buffer);
    }

而输出的结果为：

/home/zhuhj/slambook2/ch3/cmake-build-debug/examples

PS：因为我使用的clion生成的项目，所以build名称是cmake-build-debug
所以，直接针对cpp文件来写相对路径的思路是不对的，正确的相对路径应为：

string trajectory_file = "../../examples/trajectory.txt"; //后退两次

2. 代码解析：vector<Isometry3d,Eigen::aligned_allocator<Isometry3d>>

vector<type>一般不就好了吗，为什么还需要aligned_allocator?
这是因为，对eigen中的固定大小的类使用STL容器的时候，如果直接使用会出错，向量化时必须要求向量明确给定出需要分配的内存大小。（而int，char这些是c++自己定义好的，所以我们平常写的时候忽略了这一部分），所以aligned_allocator在做一件事，告诉vector容器Isometry3d的内存大小。

3. pangolin画图解析

首先是pangolin的初始化配置

// 创建绘图窗口
pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
// 启动深度检测（即遮掩物体将被剔除）
glEnable(GL_DEPTH_TEST);
// 启用混合（色块在同一像素点下以叠加方式处理而不是覆盖），只能适用于RGB模式
glEnable(GL_BLEND);
// 设置混合时源因子与目标因子间的混合权重
// 混合方式与两个参数的意义见下表
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
// 创建一个观测相机视图（类比unity的主视角）
pangolin::OpenGlRenderState s_cam(
	// ProjectionMatrix-投影矩阵，六个参数分别为(left, right, bottom, top, near和far边界值)
	//near和far就代表着相机可拉伸的深度
	pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
	// ModelViewLookAt 定义观测方向向量
	// (0,-0.1,-1.8) - 观测点位置：(mViewpointX mViewpointY mViewpointZ)
	// (0,0,0) 观测目标位置
	// (0.0,-1.0,0.0) 观测的方位向量
	pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
);
// 定义地图面板
// SetBound最后一个参数（-1024.0f/768.0f）为显示长宽比
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));

然后是根据传入的vector进行绘图

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++) {
      // 画每个位姿的三个坐标轴
      // 获得平移向量
      // 对于一个机器人来说，平移向量就代表着从世界坐标轴原点移动到当前坐标轴原点
      Vector3d Ow = poses[i].translation();  
      // 获得坐标轴变换向量
      // 原点受平移向量影响，当前坐标轴则受旋转和平移同时影响，将世界坐标轴乘以变换矩阵就可以得到变换后的坐标轴
      // 旋转后的坐标轴就构成了机器人的观测维（一般依据的是相机的变换而建立得到），观测维观测到的结果是在其当前坐标系下的结果，可以反变换映射到世界坐标系
      Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0)); 
      Vector3d Yw = poses[i] * (0.1 * Vector3d(0, 1, 0));
      Vector3d Zw = poses[i] * (0.1 * Vector3d(0, 0, 1));
      glBegin(GL_LINES);
      // 设定RGB颜色
      glColor3f(1.0, 0.0, 0.0);
      // 画连线
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Xw[0], Xw[1], Xw[2]);
      glColor3f(0.0, 1.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Yw[0], Yw[1], Yw[2]);
      glColor3f(0.0, 0.0, 1.0);
      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
  }

二、ch4

1. 环境配置：sophus版本有问题

slambook2代码提供的sophus直接cmake是有问题的，可以直接去git仓库下载下来，这个版本是正确无误的（前提是安装好eigen3库且版本在3.3以上）
Eigen3的版本可以去以下路径查看：

/usr/include/eigen3/Eigen/src/Core/util/Macros.h

// 版本信息
#define EIGEN_WORLD_VERSION 3
#define EIGEN_MAJOR_VERSION 3
#define EIGEN_MINOR_VERSION 7
// 这表示当前版本是3.3.7

sophus从git仓库备份下来的方式就是常见的那一套，如下所示：

// 因为备份时会自动新建Sophus文件夹，不需要提前新建
git clone https://github.com/strasdat/Sophus.git
cd ./Sophus/
mkdir build
cd ./build
cmake ..
make
sudo make install

这样就会在usr/local/include下新建好一个包含sophus头文件的文件夹，但是请注意，到这一步还不算完成，直接编译ch4的代码会报一个严重的错误：

Fatal error !! cannot find <fmt/core.h>

是的，这是因为缺少fmt头文件导致的，所以还需要去git下载编译fmt

git clone https://github.com/fmtlib/fmt.git
(PS: 如果github上不去，可以复制该链接，到码云中去找相同的仓库，然后再用码云相同仓库的链接替换即可：https://gitee.com/leizh74/fmt.git)
cd ./fmt
mkdir build
cmake ..
make
sudo make install

但是，实际使用过程中，如果直接编译ch4未做任何修改，你还会获得如下报错

/usr/bin/ld: CMakeFiles/useSophus.dir/useSophus.cpp.o: in function `std::make_unsigned<int>::type fmt::v7::detail::to_unsigned<int>(int)':
/usr/local/include/fmt/core.h:396: undefined reference to `fmt::v7::detail::assert_fail(char const*, int, char const*)'
/usr/bin/ld: CMakeFiles/useSophus.dir/useSophus.cpp.o: in function `fmt::v7::format_error::format_error(char const*)':
/usr/local/include/fmt/format.h:798: undefined reference to `vtable for fmt::v7::format_error'
/usr/bin/ld: CMakeFiles/useSophus.dir/useSophus.cpp.o: in function `fmt::v7::detail::throw_format_error(char const*)':
/usr/local/include/fmt/format.h:832: undefined reference to `fmt::v7::format_error::~format_error()'

这样的报错一般是由于libraray文件路径未指定/未导入，所以还需要再CMakelist.txt文件中，指定链接fmt库（注意，在target_link_libraries中，应遵循调用层次，从依赖到被依赖，比如这里useSophus依赖于Sophus，Sophus依赖于fmt）

target_link_libraries(useSophus ${Sophus_LIBRARIES} fmt)

这样，ch4的代码已经可以完整运行了
另外，如果你报fmt::v8的错误，那就是你fmt版本太高了，和sophus不匹配的原因（使用我给的链接不会存在这个问题，只是记录一下这个问题）

2. 代码解析：sophus下李群和李代数的转换

sophus下李群和李代数的转换与真实代数下是有区别的，真实代数下，李代数不是做一一次指数运算直接变成的李群，而是先转换成反对称矩阵，再由反对称矩阵做矩阵的指数运算转换成李群；同理，李群转换成李代数也是先做矩阵的对数运算，再由反对称矩阵转换到李代数。
而在sophus下，不需要我们去完成反对称矩阵与向量间的转换，这简化了中间的操作：

// 李代数到李群
Vector3d so3(1,0,0);
Sophus::SO3d SO3=Sophus::SO3d::exp(so3); //这里需指明exp的来源，避免出现误解

// 李群到李代数
Vector3d result=SO3.log()

三、ch5

1. 环境配置：安装opencv的前置依赖项出现的一系列问题

写书时不注意细节，给安装者带来了太多的问题啊，书里给出的前置依赖项安装如下代码所示：

sudo apt−get install build−essential libgtk2.0−dev libvtk5−dev libjpeg−dev libtiff4−dev libjasper−dev libopenexr−dev libtbb−dev

直接用该代码安装，会报apt-get不存在，其实在报这个错的时候我就应该发现的，那就是这行代码有着一个严重的错误，“-”号用的是中文全角，ubuntu无法识别！将全角符号全部替换，正确的代码如下：

sudo apt-get install build-essential libgtk2.0-dev libvtk5-dev libjpeg-dev libtiff4-dev libjasper-dev libopenexr-dev libtbb-dev

无法找到apt-get以及无法找到安装包的问题解决（还查找了一些相关资料，有说无法找到包的原因是安装源版本低，需要更新，参考：链接地址，这也会是无法找到安装包的可能原因，但本文不是这个原因）
之后会遇到libvtk5-dev的报错如下：

下列软件包有未满足的依赖关系：
 libvtk5-dev : 依赖: libqt4-dev 但是它将不会被安装
               依赖: libvtk5.10 (= 5.10.1+dfsg-2.1build1) 但是它将不会被安装

需要注意的是，这个问题不是前置安装包缺失，而是同名安装包的版本过高，libvtk无法完成依赖，这种情况，我们可以使用aptitude来处理这个问题，它可以一键完成高版本依赖包的删除以及需要的低版本依赖包安装，具体的处理代码如下：

sudo apt-get install aptitude
sudo aptitude install libvtk5-dev

注意的是，aptitude可以给出多个方案（但是是一个一个给出），最开始给出的方案比较保守，对于缺乏的依赖都是采取[不安装]的处理办法，遇到这种情况选择n否定该方案，直到aptitude给出不包含[不安装]的处理方案，选择y确定
这个过程中可能出现bug，我们可以再度运行sudo apt-get install XXX（也就是本段第二句代码）查看有哪些安装完毕，有哪些还有问题，并不断使用aptitude更改有问题的安装包，直到sudo apt-get install XXX后反馈如下：

则表明所有的依赖均安装完毕

2. 环境配置：安装opencv

在编译opencv的cmake工程时，出现了fatal error，报错说缺乏<vtkVersionMacros.h>头文件，vtk嚯，这不是之前安装的依赖vtk5-dev吗？是什么原因导致头文件缺失？查找资料确定，是因为vtk5的版本问题，安装vtk6即可解决，我们通过sudo apt-get install vtk6-dev，再使用apt-file查找头文件是否安装成功，查找结果如下图：

可以看到，该头文件在安装vtk6之后，已经可以成功查询到！
之后再重新make没有问题，再sudo make install后可以在usr/local/include中看到已经生成了opencv头文件库

3. 其他杂项：运行imageBasics

在运行之前，我想首先对c++main函数的argc和argv参数进行讲解，讲解内容来源于博客：地址
首先是int argc参数，argc参数表示命令行中参数的个数，其值是在输入命令时由系统按实际参数的个数自动赋值的
然后是char **argv参数，argv参数是字符串指针数组，存放命令行中的参数，长度即为参数个数argc
这是在调用可执行程序时，由命令行附加给main函数的，例如本例中的调用程序以及附加参数的代码如下：

build/imageBasics ubuntu.png xxx.png //同时附上两个图片的地址（可相对也可绝对）中间以空格隔开

而如果使用IDE，例如我使用的是CLION，又该如何完成这一附加操作？
以下内容来源于博客：地址
1、在顶部菜单栏的run中找到Edit configurations
2、在左侧选择配置的项目，如本例imageBasics
3、在 Working directory 选择运行路径（本例直接选择了图片所在文件夹）
4、在Program arguments输入参数（本例为两张图片具体路径，从上一步的设置出发，可以直接写相对路径“ ./ ”确定图片位置即可），多个参数通过空格隔开
5、点击apply并run即可
最终配置如下图所示：

4. 代码解析：stereoVision重要代码句解析

stereoVision算法的目的是实现双目视觉下的点云地图生成（即3维深度地图），这里我们对stereoVision.cpp中比较重要的一些代码进行解读：

sgbm->compute(left, right, disparity_sgbm); //通过sgbm方法生成视差矩阵

解析：
1、 视差是左右视图中描述相同现实物体的像素间的差
2、 视差可以直接反应深度（通过相似三角形），视差越大距离越近
3、 sgbm是一种计算视差图的方法，更为简单的计算视差图方法还有SAD
4、 视差图就是以某一图为基准（一般以左视图），矩阵大小为图大小，矩阵值为像素对应视差
5、 sgbm计算视差图是一种全局匹配的思想，保证在所有像素匹配误差最小的情况下计算视差
6、 SAD则是一种局部匹配思想，所以其计算速度更快，准确率要低一些

disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f); //将生成的16位整形MAT转换成32位浮点型MAT，除以16得到真实视差值

解析：
之所以除以16，是因为SGBM在计算时以16位整形存储，为了保证精度，扩大了16倍（多保留4bit），所以在还原成浮点型时，需要除以16

5. 代码解析：joinMap重要代码解析

Sophus::SE3d T = poses[i];  //poses存储的机器人到世界坐标的位姿变换（T_RW取逆）

这一部分内容可以参看三维空间刚体运动时的小萝卜的例子，这里的T是T_WR，也就是从机器人坐标系到世界坐标系的变换向量。

Eigen::Vector3d pointWorld = T * point; // 可能出现多张图观测到世界坐标系下同一点，将通过色块混合处理重复

当同一三维空间点被多张图同时观测到，将通过色块融合处理重复（这是在pangolin中设置的）