Pose extraction from multiple calibrated views

Hi

I have a camera and it's camera matrix and distortion coefficients (I used the sample program). I took several overlapping pictures from the same scene and now I want to compute their relative position and rotation.

How can I do this?

My idea is to place the first image at the origin coordinates looking along the z axis and then relate the image i+1 with the image i:

for(int i=1; i<images.size(); ++i){
previous=images[i-1]
current=images[i]

// Find and Match feature points with SURF
// Points in the previous image
previous_image_points= ...
// Match points in the second image
current_image_points=...

// Find fundamental matrix
Mat F = findFundamentalMat(previous_image_points, current_image_points, FM_RANSAC, 0.1,0.99)
// Essential matrix. K => Camera matrix
Mat R = K.t()*F*K

// Find the position and rotation between cameras
SVD svd(E);
Matx33d W(0,-1,0,
1,0,0,
0,0,1);
Matx33d Winv(0,1,0,
-1,0,0,
0,0,1);

Mat_<double> R = svd.u * Mat(W) * svd.vt;
Mat_<double> t = svd.u.col(2);

// Current image position and rotation matrix
Matx34d rotPos = Matx34d(R(0,0), R(0,1), R(0,2), t(0),
R(1,0), R(1,1), R(1,2), t(1),
R(2,0), R(2,1), R(2,2), t(2));

// Previous image position and rotation matrix
// In the case of image 0, it is the identity matrix
Matx34d previous_rot_pos=....

// Calculate the final position and rotation matrix.
// I know that these are 3x4 matrices and can't be multiplied,
// but I convert them to homogeneous before :)
rotPos = previous_rot_pos*rotPos
}

This code concatenates position and rotation matrices such that, in the end, their positions are all relative to the first one.

Thank you :)

edit retag close merge delete