solvepnp.cpp
Go to the documentation of this file.
00001 /*M///////////////////////////////////////////////////////////////////////////////////////
00002  //
00003  //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
00004  //
00005  //  By downloading, copying, installing or using the software you agree to this license.
00006  //  If you do not agree to this license, do not download, install,
00007  //  copy or use the software.
00008  //
00009  //
00010  //                           License Agreement
00011  //                For Open Source Computer Vision Library
00012  //
00013  // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
00014  // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
00015  // Third party copyrights are property of their respective owners.
00016  //
00017  // Redistribution and use in source and binary forms, with or without modification,
00018  // are permitted provided that the following conditions are met:
00019  //
00020  //   * Redistribution's of source code must retain the above copyright notice,
00021  //     this list of conditions and the following disclaimer.
00022  //
00023  //   * Redistribution's in binary form must reproduce the above copyright notice,
00024  //     this list of conditions and the following disclaimer in the documentation
00025  //     and/or other materials provided with the distribution.
00026  //
00027  //   * The name of the copyright holders may not be used to endorse or promote products
00028  //     derived from this software without specific prior written permission.
00029  //
00030  // This software is provided by the copyright holders and contributors "as is" and
00031  // any express or implied warranties, including, but not limited to, the implied
00032  // warranties of merchantability and fitness for a particular purpose are disclaimed.
00033  // In no event shall the Intel Corporation or contributors be liable for any direct,
00034  // indirect, incidental, special, exemplary, or consequential damages
00035  // (including, but not limited to, procurement of substitute goods or services;
00036  // loss of use, data, or profits; or business interruption) however caused
00037  // and on any theory of liability, whether in contract, strict liability,
00038  // or tort (including negligence or otherwise) arising in any way out of
00039  // the use of this software, even if advised of the possibility of such damage.
00040  //
00041  //M*/
00042 
00043 #include "solvepnp.h"
00044 #include <iostream>
00045 
00046 using namespace cv;
00047 
00048 namespace cv3
00049 {
00050 
00051 class PnPRansacCallback : public PointSetRegistrator::Callback
00052 {
00053 
00054 public:
00055 
00056     PnPRansacCallback(Mat _cameraMatrix=Mat(3,3,CV_64F), Mat _distCoeffs=Mat(4,1,CV_64F), int _flags=CV_ITERATIVE,
00057             bool _useExtrinsicGuess=false, Mat _rvec=Mat(), Mat _tvec=Mat() )
00058         : cameraMatrix(_cameraMatrix), distCoeffs(_distCoeffs), flags(_flags), useExtrinsicGuess(_useExtrinsicGuess),
00059           rvec(_rvec), tvec(_tvec) {}
00060 
00061     /* Pre: True */
00062     /* Post: compute _model with given points an return number of found models */
00063     int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const
00064     {
00065         Mat opoints = _m1.getMat(), ipoints = _m2.getMat();
00066 
00067         bool correspondence = solvePnP( _m1, _m2, cameraMatrix, distCoeffs,
00068                                             rvec, tvec, useExtrinsicGuess, flags );
00069 
00070         Mat _local_model;
00071         hconcat(rvec, tvec, _local_model);
00072         _local_model.copyTo(_model);
00073 
00074         return correspondence;
00075     }
00076 
00077     /* Pre: True */
00078     /* Post: fill _err with projection errors */
00079     void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const
00080     {
00081 
00082         Mat opoints = _m1.getMat(), ipoints = _m2.getMat(), model = _model.getMat();
00083 
00084         int i, count = opoints.checkVector(3);
00085         Mat _rvec = model.col(0);
00086         Mat _tvec = model.col(1);
00087 
00088 
00089         Mat projpoints(count, 2, CV_32FC1);
00090         projectPoints(opoints, _rvec, _tvec, cameraMatrix, distCoeffs, projpoints);
00091 
00092         const Point2f* ipoints_ptr = ipoints.ptr<Point2f>();
00093         const Point2f* projpoints_ptr = projpoints.ptr<Point2f>();
00094 
00095         _err.create(count, 1, CV_32FC1);
00096         float* err = _err.getMat().ptr<float>();
00097 
00098         for ( i = 0; i < count; ++i)
00099             err[i] = (float)norm( ipoints_ptr[i] - projpoints_ptr[i] );
00100 
00101     }
00102 
00103 
00104     Mat cameraMatrix;
00105     Mat distCoeffs;
00106     int flags;
00107     bool useExtrinsicGuess;
00108     Mat rvec;
00109     Mat tvec;
00110 };
00111 
00112 bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
00113                         InputArray _cameraMatrix, InputArray _distCoeffs,
00114                         OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess,
00115                         int iterationsCount, float reprojectionError, double confidence,
00116                         OutputArray _inliers, int flags)
00117 {
00118 
00119     Mat opoints0 = _opoints.getMat(), ipoints0 = _ipoints.getMat();
00120     Mat opoints, ipoints;
00121     if( opoints0.depth() == CV_64F || !opoints0.isContinuous() )
00122         opoints0.convertTo(opoints, CV_32F);
00123     else
00124         opoints = opoints0;
00125     if( ipoints0.depth() == CV_64F || !ipoints0.isContinuous() )
00126         ipoints0.convertTo(ipoints, CV_32F);
00127     else
00128         ipoints = ipoints0;
00129 
00130     int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
00131     CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
00132 
00133     CV_Assert(opoints.isContinuous());
00134     CV_Assert(opoints.depth() == CV_32F || opoints.depth() == CV_64F);
00135     CV_Assert((opoints.rows == 1 && opoints.channels() == 3) || opoints.cols*opoints.channels() == 3);
00136     CV_Assert(ipoints.isContinuous());
00137     CV_Assert(ipoints.depth() == CV_32F || ipoints.depth() == CV_64F);
00138     CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2);
00139 
00140     _rvec.create(3, 1, CV_64FC1);
00141     _tvec.create(3, 1, CV_64FC1);
00142 
00143     Mat rvec = useExtrinsicGuess ? _rvec.getMat() : Mat(3, 1, CV_64FC1);
00144     Mat tvec = useExtrinsicGuess ? _tvec.getMat() : Mat(3, 1, CV_64FC1);
00145     Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
00146 
00147     int model_points = 5;
00148     int ransac_kernel_method = CV_EPNP;
00149 
00150     if( npoints == 4 )
00151     {
00152         model_points = 4;
00153         ransac_kernel_method = CV_P3P;
00154     }
00155 
00156     Ptr<PointSetRegistrator::Callback> cb; // pointer to callback
00157     cb = Ptr<PnPRansacCallback>(new PnPRansacCallback( cameraMatrix, distCoeffs, ransac_kernel_method, useExtrinsicGuess, rvec, tvec));
00158 
00159     double param1 = reprojectionError;                // reprojection error
00160     double param2 = confidence;                       // confidence
00161     int param3 = iterationsCount;                     // number maximum iterations
00162 
00163     Mat _local_model(3, 2, CV_64FC1);
00164     Mat _mask_local_inliers(1, opoints.rows, CV_8UC1);
00165 
00166     // call Ransac
00167     int result = createRANSACPointSetRegistrator(cb, model_points,
00168         param1, param2, param3)->run(opoints, ipoints, _local_model, _mask_local_inliers);
00169 
00170     if( result > 0 )
00171     {
00172         std::vector<Point3d> opoints_inliers;
00173         std::vector<Point2d> ipoints_inliers;
00174         opoints.convertTo(opoints_inliers, CV_64F);
00175         ipoints.convertTo(ipoints_inliers, CV_64F);
00176 
00177         const uchar* mask = _mask_local_inliers.ptr<uchar>();
00178         int npoints1 = compressElems(&opoints_inliers[0], mask, 1, npoints);
00179         compressElems(&ipoints_inliers[0], mask, 1, npoints);
00180 
00181         opoints_inliers.resize(npoints1);
00182         ipoints_inliers.resize(npoints1);
00183         result = solvePnP(opoints_inliers, ipoints_inliers, cameraMatrix,
00184                           distCoeffs, rvec, tvec, false, flags == CV_P3P ? CV_EPNP : flags) ? 1 : -1;
00185     }
00186 
00187     if( result <= 0 || _local_model.rows <= 0)
00188     {
00189         rvec.copyTo(_rvec);    // output rotation vector
00190         tvec.copyTo(_tvec);    // output translation vector
00191 
00192         if( _inliers.needed() )
00193             _inliers.release();
00194 
00195         return false;
00196     }
00197     else
00198     {
00199         _local_model.col(0).copyTo(_rvec);    // output rotation vector
00200         _local_model.col(1).copyTo(_tvec);    // output translation vector
00201     }
00202 
00203     if(_inliers.needed())
00204     {
00205         Mat _local_inliers;
00206         for (int i = 0; i < npoints; ++i)
00207         {
00208             if((int)_mask_local_inliers.at<uchar>(i) != 0) // inliers mask
00209                 _local_inliers.push_back(i);    // output inliers vector
00210         }
00211         _local_inliers.copyTo(_inliers);
00212     }
00213     return true;
00214 }
00215 
00216 int RANSACUpdateNumIters( double p, double ep, int modelPoints, int maxIters )
00217 {
00218     if( modelPoints <= 0 )
00219         CV_Error( 0, "the number of model points should be positive" );
00220 
00221     p = MAX(p, 0.);
00222     p = MIN(p, 1.);
00223     ep = MAX(ep, 0.);
00224     ep = MIN(ep, 1.);
00225 
00226     // avoid inf's & nan's
00227     double num = MAX(1. - p, DBL_MIN);
00228     double denom = 1. - std::pow(1. - ep, modelPoints);
00229     if( denom < DBL_MIN )
00230         return 0;
00231 
00232     num = std::log(num);
00233     denom = std::log(denom);
00234 
00235     return denom >= 0 || -num >= maxIters*(-denom) ? maxIters : cvRound(num/denom);
00236 }
00237 
00238 class RANSACPointSetRegistrator : public PointSetRegistrator
00239 {
00240 public:
00241     RANSACPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb=Ptr<PointSetRegistrator::Callback>(),
00242                               int _modelPoints=0, double _threshold=0, double _confidence=0.99, int _maxIters=1000)
00243     : cb(_cb), modelPoints(_modelPoints), threshold(_threshold), confidence(_confidence), maxIters(_maxIters)
00244     {
00245         checkPartialSubsets = false;
00246     }
00247 
00248     int findInliers( const Mat& m1, const Mat& m2, const Mat& model, Mat& err, Mat& mask, double thresh ) const
00249     {
00250         cb->computeError( m1, m2, model, err );
00251         mask.create(err.size(), CV_8U);
00252 
00253         CV_Assert( err.isContinuous() && err.type() == CV_32F && mask.isContinuous() && mask.type() == CV_8U);
00254         const float* errptr = err.ptr<float>();
00255         uchar* maskptr = mask.ptr<uchar>();
00256         float t = (float)(thresh*thresh);
00257         int i, n = (int)err.total(), nz = 0;
00258         for( i = 0; i < n; i++ )
00259         {
00260             int f = errptr[i] <= t;
00261             maskptr[i] = (uchar)f;
00262             nz += f;
00263         }
00264         return nz;
00265     }
00266 
00267     bool getSubset( const Mat& m1, const Mat& m2,
00268                     Mat& ms1, Mat& ms2, RNG& rng,
00269                     int maxAttempts=1000 ) const
00270     {
00271         cv::AutoBuffer<int> _idx(modelPoints);
00272         int* idx = _idx;
00273         int i = 0, j, k, iters = 0;
00274         int esz1 = (int)m1.elemSize(), esz2 = (int)m2.elemSize();
00275         int d1 = m1.channels() > 1 ? m1.channels() : m1.cols;
00276         int d2 = m2.channels() > 1 ? m2.channels() : m2.cols;
00277         int count = m1.checkVector(d1), count2 = m2.checkVector(d2);
00278         const int *m1ptr = m1.ptr<int>(), *m2ptr = m2.ptr<int>();
00279 
00280         ms1.create(modelPoints, 1, CV_MAKETYPE(m1.depth(), d1));
00281         ms2.create(modelPoints, 1, CV_MAKETYPE(m2.depth(), d2));
00282 
00283         int *ms1ptr = ms1.ptr<int>(), *ms2ptr = ms2.ptr<int>();
00284 
00285         CV_Assert( count >= modelPoints && count == count2 );
00286         CV_Assert( (esz1 % sizeof(int)) == 0 && (esz2 % sizeof(int)) == 0 );
00287         esz1 /= sizeof(int);
00288         esz2 /= sizeof(int);
00289 
00290         for(; iters < maxAttempts; iters++)
00291         {
00292             for( i = 0; i < modelPoints && iters < maxAttempts; )
00293             {
00294                 int idx_i = 0;
00295                 for(;;)
00296                 {
00297                     idx_i = idx[i] = rng.uniform(0, count);
00298                     for( j = 0; j < i; j++ )
00299                         if( idx_i == idx[j] )
00300                             break;
00301                     if( j == i )
00302                         break;
00303                 }
00304                 for( k = 0; k < esz1; k++ )
00305                     ms1ptr[i*esz1 + k] = m1ptr[idx_i*esz1 + k];
00306                 for( k = 0; k < esz2; k++ )
00307                     ms2ptr[i*esz2 + k] = m2ptr[idx_i*esz2 + k];
00308                 if( checkPartialSubsets && !cb->checkSubset( ms1, ms2, i+1 ))
00309                 {
00310                     // we may have selected some bad points;
00311                     // so, let's remove some of them randomly
00312                     i = rng.uniform(0, i+1);
00313                     iters++;
00314                     continue;
00315                 }
00316                 i++;
00317             }
00318             if( !checkPartialSubsets && i == modelPoints && !cb->checkSubset(ms1, ms2, i))
00319                 continue;
00320             break;
00321         }
00322 
00323         return i == modelPoints && iters < maxAttempts;
00324     }
00325 
00326     bool run(InputArray _m1, InputArray _m2, OutputArray _model, OutputArray _mask) const
00327     {
00328         bool result = false;
00329         Mat m1 = _m1.getMat(), m2 = _m2.getMat();
00330         Mat err, mask, model, bestModel, ms1, ms2;
00331 
00332         int iter, niters = MAX(maxIters, 1);
00333         int d1 = m1.channels() > 1 ? m1.channels() : m1.cols;
00334         int d2 = m2.channels() > 1 ? m2.channels() : m2.cols;
00335         int count = m1.checkVector(d1), count2 = m2.checkVector(d2), maxGoodCount = 0;
00336 
00337         RNG rng((uint64)-1);
00338 
00339         CV_Assert( cb );
00340         CV_Assert( confidence > 0 && confidence < 1 );
00341 
00342         CV_Assert( count >= 0 && count2 == count );
00343         if( count < modelPoints )
00344             return false;
00345 
00346         Mat bestMask0, bestMask;
00347 
00348         if( _mask.needed() )
00349         {
00350             _mask.create(count, 1, CV_8U, -1, true);
00351             bestMask0 = bestMask = _mask.getMat();
00352             CV_Assert( (bestMask.cols == 1 || bestMask.rows == 1) && (int)bestMask.total() == count );
00353         }
00354         else
00355         {
00356             bestMask.create(count, 1, CV_8U);
00357             bestMask0 = bestMask;
00358         }
00359 
00360         if( count == modelPoints )
00361         {
00362             if( cb->runKernel(m1, m2, bestModel) <= 0 )
00363                 return false;
00364             bestModel.copyTo(_model);
00365             bestMask.setTo(Scalar::all(1));
00366             return true;
00367         }
00368 
00369         for( iter = 0; iter < niters; iter++ )
00370         {
00371             int i, goodCount, nmodels;
00372             if( count > modelPoints )
00373             {
00374                 bool found = getSubset( m1, m2, ms1, ms2, rng, 10000 );
00375                 if( !found )
00376                 {
00377                     if( iter == 0 )
00378                         return false;
00379                     break;
00380                 }
00381             }
00382 
00383             nmodels = cb->runKernel( ms1, ms2, model );
00384             if( nmodels <= 0 )
00385                 continue;
00386             CV_Assert( model.rows % nmodels == 0 );
00387             Size modelSize(model.cols, model.rows/nmodels);
00388 
00389             for( i = 0; i < nmodels; i++ )
00390             {
00391                 Mat model_i = model.rowRange( i*modelSize.height, (i+1)*modelSize.height );
00392                 goodCount = findInliers( m1, m2, model_i, err, mask, threshold );
00393 
00394                 if( goodCount > MAX(maxGoodCount, modelPoints-1) )
00395                 {
00396                     std::swap(mask, bestMask);
00397                     model_i.copyTo(bestModel);
00398                     maxGoodCount = goodCount;
00399                     niters = RANSACUpdateNumIters( confidence, (double)(count - goodCount)/count, modelPoints, niters );
00400                 }
00401             }
00402         }
00403 
00404         if( maxGoodCount > 0 )
00405         {
00406             if( bestMask.data != bestMask0.data )
00407             {
00408                 if( bestMask.size() == bestMask0.size() )
00409                     bestMask.copyTo(bestMask0);
00410                 else
00411                     transpose(bestMask, bestMask0);
00412             }
00413             bestModel.copyTo(_model);
00414             result = true;
00415         }
00416         else
00417             _model.release();
00418 
00419         return result;
00420     }
00421 
00422     void setCallback(const Ptr<PointSetRegistrator::Callback>& _cb) { cb = _cb; }
00423 
00424     Ptr<PointSetRegistrator::Callback> cb;
00425     int modelPoints;
00426     bool checkPartialSubsets;
00427     double threshold;
00428     double confidence;
00429     int maxIters;
00430 };
00431 
00432 class LMeDSPointSetRegistrator : public RANSACPointSetRegistrator
00433 {
00434 public:
00435     LMeDSPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb=Ptr<PointSetRegistrator::Callback>(),
00436                               int _modelPoints=0, double _confidence=0.99, int _maxIters=1000)
00437     : RANSACPointSetRegistrator(_cb, _modelPoints, 0, _confidence, _maxIters) {}
00438 
00439     bool run(InputArray _m1, InputArray _m2, OutputArray _model, OutputArray _mask) const
00440     {
00441         const double outlierRatio = 0.45;
00442         bool result = false;
00443         Mat m1 = _m1.getMat(), m2 = _m2.getMat();
00444         Mat ms1, ms2, err, errf, model, bestModel, mask, mask0;
00445 
00446         int d1 = m1.channels() > 1 ? m1.channels() : m1.cols;
00447         int d2 = m2.channels() > 1 ? m2.channels() : m2.cols;
00448         int count = m1.checkVector(d1), count2 = m2.checkVector(d2);
00449         double minMedian = DBL_MAX, sigma;
00450 
00451         RNG rng((uint64)-1);
00452 
00453         CV_Assert( cb );
00454         CV_Assert( confidence > 0 && confidence < 1 );
00455 
00456         CV_Assert( count >= 0 && count2 == count );
00457         if( count < modelPoints )
00458             return false;
00459 
00460         if( _mask.needed() )
00461         {
00462             _mask.create(count, 1, CV_8U, -1, true);
00463             mask0 = mask = _mask.getMat();
00464             CV_Assert( (mask.cols == 1 || mask.rows == 1) && (int)mask.total() == count );
00465         }
00466 
00467         if( count == modelPoints )
00468         {
00469             if( cb->runKernel(m1, m2, bestModel) <= 0 )
00470                 return false;
00471             bestModel.copyTo(_model);
00472             mask.setTo(Scalar::all(1));
00473             return true;
00474         }
00475 
00476         int iter, niters = RANSACUpdateNumIters(confidence, outlierRatio, modelPoints, maxIters);
00477         niters = MAX(niters, 3);
00478 
00479         for( iter = 0; iter < niters; iter++ )
00480         {
00481             int i, nmodels;
00482             if( count > modelPoints )
00483             {
00484                 bool found = getSubset( m1, m2, ms1, ms2, rng );
00485                 if( !found )
00486                 {
00487                     if( iter == 0 )
00488                         return false;
00489                     break;
00490                 }
00491             }
00492 
00493             nmodels = cb->runKernel( ms1, ms2, model );
00494             if( nmodels <= 0 )
00495                 continue;
00496 
00497             CV_Assert( model.rows % nmodels == 0 );
00498             Size modelSize(model.cols, model.rows/nmodels);
00499 
00500             for( i = 0; i < nmodels; i++ )
00501             {
00502                 Mat model_i = model.rowRange( i*modelSize.height, (i+1)*modelSize.height );
00503                 cb->computeError( m1, m2, model_i, err );
00504                 if( err.depth() != CV_32F )
00505                     err.convertTo(errf, CV_32F);
00506                 else
00507                     errf = err;
00508                 CV_Assert( errf.isContinuous() && errf.type() == CV_32F && (int)errf.total() == count );
00509                 std::sort(errf.ptr<int>(), errf.ptr<int>() + count);
00510 
00511                 double median = count % 2 != 0 ?
00512                 errf.at<float>(count/2) : (errf.at<float>(count/2-1) + errf.at<float>(count/2))*0.5;
00513 
00514                 if( median < minMedian )
00515                 {
00516                     minMedian = median;
00517                     model_i.copyTo(bestModel);
00518                 }
00519             }
00520         }
00521 
00522         if( minMedian < DBL_MAX )
00523         {
00524             sigma = 2.5*1.4826*(1 + 5./(count - modelPoints))*std::sqrt(minMedian);
00525             sigma = MAX( sigma, 0.001 );
00526 
00527             count = findInliers( m1, m2, bestModel, err, mask, sigma );
00528             if( _mask.needed() && mask0.data != mask.data )
00529             {
00530                 if( mask0.size() == mask.size() )
00531                     mask.copyTo(mask0);
00532                 else
00533                     transpose(mask, mask0);
00534             }
00535             bestModel.copyTo(_model);
00536             result = count >= modelPoints;
00537         }
00538         else
00539             _model.release();
00540 
00541         return result;
00542     }
00543 
00544 };
00545 
00546 Ptr<PointSetRegistrator> createRANSACPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb,
00547                                                          int _modelPoints, double _threshold,
00548                                                          double _confidence, int _maxIters)
00549 {
00550     return Ptr<PointSetRegistrator>(
00551         new RANSACPointSetRegistrator(_cb, _modelPoints, _threshold, _confidence, _maxIters));
00552 }
00553 
00554 
00555 Ptr<PointSetRegistrator> createLMeDSPointSetRegistrator(const Ptr<PointSetRegistrator::Callback>& _cb,
00556                              int _modelPoints, double _confidence, int _maxIters)
00557 {
00558     return Ptr<PointSetRegistrator>(
00559         new LMeDSPointSetRegistrator(_cb, _modelPoints, _confidence, _maxIters));
00560 }
00561 
00562 
00563 class Affine3DEstimatorCallback : public PointSetRegistrator::Callback
00564 {
00565 public:
00566     int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const
00567     {
00568         Mat m1 = _m1.getMat(), m2 = _m2.getMat();
00569         const Point3f* from = m1.ptr<Point3f>();
00570         const Point3f* to   = m2.ptr<Point3f>();
00571 
00572         const int N = 12;
00573         double buf[N*N + N + N];
00574         Mat A(N, N, CV_64F, &buf[0]);
00575         Mat B(N, 1, CV_64F, &buf[0] + N*N);
00576         Mat X(N, 1, CV_64F, &buf[0] + N*N + N);
00577         double* Adata = A.ptr<double>();
00578         double* Bdata = B.ptr<double>();
00579         A = Scalar::all(0);
00580 
00581         for( int i = 0; i < (N/3); i++ )
00582         {
00583             Bdata[i*3] = to[i].x;
00584             Bdata[i*3+1] = to[i].y;
00585             Bdata[i*3+2] = to[i].z;
00586 
00587             double *aptr = Adata + i*3*N;
00588             for(int k = 0; k < 3; ++k)
00589             {
00590                 aptr[0] = from[i].x;
00591                 aptr[1] = from[i].y;
00592                 aptr[2] = from[i].z;
00593                 aptr[3] = 1.0;
00594                 aptr += 16;
00595             }
00596         }
00597 
00598         solve(A, B, X, DECOMP_SVD);
00599         X.reshape(1, 3).copyTo(_model);
00600 
00601         return 1;
00602     }
00603 
00604     void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const
00605     {
00606         Mat m1 = _m1.getMat(), m2 = _m2.getMat(), model = _model.getMat();
00607         const Point3f* from = m1.ptr<Point3f>();
00608         const Point3f* to   = m2.ptr<Point3f>();
00609         const double* F = model.ptr<double>();
00610 
00611         int count = m1.checkVector(3);
00612         CV_Assert( count > 0 );
00613 
00614         _err.create(count, 1, CV_32F);
00615         Mat err = _err.getMat();
00616         float* errptr = err.ptr<float>();
00617 
00618         for(int i = 0; i < count; i++ )
00619         {
00620             const Point3f& f = from[i];
00621             const Point3f& t = to[i];
00622 
00623             double a = F[0]*f.x + F[1]*f.y + F[ 2]*f.z + F[ 3] - t.x;
00624             double b = F[4]*f.x + F[5]*f.y + F[ 6]*f.z + F[ 7] - t.y;
00625             double c = F[8]*f.x + F[9]*f.y + F[10]*f.z + F[11] - t.z;
00626 
00627             errptr[i] = (float)std::sqrt(a*a + b*b + c*c);
00628         }
00629     }
00630 
00631     bool checkSubset( InputArray _ms1, InputArray _ms2, int count ) const
00632     {
00633         const float threshold = 0.996f;
00634         Mat ms1 = _ms1.getMat(), ms2 = _ms2.getMat();
00635 
00636         for( int inp = 1; inp <= 2; inp++ )
00637         {
00638             int j, k, i = count - 1;
00639             const Mat* msi = inp == 1 ? &ms1 : &ms2;
00640             const Point3f* ptr = msi->ptr<Point3f>();
00641 
00642             CV_Assert( count <= msi->rows );
00643 
00644             // check that the i-th selected point does not belong
00645             // to a line connecting some previously selected points
00646             for(j = 0; j < i; ++j)
00647             {
00648                 Point3f d1 = ptr[j] - ptr[i];
00649                 float n1 = d1.x*d1.x + d1.y*d1.y;
00650 
00651                 for(k = 0; k < j; ++k)
00652                 {
00653                     Point3f d2 = ptr[k] - ptr[i];
00654                     float denom = (d2.x*d2.x + d2.y*d2.y)*n1;
00655                     float num = d1.x*d2.x + d1.y*d2.y;
00656 
00657                     if( num*num > threshold*threshold*denom )
00658                         return false;
00659                 }
00660             }
00661         }
00662         return true;
00663     }
00664 };
00665 
00666 }


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:59:22