问题
I am detecting a printed Aruco marker using opencv 3.2:
aruco::estimatePoseSingleMarkers(corners, markerLength, camMatrix, distCoeffs, rvecs,tvecs);
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:
a 3d point in the center of a 2d square.
the length of the sides of that square.
the rotation value of the square.
How can I find the 3d coordinates of the corners?
回答1:
First, let's assume that we only have one marker given with side = 2 * half_side.
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.
Then, in marker's space:
[ half_side ] [ 0 ]
E = [ 0 ], F = [ half_side ]
[ 0 ] [ 0 ]
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).
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]
for example, the center O, which is [0,0,0] in marker's world, is tvec in cam's world.
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 ]
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)
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;
}
Note 1: I hate that SO doesn't have MathJax
Note 2: there must be some faster implementation which I don't know of.
回答2:
Building upon @Quang's answer, C# code for transforming any point to camera coordinates. Of course it needs R and t vectors, so you're going to need a marker in order to get them.
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));
}
回答3:
A python implementation I wrote for the above described rotation of marker corners using rvec and tvec as returned from cv2.aruco.estimatePoseSingleMarkers(). Thanks @Quang Hoang for the detailed explanation.
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.
'''
来源：https://stackoverflow.com/questions/46363618/aruco-markers-with-opencv-get-the-3d-corner-coordinates