modules/calib3d/src/solvepnp.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2  //
   3  //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4  //
   5  //  By downloading, copying, installing or using the software you agree to this license.
   6  //  If you do not agree to this license, do not download, install,
   7  //  copy or use the software.
   8  //
   9  //
  10  //                           License Agreement
  11  //                For Open Source Computer Vision Library
  12  //
  13  // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
  14  // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
  15  // Third party copyrights are property of their respective owners.
  16  //
  17  // Redistribution and use in source and binary forms, with or without modification,
  18  // are permitted provided that the following conditions are met:
  19  //
  20  //   * Redistribution's of source code must retain the above copyright notice,
  21  //     this list of conditions and the following disclaimer.
  22  //
  23  //   * Redistribution's in binary form must reproduce the above copyright notice,
  24  //     this list of conditions and the following disclaimer in the documentation
  25  //     and/or other materials provided with the distribution.
  26  //
  27  //   * The name of the copyright holders may not be used to endorse or promote products
  28  //     derived from this software without specific prior written permission.
  29  //
  30  // This software is provided by the copyright holders and contributors "as is" and
  31  // any express or implied warranties, including, but not limited to, the implied
  32  // warranties of merchantability and fitness for a particular purpose are disclaimed.
  33  // In no event shall the Intel Corporation or contributors be liable for any direct,
  34  // indirect, incidental, special, exemplary, or consequential damages
  35  // (including, but not limited to, procurement of substitute goods or services;
  36  // loss of use, data, or profits; or business interruption) however caused
  37  // and on any theory of liability, whether in contract, strict liability,
  38  // or tort (including negligence or otherwise) arising in any way out of
  39  // the use of this software, even if advised of the possibility of such damage.
  40  //
  41  //M*/
  42
  43 #include "precomp.hpp"
  44 #include "epnp.h"
  45 #include "p3p.h"
  46 #include <iostream>
  47 using namespace cv;
  48
  49 bool cv::solvePnP( InputArray _opoints, InputArray _ipoints,
  50                   InputArray _cameraMatrix, InputArray _distCoeffs,
  51                   OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int flags )
  52 {
  53     Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
  54     int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
  55     CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
  56     _rvec.create(3, 1, CV_64F);
  57     _tvec.create(3, 1, CV_64F);
  58     Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
  59
  60     if (flags == CV_EPNP)
  61     {
  62         cv::Mat undistortedPoints;
  63         cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
  64         epnp PnP(cameraMatrix, opoints, undistortedPoints);
  65
  66         cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
  67         PnP.compute_pose(R, tvec);
  68         cv::Rodrigues(R, rvec);
  69         return true;
  70     }
  71     else if (flags == CV_P3P)
  72     {
  73         CV_Assert( npoints == 4);
  74         cv::Mat undistortedPoints;
  75         cv::undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
  76         p3p P3Psolver(cameraMatrix);
  77
  78         cv::Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
  79         bool result = P3Psolver.solve(R, tvec, opoints, undistortedPoints);
  80         if (result)
  81             cv::Rodrigues(R, rvec);
  82         return result;
  83     }
  84     else if (flags == CV_ITERATIVE)
  85     {
  86         CvMat c_objectPoints = opoints, c_imagePoints = ipoints;
  87         CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs;
  88         CvMat c_rvec = _rvec.getMat(), c_tvec = _tvec.getMat();
  89         cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix,
  90                                      c_distCoeffs.rows*c_distCoeffs.cols ? &c_distCoeffs : 0,
  91                                      &c_rvec, &c_tvec, useExtrinsicGuess );
  92         return true;
  93     }
  94     else
  95         CV_Error(CV_StsBadArg, "The flags argument must be one of CV_ITERATIVE or CV_EPNP");
  96     return false;
  97 }
  98
  99 namespace cv
 100 {
 101     namespace pnpransac
 102     {
 103         const int MIN_POINTS_COUNT = 4;
 104
 105         static void project3dPoints(const Mat& points, const Mat& rvec, const Mat& tvec, Mat& modif_points)
 106         {
 107             modif_points.create(1, points.cols, CV_32FC3);
 108             Mat R(3, 3, CV_64FC1);
 109             Rodrigues(rvec, R);
 110             Mat transformation(3, 4, CV_64F);
 111             Mat r = transformation.colRange(0, 3);
 112             R.copyTo(r);
 113             Mat t = transformation.colRange(3, 4);
 114             tvec.copyTo(t);
 115             transform(points, modif_points, transformation);
 116         }
 117
 118         class Mutex
 119         {
 120         public:
 121             Mutex() {
 122             }
 123             void lock()
 124             {
 125 #ifdef HAVE_TBB
 126                 resultsMutex.lock();
 127 #endif
 128             }
 129
 130             void unlock()
 131             {
 132 #ifdef HAVE_TBB
 133                 resultsMutex.unlock();
 134 #endif
 135             }
 136
 137         private:
 138 #ifdef HAVE_TBB
 139             tbb::mutex resultsMutex;
 140 #endif
 141         };
 142
 143         struct CameraParameters
 144         {
 145             void init(Mat _intrinsics, Mat _distCoeffs)
 146             {
 147                 _intrinsics.copyTo(intrinsics);
 148                 _distCoeffs.copyTo(distortion);
 149             }
 150
 151             Mat intrinsics;
 152             Mat distortion;
 153         };
 154
 155         struct Parameters
 156         {
 157             int iterationsCount;
 158             float reprojectionError;
 159             int minInliersCount;
 160             bool useExtrinsicGuess;
 161             int flags;
 162             CameraParameters camera;
 163         };
 164
 165         static void pnpTask(const vector<char>& pointsMask, const Mat& objectPoints, const Mat& imagePoints,
 166                      const Parameters& params, vector<int>& inliers, Mat& rvec, Mat& tvec,
 167                      const Mat& rvecInit, const Mat& tvecInit, Mutex& resultsMutex)
 168         {
 169             Mat modelObjectPoints(1, MIN_POINTS_COUNT, CV_32FC3), modelImagePoints(1, MIN_POINTS_COUNT, CV_32FC2);
 170             for (int i = 0, colIndex = 0; i < (int)pointsMask.size(); i++)
 171             {
 172                 if (pointsMask[i])
 173                 {
 174                     Mat colModelImagePoints = modelImagePoints(Rect(colIndex, 0, 1, 1));
 175                     imagePoints.col(i).copyTo(colModelImagePoints);
 176                     Mat colModelObjectPoints = modelObjectPoints(Rect(colIndex, 0, 1, 1));
 177                     objectPoints.col(i).copyTo(colModelObjectPoints);
 178                     colIndex = colIndex+1;
 179                 }
 180             }
 181
 182             //filter same 3d points, hang in solvePnP
 183             double eps = 1e-10;
 184             int num_same_points = 0;
 185             for (int i = 0; i < MIN_POINTS_COUNT; i++)
 186                 for (int j = i + 1; j < MIN_POINTS_COUNT; j++)
 187                 {
 188                     if (norm(modelObjectPoints.at<Vec3f>(0, i) - modelObjectPoints.at<Vec3f>(0, j)) < eps)
 189                         num_same_points++;
 190                 }
 191             if (num_same_points > 0)
 192                 return;
 193
 194             Mat localRvec, localTvec;
 195             rvecInit.copyTo(localRvec);
 196             tvecInit.copyTo(localTvec);
 197
 198             solvePnP(modelObjectPoints, modelImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec,
 199                      params.useExtrinsicGuess, params.flags);
 200
 201
 202             vector<Point2f> projected_points;
 203             projected_points.resize(objectPoints.cols);
 204             projectPoints(objectPoints, localRvec, localTvec, params.camera.intrinsics, params.camera.distortion, projected_points);
 205
 206             Mat rotatedPoints;
 207             project3dPoints(objectPoints, localRvec, localTvec, rotatedPoints);
 208
 209             vector<int> localInliers;
 210             for (int i = 0; i < objectPoints.cols; i++)
 211             {
 212                 Point2f p(imagePoints.at<Vec2f>(0, i)[0], imagePoints.at<Vec2f>(0, i)[1]);
 213                 if ((norm(p - projected_points[i]) < params.reprojectionError)
 214                     && (rotatedPoints.at<Vec3f>(0, i)[2] > 0)) //hack
 215                 {
 216                     localInliers.push_back(i);
 217                 }
 218             }
 219
 220             if (localInliers.size() > inliers.size())
 221             {
 222                 resultsMutex.lock();
 223
 224                 inliers.clear();
 225                 inliers.resize(localInliers.size());
 226                 memcpy(&inliers[0], &localInliers[0], sizeof(int) * localInliers.size());
 227                 localRvec.copyTo(rvec);
 228                 localTvec.copyTo(tvec);
 229
 230                 resultsMutex.unlock();
 231             }
 232         }
 233
 234         class PnPSolver
 235         {
 236         public:
 237             void operator()( const BlockedRange& r ) const
 238             {
 239                 vector<char> pointsMask(objectPoints.cols, 0);
 240                 memset(&pointsMask[0], 1, MIN_POINTS_COUNT );
 241                 for( int i=r.begin(); i!=r.end(); ++i )
 242                 {
 243                     generateVar(pointsMask);
 244                     pnpTask(pointsMask, objectPoints, imagePoints, parameters,
 245                             inliers, rvec, tvec, initRvec, initTvec, syncMutex);
 246                     if ((int)inliers.size() >= parameters.minInliersCount)
 247                     {
 248 #ifdef HAVE_TBB
 249                         tbb::task::self().cancel_group_execution();
 250 #else
 251                         break;
 252 #endif
 253                     }
 254                 }
 255             }
 256             PnPSolver(const Mat& _objectPoints, const Mat& _imagePoints, const Parameters& _parameters,
 257                       Mat& _rvec, Mat& _tvec, vector<int>& _inliers):
 258             objectPoints(_objectPoints), imagePoints(_imagePoints), parameters(_parameters),
 259             rvec(_rvec), tvec(_tvec), inliers(_inliers)
 260             {
 261                 rvec.copyTo(initRvec);
 262                 tvec.copyTo(initTvec);
 263             }
 264         private:
 265             PnPSolver& operator=(const PnPSolver&);
 266
 267             const Mat& objectPoints;
 268             const Mat& imagePoints;
 269             const Parameters& parameters;
 270             Mat &rvec, &tvec;
 271             vector<int>& inliers;
 272             Mat initRvec, initTvec;
 273
 274             static RNG generator;
 275             static Mutex syncMutex;
 276
 277             void generateVar(vector<char>& mask) const
 278             {
 279                 int size = (int)mask.size();
 280                 for (int i = 0; i < size; i++)
 281                 {
 282                     int i1 = generator.uniform(0, size);
 283                     int i2 = generator.uniform(0, size);
 284                     char curr = mask[i1];
 285                     mask[i1] = mask[i2];
 286                     mask[i2] = curr;
 287                 }
 288             }
 289         };
 290
 291         Mutex PnPSolver::syncMutex;
 292         RNG PnPSolver::generator;
 293
 294     }
 295 }
 296
 297 void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints,
 298                         InputArray _cameraMatrix, InputArray _distCoeffs,
 299                         OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess,
 300                         int iterationsCount, float reprojectionError, int minInliersCount,
 301                         OutputArray _inliers, int flags)
 302 {
 303     Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
 304     Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
 305
 306     CV_Assert(opoints.isContinuous());
 307     CV_Assert(opoints.depth() == CV_32F);
 308     CV_Assert((opoints.rows == 1 && opoints.channels() == 3) || opoints.cols*opoints.channels() == 3);
 309     CV_Assert(ipoints.isContinuous());
 310     CV_Assert(ipoints.depth() == CV_32F);
 311     CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2);
 312
 313     _rvec.create(3, 1, CV_64FC1);
 314     _tvec.create(3, 1, CV_64FC1);
 315     Mat rvec = _rvec.getMat();
 316     Mat tvec = _tvec.getMat();
 317
 318     Mat objectPoints = opoints.reshape(3, 1), imagePoints = ipoints.reshape(2, 1);
 319
 320     if (minInliersCount <= 0)
 321         minInliersCount = objectPoints.cols;
 322     cv::pnpransac::Parameters params;
 323     params.iterationsCount = iterationsCount;
 324     params.minInliersCount = minInliersCount;
 325     params.reprojectionError = reprojectionError;
 326     params.useExtrinsicGuess = useExtrinsicGuess;
 327     params.camera.init(cameraMatrix, distCoeffs);
 328     params.flags = flags;
 329
 330     vector<int> localInliers;
 331     Mat localRvec, localTvec;
 332     rvec.copyTo(localRvec);
 333     tvec.copyTo(localTvec);
 334
 335     if (objectPoints.cols >= pnpransac::MIN_POINTS_COUNT)
 336     {
 337         parallel_for(BlockedRange(0,iterationsCount), cv::pnpransac::PnPSolver(objectPoints, imagePoints, params,
 338                                                                                localRvec, localTvec, localInliers));
 339     }
 340
 341     if (localInliers.size() >= (size_t)pnpransac::MIN_POINTS_COUNT)
 342     {
 343         if (flags != CV_P3P)
 344         {
 345             int i, pointsCount = (int)localInliers.size();
 346             Mat inlierObjectPoints(1, pointsCount, CV_32FC3), inlierImagePoints(1, pointsCount, CV_32FC2);
 347             for (i = 0; i < pointsCount; i++)
 348             {
 349                 int index = localInliers[i];
 350                 Mat colInlierImagePoints = inlierImagePoints(Rect(i, 0, 1, 1));
 351                 imagePoints.col(index).copyTo(colInlierImagePoints);
 352                 Mat colInlierObjectPoints = inlierObjectPoints(Rect(i, 0, 1, 1));
 353                 objectPoints.col(index).copyTo(colInlierObjectPoints);
 354             }
 355             solvePnP(inlierObjectPoints, inlierImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec, true, flags);
 356         }
 357         localRvec.copyTo(rvec);
 358         localTvec.copyTo(tvec);
 359         if (_inliers.needed())
 360             Mat(localInliers).copyTo(_inliers);
 361     }
 362     else
 363     {
 364         tvec.setTo(Scalar(0));
 365         Mat R = Mat::eye(3, 3, CV_64F);
 366         Rodrigues(R, rvec);
 367         if( _inliers.needed() )
 368             _inliers.release();
 369     }
 370     return;
 371 }
 372