Visual SLAM 学习笔记（1）

本文为一个VSlam小白的学习之路。

本文所有代码在ubuntu 22.04下编写，ros2版本为humble。

原文链接：smoothly-vslam · 语雀

1. 背景

        近年来，随着人工智能的爆火，越来越多的行业开始将视角移向人工智能，希望借助人工智能这条快车道实现产业升级。其中，汽车，无人机等行业开始深入CV领域，将CV应用到无人驾驶方向。基于视觉的传感器在同步定位与测绘(SLAM)系统中表现出了显著的性能、准确性和高效率，使得其成为无人系统开发中不可或缺的一部分。

2. 相机投影与相机畸变

2.1 相机和相关坐标系

2.1.1 相机成像

        针孔相机模型是目前大多数相机的成像模型。小孔成像能够尽可能防止其它光线进入相机视野，从而成为了针孔相机的成像原理。小孔成像会在另一面形成一个倒立的呈比例缩小的像。一般而言，小孔越小，成像越清晰，但是图像会越暗。

        为了解决小孔成像越清晰越暗的问题，现代相机会使用透镜聚焦光线。对于透镜的物理原理不再赘述。

         在成像后，感光元件会在成像处接收。现代相机使用电子感光元件,也叫图像传感器(sensor)，经过模拟信号放大器放大，数模转换电路（DAC）变为数字图像，数字图像再经过ISP（Image Signal Processor）图像处理器进行数字图像处理，最后经过压缩编码算法储存到SD卡成为一张图片文件。

2.1.2 相机成像的几个坐标系

        相机成像的坐标系主要有四个：

        世界坐标系（下文简称世界系）：表示空间物体的绝对坐标，用(Xw, Yw, Zw)表示。世界系通过平移，旋转即可得到相机坐标系。

        相机坐标系（下文简称相机系）：以光心（可理解为透镜中心）为坐标系原点， Xc, Yc轴平行于图像坐标系的x,y轴，以光轴为Zc轴。

        图像坐标系（下文简称图像系）：以图像平面的中心为坐标原点，坐标用(x, y)来表示。

        像素坐标系（下文简称像素系）：表示三维物体在图像平面上的投影。像素是离散的，在从图像系到像素系的过程中需要离散化。坐标中心在图像的左上角。坐标用(u, v)来表示，u轴正方向水平向右，v轴正方向竖直向下。图像宽度为W，高度为H。

2.2 坐标变换

        对于从三维坐标投影到二维平面，由相似三角形可得：

        其中，f为成像焦距。

        为了方便计算，我们取对称（去掉负号），得：

       从而得到：

        在计算机中，图像是一一个个离散的像素构成的。而且，坐标原点换到了图像的左上角而非图像中心，y轴也由竖直朝上变为v轴竖直向下。所以，通过图像系和像素系之间的坐标差值cx,cy，和像素系中一个像素的长和宽，就可以得到投影坐标P'到像素系下的坐标：

        其中，ax, ay分别是每一个小像素的长和宽。再加上cx, cy，可得：

        引入线性代数，可以得到如下的矩阵运算：

        其中：

        这个就算我们常说的相机内参矩阵。

        通过内参矩阵，就可以把相机系下的三维坐标转换为像素系下的坐标。

        我们在上面之求解了从相机系到像素系的内参矩阵，那么，如果我们需要从世界系到像素系的转换呢？

        推导如下：

        从三维到二维，假设世界系和相机系不存在旋转，我们可以直接投影到像素系，得到：

        单位化得到：

        将fk合成为a,fl和成为b，得到： 

        以矩阵形式表示可得：

2.3 图像畸变

        实际情况下，相机由于其镜头的光学性质，会产生一定程度上的畸变，分为径向畸变和切向畸变。

2.3.1 径向畸变

        径向畸变来自透镜形状的不规则以及建模的方式，导致镜头不同部分焦距不同。光线在远离透镜中心的地方偏折更大，反之更小。

对于径向畸变，常用以下公式进行修正：

k1，k2，k3被称为矫正系数。

