Ask Your Question
3

solvePnP fails with perfect coordinates (and cvPOSIT passes)

asked 2016-02-12 19:56:50 -0500

pthomet gravatar image

updated 2016-02-15 02:43:29 -0500

Hi,

I have found a set of parameters for which solvePnP does not give correct results although :

  • the image points are perfect coordinates obtained with projectPoints applied to the objectPoints (no error at all in the image points)
  • the objects points consist of 4 non coplanar points
  • the parameters are quite innocent looking
  • cvPOSIT is perfectly able to reconstruct the pose, when solvePnp seems not to be able to do so

This problem occurs with opencv 2.4.1 and 3.1.0, under osx and windows

When I reproject the point using the rotation and translation provided by solvePnP, I have a total error of about 8 pixels (I have tried all flags : ITERATIVE, P3P and EPNP)

If instead I use the rotation and translation provided by cvPOSIT the reprojection error can be reduced to 0.00014 pixels !

Here is a short program that demonstrates the error : I simply instantiate a camera matrix, some object points and their corresponding image points obtained through projectPoints. Then I compare the reprojection and I get 8 pixels of error!

#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <iostream>

void TestSolvePnp()
{
  std::vector<cv::Point3f> objectPoints;

  objectPoints.push_back(cv::Point3f(0.f, 0.f, 0.f));
  objectPoints.push_back(cv::Point3f(62.3174629f, 15.7940502f, 0.819983721f));
  objectPoints.push_back(cv::Point3f(-62.1225319f, 15.7540569f, 0.819464564f));
  objectPoints.push_back(cv::Point3f(-0.372639507f, 16.4230633f,  -36.5060043f));

  cv::Mat_<double> cameraIntrinsicParams(cv::Size(3, 3));
  cameraIntrinsicParams = 0.;
  cameraIntrinsicParams(0, 0) = 3844.4400000000001f;
  cameraIntrinsicParams(1, 1) = 3841.0599999999999f;
  cameraIntrinsicParams(0, 2) = 640.f;
  cameraIntrinsicParams(1, 2) = 380.f;
  cameraIntrinsicParams(2, 2) = 1.f;

  cv::Mat_<double> distCoeffs(cv::Size(1, 4));
  distCoeffs = 0.f;


  cv::Mat_<double> rotation(cv::Size(3, 1));
  rotation(0,0) = 0.07015543380659847f;
  rotation(0,1) = 0.06922079477774973f;
  rotation(0,2) = -0.00254676088325f;

  cv::Mat_<double> translation(cv::Size(3, 1));
  translation(0,0) = -35.3236f;
  translation(0,1) = -48.1699f;
  translation(0,2) = 769.068f;

  std::vector<cv::Point2f> imagePoints;
  cv::projectPoints(objectPoints, rotation, translation, cameraIntrinsicParams, distCoeffs, imagePoints);


  cv::Mat_<double> rotation2(cv::Size(3, 1));
  cv::Mat_<double> translation2(cv::Size(3, 1));

  cv::solvePnP(objectPoints, imagePoints,                 
               cameraIntrinsicParams, distCoeffs,
               rotation2, translation2,
               //false,//useExtrinsicGuess

               //Choose solvepnp flag below
               //cv::SOLVEPNP_UPNP
               //cv::SOLVEPNP_DLS
               //cv::SOLVEPNP_EPNP
               //cv::SOLVEPNP_P3P
               cv::SOLVEPNP_ITERATIVE
  );

  std::vector<cv::Point2f> imagePoints_Reproj(3);
  cv::projectPoints(objectPoints, rotation2, translation2, cameraIntrinsicParams, distCoeffs, imagePoints_Reproj);

  float sum = 0.;
  for (size_t i = 0; i < imagePoints.size(); i++)
    sum += DistPoint(imagePoints_Reproj[i], imagePoints[i]);

  std::cout << "sum=" << sum << std::endl;
}

int main(int argc, char **argv)
{
  TestSolvePnp();
}

However, if I replace the call to solvePnp by a call to cvPOSIT, the reprojection error goes down to 0.00014 pixels !

Below is the implementation of a function named MySolvePnpPosit() that reproduces the behavior of solvePnP, but uses cvPOSIT internally => when I use it, the error goes down almost zero.

