Pose extraction from multiple calibrated views [closed]

edit

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 :)