Add solvePnPRefineLM and solvePnPRefineVVS #14431

catree · 2019-04-26T17:55:25Z

This is a draft to see how adding a separate function to refine a pose after solvePnP would look:

solvePnPRefineLM the classical Levenberg-Marquardt iterative minimization process
solvePnPRefineVVS minimize the projection error using a virtual visual servoing scheme

LMSolver should be compared to the C implementation code for the Levenberg-Marquardt:

opencv/modules/calib3d/src/calibration.cpp

Lines 1221 to 1248 in 7b8ce63

    
           // refine extrinsic parameters using iterative algorithm 
        
           CvLevMarq solver( 6, count*2, cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,max_iter,FLT_EPSILON), true); 
        
           cvCopy( &_param, solver.param ); 
        
           for(;;) 
        
           { 
        
               CvMat *matJ = 0, *_err = 0; 
        
               const CvMat *__param = 0; 
        
               bool proceed = solver.update( __param, matJ, _err ); 
        
               cvCopy( __param, &_param ); 
        
               if( !proceed || !_err ) 
        
                   break; 
        
               cvReshape( _err, _err, 2, 1 ); 
        
               if( matJ ) 
        
               { 
        
                   cvGetCols( matJ, &_dpdr, 0, 3 ); 
        
                   cvGetCols( matJ, &_dpdt, 3, 6 ); 
        
                   cvProjectPoints2( matM, &_r, &_t, &matA, distCoeffs, 
        
                                     _err, &_dpdr, &_dpdt, 0, 0, 0 ); 
        
               } 
        
               else 
        
               { 
        
                   cvProjectPoints2( matM, &_r, &_t, &matA, distCoeffs, 
        
                                     _err, 0, 0, 0, 0, 0 ); 
        
               } 
        
               cvSub(_err, _m, _err); 
        
               cvReshape( _err, _err, 1, 2*count ); 
        
           }

Also, I have added the possibility to set the epsilon in the LM implementation.

alalek

Thank you for the contribution!

SolvePnP() with SOLVEPNP_ITERATIVE should use Levenberg-Marquardt too.

alalek · 2019-04-27T19:37:49Z

modules/calib3d/src/solvepnp.cpp

+    Mat opoints, ipoints;
+    opoints_.convertTo(opoints, CV_64F);
+    ipoints_.convertTo(ipoints, CV_64F);
+    int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));


After opoints_.convertTo(opoints, CV_64F); this check should always fail: opoints.checkVector(3, CV_32F)

alalek · 2019-04-27T19:39:12Z

modules/calib3d/src/solvepnp.cpp

+    double theta = sqrt(wx*wx + wy*wy + wz*wz);
+    double sinc = abs(theta) < 1e-8 ? 1 : sin(theta) / theta;
+    double mcosc = (abs(theta) < 1e-8) ? 0.5 : (1-cos(theta)) / (theta*theta);
+    double msinc = (abs(theta) < 1e-8) ? (1/6.0) : (1-sinc) / (theta*theta);


Consider using of std::fabs() instead of abs()

alalek · 2019-04-27T19:39:55Z

modules/calib3d/src/solvepnp.cpp

+    double wy = twist.at<double>(4,0);
+    double wz = twist.at<double>(5,0);
+
+    Mat rvec = (Mat_<double>(3,1) << wx, wy, wz);


Matx31d rvec(wx, wy, wz);

catree · 2019-04-29T18:22:32Z

@alalek
Yes, there are two Levenberg-Marquardt implementations:

one from the C-API and SOLVEPNP_ITERATIVE uses this one:

opencv/modules/calib3d/src/compat_ptsetreg.cpp

Line 53 in faca45a

////////////////// Levenberg-Marquardt engine (the old variant) ////////////////////////
another public one that I have used for solvePnPRefineLM:

opencv/modules/calib3d/src/levmarq.cpp

Line 80 in faca45a

class LMSolverImpl CV_FINAL : public LMSolver

In the future, it would be good to drop the C-version and use LMSolver instead if:

they provide the same results
it is a one-to-one replacement (same features available) or implement the missing features

Related #5206

…rdt iterative minimization process. Add solvePnPRefineVVS to refine a pose using a virtual visual servoing scheme.

catree · 2019-05-02T18:21:33Z

