Во-первых, давайте предположим, что у нас есть только один маркер, заданный с помощью side = 2 * half_side
.
![введите описание изображения здесь](https://i.stack.imgur.com/LnsMG.png)
Во-вторых, 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: мне не нравится, что в SO нет MathJax
Примечание 2: должна быть более быстрая реализация, о которой я не знаю.
person
Quang Hoang
schedule
22.09.2017