视觉SLAM十四讲第二版代码运行问题记录

接下来是运行视觉SLAM十四讲第二版中我进行代码修改的部分：
ch3
useGeometry文件夹的CMakeLists.txt:
将add_executable( eigenGeometry eigenGeometry.cpp …/examples/coordinateTransform.cpp)
更改为：

add_executable( eigenGeometry eigenGeometry.cpp)

ch4
CMakeLists.txt中添加如下代码，添加对c++11的支持：

SET(CMAKE_CXX_FLAGS "-std=c++0x")
或者
set(CMAKE_CXX_FLAGS "-std=c++11")

ch5
imageBasics文件夹：

CMakeLists.txt更改为如下内容:

project(imageBasics)
#加入对c++11的支持
set(CMAKE_CXX_FLAGS "-std=c++11")
#ubuntu内置了openCV2的版本，这里的语句是选择使用openCV3
#并加入openCV3的路径
set(OpenCV_DIR /usr/local/opencv3/share/OpenCV)
find_package(OpenCV 3 REQUIRED)
include_directories( ${OpenCV_DIR} )
include_directories(/usr/local/opencv3/include)

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

add_executable(imageBasics imageBasics.cpp)
# 链接OpenCV库
target_link_libraries(imageBasics ${OpenCV_LIBS})

add_executable(undistortImage undistortImage.cpp)

cmake make之后，利用如下语句执行：

./imageBasics ubuntu.png

stereo：
CMakeLists.txt添加如下内容:
set(CMAKE_CXX_FLAGS “-std=c++11”)
set(OpenCV_DIR /usr/local/opencv3/share/OpenCV)
find_package(OpenCV 3 REQUIRED)
include_directories( ${OpenCV_DIR} )
include_directories(/usr/local/opencv3/include)

rgbd：
CMakeLists.txt添加如下内容:
set(CMAKE_CXX_FLAGS “-std=c++11”)
et(OpenCV_DIR /usr/local/opencv3/share/OpenCV)
find_package(OpenCV 3 REQUIRED)
include_directories( ${OpenCV_DIR} )
include_directories(/usr/local/opencv3/include)
并将pose.txt和depth、color文件夹都放进build中去，这样运行joinMap即可，参考了：参考链接。

ch6
CMakeLists.txt添加如下内容：

set(OpenCV_DIR /usr/local/opencv3/share/OpenCV)
find_package(OpenCV 3 REQUIRED)
include_directories( ${OpenCV_DIR} )
include_directories(/usr/local/opencv3/include)

ch7
如果报如下错误，则代表需要添加Opencv3的路径。

terminate called after throwing an instance of 'cv::Exception'
  what():  OpenCV(3.4.6) /home/fuyouao/opencv-3.4.6/opencv-3.4.6/modules/imgproc/src/color.cpp:182: error: (-215:Assertion failed) !_src.empty() in function 'cvtColor'

这本书的代码有关opencv的部分，都需要在每个CMakeLists.txt添加如下内容：

set(OpenCV_DIR /usr/local/opencv3/share/OpenCV)
find_package(OpenCV 3 REQUIRED)
include_directories( ${OpenCV_DIR} )
include_directories(/usr/local/opencv3/include)

运行程序示例（图片都放到build里去）：

 ./orb_cv 1.png 2.png

错误2：

error: (-215:Assertion failed) size.width>0 && size.height>0 in function 'imshow'

Aborted (core dumped)

表示虚拟机没链接摄像头，在虚拟机-可移动设备里把摄像头连接即可。
之后继续运行和安装还会再进行更新。

ch8 自己敲第一版project代码时遇到的问题
1、sophus相关的错误
第二版和第一版所用的sophus一个是模板类，一个是非模板类。我安装的是第二版的模板类，两个版本代码需要改动很多。
总之第一版的project里，头文件引入库的时候，se3.h和so3.h要修改为：se3.hpp和so3.hpp；using Sophus::SO3和using Sophus::SE3改为using Sophus::SO3d和using Sophus::SE3d，下面代码里有关的变量声明也要加上d； 但这次改了一圈也没解决问题，尤其是这个语句：
T_c_r_estimated_ = SE3(
SO3(rvec.at(0,0), rvec.at(1,0), rvec.at(2,0)),
Vector3d( tvec.at(0,0), tvec.at(1,0), tvec.at(2,0))
);
怎么改都不行，放弃这版本了。
如果是自己安装的sophus，要注意eigen和ceres的版本，可能需要更新，参考链接，又安了个fmt。
2、g2o相关的错误（参考链接，感谢）
报错是说g2o没有cmake_modules，或者是没有g20.cmake这样类似的问题。
因为我是自己照着书一点点敲代码，书上没有提g2o这里有个需要做的事，就是把git g2o时候的文件夹（或者是代码3rdparty的g2o文件夹），总之就是安装g2o的那个文件夹里的cmake_modules文件夹粘贴到项目根目录去（把报错说没找到的那几个cmake文件复制过来也可以）。

