这篇文章解决的问题是通过深度图获取点云来自于实际生产的项目。核心内容是相机的成像原理根据原理解读世界坐标和像素坐标的变换关系再从这个关系中得到图像到点云的转换公式根据转换公式写出了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轴旋转180x轴向右y轴向下。像素坐标在感光面上将原点到左上角x轴向右y轴向下将x轴y轴的尺寸根据感光单元放缩。世界坐标系和相机坐标系转换世界坐标和相机坐标系之间的转换为刚体变换即旋转和平移旋转矩阵为R大小为3x3平移矩阵为T大小为3x1转换关系,世界坐标到相机坐标为Rw2c[R|T],大小为3x4对世界坐标系下P点添加一个维度Pw(xw, yw, zw1)进行变换得到相机坐标系下PcRw2c*Pw,大小为3x1Pc(xc,yc,zc)将相机坐标系和图像坐标系转换为了方便解释坐标变化关系将感光面等价放置到光心前方坐标系建立方式为光轴和感光面垂直感光面和光心的距离为焦距f光轴和感光面交点为原点根据小孔成像原理xy轴旋转180x轴向左y轴向上。可看出相机坐标系下Pc(wc,yc,zc)和感光面坐标系下Ps(xs,ys)从图像可以看到两个坐标系下的P点存在相似三角形关系所以有zc/fxc/xsyc/ys图像坐标Ps(xc.(zc/f),yc.(zc/f))变换形式zc.xsxc.f,zc.ysyc.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。像素坐标系的坐标原点为左上角在这坐标系下感光面的中心点描述为u0v0Ps(xs,ys,1)Pi(u,v)所以有Pi(u0xs/dx,v0ys/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.PiRs2i.Rc2s.Rw2c.Pw由于相机坐标系到感光面和感光面到像素坐标系的转换关系都是固定的都是相机内部固定数据所以两个矩阵相乘的结果也是固定的所以可以提前计算出来将结果保存下来直接使用这就是相机内参矩阵kk[f/dx 0 u0 ;0 f/dy v0 ;0 0 1 ]定义fxf/dx,fyf/dy有k[fx 0 u0 ;0 fy v0 ;0 0 1 ]此时变换关系为zcPik.Rw2c.Pw将深度图上一点投影到相机坐标系下将深度图装为三维点云此时的假设就是世界坐标系和相机坐标系重合哪世界坐标系和相机坐标系的变换Rw2c为单位矩阵I将Rw2cI带入zcPik.Rw2c.Pw得到素坐标系下一点到相机坐标系下的转换关系zc.Pik.Pw其中Pi(u,v)Pw(xw, yw, zw),zc为Pi的深度值由此可以看出图像上一个2D点无法直接得到相机坐标系下的3D点直接带入2d的Pi得到的结果是一条通过光心的射线这条射线上的任意一点都可以得到Pi的深度值所以需要知道Pi的深度值才能得到Pw的坐标。将深度图转换为点云根据上一节得到的结论当每个像素点都有对应的zc时就可以得到Pc的坐标深度图自带了Pi的深度值即zc,所以可以直接得到Pc的坐标。将深度图上的一点投影到相机坐标系下的公式进行变形得到Pczc.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##输入深度图相机内参矩阵##输出点云defdepth_to_pointcloud(depth,K):h,wdepth.shape# 1. 创建齐次像素坐标网格u,vnp.meshgrid(np.arange(w),np.arange(h))onesnp.ones_like(u)# 2. 像素齐次坐标 [3, H, W] - 重塑为 [3, H*W]pixel_coordsnp.stack([u,v,ones],axis0)# 形状: (3, h, w)pixel_coords_2dpixel_coords.reshape(3,-1)# 形状: (3, h*w)# 3. 计算相机逆矩阵K_invnp.linalg.inv(K)# 形状: (3, 3)# 4. 使用np.dot进行矩阵乘法# K_inv: (3,3), pixel_coords_2d: (3, h*w)# 结果: (3, h*w)rays_2dnp.dot(K_inv,pixel_coords_2d)# 5. 重塑回3D形状raysrays_2d.reshape(3,h,w)# 形状: (3, h, w)# 6. 用深度缩放得到3D点depth_expandednp.expand_dims(depth,axis0)# 形状: (1, h, w)points_camerarays*depth_expanded# 形状: (3, h, w)# 7. 转置和重塑pointspoints_camera.transpose(1,2,0).reshape(-1,3)# 形状: (h*w, 3)cpp代码// 将深度图像转换为点云std::shared_ptropen3d::geometry::PointCloudDepthToPointCloudWithDot_u(constcv::Matdepth,constcv::MatK){// 获取深度图像尺寸inthdepth.rows;intwdepth.cols;// 1. 创建齐次像素坐标网格cv::Matpixel_coords(3,h*w,CV_64F);for(intv0;vh;v){for(intu0;uw;u){pixel_coords.atdouble(0,v*wu)static_castdouble(u);// x坐标pixel_coords.atdouble(1,v*wu)static_castdouble(v);// y坐标pixel_coords.atdouble(2,v*wu)1.0;// 齐次坐标}}// 2. 计算相机逆矩阵cv::Mat K_inv;cv::invert(K,K_inv);// 3. 使用矩阵乘法计算射线方向cv::Mat rays_2dK_inv*pixel_coords;// 4. 创建点云对象autopoint_cloudstd::make_sharedopen3d::geometry::PointCloud();// 5. 用深度缩放得到3D点并过滤有效点std::vectorEigen::Vector3dvalid_points;for(inti0;ih*w;i){valid_points.push_back(Eigen::Vector3d(rays_2d.atdouble(0,i)*depth_value,rays_2d.atdouble(1,i)*depth_value,rays_2d.atdouble(2,i)*depth_value));}// 6. 设置点云点集point_cloud-points_valid_points;returnpoint_cloud;}
3d视觉——深度图像转换为点云（相机原理、坐标转换关系、python\cpp）
这篇文章解决的问题是通过深度图获取点云来自于实际生产的项目。核心内容是相机的成像原理根据原理解读世界坐标和像素坐标的变换关系再从这个关系中得到图像到点云的转换公式根据转换公式写出了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轴旋转180x轴向右y轴向下。像素坐标在感光面上将原点到左上角x轴向右y轴向下将x轴y轴的尺寸根据感光单元放缩。世界坐标系和相机坐标系转换世界坐标和相机坐标系之间的转换为刚体变换即旋转和平移旋转矩阵为R大小为3x3平移矩阵为T大小为3x1转换关系,世界坐标到相机坐标为Rw2c[R|T],大小为3x4对世界坐标系下P点添加一个维度Pw(xw, yw, zw1)进行变换得到相机坐标系下PcRw2c*Pw,大小为3x1Pc(xc,yc,zc)将相机坐标系和图像坐标系转换为了方便解释坐标变化关系将感光面等价放置到光心前方坐标系建立方式为光轴和感光面垂直感光面和光心的距离为焦距f光轴和感光面交点为原点根据小孔成像原理xy轴旋转180x轴向左y轴向上。可看出相机坐标系下Pc(wc,yc,zc)和感光面坐标系下Ps(xs,ys)从图像可以看到两个坐标系下的P点存在相似三角形关系所以有zc/fxc/xsyc/ys图像坐标Ps(xc.(zc/f),yc.(zc/f))变换形式zc.xsxc.f,zc.ysyc.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。像素坐标系的坐标原点为左上角在这坐标系下感光面的中心点描述为u0v0Ps(xs,ys,1)Pi(u,v)所以有Pi(u0xs/dx,v0ys/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.PiRs2i.Rc2s.Rw2c.Pw由于相机坐标系到感光面和感光面到像素坐标系的转换关系都是固定的都是相机内部固定数据所以两个矩阵相乘的结果也是固定的所以可以提前计算出来将结果保存下来直接使用这就是相机内参矩阵kk[f/dx 0 u0 ;0 f/dy v0 ;0 0 1 ]定义fxf/dx,fyf/dy有k[fx 0 u0 ;0 fy v0 ;0 0 1 ]此时变换关系为zcPik.Rw2c.Pw将深度图上一点投影到相机坐标系下将深度图装为三维点云此时的假设就是世界坐标系和相机坐标系重合哪世界坐标系和相机坐标系的变换Rw2c为单位矩阵I将Rw2cI带入zcPik.Rw2c.Pw得到素坐标系下一点到相机坐标系下的转换关系zc.Pik.Pw其中Pi(u,v)Pw(xw, yw, zw),zc为Pi的深度值由此可以看出图像上一个2D点无法直接得到相机坐标系下的3D点直接带入2d的Pi得到的结果是一条通过光心的射线这条射线上的任意一点都可以得到Pi的深度值所以需要知道Pi的深度值才能得到Pw的坐标。将深度图转换为点云根据上一节得到的结论当每个像素点都有对应的zc时就可以得到Pc的坐标深度图自带了Pi的深度值即zc,所以可以直接得到Pc的坐标。将深度图上的一点投影到相机坐标系下的公式进行变形得到Pczc.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##输入深度图相机内参矩阵##输出点云defdepth_to_pointcloud(depth,K):h,wdepth.shape# 1. 创建齐次像素坐标网格u,vnp.meshgrid(np.arange(w),np.arange(h))onesnp.ones_like(u)# 2. 像素齐次坐标 [3, H, W] - 重塑为 [3, H*W]pixel_coordsnp.stack([u,v,ones],axis0)# 形状: (3, h, w)pixel_coords_2dpixel_coords.reshape(3,-1)# 形状: (3, h*w)# 3. 计算相机逆矩阵K_invnp.linalg.inv(K)# 形状: (3, 3)# 4. 使用np.dot进行矩阵乘法# K_inv: (3,3), pixel_coords_2d: (3, h*w)# 结果: (3, h*w)rays_2dnp.dot(K_inv,pixel_coords_2d)# 5. 重塑回3D形状raysrays_2d.reshape(3,h,w)# 形状: (3, h, w)# 6. 用深度缩放得到3D点depth_expandednp.expand_dims(depth,axis0)# 形状: (1, h, w)points_camerarays*depth_expanded# 形状: (3, h, w)# 7. 转置和重塑pointspoints_camera.transpose(1,2,0).reshape(-1,3)# 形状: (h*w, 3)cpp代码// 将深度图像转换为点云std::shared_ptropen3d::geometry::PointCloudDepthToPointCloudWithDot_u(constcv::Matdepth,constcv::MatK){// 获取深度图像尺寸inthdepth.rows;intwdepth.cols;// 1. 创建齐次像素坐标网格cv::Matpixel_coords(3,h*w,CV_64F);for(intv0;vh;v){for(intu0;uw;u){pixel_coords.atdouble(0,v*wu)static_castdouble(u);// x坐标pixel_coords.atdouble(1,v*wu)static_castdouble(v);// y坐标pixel_coords.atdouble(2,v*wu)1.0;// 齐次坐标}}// 2. 计算相机逆矩阵cv::Mat K_inv;cv::invert(K,K_inv);// 3. 使用矩阵乘法计算射线方向cv::Mat rays_2dK_inv*pixel_coords;// 4. 创建点云对象autopoint_cloudstd::make_sharedopen3d::geometry::PointCloud();// 5. 用深度缩放得到3D点并过滤有效点std::vectorEigen::Vector3dvalid_points;for(inti0;ih*w;i){valid_points.push_back(Eigen::Vector3d(rays_2d.atdouble(0,i)*depth_value,rays_2d.atdouble(1,i)*depth_value,rays_2d.atdouble(2,i)*depth_value));}// 6. 设置点云点集point_cloud-points_valid_points;returnpoint_cloud;}
