【视觉SLAM十四讲】视觉里程计 1

文章目录

- 特征点匹配
- - [ORB 特征](#ORB 特征)
  - - [FAST 关键点](#FAST 关键点)
  - [BRIEF 描述子](#BRIEF 描述子)
  - [ORB 的改进](#ORB 的改进)
  - - 尺度不变性
    - 旋转不变性
  - 特征匹配
- 2D-2D：对极几何
- - 本质矩阵
  - - [多于 8 对点的情况](#多于 8 对点的情况)
  - 单应矩阵
  - 三角测量
- 3D-2D：PnP
- 3D-3D：ICP
- - [SVD 法求解](#SVD 法求解)
  - [SVD 法代码实现](#SVD 法代码实现)

特征点匹配

视觉里程计的核心问题是根据图像估计相机运动，而这通常通过帧间的图像变化实现。为了简化运算，我们会从图像中选取特征点，这些点在相机视角发生较小变化时仍然存在，通过特征点在图像上的变化，估计相机位姿的改变。

特征点由关键点 和描述子两部分组成。前者指的是特征点在图像中的位置，后者则指该关键点周围像素的信息，这里讲讲比较有代表性的 ORB 特征

ORB 特征

FAST 关键点

FAST 算法的思路为：如果一个像素与周围邻域的像素差别过大时，就认为其为一个角点。由于仅仅需要比较像素之间的灰度值大小，计算量要小得多

其实现步骤如下：

在图像中选取一像素 p p p ，假设其亮度为 I P I_P IP
设定一个合适的阈值 T T T
选取以 p p p 为中心，半径为 3 的圆上的 16 个像素点
如果选取的像素点存在连续的 N 个点的亮度大于 I P + T I_P+T IP+T 或者小于 I P − T I_P-T IP−T ，那么该点可视为一个角点（ N N N 通常取12）

对于 FAST-12 算法，还有个预测试操作：直接检测邻域圆上的第 1，5，9，13 的像素点亮度，只有至少其中 3 个亮度大于 I P + T I_P+T IP+T 或者小于 I P − T I_P-T IP−T ，才有可能是一个角点

BRIEF 描述子

在提取到关键点之后，我们要对每个点计算其描述子。BRIEF 描述子是一种二进制描述子。在特征点周围选取 256 对随机像素 p , q p,q p,q ，如果 I P > I q I_P > I_q IP>Iq ，则即为 1，反之则即为 0

BRIEF 描述子之间的相似性通常用汉明距离来体现，即两个描述子按位比较，不相同的位数有多少，汉明距离就是多少

ORB 的改进

FAST 和 BRIEF 主要有两个缺点：

不具有尺度不变性：同一目标在图像中可能因拍摄距离不同而呈现不同大小，可能被误判为其他物体
不具有旋转不变性：同上，同一目标的不同旋转状态可能被误判为其他物体

尺度不变性

ORB 采用的是图像金字塔，即以原始图像为第 0 层，进行多次降采样（缩小）生成一系列不同分辨率的图像，这样就可以在进行特征匹配时达到尺度不变性的效果

举个例子，我们在当前帧（frame 1）找到了一个特征点，此时相机向前移动靠近该特征点，使其在图像（frame 2）上占比变大。我们从 frame 2 图像金字塔的顶层开始搜索找到特征点的大致位置，然后将预测结果放大相应的倍数传递到下一层，如此反复直至在某一次获得了精确的匹配位置

旋转不变性

ORB 采用的是灰度质心法，其核心思想是为特征点计算出一个"主方向"，后续生成描述子时，会先将图像块旋转到该主方向后再进行计算

假设我们有一大小为 N × N N\times N N×N 的图像块 A A A ，其几何中心为 C C C

计算零阶距（ m 00 m_{00} m00）和一阶距（ m 10 , m 01 m_{10},m_{01} m10,m01）：灰度值加权的坐标和，即"质量"分布中心
m p q = ∑ x , y ∈ B x p y q I ( x , y ) p , q = 0 , 1 m_{pq}=\sum_{x,y\in B}x^py^qI(x,y)\quad p,q={0,1} mpq=∑x,y∈BxpyqI(x,y)p,q=0,1
计算质心： C = ( m 10 m 00 , m 01 m 00 ) C=\left( \frac{m_{10}}{m_{00}},\frac{m_{01}}{m_{00}} \right) C=(m00m10,m00m01)
连接几何中心 O O O 和质心 C C C ，得到一方向向量 O C → \overrightarrow {OC} OC ，于是特征点的方向可定义为
θ = arctan ⁡ ( m 01 m 10 ) \theta = \arctan \left(\frac{m_{01}}{m_{10}}\right) θ=arctan(m10m01)

下面是一份灰度质心法的使用和描述子计算的大致代码：

cpp 复制代码

void computeORB(const cv::Mat img, std::vector<cv::KeyPoint> &keyPoints, std::vector<DescType> &descriptors)
{
    const int half_patch_size = 8;               // 用于计算方向的图像块大小
    const int half_boundary = 16;                // 定义图像边界缓冲区
    for (auto &kp : keyPoints)
    {
        // 除去边界上的点
        if (kp.pt.x < half_boundary || kp.pt.y < half_boundary || kp.pt.x >= img.cols - half_boundary || kp.pt.y >= img.rows - half_boundary)
        {
            descriptors.push_back({});
            continue;
        }
        
        // 计算一阶矩
        float m10 = 0, m01 = 0;
        for (int dx = -half_patch_size; dx < half_patch_size; dx++)
        {
            for (int dy = -half_patch_size; dy < half_patch_size; dy++)
            {
                // 获取当前像素值
                uchar pixel = img.at<uchar>(kp.pt.y + dy, kp.pt.x + dx);
                m10 += dx * pixel;
                m01 += dy * pixel;
            }
        }

        // 计算特征点方向
        float m_sqrt = sqrt(m10 * m10 + m01 * m01);
        float sin_theta = m01 / m_sqrt;
        float cos_theta = m10 / m_sqrt;

        DescType desc(8, 0);
        for (int i = 0; i < 8; i++)
        {
            uint32_t d = 0;
            for (int j = 0; j < 32; j++)
            {
                int idx_pq = i * 8 + j;
                cv::Point2f p(ORB_pattern[idx_pq * 4], ORB_pattern[idx_pq * 4 + 1]);
                cv::Point2f q(ORB_pattern[idx_pq * 4 + 2], ORB_pattern[idx_pq * 4 + 3]);

                // 计算旋转后的坐标
                cv::Point2f p_rot = cv::Point2f(p.x * cos_theta - p.y * sin_theta, p.x * sin_theta + p.y * cos_theta) + kp.pt;
                cv::Point2f q_rot = cv::Point2f(q.x * cos_theta - q.y * sin_theta, q.x * sin_theta + q.y * cos_theta) + kp.pt;

                if (img.at<uchar>(p_rot.y, p_rot.x) < img.at<uchar>(q_rot.y, q_rot.x))
                {
                    // 通过位运算 i << j，利用一个32位的整数存储32个二进制结果
                    d |= 1 << j;
                }
            }
            desc[i] = d;
        }
        descriptors.push_back(desc);
    }
}

特征匹配

通过上面的步骤，我们现在得到两帧图像各自的特征点，那么我们该怎么找到他们之间的对应关系呢？

暴力匹配：对于两张图片的描述子集，一一计算汉明距离以找到对应的匹配点。实现简单，计算精确；但计算量大，仅适合特征点少的情况
快速近似最近邻(FLANN)：在匹配前，FLANN 会先用其中一张图像的描述子集构建一个数据索引结构，这一步比较耗时但仅需进行一次，对于另一张图像的每一个描述子，在这个预先构建的索引结构上快速搜索找到近似的几个目标，最后利用最近邻距离和次近邻距离的比值筛选匹配结果。对于大规模的特征匹配速度几块，但结果相对并不精确，而且构建索引比较耗时，在较小特征点时使用可能得不偿失

2D-2D：对极几何

对于已匹配到特征点的相邻帧，可建立如下模型

此时存在对极约束
x 2 T t ∧ R x 1 = p 2 T K − T t ∧ R K − 1 p 1 = 0 ( x 是 p 在归一化平面上的像素坐标 ) x_2^Tt^\wedge Rx_1 =p_2^TK^{-T}t^\wedge RK^{-1}p_1=0\quad(x是p在归一化平面上的像素坐标) x2Tt∧Rx1=p2TK−Tt∧RK−1p1=0(x是p在归一化平面上的像素坐标)

其中，我们通常定义基础矩阵 E = t ∧ R , 本质矩阵 F = K − T E K − 1 \text{基础矩阵}~E=t^\wedge R,\quad \text{本质矩阵}~F=K^{-T}EK^{-1} 基础矩阵 E=t∧R,本质矩阵 F=K−TEK−1

可以进一步简化对极约束： x 2 T E x 1 = p 2 T F p 1 = 0 x_2^TEx_1=p_2^TFp_1=0 x2TEx1=p2TFp1=0

本质矩阵

本质矩阵 E E E 是一个 3 × 3 3\times3 3×3 矩阵

对 E E E ，乘以任意一个非零常数，对极约束仍然满足，故可认为 E E E 在不同尺度下是等价的
本质矩阵的奇异值必定是 [ σ , σ , 0 ] T ( σ > 0 ) [\sigma,\sigma,0]^T(\sigma>0) [σ,σ,0]T(σ>0)
由于平移和旋转各有 3 个自由度，故 t ∧ R t^\wedge R t∧R 共有 6 个自由度，由于尺度等价性，故 E E E 实际有 5 个自由度

通常使用八点法 来线性求解 E E E ，八点法利用 8 对匹配点构建线性方程组，通过 SVD 求解 E E E

对于一对匹配点，其归一化坐标为 x 1 = [ u 1 , v 1 , 1 ] , x 2 [ u 2 , v 2 , 1 ] x_1=[u_1,v_1,1],~~x_2[u_2,v_2,1] x1=[u1,v1,1], x2[u2,v2,1]
( u 2 , v 2 , 1 ) ( e 1 e 2 e 3 e 4 e 5 e 6 e 7 e 8 e 9 ) ( u 1 u 2 u 3 ) = 0 \left( u_2,v_2,1 \right)\begin{pmatrix} e_1 & e_2 & e_3 \\ e_4 & e_5 & e_6 \\ e_7 & e_8 & e_9 \end{pmatrix}\begin{pmatrix} u_1 \\ u_2 \\ u_3 \end{pmatrix} = 0 (u2,v2,1) e1e4e7e2e5e8e3e6e9 u1u2u3 =0

将 e e e 转化为列向量，写成与 e e e 有关的线性形式

根据李代数的左扰动模型
∂ P ′ ∂ δ ξ = [ I , − P ′ ∧ ] = [ 1 0 0 0 Z ′ − Y ′ 0 1 0 − Z ′ 0 X ′ 0 0 1 Y ′ − X ′ 0 ] \frac{\partial P'}{\partial\delta\xi}=\begin{bmatrix}I,-P'^\wedge\end{bmatrix}=\begin{bmatrix}1&0&0&0&Z'&-Y'\\0&1&0&-Z'&0&X'\\0&0&1&Y'&-X'&0\end{bmatrix} ∂δξ∂P′=[I,−P′∧]= 1000100010−Z′Y′Z′0−X′−Y′X′0

合并两部分即可得到雅可比矩阵
J = ∂ e ∂ P ′ ⋅ ∂ P ′ ∂ δ ξ = − [ f x Z ′ 0 − f x X ′ Z ′ 2 − f x X ′ Y ′ Z ′ 2 f x + f x X ′ 2 Z ′ 2 − f x Y ′ Z ′ 0 f y Z ′ − f y Y ′ Z ′ 2 − f y − f y Y ′ 2 Z ′ 2 f y X ′ Y ′ Z ′ 2 f y X ′ Z ′ ] J=\frac{\partial e}{\partial P'}\cdot\frac{\partial P'}{\partial \delta\xi}=-\begin{bmatrix} \frac{f_x}{Z'}&0&-\frac{f_xX'}{Z'^2}&-\frac{fxX'Y'}{Z'^2}&f_x+\frac{f_xX'^2}{Z'^2}&-\frac{f_xY'}{Z'}\\0&\frac{f_y}{Z'}&-\frac{f_yY'}{Z'^2}&-f_y-\frac{f_yY'^2}{Z'^2}&\frac{f_yX'Y'}{Z'^2}&\frac{f_yX'}{Z'} \end{bmatrix} J=∂P′∂e⋅∂δξ∂P′=−[Z′fx00Z′fy−Z′2fxX′−Z′2fyY′−Z′2fxX′Y′−fy−Z′2fyY′2fx+Z′2fxX′2Z′2fyX′Y′−Z′fxY′Z′fyX′]

如果仍需优化特征点的空间位置
∂ e ∂ P = ∂ e ∂ P ′ ∂ P ′ ∂ P \frac{\partial e}{\partial P}=\frac{\partial e}{\partial P'}\frac{\partial P'}{\partial P} ∂P∂e=∂P′∂e∂P∂P′

其中 ∂ e ∂ P ′ \frac{\partial e}{\partial P'} ∂P′∂e 在上方已求出，由于 P ′ = R P + t P'=RP+t P′=RP+t ，易得 ∂ P ′ ∂ P = R \frac{\partial P'}{\partial P}=R ∂P∂P′=R

因此：
∂ e ∂ P = − [ f x Z ′ 0 − f x X ′ Z ′ 2 0 f y Z ′ − f y Y ′ Z ′ 2 ] R \frac{\partial e}{\partial P}=-\begin{bmatrix}\frac{f_x}{Z'}&0&-\frac{f_xX'}{Z'^2}\\0&\frac{f_y}{Z'}&-\frac{f_yY'}{Z'^2}\end{bmatrix}R ∂P∂e=−[Z′fx00Z′fy−Z′2fxX′−Z′2fyY′]R

优化算法代码实现

这里提供了大致代码的实现

高斯牛顿法：

cpp 复制代码

void optimizePoseGaussNewton(const std::vector<cv::Point3d>& points_3d, const std::vector<cv::Point2d>& points_2d, cv::Mat& K, Sophus::SE3d& pose)
{
    const int iterations = 10;                 // 最大迭代次数
    double cost = 0, last_cost = 0;            // 当前误差和上次迭代误差

    double fx = K.at<double>(0, 0);
    double fy = K.at<double>(1, 1);
    double cx = K.at<double>(0, 2);
    double cy = K.at<double>(1, 2);

    // 构建相机内参矩阵
    Eigen::Matrix3d K_eigen;
    K_eigen << fx,  0, cx,
                0, fy, cy,
                0,  0,  1;

    for (int iter = 0; iter < iterations; iter++)
    {
        Sophus::Matrix6d H = Sophus::Matrix6d::Zero();         // 初始化海森矩阵
        Sophus::Vector6d b = Sophus::Vector6d::Zero();         // 初始化梯度
        cost = 0;

        // 计算雅可比矩阵和残差
        for (size_t i = 0; i < points_3d.size(); i++)
        {
            // 将3D点转换到相机坐标系下
            Eigen::Vector3d pw(points_3d[i].x, points_3d[i].y, points_3d[i].z);
            Eigen::Vector3d pc = pose * pw;          // P_camera = T * P_world

            double X = pc[0], Y = pc[1], Z = pc[2];

            // 检查深度
            if (Z <= 0)
            {
                std::cerr << "Depth is zero or negative!" << std::endl;
                continue;
            }

            // 投影到图像平面并计算重投影误差
            Eigen::Vector3d pixel_homo = K_eigen * pc;          // 齐次坐标形式
            Eigen::Vector2d pixel(pixel_homo[0] / pixel_homo[2], pixel_homo[1] / pixel_homo[2]);     // 转换回像素坐标

            Eigen::Vector2d observerd(points_2d[i].x, points_2d[i].y);
            Eigen::Vector2d error = observerd - pixel;

            // 累加误差平方和
            cost += error.squaredNorm();

            // 计算雅可比矩阵
            Eigen::Matrix<double, 2, 6> J;

            J(0, 0) = fx / Z;
            J(0, 1) = 0;
            J(0, 2) = -fx * X / (Z * Z);
            J(0, 3) = -fx * X * Y / (Z * Z);
            J(0, 4) = fx + fx * X * X / (Z * Z);
            J(0, 5) = -fx * Y / Z;

            J(1, 0) = 0;
            J(1, 1) = fy / Z;
            J(1, 2) = -fy * Y / (Z * Z);
            J(1, 3) = -fy - fy * Y * Y / (Z * Z);
            J(1, 4) = fy * X * Y / (Z * Z);
            J(1, 5) = fy * X / Z;

            // 构建高斯-牛顿方程
            H += J.transpose() * J;
            b += -J.transpose() * error;
        }

        // 求解线性方程 H * dx = b
        Sophus::Vector6d dx;
        try
        {
            dx = H.ldlt().solve(b);   // H 明确是对称正定矩阵，使用 LDLT 分解求解
        }
        catch (const std::exception& e)
        {
            std::cerr << "Linear system solve failed: " << e.what() << std::endl;
            break;
        }

        // 检查解
        if (!dx.allFinite())
        {
            std::cerr << "Solution is not finite!" << std::endl;
            break;
        }

        // 检查收敛性
        if (iter > 0 && cost >= last_cost)
        {
            std::cout << "Iteration " << iter << ", cost increased: " << cost << " -> " << last_cost << std::endl;
            break;
        }

        // 更新位姿
        pose = Sophus::SE3d::exp(dx) * pose;

        // 更新误差
        last_cost = cost;

        // 输出迭代信息
        std::cout << "Iteration " << iter << ", cost = " << cost << std::endl;
        std::cout << "dx norm = " << dx.norm() << std::endl;

        // 判断收敛
        if (dx.norm() < 1e-6)
        {
            std::cout << "Converged in " << iter << " iterations." << std::endl;
            break;
        }
    }
}

列文伯格-马夸尔特优化法：

cpp 复制代码

void optimizePoseLM(const std::vector<cv::Point3d>& points_3d, const std::vector<cv::Point2d>& points_2d, cv::Mat& K, Sophus::SE3d& pose)
{
    // 前面直到计算雅可比矩阵和残差的部分与 Gauss-Newton 相同，不再重复
        // 自适应阻尼
        Sophus::Vector6d dx;
        bool updateAccepted = false;

        while (!updateAccepted)
        {
            try
            {
                // (H + lambda*I) * dx = b
                Sophus::Matrix6d H_lm = H + lambda * Sophus::Matrix6d::Identity();
                dx = H_lm.ldlt().solve(b);

                if (!dx.allFinite())
                {
                    throw std::runtime_error("Solution is not finite!");
                }
            }
            catch (const std::exception& e)
            {
                std::cerr << "Linear system solve failed: " << e.what() << std::endl;
                lambda *= 4;  // 增大阻尼因子，重新尝试
                continue;
            }

            // 尝试更新位姿
            Sophus::SE3d newPose = Sophus::SE3d::exp(dx) * pose;

            // 计算新的误差
            double newCost = 0;
            for (size_t i = 0; i < points_3d.size(); i++)
            {
                Eigen::Vector3d pw(points_3d[i].x, points_3d[i].y, points_3d[i].z);
                Eigen::Vector3d pc = newPose * pw;

                // 投影到图像平面并计算重投影误差
                Eigen::Vector3d pixel_homo = K_eigen * pc;
                Eigen::Vector2d pixel(pixel_homo[0] / pixel_homo[2], pixel_homo[1] / pixel_homo[2]);

                Eigen::Vector2d observerd(points_2d[i].x, points_2d[i].y);
                Eigen::Vector2d error = observerd - pixel;

                newCost += error.squaredNorm();
            }

            /*
             * 计算近似质量因子 ρ
             * ρ = [实际误差减少量] / [理论误差减少量]
             * 理论误差减少量 = -dx^T * (J^T * f + 0.5 * H * dx)
             * 简化后 ≈ dx^T * (λ * dx - b) (因为 H * dx ≈ λ * dx )
             */
            double modelImprovement = dx.dot(b - lambda * dx);
            double actualImprovement = cost - newCost;
            double rho = actualImprovement / modelImprovement;

            // 根据 ρ 调整阻尼因子
            if (rho > 0)
            {
                // 模型预测准确，接受更新
                pose = newPose;
                cost = newCost;
                updateAccepted = true;

                // 调整 λ：当模型很好时减少 λ，使模型更接近高斯-牛顿法
                lambda *= std::max(1.0 / 3.0, 1 - std::pow(2 * rho - 1, 3));
                lambda = std::max(lambda, 1e-6);  // 防止 λ 过小
            }
            else
            {
                // 模型预测不准确，拒绝更新
                lambda *= 2;  // 增大阻尼因子，重新尝试
                lambda = std::min(lambda, 1e5);  // 防止 λ 过大
            }

            // 防止无限循环
            if (lambda > 1e5)
            {
                std::cerr << "Lambda exploded! Exiting." << std::endl;
                break;
            }
        }

        // 输出迭代信息
        std::cout << "Iteration " << iter << ", cost = " << cost << ", lambda = " << lambda << std::endl;
        std::cout << "dx norm = " << dx.norm() << std::endl;

        // 判断收敛
        if (iter > 0 && cost >= last_cost - 1e-6)
        {
            std::cout << "Converged in " << iter << " iterations." << std::endl;
            break;
        }

        if (dx.norm() < 1e-6)
        {
            std::cout << "Converged in " << iter << " iterations." << std::endl;
            break;
        }

        // 更新误差
        last_cost = cost;
    }
}

这里建议搭配上一章的非线性优化一起看

3D-3D：ICP

当我们观测到两组 3D 点时（通过RGB-D相机或者三角化得到），我们可以直接使用 3D-3D 匹配点来估计相机运动

已知两组匹配 3D 点：
P = { p 1 , p 2 , ... , p n } , Q = { q 1 , q 2 , ... , q n } P=\{p_1,p_2,\dots,p_n\},\quad Q=\{q_1,q_2,\dots,q_n\} P={p1,p2,...,pn},Q={q1,q2,...,qn}

构建最小二乘问题
min ⁡ R , t 1 2 ∑ i = 1 n ∣ ∣ q i − ( R p i + t ) ∣ ∣ 2 \underset{R,t}{\min}\frac{1}{2}\sum_{i=1}^n||q_i-(Rp_i+t)||^2 R,tmin21i=1∑n∣∣qi−(Rpi+t)∣∣2

SVD 法求解

计算质心
μ p = 1 n ∑ i = 1 n p i , μ q = 1 n ∑ i = 1 n q i \mu_p=\frac{1}{n}\sum_{i=1}^np_i,\quad \mu_q=\frac{1}{n}\sum_{i=1}^nq_i μp=n1i=1∑npi,μq=n1i=1∑nqi
去中心化
p i ′ = p i − μ p , q i ′ = q i − μ q p_i'=p_i-\mu_p,\quad q_i'=q_i-\mu_q pi′=pi−μp,qi′=qi−μq
计算旋转矩阵

构建矩阵：
W = ∑ i = 1 n q i ′ p ′ i T W=\sum_{i=1}^nq_i'{p'}_i^T W=i=1∑nqi′p′iT

进行 SVD 分解
W = U Σ V T W=U\Sigma V^T W=UΣVT

则旋转矩阵可定义为
R = U V T R=UV^T R=UVT

（如果 det ⁡ ( R ) = − 1 \det(R)=-1 det(R)=−1 ，需要去负号修正： R = U ⋅ diag ( 1 , 1 , − 1 ) ⋅ V T R=U\cdot\text{diag}(1,1,-1)\cdot V^T R=U⋅diag(1,1,−1)⋅VT）

注：奇异值分解可以视为矩阵先进行了一次旋转 U U U ，沿着坐标轴进行缩放 Σ \Sigma Σ ，最后再进行一次旋转 V T V^T VT ，故旋转矩阵可以视为 R = U V T R=UV^T R=UVT
计算平移向量
t = μ q − R μ p t=\mu_q-R\mu_p t=μq−Rμp

SVD 法代码实现

cpp 复制代码

Sophus::SE3d solveICPSVD(const std::vector<Eigen::Vector3d>& pt1, const std::vector<Eigen::Vector3d>& pt2)
{
    // 1. 计算质心
    Eigen::Vector3d centroid1(0, 0, 0);
    Eigen::Vector3d centroid2(0, 0, 0);

    for (const auto& p : pt1) centroid1 += p;
    for (const auto& p : pt2) centroid2 += p;

    centroid1 /= pt1.size();
    centroid2 /= pt2.size();

    // 2. 去中心化
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for (size_t i = 0; i < pt1.size(); ++i)
    {
        Eigen::Vector3d p1 = pt1[i] - centroid1;
        Eigen::Vector3d p2 = pt2[i] - centroid2;
        W += p1 * p2.transpose();
    }

    // 3. SVD分解
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();

    // 4. 计算旋转矩阵
    Eigen::Matrix3d R = V * U.transpose();

    // 5. 检查旋转矩阵的行列式
    if (R.determinant() < 0)
    {
        Eigen::Matrix3d V_fixed = V;
        V_fixed.col(2) *= -1;  // 翻转第二列
        R = V_fixed * U.transpose();
    }

    // 6. 计算平移向量
    Eigen::Vector3d t = centroid2 - R * centroid1;

    return Sophus::SE3d(R, t);
}

当然也可以通过非线性优化来求解，和上面大差不差，这里就不赘述了

特性	对极几何（2D-2D）	PnP（3D-2D）	ICP（3D-3D）
输入	两帧图像的2D特征匹配点	3D地图点+当前帧的2D检测	两组3D点云
输出	相机相对运动位姿（R、t）特征点深度	相机位姿（R、t）	点云间的刚体变换（R、t）
适用场景	单目相机SLAM初始化、双目相机	已知地图定位	RGB-D相机SLAM、激光SLAM、点云配准
退化情况	纯旋转运动，匹配点共面、无纹理区域	匹配点共面、观测点噪声过高	点云特征不明显、初始位姿偏差过大
常用算法	八点法、五点法、RANSAC+八点法	P3P、EPnP、DLT、非线性优化	SVD、非线性优化