深入理解SLAM中的3D-3D位姿估计：基于SVD与图优化的实现-优快云博客

本文链接：https://blog.youkuaiyun.com/gitblog_00330/article/details/148464695

深入理解SLAM中的3D-3D位姿估计：基于SVD与图优化的实现

slambook 项目地址: https://gitcode.com/gh_mirrors/sl/slambook

概述

本文主要探讨计算机视觉与SLAM领域中一个核心问题：如何从两组3D点对应关系中估计相机运动（即旋转矩阵R和平移向量t）。我们将基于gaoxiang12/slambook项目中的pose_estimation_3d3d.cpp实现，详细解析3D-3D位姿估计的两种主要方法：SVD分解法和图优化法。

3D-3D位姿估计问题定义

给定两组3D点集pts1和pts2，其中pts1中的每个点与pts2中的点一一对应，我们的目标是找到一个刚体变换(R,t)，使得对于所有i，有：

pts1[i] = R * pts2[i] + t

这个问题在SLAM中非常常见，特别是在RGB-D SLAM或激光SLAM中，当我们能够直接获取场景的三维信息时。

实现流程解析

1. 特征提取与匹配

代码首先通过ORB特征检测器和描述子提取器在两幅图像中找到匹配的特征点：

Ptr<FeatureDetector> detector = ORB::create();
Ptr<DescriptorExtractor> descriptor = ORB::create();
Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");

ORB(Oriented FAST and Rotated BRIEF)是一种快速且具有旋转不变性的特征点检测与描述算法，非常适合实时SLAM系统。

2. 3D点云构建

对于每个匹配的特征点，利用深度图信息将其转换为3D点：

Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
float dd1 = float(d1)/5000.0;
pts1.push_back(Point3f(p1.x*dd1, p1.y*dd1, dd1));

这里pixel2cam函数将像素坐标转换为相机坐标系下的归一化坐标，再结合深度值得到3D坐标。

3. SVD求解位姿

核心算法是通过SVD分解求解最优的R和t：

void pose_estimation_3d3d(const vector<Point3f>& pts1, const vector<Point3f>& pts2, Mat& R, Mat& t)
{
    // 计算质心
    Point3f p1, p2;
    int N = pts1.size();
    for(int i=0; i<N; i++) {
        p1 += pts1[i];
        p2 += pts2[i];
    }
    p1 = p1 / N; p2 = p2 / N;
    
    // 去中心化
    vector<Point3f> q1(N), q2(N);
    for(int i=0; i<N; i++) {
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }
    
    // 计算W矩阵
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for(int i=0; i<N; i++) {
        W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * 
             Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose();
    }
    
    // SVD分解
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU|Eigen::ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();
    
    // 处理反射情况
    if(U.determinant() * V.determinant() < 0) {
        for(int x=0; x<3; ++x)
            U(x,2) *= -1;
    }
    
    Eigen::Matrix3d R_ = U * (V.transpose());
    Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);
    
    // 转换为OpenCV格式输出
    R = (Mat_<double>(3,3) << R_(0,0), R_(0,1), R_(0,2),
                              R_(1,0), R_(1,1), R_(1,2),
                              R_(2,0), R_(2,1), R_(2,2);
    t = (Mat_<double>(3,1) << t_(0,0), t_(1,0), t_(2,0));
}

这个实现基于以下数学原理：

计算两组点集的质心p1和p2
对点集去中心化得到q1和q2
计算矩阵W = Σ(q1 * q2^T)
对W进行SVD分解：W = UΣV^T
最优旋转矩阵R = UV^T（需处理反射情况）
最优平移向量t = p1 - R*p2

4. 图优化精修

SVD解法给出了一个闭式解，但我们可以进一步使用图优化来精修结果：

void bundleAdjustment(const vector<Point3f>& pts1, const vector<Point3f>& pts2, Mat& R, Mat& t)
{
    // 初始化g2o优化器
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,3>> Block;
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverEigen<Block::PoseMatrixType>();
    Block* solver_ptr = new Block(linearSolver);
    g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr);
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    
    // 添加顶点(位姿)
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
    pose->setId(0);
    pose->setEstimate(g2o::SE3Quat(
        Eigen::Matrix3d::Identity(),
        Eigen::Vector3d(0,0,0)
    ));
    optimizer.addVertex(pose);
    
    // 添加边(观测)
    for(size_t i=0; i<pts1.size(); i++) {
        EdgeProjectXYZRGBDPoseOnly* edge = new EdgeProjectXYZRGBDPoseOnly(
            Eigen::Vector3d(pts2[i].x, pts2[i].y, pts2[i].z));
        edge->setVertex(0, dynamic_cast<g2o::VertexSE3Expmap*>(pose));
        edge->setMeasurement(Eigen::Vector3d(pts1[i].x, pts1[i].y, pts1[i].z));
        edge->setInformation(Eigen::Matrix3d::Identity()*1e4);
        optimizer.addEdge(edge);
    }
    
    // 执行优化
    optimizer.initializeOptimization();
    optimizer.optimize(10);
}

自定义的边类EdgeProjectXYZRGBDPoseOnly定义了误差计算和雅可比矩阵：

class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap>
{
public:
    virtual void computeError() {
        const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
        _error = _measurement - pose->estimate().map(_point);
    }
    
    virtual void linearizeOplus() {
        // 雅可比矩阵计算...
    }
    
protected:
    Eigen::Vector3d _point;
};