这篇文章解决的问题是通过深度图获取点云，来自于实际生产的项目。
核心内容是相机的成像原理，根据原理解读世界坐标和像素坐标的变换关系，再从这个关系中得到图像到点云的转换公式，
根据转换公式写出了python的转换代码，再根据python的原型开发出cpp的代码。

成像基本原理

以一个点P为例子，将这个点在世界中最终反映在图像上的状态作为讲解。

1780048734973)

核心概念

小孔成像：物体的光线通过一个小孔，假设小孔后面有一个平面，光线通过小孔就会在平面上出现一个颠倒的图像。
光心：相机的原理是小孔成像，但是实际是由若干透镜组成的系统，这个系统可以等效出一个小孔，这个小孔就是光心。
感光面：在相机中小孔成像的平面就是感光面，这个平面由大量感光单元组成，感光单元能生成电信号，电信号经过电路就能生成数字图像。

成像过程

将相机成像简化为小孔成像，光心就是那个小孔，感光面就是小孔成像的平面。
当世界上有一个点p,这个点发出光线，光线被摄像头捕捉到后，光线就通过光心，到达到感光面上，感光面将光线转换为电信号。
此时感光面的这个点是旋转180度的点，电路会将数据旋转回来，同时图像的原点不在感光面中点，需要将原点移动到左上角。

世界坐标和像素坐标的变换关系

存在一个点p
世界坐标系下描述：Pw=(xw, yw, zw)
在相机坐标系下描述：Pc=(wc,yc,zc)
感光面上描述：Ps=(xs,ys)
像素坐标系下描述：Pi=(u,v)

核心概念

世界坐标：自己定义的一个坐标系，可以在任意地方，用于方便任务的开展，比如机器人一般在机械臂的基座上。
相机坐标：光心为原点，光轴为z轴，通常来说x轴为右方向，y轴为下方向，但为了方便解释坐标变化关系，将xoy平面旋转180度，x轴指向左，y轴指向上。
图像坐标：在感光面上，光轴和感光面垂直，感光面和光心的距离为焦距f，光轴和感光面交点为原点，根据小孔成像原理，将相机坐标系的xy轴旋转180，x轴向右，y轴向下。
像素坐标：在感光面上，将原点到左上角，x轴向右，y轴向下，将x轴y轴的尺寸根据感光单元放缩。

世界坐标系和相机坐标系转换

世界坐标和相机坐标系之间的转换为刚体变换，即旋转和平移，旋转矩阵为R，大小为3x3，平移矩阵为T，大小为3x1
转换关系,世界坐标到相机坐标为：Rw2c=[R|T],大小为3x4
对世界坐标系下P点添加一个维度，Pw=(xw, yw, zw，1)
进行变换得到相机坐标系下Pc=Rw2c*Pw,大小为3x1
Pc=(xc,yc,zc)

将相机坐标系和图像坐标系转换

为了方便解释坐标变化关系，将感光面等价放置到光心前方
坐标系建立方式为：光轴和感光面垂直，感光面和光心的距离为焦距f，光轴和感光面交点为原点，根据小孔成像原理，xy轴旋转180，x轴向左，y轴向上。
可看出相机坐标系下Pc=(wc,yc,zc)和感光面坐标系下Ps=(xs,ys)
从图像可以看到两个坐标系下的P点存在相似三角形关系，所以有zc/f=xc/xs=yc/ys
图像坐标：Ps=(xc.(zc/f),yc.(zc/f))
变换形式：zc.xs=xc.f,zc.ys=yc.f
变换关系为：zc.Ps=[f 0 0 ;0 f 0 ;0 0 1 ].Pc
变化矩阵为：Rc2s=[f 0 0 ;0 f 0 ;0 0 1 ],大小为3x3

图像坐标系和像素坐标系的转换

感光面是由多个感光单元组成的，每个感光单元的尺寸为像素尺寸，像素尺寸内的点都统一为一个点，像素尺寸宽为dx，高位dy，转换关系为w/dx、h/dy。
像素坐标系的坐标原点为左上角，在这坐标系下感光面的中心点描述为（u0，v0）
Ps=(xs,ys,1)
Pi=(u,v)
所以有Pi=(u0+xs/dx）,v0+ys/dy)
变换关系为：Pi=[1/dx 0 u0 ;0 1/dy v0 ;0 0 1 ]*Ps
变换矩阵为：Rs2i=[1/dx 0 u0 ;0 1/dy v0 ;0 0 1 ],大小为3x3

将世界坐标系投影到像素坐标系

将三个变化关系进行累乘就可以得到世界坐标系到像素坐标系的转换关系
Pi=(u,v)
Pw=(xw, yw, zw)
zc.Pi=Rs2i.Rc2s.Rw2c.Pw
由于相机坐标系到感光面和感光面到像素坐标系的转换关系都是固定的，都是相机内部固定数据
所以两个矩阵相乘的结果也是固定的，所以可以提前计算出来，将结果保存下来，直接使用，这就是相机内参矩阵k
k=[f/dx 0 u0 ;0 f/dy v0 ;0 0 1 ]
定义fx=f/dx,fy=f/dy
有k=[fx 0 u0 ;0 fy v0 ;0 0 1 ]
此时变换关系为：zcPi=k.Rw2c.Pw

