# camera pose from solvePnPRansac using 3d-2d motion estimation in monocular visual odometry

In paper Visual Odometry Part I,it said that we can get camera pose using 3d-2d motion estimation for the monocular case(It needs three images).

So, I got Rvec and tvec from function solvePnPRansac using 3d-2d motion estimation.

The question is how can I get camera position and draw its trajectory from Rvec and tvec. The following two formulas are right?

1.Rcur=Rvec * Rcur

2.tcur=tcur+scale*(Rcur * tvec)

Beacuase I find Avi Singh use them in 2d-2d motion estimation.

Is there any paper about monocular visual odometry using 3d-2d motion estimation to recommend.

edit retag close merge delete

Sort by » oldest newest most voted To draw the trajectory, you need the position of the camera at each time. Using rvec and tvec, you can get this by the following. I'm assuming rvec is the 3x1 rotation angles and tvec is the 1x3 translation.

Rodrigues(rvec, R);
R = R.t();
t = (-R * tvec);


t now contains the camera coordinates relative to the origin of your coordinate system. Now, if your coordinate origin remains the same from frame to frame, you're done.

I haven't had a chance to read everything at the link, but I suspect the origin changes to center on the camera with each frame. If so, you need to add each t to a total you keep that is the total translation from the first frame.

EDIT: Ok, I did read the link, and it is the motion from frame to frame. I should also point out that the direction changes from frame to frame, so you need to chain rotation matrices together to get the direction too.

Is that all of what you were wanting to know?

more

Thank you very much for your detailed reply.You are right,it is the motion from frame to frame.I have a question,how to chain rotation matrices to get the direction? t = (-R * tvec)+R * t?

Rn = R1R2R3R4...Rnow-1 Or Rn = Rn-1*R_now, just like you can see HERE.

You'll want to use the final R for this. So do R = -(R.t()), then chain them together and use the result on your current t. This will put your camera position into your starting coordinate system.

Make sense, or did I miss something in my explanation?

so,the final formulas are R_f=Rvec*R_f and tvec=-R_f *tvec ? Then ,the camera position is tvec(x,y,z)?

You need to switch the order of the rotations, I think. R_f = R_f*Rmat. Rmat is the output of cv::Rodrigues(rvec,Rmat);

Keep in mind that the translation you get will be the translation of just the latest. So you will need to rotate it (as you do here) and then add it to your cumulative position to get the final position from start.

Official site

GitHub

Wiki

Documentation