写在前面：有摘抄网络部分图片，侵删

首先，因为使用了摄像头，所以在定位之前要对相机进行标定！！

在这里，我们只需要用到相机内参和畸变参数两个参数。

先说一下我的思路：我们通过摄像头识别能得到二维坐标，然后再自己定义一个世界坐标系，目标点的三维坐标由我们自己定义，以此求得相机的坐标。因为位置的相对关系，能得到目标点相对于相机的坐标。再通过欧拉角变换，tf转换得到在世界坐标系下目标点的坐标

再讲一个概念：齐次坐标，即用N+1维表示N维。因为坐标的平移是矩阵加减、坐标旋转是矩阵乘法。故齐次坐标的好处在于增加一个维度，将加法乘法表示在一个式子里。

上面这个公式是坐标转换的公式，【R|t】为一个增广矩阵，包含旋转和平移。

那我们只要知道了旋转矩阵就能完成转换了。

上图是有两个坐标系——世界坐标系和相机坐标系。坐标系能通过三次旋转得到另一个坐标系

欧拉角定义了一组物体的旋转次序，绕某个轴旋转多少度。很多人会忽略旋转顺序的问题，很多书上或者博客叫做顺规，可以理解为欧拉角旋转顺序的规定。(α, β, γ)在不同的旋转顺序下会有不同的结果，先绕X轴旋转α，还是先绕y轴旋转β，最后的结果是不一样的。欧拉角的顺规有很多，例如Z-X-Y, X-Y-Z, X-Y-X, Z-X-Y ···，其拥有众多的排列组合。

如图所示点v绕原点旋转θ 角，得到点v’，假设 v点的坐标是(x, y) ，那么可以推导得到 v’点的坐标（x’, y’)(设原点到v的距离是r，原点到v点的向量与x轴的夹角是ϕ
x=rcosϕ

y=rsinϕ
x′=rcos(θ+ϕ)

y′=rsin(θ+ϕ)
通过三角函数展开得到
x′=rcosθcosϕ−rsinθsinϕ
y′=rsinθcosϕ+rcosθsinϕ
带入x和y表达式得到
x′=xcosθ−ysinθ
y′=xsinθ+ycosθ
写成矩阵的形式是：

因为之前说过了要升维度，所以最终形式是：

绕x轴旋转

绕y轴旋转

绕z轴旋转

求出了各个轴分别的旋转矩阵，相乘就得到整个的旋转矩阵。因为旋转矩阵是左乘，所以Y-X-Z顺规的欧拉角对应的组合旋转矩阵就是R = RzRxRy。如果是Z-Y-X顺规的欧拉角，对应的组合旋转矩阵就是R = RxRyRz。R是一个3*3的矩阵，为整个坐标转换的旋转矩阵。

我们再通过反三角函数求出角度。

注：atan2()为C++中的函数，atan2(y，x) 求的是y/x的反正切，其返回值为[-pi,+pi]之间的一个数。
这就是坐标转换的数学推导。