世界坐标系，相机坐标系，像素坐标系转换 详细说明（附代码）

几个坐标系介绍，相机内外参的回顾参考此文。
本文主要说明如何在几个坐标系之间转换。

本文涉及：

1. 使用相机内参 在 像素坐标系 和 相机坐标系 之间转换。
2. 使用相机外参（位姿）在相机坐标系 和 世界坐标系 之间转换。
3. (qw,qx,qy,qz,tx,ty,tz)形式的外参如何使用。
4. 以具体情景为例，每一步详细说明，并结合代码进一步理解每个步骤。

以下面的情景为例。
假设 I1 (img1) 上有一点p1，现在要通过相机1，相机2的内外参把p1映射到 I2 (img2)上的对应点p2.
还需要知道p1的深度，假设有img1的深度图，可以读取p1处的深度。

整体思路：

p1在图片 I1 上，是像素坐标系，根据camera1的内参把它转到camera1的相机坐标系，得到(xc1, yc1, zc1),
根据camera1的外参把 (xc1, yc1, zc1) 转到 世界坐标系，得到上图中的P点坐标(xw1, yw1, zw1),
根据camera2的外参把P点 (xw1, yw1, zw1) 转到camera2的相机坐