我正在使用opencv 3.2检测打印的Aruco标记:
<?xml version="1.0" encoding="utf-16"?>
<Request xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns:xsd="http://www.w3.org/2001/XMLSchema">
<AccountStage att1="abcd" />
</Request>
这将返回标记的平移和旋转向量。我需要的是标记每个角落的3d坐标。
我知道标记长度,我可以做类似
的事情aruco::estimatePoseSingleMarkers(corners, markerLength, camMatrix, distCoeffs, rvecs,tvecs);
...
但是有更好的方法吗?还是现有的功能? 总而言之,我有:
在2d广场中心的3d点。
该广场两侧的长度。
方形的旋转值。
如何找到角落的三维坐标?
答案 0 :(得分:10)
首先,我们假设我们只有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)
计算。
现在,使用针孔camera model,凸轮世界中点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]
在cam世界中为tvec
。
因此,cam世界中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:必须有一些我不知道的更快的实现。
答案 1 :(得分:0)
基于@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));
}
答案 2 :(得分:0)
我使用rvec和tvec为从cv2.aruco.estimatePoseSingleMarkers()返回的上述标记点的旋转编写了python实现。感谢@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.
'''