坐标系介绍

一般分为三种坐标系：
- 世界坐标系：描述全局地图信息的坐标系，一般以全局地图左上方为坐标原点
- 相机坐标系：顾名思义就是以相机的中心为原点的坐标轴，会随着robot移动
- 图像坐标系：描述深度图像信息的坐标系一般位于深度图像的左上方

坐标系的转换和平移

具体见机器人导论的“刚体运动描述”

图像坐标系转相机坐标系

假设图像坐标系0（左上角为远点）中一点P（Xp0，Yp0）
①首先需要把图像坐标系0从图像的左上角转化到以图像中心为原点的另一个图像坐标系1，以便图像坐标系1的Z轴（深度方向）与相机坐标系的Z轴重合，得到P（Xp1，Yp1）
②如上图，f为相机内参中的焦距，为已知常量；假设P对应空间中的坐标（相对于相机坐标系）为P（Xc，Yc，Zc）；由相似三角形知识可得 Yc / Yp1 = Zc / f ；故空间中的Yc = Zc*Yp1 / f ；同理可得Xc = Zc * Xp1 / f ；而Zc为深度图中对应（Xp1，Yp1）的通道值；至此得到相对于相机坐标系的空间坐标P（Xc，Yc，Zc）
经过上面一步即可以得到以相机为中心的点云信息，可以以此来构建以自我为中心的局部地图（egocentric map）

代码实现

tensor实现

        # 交换维度 -> b c h wdepth = depth.permute(0, 3, 1, 2)_, _, imh, imw = depth.shape   # batchsize, 1, imh, imw# 相当于给矩阵的每个位置编号# eg：左上角的第一个元素标号为（Xp0,Yp0） = （0，0）x    = rearrange(torch.arange(0, imw), 'w -> () () () w').to(self.device)y    = rearrange(torch.arange(imh, 0, step=-1), 'h -> () () h ()').to(self.device)# self.cx 指的是depth图像的一半大小# 将（Xp0,Yp0）转化为（Xp1,Yp1）在转化为 （Xc，Yc）xx   = (x - self.cx) / self.fxyy   = (y - self.cy) / self.fy# 3D real-world coordinates (in meters)#（Xc，Yc）转化为（Xw，Yw）Z = depthX = xx * ZY = yy * Z

numpy实现

def get_point_cloud_from_z(Y, camera_matrix):"""Projects the depth image Y into a 3D point cloud.Inputs:Y is ...xHxWcamera_matrixOutputs:X is positive going rightY is positive into the imageZ is positive up in the imageXYZ is ...xHxWx3"""# 生成网格点坐标矩阵# 1-255 255-1x, z = np.meshgrid(np.arange(Y.shape[-1]),np.arange(Y.shape[-2]-1, -1, -1))for i in range(Y.ndim-2):x = np.expand_dims(x, axis=0)z = np.expand_dims(z, axis=0)X = (x-camera_matrix.xc) * Y / camera_matrix.fZ = (z-camera_matrix.zc) * Y / camera_matrix.fnewx = X[...,np.newaxis]XYZ = np.concatenate((newx, Y[...,np.newaxis],Z[...,np.newaxis]), axis=X.ndim)return XYZ

相机坐标系转世界坐标系

相机坐标系与世界坐标系的关系会因为robot在空间中的位置和朝向不同而变化，朝向影响相机的课室范围；所以相机坐标系转换到世界坐标系只需要知道robot的朝向以及robot在预先定义好的全局地图中的位置（一般设定robot起始位置为全局地图的中心）即可得到转化的旋转矩阵（robot朝向）和平移矩阵（robot位置）
旋转的实现可以参考仿射变换（pytorch实现）或者利用上文提到的刚体运动学求解
即图中两坐标系的关系
至此可以得到相对于世界坐标的P（Xw，Yw，Zw），即相对于全局地图左上角的全局点云信息。

点云表示为地图信息请看下一篇文章

（Visual Navigation）深度图像转点云相关推荐

深度图像转换为点云数据计算原理及代码实现
深度图像转换为点云数据计算原理及代码实现 1.开发环境 2. 深度图转点云计算原理 3.代码实现 3.1 头文件Depth_TO_PointCloud.h 3.2Depth_TO_PointCloud ...
深度图像转点云数据（激光雷达数据）
文章目录一.简介二.代码实现三.实现效果参考文献一.简介深度图像的获取有很多方式,如激光雷达.结构光以及深度相机等,网上很多教程都是在讲解通过深度相机所获取的深度图像转换为三维点云数据(相 ...
深度图像+rgb转化点云数据、点云数据打开、显示以及保存
头文件 #include<iostream> #include <fstream> #include <stdio.h> #include <string.h ...
自动驾驶中图像与点云融合的深度学习研究综述
Deep Learning for Image and Point Cloud Fusion in Autonomous Driving: A Review IEEE TRANSACTIONS ON ...
PCL点云与深度图像
PCL点云与深度图像 1 RangeImage概念及相关算法 1.1 深度图像简介 1.2 PCL中RangeImage的相关类 2 从一个点云创建一个深度图像 3 从深度图像中提取边界 4 点云到深 ...
如何从点云创建深度图像，看这篇你就懂了（附详细代码）
作者I Roar冷颜@CSDN 编辑I 3D视觉开发者社区前言目前,深度图像的获取方法有:激光雷达深度成像法.计算机立体视觉成像.坐标测量机法.莫尔条纹法.结构光法等.针对深度图像的研究重点主要集 ...
点云数据转深度图像（二）
文章目录一.简介二.图像生成代码三.实现效果参考资料一.简介之前使用过PCL中的生成深度图像功能,就想着使用MATLAB也实现一下类似的功能,整个过程是一个旋转水平和竖直角度的采样过程,如 ...
PCL深度图像(1)
目前深度图像的获取方法有激光雷达深度成像法,计算机立体视觉成像,坐标测量机法,莫尔条纹法,结构光法等等,针对深度图像的研究重点主要集中在以下几个方面,深度图像的分割技术 ,深度图像的边缘检测技术 ,基 ...
深度图像基础知识（一）
深度图像(depth image)也被称为距离影像(range image),是指将从图像采集器到场景中各点的距离(深度)作为像素值的图像,它直接反映了景物可见表面的几何形状.深度图像经过坐标转换可以 ...

（Visual Navigation）深度图像转点云

文章目录

坐标系介绍

坐标系的转换和平移

图像坐标系转相机坐标系

代码实现

相机坐标系转世界坐标系

点云表示为地图信息请看下一篇文章

（Visual Navigation）深度图像转点云相关推荐

最新文章

热门文章