使用 openCv 的 Aruco 标记，获取 3d 角坐标？

Question

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

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

此 returns 标记的平移和旋转向量。我需要的是标记每个角的 3d 坐标。

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

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

.....

但是有更好的方法吗？还是已有的功能？综上所述，我有：

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

那个正方形边的长度。

方块的旋转值

如何找到角的 3d 坐标？

Answer 1

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

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

然后，在标记的 space:

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

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

现在，使用针孔 camera model，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，在marker的世界里是[0,0,0]，在cam的世界里是tvec。

所以E在cam世界的坐标是：

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

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

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

Answer 2

基于@Quang 的回答，用于将任何点转换为相机坐标的 C# 代码。当然它需要 R 和 t 向量，所以你需要一个标记才能得到它们。

private Point3d GetWorldPoint(Point3d input, Vec3d rvec, Vec3d tvec)
{
    var rot_mat = new Mat();

    Cv2.Rodrigues(MatOfDouble.FromArray(rvec.Item0, rvec.Item1, rvec.Item2), rot_mat);

    var pointProject = (rot_mat * MatOfDouble.FromArray(input.X, input.Y, input.Z)).ToMat();
    return tvec + new Point3d(pointProject.Get<double>(0, 0), pointProject.Get<double>(0, 1), pointProject.Get<double>(0, 2));
}

Answer 3

我为上述标记角旋转编写的 python 实现，使用从 cv2.aruco.estimatePoseSingleMarkers() 返回的 rvec 和 tvec。感谢@Quang Hoang 的详细讲解

import numpy as np

# rotate a markers corners by rvec and translate by tvec if given
# input is the size of a marker.
# In the markerworld the 4 markercorners are at (x,y) = (+- markersize/2, +- markersize/2)
# returns the rotated and translated corners and the rotation matrix
def rotate_marker_corners(rvec, markersize, tvec = None):

    mhalf = markersize / 2.0
    # convert rot vector to rot matrix both do: markerworld -> cam-world
    mrv, jacobian = cv2.Rodrigues(rvec)

    #in markerworld the corners are all in the xy-plane so z is zero at first
    X = mhalf * mrv[:,0] #rotate the x = mhalf
    Y = mhalf * mrv[:,1] #rotate the y = mhalf
    minusX = X * (-1)
    minusY = Y * (-1)

    # calculate 4 corners of the marker in camworld. corners are enumerated clockwise
    markercorners = []
    markercorners.append(np.add(minusX, Y)) #was upper left in markerworld
    markercorners.append(np.add(X, Y)) #was upper right in markerworld
    markercorners.append(np.add( X, minusY)) #was lower right in markerworld
    markercorners.append(np.add(minusX, minusY)) #was lower left in markerworld
    # if tvec given, move all by tvec
    if tvec is not None:
        C = tvec #center of marker in camworld
        for i, mc in enumerate(markercorners):
            makercorners[i] = np.add(C,mc) #add tvec to each corner
    #print('Vec X, Y, C, dot(X,Y)', X,Y,C, np.dot(X,Y)) # just for debug
    markercorners = np.array(markercorners,dtype=np.float32) # type needed when used as input to cv2
    return markercorners, mrv

'''
Copyright 2019 Marco Noll, Garmin International Inc. Licensed under the Apache 
License, Version 2.0 (the "License"); you may not use this file except in compliance 
with the License. You may obtain a copy of the License at

http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software distributed 
under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR 
CONDITIONS OF ANY KIND, either express or implied. See the License for the specific 
language governing permissions and limitations under the License.
'''

使用 openCv 的 Aruco 标记，获取 3d 角坐标？

Aruco markers with openCv, get the 3d corner coordinates?

c++

opencv

aruco