将深度图上一点投影到相机坐标系下

将深度图装为三维点云
此时的假设就是，世界坐标系和相机坐标系重合，哪世界坐标系和相机坐标系的变换Rw2c为单位矩阵I
将Rw2c=I带入zcPi=k.Rw2c.Pw
得到素坐标系下一点到相机坐标系下的转换关系，zc.Pi=k.Pw
其中，Pi=(u,v)，Pw=(xw, yw, zw),zc为Pi的深度值
由此可以看出，图像上一个2D点无法直接得到相机坐标系下的3D点
直接带入2d的Pi，得到的结果是一条通过光心的射线，这条射线上的任意一点都可以得到Pi的深度值，所以需要知道Pi的深度值才能得到Pw的坐标。

将深度图转换为点云

根据上一节得到的结论，当每个像素点都有对应的zc时就可以得到Pc的坐标，
深度图自带了Pi的深度值，即zc,所以可以直接得到Pc的坐标。
将深度图上的一点投影到相机坐标系下的公式进行变形，得到Pc=zc.k^(-1).Pi,
其中Pi=(u,v),Pc=(xc,yc,zc),k^(-1)为k的逆矩阵，大小为3x3
对图像中的每一个点进行变换，得到点云

python代码

获得点云需要深度图和相机内参矩阵，步骤为：
1.根据图像尺寸生成像素坐标网格，每一个网格坐标都是一个点云的预备
2.计算相机内存逆矩阵，使用np.dot进行矩阵乘法，实现内参与像素坐标的矩阵运算，这就获得了每个像素的空间射线
3.使用深度图的值和矩阵运算的结果相乘，得到像素点对应的点云坐标
4.调整数据格式，得到点云，格式为(h*w, 3)，第一个维度为点云的数量，第二个维度为点云的坐标，数据格式为np

##输入：深度图，相机内参矩阵
##输出：点云
def depth_to_pointcloud(depth,K):
    h, w = depth.shape

    # 1. 创建齐次像素坐标网格
    u, v = np.meshgrid(np.arange(w), np.arange(h))
    ones = np.ones_like(u)
    
    # 2. 像素齐次坐标 [3, H, W] -> 重塑为 [3, H*W]
    pixel_coords = np.stack([u, v, ones], axis=0)  # 形状: (3, h, w)
    pixel_coords_2d = pixel_coords.reshape(3, -1)   # 形状: (3, h*w)
    
    # 3. 计算相机逆矩阵
    K_inv = np.linalg.inv(K)  # 形状: (3, 3)
    
    # 4. 使用np.dot进行矩阵乘法
    # K_inv: (3,3), pixel_coords_2d: (3, h*w)
    # 结果: (3, h*w)
    rays_2d = np.dot(K_inv, pixel_coords_2d)
    
    # 5. 重塑回3D形状
    rays = rays_2d.reshape(3, h, w)  # 形状: (3, h, w)
    
    # 6. 用深度缩放得到3D点
    depth_expanded = np.expand_dims(depth, axis=0)  # 形状: (1, h, w)
    points_camera = rays * depth_expanded  # 形状: (3, h, w)
    
    # 7. 转置和重塑
    points = points_camera.transpose(1, 2, 0).reshape(-1, 3)  # 形状: (h*w, 3)
    

cpp代码

// 将深度图像转换为点云
std::shared_ptr<open3d::geometry::PointCloud> DepthToPointCloudWithDot_u(
    const cv::Mat &depth,
    const cv::Mat &K)
{
  // 获取深度图像尺寸
  int h = depth.rows;
  int w = depth.cols;

  // 1. 创建齐次像素坐标网格
  cv::Mat pixel_coords(3, h * w, CV_64F);
  for (int v = 0; v < h; ++v)
  {
    for (int u = 0; u < w; ++u)
    {
      pixel_coords.at<double>(0, v * w + u) = static_cast<double>(u); // x坐标
      pixel_coords.at<double>(1, v * w + u) = static_cast<double>(v); // y坐标
      pixel_coords.at<double>(2, v * w + u) = 1.0;                    // 齐次坐标
    }
  }

  // 2. 计算相机逆矩阵
  cv::Mat K_inv;
  cv::invert(K, K_inv);

  // 3. 使用矩阵乘法计算射线方向
  cv::Mat rays_2d = K_inv * pixel_coords;

  // 4. 创建点云对象
  auto point_cloud = std::make_shared<open3d::geometry::PointCloud>();

  // 5. 用深度缩放得到3D点并过滤有效点
  std::vector<Eigen::Vector3d> valid_points;
  for (int i = 0; i < h * w; ++i)
  {
    valid_points.push_back(Eigen::Vector3d(
        rays_2d.at<double>(0, i) * depth_value,
        rays_2d.at<double>(1, i) * depth_value,
        rays_2d.at<double>(2, i) * depth_value));
  }

  // 6. 设置点云点集
  point_cloud->points_ = valid_points;
  return point_cloud;
}