深度图转点云高速化（Python，numpy）

之前也有用python和numpy写过深度图转点云的代码，但是速度慢出天际 无法实时处理视频流

本来以为慢的理由是毕竟python本来就慢，打算找时间用C++重写一遍

不过重写之前先在之前python写的基础上优化一下提提速，结果发现其实优化好后一点也不慢，慢的竟是我自己

作为测试，输入一张（960，720）大小的深度图，转成点云循环一千次，程序用时17秒

其中7秒是参数初始化时间，真正处理图片的地方只用了10秒

考虑到实际运用的时候可以无视初始化时间，以及不会用到这么大的深度图大小（太大了之后别的程序也处理不动这点云）

远远超过使用需求了（1秒20帧以上）

代码：

测试用例：

说明：

首先python计算想要快，那就得尽可能别在python里面进行计算和循环

这次优化就是尽可能让numpy来完成所有的计算和循环，在python里面循环越少 那么就越快

还有一些是对本来的计算方法的一些优化，特别运用场景是实时视频流的场合

为了用numpy来计算，那就全部用矩阵计算来搞定

首先是投影矩阵  相机坐标系——图像坐标系

为了描述方便，简写成

其中[P]为画像坐标系，也就是图像上每一个像素的坐标

[C]为相机内参矩阵

[D]为相机坐标系的三维坐标，因为不考虑相机外参（这部分转换交给ROS更方便），所以[D]就是这次求解的目标，然后相机坐标系的Z 也就是深度图里面的depth

这样的话[D]的求解就应该为

所以初始化的时候首先导入相机内参矩阵，这里用的是我的摄像头标定的数据，为了后期调整大小方便引入了一个缩放系数

当输入图像缩放的时候，相机内参矩阵也需要按照这样子同比例缩放

numpy初始化一个[P]矩阵

计算相机矩阵的逆矩阵 [C]逆

重新再看一遍投影方程

会发现，如果输入的是一个视频流  在图像尺寸以及相机内参都不会发生改变的情况下

那么随时间变化Z是会改变的，但是[P]和[C]逆不会变

也就是

所以避免重复计算，我们先行计算这个[K]=[P][C]逆

先来numpy初始化一个尺寸和画像大小相同，但是一个像素位里面有三个格子空间的数组来存放数据，其实就和普通的RGB图像是一个维度 只不过这里我们用来存放XYZ而不是RGB

然后用numpy.nditer迭代一个和深度图同样大小的数组，计算所有位置的[K]，保存到上面创建的容器的对应坐标里面

计算完成后初始化准备工作就完成了

接下来开始实际计算点云坐标

先拿到深度图，需要缩放的话就缩放

为了避免计算改变初始化好的那个容器，需要用copy来复制一个新的

有了之前的准备，现在我们只需要把深度图上每一个像素的深度值Z乘以这个vector_array对应的每一个像素里面的[XYZ]就可以

而这个对应位置相乘直接用numpy.array和numpy.array的*乘就可以搞定

也就是

运算效果上是

至此点云的计算就完成了，最后只是改成标准形状

如果是要发布到ROS里面，还涉及一个左手系到右手系的变换

放以前的话我会在循环后面这么写

但是这么一来也是多做了无用的计算，开头的测试用例的话会从17秒增加到27秒

虽然还是绰绰有余，但是更好的方式显然是在初始化的时候就完成坐标变换

在初始化函数最后加上

运行测试用例，完成了坐标系变换同时 运行时间还是17秒  可喜可贺 可喜可贺