网易严选的网站建设,php+mysql网站开发...,网站技术支持,flash属于网页制作平台吗几个坐标系介绍#xff0c;相机内外参的回顾参考此文。 本文主要说明如何在几个坐标系之间转换。
本文涉及#xff1a;
使用相机内参 在 像素坐标系 和 相机坐标系 之间转换。使用相机外参#xff08;位姿#xff09;在相机坐标系 和 世界坐标系 之间转换。(qw,qx,qy,qz,…几个坐标系介绍相机内外参的回顾参考此文。 本文主要说明如何在几个坐标系之间转换。
本文涉及
使用相机内参 在 像素坐标系 和 相机坐标系 之间转换。使用相机外参位姿在相机坐标系 和 世界坐标系 之间转换。(qw,qx,qy,qz,tx,ty,tz)形式的外参如何使用。以具体情景为例每一步详细说明并结合代码进一步理解每个步骤。
以下面的情景为例。 假设 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的相机坐标系得到 (xc2, yc2, zc2). 最后根据camera2的内参 把 (xc2, yc2, zc2) 转到像素坐标系得到图像 I2 上的 p2 点坐标(x2, y2).
整个坐标系的转换关系像素1 - 相机1 - 世界 - 相机2 - 像素2
其中像素坐标系为2D其他都是3D。 相机外参 也 称为 位姿 (pose).
具体步骤
(1). p1 像素坐标 -- 相机1 坐标
这两个坐标系的关系由相机内参决定 相机内参fx, fy, cx, cy) 假设像素坐标为(x1, y1), 相机1坐标为(xc1, yc1, zc1), 其中zc1为 I1 的深度图 (xc1, yc1)处的值那么 x 1 f x x c 1 z c 1 c x x_{1} f_{x}\frac{x_{c1}}{z_{c1}} c_{x} x1fxzc1xc1cx, y 1 f y y c 1 z c 1 c y y_{1} f_{y}\frac{y_{c1}}{z_{c1}} c_{y} y1fyzc1yc1cy (1)
现在要求 xc1 和 yc1, 由1得到 x c 1 ( x 1 − c x ) ∗ z c 1 / f x x_{c1} (x_{1}- c_{x}) * z_{c1} / f_{x} xc1(x1−cx)∗zc1/fx y c 1 ( y 1 − c y ) ∗ z c 1 / f y y_{c1} (y_{1}- c_{y}) * z_{c1} / f_{y} yc1(y1−cy)∗zc1/fy
代码
depth1_ori cv2.imread(depth1.png, -1) #uint16型
depth1 cv2.split(depth1_ori)[0]
#p1点对应的相机坐标
zc1 depth1[y1, x1] / 1000.0 #这里深度单位是mm
xc1 (x1 - cx) * zc1 / fx
yc1 (y1 - cy) * zc1 / fy(2). p1 的相机1 坐标 -- 世界坐标
转换关系 相机坐标 T * 世界坐标 世界坐标 T-1 * 相机坐标 其中 T 为world - camera的转换矩阵。
如何求得转换矩阵 T 先从概念介绍开始
旋转矩阵R 3 * 3矩阵 平移向量 t : 3 * 1矩阵 把R 和 t 拼成转换矩阵 T 4 * 4矩阵 T [ R t 0 T 1 ] T \begin{bmatrix} R t\\ 0^{T}1 \end{bmatrix} T[R0Tt1]
顺便提一下李群李代数T是SE(3), R是SO(3).
话题回到坐标(xc1, yc1, zc1)为相机1坐标(xw, yw, zw) 为世界坐标那么世界坐标转相机坐标为 [ x c 1 y c 1 z c 1 1 ] T ⋅ [ x w y w z w 1 ] \begin{bmatrix} x_{c1} \\ y_{c1}\\ z_{c1}\\ 1 \end{bmatrix} T \cdot \begin{bmatrix} x_{w} \\ y_{w}\\ z_{w}\\ 1 \end{bmatrix} xc1yc1zc11 T⋅ xwywzw1
你肯定很好奇为什么要加一维呢 如果 T T T 不加最后一行的 [ 0 T 1 ] \begin{bmatrix} 0^{T}1 \end{bmatrix} [0T1]坐标也不加最后一维的1直接 T [ R t ] T \begin{bmatrix} R t \end{bmatrix} T[Rt] 也能计算为什么一定要加一维 [ x c y c z c ] T ⋅ [ x w y w z w ] \begin{bmatrix} x_{c} \\ y_{c}\\ z_{c} \end{bmatrix} T \cdot \begin{bmatrix} x_{w} \\ y_{w}\\ z_{w} \end{bmatrix} xcyczc T⋅ xwywzw , 这里 T [ R t ] T \begin{bmatrix}R t\end{bmatrix} T[Rt]
是这样的现在是从 世界坐标 转 相机1坐标 如果要把 相机1坐标 转 世界坐标 呢 我们现在要做的就是把 p1的 相机1坐标 转到 世界坐标。
那就需要这么计算了, [ x w y w z w ] T − 1 ⋅ [ x c y c z c ] \begin{bmatrix} x_{w} \\ y_{w}\\ z_{w} \end{bmatrix} T^{-1}\cdot \begin{bmatrix} x_{c} \\ y_{c}\\ z_{c} \end{bmatrix} xwywzw T−1⋅ xcyczc 这里 T [ R t ] T \begin{bmatrix}R t\end{bmatrix} T[Rt]无法求逆矩阵
求 T 的逆矩阵T 必须是square行数 列数的不能是3 * 4, 必须是4 * 4的。
所以加上一行凑成 4 * 4 矩阵 T [ R t 0 T 1 ] T \begin{bmatrix} R t\\ 0^{T}1 \end{bmatrix} T[R0Tt1]
那么 相机坐标 -- 世界坐标 就变为 [ x w y w z w 1 ] T − 1 ⋅ [ x c y c z c 1 ] \begin{bmatrix} x_{w} \\ y_{w}\\ z_{w}\\ 1 \end{bmatrix} T^{-1} \cdot \begin{bmatrix} x_{c} \\ y_{c}\\ z_{c}\\ 1 \end{bmatrix} xwywzw1 T−1⋅ xcyczc1
有的程序中会使用Twc, Tcw这样的称呼这里w指world, 是世界坐标c指camera, 是相机坐标。 T表示转换矩阵至于Twc 是world转camera 还是camera转world, 需要根据实际情况而定每个开发者习惯不一样。
实际中到了这里估计还是不知如何计算 T问题在哪呢
我们拿到的 相机外参 一般会是一个四元数平移向量的形式其中并没有R矩阵。 相机外参(qw, qx, qy, qz, tx, ty, tz), 这个顺序要根据实际情况而定有的相机顺序并不是这样。
这里用四元数 q (qw, qx, qy, qz) 代替了R矩阵 原因在于R是3 * 3矩阵有9个量而一次旋转只有3个自由度这种表达方式是冗余的四元数的表达更紧凑。
上面是涉及到的相关概念现在开始计算T。
计算转换矩阵 T
现在要先把 q 转为 R再由R, t 得到T。 q (qw, qx, qy, qz), 一定是qw, qx, qy ,qz的顺序不是的先调整到这个顺序 t (tx, ty, tz), 这里要注意t 的单位如果是mm, 需要 / 1000.0.
如果用Eigen库可以这么得到T Isometry3d是4 * 4 欧式变换矩阵就是T的格式参考
Eigen::Quaterniond q(qw, qx, qy, qz);
Eigen::Isometry3d T(q);
//先设置的旋转矩阵下面平移要在旋转前的坐标系上平移所以是pretranslate
T.pretranslate(Eigen::Vector3d(tx, ty, tz));如果用Sophus::SE3d
SE3d T SE3d(Quaterniond(qw, qx, qy, qz),Vector3d(tx, ty, tz)));直接计算的话由四元数 q 到旋转矩阵 R 的公式为转此处的图 这里q0, q1, q2, q3分别对应 qw, qx, qy, qz. 结合 (tx, ty, tz), 下面再加一行 [ 0 T 1 ] \begin{bmatrix} 0^{T}1 \end{bmatrix} [0T1]得到T1 由相机1的外参得到。
T1 np.array([[1 - 2 * q2 ** 2 - 2 * q3 ** 2,2 * q1 * q2 - 2 * q0 * q3,2 * q1 * q3 2 * q0 * q2,tx, #注意单位如果是mm,要/1000.0],[2 * q1 * q2 2 * q0 * q3,1 - 2 * q1 ** 2 - 2 * q3 ** 2,2 * q2 * q3 - 2 * q0 * q1,ty, #注意单位如果是mm,要/1000.0],[2 * q1 * q3 - 2 * q0 * q2,2 * q2 * q3 2 * q0 * q1,1 - 2 * q1 ** 2 - 2 * q2 ** 2,tz, #注意单位如果是mm,要/1000.0],[0,0,0,1],])已经得到了T1下面可把相机坐标转为世界坐标 [ x w y w z w 1 ] T 1 − 1 ⋅ [ x c 1 y c 1 z c 1 1 ] \begin{bmatrix} x_{w} \\ y_{w}\\ z_{w}\\ 1 \end{bmatrix} T_{1}^{-1} \cdot \begin{bmatrix} x_{c1} \\ y_{c1}\\ z_{c1}\\ 1 \end{bmatrix} xwywzw1 T1−1⋅ xc1yc1zc11
代码
p1_c np.array([xc1, yc1, zc1, 1])
p_w np.matmul(np.linalg.inv(T1), np.expand_dims(p1_c,1))(3). 世界坐标 -- 相机2坐标
上面已经说明了如何由 世界坐标 转 相机坐标。 注意上面求的T1 是由相机1的外参得到 这里要用到相机2的外参camera2: (qw2, qx2, qy2, qz2, tx2, ty2, tz2), 求得T2 后由下式得到 P 的相机2坐标 [ x c 2 y c 2 z c 2 1 ] T 2 ⋅ [ x w y w z w 1 ] \begin{bmatrix} x_{c2} \\ y_{c2}\\ z_{c2}\\ 1 \end{bmatrix} T_{2} \cdot \begin{bmatrix} x_{w} \\ y_{w}\\ z_{w}\\ 1 \end{bmatrix} xc2yc2zc21 T2⋅ xwywzw1
p2_c np.matmul(T2, p_w)(4) 相机2坐标 -- 像素坐标2
相机内参fx, fy, cx, cy) x 2 f x x c 2 z c 2 c x x_{2} f_{x}\frac{x_{c2}}{z_{c2}} c_{x} x2fxzc2xc2cx, y 2 f y y c 2 z c 2 c y y_{2} f_{y}\frac{y_{c2}}{z_{c2}} c_{y} y2fyzc2yc2cy
xc2 p2_c[0]
yc2 p2_c[1]
zc2 p2_c[2]
x2 xc2 * fx / zc2 cx
y2 yc2 * fy / zc2 cy这样就得到了图像 I2 上的映射点 p2的坐标。
