ODetect3D.cc
Go to the documentation of this file.
00001 
00013 #include <blort/Recognizer3D/ODetect3D.hh>
00014 #include <ros/console.h>
00015 #include <blort/TomGine/tgPose.h>
00016 #include <blort/blort/pal_util.h>
00017 #include <blort/Recognizer3D/Recognizer3D.h>
00018 
00019 #include <opencv2/core/core.hpp>
00020 #include <opencv2/calib3d/calib3d.hpp>
00021 #include <opencv2/gpu/gpu.hpp>
00022 
00023 namespace P 
00024 {
00025 
00026 
00027     /********************** ODetect3D ************************
00028      * Constructor/Destructor
00029      */
00030     ODetect3D::ODetect3D()
00031         : dbg(0)
00032     {
00033         nn_far_enough_threshold = (Def::DO_MIN_DESC_DIST*Def::DO_MIN_DESC_DIST)/100.0;
00034         n_points_to_match = 4;
00035 
00036         cameraMatrix = cvCreateMat( 3, 3, CV_32F );
00037         distCoeffs = cvCreateMat( 4, 1, CV_32F );
00038 
00039         matcherSize = 4096;
00040         matcher = new SiftMatchGPU(4096);
00041         matcher->VerifyContextGL();
00042 
00043         //we assume undistort images!!!!
00044         cvmSet( distCoeffs, 0, 0, 0. );
00045         cvmSet( distCoeffs, 1, 0, 0. );
00046         cvmSet( distCoeffs, 2, 0, 0. );
00047         cvmSet( distCoeffs, 3, 0, 0. );
00048     }
00049 
00050     ODetect3D::~ODetect3D()
00051     {
00052         delete matcher;
00053         if (cameraMatrix!=0) cvReleaseMat(&cameraMatrix);
00054         if (distCoeffs!=0) cvReleaseMat(&distCoeffs);
00055         DeletePairs(matches);
00056     }
00057 
00061     void ODetect3D::DeletePairs(Array<KeyClusterPair*> &matches)
00062     {
00063         for (unsigned i=0; i<matches.Size(); i++)
00064             delete matches[i];
00065         matches.Clear();
00066     }
00067 
00068 
00073     void ODetect3D::MatchKeypoints2(Array<KeypointDescriptor *> &keys, Array<CodebookEntry *> &cb, Array<KeyClusterPair* > &matches)
00074     {
00075         double ticksBefore = cv::getTickCount();
00076         float dist, dist1, dist2;
00077         unsigned idx = 0;
00078         KeypointDescriptor *key;
00079 
00080         for (unsigned i=0; i<keys.Size(); i++)
00081         {
00082             if (keys[i]->id != 0)
00083             {
00084                 dist1 = dist2 = FLT_MAX;
00085                 key = (KeypointDescriptor*)keys[i];
00086                 for (unsigned j=0; j<cb.Size(); j++)
00087                 {
00088                     if(key->GetType() == cb[j]->model->GetType())
00089                     {
00090                         dist = key->DistSqr(key->GetVec(), cb[j]->model->GetVec(), key->GetSize());
00091                         if (dist < dist1)
00092                         {
00093                             dist2=dist1;
00094                             dist1=dist;
00095                             idx=j;
00096                         }
00097                         else
00098                         {
00099                             if (dist<dist2) dist2=dist;
00100                         }
00101                     }
00102                 }
00103                 if(double(dist1)/dist2 < nn_far_enough_threshold)
00104                     matches.PushBack(new KeyClusterPair(key, cb[idx]));
00105             }
00106         }
00107         ROS_INFO("odetect3d::matchkeypoints() run: %f ms",
00108                  1000*(cv::getTickCount() - ticksBefore)/cv::getTickFrequency());
00109     }
00110 
00115     void ODetect3D::MatchKeypoints(Array<KeypointDescriptor *> &keys, Array<CodebookEntry *> &cb, Array<KeyClusterPair* > &matches)
00116     {
00117         float dist, dist1;
00118         unsigned idx = 0;
00119         KeypointDescriptor *key;
00120 
00121         for (unsigned i=0; i<keys.Size(); i++)
00122         {
00123             if (keys[i]->id != 0)                //upppps HACK!!!!
00124             {
00125                 dist1=FLT_MAX;
00126                 key = (KeypointDescriptor*)keys[i];
00127                 for (unsigned j=0; j<cb.Size(); j++)
00128                 {
00129                     dist = key->Match(cb[j]->model);
00130                     if (dist < dist1)
00131                     {
00132                         dist1=dist;
00133                         idx=j;
00134                     }
00135                 }
00136                 if (!IsEqual(dist1,FLT_MAX))
00137                     matches.PushBack(new KeyClusterPair(key, cb[idx], dist1));
00138             }
00139         }
00140     }
00141 
00145     void ODetect3D::MatchKeypointsGPU(Array<KeypointDescriptor *> &keys, Array<CodebookEntry *> &cb, Array<KeyClusterPair* > &matches)
00146     {
00147       double ticksBefore = cv::getTickCount();
00148 
00149       if (keys.Size()<4)
00150         return;
00151 
00152       int num;
00153       int (*match_buf)[2] = new int[(int)keys.Size()][2];
00154 
00155       if (matcherSize < (int)keys.Size()) matcher->SetMaxSift((int)keys.Size());
00156       if (matcherSize < (int)cb.Size()) matcher->SetMaxSift((int)cb.Size());
00157 
00158       P::Array<float> desc1(cb.Size()*128);
00159       P::Array<float> desc2(keys.Size()*128);
00160 
00161       for (unsigned i=0; i<cb.Size(); i++)
00162         CopyVec(cb[i]->model->vec, &(desc1[i*128]), cb[i]->model->GetSize());
00163       for (unsigned i=0; i<keys.Size(); i++)
00164         CopyVec(keys[i]->vec, &(desc2[i*128]), keys[i]->GetSize());
00165 
00166       matcher->SetDescriptors(0, (int)cb.Size(), &desc1[0]); //codebook
00167       matcher->SetDescriptors(1, (int)keys.Size(), &desc2[0]); //keys
00168 
00169       num = matcher->GetSiftMatch(keys.Size(), match_buf);
00170 
00171       for (unsigned i=0; i<(unsigned)num; i++)
00172         matches.PushBack(new KeyClusterPair(keys[match_buf[i][1]], cb[match_buf[i][0]], 0));
00173 
00174       delete[] match_buf;
00175 
00176       ROS_INFO("\tODetect3D::MatchKeypointsGPU() time: %f ms",
00177                1000*(cv::getTickCount() - ticksBefore)/cv::getTickFrequency());
00178     }
00179 
00180 
00184     void ODetect3D::GetRandIdx(unsigned size, unsigned num, P::Array<unsigned> &idx)
00185     {
00186         unsigned temp;
00187         idx.Clear();
00188         for (unsigned i=0; i<num; i++)
00189         {
00190             do{
00191                 temp = rand()%size;
00192             }while(idx.Contains(temp));
00193             idx.PushBack(temp);
00194         }
00195     }
00196 
00200     void ODetect3D::GetInlier(Array<KeyClusterPair*> &matches, PoseCv &pose, int &inl)
00201     {
00202         inl=0;
00203         Vector2 p;
00204         CodebookEntry *cbe;
00205         KeypointDescriptor *k;
00206         CvMat *pos = cvCreateMat( 3, 1, CV_32F );
00207 
00208         bool stop=false;
00209         for (unsigned i=0; i<matches.Size() && !stop; i++)
00210         {
00211             cbe = matches[i]->c;
00212             k = matches[i]->k;
00213 
00214             for (unsigned j=0; j<cbe->Size(); j++)
00215             {
00216                 cvMatMulAdd( pose.R, cbe->occurrences[j]->pos, pose.t, pos );
00217                 ProjectPoint2Image(pos->data.fl[0], pos->data.fl[1], pos->data.fl[2], cameraMatrix, p.x, p.y);
00218 
00219                 if (Distance(p,k->p) < Def::DO_RANSAC_INL_DIST)
00220                 {
00221                     if (pos->data.fl[2]<0)
00222                     {
00223                         inl=0;
00224                         stop=true;
00225                     }
00226                     else
00227                     {
00228                         inl++;
00229                     }
00230                     break;
00231                 }
00232             }
00233         }
00234 
00235         cvReleaseMat(&pos);
00236     }
00237 
00238     void ODetect3D::FitModelRANSAC_GPU(Array<KeyClusterPair*> &matches, PoseCv &pose, unsigned &numInl)
00239     {
00240       cv::Mat camMatrix(cameraMatrix);
00241       cv::Mat distortionCoeff(distCoeffs);
00242       cv::Mat modelPoints, imgPoints;
00243       cv::Mat rvec, tvec;
00244 
00245       modelPoints.create(1, matches.Size(), CV_32FC3);
00246       imgPoints.create(1, matches.Size(), CV_32FC2);
00247 
00248       KeyClusterPair* kp;
00249 
00250       double ticksBefore = cv::getTickCount();
00251       for (unsigned i=0; i<matches.Size(); i++)
00252       {
00253         kp = matches[i];
00254         CvMat *pos = kp->c->occurrences[rand()%kp->c->Size()]->pos;
00255 
00256         cv::Point3f p3f;
00257         p3f.x = pos->data.fl[0]; p3f.y = pos->data.fl[1]; p3f.z = pos->data.fl[2];
00258 
00259         modelPoints.at<cv::Point3f>(0, i) = p3f;
00260 
00261         cv::Point2f p2f;
00262         p2f.x = kp->k->p.x; p2f.y = kp->k->p.y;
00263         imgPoints.at<cv::Point2f>(0, i) = p2f;
00264       }
00265 
00266       bool useGPU = false;
00267 
00268       if ( useGPU )
00269       {
00270         std::vector<int> inliersIdx;
00271         //reemh3-2m timing with:
00272         //rosbag play pringles_single_stereo_image.bag --loop
00273         //roslaunch blort_ros tracking.launch
00274         //time: 789 ms
00275         cv::gpu::solvePnPRansac(modelPoints, imgPoints, camMatrix, distortionCoeff, rvec, tvec, false, Def::DO_MAX_RANSAC_TRIALS, Def::DO_RANSAC_INL_DIST, 100, &inliersIdx);
00276         numInl = inliersIdx.size();
00277         ROS_INFO("\tODetect3D::Detect: FitModelRANSAC_GPU(OpenCV::gpu) time: %.01f ms", 1000*(cv::getTickCount() - ticksBefore)/cv::getTickFrequency());
00278       }
00279       else
00280       {
00281         cv::Mat inliersIdx;
00282         //reemh3-2m timing with:
00283         //rosbag play pringles_single_stereo_image.bag --loop
00284         //roslaunch blort_ros tracking.launch
00285         //time: 706 ms
00286         cv::solvePnPRansac(modelPoints, imgPoints, camMatrix, distortionCoeff, rvec, tvec, false, Def::DO_MAX_RANSAC_TRIALS, Def::DO_RANSAC_INL_DIST, 100, inliersIdx);
00287         numInl = inliersIdx.rows;
00288         ROS_INFO("\tODetect3D::Detect: FitModelRANSAC_GPU(OpenCV) time: %.01f ms", 1000*(cv::getTickCount() - ticksBefore)/cv::getTickFrequency());
00289       }
00290 
00291       ROS_INFO_STREAM("\tInliers: " << numInl);
00292 
00293       cv::Mat rotation;
00294       cv::Rodrigues(rvec, rotation);      
00295       ROS_INFO_STREAM("Rotation (depth: " << rotation.depth() << ") :\n" << rotation << "\n");
00296 
00297       if ( rotation.depth() == CV_32F )
00298       {
00299         cvmSet(pose.R, 0, 0, rotation.at<float>(0,0)); cvmSet(pose.R, 0, 1, rotation.at<float>(0,1)); cvmSet(pose.R, 0, 2, rotation.at<float>(0,2));
00300         cvmSet(pose.R, 1, 0, rotation.at<float>(1,0)); cvmSet(pose.R, 1, 1, rotation.at<float>(1,1)); cvmSet(pose.R, 1, 2, rotation.at<float>(1,2));
00301         cvmSet(pose.R, 2, 0, rotation.at<float>(2,0)); cvmSet(pose.R, 2, 1, rotation.at<float>(2,1)); cvmSet(pose.R, 2, 2, rotation.at<float>(2,2));
00302       }
00303       else
00304       {
00305         cvmSet(pose.R, 0, 0, rotation.at<double>(0,0)); cvmSet(pose.R, 0, 1, rotation.at<double>(0,1)); cvmSet(pose.R, 0, 2, rotation.at<double>(0,2));
00306         cvmSet(pose.R, 1, 0, rotation.at<double>(1,0)); cvmSet(pose.R, 1, 1, rotation.at<double>(1,1)); cvmSet(pose.R, 1, 2, rotation.at<double>(1,2));
00307         cvmSet(pose.R, 2, 0, rotation.at<double>(2,0)); cvmSet(pose.R, 2, 1, rotation.at<double>(2,1)); cvmSet(pose.R, 2, 2, rotation.at<double>(2,2));
00308       }
00309 
00310       if ( tvec.depth() == CV_32F )
00311       {
00312         cvmSet(pose.t, 0, 0, tvec.at<float>(0,0));
00313         cvmSet(pose.t, 1, 0, tvec.at<float>(1,0));
00314         cvmSet(pose.t, 2, 0, tvec.at<float>(2,0));
00315       }
00316       else
00317       {
00318         cvmSet(pose.t, 0, 0, tvec.at<double>(0,0));
00319         cvmSet(pose.t, 1, 0, tvec.at<double>(1,0));
00320         cvmSet(pose.t, 2, 0, tvec.at<double>(2,0));
00321       }
00322     }
00323 
00327     void ODetect3D::FitModelRANSAC(Array<KeyClusterPair*> &matches,
00328                                    PoseCv &pose, unsigned &numInl)
00329     {     
00330       //reemh3-2m timing with:
00331       //rosbag play pringles_single_stereo_image.bag --loop
00332       //roslaunch blort_ros tracking.launch
00333       //time: 408 ms
00334 
00335       if (matches.Size()<n_points_to_match)
00336       {
00337         ROS_WARN("matches.Size(): %d while n_points_to_match is %d",
00338                  matches.Size(), n_points_to_match);
00339         return;
00340       }
00341 
00342       double eps = n_points_to_match/(double)matches.Size();
00343       int inl, inls = 0;
00344       Array<unsigned> idx, idx2;
00345       CvMat *modelPoints = cvCreateMat( n_points_to_match, 3, CV_32F );
00346       CvMat *imgPoints = cvCreateMat( n_points_to_match, 2, CV_32F );
00347       srand(time(NULL));
00348       KeyClusterPair* kp;
00349       PoseCv tempPose;
00350       CvMat *rod = cvCreateMat(3,1,CV_32F);
00351 
00352       double ticksBefore = cv::getTickCount();
00353       int k, iterations = 0;
00354       for(k=0; (pow(1. - pow(eps,n_points_to_match),k) >= Def::DO_RANSAC_ETA0 &&
00355                 k<Def::DO_MAX_RANSAC_TRIALS); ++k)
00356       {
00357         GetRandIdx(matches.Size(), n_points_to_match, idx);
00358 
00359         for (unsigned i=0; i<n_points_to_match; i++)
00360         {
00361           kp = matches[idx[i]];
00362           CvMat *pos = kp->c->occurrences[rand()%kp->c->Size()]->pos;
00363           cvmSet( modelPoints, i, 0, pos->data.fl[0] );
00364           cvmSet( modelPoints, i, 1, pos->data.fl[1] );
00365           cvmSet( modelPoints, i, 2, pos->data.fl[2] );
00366           cvmSet( imgPoints, i, 0, kp->k->p.x);
00367           cvmSet( imgPoints, i, 1, kp->k->p.y);
00368         }
00369 
00370         cvFindExtrinsicCameraParams2(modelPoints, imgPoints,
00371                                      cameraMatrix, distCoeffs, rod, tempPose.t);
00372         cvRodrigues2(rod,tempPose.R);
00373         GetInlier(matches,tempPose,inl);
00374 
00375         //if it is the transfom providing most inliers save it
00376         if (inl > inls)
00377         {
00378           inls = inl;
00379           eps = (double)inls / (double)matches.Size();
00380           CopyPoseCv(tempPose, pose);
00381         }
00382         ++iterations;
00383       }
00384 
00385       numInl=inls;
00386 
00387       ROS_INFO_STREAM("Inliers: " << numInl << " iterations: " << iterations);
00388 
00389       cvReleaseMat(&modelPoints);
00390       cvReleaseMat(&imgPoints);
00391       cvReleaseMat(&rod);
00392 
00393       ROS_INFO("\tODetect3D::Detect: FitModelRANSAC time: %.01f ms", 1000*(cv::getTickCount() - ticksBefore)/cv::getTickFrequency());
00394     }
00395 
00399     bool ODetect3D::GetBestCorner(PoseCv &pose, KeypointDescriptor *k, CodebookEntry *cbe, Point3D &pos, double &minDist)
00400     {
00401         Vector2 p;
00402 
00403         double dist;
00404         minDist = DBL_MAX;
00405         CvMat *tempPos = cvCreateMat( 3, 1, CV_32F );
00406 
00407 
00408         for (unsigned j=0; j<cbe->Size(); j++)
00409         {
00410             cvMatMulAdd( pose.R, cbe->occurrences[j]->pos, pose.t, tempPos );
00411             ProjectPoint2Image(tempPos->data.fl[0], tempPos->data.fl[1], tempPos->data.fl[2], cameraMatrix, p.x, p.y);
00412 
00413             dist = Distance(p,k->p);
00414             if (dist < minDist)
00415             {
00416                 minDist = dist;
00417                 pos.x = cbe->occurrences[j]->pos->data.fl[0];
00418                 pos.y = cbe->occurrences[j]->pos->data.fl[1];
00419                 pos.z = cbe->occurrences[j]->pos->data.fl[2];
00420             }
00421         }
00422 
00423         cvReleaseMat(&tempPos);
00424 
00425         if (minDist < Def::DO_RANSAC_INL_DIST)
00426             return true;
00427 
00428         return false;
00429     }
00430 
00434     void ODetect3D::RefinePoseLS(Array<KeyClusterPair*> &matches, PoseCv &pose, unsigned &inl, double &err)
00435     {
00436         mat3 cR;
00437         vec3 ct;
00438         cR[0] = cvmGet(pose.R,0,0); cR[3] = cvmGet(pose.R,0,1); cR[6] = cvmGet(pose.R,0,2);
00439         cR[1] = cvmGet(pose.R,1,0); cR[4] = cvmGet(pose.R,1,1); cR[7] = cvmGet(pose.R,1,2);
00440         cR[2] = cvmGet(pose.R,2,0); cR[5] = cvmGet(pose.R,2,1); cR[8] = cvmGet(pose.R,2,2);
00441 
00442         ct.x = cvmGet(pose.t,0,0);
00443         ct.y = cvmGet(pose.t,1,0);
00444         ct.z = cvmGet(pose.t,2,0);
00445 
00446         ROS_INFO("\t\tct and cR in RefinePoseLS:");
00447         ROS_INFO("\t\t [%.04f  %.04f  %.04f]", ct.x, ct.y, ct.z);
00448         ROS_INFO("\t\t [%.04f  %.04f  %.04f]", cR[0], cR[3], cR[6]);
00449         ROS_INFO("\t\t [%.04f  %.04f  %.04f]", cR[1], cR[4], cR[7]);
00450         ROS_INFO("\t\t [%.04f  %.04f  %.04f]", cR[2], cR[5], cR[8]);
00451 
00452         double dist, minz=DBL_MAX, maxz=DBL_MIN;
00453         Array<Point3D> modelPoints;
00454         Array<Point2D> imgPoints;
00455         CvMat modelPointsCv;
00456         CvMat imgPointsCv;
00457         CvMat *rod;
00458         err=0;
00459         PoseCv tmpPose;
00460         Vector3 t, p;
00461 
00462         inlier.Clear();
00463 
00464         modelPoints.PushBack(Point3D());
00465         imgPoints.PushBack(Point2D());
00466         for (unsigned i=0; i<matches.Size(); i++)
00467         {
00468             if (GetBestCorner(pose, matches[i]->k, matches[i]->c, modelPoints.Last(), dist))
00469             {
00470                 err+=dist;
00471                 imgPoints.Last().x = (float)matches[i]->k->p.x;
00472                 imgPoints.Last().y = (float)matches[i]->k->p.y;
00473 
00474                 if (modelPoints.Last().z>maxz)
00475                     maxz=modelPoints.Last().z;
00476                 else if (modelPoints.Last().z<minz)
00477                     minz=modelPoints.Last().z;
00478 
00479                 modelPoints.PushBack(Point3D());
00480                 imgPoints.PushBack(Point2D());
00481                 inlier.PushBack(matches[i]->k);      //just store pointers to inlier for drawing....
00482             }
00483         }
00484         modelPoints.EraseLast();
00485         imgPoints.EraseLast();
00486 
00487         CopyPoseCv(pose,tmpPose);
00488         if (modelPoints.Size()>4)
00489         {
00490             err/=(double)modelPoints.Size();
00491             rod = cvCreateMat(3,1,CV_32F);
00492             cvInitMatHeader( &modelPointsCv, modelPoints.Size(), 3, CV_32F, &modelPoints[0].x );
00493             cvInitMatHeader( &imgPointsCv, imgPoints.Size(), 2, CV_32F, &imgPoints[0].x );
00494 
00495             cvFindExtrinsicCameraParams2(&modelPointsCv, &imgPointsCv, cameraMatrix, distCoeffs, rod, pose.t);
00496             cvRodrigues2(rod,pose.R);
00497 
00498             cvReleaseMat(&rod);
00499         }
00500 
00501         //up to now we have no keypoint normals, i.e. no visibility
00502         //so for now if least squares fails just copy back the RANSAC pose!!!
00503         p = Vector3(cvmGet(pose.t,0,0),cvmGet(pose.t,1,0),cvmGet(pose.t,2,0));
00504         t = Vector3(cvmGet(tmpPose.t,0,0),cvmGet(tmpPose.t,1,0),cvmGet(tmpPose.t,2,0));
00505         if (Distance(p,t) < (maxz-minz) * Def::DISTANCE_RATIO)
00506         {
00507             inl = modelPoints.Size();
00508         }else
00509         {
00510             CopyPoseCv(tmpPose,pose);
00511         }
00512     }
00513 
00517     void ODetect3D::ComputeConfidence(Array<KeypointDescriptor *> &keys, unsigned &numInl, Object3D &object)
00518     {
00519         Vector2 p;
00520         Array<Vector2> cs;
00521         CodebookEntry *cbe;
00522 
00523         if (numInl>=4)
00524         {
00525             unsigned numKeys=0;
00526             CvMat *pos = cvCreateMat( 3, 1, CV_32F );
00527             for (unsigned i=0; i<object.codebook.Size(); i++)
00528             {
00529                 cbe=object.codebook[i];
00530                 for (unsigned j=0; j<cbe->occurrences.Size(); j++)
00531                 {
00532                     cvMatMulAdd( object.pose.R, cbe->occurrences[j]->pos, object.pose.t, pos );
00533                     ProjectPoint2Image(pos->data.fl[0],pos->data.fl[1],pos->data.fl[2], cameraMatrix, p.x, p.y);
00534                     cs.PushBack(p);
00535                 }
00536             }
00537             cvReleaseMat(&pos);
00538             ConvexHull(cs, object.contour.v);
00539 
00540             for (unsigned i=0; i<keys.Size(); i++)
00541                 if (object.contour.Inside(keys[i]->p))
00542                     numKeys++;
00543 
00544             if (numKeys>4)
00545             {
00546                 object.conf = (double)numInl / (double)numKeys;
00547                 return;
00548             }
00549         }
00550 
00551         object.conf = 0.;
00552     }
00553 
00554 
00555 
00556     /******************************** PUBLIC **************************/
00560     bool ODetect3D::Detect(Array<KeypointDescriptor *> &keys, Object3D &object)
00561     {
00562 #ifdef DEBUG
00563         struct timespec start0, end0, start1, end1, start2, end2, start3, end3;
00564         clock_gettime(CLOCK_REALTIME, &start1);
00565         clock_gettime(CLOCK_REALTIME, &start0);
00566 #endif
00567 
00568         unsigned numInl=0;
00569         DeletePairs(matches); // make sure the vector is empty, we'll keep them for later to draw
00570 
00571         if (Def::DO_MATCHER==2)              // match keypoints with codebook  gpu
00572             MatchKeypointsGPU(keys, object.codebook, matches);
00573         else if (Def::DO_MATCHER==1)         // threshold matches
00574             MatchKeypoints(keys, object.codebook, matches);
00575         else                                 // use second nearest neighbour
00576             MatchKeypoints2(keys, object.codebook, matches);
00577 
00578         for (unsigned i=0; i<matches.Size(); i++)
00579             KeypointDescriptor::Draw(dbg,*matches[i]->k,CV_RGB(255,255,0));
00580 
00581 #ifdef DEBUG
00582         clock_gettime(CLOCK_REALTIME, &end0);
00583         if (dbg!=0)
00584             for (unsigned i=0; i<matches.Size(); i++)
00585                 KeypointDescriptor::Draw(dbg,*matches[i]->k,CV_RGB(255,0,0));
00586         clock_gettime(CLOCK_REALTIME, &start2);
00587 #endif
00588 
00589         FitModelRANSAC(matches, object.pose, numInl); //This is the fastest
00590         //FitModelRANSAC_GPU(matches, object.pose, numInl);
00591 
00592 #ifdef DEBUG
00593         clock_gettime(CLOCK_REALTIME, &end2);
00594         clock_gettime(CLOCK_REALTIME, &start3);
00595 #endif
00596 
00597         double ticksBefore = cv::getTickCount();
00598         RefinePoseLS(matches, object.pose, numInl, object.err);     // least squares pose
00599         ROS_INFO("\tODetect3D::Detect: RefinePoseLS time: %.01f ms", 1000*(cv::getTickCount() - ticksBefore)/cv::getTickFrequency());
00600 
00601         ComputeConfidence(keys, numInl, object);
00602 
00603 #ifdef DEBUG
00604         clock_gettime(CLOCK_REALTIME, &end3);
00605         clock_gettime(CLOCK_REALTIME, &end1);
00606         cout<<"id="<<object.id<<", conf="<<object.conf<<", err="<<object.err<<", numInl="<<numInl
00607                 <<", cbSize="<<object.codebook.Size()<<endl;
00608         cout<<"Pose translation: ["<<cvmGet(object.pose.t,0,0)<<" "<<cvmGet(object.pose.t,1,0)<<" "<<cvmGet(object.pose.t,2,0)<<"]"<<endl;
00609         cout<<"Time for matching keypoints [s]: "<<P::timespec_diff(&end0, &start0)<<endl;
00610         cout<<"Time for RANSAC [s]: "<<P::timespec_diff(&end2, &start2)<<endl;
00611         cout<<"Time for least squares pose [s]: "<<P::timespec_diff(&end3, &start3)<<endl;
00612         cout<<"Time for object detection (matching and RANSAC) [s]: "<<P::timespec_diff(&end1, &start1)<<endl;
00613 #endif
00614 
00615         if (numInl>=4)
00616             return true;
00617 
00618         object.conf=0.;
00619         object.err=DBL_MAX;
00620 
00621         return false;
00622     }
00623 
00630     void ODetect3D::SetCameraParameter(CvMat *C)
00631     {
00632         cvCopy(C,cameraMatrix);
00633     }
00634 
00640     void ODetect3D::DrawInlier(IplImage *img, CvScalar col)
00641     {
00642         for (unsigned i=0; i<inlier.Size(); i++)
00643             KeypointDescriptor::Draw(img,*inlier[i], col);
00644     }
00645 
00646 }
00647 


blort
Author(s): Michael Zillich, Thomas Mörwald, Johann Prankl, Andreas Richtsfeld, Bence Magyar (ROS version)
autogenerated on Thu Jan 2 2014 11:38:25