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
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;
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
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);
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
00120
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
00161
00162
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
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
00227
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
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
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
00374 if(nextNDT[i]!=NULL) delete nextNDT[i];
00375 }
00376
00377
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 }