For me solvePnPRefineLM and solvePnPRefineVVS are fine but I am wondering if solve prefix could be misleading (these functions need an initial solution and are not a combination of solvePnP followed by a refinement method) for some users?
Otherwise refinePnPLM / refinePnPWithLM and refinePnPVVS / refinePnPWithVVS maybe?
With the class approach counterpart, PnPSolver and PnPRefiner are non-ambiguous and clear for me.

alalek

Awesome! Thank you 👍

tobycollins · 2019-05-03T19:39:47Z

I think the class approach is the good way to go. PnPSolver and PnPRefiner are clear names in my opinion.

catree mentioned this pull request Apr 26, 2019

calib3d: bindings-friendly solvePnPExtrinsicGuess() #14181

Closed

alalek reviewed Apr 27, 2019

View reviewed changes

catree force-pushed the feat_solvePnPRefine branch from e8c2160 to b66aaa3 Compare April 29, 2019 18:10

Add solvePnPRefineLM to refine a pose according to a Levenberg-Marqua…

dac31e8

…rdt iterative minimization process. Add solvePnPRefineVVS to refine a pose using a virtual visual servoing scheme.

catree force-pushed the feat_solvePnPRefine branch from b66aaa3 to dac31e8 Compare April 30, 2019 12:33

catree marked this pull request as ready for review April 30, 2019 12:34

alalek approved these changes May 2, 2019

View reviewed changes

opencv-pushbot merged commit dac31e8 into opencv:3.4 May 2, 2019

opencv-pushbot pushed a commit that referenced this pull request May 2, 2019

Merge pull request #14431 from catree:feat_solvePnPRefine

fc57129

alalek mentioned this pull request May 7, 2019

Merge 3.4 #14503

Merged

tobycollins mentioned this pull request Oct 5, 2019

PnP issue 15647 #15650

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

Uh oh!

Add solvePnPRefineLM and solvePnPRefineVVS #14431

Add solvePnPRefineLM and solvePnPRefineVVS #14431

Uh oh!

catree commented Apr 26, 2019 •

edited

Loading

Uh oh!

alalek left a comment

Uh oh!

alalek Apr 27, 2019

Uh oh!

alalek Apr 27, 2019

Uh oh!

alalek Apr 27, 2019

Uh oh!

catree commented Apr 29, 2019

Uh oh!

catree commented May 2, 2019

Uh oh!

alalek left a comment

Uh oh!

tobycollins commented May 3, 2019

Uh oh!

Uh oh!

	// refine extrinsic parameters using iterative algorithm
	CvLevMarq solver( 6, count*2, cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,max_iter,FLT_EPSILON), true);
	cvCopy( &_param, solver.param );

	for(;;)
	{
	CvMat matJ = 0, _err = 0;
	const CvMat *__param = 0;
	bool proceed = solver.update( __param, matJ, _err );
	cvCopy( __param, &_param );
	if( !proceed \|\| !_err )
	break;
	cvReshape( _err, _err, 2, 1 );
	if( matJ )
	{
	cvGetCols( matJ, &_dpdr, 0, 3 );
	cvGetCols( matJ, &_dpdt, 3, 6 );
	cvProjectPoints2( matM, &_r, &_t, &matA, distCoeffs,
	_err, &_dpdr, &_dpdt, 0, 0, 0 );
	}
	else
	{
	cvProjectPoints2( matM, &_r, &_t, &matA, distCoeffs,
	_err, 0, 0, 0, 0, 0 );
	}
	cvSub(_err, _m, _err);
	cvReshape( _err, _err, 1, 2*count );
	}

Uh oh!

Add solvePnPRefineLM and solvePnPRefineVVS #14431

Add solvePnPRefineLM and solvePnPRefineVVS #14431

Uh oh!

Conversation

catree commented Apr 26, 2019 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

alalek left a comment

Choose a reason for hiding this comment

Uh oh!

alalek Apr 27, 2019

Choose a reason for hiding this comment

Uh oh!

alalek Apr 27, 2019

Choose a reason for hiding this comment

Uh oh!

alalek Apr 27, 2019

Choose a reason for hiding this comment

Uh oh!

catree commented Apr 29, 2019

Uh oh!

catree commented May 2, 2019

Uh oh!

alalek left a comment

Choose a reason for hiding this comment

Uh oh!

tobycollins commented May 3, 2019

Uh oh!

Uh oh!

catree commented Apr 26, 2019 •

edited

Loading