ch12 运行参考链接:
1、编译之前需要在dense_mono/CMakeFiles/dense_mapping.dir/dense_mapping.cpp文件，bool update 函数里面补充 上 return true（参考链接）。
2、error while loading shared libraries: libvtkRenderingCore-7.1.so.1: cannot open shared object file: No such file or directory：
（参考链接）

export LD_LIBRARY_PATH=/usr/local/lib:$LD_LIBRARY_PATH

3、surfel_mapping.cpp:31:9: error:
‘class pcl::MovingLeastSquares’
has no member named ‘setPolynomialFit’;
did you mean ‘setPolynomialOrder’?：

#按提示修改
mls.setPolynomialFit(polynomial_order > 1);
改为
mls.setPolynomialOrder(polynomial_order > 1);

课后习题部分
第三章 三微空间刚体运动
本章CMakeLists.txt皆是如下代码：

cmake_minimum_required(VERSION 2.8)
project(useEigen)

set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-O3")

# 添加Eigen头文件
include_directories("/usr/include/eigen3")
add_executable(eigenMatrix eigenMatrix.cpp)

1、提取矩阵左上角3*3大小的块，并赋值为单位矩阵。
重点：MatrixXd中的block函数可直接提取块，block(i,j)代表从矩阵的i行j列开始提取一个(a,b)大小的矩阵。索引i和j从0开始，即左上角第一个元素是（0,0）。

#include 

using namespace std;

#include 
// Eigen 核心部分
#include 
// 稠密矩阵的代数运算（逆，特征值等）
#include 

using namespace Eigen;

#define MATRIX_SIZE 5  //这里我设成5*5的矩阵


int main(int argc, char **argv) {
  
  //随机初始化，生成5*5大小的矩阵
  Eigen::MatrixXd matrix_f = Eigen::MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
  //MatrixXd中的block函数可直接提取块，block(i,j)代表从矩阵的i行j列开始提取一个(a,b)大小的矩阵。索引i和j从0开始，即左上角第一个元素是（0,0）。
  Eigen::MatrixXd matrix_part = matrix_f.block<3,3>(0,0);
  cout << "yuan:"<< matrix_f << endl;
  cout << "bufen:"<< matrix_part << endl;
  matrix_part = Matrix::Identity();
  cout << matrix_part << endl;

  return 0;
}

2、小萝卜二号坐标系下的坐标
重点：本题 q 和 t 表达的是Tcw，即从世界坐标系到相机坐标系的变换关系。
题目可理解为：世界坐标系下有一点P，它对应小萝卜一号相机坐标系下的点P1，也对应着小萝卜二号相机坐标系下的点P2。由给出的q1、t1、q2、t2，可以得到欧式变换矩阵T1与T2。
有如下两个等式，等式中P代表同一个点：
$P1=T1\ast P$
$P2=T2\ast P$
即世界坐标系下的点P通过旋转平移变换在两个相机坐标系下得到了P1和P2。
两个等式可以解出P2：
$P2=T2\ast T{1}^{-1}\ast P1$
代码即是以上思路的实现：

#include 

using namespace std;

#include 
// Eigen 核心部分
#include 
// 稠密矩阵的代数运算（逆，特征值等）
#include 
using namespace Eigen;

int main(int argc, char **argv) {
  
	Eigen::Quaterniond q1(0.55,0.3,0.2,0.2),q2(-0.1,0.3,-0.7,0.2);
	cout<

可以看到，欧式变换矩阵的赋值有两种写法。
一是先初始化成单位阵，再单独设置旋转矩阵（这里要先把四元数转换成旋转矩阵，用toRotationMatrix()）：

Eigen::Isometry3d T1=Eigen::Isometry3d::Identity();
T1.rotate(q1.toRotationMatrix());

二是直接在初始化时把值传进去，应该是会直接进行四元数和旋转矩阵的转换（网上没搜到，这两种结果一样，故猜测）：

Eigen::Isometry3d T1(q1);

3、$AX=b$ 的不同求解方法
高博士给出了三种例子，直接求、QR分解和cholesky分解。

#include 

using namespace std;

#include 
// Eigen 核心部分
#include 
// 稠密矩阵的代数运算（逆，特征值等）
#include 

using namespace Eigen;

#define MATRIX_SIZE 50

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

  // 解方程
  // 我们求解 matrix_NN * x = v_Nd 这个方程
  // N的大小在前边的宏里定义，它由随机数生成
  // 直接求逆自然是最直接的，但是求逆运算量大

  Matrix matrix_NN
      = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
  matrix_NN = matrix_NN * matrix_NN.transpose();  // 保证半正定
  Matrix v_Nd = MatrixXd::Random(MATRIX_SIZE, 1);

  clock_t time_stt = clock(); // 计时
  // 直接求逆
  Matrix x = matrix_NN.inverse() * v_Nd;
  cout << "time of normal inverse is "
       << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
  cout << "x = " << x.transpose() << endl;

  // 通常用矩阵分解来求，例如QR分解，速度会快很多
  time_stt = clock();
  x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
  cout << "time of Qr decomposition is "
       << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
  cout << "x = " << x.transpose() << endl;

  // 对于正定矩阵，还可以用cholesky分解来解方程
  time_stt = clock();
  x = matrix_NN.ldlt().solve(v_Nd);
  cout << "time of ldlt decomposition is "
       << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
  cout << "x = " << x.transpose() << endl;

  return 0;
}

