Multi-view solvePnP routine

edit

Hi all!

I need a multi-view version of the solvePnP function.

Qualitively: We want to resolve the pose (rotation + translation) of an object in space using projections of landmarks on that object onto multiple image planes. Each image plane represents a calibrated camera at fixed locations in the world (for each we have a priori : cameraMatrix, distortionCoefficients, rotation, translation). The object is covered in markers (e.g. 10-20 markers, perhaps 4-8 will be in the camera's view at any time) which can be seen and identified in the cameras and are at known 3D positions in object space and known corresponding 2D positions in each image plane. Using the correspondences of 3D points in object space to 2D points in projected image space for each camera, we must reliably (and quickly) discover the rotation and translation for the object.

Quantitively:

• Inputs
  • Set[ intrinsics, extrinsics ] views // size N
  • Set[ Set[ObjectPoints], Set[ImagePoints] ] // size N
• Outputs
  • rotation of object // 3-vector
  • translation of object // 3-vector

I have some notes at: https://paper.dropbox.com/doc/KC35-st...

And posed a freelancer posting at: https://www.upwork.com/jobs/~01b0f0c4...

Using a single camera this is possible using the solvePnP function (which optimises the rotation and translation so that the projections of object points match the observed points on the image plane)

Template for the function could be:

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

(these functions would all call the same code internally, but just have different ways of being used)

The object we're trying to track is a tree (with known mesh taken from photo-scan). The tree is covered in retroreflective markers. We are projecting onto the tree from a set of moving video projectors (attached to robot arm). I'm pretty confident I can figure out which marker is which before we get to the solvePnP stage. This is all part of a new artwork by our studio (please check http://kimchiandchips.com#lightbarrie... for an example of previous work).

Notes:

• Routine should take less than 3ms on Core i7 for 2 views with 10 object points each.
• Ideally don't use any libraries other than OpenCV (would be even be great to PR this into OpenCV)
• I think OpenCV's only numerical solver is CvLevMarq which is C only, but I'd like to use C++ style where possible (i.e. it seems probably might ...