第四讲---第五讲学习笔记如下:
视觉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维向量ζ。
更正:
- 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)可以对双目相机进行算法处理。