Stereo camera pose estimation from solvePnPRansac using 3D points given wrt. the camera coordinate system

edit

Thanks! Reading the articles helped me with further understanding of the concept! Especially regarding what 2D-3D points to use when solving the PnP.

So now I'm using previous frame 3D points in camera coordinates and current frame 2D points at each iteration in solvePnP. The results seem to be ok, however they contain lots of noise, and that makes it hard to tell if the global transform is correct.

If I construct a transformation matrix like this with each R and t (R gotten from Rodrigues and t=tvec): T_current = [R t; 0 1], and I have my T_global (which initially is a 4x4 identity matrix) is this the correct update formula: T_global = T_global * T_current? Does T_global now represent the transformation from the initial frame to the current frame? Is T_current = [R t; 0 1] correct?

Use cv::composeRT to concatenate your transformations. To reduce noise look into using keyframes where results are aggregated before moving on to the next keyframe. You'll need a concept of stereo error. I recommend reading "Fast, Unconstrained Camera Motion Estimation from Stereo without Tracking and Robust Statistics" by Heiko Hirschmuller.

Thanks again.

I've actually been doing the same inlier detection step as they do in the paper (been kind of following this approach, where the same inlier detection step is done in step 6: https://avisingh599.github.io/vision/...). However, I noticed that I've been doing it on the 2D feature points, and I understand that I need to detect inliers among the 3D points. As of now, I only have a constant number as the "distance error threshold" for determining if a point is an inlier or outlier, but I will look into the more "dynamical" approach described in the paper (eq. 10).

Just to be sure that I use composeRT correctly: rvec1/tvec1 equals the global transform, while rvec2/tvec2 is the relative one? Which gives me the transformation from initial to the current frame?

*I can compute what rvec3/tvec3 equals from the formulas given in the documentation.

BUT, there is one thing that I've never quite understood, and would appreciate if you could help me with my confusion (because this affects the results from composeRT):

The rvec and tvec returned by solvePnPRansac, what exactly do they represent? What I've believed:

• rvec/the rotation matrix formed by rvec represents the rotation from the previous coordinate frame to the current frame.

• tvec is the vector from the origin of the current frame to the origin of the previous frame, with coordinates given in the current frame/coordinate system. I. e. t^n_{n, n-1}, if that notation makes sense to you.

Is this a correct interpretation of rvec and tvec returned by solvePnPRansac?

You have the freedom of choice to put these things together however you choose. rvec and tvec from cv::solvePnPRansac bring the model coords to the camera frame, so you will likely end up inverting the rotation matrix and negating the translation. Using the current frame's 2D points and the previous frame's 3D points is necessary for keyframe 3D-data fusion, however, you could swap it around and still do frame-to-frame VO just fine. I hope that helps. Just print out your values (translation and euler angles) for a short sequence and verify it makes sense.

Ok, I opened another question about composeRT and it makes more sense to me now. However, I still wonder about the tvec returned from solvePnPRansac. Yes, rvec and tvec brings points from the model cords to the camera cord. But, if I keep the camera absolute stlll and just print out tvec, shouldn't tvec remain somewhat close to (0, 0, 0) when I use previous 3D points (wrt camera frame) and current corresponding 2D points? Because now it doensn't do that at all (some of it I recognize as noise, but not all of it).

If you are using cv::solvePnPRansac, then there is little reason to first do inlier detection (as opposed to outlier rejection) unless you believe that the ratio of inliers to outliers is small. You also might have a very noisy 3D image or one with large errors if the baseline is small. If the 3D positions are not stable then you might observe the strange results you mention. Also, keep in mind that cv::solvePnPRansac does no weighting of points. The 3D error increases quadratically as one moves away from the camera and so in reality these points really should be given a much reduced weighting, but OpenCV does not (yet) provide such a capability. Maybe there are some visualizations you can add to better understand what is going on? Try a 3D rigid body transformation of current points?

I will look into the accuracy of the 3D points, maybe try to visualize them in some way. I've also asked the developers of the stereo camera about the accuracy of the 3D points, since I get them from a function in an API/SDK provided by them. The baseline of the camera is only 30 mm with a stated operating range of 2.5 meters, so I try to keep the scene close to the lenses (<0.5 m).

But, just to make sure and to have some sort of goal: The tvec output from solvePnPRansac should stay close to (0, 0, 0) if I don't move the camera?

Side note: I just realized that I've been using your code provided in this answer for the implementation of a max clique approximation! http://answers.opencv.org/question/75...

My understanding is that if you have a static scene and camera, and you are accumulating transformations frame after frame, you will observe a random walk away from 0. Are you using the Duo3D? Since I posted that answer I've improved the code to take into account 3D-error. For frame-to-frame VO, my experience is that the inlier/max-clique with 3D-error method, though a little slower, provides more accurate results than RANSAC PnP, though it has more regular catastrophic errors (probably my fault somewhere). Hirschmuller also suggests an angular rejection test, which should be simple enough to add. It would be no mean feat to do keyframe VO and weighted PnP, either via RANSAC or inlier detection. You might also want to consider cv::matchGMS as a 2D-outlier rejection scheme.

Yes, I'm using the Duo3D M. I agree that by accumulating transformations frame after frame I need to expect some drift. But what I meant was that if I just print out the tvec returned from solvePnPRansac for each iteration, i. e. the local translation between two consequtive frames, then shouldn't it stay approximately at (0, 0, 0) if the points are accurate enough? Have you uploaded your updated max-clique script anywhere? And what exactly is the "inlier/max-clique with 3D-error method"? Does it include pose estimation (solving the PnP-problem), as an alternative to cv::solvePnP(Ransac)?