Search code examples
c++opencvslamodometry

SolvePNP return bad rvec and tvec after some time

I am trying to estimate camera trajectory using stereo camera pair from KITTI dataset. The program uses cv::SolvePNP() at some point and for first 1500 frames it returns quite good results, but after that it goes completely wild.

Here's what I'm currently doing:

  1. load 2 frames, will call them P and Q (one frame is left and right picture from stereo pair)
  2. get features using SIFT/SURF/ORB (tried all of those)
  3. match the features in P, and match between P.left and Q.left
  4. based on matches I filter out the keypoints that aren't present in the matches.
  5. I triangulate the points on the frame P (note that I already have camera matrix provided by KITTI dataset)
  6. I convert the points from Homogeneous

Now comes the spicy part, I'm trying to implement my own RANSAC that will utilise solvePNP in order to get better rvec and tvec from the standard cv::solvePNP (note that I tried using cv::solvePNPRansac and it worked okay but started failing after some time as well). so the Ransac process is like this:

7.1 I randomize the 3D space (triangulated points on P) and corresponding keypoints on Q.left

7.2 I put them N points (N is my RANSAC param) into cv::SolvePNP() in order to get how much camera moved between the 2 frames (note that my dist Coefs are zero matrix) and it return me some tvec and rvec that I transform into Matrix using cv::Rodrigues()

7.3 I concatenate those vectors

7.4 I try to project all the triangulated points (not just the ones used in solvePNP) back to 2D image using this formula:

https://i.sstatic.net/xOM5Z.png

(note that I don't get 1 at the left side but instead I get some x,y,z values so I devide x and y with z).

7.5 I find the difference between projected points and keypoints on P.left

7.6 now my RANSAC has some threshold value that I use to filter Outliers and not use them in next iteration of RANSAC.

7.7 I repeat the process with the new (filtered) points some K times (K is also RANSAC param)

Now this works for 80% frames, after some time it just fails because solvePNP recives too few points. This happens because none of the projected points actually passed the distance threshold. So the problem is in rvec and tvec from the frame before.

Really don't know what part of code I shall show, request and I will write it.

I expected minor error in rvec and tvec but error is too high.

Solution

  • Apparently it was due to bunch of wrongly matched feature that highly effected later triangulation process. Problem can be fixed with implementing RANSAC or making the match threshold higher.