KC35 - solvePnPMultiView pseudo code

Function signature

ideally the function comes in the following 3 forms:

double solvePnPMultiView(vector<vector<cv::Point3f>> objectPointsPerView

, vector<vector<cv::Point2f>> imagePointProjectionsPerView

, vector<cv::Mat> cameraMatrixPerView

, vector<cv::Mat> distortionCoefficientsPerView

, vector<cv::Mat> translationPerView

, vector<cv::Mat> rotationVectorPerView

, cv::Mat & objectRotationVector

, cv::Mat & objectTranslation

, bool useExtrinsicGuess);

//same function but with different data format

double solvePnPMultiView(vector<vector<cv::Point3f>> objectPointsPerView

, vector<vector<cv::Point2f>> undsitortedImagePointProjectionsPerView

, vector<cv::Mat> rectifiedProjectionMatrixPerView

, cv::Mat & objectRotationVector

, cv::Mat & objectTranslation

, bool useExtrinsicGuess);

//specific version for stereo (calls one of the functions above)

double solvePnPStereo(vector<cv::Point3f> objectPointsObservedInCamera1

, vector<cv::Point2f> projectedImagePointsObservedInCamera1

, vector<cv::Point3f> objectPointsObservedInCamera2

, vector<cv::Point2f> projectedImagePointsObservedInCamera2

, cv::Mat cameraMatrix1

, cv::Mat distortionCoefficientsCamera1

, cv::Mat cameraMatrix2

, cv::Mat distortionCoefficientsCamera2

, cv::Mat camera1ToCamera2RotationVector

, cv::Mat camera1ToCamera2Translation

, cv::Mat & objectRotationVector

, cv::Mat & objectTranslation

, bool useExtrinsicGuess);

Adapting prior art

looking at the function cvFindExtrinsicCameraParams2 for reference, we see:

// 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 );

}

cvCopy( solver.param, &_param );

There is more code in this function above and below the block which i’ve extracted here, but this section is the important part which actually performs the solve. It uses CvLevMarq (i.e. a C implementation of Levenberq Marquadt solver in OpenCV).

To help understand this function better, we can clean it up into pseudocode as:

// refine extrinsic parameters using iterative algorithm

CvLevMarq solver( 6 parameters);

while (solver.update(parameters, error, jacobian) hasn't reached a solution yet)

{

rotationObjectToCamera = parameters[0..2];

translationObjectToCamera = parameters[3..5];

cvProjectPoints2( objectPoints

, rotationObjectToCamera

, translationObjectToCamera

, cameraMatrix

, distortionCoefficients

, calculatedImagePoints, jacobian);

error = distance(imagePoints - calculatedImagePoints);

}

So essentially it:

Starts with a guess of rotation and translation

Uses that rotation and translation with cvProjectPoints2 to create a guess of where each object point should be projected in the image plane

Calculates the error of those guessed projections vs the real projections

Also calculates the jacobian (i.e. the differential of error w.r.t. each incoming parameter)

Feeds error and jacobian back into the solver to get a new guess

Go back to 1 and repeat until the error is acceptable or maximum number of iterations has been reached

We could try and expand this for 2 cameras when we already have a known rigid translation and rotation between camera1 and camera2:

// refine extrinsic parameters using iterative algorithm

CvLevMarq solver( 6 parameters);

//already known:

// cameraMatrix1, distortionCoefficients1

// cameraMatrix2, distortionCoefficients2

// transformCamera1ToCamera2

// vector<Point3f> objectPointsSeenInCamera1

// vector<Point3f> objectPointsSeenInCamera2

// vector<Point2f> projectedImagePointsObservedInCamera1

// vector<Point2f> projectedImagePointsObservedInCamera2

//transformCamera1ToCamera2 is already kown

while (solver.update(parameters, combinedError, combinedJacobian) hasn't reached a solution yet)

{

rotationObjectToCamera1 = parameters[0..2];

translationObjectToCamera1 = parameters[3..5];

error = [];

cvProjectPoints2( objectPointsInCamera1

, rotationObjectToCamera1

, translationObjectToCamera1

, cameraMatrix1

, distortionCoefficients1

, calculatedImagePoints1

, jacobian1);

error = vstack(error, projectedImagePointsObservedInCamera1 - calculatedImagePoints1);

composeRT(rotationObjectToCamera1

, translationObjectToCamera1

, transformcamera1ToCamera2

, rotationObjectToCamera2

, translationObjectToCamera2

, jacobianTransform);

cvProjectPoints2( objectPoints

, rotationObjectToCamera2

, translationObjectToCamera2

, cameraMatrix2

, distortionCoefficients2

, calculatedImagePoints2

, jacobian2);

error = vstack(error, distance(projectedImagePointsObservedInCamera2 - calculatedImagePoints2));

combinedJacobian = someFunction(jacobian1, jacobian2, jacobianTransform);

}

Stacking all the the errors is obviously quite easy

but combining the jacobians i think is more tricky, and I’m not really familiar with where to start here. Especially with how the jacobianTransform returned from composeRT needs to be applied to jacobian2 to get it into the right format for the CvLevMarq solver.

ORB2_Slam

This has a PnPSolve function, and the library is for stereo algorithms so this might be a good point to look.

Code at function:

PnPsolver::estimate_R_and_t(double R[3][3], double t[3])

KC35 - solvePnPMultiView pseudo code
Function signature
Adapting prior art
ORB2_Slam

​​Function signature

​​Adapting prior art

​​ORB2_Slam

Function signature

Adapting prior art

ORB2_Slam