2.3.2 切向畸变

切向畸变是由于透镜制造上的缺陷使透镜和图像平面不平行而造成的。如下图：

        （Cheep?蚌住了）

        切向畸变常用如下公式修正：

        其中，p1，p2，为切向畸变系数。

        把切向畸变和径向畸变组合在一起，可得下面公式：

2.4 鱼眼相机

        鱼眼相机由于其复杂的光学结构和成像特性，能够获取更大的视野，但是，其径向畸变非常严重。

2.5 相机标定

        目前为止，我们所学的相机成像有两大转换过程：相机投影和畸变消除，主要由内参矩阵和畸变参数来决定。而求出这两个参数的过程，叫做相机标定。目前常借助棋盘格标定板来进行相机标定。标定板上下左右移动时，我们可以通过标定算法来求解以上参数。常用的标定算法有张正友标定法等。

思考      

1.叙述相机内参的物理意义。如果一部相机的分辨率变为原来的两倍而其他地方不变，那么它的内参将如何变化？

注：分辨率在不同场合意义不尽相同，这里取传感器的总的感光单元数量作为分辨率，而一些资料中会以某边排列的感光元件数量作为分辨率。在底片面积不变的情况下，分辨率变为原来两倍，指感光单元数量变为原来两倍。如1200w像素变为2400w像素。

答：由于 ， ， 现在ax和ay都变为原来的二分之一，那么和都变为原来的两倍。同样的，由于由于分辨率提高，相当于每个像素的长宽都为原来的1/2，那么cx, cy也会变为原来的两倍。

2.调研全局快门(global shutter)相机和卷帘快门(rolling shutter)相机的异同。它们在SLAM中有何优缺点？

答：全局快门相机和卷帘快门相机是两种常见的图像传感器工作方式。

        全局快门相机拥有以下优点：

        1. 消除了图像丢失：全局快门相机可以避免物体在快门打开期间移动而导致的图像失真问题，因为所有像素同时捕获图像。

        2. 高动态范围：由于全局快门模式下每个像素同时曝光，因此它们通常具有较高的动态范围，能够更好地捕捉亮度变化较大的场景。

        但同时也有缺点：

        1. 昂贵：全局快门相机相对来说更昂贵，因为需要更复杂的传感器设计来实现所有像素的同时曝光和读取。

        2. 读取速度较慢：由于所有像素同时读取，全局快门相机的帧率可能较低，特别是对于高分辨率的图像。

        而卷帘快门相机有以下优点：

        1. 成本较低：卷帘快门相机相对成本更低，因为它们的传感器设计相对简单。

        2. 更高的帧率：由于逐行读取图像，卷帘快门相机可以达到更高的帧率，特别是对于高分辨率的图像。

        缺点是：

        1. 图像失真：卷帘快门相机在拍摄运动物体或相机快速移动时容易出现图像失真，因为不同行的图像捕获时间不同，导致物体出现形变或扭曲。

        2. 动态范围较低：由于不同行的图像捕获时间差异，卷帘快门相机的动态范围可能较低，不能很好地处理亮度变化较大的场景。

        因此，全局快门相机通常更适用于需要准确时间同步和较高动态范围的SLAM应用。由于它们能够消除图像失真，可以提供更准确的相机位姿估计和地图构建。

        卷帘快门相机尽管在成本和帧率方面具有优势，但在SLAM中可能遇到图像失真和动态范围不足的挑战。但对于某些应用或场景，卷帘快门相机仍然可能是一个可行的选择。

3.  坐标系变换与外参标定

3.1 坐标系变换

        同一物体，在不同坐标系，不同基底下，坐标是不相同的。如果知道两个坐标系的基，就可以得到在A坐标系下的P点在B坐标系下的坐标，这就需要通过坐标系转换。

