ndt_frame.hpp
Go to the documentation of this file.
00001 //#include <sba/sba.h>
00002 #include <Eigen/SVD>
00003 #include <Eigen/LU>
00004 #include <iostream>
00005 #include <limits>
00006 #include "opencv2/core/core.hpp"
00007 #include "opencv2/features2d/features2d.hpp"
00008 #include "opencv2/highgui/highgui.hpp"
00009 
00010 namespace ndt_feature_reg
00011 {
00012 
00013 template <typename PointSource, typename PointTarget>
00014 PoseEstimator<PointSource,PointTarget>::PoseEstimator(int NRansac,
00015         double maxidx, double maxidd)
00016 {
00017     numRansac = NRansac;
00018     maxInlierXDist2 = maxidx*maxidx;
00019     maxInlierDDist2 = maxidd*maxidd;
00020     rot.setIdentity();
00021     trans.setZero();
00022 
00023     // matcher
00024     matcher = new cv::BFMatcher(cv::NORM_L2);
00025     wx = 92;
00026     wy = 48;
00027     windowed = false;
00028     projectMatches = true;
00029 
00030     maxDist = std::numeric_limits<double>::max();
00031     minDist = -1.;
00032 }
00033 
00034 template <typename PointSource, typename PointTarget>
00035 void
00036 PoseEstimator<PointSource,PointTarget>::matchFrames(const NDTFrame<PointSource>& f0, const NDTFrame<PointTarget>& f1, std::vector<cv::DMatch>& fwd_matches)
00037 {
00038     cv::Mat mask;
00039     if (windowed)
00040         mask = cv::windowedMatchingMask(f0.kpts, f1.kpts, wx, wy);
00041 
00042     //require at least 3 kpts each
00043     if(f0.kpts.size() > 3 && f1.kpts.size() > 3) 
00044     {
00045         matcher->match(f0.dtors, f1.dtors, fwd_matches, mask);
00046     }
00047 }
00048 
00049 
00050 template <typename PointSource, typename PointTarget>
00051 size_t PoseEstimator<PointSource,PointTarget>::estimate(const NDTFrame<PointSource> &f0, const NDTFrame<PointTarget> &f1)
00052 {
00053     // set up match lists
00054     matches.clear();
00055     inliers.clear();
00056 
00057 //     std::cout<<"N KPTS: "<<f0.kpts.size()<<" "<<f1.kpts.size()<<std::endl;
00058 //     std::cout<<"descriptors: "<<f0.dtors.size().width<<"x"<<f0.dtors.size().height<<" "
00059 //                             <<f1.dtors.size().width<<"x"<<f1.dtors.size().height<<std::endl;
00060     // do forward and reverse matches
00061     std::vector<cv::DMatch> fwd_matches, rev_matches;
00062     matchFrames(f0, f1, fwd_matches);
00063     matchFrames(f1, f0, rev_matches);
00064 //     printf("**** Forward matches: %d, reverse matches: %d ****\n", (int)fwd_matches.size(), (int)rev_matches.size());
00065 
00066     // combine unique matches into one list
00067     for (int i = 0; i < (int)fwd_matches.size(); ++i)
00068     {
00069         if (fwd_matches[i].trainIdx >= 0)
00070             matches.push_back( cv::DMatch(i, fwd_matches[i].trainIdx, fwd_matches[i].distance) );
00071         //matches.push_back( cv::DMatch(fwd_matches[i].queryIdx, fwd_matches[i].trainIdx, fwd_matches[i].distance) );
00072     }
00073     for (int i = 0; i < (int)rev_matches.size(); ++i)
00074     {
00075         if (rev_matches[i].trainIdx >= 0 && i != fwd_matches[rev_matches[i].trainIdx].trainIdx)
00076             matches.push_back( cv::DMatch(rev_matches[i].trainIdx, i, rev_matches[i].distance) );
00077         //matches.push_back( cv::DMatch(rev_matches[i].trainIdx, rev_matches[i].queryIdx, rev_matches[i].distance) );
00078     }
00079 //     printf("**** Total unique matches: %d ****\n", (int)matches.size());
00080 
00081     // do it
00082     return estimate(f0, f1, matches);
00083 }
00084 
00085 
00086 template <typename PointSource, typename PointTarget>
00087 size_t PoseEstimator<PointSource,PointTarget>::estimate(const NDTFrame<PointSource>& f0, const NDTFrame<PointTarget>& f1,
00088         const std::vector<cv::DMatch> &matches)
00089 {
00090     // convert keypoints in match to 3d points
00091     std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > p0; // homogeneous coordinates
00092     std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > p1;
00093 
00094     int nmatch = matches.size();
00095     //srand(getDoubleTime());
00096 
00097     // set up data structures for fast processing
00098     // indices to good matches
00099     std::vector<int> m0, m1;
00100     for (int i=0; i<nmatch; i++)
00101     {
00102         m0.push_back(matches[i].queryIdx);
00103         m1.push_back(matches[i].trainIdx);
00104         //std::cout<<m0[i]<<" "<<m1[i]<<std::endl;
00105     }
00106 
00107     nmatch = m0.size();
00108 
00109     if (nmatch < 3) return 0;   // can't do it...
00110 
00111     int bestinl = 0;
00112 
00113     // RANSAC loop
00114 //#pragma omp parallel for shared( bestinl )
00115     for (int i=0; i<numRansac; i++)
00116     {
00117         //std::cout << "ransac loop : " << i << std::endl;
00118         // find a candidate
00119         int a=rand()%nmatch;
00120         int b = a;
00121         while (a==b)
00122             b=rand()%nmatch;
00123         int c = a;
00124         while (a==c || b==c)
00125             c=rand()%nmatch;
00126 
00127         int i0a = m0[a];
00128         int i0b = m0[b];
00129         int i0c = m0[c];
00130         int i1a = m1[a];
00131         int i1b = m1[b];
00132         int i1c = m1[c];
00133 
00134         if (i0a == i0b || i0a == i0c || i0b == i0c ||
00135                 i1a == i1b || i1a == i1c || i1b == i1c)
00136             continue;
00137 
00138         //std::cout<<a<<" "<<b<<" "<<c<<std::endl;
00139         //std::cout<<i0a<<" "<<i0b<<" "<<i0c<<std::endl;
00140         //std::cout<<i1a<<" "<<i1b<<" "<<i1c<<std::endl;
00141 
00142         // get centroids
00143         Eigen::Vector3d p0a = f0.pts[i0a].head(3);
00144         Eigen::Vector3d p0b = f0.pts[i0b].head(3);
00145         Eigen::Vector3d p0c = f0.pts[i0c].head(3);
00146         Eigen::Vector3d p1a = f1.pts[i1a].head(3);
00147         Eigen::Vector3d p1b = f1.pts[i1b].head(3);
00148         Eigen::Vector3d p1c = f1.pts[i1c].head(3);
00149 
00150         Eigen::Vector3d c0 = (p0a+p0b+p0c)*(1.0/3.0);
00151         Eigen::Vector3d c1 = (p1a+p1b+p1c)*(1.0/3.0);
00152 
00153         //std::cout<<c0.transpose()<<std::endl;
00154         //std::cout<<c1.transpose()<<std::endl;
00155         // subtract out
00156         p0a -= c0;
00157         p0b -= c0;
00158         p0c -= c0;
00159         p1a -= c1;
00160         p1b -= c1;
00161         p1c -= c1;
00162 
00163         Eigen::Matrix3d H = p1a*p0a.transpose() + p1b*p0b.transpose() +
00164                             p1c*p0c.transpose();
00165 
00166         // do the SVD thang
00167         Eigen::JacobiSVD<Eigen::Matrix3d> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
00168         Eigen::Matrix3d V = svd.matrixV();
00169         Eigen::Matrix3d R = V * svd.matrixU().transpose();
00170         double det = R.determinant();
00171         //ntot++;
00172         if (det < 0.0)
00173         {
00174             //nneg++;
00175             V.col(2) = V.col(2)*-1.0;
00176             R = V * svd.matrixU().transpose();
00177         }
00178         Eigen::Vector3d tr = c0-R*c1;    // translation
00179 
00180         // transformation matrix, 3x4
00181         Eigen::Matrix<double,3,4> tfm;
00182         //        tfm.block<3,3>(0,0) = R.transpose();
00183         //        tfm.col(3) = -R.transpose()*tr;
00184         tfm.block<3,3>(0,0) = R;
00185         tfm.col(3) = tr;
00186 
00187 #if 0
00188         // find inliers, based on image reprojection
00189         int inl = 0;
00190         for (int i=0; i<nmatch; i++)
00191         {
00192             Vector3d pt = tfm*f1.pts[m1[i]];
00193             Vector3d ipt = f0.cam2pix(pt);
00194             const cv::KeyPoint &kp = f0.kpts[m0[i]];
00195             double dx = kp.pt.x - ipt.x();
00196             double dy = kp.pt.y - ipt.y();
00197             double dd = f0.disps[m0[i]] - ipt.z();
00198             if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 &&
00199                     dd*dd < maxInlierDDist2)
00200             {
00201                 inl+=(int)sqrt(ipt.z()); // clever way to weight closer points
00202 //               inl+=(int)sqrt(ipt.z()/matches[i].distance);
00203 //               cout << "matches[i].distance : " << matches[i].distance << endl;
00204 //               inl++;
00205             }
00206         }
00207 #endif
00208         int inl = 0;
00209         for (int i=0; i<nmatch; i++)
00210         {
00211             Eigen::Vector3d pt1 = tfm*f1.pts[m1[i]];
00212             Eigen::Vector3d pt0 = f0.pts[m0[i]].head(3);
00213 
00214 //             double z = fabs(pt1.z() - pt0.z())*0.5;
00215             double z = pt1.z();
00216             double dx = pt1.x() - pt0.x();
00217             double dy = pt1.y() - pt0.y();
00218             double dd = pt1.z() - pt0.z();
00219 
00220             if (projectMatches)
00221             {
00222                 // The central idea here is to divide by the distance (this is essentially what cam2pix does).
00223                 dx = dx / z;
00224                 dy = dy / z;
00225             }
00226             if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 &&
00227                     dd*dd < maxInlierDDist2)
00228             {
00229 //----              inl+=(int)sqrt(pt0.z()); // clever way to weight closer points
00230 //               inl+=(int)sqrt(ipt.z()/matches[i].distance);
00231 //               cout << "matches[i].distance : " << matches[i].distance << endl;
00232                 inl++;
00233             }
00234         }
00235 
00236 //#pragma omp critical
00237         if (inl > bestinl)
00238         {
00239             bestinl = inl;
00240             rot = R;
00241             trans = tr;
00242 //             std::cout << "bestinl : " << bestinl << std::endl;
00243         }
00244     }
00245 
00246     //printf("Best inliers: %d\n", bestinl);
00247     //printf("Total ransac: %d  Neg det: %d\n", ntot, nneg);
00248 
00249     // reduce matches to inliers
00250     std::vector<cv::DMatch> inls;    // temporary for current inliers
00251     inliers.clear();
00252     Eigen::Matrix<double,3,4> tfm;
00253     tfm.block<3,3>(0,0) = rot;
00254     tfm.col(3) = trans;
00255 
00256     //std::cout<<"f0: "<<f0.pts.size()<<" "<<f0.kpts.size()<<" "<<f0.pc_kpts.size()<<std::endl;
00257     //std::cout<<"f1: "<<f1.pts.size()<<" "<<f1.kpts.size()<<" "<<f1.pc_kpts.size()<<std::endl;
00258 
00259     nmatch = matches.size();
00260     for (int i=0; i<nmatch; i++)
00261     {
00262         Eigen::Vector3d pt1 = tfm*f1.pts[matches[i].trainIdx];
00263         //Eigen::Vector3d pt1_unchanged = f1.pts[matches[i].trainIdx].head(3);
00264         //Vector3d pt1 = pt1_unchanged;
00265 #if 0
00266         Vector3d ipt = f0.cam2pix(pt);
00267         const cv::KeyPoint &kp = f0.kpts[matches[i].queryIdx];
00268         double dx = kp.pt.x - ipt.x();
00269         double dy = kp.pt.y - ipt.y();
00270         double dd = f0.disps[matches[i].queryIdx] - ipt.z();
00271 #endif
00272         Eigen::Vector3d pt0 = f0.pts[matches[i].queryIdx].head(3);
00273 
00274         //double z = fabs(pt1.z() - pt0.z())*0.5;
00275         double z = pt1.z();
00276         double dx = pt1.x() - pt0.x();
00277         double dy = pt1.y() - pt0.y();
00278         double dd = pt1.z() - pt0.z();
00279 
00280         if (projectMatches)
00281         {
00282             // The central idea here is to divide by the distance (this is essentially what cam2pix does).
00283             dx = dx / z;
00284             dy = dy / z;
00285         }
00286 
00287         if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 &&
00288                 dd*dd < maxInlierDDist2)
00289         {
00290             if (z < maxDist && z > minDist)
00291 
00292 //             if (fabs(f0.kpts[matches[i].queryIdx].pt.y - f1.kpts[matches[i].trainIdx].pt.y) > 300)
00293             {
00294 //                 std::cout << " ---------- " << dx << "," << dy << "," << dd << ",\npt0 " << pt0.transpose() << "\npt1 " << pt1.transpose() << f0.kpts[matches[i].queryIdx].pt << "," <<
00295 //                       f1.kpts[matches[i].trainIdx].pt << "\n unchanged pt1 " << pt1_unchanged.transpose() << std::endl;
00296                 inliers.push_back(matches[i]);
00297             }
00298         }
00299     }
00300 
00301 #if 0
00302     // Test with the SBA...
00303     {
00304         // system
00305         SysSBA sba;
00306         sba.verbose = 0;
00307 
00308 #if 0
00309         // set up nodes
00310         // should have a frame => node function
00311         Vector4d v0 = Vector4d(0,0,0,1);
00312         Quaterniond q0 = Quaternion<double>(Vector4d(0,0,0,1));
00313         sba.addNode(v0, q0, f0.cam, true);
00314 
00315         Quaterniond qr1(rot);   // from rotation matrix
00316         Vector4d temptrans = Vector4d(trans(0), trans(1), trans(2), 1.0);
00317 
00318         //        sba.addNode(temptrans, qr1.normalized(), f1.cam, false);
00319         qr1.normalize();
00320         sba.addNode(temptrans, qr1, f1.cam, false);
00321 
00322         int in = 3;
00323         if (in > (int)inls.size())
00324             in = inls.size();
00325 
00326         // set up projections
00327         for (int i=0; i<(int)inls.size(); i++)
00328         {
00329             // add point
00330             int i0 = inls[i].queryIdx;
00331             int i1 = inls[i].trainIdx;
00332             Vector4d pt = f0.pts[i0];
00333             sba.addPoint(pt);
00334 
00335             // projected point, ul,vl,ur
00336             Vector3d ipt;
00337             ipt(0) = f0.kpts[i0].pt.x;
00338             ipt(1) = f0.kpts[i0].pt.y;
00339             ipt(2) = ipt(0)-f0.disps[i0];
00340             sba.addStereoProj(0, i, ipt);
00341 
00342             // projected point, ul,vl,ur
00343             ipt(0) = f1.kpts[i1].pt.x;
00344             ipt(1) = f1.kpts[i1].pt.y;
00345             ipt(2) = ipt(0)-f1.disps[i1];
00346             sba.addStereoProj(1, i, ipt);
00347         }
00348 
00349         sba.huber = 2.0;
00350         sba.doSBA(5,10e-4,SBA_DENSE_CHOLESKY);
00351         int nbad = sba.removeBad(2.0); // 2.0
00352         cout << endl << "Removed " << nbad << " projections > 2 pixels error" << endl;
00353         sba.doSBA(5,10e-5,SBA_DENSE_CHOLESKY);
00354 
00355 //        cout << endl << sba.nodes[1].trans.transpose().head(3) << endl;
00356 
00357         // get the updated transform
00358         trans = sba.nodes[1].trans.head(3);
00359         Quaterniond q1;
00360         q1 = sba.nodes[1].qrot;
00361         quat = q1;
00362         rot = q1.toRotationMatrix();
00363 
00364         // set up inliers
00365         inliers.clear();
00366         for (int i=0; i<(int)inls.size(); i++)
00367         {
00368             ProjMap &prjs = sba.tracks[i].projections;
00369             if (prjs[0].isValid && prjs[1].isValid) // valid track
00370                 inliers.push_back(inls[i]);
00371         }
00372 
00373         printf("Inliers: %d   After polish: %d\n", (int)inls.size(), (int)inliers.size());
00374 #endif
00375     }
00376 #endif
00377 
00378 //     std::cout << std::endl << trans.transpose().head(3) << std::endl << std::endl;
00379 //     std::cout << rot << std::endl;
00380 
00381     return inliers.size();
00382 }
00383 }


ndt_feature_reg
Author(s): Henrik Andreasson, Todor Stoyanov, Jari Saarinen
autogenerated on Wed Aug 26 2015 15:25:07