双目视差图到点云的转换（原理+代码）_双目相机怎么得到点云数据

双目立体视觉是一种通过两个相机（模拟人类的双眼）来获取三维空间信息的技术。这一过程涉及将双目视差图转换为点云，下面详细介绍这一转换过程。

1. 双目相机系统

1.1 相机校准

• 内部参数：包括焦距和光心。

• 外部参数：包括相机间的相对位置和姿态。

  内部参数

  内部参数是指相机本身的特性，包括：

  • 焦距 (fx, fy): 相机镜头的焦距，影响图像的放大程度。

  • 光心 (cx, cy): 图像平面中相机镜头的中心点。

  • 焦距

    焦距表示相机镜头到成像平面的距离，通常表示为 (fx, fy)，分别对应于图像的水平和垂直方向。

    光心

    光心是图像平面上与相机光轴相交的点，通常表示为 (cx, cy)，分别是图像水平和垂直坐标轴上的点。

    内参矩阵

    相机内参可以通过内参矩阵表示，其形式如下：

    K = | f_x  0   c_x |
        | 0   f_y  c_y |
        | 0   0    1  |

    在这个矩阵中：

    • 第一行和第二行的第一和第二个元素分别是沿图像平面水平和垂直轴的焦距。

    • c_x 和 c_y分别是图像平面上光心的水平和垂直坐标。

    • 矩阵的第三行为 [0, 0, 1]，用于保持坐标的同质性。

  外部参数

  外部参数描述了相机之间的空间关系，包括：

  • 相对位置：两个相机之间的物理位置关系。

  • 姿态：两个相机相对于彼此的方向。

  外部参数的准确性对于正确计算视差和深度至关重要，我们主要会用到基线长度baseline。

  校准过程

  1. 捕获标定图像：使用双目相机系统拍摄标定模式（如棋盘格）。

  2. 角点检测：识别标定图像中的特征点。

  3. 求解相机参数：使用识别的特征点计算相机的内部和外部参数。

1.2 图像配准和视差计算

• 通过匹配左右相机图像中的相应像素位置，计算视差图。可以使用很多种寻找视差的方法，比如SGBM等。

2. 视差转深度

视差值转换为深度信息，涉及到三角测量的原理。如下图所示：

视差到深度的转换公式

在双目视觉系统中，从视差图（disparity map）计算深度（depth）可以使用以下公式：

其中：

• f 是相机的焦距。

• baseline 是两个相机之间的距离，也称为基线。

• U_L 是左相机图像平面上点的水平坐标。

• U_R 是右相机图像平面上点的水平坐标。

• 视差（disparity）为，即两个坐标的差值。

3. 点云的生成

每个像素点的深度信息转换为三维空间中的点。

3.1 计算公式

对于每个像素点 (u,v)，其三维坐标 (X,Y,Z) 可以通过以下公式计算：

X = depth * (u - cx) / fx
Y = depth * (v - cy) / fy
Z = depth

• 其中 depth 是基于视差计算得到的深度值。

• fx 和 fy 是相机的焦距。

• cx 和 cy 是图像的光心坐标。

---

以上公式是根据针孔相机模型推导出来的，它们将二维图像坐标转换为三维世界坐标。针孔相机模型是计算机视觉中一个理想化的相机模型，用于近似实际相机的成像过程。在这个模型中，一个三维空间点P(X,Y,Z) 通过相机投影到图像平面上的点 p(u,v)

1. 首先，图像平面上的点 p(u, v) 可以通过以下公式转换为相机坐标系中的点 P(X_c, Y_c, Z_c) ：

其中， f_x  和  f_y 是相机的焦距， C_x 和 C_y 是图像的光心坐标。

2. 为了从图像坐标 (u, v) 计算回相机坐标系中的 (X_c, Y_c, Z_c) ，我们需要做一些变换。首先，根据视差  d 计算深度 Z_c：