3.1.1 二维变换

         对于XOY坐标系下的点P，我们想将其转换到X'OY'坐标系下，由三角函数，可以轻松得到：

         P*(x*, y*):

        写成矩阵形式可得：

        其中矩阵R为从XOY到X'OY'的旋转矩阵。

        当然，坐标系之间不仅有旋转关系，还可以平移，如下图：

       这就简单多了：

         而如果我们把旋转和平移结合起来，就能得到：

 3.1.2 三维变换

         有了二维的变换矩阵，我们可以将其推广到三维：

         其中R是旋转矩阵，t是平移矩阵。

        我们一般把计算形式写成齐次式，如下：

         在原三维向量下，添加1后的坐标，称为齐次坐标，这个是几何变换中常用的数学技巧。

         当然，我们也可以再化简一下上面的形式：

         这就舒服多了~

         使用坐标系变换，我们就可以把一个世界系坐标转化为相机系坐标。当然，我们也可以通过逆过程将相机系坐标转换到世界系下。

3.2 相机外参

3.2.1  定义

         对于一般载体而言，相机只是载体的视觉传感器（sensor），而多数情况下，我们假设载体作为世界系坐标原点，那么相机系一般不会与世界系重合，这时候我们就需要用到变换矩阵来进行坐标转换。而这个变换矩阵，就是相机外参。

        通俗来讲，相机外参描述了相机安装在载体的什么地方，安装角度是怎样的。

 3.2.2 外参标定

        虽然在安装相机的时候，我们是可以得到相机与载体之间的夹角的。但是，如果载体运动过程中发生抖动，那么原来的夹角就不一定适用来。实时更新变换矩阵，进行外参标定，是一个更妥善的选择。我们可以采用以下几种工具来进行外参标定：

        OpenCV：既可以进行内参标定，也提供了相机外参标定相关的API。

        Matlab：Matlab也提供了相机内外参标定的toolbox，包含了单体和立目的标定工具。

        Kalib：一款强大且开源的相机标定工具支持单目，双目和多目相机的标定。

3.2.3 外参标定的意义

        1.特征匹配：如果我们得到了世界系下的一片点云，我们想要对其标注特征，如果外参标定不正确，将会错误地匹配特征。

        2.三维重建：进行三维重建时，我们需要将多张图片中的点匹配到世界系中，如果外参标定错误，图片中的点就会发生偏移，导致三维重建出错。

        3.目标跟踪：进行目标跟踪时，我们需要将跟踪目标从图片转换到世界系中。如果外参标定错误，就会导致跟踪失败。

        4.相机位姿估计：估计自身位姿时，需要将相机的多个图像的朝向匹配到世界系中。由于外参出错，我们无法正确估计位姿。

        总而言之，只要是和从相机系到世界系之间的转换相关的，外参标定都有很大的意义。

3.2.4 相机标定类型

        相机标定是指确定相机内部参数和外部参数的过程，以便在图像中恢复真实世界中的几何信息。常用的相机标定类型包括以下几种：

1. 内部标定：用于确定相机的内部参数，包括焦距、主点位置、畸变系数等。内部标定通常使用标定板等已知几何形状的物体，并采用校正方法进行计算。
2. 外部标定：用于确定相机在世界坐标系中的位置和方向。外部标定通常使用已知位置的标定板或者其他几何形状的物体，并采用三维重建方法进行计算。
3. 传感器到传感器标定：用于确定多个相机之间的相对位置和方向，以便进行双目或多目视觉处理。传感器到传感器标定通常使用标定板或者球形标定物等，采用立体匹配算法进行计算。
4. 传感器到车体标定：用于确定相机在车体坐标系中的位置和方向，以便进行车载视觉处理。传感器到车体标定通常使用车体固定的标定板或者其他几何形状的物体，采用三维重建方法进行计算。
5. 姿态标定：用于确定相机在平面或者空间中的朝向，以便进行视觉导航或者机器人控制。姿态标定通常使用旋转平台或者陀螺仪等设备，采用角度解算方法进行计算。
6. 相机-激光雷达标定：用于确定相机和激光雷达之间的相对位置和方向，以便进行三维点云重建或者障碍物检测。相机-激光雷达标定通常使用已知几何形状的标定板或者球形标定物，采用多视角几何约束方法进行计算。
7. 相机-IMU标定：用于确定相机和惯性测量单元（IMU）之间的相对位置和方向，以便进行惯性辅助导航或者姿态估计。相机-IMU标定通常使用运动平台或者旋转平台等设备，采用卡尔曼滤波或者优化方法进行计算。

