I am currently working on a camera pose estimation project using only one marker with ARUCO.
I used Aruco's Marker Detector to detect markers and get the marker's Rvec and Tvec. I understand these two vectors represent the transform from the marker to the camera, which is the marker's pose w.r.t camera. I form a 4 by 4 matrix called T_marker_camera using these two vectors.
Then, I set up a world frame (left handed) and get the marker's world pose, which is a 4 by 4 transform matrix.
I want to calculate the pose of the camera w.r.t the world frame, and I use the following formula to calculate it:
T_camera_world = T_marker_world * T_marker_camera_inv
Before I perform the above formula, I convert the OpenCV coordinates to the left handed one (flip the sign of x axis).
However, I didn't get the correct x, y, z of the camera w.r.t the world frame.
What did I miss to get the correct answer?
Thanks
The one equation you gave looks right, so the issue is probably somewhere that you didn't show/describe.
A fix in your notation will help clarify.
Write the pose/source frame on the right (input), the reference/destination frame on the left (output). Then your matrices "match up" like dominos.
rvec and tvec yield a matrix that should be called T_cam_marker.
If you want the pose of your camera in the world frame, that is
T_world_cam = T_world_marker * T_marker_cam
T_world_cam = T_world_marker * inv(T_cam_marker)
(equivalent to what you wrote, but domino)
Be sure that you do matrix multiplication, not element-wise multiplication.
To move between left-handed and right-handed coordinate systems, insert a matrix that maps coordinates accordingly. Frames:
OpenCV camera/screen: right-handed, {X right, Y down, Z far}
ARUCO (in OpenCV anyway): right-handed, {X right, Y far, Z up}, first corner is top left (-X+Y quadrant)
whatever leftie frame you have, let's say {X right, Y up, Z far} and it's a screen or something
The hand-change matrix for typical frames on screens is an identity but with the entry for Y being a -1. I don't know why you would flip the X but that's "equivalent", ignoring any rotations.
I'm looking for the correct way to apply an offset to a set of Euler rotations. I would like to have a transformation where given a specific set of Euler angles (x1,y1,z1), if I transform them I would get an Euler sequence of (0,0,0). All other Euler sequences (xi,yi,zi) transformed would then act like (x1,y1,z1) were (0,0,0).
Background:
I'm using Oculus DK2 HMD to display a virtual environment (Vizard 5, Python 2.7.5) while using a Vicon motion capture system (Nexus 2) to update the position and orientation. I can get the Euler angles of the HMD (from a marker cluster, not the DK2 gyroscope) from the Vicon system, but when facing my desired (0,0,0) orientation the HMD has a non-zero rotation sequence.
Problem:
I'm having a hard time thinking about what transformation (rotation matrix?) I could find that could take a sequence like (-105, 110, -30) and make it (0,0,0) without being a useless matrix of zeros. If the rotation matrix was all zeros, any sequence would be transformed to (0,0,0)). The formula I have in mind is (please ignore syntax):
[0,0,0] = (3x3)R*(3x1)[-105,110,-30] what is R? R cannot be 3x3 zero matrix.
Attempt:
I stupidly tried to simply subtract the offest euler angles like so:
import viz
viz.go()
navigationNode = viz.addGroup() #create node
viewLink = viz.link(navigationNode, viz.MainView) #link it to the main view
#I am not showing how I get variables HMDX HMDY HMDZ but it's not important for this question
navigationNode.setEuler(HMDRZ-90,HMDRX+180,-1*(HMDRY)+30) #update the view orientation
Alas,
I'm sure this is possible, in fact I've done similar things in the past where I had to transform marker clusters into a rigid body's frame at a calibration position. But I just can't get around the trivial solution in this case (zeros matrix). If by chance anyone would explain this using quaternions, I can also use those.
The right way to do this is to determine the transformation between the hmd orientation and the desired view orientation at a desired point. I call this time zero, even though time has little to do with it, really it's the "home" orientation. Once the transformation is known, it can be used to determine the view orientation using the data from the hmd, since the coordinate frames of the hmd and view can be thought of as mounted on the same rigid body (meaning their relative transformation will not change with time).
Here is the math:
Here is how I coded it (it works!):
import numpy as np
import math
#setup the desired calibration transformation (hmd orientation that will result in looking straight ahead
#Euler angles for the hmd at desired "zero" orientation
a0 = -177.9*math.pi/180
b0 = 31.2*math.pi/180
g0 = 90.4*math.pi/180
#make the matrices
Ra0 = np.matrix([[1,0,0],[0, float(math.cos(a0)),float(-1*math.sin(a0))],[0,float(math.sin(a0)),float(math.cos(a0))]],dtype=np.float)
Rb0 = np.matrix([[math.cos(b0),0,math.sin(b0)],[0,1,0],[-1*math.sin(b0),0,math.cos(b0)]],dtype=np.float)
Rg0 = np.matrix([[math.cos(g0),-1*math.sin(g0),0],[math.sin(g0),math.cos(g0),0],[0,0,1]],dtype=np.float)
#the hmd rotation matrix in the "zero" orientation
global RhmdU0
RhmdU0 = Ra0*Rb0*Rg0
#the view orientation when the hmd is in the "zero" orientation (basically a zero degree turn about the Z axis)
global RdU0
RdU0 = np.matrix([[1,0,0],[0,1,0],[0,0,1]],dtype=np.float)
#this can be called in a loop, inputs are Euler angles of the hmd
def InverseK(at,bt,gt):
global RdU0
global RhmdU0
#given 3 Euler sequence of the hmd in universal space, determine the viewpoint
Rat = np.matrix([[1,0,0],[0, float(math.cos(at)), float(-1*math.sin(at))],[0,float(math.sin(at)),float(math.cos(at))]],dtype=np.float)
Rbt = np.matrix([[math.cos(bt),0,math.sin(bt)],[0,1,0],[-1*math.sin(bt),0,math.cos(bt)]],dtype=np.float)
Rgt = np.matrix([[math.cos(gt),-1*math.sin(gt),0],[math.sin(gt),math.cos(gt),0],[0,0,1]],dtype=np.float)
RhmdUt = Rat*Rbt*Rgt
RdUt = RhmdUt*RhmdU0.transpose()*RdU0
#do inverse K to get euler sequence out:
beta = math.atan2(RdUt[0,2],math.sqrt(RdUt[1,2]**2+RdUt[2,2]**2))
alpha = math.atan2(-1*RdUt[1,2]/math.cos(beta),RdUt[2,2]/math.cos(beta))
gamma = math.atan2(-1*RdUt[0,1]/math.cos(beta),RdUt[0,0]/math.cos(beta))
return(alpha,beta,gamma)
I'm using Matlab for camera calibration using Jean-
Yves Bouget's Camera Calibration Toolbox. I have all the camera
parameters from the calibration procedure. When I use a new image not
in the calibration set, I can get its transformation equation e.g.
Xc=R*X+T, where X is the 3D point of the calibration rig (planar) in
the world frame, and Xc its coordinates in the camera frame. In other
words, I have everything (both extrinsic and intrinsic parameters).
What I want to do is to perform perspective correction on this image
i.e. I want it to remove any perspective and see the calibration rig
undistorted (its a checkerboard).
Matlab's new Computer Vision toolbox has an object that performs a perspective transformation on an
image, given a 3X3 matrix H. The problem is, I can't compute this
matrix from the known intrinsic and extrinsic parameters!
To all who are still interested in this after so many months, i've managed to get the correct homography matrix using Kovesi's code (http://www.csse.uwa.edu.au/~pk/research/matlabfns), and especially the homography2d.m function. You will need however the pixel values of the four corners of the rig. If the camera is steady fixed, then you will need to do this once. See example code below:
%get corner pixel coords from base image
p1=[33;150;1];
p2=[316;136;1];
p3=[274;22;1];
p4=[63;34;1];
por=[p1 p2 p3 p4];
por=[0 1 0;1 0 0;0 0 1]*por; %swap x-y <--------------------
%calculate target image coordinates in world frame
% rig is 9x7 (X,Y) with 27.5mm box edges
XXw=[[0;0;0] [0;27.5*9;0] [27.5*7;27.5*9;0] [27.5*7;0;0]];
Rtarget=[0 1 0;1 0 0;0 0 -1]; %Rotation matrix of target camera (vertical pose)
XXc=Rtarget*XXw+Tc_ext*ones(1,4); %go from world frame to camera frame
xn=XXc./[XXc(3,:);XXc(3,:);XXc(3,:)]; %calculate normalized coords
xpp=KK*xn; %calculate target pixel coords
% get homography matrix from original to target image
HH=homography2d(por,xpp);
%do perspective transformation to validate homography
pnew=HH*por./[HH(3,:)*por;HH(3,:)*por;HH(3,:)*por];
That should do the trick. Note that Matlab defines the x axis in an image ans the rows index and y as the columns. Thus one must swap x-y in the equations (as you'll probably see in the code above). Furthermore, i had managed to compute the homography matrix from the parameters solely, but the result was slightly off (maybe roundoff errors in the calibration toolbox). The best way to do this is the above.
If you want to use just the camera parameters (that is, don't use Kovesi's code), then the Homography matrix is H=KK*Rmat*inv_KK. In this case the code is,
% corner coords in pixels
p1=[33;150;1];
p2=[316;136;1];
p3=[274;22;1];
p4=[63;34;1];
pmat=[p1 p2 p3 p4];
pmat=[0 1 0;1 0 0;0 0 1]*pmat; %swap x-y
R=[0 1 0;1 0 0;0 0 1]; %rotation matrix of final camera pose
Rmat=Rc_ext'*R; %rotation from original pose to final pose
H=KK*Rmat*inv_KK; %homography matrix
pnew=H*pmat./[H(3,:)*pmat;H(3,:)*pmat;H(3,:)*pmat]; %do perspective transformation
H2=[0 1 0;-1 0 0;0 0 1]*H; %swap x-y in the homography matrix to apply in image
Approach 1:
In the Camera Calibration Toolbox you should notice that there is an H matrix for each image of your checkerboard in your workspace. I am not familiar with the computer vision toolbox yet but perhaps this is the matrix you need for your function. It seems that H is computed like so:
KK = [fc(1) fc(1)*alpha_c cc(1);0 fc(2) cc(2); 0 0 1];
H = KK * [R(:,1) R(:,2) Tc]; % where R is your extrinsic rotation matrix and Tc the translation matrix
H = H / H(3,3);
Approach 2:
If the computer vision toolbox function doesn't work out for you then to find the prospective projection of an image I have used the interp2 function like so:
[X, Y] = meshgrid(0:size(I,2)-1, 0:size(I,1)-1);
im_coord = [X(:), Y(:), ones(prod(size(I_1)))]';
% Insert projection here for X and Y to XI and YI
ZI = interp2(X,Y,Z,XI,YI);
I have used prospective projections on a project a while ago and I believe that you need to use homogeneous coordinates. I think I found this wikipedia article quite helpful.
In OpenCV I am using feature matching techniques to find matching objects in other images. When I find a matching object I calculate the perspective transformation using the "findHomography" method.
FindHomography
This works fine and I can transform images based on this matrix. I have a video which alpha blends between the original and transformed images but now I want to animate the transition between the original and transformed position instead of just alpha blending between the two.
I have the 3x3 Homography matrix which gives me the full transformation but how would I interpolate between no transformation and this? If the 3x3 matrix had single values then I would interpolate between 0 and the Matrix value for however many time steps. However each element of the 3x3 matrix is made up of 3 values, I'm guessing because they are homogenous coordinates.
Could anyone advise the best way to approach this issue.
EDIT
Trying the method suggested by AldurDisciple I am creating the identity matrix with:
Mat eye = Mat::eye(3,3,CV_32F);
And performing the suggested calculation with:
Mat newH = (1-calc) * eye + calc * H;
where calc = k/N for the step/total number of steps.
I get an assertion failed error trying to calculate newH with the error being:
src1.type() == src2.type() in function scaleAdd
One simple way to approach this is to use linear interpolation between the identity matrix (i.e. no transformation) and the homography matrix you estimated with findHomograhy.
If H is the estimated homography and N is the number of time steps you want to use, then the transformation to apply at step k in [0,N] is as follows: Hk = (1-ak) * Id + ak * H, with ak = k/N.
I have used this code as a basis to detect my rectangular target in a scene.I use ORB and Flann Matcher.I have been able to draw the bounding box of the detected target in my scene successfully using the findHomography() and perspectiveTransform() functions.
The reference image (img_object in the above code) is a straight view of only the rectangular target.Now the target in my scene image may be tilted forwards or backwards.I want to find out the angle by which it has been tilted.I have read various posts and came to the conclusion that the homography returned by findHomography() can be decomposed to the rotation matrix and translation vector. I have used code from https:/gist.github.com/inspirit/740979 recommended by this link translated to C++.This is the Zhang SVD decomposition code got from the camera calibration module of OpenCV.I got the complete explanation of this decomposition code from O'Reilly's Learning OpenCV book.
I also used solvePnP() on the the keypoints returned by the matcher to cross check the rotation matrix and the translation vector returned from the homography decomposition but they do not seem to the same.
I have already the measurements of the tilts of all my scene images.i found 2 ways to retrieve the angles from the rotation matrix to check how well they match my values.
Given a 3×3 rotation matrix
R = [ r_{11} & r_{12} & r_{13} ]
[ r_{21} & r_{22} & r_{23} ]
[ r_{31} & r_{32} & r_{33} ]
The 3 Euler angles are
theta_{x} = atan2(r_{32}, r_{33})
theta_{y} = atan2(-r_{31}, sqrt{r_{32}^2 + r_{33}^2})
theta_{z} = atan2(r_{21}, r_{11})
The axis,angle representation - Being R a general rotation matrix, its corresponding rotation axis u
and rotation angle θ can be retrieved from:
cos(θ) = ( trace(R) − 1) / 2
[u]× = (R − R⊤) / 2 sin(θ)
I calculated the angles using both the methods for the rotation matrices obtained from the homography decomposition and the solvepnp().All the angles are different and give very unexpected values.
Is there a hole in my understanding?I do not understand where my calculations are wrong.Are there any alternatives i can use?
Why do you expect them to be the same? They are not the same thing at all.
The Euler angles are three angles of rotation about one axis at a time, starting from the world frame.
Rodriguez's formula gives components of one vector in the world frame, and an angle of rotation about that vector.