Маркеры Aruco с openCv, получить координаты угла 3d?

Я обнаружил напечатанный маркер Aruco, используя opencv 3.2:

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

это возвращает вектор перемещения и вращения для маркера. Что мне нужно, это 3D-координаты для каждого угла маркера.

Поскольку я знаю длину маркера, я мог бы сделать что-то вроде

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

....

Но есть ли лучший способ? Или существующая функция? Подводя итог, я имею:

3d точка в центре 2-го квадрата.

длина сторон этого квадрата.

значение вращения квадрата.

Как я могу найти 3D-координаты углов?

Источник

2 ответа

Решение

Во-первых, давайте предположим, что у нас есть только один маркер с side = 2 * half_side,

Во-вторых, aruco::detectMarker возвращает относительное положение камеры в мире маркера. Таким образом, я предполагаю, что вы ищете координаты углов в мире камеры.

Тогда в пространстве маркера:

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

где центр O квадрат имеет координату tvec (в мире камеры) и вращающийся коврик маркера rot_mat вычисляется cv::Rodrigues(rvec,rot_mat),

Теперь, используя модель камеры- обскуры, связь между координатами точки 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 в мире кулачка.

Итак, координаты E в мире кулачков есть:

[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: я ненавижу, что ТАК не имеет MathJax

Примечание 2: должна быть более быстрая реализация, о которой я не знаю.

Источник

Реализация python, которую я написал для описанного выше поворота углов маркера с использованием rvec и tvec, возвращенных из cv2.aruco.estimatePoseSingleMarkers(). Спасибо @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.
'''
Источник

Основываясь на ответе @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));
}
Источник
Другие вопросы по тегам