Aruco 标记与 openCv，获取 3d 角坐标？

我正在使用 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 坐标？

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

Second, aruco::detectMarker返回相机在标记世界中的相对位置。因此，我假设您正在寻找角点的坐标在相机的世界里.

然后，在标记的空间中：

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

中心在哪里O正方形的坐标有tvec（在相机的世界中）和标记的旋转垫rot_mat计算公式为cv::Rodrigues(rvec,rot_mat).

现在，使用针孔相机型号 http://docs.opencv.org/3.0-beta/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html，点坐标之间的关系P在凸轮的世界和标记的世界中是：

[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在 cam 的世界里有：

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

神奇的是，它是rot_mat的第一列乘以half_size and tvec。相似地， 的坐标F is rot_mat的第二列乘以half_size and tvec.

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

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

where 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：一定有一些我不知道的更快的实现。