第四章 李群与李代数
1、两个轨迹，并求RMSE （注释版本）

#include 
#include 
#include 
#include 
#include 

using namespace Sophus;
using namespace std;

string groundtruth_file = "./groundtruth.txt";
string estimated_file = "./estimated.txt";

//为某种数据类型定义新的名字
//eigen自己定义的内存分配器，即aligned_allocator。
typedef vector> TrajectoryType; 

void DrawTrajectory(const TrajectoryType >, const TrajectoryType &esti);

TrajectoryType ReadTrajectory(const string &path);//读取文本的函数,const引用参数在函数内不可改变

int main(int argc, char **argv) {
  TrajectoryType groundtruth = ReadTrajectory(groundtruth_file);
  TrajectoryType estimated = ReadTrajectory(estimated_file);
  assert(!groundtruth.empty() && !estimated.empty());
  assert(groundtruth.size() == estimated.size());

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

  DrawTrajectory(groundtruth, estimated);
  return 0;
}

TrajectoryType ReadTrajectory(const string &path) {
  ifstream fin(path);
  TrajectoryType 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::SE3d p1(Eigen::Quaterniond(qx, qy, qz, qw), Eigen::Vector3d(tx, ty, tz)); //q,t
    trajectory.push_back(p1);//在结尾插入元素
  }
  return trajectory;
}

void DrawTrajectory(const TrajectoryType >, const TrajectoryType &esti) {
  // 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);
//OpenGL会把源颜色和目标颜色各自取出，并乘以一个系数（源颜色乘以的系数称为“源因子”，目标颜色乘以的系数称为“目标因子”）
//然后相加，这样就得到了新的颜色。
//GL_SRC_ALPHA：表示使用源颜色的alpha值来作为因子。
//GL_DST_ALPHA：表示使用目标颜色的alpha值来作为因子。
//GL_ONE_MINUS_SRC_ALPHA：表示用1.0减去源颜色的alpha值来作为因子。
//GL_ONE_MINUS_DST_ALPHA：表示用1.0减去目标颜色的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 < 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];
      //p1.translation() SE3d p1中的平移向量，平移代表位置
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);//指定坐标(qx, qy, qz)
      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
  }

}

第五章 相机与图像
1、图像去畸变

	    //#include ``
	    double x=(u-cx)/fx,y=(v-cy)/fy;//像素点对应的相机平面坐标
	    double r=sqrt(pow(x,2)+pow(y,2));//坐标点到原点距离
            u_distorted=x*(1+k1*pow(r,2)+k2*pow(r,4))+2*p1*x*y+p2*(pow(r,2)+2*pow(x,2));
            v_distorted=y*(1+k1*pow(r,2)+k2*pow(r,4))+2*p2*x*y+p1*(pow(r,2)+2*pow(y,2));
            u_distorted=fx*u_distorted+cx;
            v_distorted=fy*v_distorted+cy;

2、双目摄像机

            double x = (u - cx) / fx;
            double y = (v - cy) / fy;
            double depth = fx * b / (disparity.at(v, u));//disparity求视差
            point[0] = x * depth;
            point[1] = y * depth;
            point[2] = depth;

            pointcloud.push_back(point);

第六章 非线性优化

数据结构的知识都忘光了：

// 代价函数的计算模型
struct CURVE_FITTING_COST {
  //构造函数，最后这里没有分号
  //构造函数数组初始化时调用
  CURVE_FITTING_COST(double x, double y) :_x(x), _y(y)  {}

  // 残差的计算
  template<typename T>
  bool operator()(
    const T *const abc, // 模型参数，有3维
    T *residual) const { //这个函数不能访问类中所有this所能调用的内存，即这是个只读函数
    residual[0] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2]); // y-exp(ax^2+bx+c)
    return true;
  }

  const double _x, _y;    // x,y数据
};

这章非常详细的参考链接

第七章 特征点视觉里程计
课后习题
5、solvePnP里有三种解法：P3P， EPnP，迭代法（默认）；opencv2里参数分别为CV_P3P，CV_EPNP，CV_ITERATIVE （opencv3里多了DLS和UPnP解法）。参考链接
主要修改：

solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false);