深入理解slambook中的3D-2D位姿估计实现-CSDN博客

深入理解slambook中的3D-2D位姿估计实现

概述

本文主要分析slambook项目中第七章的3D-2D位姿估计算法实现。该算法通过已知的3D空间点和它们在图像中的2D投影点，计算相机的位姿（旋转和平移）。这是计算机视觉和机器人导航领域中的基础问题，广泛应用于SLAM、增强现实等场景。

算法流程解析

1. 特征点提取与匹配

算法首先使用ORB特征检测器在两幅图像中寻找特征点，并计算它们的描述子：

Ptr<FeatureDetector> detector = ORB::create();
Ptr<DescriptorExtractor> descriptor = ORB::create();
detector->detect(img_1,keypoints_1);
descriptor->compute(img_1, keypoints_1, descriptors_1);

ORB(Oriented FAST and Rotated BRIEF)是一种快速且具有旋转不变性的特征检测算法，非常适合实时应用。特征匹配阶段使用汉明距离作为相似性度量：

Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
matcher->match(descriptors_1, descriptors_2, match);

匹配完成后，算法会进行筛选，去除不可靠的匹配对：

if(match[i].distance <= max(2*min_dist, 30.0)) {
    matches.push_back(match[i]);
}

2. 3D点云构建

利用第一幅图像的深度信息，将2D特征点转换为3D空间点：

ushort d = d1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
float dd = d/5000.0;
Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
pts_3d.push_back(Point3f(p1.x*dd, p1.y*dd, dd));

这里pixel2cam函数将像素坐标转换为相机归一化平面坐标，考虑了相机的内参矩阵K。

3. PnP问题求解

使用OpenCV的solvePnP函数求解位姿：

solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false);
cv::Rodrigues(r, R);

solvePnP实现了多种求解PnP问题的方法，包括EPnP、DLS等。Rodrigues公式将旋转向量转换为旋转矩阵。

4. 光束法平差优化

为了提高位姿估计精度，算法使用g2o框架进行光束法平差(Bundle Adjustment)优化：

typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,3>> Block;
Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);

优化问题建模包括：

相机位姿顶点(VertexSE3Expmap)
3D点顶点(VertexSBAPointXYZ)
重投影误差边(EdgeProjectXYZ2UV)

关键技术点

1. 相机模型与坐标转换

算法中实现了像素坐标到相机坐标的转换：

Point2d pixel2cam(const Point2d& p, const Mat& K) {
    return Point2d(
        (p.x - K.at<double>(0,2))/K.at<double>(0,0),
        (p.y - K.at<double>(1,2))/K.at<double>(1,1)
    );
}

这个转换去除了相机内参的影响，将像素坐标映射到归一化平面。

2. g2o优化框架

g2o(General Graph Optimization)是用于图优化的C++框架。在本实现中：

定义顶点(Vertex)：
- 相机位姿顶点：6自由度(3旋转+3平移)
- 3D点顶点：3D坐标
定义边(Edge)：
- 重投影误差边：测量值是2D图像点，连接相机位姿和3D点
优化算法：
- 使用Levenberg-Marquardt算法进行非线性优化

3. 深度信息处理

深度图处理需要注意：

深度值为0表示无效测量
深度值需要根据传感器特性进行缩放(本实现中除以5000.0)
深度信息结合相机内参才能得到准确的3D坐标

性能考量

算法在优化阶段记录了时间消耗：

chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.optimize(100);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();

实际应用中，可以根据需求调整优化迭代次数，平衡精度和速度。