ndt_feature_test.cc
Go to the documentation of this file.
00001 // It works if the --p2p option is given!??
00002 #include <iostream>
00003 #include <boost/program_options.hpp>
00004 #include <boost/math/distributions/normal.hpp>
00005 #include <boost/random.hpp>
00006 #include <boost/random/uniform_real.hpp>
00007 #include <boost/random/variate_generator.hpp>
00008 
00009 #include <ndt_registration/ndt_matcher_d2d.h>
00010 #include <ndt_registration/ndt_matcher_d2d_feature.h>
00011 #include <ndt_map/ndt_map.h>
00012 #include <ndt_map/cell_vector.h>
00013 #include <ndt_map/lazy_grid.h>
00014 #include <pointcloud_vrml/pointcloud_utils.h>
00015 
00016 using namespace std;
00017 using namespace lslgeneric;
00018 namespace po = boost::program_options;
00019 
00020 int main(int argc, char** argv)
00021 {
00022     cout << "--------------------------------------------------" << endl;
00023     cout << "Small test program of CellVector + F2F matcher " << endl;
00024     cout << "--------------------------------------------------" << endl;
00025 
00026     po::options_description desc("Allowed options");
00027     Eigen::Matrix<double,6,1> pose_increment_v;
00028     string static_file_name, moving_file_name;
00029     int nb_clusters, nb_points;
00030     double std_dev, min, max;
00031     desc.add_options()
00032     ("help", "produce help message")
00033     ("x", po::value<double>(&pose_increment_v(0))->default_value(1.), "x pos gt offset")
00034     ("y", po::value<double>(&pose_increment_v(1))->default_value(1.), "y pos gt offset")
00035     ("z", po::value<double>(&pose_increment_v(2))->default_value(1.), "z pos gt offset")
00036     ("X", po::value<double>(&pose_increment_v(3))->default_value(0.1), "x axis rot gt offset")
00037     ("Y", po::value<double>(&pose_increment_v(4))->default_value(0.1), "y axis rot gt offset")
00038     ("Z", po::value<double>(&pose_increment_v(5))->default_value(0.1), "z axis rot gt offset")
00039     ("nb_clusters", po::value<int>(&nb_clusters)->default_value(20), "number of clusters")
00040     ("nb_points", po::value<int>(&nb_points)->default_value(10), "number of points per clusters")
00041     ("std_dev", po::value<double>(&std_dev)->default_value(0.1), "standard deviation of the points drawn from a normal distribution (here independent on the axes")
00042     ("min", po::value<double>(&min)->default_value(-10), "minimum center point")
00043     ("max", po::value<double>(&max)->default_value(10), "maximum center point")
00044     ("lazzy", "use lazzygrid")
00045     ("p2preg", "calculate NDTMatchF2F using two pointclouds")
00046     ("irregular_grid", "use irregular grid in the p2p registration")
00047     ("nfeaturecorr", "feature_f2f should NOT be evaluated (NDTMatcherFeatureF2F)")
00048     ("singleres", "use single resolution in the 'p2p' matching")
00049     ("nf2f", "if f2f should NOT be evaluated")
00050     ("usegt", "if the ground truth should be used as initial estimate")
00051     ;
00052 
00053     po::variables_map vm;
00054     po::store(po::parse_command_line(argc, argv, desc), vm);
00055     po::notify(vm);
00056 
00057     if (vm.count("help"))
00058     {
00059         cout << desc << "\n";
00060         return 1;
00061     }
00062     bool use_lazzy = vm.count("lazzy");
00063     bool use_p2preg = vm.count("p2preg");
00064     bool use_irregular_grid = vm.count("irregular_grid");
00065     bool use_featurecorr = !vm.count("nfeaturecorr");
00066     bool use_singleres = vm.count("singleres");
00067     bool use_f2f = !vm.count("nf2f");
00068     bool usegt = vm.count("usegt");
00069     pcl::PointCloud<pcl::PointXYZ> static_pc, moving_pc, tmp_pc;
00070     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> gt_transform;
00071 
00072     // Generate the static point cloud + indices...
00073     boost::mt19937 rng;
00074     boost::uniform_real<> ud(min,max);
00075     boost::normal_distribution<> nd(0.0, std_dev);
00076     boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor(rng, nd);
00077     boost::variate_generator<boost::mt19937&, boost::uniform_real<> > var_uni(rng, ud);
00078     std::vector<std::pair<int,int> > corresp;
00079 
00080     size_t index = 0;
00081     std::vector<std::vector<size_t> > all_indices;
00082     for (int i = 0; i < nb_clusters; i++)
00083     {
00084         std::vector<size_t> indices;
00085         double c_x = var_uni();
00086         double c_y = var_uni();
00087         double c_z = var_uni();
00088         for (int j = 0; j < nb_points; j++)
00089         {
00090             static_pc.push_back(pcl::PointXYZ(c_x+var_nor(), c_y+var_nor(), c_z+var_nor()));
00091             indices.push_back(index);
00092             index++;
00093         }
00094         all_indices.push_back(indices);
00095         corresp.push_back(std::pair<int,int>(i,nb_clusters-1-i)); // nb_clusters-1-i -> To check the da functions is working.
00096     }
00097     std::vector<std::vector<size_t> > all_indices_moving(all_indices.size());  // Reverse the correspondances (To check the da functions)
00098     std::reverse_copy(all_indices.begin(), all_indices.end(), all_indices_moving.begin());
00099 
00100     for (int i = 0; i < 1/*nb_clusters*/; i++)
00101     {
00102         tmp_pc.push_back(pcl::PointXYZ(i,i,i));
00103     }
00104 
00105     // Specify some offset...
00106     gt_transform = Eigen::Translation<double,3>(pose_increment_v(0),pose_increment_v(1),pose_increment_v(2))*
00107                    Eigen::AngleAxis<double>(pose_increment_v(3),Eigen::Vector3d::UnitX()) *
00108                    Eigen::AngleAxis<double>(pose_increment_v(4),Eigen::Vector3d::UnitY()) *
00109                    Eigen::AngleAxis<double>(pose_increment_v(5),Eigen::Vector3d::UnitZ()) ;
00110 
00111 
00112     std::vector<double> resolutions;
00113     if (use_singleres)
00114         resolutions.push_back(1.);
00115     NDTMatcherD2D<pcl::PointXYZ,pcl::PointXYZ> matcher(use_irregular_grid,!use_singleres,resolutions);
00116     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> T_f2f,T_p2p, T_feat;
00117     T_f2f.setIdentity();
00118     T_p2p.setIdentity();
00119     T_feat.setIdentity();
00120     if (usegt)
00121     {
00122         T_f2f = gt_transform;
00123         T_p2p = gt_transform;
00124         T_feat = gt_transform;
00125     }
00126 
00127     moving_pc = transformPointCloud(gt_transform, static_pc);
00128 
00129     if (use_p2preg)
00130     {
00131         matcher.match(moving_pc, static_pc, T_p2p);
00132     }
00133     {
00134         double current_resolution = 1;
00135 
00136         SpatialIndex<pcl::PointXYZ>* index = NULL;
00137         if (use_lazzy)
00138         {
00139             index = new LazyGrid<pcl::PointXYZ>(current_resolution);
00140         }
00141         else
00142         {
00143             index = new CellVector<pcl::PointXYZ>();
00144         }
00145 
00146         NDTMap<pcl::PointXYZ> ndt(index);
00147         if (!use_lazzy)
00148             ndt.loadPointCloud( static_pc, all_indices );
00149         else
00150             ndt.loadPointCloud (static_pc );
00151         ndt.computeNDTCells();
00152 
00153         NDTMap<pcl::PointXYZ> mov(index);
00154         if (!use_lazzy)
00155             mov.loadPointCloud( moving_pc, all_indices_moving );
00156         else
00157             mov.loadPointCloud( moving_pc );
00158         mov.computeNDTCells();
00159 
00160         if (use_f2f)
00161         {
00162             matcher.match( mov, ndt, T_f2f );
00163         }
00164         if (use_featurecorr)
00165         {
00166             NDTMatcherFeatureD2D<pcl::PointXYZ,pcl::PointXYZ> matcher_feat(corresp);
00167             matcher_feat.match( mov, ndt, T_feat );
00168         }
00169         delete index;
00170     }
00171 
00172     std::cout<<"GT translation "<<gt_transform.translation().transpose()
00173              <<" (norm) "<<gt_transform.translation().norm()<<std::endl;
00174     std::cout<<"GT rotation "<<gt_transform.rotation().eulerAngles(0,1,2).transpose()
00175              <<" (norm) "<<gt_transform.rotation().eulerAngles(0,1,2).norm()<<std::endl;
00176 
00177     if (use_f2f)
00178     {
00179         std::cout<<"f2f translation "<<T_f2f.translation().transpose()
00180                  <<" (norm) "<<T_f2f.translation().norm()<<std::endl;
00181         std::cout<<"f2f rotation "<<T_f2f.rotation().eulerAngles(0,1,2).transpose()
00182                  <<" (norm) "<<T_f2f.rotation().eulerAngles(0,1,2).norm()<<std::endl;
00183     }
00184 
00185     if (use_featurecorr)
00186     {
00187         std::cout<<"feat translation "<<T_feat.translation().transpose()
00188                  <<" (norm) "<<T_feat.translation().norm()<<std::endl;
00189         std::cout<<"feat rotation "<<T_feat.rotation().eulerAngles(0,1,2).transpose()
00190                  <<" (norm) "<<T_feat.rotation().eulerAngles(0,1,2).norm()<<std::endl;
00191     }
00192 
00193     if (use_p2preg)
00194     {
00195         std::cout<<"p2preg translation "<<T_p2p.translation().transpose()
00196                  <<" (norm) "<<T_p2p.translation().norm()<<std::endl;
00197         std::cout<<"p2preg rotation "<<T_p2p.rotation().eulerAngles(0,1,2).transpose()
00198                  <<" (norm) "<<T_p2p.rotation().eulerAngles(0,1,2).norm()<<std::endl;
00199     }
00200     cout << "done." << endl;
00201 }


ndt_registration
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Wed Aug 26 2015 15:24:52