frame_matching_test.cc
Go to the documentation of this file.
00001 #include <iostream>
00002 #include <boost/program_options.hpp>
00003 #include <cv.h>
00004 #include <highgui.h>
00005 #include "pcl/io/pcd_io.h"
00006 #include <pcl/filters/passthrough.h>
00007 
00008 #include <ndt_feature_reg/Frame.hh>
00009 #include <NDTMatcherF2F.hh>
00010 #include <NDTMatcherFeatureF2F.hh>
00011 #include <PointCloudUtils.hh>
00012 
00013 using namespace std;
00014 using namespace ndt_feature_reg;
00015 using namespace lslgeneric;
00016 namespace po = boost::program_options;
00017 
00018 int main(int argc, char** argv)
00019 {
00020     cout << "--------------------------------------------------" << endl;
00021     cout << "Small test program of the frame matching between 2" << endl;
00022     cout << "pair of depth + std (RGB) images. Each image pair " << endl;
00023     cout << "is assumed to have 1-1 correspondance, meaning    " << endl;
00024     cout << "that for each pixel in the std image we have the  " << endl;
00025     cout << "corresponding depth / disparity pixel at the same " << endl;
00026     cout << "location in the depth image.                      " << endl;
00027     cout << "--------------------------------------------------" << endl;
00028 
00029     string std1_name, depth1_name, std2_name, depth2_name, pc1_name, pc2_name;
00030     double scale, max_inldist_xy, max_inldist_z;
00031     Eigen::Matrix<double,6,1> pose_increment_v;
00032     int nb_ransac;
00033 
00034     po::options_description desc("Allowed options");
00035     desc.add_options()
00036     ("help", "produce help message")
00037     ("debug", "print debug output")
00038     ("visualize", "visualize the output")
00039     ("depth1", po::value<string>(&depth1_name), "first depth image")
00040     ("depth2", po::value<string>(&depth2_name), "second depth image")
00041     ("std1", po::value<string>(&std1_name), "first standard image")
00042     ("std2", po::value<string>(&std2_name), "second standard image")
00043     ("pc1", po::value<string>(&pc1_name), "first pointcloud")
00044     ("pc2", po::value<string>(&pc2_name), "second pointcloud")
00045     ("scale", po::value<double>(&scale)->default_value(0.0002), "depth scale (depth = scale * pixel value)")
00046     ("max_inldist_xy", po::value<double>(&max_inldist_xy)->default_value(0.1), "max inlier distance in xy related to the camera plane (in meters)")
00047     ("max_inldist_z", po::value<double>(&max_inldist_z)->default_value(0.1), "max inlier distance in z - depth (in meters)")
00048     ("nb_ransac", po::value<int>(&nb_ransac)->default_value(100), "max number of RANSAC iterations")
00049     ;
00050 
00051     po::variables_map vm;
00052     po::store(po::parse_command_line(argc, argv, desc), vm);
00053     po::notify(vm);
00054 
00055     if (vm.count("help"))
00056     {
00057         cout << desc << "\n";
00058         return 1;
00059     }
00060     if ((vm.count("pc1") && vm.count("depth1")) || (vm.count("pc2") && vm.count("depth2")))
00061     {
00062         cout << "ERROR: both a poincloud and a depth image are provided as input  - quitting\n";
00063         return 1;
00064     }
00065     bool debug = vm.count("debug");
00066     bool visualize = vm.count("visualize");
00067     if (debug)
00068         cout << "scale : " << scale << endl;
00069 
00070     // TODO - this are the values from the Freiburg 1 camera.
00071     double fx = 517.3;
00072     double fy = 516.5;
00073     double cx = 318.6;
00074     double cy = 255.3;
00075     double d0 = 0.2624;
00076     double d1 = -0.9531;
00077     double d2 = -0.0054;
00078     double d3 = 0.0026;
00079     double d4 = 1.1633;
00080     double ds = 1.035; // Depth scaling factor.
00081 
00082     cv::Mat camera_mat = getCameraMatrix(fx, fy, cx, cy);
00083     cv::Mat dist_vec = getDistVector(d0, d1, d2, d3, d4);
00084 
00085     cv::Mat std1,std2,depth1,depth2, lookup_table;
00086     // Load the files.
00087     pcl::PCDReader reader;
00088     double t1,t2,t3;
00089 
00090     if (debug)
00091         cout << "loading imgs : " << std1_name << " and " << std2_name << endl;
00092     NDTFrame frame1, frame2;
00093     frame1.img = cv::imread(std1_name, 0);
00094     frame2.img = cv::imread(std2_name, 0);
00095 
00096     if (vm.count("pc1") && vm.count("pc2"))
00097     {
00098         if (debug)
00099             cout << "loading pc : " << pc1_name << " and " << pc2_name << endl;
00100         reader.read<pcl::PointXYZ>(pc1_name, frame1.pc);
00101         reader.read<pcl::PointXYZ>(pc2_name, frame2.pc);
00102     }
00103     else if (vm.count("depth1") && vm.count("depth2"))
00104     {
00105         if (debug)
00106             cout << "loading depth img : " << depth1_name << " and " << depth2_name << endl;
00107         frame1.depth_img = cv::imread(depth1_name, CV_LOAD_IMAGE_ANYDEPTH); // CV_LOAD_IMAGE_ANYDEPTH is important to load the 16bits image
00108         frame2.depth_img = cv::imread(depth2_name, CV_LOAD_IMAGE_ANYDEPTH);
00109 
00110         if (debug)
00111         {
00112             cout << "using camera matrix : \n" << camera_mat << endl;
00113             cout << "using distortion vec: \n" << dist_vec << endl;
00114             cout << "depth1.depth() : " << frame1.depth_img.depth() << endl;
00115             cout << "depth1.channels() : " << frame1.depth_img.channels() << endl;
00116         }
00117         if (visualize)
00118         {
00119 //             cv::imshow("frame1.depth_img", frame1.depth_img);
00120 //             cv::waitKey(0);
00121         }
00122 
00123         t1 = getDoubleTime();
00124         lookup_table = getDepthPointCloudLookUpTable(frame1.depth_img.size(), camera_mat, dist_vec, ds*scale);
00125         t2 = getDoubleTime();
00126         if (debug)
00127         {
00128             cout << "lookup: " << t2-t1 << endl;
00129             t1 = getDoubleTime();
00130             convertDepthImageToPointCloud(frame1.depth_img, lookup_table, frame1.pc);
00131             t2 = getDoubleTime();
00132             convertDepthImageToPointCloud(frame1.depth_img, lookup_table, frame1.pc);
00133             t3 = getDoubleTime();
00134             cout << "convert: " << t2-t1 << endl;
00135             cout << "convert2: " << t3-t2 << endl;
00136             convertDepthImageToPointCloud(frame2.depth_img, lookup_table, frame2.pc);
00137             pcl::io::savePCDFileASCII ("frame1_depth.pcd", frame1.pc);
00138             pcl::io::savePCDFileASCII ("frame2_depth.pcd", frame2.pc);
00139         }
00140     }
00141     else
00142     {
00143         cout << "Check depth data input - quitting\n";
00144         return 1;
00145     }
00146 
00147     cv::Ptr<cv::FeatureDetector> detector = cv::FeatureDetector::create("SURF");
00148     cv::Ptr<cv::DescriptorExtractor> extractor = cv::DescriptorExtractor::create("SURF");
00149     t1 = getDoubleTime();
00150     detector->detect(frame1.img, frame1.kpts);
00151     t2 = getDoubleTime();
00152     detector->detect(frame2.img, frame2.kpts);
00153 
00154     cv::Mat img_draw1, img_draw2;
00155     cv::drawKeypoints(frame1.img, frame1.kpts, img_draw1, cv::Scalar(0,255,0));
00156     cv::drawKeypoints(frame2.img, frame2.kpts, img_draw2, cv::Scalar(0,255,0));
00157 
00158     if (visualize)
00159     {
00160 //        cv::imshow("frame1.img", img_draw1);
00161 //        cv::imshow("frame2.img", img_draw2);
00162 //        cv::waitKey(0);
00163     }
00164     if (debug)
00165         cout << "Computing NDT ..." << endl;
00166     t1 = getDoubleTime();
00167     frame1.computeNDT();
00168     t2 = getDoubleTime();
00169     cout << "compute NDT : " << t2 - t1 << endl;
00170     frame2.computeNDT();
00171     if (debug)
00172         cout << " - done." << endl;
00173 
00174     std::cout << "detect() : " << t2 - t1 << std::endl;
00175     extractor->compute(frame1.img, frame1.kpts, frame1.dtors);
00176     extractor->compute(frame2.img, frame2.kpts, frame2.dtors);
00177 
00178     if (debug)
00179     {
00180         pcl::PointCloud<pcl::PointXYZRGB> pc_color;
00181         cout << "create colored point cloud" << endl;
00182         cout << "frame2.img.width : " << frame2.img.cols << endl;
00183         cout << "frame2.img.height : " << frame2.img.rows << endl;
00184         cout << "pc.width : " << frame2.pc.width << endl;
00185         cout << "pc.height : " << frame2.pc.height << endl;
00186         createColoredPointCloud(frame2.img, frame2.pc, pc_color);
00187         colorKeyPointsInPointCloud(pc_color, frame2.kpts_pc_indices);
00188         cout << "create colored point cloud - done" << endl;
00189         pcl::io::savePCDFileASCII ("frame2_color.pcd", pc_color);
00190     }
00191 
00192     // Do matching
00193     PoseEstimator pe(nb_ransac, max_inldist_xy, max_inldist_z);
00194     frame1.assignPts();
00195     frame2.assignPts();
00196 
00197     pe.estimate(frame1, frame2);
00198 
00199     if (visualize)
00200     {
00201         cv::Mat display;
00202         drawMatches(frame1.img, frame1.kpts, frame2.img, frame2.kpts, pe.inliers, display);
00203         const std::string window_name = "matches";
00204         cv::namedWindow(window_name,0);
00205         cv::imshow(window_name, display);
00206         cv::waitKey(0);
00207     }
00208 
00209 
00210 
00211     frame1.ndt_map.writeToVRML("/home/han/tmp/ndt1.wrl");
00212     frame2.ndt_map.writeToVRML("/home/han/tmp/ndt2.wrl");
00213 
00214     if (debug)
00215         cout << "Creating key point cloud ..." << endl;
00216     frame1.createKeyPointCloud();
00217     frame2.createKeyPointCloud();
00218     if (debug)
00219         cout << "Creating key point cloud - done." << endl;
00220 
00221     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> transform, transform_p2p, transform_ndt2ndt, transform_ndt2ndt_corr, transform_p2p_corr, transform_p2p_corr_rot, transform_p2p_corr_transl;
00222     transform_p2p.setIdentity();
00223     transform_ndt2ndt.setIdentity();
00224     transform_ndt2ndt_corr.setIdentity();
00225     transform_p2p_corr.setIdentity();
00226 //     transform_p2p_corr.translate(pe.trans).rotate(pe.quat);
00227 //     transform_p2p_corr.rotate(pe.quat).translate(pe.trans);
00228     transform_p2p_corr.rotate(pe.rot);
00229     transform_p2p_corr.translate(pe.trans);
00230 
00231 #if 0
00232     transform = Eigen::Translation<double,3>(pose_increment_v(0),pose_increment_v(1),pose_increment_v(2))*
00233                 Eigen::AngleAxis<double>(pose_increment_v(3),Eigen::Vector3d::UnitX()) *
00234                 Eigen::AngleAxis<double>(pose_increment_v(4),Eigen::Vector3d::UnitY()) *
00235                 Eigen::AngleAxis<double>(pose_increment_v(5),Eigen::Vector3d::UnitZ()) ;
00236 #endif
00237     NDTMatcherF2F matcher;
00238 
00239     transform.setIdentity();
00240     // Not possible due to that these pc's are dense and contains NaN's.
00241     pcl::PassThrough<pcl::PointXYZ> pass;
00242     pass.setInputCloud(frame1.pc.makeShared());
00243     pass.filter(frame1.pc);
00244     pass.setInputCloud(frame2.pc.makeShared());
00245     pass.filter(frame2.pc);
00246 
00247     if (debug)
00248         cout << "Matching ..." << endl;
00249     t1 = getDoubleTime();
00250     matcher.match(frame1.pc, frame2.pc, transform);
00251     transform_p2p = transform;
00252     t2 = getDoubleTime();
00253     std::cout << "p2p matching : " << t2 - t1 << std::endl;
00254     if (debug)
00255         cout << "Matching - done." << endl;
00256     std::cout<<"est translation (points) "<<transform.translation().transpose()
00257              <<" (norm) "<<transform.translation().norm()<<std::endl;
00258     std::cout<<"est rotation (points) "<<transform.rotation().eulerAngles(0,1,2).transpose()
00259              <<" (norm) "<<transform.rotation().eulerAngles(0,1,2).norm()<<std::endl;
00260 
00261 
00262     t1 = getDoubleTime();
00263     matcher.match(frame1.ndt_map, frame2.ndt_map, transform);
00264     transform_ndt2ndt = transform;
00265     t2 = getDoubleTime();
00266     std::cout << "ndt2ndt matching : " << t2 - t1 << std::endl;
00267 
00268     std::cout<<"est translation (ndt - no corr) "<<transform.translation().transpose()
00269              <<" (norm) "<<transform.translation().norm()<<std::endl;
00270     std::cout<<"est rotation (ndt - no corr) "<<transform.rotation().eulerAngles(0,1,2).transpose()
00271              <<" (norm) "<<transform.rotation().eulerAngles(0,1,2).norm()<<std::endl;
00272 
00273 
00274     std::vector<std::pair<int,int> > corr;
00275     for (size_t i = 0; i < pe.inliers.size(); i++)
00276     {
00277         corr.push_back(std::pair<int,int>(pe.inliers[i].queryIdx, pe.inliers[i].trainIdx));
00278 //        corr.push_back(std::pair<int,int>(pe.inliers[i].trainIdx, pe.inliers[i].queryIdx));
00279         assert(pe.inliers[i].queryIdx < frame1.ndt_map.getMyIndex()->size() && pe.inliers[i].queryIdx >= 0);
00280         assert(pe.inliers[i].trainIdx < frame2.ndt_map.getMyIndex()->size() && pe.inliers[i].trainIdx >= 0);
00281     }
00282     cout << "frame1.ndt_map.getMyIndex()->size() : " << frame1.ndt_map.getMyIndex()->size() << endl;
00283     cout << "frame2.ndt_map.getMyIndex()->size() : " << frame2.ndt_map.getMyIndex()->size() << endl;
00284     NDTMatcherFeatureF2F matcher_feat(corr);
00285     cout << "Matching ..." << endl;
00286     t1 = getDoubleTime();
00287     matcher_feat.match(frame1.ndt_map, frame2.ndt_map, transform);
00288     transform_ndt2ndt_corr = transform;
00289     t2 = getDoubleTime();
00290     cout << "Matching ... - done" << endl;
00291     std::cout << "ndt2ndt matching : " << t2 - t1 << std::endl;
00292 
00293     std::cout<<"est translation (ndt - no corr) "<<transform.translation().transpose()
00294              <<" (norm) "<<transform.translation().norm()<<std::endl;
00295     std::cout<<"est rotation (ndt - no corr) "<<transform.rotation().eulerAngles(0,1,2).transpose()
00296              <<" (norm) "<<transform.rotation().eulerAngles(0,1,2).norm()<<std::endl;
00297 
00298 
00299 
00300     {
00301         transform = transform_p2p;
00302         char fname[50];
00303         snprintf(fname,49,"/home/han/tmp/output_p2p.wrl");
00304         FILE *fout = fopen(fname,"w");
00305         fprintf(fout,"#VRML V2.0 utf8\n");
00306         std::vector<NDTCell*> nextNDT = frame2.ndt_map.pseudoTransformNDT(transform.inverse());
00307 
00308         for(unsigned int i=0; i<nextNDT.size(); i++)
00309         {
00310             nextNDT[i]->writeToVRML(fout,Eigen::Vector3d(0,1,0));
00311             if(nextNDT[i]!=NULL) delete nextNDT[i];
00312         }
00313 
00314         frame1.ndt_map.writeToVRML(fout, false);
00315         writeToVRML(fout, frame1.pc);
00316         transformPointCloudInPlace(transform, frame2.pc);
00317         writeToVRML(fout, frame2.pc);
00318         fclose(fout);
00319     }
00320 
00321     {
00322         transform = transform_ndt2ndt;
00323         char fname[50];
00324         snprintf(fname,49,"/home/han/tmp/output_ndt2ndt.wrl");
00325         FILE *fout = fopen(fname,"w");
00326         fprintf(fout,"#VRML V2.0 utf8\n");
00327         std::vector<NDTCell*> nextNDT = frame2.ndt_map.pseudoTransformNDT(transform.inverse());
00328 
00329         for(unsigned int i=0; i<nextNDT.size(); i++)
00330         {
00331             nextNDT[i]->writeToVRML(fout,Eigen::Vector3d(0,1,0));
00332             if(nextNDT[i]!=NULL) delete nextNDT[i];
00333         }
00334 
00335         frame1.ndt_map.writeToVRML(fout, false);
00336         writeToVRML(fout, frame1.pc);
00337         transformPointCloudInPlace(transform, frame2.pc);
00338         writeToVRML(fout, frame2.pc);
00339         fclose(fout);
00340     }
00341 
00342     {
00343         transform = transform_ndt2ndt_corr;
00344         char fname[50];
00345         snprintf(fname,49,"/home/han/tmp/output_ndt2ndt_corr.wrl");
00346         FILE *fout = fopen(fname,"w");
00347         fprintf(fout,"#VRML V2.0 utf8\n");
00348         std::vector<NDTCell*> nextNDT = frame2.ndt_map.pseudoTransformNDT(transform.inverse());
00349 
00350         for(unsigned int i=0; i<nextNDT.size(); i++)
00351         {
00352             nextNDT[i]->writeToVRML(fout,Eigen::Vector3d(0,1,0));
00353             if(nextNDT[i]!=NULL) delete nextNDT[i];
00354         }
00355 
00356         frame1.ndt_map.writeToVRML(fout, false);
00357         writeToVRML(fout, frame1.pc);
00358         transformPointCloudInPlace(transform, frame2.pc);
00359         writeToVRML(fout, frame2.pc);
00360         fclose(fout);
00361     }
00362 
00363     {
00364         transform = transform_p2p_corr;
00365         char fname[50];
00366         snprintf(fname,49,"/home/han/tmp/output_p2p_corr.wrl");
00367         FILE *fout = fopen(fname,"w");
00368         fprintf(fout,"#VRML V2.0 utf8\n");
00369         std::vector<NDTCell*> nextNDT = frame2.ndt_map.pseudoTransformNDT(transform.inverse());
00370 
00371         for(unsigned int i=0; i<nextNDT.size(); i++)
00372         {
00373 //             nextNDT[i]->writeToVRML(fout,Eigen::Vector3d(0,1,0));
00374             if(nextNDT[i]!=NULL) delete nextNDT[i];
00375         }
00376 
00377 //        frame1.ndt_map.writeToVRML(fout, false);
00378 
00379         writeToVRML(fout, frame1.pc);
00380         transformPointCloudInPlace(transform, frame2.pc);
00381         writeToVRML(fout, frame2.pc, Eigen::Vector3d(0,1,0));
00382         fclose(fout);
00383     }
00384 
00385 
00386 }


ndt_feature_reg
Author(s): Henrik Andreasson, Todor Stoyanov
autogenerated on Mon Jan 6 2014 11:36:18