使用openCv的Aruco标记，获取3D角坐标? [英] Aruco markers with openCv, get the 3d corner coordinates?

问题描述

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

I am detecting a printed Aruco marker using opencv 3.2:

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

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

this returns a translation and rotation vector for the marker. What I need, is the 3d coordinates for each corner of the marker.

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

As i know the marker length, i could do something like

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

....

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

But is there an better way? Or an existing function? To sum up, I have:

在2d正方形中心的3d点.

a 3d point in the center of a 2d square.

那个正方形的边长.

正方形的旋转值.

如何找到角的3d坐标?

How can I find the 3d coordinates of the corners?

推荐答案

首先，让我们假设我们只有一个用side = 2 * half_side给出的标记.

First, let's assume that we only have one marker given with side = 2 * half_side.

第二，aruco::detectMarker返回摄像机在标记世界中的相对位置.因此，我假设您正在寻找相机世界中 的角点坐标 .

Second, aruco::detectMarker returns the relative position of the camera in the marker's world. Thus, I assume that you are looking for the coordinates of the corners in camera's world.

然后，在标记的空白处

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

，其中正方形的中心O具有坐标tvec(在相机世界中)，并且标记rot_mat的旋转垫由cv::Rodrigues(rvec,rot_mat)计算.

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).

现在，使用针孔相机模型，凸轮世界和标记世界中的点P的坐标之间的关系是:

Now, using the pinhole camera model, the relation between coordinates of a point P in cam's world and marker's world is:

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

例如，

，在标记世界中为[0,0,0]的中心O在凸轮世界中为tvec.

for example, the center O, which is [0,0,0] in marker's world, is tvec in cam's world.

因此，E在cam的世界中的坐标为:

So, the coordinates of E in cam's world are:

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

魔术地讲，它是rot_mat的第一列的总和乘以half_size和tvec.相似地， F的余项是rot_mat的第二列乘以half_size和tvec.

Magically, it is the sum of rot_mat's first column multiplied by half_size and tvec. Similarly, the coodinates of F is rot_mat's second column multiplied by half_size and tvec.

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

Now, the corners can be computed, for example

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

其中E-O恰好是rot_mat的第一列乘以half_size.

where E-O is exactly rot_mat's first column multiplied by half_size.

牢记所有这些，我们可以编写函数:

With all that in mind, we can compose the function:

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

---

Note 1: I hate that SO doesn't have MathJax

注2:必须有一些我不知道的更快的实现.

Note 2: there must be some faster implementation which I don't know of.

这篇关于使用openCv的Aruco标记，获取3D角坐标?的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！