识别定位——二维、三维坐标转换的数学推导

写在前面:有摘抄网络部分图片,侵删

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

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

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

再讲一个概念:齐次坐标,即用N+1维表示N维。因为坐标的平移是矩阵加减、坐标旋转是矩阵乘法。故齐次坐标的好处在于增加一个维度,将加法乘法表示在一个式子里。
识别定位——二维、三维坐标转换的数学推导_第1张图片
上面这个公式是坐标转换的公式,【R|t】为一个增广矩阵,包含旋转和平移。

那我们只要知道了旋转矩阵就能完成转换了。
识别定位——二维、三维坐标转换的数学推导_第2张图片

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

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

识别定位——二维、三维坐标转换的数学推导_第3张图片

如图所示点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θ
写成矩阵的形式是:

识别定位——二维、三维坐标转换的数学推导_第4张图片
因为之前说过了要升维度,所以最终形式是:

绕x轴旋转
识别定位——二维、三维坐标转换的数学推导_第5张图片
绕y轴旋转
识别定位——二维、三维坐标转换的数学推导_第6张图片
绕z轴旋转
识别定位——二维、三维坐标转换的数学推导_第7张图片
求出了各个轴分别的旋转矩阵,相乘就得到整个的旋转矩阵。因为旋转矩阵是左乘,所以Y-X-Z顺规的欧拉角对应的组合旋转矩阵就是R = RzRxRy。如果是Z-Y-X顺规的欧拉角,对应的组合旋转矩阵就是R = RxRyRz。R是一个3*3的矩阵,为整个坐标转换的旋转矩阵。

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

识别定位——二维、三维坐标转换的数学推导_第8张图片
注:atan2()为C++中的函数,atan2(y,x) 求的是y/x的反正切,其返回值为[-pi,+pi]之间的一个数。
这就是坐标转换的数学推导。

你可能感兴趣的:(computer,vision,定位,slam)