带有openCv的Aruco标记，获取3d角坐标？

Question

我正在使用 opencv 3.2 检测打印的 Aruco 标记：

 aruco::estimatePoseSingleMarkers(corners, markerLength, camMatrix, distCoeffs, rvecs,tvecs);

这将返回标记的平移和旋转向量。我需要的是标记每个角的 3d 坐标。

我知道标记长度，我可以做类似的事情

corner1 = tvecs[0] - markerlength /2;
corner2 = tvecs[0] + markerlength /2;

……

但是有更好的方法吗？还是现有的功能？总结一下，我有：

2d 正方形中心的 3d 点。

该正方形边的长度。

正方形的旋转值。

如何找到角的 3d 坐标？

原文由 anti 发布，翻译遵循 CC BY-SA 4.0 许可协议

Answer 1

首先，假设我们只有一个标记 side = 2 * half_side 。

其次， aruco::detectMarker 返回相机在标记世界中的相对位置。因此，我假设您正在寻找 相机世界 中角落的坐标。

然后，在标记的空间：

      [ half_side ]      [     0     ]
E  = [     0     ], F = [ half_side ]
     [     0     ]      [     0     ]

where the center O of the square has coordinate tvec (in camera’s world) and rotation mat of the marker rot_mat is computed by cv::Rodrigues(rvec,rot_mat) .

现在，使用针孔 相机模型，cam世界中的点 P 与marker世界的坐标之间的关系是：

 [P_x_cam]             [P_x_marker]
[P_y_cam] = rot_mat * [P_y_marker] + tvec
[P_z_cam]             [P_z_marker]

例如，中心 O [0,0,0] 即标记世界中的 tvec ，在凸轮世界中是 --- 。

所以，cam世界中 E 的坐标是：

 [E_x_cam]             [half_side]
|E_y_cam| = rot_mat * |    0    | + tvec
[E_z_cam]             [    0    ]

神奇的是，它是 rot_mat 的第一列乘以 half_size 和 tvec 的总和。 Similarly, the coodinates of F is rot_mat ’s second column multiplied by half_size and tvec .

现在，可以计算角点，例如

C - O = (E - O) + (F - O), B - O = (E - O) - (F - O)

其中 E-O 正是 rot_mat 的第一列乘以 half_size 。

考虑到所有这些，我们可以编写函数：

 vector<Point3f> getCornersInCameraWorld(double side, Vec3d rvec, Vec3d tvec){

     double half_side = side/2;

     // compute rot_mat
     Mat rot_mat;
     Rodrigues(rvec, rot_mat);

     // transpose of rot_mat for easy columns extraction
     Mat rot_mat_t = rot_mat.t();

     // the two E-O and F-O vectors
     double * tmp = rot_mat_t.ptr<double>(0);
     Point3f camWorldE(tmp[0]*half_side,
                       tmp[1]*half_side,
                       tmp[2]*half_side);

     tmp = rot_mat_t.ptr<double>(1);
     Point3f camWorldF(tmp[0]*half_side,
                       tmp[1]*half_side,
                       tmp[2]*half_side);

     // convert tvec to point
     Point3f tvec_3f(tvec[0], tvec[1], tvec[2]);

     // return vector:
     vector<Point3f> ret(4,tvec_3f);

     ret[0] +=  camWorldE + camWorldF;
     ret[1] += -camWorldE + camWorldF;
     ret[2] += -camWorldE - camWorldF;
     ret[3] +=  camWorldE - camWorldF;

     return ret;
}

---

注 1：我讨厌 SO 没有 MathJax

注意2：必须有一些我不知道的更快的实现。

原文由 Quang Hoang 发布，翻译遵循 CC BY-SA 3.0 许可协议