3.3 相机标定示例代码

       我们假设载体上有一个相机，相机与其它传感器通过ROS的topic进行通讯，在某一目录下已经有相机拍摄标定板的照片。代码如下：

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include <memory>
#include <vector>
#include <queue>
#include "opencv4/opencv2/opencv.hpp"
#include "cv_bridge/cv_bridge.h"
#include <thread>

std::mutex globalLock;

class Transition : public rclcpp::Node
{
public:
    Transition(std::string nodeName, std::string subscribeTopic) : Node(nodeName)
    {
        getParameters("your path to image folder", 10);
        subscription = this->create_subscription<sensor_msgs::msg::Image>(subscribeTopic, 10, [this](const sensor_msgs::msg::Image msg) mutable -> void
        {
            cv_bridge::CvImagePtr cvPtr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_8UC3);
            cv::Mat image = cvPtr->image;
            if (tempImages.size() < imageSize)
            {
                tempImages.push(image);
            }
            else
            {
                dealing();
            } 
        });
    }

private:
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription;

    const int imageSize = 10;
    std::queue<cv::Mat> tempImages;

    cv::Mat K;
    cv::Mat D;

    cv::Mat R;
    cv::Mat T;

    void getParameters(std::string path, int imageNum)
    {
        cv::Size patternSize(9, 6);
        float boxSize = 1;

        std::vector<std::vector<cv::Point2f>> imagePoints;
        std::vector<std::vector<cv::Point3f>> worldPoints;

        for (int i = 0; i < imageNum; i++)
        {
            cv::Mat image = cv::imread(path + "/image" + std::to_string(i) + ".jpg");
            cv::Mat gray;
            cv::cvtColor(image, gray, cv::COLOR_BayerBG2GRAY);

            std::vector<cv::Point2f> cornerPoints;
            if (cv::findChessboardCorners(gray, patternSize, cornerPoints))
            {
                imagePoints.push_back(cornerPoints);
                std::vector<cv::Point3f> worldPoint;
                for (int y = 0; y < patternSize.height; y++)
                {
                    for (int x = 0; i < patternSize.width; x++)
                    {
                        worldPoint.push_back(cv::Point3f(x * boxSize, y * boxSize, 0));
                    }
                }
                worldPoints.push_back(worldPoint);
            }

            std::vector<cv::Mat> rvecs, tvecs;
            cv::calibrateCamera(worldPoints, imagePoints, cv::Size(gray.cols, gray.rows), K, D, rvecs, tvecs);

            cv::solvePnP(worldPoints, imagePoints, K, D, R, T);
        }
    }

    void dealing()
    {
        globalLock.lock();
        while (!tempImages.empty())
        {
            /*
            code to deal with images
            */
            tempImages.pop();
        }
        globalLock.unlock();
    }
};