3. 然后，使用深度 Z_c 和图像坐标 (u, v) ，我们可以解出 X_c 和 Y_c：

4. 在此基础上，深度 Z_c 被简化为 `depth`，然后 X_c 和 Y_c 可以转换为世界坐标系中的 p.x  和  p.y 。

4. 代码部分

4.1 自定义点云格式（由于后期要转栅格地图，不需要比较复杂的点云格式）

#define SIMPLE_POINT_CLOUD_H
 
#include <vector>
 
/**
 * @brief 简单的点云数据结构，包含每个点的三维坐标和置信度。
 */
struct PointXYZConfidence {
    float x, y, z; // The coordinates of the point.
    uint8_t confidence; // The confidence level of the point.
};
 
/**
 * @brief 简单的点云数据类型，是一系列 SimplePoint 结构的集合。
 */
struct SimplePointCloud {
    std::vector<PointXYZConfidence> points; // A vector to store multiple points.
 
    // Add a point to the point cloud.
    void addPoint(const PointXYZConfidence& point) {
        points.push_back(point);
    }
 
    // Assuming there's a need to calculate the average height for each point.
    float getAverageHeight() const {
        if (points.empty()) return 0.0f;
        
        float total_height = 0.0f;
        for (const auto& point : points) {
            total_height += point.z; // Assuming 'z' is the height component.
        }
        return total_height / points.size();
    }
};
 
#endif // SIMPLE_POINT_CLOUD_H

4.2 DisparityToPointCloud.cpp

#include "DisparityToPointCloud.h"
#include <cmath>
 
// 构造函数初始化相机参数
DisparityToPointCloud::DisparityToPointCloud(double fx, double fy, double cx, double cy, double baseline)
    : fx_(fx), fy_(fy), cx_(cx), cy_(cy), baseline_(baseline) {}
 
// 根据视差信息转换为点云
// 输入的视差数据格式是16位的自定义的格式，需要做一些处理，这里不做叙述
SimplePointCloud DisparityToPointCloud::ConvertToPointCloud(const TCP_NX2PC_MSG* disparity_data) {
    SimplePointCloud cloud;
    int width = disparity_data->info.width;   // 视差图宽度
    int height = disparity_data->info.height; // 视差图高度
 
    // 遍历视差图每个像素
    for (int v = 0; v < height; ++v) {
        for (int u = 0; u < width; ++u) {
            // 提取视差值和置信度，这里不做叙述，实际情况可能不需要自定义数据格式
            uint16_t disparity_value = disparity_data->data.data[v * width + u];
            uint8_t confidence = (disparity_value >> 12) & 0xF;
            uint8_t integer_part = (disparity_value >> 5) & 0x7F;
            uint8_t fractional_part = disparity_value & 0x1F;
 
            // 计算实际视差
            double disparity = static_cast<double>(integer_part) + static_cast<double>(fractional_part) / 32.0;
            // 跳过置信度低或视差值无效的点
            if (confidence < min_confidence_ || disparity <= 0.0) continue;
 
            // 计算点的三维坐标
            double depth = fx_ * baseline_ / disparity; // 计算深度
            PointXYZConfidence point;
            point.x = (u - cx_) * depth / fx_;
            point.y = (v - cy_) * depth / fy_;
            point.z = depth;
            point.confidence = confidence;
 
            // 添加到点云
            cloud.addPoint(point);
        }
    }
    return cloud;
}
 
// 更新栅格地图占用状态，栅格地图部分再做叙述
// void DisparityToPointCloud::UpdateGridMap(GridMap& gridMap, const // SimplePointCloud& cloud, float img_width, float fov, float beta_r) {
    // 使用点云数据更新栅格地图的状态
//     gridMap.updateFromPointCloud(cloud, img_width, fov, beta_r);
// }

参考资料

https://aitechtogether.com/article/26378.html

SLAM14讲

我的github代码位置：StereoGridMap