namespace
{
  void Posit_IdealCameraCoords(const std::vector<cv::Point3f> & objectPoints, const std::vector<cv::Point2f> & imagePoints,
             cv::Mat &outRotationEstimated, cv::Mat & outTranslationEstimated)
  {

    CvPoint2D32f * imagePoints_c = (CvPoint2D32f ...
(more)
edit retag flag offensive close merge delete

Comments

How did you create the rotation vector? It works if I set it to zero, so maybe there is something wrong. If you don't want to use distortion factors, just pass an empty matrix. And btw: you don;t have to implement DistPoint yourself, you can use cv::Norm(v1-v2); (in which units are you working?)

FooBar gravatar imageFooBar ( 2016-02-13 05:44:13 -0500 )edit

Hello FooBar, The pixel coordinates and object Points + rotation were obtained from a real image on which my project works. The rotation was obtained through cvPOSIT and confirmed with Matlab. Nothing special about this (rodrigues) rotation vector. Concerning the unit, the sizes are in millimeters. I tried to change it to meters (i.e divide the object size and translation by 1000), with no success. However if I divide the object size and translation by 10000, the error goes down to zero. Strange...

pthomet gravatar imagepthomet ( 2016-02-13 14:41:54 -0500 )edit

Which units did you use while calibrating the camera?

FooBar gravatar imageFooBar ( 2016-02-13 16:41:36 -0500 )edit

millimeters also, with a chessboard and opencv calibration functions. Are you surprised like me to see that cvPOSIT works here and solvePnp seems to fail ?

pthomet gravatar imagepthomet ( 2016-02-14 02:06:02 -0500 )edit

2 answers

Sort by ยป oldest newest most voted
3

answered 2016-02-14 09:42:21 -0500

Eduardo gravatar image

updated 2016-02-15 14:40:59 -0500

Edit:

I just realize that I have copied / pasted your code and the 7th argument of cv::solvePnP is commented. When I thought modify the solvePnP method, only the useExtrinsicGuess parameter was modified.

So, I have no issue with OpenCL and the results are the same regardless the value of cv::setUseOptimized(); on my computer.

With cv::SOLVEPNP_DLS, cv::SOLVEPNP_EPNP, cv::SOLVEPNP_UPNP, the results are:

cv::useOptimized()=1
sum=44.8698093316393
True rotation=[0.07015543431043625, 0.06922079622745514, -0.002546760952100158]
Est rotation=[0.02331595776374525, 0.0162874860835041, -0.002505115278821771]
True_euler=[4.02098607576823, 3.96792301903936, -0.00669075977245175]
Est_euler=[1.3348540578548, 0.93479208297899, -0.132652269598486]
True translation=[-35.32360076904297, -48.16989898681641, 769.0679931640625]
Est translation=[-37.49101751741978, -49.68669094111537, 809.825952217276]

The 3 methods return the same results because in the code it is in fact the EPnP method that is called.

There is a pull request to adress this issue: Reactivating SOLVEPNP_DLS and SOLVEPNP_UPNP.

With cv::SOLVEPNP_P3P, the results are:

cv::useOptimized()=1
sum=0.205146641068635
True rotation=[0.07015543431043625, 0.06922079622745514, -0.002546760952100158]
Est rotation=[0.07112899814187482, 0.06897453111242667, -0.002581821704825981]
True_euler=[4.02098607576823, 3.96792301903936, -0.00669075977245175]
Est_euler=[4.07675884095018, 3.95387644149153, -0.00726862624869644]
True translation=[-35.32360076904297, -48.16989898681641, 769.0679931640625]
Est translation=[-35.32396572124871, -48.16931733672763, 769.072988680349]

With cv::SOLVEPNP_ITERATIVE and useExtrinsicGuess==false, the results are:

cv::useOptimized()=1
sum=8.9429127761996
True rotation=[0.07015543431043625, 0.06922079622745514, -0.002546760952100158]
Est rotation=[-0.3014129665370845, -0.01262643221321573, 3.115058790339473]
True_euler=[4.02098607576823, 3.96792301903936, -0.00669075977245175]
Est_euler=[-0.536089671153311, 11.0500652280158, 179.259679528549]
True translation=[-35.32360076904297, -48.16989898681641, 769.0679931640625]
Est translation=[34.50784095206554, 48.15119764387534, -768.3810038581826]

With cv::SOLVEPNP_ITERATIVE, useExtrinsicGuess==true, rotation2 = 0.0; and translation2 = 0.0;, the results are (only the method cv::SOLVEPNP_ITERATIVE accepts an initial guess for the pose):

cv::useOptimized()=1
sum=0
True rotation=[0.07015543431043625, 0.06922079622745514, -0.002546760952100158]
Est rotation=[0.07015537539390476; 0.06922079919505154; -0.002546761447480771]
True_euler=[4.02098607576823, 3.96792301903936, -0.00669075977245175]
Est_euler=[4.02098269404469, 3.96792319109631, -0.00669089927595358]
True translation=[-35.32360076904297, -48.16989898681641, 769.0679931640625]
Est translation=[-35.32360147801332; -48.16989817918626; 769.0679944279347]

If in your code the parameter useExtrinsicGuess is also commented, maybe the unpredictable results on OSX is caused by the variables rotation2 and translation2 that were not explicitly initialized to zero, so when useExtrinsicGuess==true, these variables contain random values from memory ?


I tested your code with OpenCV 3.1. On my computer, I have two distinct problems:

  • cv::SOLVEPNP_ITERATIVE returns bad result, all PnP methods return bad result except cv::SOLVEPNP_P3P
  • all PnP methods return bad result but if I turn off OPENCL (cv::setUseOptimized(false);), only cv::SOLVEPNP_ITERATIVE returns bad result

For the first issue, I guess that cv::SOLVEPNP_ITERATIVE use an initial pose before iteratively minimizing the reprojection error.

In the code, we can see that cv::SOLVEPNP_ITERATIVE use cvFindExtrinsicCameraParams2 which use Direct Linear Transform (DLT) when the points are non coplanar to estimate an initial pose before iteratively refines the pose using the Levenberg-Marquardt method.

My guess is ...

(more)
edit flag offensive delete link more

Comments

1

Thanks Eduardo, You were right about the 7th argument that was commented out. It was causing the unstable results under osx. I now have the exact same results as you : with cv::SOLVEPNP_DLS, cv::SOLVEPNP_EPNP, cv::SOLVEPNP_UPNP the reprojection error is about 44, with SOLVE_ITERATIVE about 8.94 (and zero if you set useExtrinsicGuess to true with initial vector set to zero).

pthomet gravatar imagepthomet ( 2016-02-15 17:44:01 -0500 )edit
0

answered 2016-02-15 02:26:07 -0500

pthomet gravatar image

updated 2016-02-16 13:44:41 -0500

Note 1 : this answer was edited following Eduardo's edit of his own answer. The initial unstable results under OSX were due to an error in the code. Sorry about this.

Note 2 : I made a more thorough analysis, that compares several strategies (after having added the distorsion coeffs of the camera). The code would not fit here, but it can be found at https://github.com/pthom/TestSolvePnp

enum SolvePnpStrategy
{
  Strategy_MySolvePnp_Epnp, //an home baked adapter of epnp using the epnp library source code
  Strategy_MySolvePnpPosit, //an home baked adapter of cvPOSIT (deprecated "OpenCV 1" pose estimation method)
  Strategy_solvePnp_P3p,    // opencv SOLVEPNP_P3P method
  Strategy_solvePnp_Iterative_InitialGuess, // opencv SOLVEPNP_ITERATIVE method with an initial guess
  Strategy_solvePnp_Epnp //opencv SOLVEPNP_EPNP method
};

Based on my experimentations, the order of the 3d points and image points does matter. It has to be adapted depending upon the strategy !

  • With opencv's SOLVEPNP_EPNP the error can go down to 23.03 pixel. The order of the points does matter
  • With MySolvePnpEpnp (an home baked adapter of epnp using the epnp library source code), the error is about 6.742 pixels, and the order is important It is strange that this "rewrite" gives different results
  • With MySolvePnpPosit, the error is about 4.911 pixels and the order is important (in other cases the reprojection error is about 1278 pixels !)
  • With solvePnp_P3p (cv::SOLVEPNP_P3P) the error is about 0.02961 pixels and the order does not matter much
  • With solvePnp_Iterative_InitialGuess (cv::SOLVEPNP_ITERATIVE) the error can be 0 pixels if a good initial extrinsic guess is given (otherwise don't hope for any convergence). The order does not matter much with SOLVEPNP_ITERATIVE.

Original answer below


Many thanks Eduardo for your thorough analysis, and the time you invested for it :-)

This is not an answer, but a recap of my results following your suggestions.

Note : my tests were run under OSX El Capitan. I did not test linux or windows yet. I have the same results as you :

  • With cv::SOLVEPNP_DLS, cv::SOLVEPNP_EPNP, cv::SOLVEPNP_UPNP , the reprojection error is 44.8698 pixels
  • With cv::SOLVEPNP_ITERATIVE the reprojection error is 8.94 pixels (except if you set useExtrinsicGuess to true and use null vectors as inital rotation and translation}, and the returned pose is behind the camera
  • With cv::SOLVEPNP_P3P the reprojection error is about 0.21 pixels

I was much less sucessful than you : I saw unstable results when using the optimized version, and no success when disabling the optimizations.

Here is a recap of what I saw:

  • using opencv 2.4.11: cv::setUseOptimized(false) does not change the result
  • using opencv 3.1 : cv::setUseOptimized(false) leads to unstable results!

Here is a full recap of my results with opencv 3.1. I also tested the master branch, with the same results:

  • Using cv::SOLVEPNP_UPNP

    • optimized version : if you run the program several times, the result you get can be 4535.94, 711.716, 302630, nan or 0 !!!
    • unoptimized version : result is always 711.832
  • Using cv::SOLVEPNP_DLS

    • optimized version: result vary. I obtained ...
(more)
edit flag offensive delete link more

Comments

"We do have a bug, don't you think ?" --

https://github.com/itseez/opencv/issues

berak gravatar imageberak ( 2016-02-15 02:29:41 -0500 )edit
1

I just did post an issue : https://github.com/Itseez/opencv/issu...

pthomet gravatar imagepthomet ( 2016-02-15 02:49:54 -0500 )edit

double val_double_cast = 0.07015543380659847f;

double val_double = 0.07015543380659847;

val_double_cast=0.070155434310436249

val_double=0.070155433806598472

I observed a completly different behavior with and without the cast to float but that is not related to your issue, just that a small "noise" produces very different results for the DLT.

I tested on W7 x64, VS2010 ==> Iterative failed, but other methods are OK if cv::setUseOptimized(false); I tested on Fedora 20 ==> Iterative failed, but other methods are OK regardless the value of cv::setUseOptimized();.

Eduardo gravatar imageEduardo ( 2016-02-15 04:29:07 -0500 )edit

Thanks for posting the issue.

I just tested on Fedora 20 and I have correct result even with cv::setUseOptimized(true);.

I checked also with std::cout << "cv::useOptimized()=" << cv::useOptimized() << std::endl;.

Multiple runs produce similar results on my computer: TestSolvePnp(); TestSolvePnp(); TestSolvePnp();

In my opinion, the fact that cvPosit produces correct results whereas cv::SOLVEPNP_ITERATIVE not is not illogical since it uses the DLT method (which is inferior to the other methods) to get the initial pose.

Anyway, the issue is very strange as you cannot get correct results whereas I can and I have different behavior with 2 different configurations.

Maybe it could help if someone else performs the test ?

Eduardo gravatar imageEduardo ( 2016-02-15 04:52:07 -0500 )edit

I teste the program under linux, and it works ok. So it seems that the issue of having unpredictable results is limited to OSX.

pthomet gravatar imagepthomet ( 2016-02-15 12:04:22 -0500 )edit

Sorry about the confusion about unstable results : it was caused by an error in the code (see Eduardo's answer).

pthomet gravatar imagepthomet ( 2016-02-15 17:46:33 -0500 )edit
Login/Signup to Answer

Question Tools

2 followers

Stats

Asked: 2016-02-12 19:55:26 -0500

Seen: 4,285 times

Last updated: Feb 16 '16