视觉SLAM十四讲学习笔记-第四讲---第五讲学习笔记总结---李群和李代数、相机

第四讲---第五讲学习笔记如下：

视觉SLAM十四讲学习笔记-第四讲-李群与李代数基础和定义、指数和对数映射_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲-李代数求导与扰动模型_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲-Sophus实践、相似变换群与李代数_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第五讲-相机模型_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第五讲-图像和实践_goldqiu的博客-CSDN博客_global shutter

前三讲总结：

视觉SLAM十四讲学习笔记---前三讲学习笔记总结之SLAM的作用、变换和位姿表示_goldqiu的博客-CSDN博客

第四讲：

总结：

1.在SLAM中, 位姿作为状态变量是未知的，需要解决什么样的相机位姿最符合当前观测数据这样的问题。一种典型的方式是把它构建成一个优化问题，求解最优的R, t，使得误差最小化。旋转矩阵自身是带有约束的（正交且行列式为1）。它们作为优化变量时，会引入额外的约束，使优化变得困难。通过李群——李代数间的转换关系能够把位姿估计变成无约束的优化问题，简化求解方式。

2. 外积引入了∧符号，将一个向量变成了反对称矩阵。同理，对于任意反对称矩阵，亦能找到一个与之对应的向量，把这个运算用符号∨表示。（向量加了∧符号变成反对称矩阵，矩阵加了∨符号变成对应的向量）

3. 经过公式推导：每对旋转矩阵求一次导数，只需左乘一个 ϕ(t)∧矩阵（反对称矩阵）即可。最终有：

说明在 t = 0 附近，旋转矩阵可以由 exp (ϕ0∧t)计算出来，即旋转矩阵 R 与另一个反对称矩阵 ϕ0∧t 通过指数关系发生了联系。我们可以知道：给定某时刻的R，就能求得一个ϕ，它描述了R在局部的导数关系。ϕ正是对应到SO(3) 上的李代数so(3)；李代数（ϕ）到李群（R）是指数关系，而李群（R）到李代数（ϕ）是对数关系。

4. ϕ是一种李代数。SO(3)对应的李代数是定义在R3上的向量，记作ϕ，每个ϕ都可以生成一个反对称矩阵；而se(3)可以理解成由一个平移加上一个so(3)元素构成的向量。

5. so(3)实际上就是由旋转向量组成的空间，而指数映射即罗德里格斯公式。通过指数映射，把so(3)中任意一个向量对应到了一个位于SO(3)中的旋转矩阵。反之，如果定义对数映射，也能把SO(3)中的元素对应到so(3)中。SO(3)与so(3)的结论和前面的旋转向量与旋转矩阵很相似，而指数映射即是罗德里格斯公式。旋转矩阵的导数可以由旋转向量指定，指导着如何在旋转矩阵中进行微积分运算。总结如下图：

6. 李代数计算使用Sophus库，其使用方法也是网上查找库使用说明。

7. 在单目视觉中有用到相似变换群 Sim(3)，其李代数 sim(3) 元素是一个7维向量ζ。

更正：

1. Sophus库的基本使用操作：

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;
  cout << "they are equal" << endl;

  // 使用对数映射获得它的李代数
  Vector3d so3 = SO3_R.log();
  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;
  cout << "SO3 updated = \n" << SO3_updated.matrix() << endl;

  cout << "*******************************" << 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();
  cout << "se3 = " << se3.transpose() << endl;
  // 观察输出，会发现在Sophus中，se(3)的平移在前，旋转在后.
  // 同样的，有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-4;
  Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_Rt;
  cout << "SE3 updated = " << endl << SE3_updated.matrix() << endl;

  return 0;
}

其主要步骤如下：

a. 用旋转矩阵或者四元数定义李群

b. 通过李群的对数映射获得李代数。

c. 在SLAM中，增量扰动模型很关键，对扰动向量进行对数映射，并左乘到原矩阵，就完成了增量扰动的更新。

注意：对于so3和se3都操作都是类似的，在程序中由于对乘法运算发进行了重载，重载功能中完成了相关运算，所以调库编程比实际计算要简单得多。

2. 轨迹误差的评估：需要有一条估计的轨迹和一条真实的轨迹，真实的轨迹一般叫做groundtruth，一般是采用高精度设备事先去测得的。主要代码如下:

 // 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);

这里计算的是绝对轨迹误差（Absolute Trajectory Error, ATE），这是每个位姿李代数的均方根误差（Root-Mean-Squared Error, RMSE）。具体步骤是按照公式计算出每个位姿李代数，然后再做均方根。

第五讲：

总结：

1.单目相机对应的是针孔相机模型，而双目和RGB-D有对应的模型。相机由于透镜的存在和组装的误差存在畸变。

2.相机把三维世界中的信息转换成了一张由像素组成的照片，存储在计算机中，作为后续处理的数据来源。在数学中，图像可以用一个矩阵来描述；而在计算机中，它们占据一段连续的磁盘或内存空间，可以用二维数组来表示。然而计算机并不能表达实数空间，所以需要对下标和图像读数在某个范围内进行量化（类似于模拟到数字的概念）。在常见的灰度图中，用 0~255 的整数（一个 unsigned char或1 个字节）来表达图像的灰度读数。那么，一张宽度为 640 像素、高度为 480 像素分辨率的灰度图就可以表示为：

unsigned char image[480][640] //二维数组表达图像

在程序中，图像以二维数组形式存储。它的第一个下标是指数组的行，而第二个下标则是列。在图像中，数组的行数对应图像的高度，而列数对应图像的宽度。当访问某一个像素时，需要指明它所处的坐标。像素坐标系原点位于图像的左上角，X 轴向右，Y 轴向下（也就是u,v* 坐标）。如果还有第三个轴—Z 轴，根据右手法则，Z 轴向前。这种定义方式是与相机坐标系一致的。图像的宽度或列数，对应着X 轴；而图像的行数或高度，则对应着它的Y 轴。根据这种定义方式，访问一个位于 x,y 处的像素，那么在程序中应该是：

unsigned char pixel = image[y][x];  //访问图像像素

它对应着灰度值 I(x,y) 的读数。

3. 在 RGB-D 相机的深度图中，记录了各个像素与相机之间的距离。

4. 彩色图像的表示则需要通道（channel）的概念。在计算机中，用红色、绿色和蓝色这三种颜色的组合来表达任意一种色彩。于是对于每一个像素，就要记录其 R、G、B 三个数值，每一个数值就称为一个通道。最常见的彩色图像有三个通道，每个通道都由 8 位整数表示。在这种规定下，一个像素占据 24 位空间。在 OpenCV 的彩色图像中，通道的默认顺序是 B、G、R。也就是说，当得到一个 24 位的像素时，前 8 位表示蓝色数值，中间 8 位为绿色，最后 8 位为红色。如果还想表达图像的透明度，就使用 R、G、B、A 四个通道。

5. OpenCV提供了大量的开源图像算法，是计算机视觉中使用极广的图像处理算法库。其如何处理图像可以网上查找。如果电脑上有GPU并且安装了相关依赖项，OpenCV会把GPU加速打开。

6. 图像去畸变：通过cv::Undistort() 可以进行图像去畸变，但是得先进行相机内参的标定，这块opencv也提供了成熟的接口。可以参考以前写的相机内参标定的文章：

goldqiu：二十.激光、视觉和惯导LVIO-SLAM框架学习之相机内参标定

7. OpenCV实现的SGBM算法（Semi-global Batch Matching）可以对双目相机进行算法处理。