int main(int argc, char **argv)
{
    std::shared_ptr<Transition> node = std::make_shared<Transition>(Transition("transform", "inerParam"));
    rclcpp::init(argc, argv);
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

思考

1.相机外参的作用是什么？

答：

1. 三维重建：在进行三维重建或点云生成时，相机外参用于将相机捕捉到的图像点映射到三维空间中的实际点。相机外参提供了相机坐标系与世界坐标系之间的转换关系，从而使得可以将图像点转化为其在三维空间中的实际位置。

2. 相机位姿估计：相机外参对于估计相机的位姿（位置和方向）非常关键。通过与其他传感器（如IMU或GPS）进行融合，可以使用相机外参来估计相机在三维空间中的位置和方向，实现相机的定位和导航。

3. 增强现实：在增强现实应用中，相机外参用于将虚拟对象与实际场景进行对齐。通过根据相机外参的变换将虚拟内容与相机图像进行叠加，可以实现虚拟对象在现实世界中的准确呈现。

4. 目标检测和跟踪：在目标检测和跟踪任务中，相机外参可以用于转换检测或跟踪结果的坐标系。通过将检测或跟踪结果转换到世界坐标系，可以更好地理解目标的位置和运动。

2.一个载体搭载着摄像头，在平面上运动，相机外参【t(x,y,z)与R(roll,pitch,yaw)】哪些量不可以被标定出来？

答：就本题而言，平移矩阵t的z，旋转矩阵R中的pitch，yaw难以标定。

在平面运动中，pitch 角度通常指的是绕 x 轴的旋转角度。由于平面运动限制了绕 x 轴的旋转，因为平面运动保持了载体相对于参考平面的接近恒定的高度，所以 pitch 角度的变化可能较小或不明显。这可能导致对 pitch 角度的准确标定相对困难。

类似地，对于绕 y 轴的旋转（yaw），由于平面运动限制了绕 y 轴的旋转，因此 yaw 角度的变化也可能较小。

同理，由于在平面运动，z也很难被确定。z轴的平移变化也相当小，导致z轴也难以标定。

4. 三维空间刚体运动

        三维空间中的刚体运动是由旋转和平移构成。平移和旋转相结合，称为欧式变换。

        上一章我们在描述相机与载体之间的坐标变换时假设了载体本身为世界系坐标原点，但这是明显错误的。世界系本身不应该发生任何变化，而载体肯定是可动的。

4.1  世界坐标系与里程坐标系

        世界坐标系一般指固定坐标系，全局不发生任何改变，默认旋转角为0；而里程坐标系一般指运动坐标系。在VSLAM中，一般指相机坐标系。

        上一章的内容中，坐标变换只是把点投射到了另一个坐标系下，但是对于向量而言，却没有发生任何变化，因为我们没有考虑载体在世界系下的位姿变化。而这就是我们这一章要学习的。

        我们可以这么看，在一个时间段内，里程坐标系进行了一定的旋转，则在一个相对较小的时间尺度下，我们可以将一整段旋转切割为一个个小旋转，通过不断计量每一个小旋转之间的角度，我们就可以得出对该时间段内每一个时间尺度的旋转。

4.2 欧式变换的累加

        假设c1原点在w坐标系下坐标为P1，旋转为R1，c2原点在c1坐标系下坐标为△P，旋转为△R，则c2在w坐标系下的欧式变换P2与R2为：

        P2 = R1*△P+P1

        旋转：

        R2 = R1*△R;

        为了简洁，我们可以用一个矩阵来表示：

        我们也将上述变换写成分块矩阵：

         可以得知，只要知道当前时刻的坐标系相对于之前的坐标系的位移及旋转，就可以得到当前时刻的全局欧式变换。只要不断累加，就能获得任意时刻的刚体运动。

4.3 平移与旋转的表示

4.3.1 平移

        平移就是个三维向量t(x, y, z)。

4.3.2 旋转

        旋转群通常由一组旋转矩阵表示。当然，四元数也是个很好的表达。不过由于鼠鼠没有学过四元数，本文只对旋转矩阵做笔记（原文也提到在自动驾驶中不会发生万向节死锁，暂时先这样吧）。

4.3.2.1 旋转矩阵

        旋转矩阵是一个3x3的正交矩阵，用来描述一个刚体绕着某个轴旋转的情况。

        相对于上一节，这里主要从向量的角度进行推导。

        假设空间中有一向量a，在坐标系1中的坐标为a1，坐标系1旋转一个角度后的坐标系为2，假设在2坐标系中向量a坐标为a2，则有：

 向量在坐标系中的表示是基的线性组合，即：

 旋转过程中，向量不变，则有：

 在两边同时乘以坐标系1基的转置，则有：

 这个旋转矩阵是两个坐标系的内积，由于基向量长度为1，所以实际上是各基向量夹角的余弦值，所以这个矩阵也叫余弦矩阵。

        旋转矩阵的自由度为3。

4.3.2.2 欧拉角

        简单来讲，比较常用的欧拉角为“偏转-俯仰-滚转”(yaw-pitch-roll)三个角度。

        我们常见的roll-pitch-yaw角就是欧拉角旋转轴选取(x, y, z)的情况。

        （z, x, z'）旋转方式如下图：

        欧拉角计算旋转矩阵很简单，直接将旋转对应的旋转矩阵连续右乘。以下是(z, x, z')的计算：

        当然，我们也可以尝试写出（x, y, z）的旋转矩阵计算如下：

                      

        欧拉角有是非一一对应问题和存在万向锁问题。

        是非一一对应问题主要由描述带来的歧义造成。万向锁是因为如果第一次旋转90度，那么就直接对掉了一个自由度，如下图：

 4.3.2.3 旋转向量

         我们希望有一种方式能够紧凑地描述旋转和平移，比如用一个三维向量来表示旋转，用一个六维向量来表示变换。

        事实上，任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是，我们可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量称为旋转向量(或轴角、角轴，Axis-Angle),只需一个三维向量即可描述旋转。同样，对于变换矩阵，我们使用一个旋转向量和一个平移向量即可表达一次变换。这时的变量维数正好是六维。

        对应于一个三维向量，其方向与旋转轴一致，而长度等于旋转角。这种向量称为旋转向量(或轴角、角轴，Axis-Angle)，该三维向量即可描述旋转。

从轴角到旋转矩阵，可以用罗德里格斯公式来转换：

（备注：上图的应该是）

符号^是向量到反对称矩阵的转换符。罗德里格斯公式：

假设有一个三维向量v，我们需要将其绕一个单位向量u旋转一个角度θ。根据罗德里格斯公式，可以计算出旋转后的向量v'：

v' = v*cos(θ) + (u x v)*sin(θ) + u*(u · v)*(1 - cos(θ))

此处仅为使用，不做推导。

对上式去迹得：

我们取反三角：

 思考

1.验证四元数旋转某个点后，结果是一个虚四元数（实部为零），所以仍然对应到一个三维

空间点。

答：缺乏相关的数学基础，无法作答。

2.一般线性方程AX=B有哪几种做法？你能在Eigen中实现吗？

答：在大学线性代数学习中我只记得一种解法。

首先，将B并到A的右边，记作A|B，通过矩阵的基础线性变换化简，求出R(A)和R(A|B)，判断两者关系，如果R(A) != R(A|B)，那么无解。如果R(A) = R(A|B) = n，有唯一解，如果R(A) = R(A|B) < n，有无穷多解。

然后，通过化简以后的矩阵列出同解方程组。

最后，通过设系数k来得出通解和特解。

通过Eigen实现如下：

#include "eigen3/Eigen/Eigen"
#include <iostream>

int main()
{
	Eigen::Matrix<double, 4, 5> A
	{
		{1, 2, 3, 4, 5}, 
		{5, 6, 7, 8, 9},
		{9, 10, 11, 12, 13},
		{13, 14, 15, 16}
	};
	Eigen::Vector<double, 4> B
	{
		{17, 18, 19, 20}
	};

	Eigen::VectorXd X = A.fullPivLu().solve(B);

	if (A.fullPivLu().rank() < A.cols()) 
	{
        std::cout << "方程组无解" << std::endl;
    } 
	else if (A.fullPivLu().rank() < A.cols() - 1) 
	{
		Eigen::MatrixXd nullspace = A.fullPivLu().kernel();
        std::cout << "方程组有无穷多解" << std::endl;
		std::cout << "Solution:\nX = ";
		for (int i = 0; i < nullspace.cols(); i++)
		{
			Eigen::VectorXd generalSolution = nullspace.col(i);
			std::cout << " " << generalSolution << " * n" << std::to_string(i);
		}
		std::cout << std::endl;
    } 
	else 
	{
        std::cout << "方程组有唯一解" << std::endl;
		std::cout << "Solution:\n X = " << X << std::endl;
    }

	return 0;
}

这次学习就先写到这吧！