samples/cpp/build3dmodel.cpp

   1 #include "opencv2/core.hpp"
   2 #include <opencv2/core/utility.hpp>
   3 #include "opencv2/imgproc.hpp"
   4 #include "opencv2/calib3d.hpp"
   5 #include "opencv2/features2d.hpp"
   6 #include "opencv2/highgui.hpp"
   7
   8 #include <map>
   9
  10 #include <ctype.h>
  11 #include <stdio.h>
  12 #include <stdlib.h>
  13
  14 using namespace cv;
  15 using namespace std;
  16
  17 static void help()
  18 {
  19     printf("\nSigh: This program is not complete/will be replaced. \n"
  20            "So:   Use this just to see hints of how to use things like Rodrigues\n"
  21            "      conversions, finding the fundamental matrix, using descriptor\n"
  22            "      finding and matching in features2d and using camera parameters\n"
  23            "Usage: build3dmodel -i <intrinsics_filename>\n"
  24            "\t[-d <detector>] [-de <descriptor_extractor>] -m <model_name>\n\n");
  25     return;
  26 }
  27
  28
  29 static bool readCameraMatrix(const string& filename,
  30                              Mat& cameraMatrix, Mat& distCoeffs,
  31                              Size& calibratedImageSize )
  32 {
  33     FileStorage fs(filename, FileStorage::READ);
  34     fs["image_width"] >> calibratedImageSize.width;
  35     fs["image_height"] >> calibratedImageSize.height;
  36     fs["distortion_coefficients"] >> distCoeffs;
  37     fs["camera_matrix"] >> cameraMatrix;
  38
  39     if( distCoeffs.type() != CV_64F )
  40         distCoeffs = Mat_<double>(distCoeffs);
  41     if( cameraMatrix.type() != CV_64F )
  42         cameraMatrix = Mat_<double>(cameraMatrix);
  43
  44     return true;
  45 }
  46
  47 static bool readModelViews( const string& filename, vector<Point3f>& box,
  48                            vector<string>& imagelist,
  49                            vector<Rect>& roiList, vector<Vec6f>& poseList )
  50 {
  51     imagelist.resize(0);
  52     roiList.resize(0);
  53     poseList.resize(0);
  54     box.resize(0);
  55
  56     FileStorage fs(filename, FileStorage::READ);
  57     if( !fs.isOpened() )
  58         return false;
  59     fs["box"] >> box;
  60
  61     FileNode all = fs["views"];
  62     if( all.type() != FileNode::SEQ )
  63         return false;
  64     FileNodeIterator it = all.begin(), it_end = all.end();
  65
  66     for(; it != it_end; ++it)
  67     {
  68         FileNode n = *it;
  69         imagelist.push_back((string)n["image"]);
  70         FileNode nr = n["roi"];
  71         roiList.push_back(Rect((int)nr[0], (int)nr[1], (int)nr[2], (int)nr[3]));
  72         FileNode np = n["pose"];
  73         poseList.push_back(Vec6f((float)np[0], (float)np[1], (float)np[2],
  74                                  (float)np[3], (float)np[4], (float)np[5]));
  75     }
  76
  77     return true;
  78 }
  79
  80
  81 struct PointModel
  82 {
  83     vector<Point3f> points;
  84     vector<vector<int> > didx;
  85     Mat descriptors;
  86     string name;
  87 };
  88
  89
  90 static void writeModel(const string& modelFileName, const string& modelname,
  91                        const PointModel& model)
  92 {
  93     FileStorage fs(modelFileName, FileStorage::WRITE);
  94
  95     fs << modelname << "{" <<
  96         "points" << "[:" << model.points << "]" <<
  97         "idx" << "[:";
  98
  99     for( size_t i = 0; i < model.didx.size(); i++ )
 100         fs << "[:" << model.didx[i] << "]";
 101     fs << "]" << "descriptors" << model.descriptors;
 102 }
 103
 104
 105 static void unpackPose(const Vec6f& pose, Mat& R, Mat& t)
 106 {
 107     Mat rvec = (Mat_<double>(3,1) << pose[0], pose[1], pose[2]);
 108     t = (Mat_<double>(3,1) << pose[3], pose[4], pose[5]);
 109     Rodrigues(rvec, R);
 110 }
 111
 112
 113 static Mat getFundamentalMat( const Mat& R1, const Mat& t1,
 114                               const Mat& R2, const Mat& t2,
 115                               const Mat& cameraMatrix )
 116 {
 117     Mat_<double> R = R2*R1.t(), t = t2 - R*t1;
 118     double tx = t.at<double>(0,0), ty = t.at<double>(1,0), tz = t.at<double>(2,0);
 119     Mat E = (Mat_<double>(3,3) << 0, -tz, ty, tz, 0, -tx, -ty, tx, 0)*R;
 120     Mat iK = cameraMatrix.inv();
 121     Mat F = iK.t()*E*iK;
 122
 123 #if 0
 124     static bool checked = false;
 125     if(!checked)
 126     {
 127         vector<Point3f> objpoints(100);
 128         Mat O(objpoints);
 129         randu(O, Scalar::all(-10), Scalar::all(10));
 130         vector<Point2f> imgpoints1, imgpoints2;
 131         projectPoints(Mat(objpoints), R1, t1, cameraMatrix, Mat(), imgpoints1);
 132         projectPoints(Mat(objpoints), R2, t2, cameraMatrix, Mat(), imgpoints2);
 133         double* f = (double*)F.data;
 134         for( size_t i = 0; i < objpoints.size(); i++ )
 135         {
 136             Point2f p1 = imgpoints1[i], p2 = imgpoints2[i];
 137             double diff = p2.x*(f[0]*p1.x + f[1]*p1.y + f[2]) +
 138                  p2.y*(f[3]*p1.x + f[4]*p1.y + f[5]) +
 139                 f[6]*p1.x + f[7]*p1.y + f[8];
 140             CV_Assert(fabs(diff) < 1e-3);
 141         }
 142         checked = true;
 143     }
 144 #endif
 145     return F;
 146 }
 147
 148
 149 static void findConstrainedCorrespondences(const Mat& _F,
 150                 const vector<KeyPoint>& keypoints1,
 151                 const vector<KeyPoint>& keypoints2,
 152                 const Mat& descriptors1,
 153                 const Mat& descriptors2,
 154                 vector<Vec2i>& matches,
 155                 double eps, double ratio)
 156 {
 157     float F[9]={0};
 158     int dsize = descriptors1.cols;
 159
 160     Mat Fhdr = Mat(3, 3, CV_32F, F);
 161     _F.convertTo(Fhdr, CV_32F);
 162     matches.clear();
 163
 164     for( int i = 0; i < (int)keypoints1.size(); i++ )
 165     {
 166         Point2f p1 = keypoints1[i].pt;
 167         double bestDist1 = DBL_MAX, bestDist2 = DBL_MAX;
 168         int bestIdx1 = -1;//, bestIdx2 = -1;
 169         const float* d1 = descriptors1.ptr<float>(i);
 170
 171         for( int j = 0; j < (int)keypoints2.size(); j++ )
 172         {
 173             Point2f p2 = keypoints2[j].pt;
 174             double e = p2.x*(F[0]*p1.x + F[1]*p1.y + F[2]) +
 175                        p2.y*(F[3]*p1.x + F[4]*p1.y + F[5]) +
 176                        F[6]*p1.x + F[7]*p1.y + F[8];
 177             if( fabs(e) > eps )
 178                 continue;
 179             const float* d2 = descriptors2.ptr<float>(j);
 180             double dist = 0;
 181             int k = 0;
 182
 183             for( ; k <= dsize - 8; k += 8 )
 184             {
 185                 float t0 = d1[k] - d2[k], t1 = d1[k+1] - d2[k+1];
 186                 float t2 = d1[k+2] - d2[k+2], t3 = d1[k+3] - d2[k+3];
 187                 float t4 = d1[k+4] - d2[k+4], t5 = d1[k+5] - d2[k+5];
 188                 float t6 = d1[k+6] - d2[k+6], t7 = d1[k+7] - d2[k+7];
 189                 dist += t0*t0 + t1*t1 + t2*t2 + t3*t3 +
 190                         t4*t4 + t5*t5 + t6*t6 + t7*t7;
 191
 192                 if( dist >= bestDist2 )
 193                     break;
 194             }
 195
 196             if( dist < bestDist2 )
 197             {
 198                 for( ; k < dsize; k++ )
 199                 {
 200                     float t = d1[k] - d2[k];
 201                     dist += t*t;
 202                 }
 203
 204                 if( dist < bestDist1 )
 205                 {
 206                     bestDist2 = bestDist1;
 207                     //bestIdx2 = bestIdx1;
 208                     bestDist1 = dist;
 209                     bestIdx1 = (int)j;
 210                 }
 211                 else if( dist < bestDist2 )
 212                 {
 213                     bestDist2 = dist;
 214                     //bestIdx2 = (int)j;
 215                 }
 216             }
 217         }
 218
 219         if( bestIdx1 >= 0 && bestDist1 < bestDist2*ratio )
 220         {
 221             Point2f p2 = keypoints1[bestIdx1].pt;
 222             double e = p2.x*(F[0]*p1.x + F[1]*p1.y + F[2]) +
 223                         p2.y*(F[3]*p1.x + F[4]*p1.y + F[5]) +
 224                         F[6]*p1.x + F[7]*p1.y + F[8];
 225             if( e > eps*0.25 )
 226                 continue;
 227             double threshold = bestDist1/ratio;
 228             const float* d22 = descriptors2.ptr<float>(bestIdx1);
 229             int i1 = 0;
 230             for( ; i1 < (int)keypoints1.size(); i1++ )
 231             {
 232                 if( i1 == i )
 233                     continue;
 234                 Point2f pt1 = keypoints1[i1].pt;
 235                 const float* d11 = descriptors1.ptr<float>(i1);
 236                 double dist = 0;
 237
 238                 e = p2.x*(F[0]*pt1.x + F[1]*pt1.y + F[2]) +
 239                     p2.y*(F[3]*pt1.x + F[4]*pt1.y + F[5]) +
 240                     F[6]*pt1.x + F[7]*pt1.y + F[8];
 241                 if( fabs(e) > eps )
 242                     continue;
 243
 244                 for( int k = 0; k < dsize; k++ )
 245                 {
 246                     float t = d11[k] - d22[k];
 247                     dist += t*t;
 248                     if( dist >= threshold )
 249                         break;
 250                 }
 251
 252                 if( dist < threshold )
 253                     break;
 254             }
 255             if( i1 == (int)keypoints1.size() )
 256                 matches.push_back(Vec2i(i,bestIdx1));
 257         }
 258     }
 259 }
 260
 261
 262 static Point3f findRayIntersection(Point3f k1, Point3f b1, Point3f k2, Point3f b2)
 263 {
 264     float a[4], b[2], x[2];
 265     a[0] = k1.dot(k1);
 266     a[1] = a[2] = -k1.dot(k2);
 267     a[3] = k2.dot(k2);
 268     b[0] = k1.dot(b2 - b1);
 269     b[1] = k2.dot(b1 - b2);
 270     Mat_<float> A(2, 2, a), B(2, 1, b), X(2, 1, x);
 271     solve(A, B, X);
 272
 273     float s1 = X.at<float>(0, 0);
 274     float s2 = X.at<float>(1, 0);
 275     return (k1*s1 + b1 + k2*s2 + b2)*0.5f;
 276 }
 277
 278
 279 static Point3f triangulatePoint(const vector<Point2f>& ps,
 280                                 const vector<Mat>& Rs,
 281                                 const vector<Mat>& ts,
 282                                 const Mat& cameraMatrix)
 283 {
 284     Mat_<double> K(cameraMatrix);
 285
 286     /*if( ps.size() > 2 )
 287     {
 288         Mat_<double> L(ps.size()*3, 4), U, evalues;
 289         Mat_<double> P(3,4), Rt(3,4), Rt_part1=Rt.colRange(0,3), Rt_part2=Rt.colRange(3,4);
 290
 291         for( size_t i = 0; i < ps.size(); i++ )
 292         {
 293             double x = ps[i].x, y = ps[i].y;
 294             Rs[i].convertTo(Rt_part1, Rt_part1.type());
 295             ts[i].convertTo(Rt_part2, Rt_part2.type());
 296             P = K*Rt;
 297
 298             for( int k = 0; k < 4; k++ )
 299             {
 300                 L(i*3, k) = x*P(2,k) - P(0,k);
 301                 L(i*3+1, k) = y*P(2,k) - P(1,k);
 302                 L(i*3+2, k) = x*P(1,k) - y*P(0,k);
 303             }
 304         }
 305
 306         eigen(L.t()*L, evalues, U);
 307         CV_Assert(evalues(0,0) >= evalues(3,0));
 308
 309         double W = fabs(U(3,3)) > FLT_EPSILON ? 1./U(3,3) : 0;
 310         return Point3f((float)(U(3,0)*W), (float)(U(3,1)*W), (float)(U(3,2)*W));
 311     }
 312     else*/
 313     {
 314         Mat_<float> iK = K.inv();
 315         Mat_<float> R1t = Mat_<float>(Rs[0]).t();
 316         Mat_<float> R2t = Mat_<float>(Rs[1]).t();
 317         Mat_<float> m1 = (Mat_<float>(3,1) << ps[0].x, ps[0].y, 1);
 318         Mat_<float> m2 = (Mat_<float>(3,1) << ps[1].x, ps[1].y, 1);
 319         Mat_<float> K1 = R1t*(iK*m1), K2 = R2t*(iK*m2);
 320         Mat_<float> B1 = -R1t*Mat_<float>(ts[0]);
 321         Mat_<float> B2 = -R2t*Mat_<float>(ts[1]);
 322         return findRayIntersection(*K1.ptr<Point3f>(), *B1.ptr<Point3f>(),
 323                                    *K2.ptr<Point3f>(), *B2.ptr<Point3f>());
 324     }
 325 }
 326
 327
 328 static void triangulatePoint_test(void)
 329 {
 330     int i, n = 100;
 331     vector<Point3f> objpt(n), delta1(n), delta2(n);
 332     Mat rvec1(3,1,CV_32F), tvec1(3,1,CV_64F);
 333     Mat rvec2(3,1,CV_32F), tvec2(3,1,CV_64F);
 334     Mat objptmat(objpt), deltamat1(delta1), deltamat2(delta2);
 335     randu(rvec1, Scalar::all(-10), Scalar::all(10));
 336     randu(tvec1, Scalar::all(-10), Scalar::all(10));
 337     randu(rvec2, Scalar::all(-10), Scalar::all(10));
 338     randu(tvec2, Scalar::all(-10), Scalar::all(10));
 339
 340     randu(objptmat, Scalar::all(-10), Scalar::all(10));
 341     double eps = 1e-2;
 342     randu(deltamat1, Scalar::all(-eps), Scalar::all(eps));
 343     randu(deltamat2, Scalar::all(-eps), Scalar::all(eps));
 344     vector<Point2f> imgpt1, imgpt2;
 345     Mat_<float> cameraMatrix(3,3);
 346     double fx = 1000., fy = 1010., cx = 400.5, cy = 300.5;
 347     cameraMatrix << fx, 0, cx, 0, fy, cy, 0, 0, 1;
 348
 349     projectPoints(Mat(objpt)+Mat(delta1), rvec1, tvec1, cameraMatrix, Mat(), imgpt1);
 350     projectPoints(Mat(objpt)+Mat(delta2), rvec2, tvec2, cameraMatrix, Mat(), imgpt2);
 351
 352     vector<Point3f> objptt(n);
 353     vector<Point2f> pts(2);
 354     vector<Mat> Rv(2), tv(2);
 355     Rodrigues(rvec1, Rv[0]);
 356     Rodrigues(rvec2, Rv[1]);
 357     tv[0] = tvec1; tv[1] = tvec2;
 358     for( i = 0; i < n; i++ )
 359     {
 360         pts[0] = imgpt1[i]; pts[1] = imgpt2[i];
 361         objptt[i] = triangulatePoint(pts, Rv, tv, cameraMatrix);
 362     }
 363     double err = norm(Mat(objpt), Mat(objptt), NORM_INF);
 364     CV_Assert(err < 1e-1);
 365 }
 366
 367 typedef pair<int, int> Pair2i;
 368 typedef map<Pair2i, int> Set2i;
 369
 370 struct EqKeypoints
 371 {
 372     EqKeypoints(const vector<int>* _dstart, const Set2i* _pairs)
 373     : dstart(_dstart), pairs(_pairs) {}
 374
 375     bool operator()(const Pair2i& a, const Pair2i& b) const
 376     {
 377         return pairs->find(Pair2i(dstart->at(a.first) + a.second,
 378                                   dstart->at(b.first) + b.second)) != pairs->end();
 379     }
 380
 381     const vector<int>* dstart;
 382     const Set2i* pairs;
 383 };
 384
 385 template<typename _Tp, class _EqPredicate> static
 386 int partition( const std::vector<_Tp>& _vec, std::vector<int>& labels,
 387            _EqPredicate predicate=_EqPredicate())
 388 {
 389     int i, j, N = (int)_vec.size();
 390     const _Tp* vec = &_vec[0];
 391
 392     const int PARENT=0;
 393     const int RANK=1;
 394
 395     std::vector<int> _nodes(N*2);
 396     int (*nodes)[2] = (int(*)[2])&_nodes[0];
 397
 398     // The first O(N) pass: create N single-vertex trees
 399     for(i = 0; i < N; i++)
 400     {
 401         nodes[i][PARENT]=-1;
 402         nodes[i][RANK] = 0;
 403     }
 404
 405     // The main O(N^2) pass: merge connected components
 406     for( i = 0; i < N; i++ )
 407     {
 408         int root = i;
 409
 410         // find root
 411         while( nodes[root][PARENT] >= 0 )
 412             root = nodes[root][PARENT];
 413
 414         for( j = 0; j < N; j++ )
 415         {
 416             if( i == j || !predicate(vec[i], vec[j]))
 417                 continue;
 418             int root2 = j;
 419
 420             while( nodes[root2][PARENT] >= 0 )
 421                 root2 = nodes[root2][PARENT];
 422
 423             if( root2 != root )
 424             {
 425                 // unite both trees
 426                 int rank = nodes[root][RANK], rank2 = nodes[root2][RANK];
 427                 if( rank > rank2 )
 428                     nodes[root2][PARENT] = root;
 429                 else
 430                 {
 431                     nodes[root][PARENT] = root2;
 432                     nodes[root2][RANK] += rank == rank2;
 433                     root = root2;
 434                 }
 435                 CV_Assert( nodes[root][PARENT] < 0 );
 436
 437                 int k = j, parent;
 438
 439                 // compress the path from node2 to root
 440                 while( (parent = nodes[k][PARENT]) >= 0 )
 441                 {
 442                     nodes[k][PARENT] = root;
 443                     k = parent;
 444                 }
 445
 446                 // compress the path from node to root
 447                 k = i;
 448                 while( (parent = nodes[k][PARENT]) >= 0 )
 449                 {
 450                     nodes[k][PARENT] = root;
 451                     k = parent;
 452                 }
 453             }
 454         }
 455     }
 456
 457     // Final O(N) pass: enumerate classes
 458     labels.resize(N);
 459     int nclasses = 0;
 460
 461     for( i = 0; i < N; i++ )
 462     {
 463         int root = i;
 464         while( nodes[root][PARENT] >= 0 )
 465             root = nodes[root][PARENT];
 466         // re-use the rank as the class label
 467         if( nodes[root][RANK] >= 0 )
 468             nodes[root][RANK] = ~nclasses++;
 469         labels[i] = ~nodes[root][RANK];
 470     }
 471
 472     return nclasses;
 473 }
 474
 475 static void build3dmodel( const Ptr<FeatureDetector>& detector,
 476                           const Ptr<DescriptorExtractor>& descriptorExtractor,
 477                           const vector<Point3f>& /*modelBox*/,
 478                           const vector<string>& imageList,
 479                           const vector<Rect>& roiList,
 480                           const vector<Vec6f>& poseList,
 481                           const Mat& cameraMatrix,
 482                           PointModel& model )
 483 {
 484     int progressBarSize = 10;
 485
 486     const double Feps = 5;
 487     const double DescriptorRatio = 0.7;
 488
 489     vector<vector<KeyPoint> > allkeypoints;
 490     vector<int> dstart;
 491     vector<float> alldescriptorsVec;
 492     vector<Vec2i> pairwiseMatches;
 493     vector<Mat> Rs, ts;
 494     int descriptorSize = 0;
 495     Mat descriptorbuf;
 496     Set2i pairs, keypointsIdxMap;
 497
 498     model.points.clear();
 499     model.didx.clear();
 500
 501     dstart.push_back(0);
 502
 503     size_t nimages = imageList.size();
 504     size_t nimagePairs = (nimages - 1)*nimages/2 - nimages;
 505
 506     printf("\nComputing descriptors ");
 507
 508     // 1. find all the keypoints and all the descriptors
 509     for( size_t i = 0; i < nimages; i++ )
 510     {
 511         Mat img = imread(imageList[i], 1), gray;
 512         cvtColor(img, gray, COLOR_BGR2GRAY);
 513
 514         vector<KeyPoint> keypoints;
 515         detector->detect(gray, keypoints);
 516         descriptorExtractor->compute(gray, keypoints, descriptorbuf);
 517         Point2f roiofs = roiList[i].tl();
 518         for( size_t k = 0; k < keypoints.size(); k++ )
 519             keypoints[k].pt += roiofs;
 520         allkeypoints.push_back(keypoints);
 521
 522         Mat buf = descriptorbuf;
 523         if( !buf.isContinuous() || buf.type() != CV_32F )
 524         {
 525             buf.release();
 526             descriptorbuf.convertTo(buf, CV_32F);
 527         }
 528         descriptorSize = buf.cols;
 529
 530         size_t prev = alldescriptorsVec.size();
 531         size_t delta = buf.rows*buf.cols;
 532         alldescriptorsVec.resize(prev + delta);
 533         std::copy(buf.ptr<float>(), buf.ptr<float>() + delta,
 534                   alldescriptorsVec.begin() + prev);
 535         dstart.push_back(dstart.back() + (int)keypoints.size());
 536
 537         Mat R, t;
 538         unpackPose(poseList[i], R, t);
 539         Rs.push_back(R);
 540         ts.push_back(t);
 541
 542         if( (i+1)*progressBarSize/nimages > i*progressBarSize/nimages )
 543         {
 544             putchar('.');
 545             fflush(stdout);
 546         }
 547     }
 548
 549     Mat alldescriptors((int)alldescriptorsVec.size()/descriptorSize, descriptorSize, CV_32F,
 550                        &alldescriptorsVec[0]);
 551
 552     printf("\nOk. total images = %d. total keypoints = %d\n",
 553            (int)nimages, alldescriptors.rows);
 554
 555     printf("\nFinding correspondences ");
 556
 557     int pairsFound = 0;
 558
 559     vector<Point2f> pts_k(2);
 560     vector<Mat> Rs_k(2), ts_k(2);
 561     //namedWindow("img1", 1);
 562     //namedWindow("img2", 1);
 563
 564     // 2. find pairwise correspondences
 565     for( size_t i = 0; i < nimages; i++ )
 566         for( size_t j = i+1; j < nimages; j++ )
 567         {
 568             const vector<KeyPoint>& keypoints1 = allkeypoints[i];
 569             const vector<KeyPoint>& keypoints2 = allkeypoints[j];
 570             Mat descriptors1 = alldescriptors.rowRange(dstart[i], dstart[i+1]);
 571             Mat descriptors2 = alldescriptors.rowRange(dstart[j], dstart[j+1]);
 572
 573             Mat F = getFundamentalMat(Rs[i], ts[i], Rs[j], ts[j], cameraMatrix);
 574
 575             findConstrainedCorrespondences( F, keypoints1, keypoints2,
 576                                             descriptors1, descriptors2,
 577                                             pairwiseMatches, Feps, DescriptorRatio );
 578
 579             //pairsFound += (int)pairwiseMatches.size();
 580
 581             //Mat img1 = imread(format("%s/frame%04d.jpg", model.name.c_str(), (int)i), 1);
 582             //Mat img2 = imread(format("%s/frame%04d.jpg", model.name.c_str(), (int)j), 1);
 583
 584             //double avg_err = 0;
 585             for( size_t k = 0; k < pairwiseMatches.size(); k++ )
 586             {
 587                 int i1 = pairwiseMatches[k][0], i2 = pairwiseMatches[k][1];
 588
 589                 pts_k[0] = keypoints1[i1].pt;
 590                 pts_k[1] = keypoints2[i2].pt;
 591                 Rs_k[0] = Rs[i]; Rs_k[1] = Rs[j];
 592                 ts_k[0] = ts[i]; ts_k[1] = ts[j];
 593                 Point3f objpt = triangulatePoint(pts_k, Rs_k, ts_k, cameraMatrix);
 594
 595                 vector<Point3f> objpts;
 596                 objpts.push_back(objpt);
 597                 vector<Point2f> imgpts1, imgpts2;
 598                 projectPoints(Mat(objpts), Rs_k[0], ts_k[0], cameraMatrix, Mat(), imgpts1);
 599                 projectPoints(Mat(objpts), Rs_k[1], ts_k[1], cameraMatrix, Mat(), imgpts2);
 600
 601                 double e1 = norm(imgpts1[0] - keypoints1[i1].pt);
 602                 double e2 = norm(imgpts2[0] - keypoints2[i2].pt);
 603                 if( e1 + e2 > 5 )
 604                     continue;
 605
 606                 pairsFound++;
 607
 608                 //model.points.push_back(objpt);
 609                 pairs[Pair2i(i1+dstart[i], i2+dstart[j])] = 1;
 610                 pairs[Pair2i(i2+dstart[j], i1+dstart[i])] = 1;
 611                 keypointsIdxMap[Pair2i((int)i,i1)] = 1;
 612                 keypointsIdxMap[Pair2i((int)j,i2)] = 1;
 613                 //CV_Assert(e1 < 5 && e2 < 5);
 614                 //Scalar color(rand()%256,rand()%256, rand()%256);
 615                 //circle(img1, keypoints1[i1].pt, 2, color, -1, CV_AA);
 616                 //circle(img2, keypoints2[i2].pt, 2, color, -1, CV_AA);
 617             }
 618             //printf("avg err = %g\n", pairwiseMatches.size() ? avg_err/(2*pairwiseMatches.size()) : 0.);
 619             //imshow("img1", img1);
 620             //imshow("img2", img2);
 621             //waitKey();
 622
 623             if( (i+1)*progressBarSize/nimagePairs > i*progressBarSize/nimagePairs )
 624             {
 625                 putchar('.');
 626                 fflush(stdout);
 627             }
 628         }
 629
 630     printf("\nOk. Total pairs = %d\n", pairsFound );
 631
 632     // 3. build the keypoint clusters
 633     vector<Pair2i> keypointsIdx;
 634     Set2i::iterator kpidx_it = keypointsIdxMap.begin(), kpidx_end = keypointsIdxMap.end();
 635
 636     for( ; kpidx_it != kpidx_end; ++kpidx_it )
 637         keypointsIdx.push_back(kpidx_it->first);
 638
 639     printf("\nClustering correspondences ");
 640
 641     vector<int> labels;
 642     int nclasses = partition( keypointsIdx, labels, EqKeypoints(&dstart, &pairs) );
 643
 644     printf("\nOk. Total classes (i.e. 3d points) = %d\n", nclasses );
 645
 646     model.descriptors.create((int)keypointsIdx.size(), descriptorSize, CV_32F);
 647     model.didx.resize(nclasses);
 648     model.points.resize(nclasses);
 649
 650     vector<vector<Pair2i> > clusters(nclasses);
 651     for( size_t i = 0; i < keypointsIdx.size(); i++ )
 652         clusters[labels[i]].push_back(keypointsIdx[i]);
 653
 654     // 4. now compute 3D points corresponding to each cluster and fill in the model data
 655     printf("\nComputing 3D coordinates ");
 656
 657     int globalDIdx = 0;
 658     for( int k = 0; k < nclasses; k++ )
 659     {
 660         int i, n = (int)clusters[k].size();
 661         pts_k.resize(n);
 662         Rs_k.resize(n);
 663         ts_k.resize(n);
 664         model.didx[k].resize(n);
 665         for( i = 0; i < n; i++ )
 666         {
 667             int imgidx = clusters[k][i].first, ptidx = clusters[k][i].second;
 668             Mat dstrow = model.descriptors.row(globalDIdx);
 669             alldescriptors.row(dstart[imgidx] + ptidx).copyTo(dstrow);
 670
 671             model.didx[k][i] = globalDIdx++;
 672             pts_k[i] = allkeypoints[imgidx][ptidx].pt;
 673             Rs_k[i] = Rs[imgidx];
 674             ts_k[i] = ts[imgidx];
 675         }
 676         Point3f objpt = triangulatePoint(pts_k, Rs_k, ts_k, cameraMatrix);
 677         model.points[k] = objpt;
 678
 679         if( (i+1)*progressBarSize/nclasses > i*progressBarSize/nclasses )
 680         {
 681             putchar('.');
 682             fflush(stdout);
 683         }
 684     }
 685
 686     Mat img(768, 1024, CV_8UC3);
 687     vector<Point2f> imagePoints;
 688     namedWindow("Test", 1);
 689
 690     // visualize the cloud
 691     for( size_t i = 0; i < nimages; i++ )
 692     {
 693         img = imread(format("%s/frame%04d.jpg", model.name.c_str(), (int)i), 1);
 694         projectPoints(Mat(model.points), Rs[i], ts[i], cameraMatrix, Mat(), imagePoints);
 695
 696         for( int k = 0; k < (int)imagePoints.size(); k++ )
 697             circle(img, imagePoints[k], 2, Scalar(0,255,0), -1, LINE_AA, 0);
 698
 699         imshow("Test", img);
 700         int c = waitKey();
 701         if( c == 'q' || c == 'Q' )
 702             break;
 703     }
 704 }
 705
 706
 707 int main(int argc, char** argv)
 708 {
 709     const char* intrinsicsFilename = 0;
 710     const char* modelName = 0;
 711     const char* detectorName = "SURF";
 712     const char* descriptorExtractorName = "SURF";
 713
 714     vector<Point3f> modelBox;
 715     vector<string>  imageList;
 716     vector<Rect>    roiList;
 717     vector<Vec6f>   poseList;
 718
 719     if(argc < 3)
 720     {
 721         help();
 722         return -1;
 723     }
 724
 725     for( int i = 1; i < argc; i++ )
 726     {
 727         if( strcmp(argv[i], "-i") == 0 )
 728             intrinsicsFilename = argv[++i];
 729         else if( strcmp(argv[i], "-m") == 0 )
 730             modelName = argv[++i];
 731         else if( strcmp(argv[i], "-d") == 0 )
 732             detectorName = argv[++i];
 733         else if( strcmp(argv[i], "-de") == 0 )
 734             descriptorExtractorName = argv[++i];
 735         else
 736         {
 737             help();
 738             printf("Incorrect option\n");
 739             return -1;
 740         }
 741     }
 742
 743     if( !intrinsicsFilename || !modelName )
 744     {
 745         printf("Some of the required parameters are missing\n");
 746         help();
 747         return -1;
 748     }
 749
 750     triangulatePoint_test();
 751
 752     Mat cameraMatrix, distCoeffs;
 753     Size calibratedImageSize;
 754     readCameraMatrix(intrinsicsFilename, cameraMatrix, distCoeffs, calibratedImageSize);
 755
 756     Ptr<FeatureDetector> detector = FeatureDetector::create(detectorName);
 757     Ptr<DescriptorExtractor> descriptorExtractor = DescriptorExtractor::create(descriptorExtractorName);
 758
 759     string modelIndexFilename = format("%s_segm/frame_index.yml", modelName);
 760     if(!readModelViews( modelIndexFilename, modelBox, imageList, roiList, poseList))
 761     {
 762         printf("Can not read the model. Check the parameters and the working directory\n");
 763         help();
 764         return -1;
 765     }
 766
 767     PointModel model;
 768     model.name = modelName;
 769     build3dmodel( detector, descriptorExtractor, modelBox,
 770                   imageList, roiList, poseList, cameraMatrix, model );
 771     string outputModelName = format("%s_model.yml.gz", modelName);
 772
 773
 774     printf("\nDone! Now saving the model ...\n");
 775     writeModel(outputModelName, modelName, model);
 776
 777     return 0;
 778 }