from_jff.cc
Go to the documentation of this file.
00001 #include <ndt_registration/ndt_matcher_p2d.h>
00002 #include <ndt_registration/ndt_matcher_d2d_2d.h>
00003 #include <ndt_registration/ndt_matcher_d2d.h>
00004 #include <ndt_map/ndt_map.h>
00005 #include <pointcloud_vrml/pointcloud_utils.h>
00006 
00007 #include "pcl/point_cloud.h"
00008 #include <cstdio>
00009 #include <Eigen/Eigen>
00010 #include <Eigen/Geometry>
00011 
00012 #include <iostream>
00013 #include <sstream>
00014 
00015 using namespace std;
00016 
00017 int
00018 main (int argc, char** argv)
00019 {
00020     if(argc!=9) {
00021         std::cout<<"usage "<<argv[0]<<" x y z r p t cloud1.wrl cloud2.wrl\n";
00022         return -1;
00023     }
00024 
00025     istringstream roll_c(argv[4]),pitch_c(argv[5]),yaw_c(argv[6]),xoffset_c(argv[1]),yoffset_c(argv[2]),zoffset_c(argv[3]);
00026     double roll,pitch,yaw,xoffset,yoffset,zoffset;
00027     roll_c >> roll;
00028     pitch_c >> pitch;
00029     yaw_c >> yaw;
00030     xoffset_c >> xoffset;
00031     yoffset_c >> yoffset;
00032     zoffset_c >> zoffset;
00033 
00034     printf("X %f Y %f Z %f Roll %f Pitch %f Yaw %f \n",xoffset,yoffset,zoffset,roll,pitch,yaw); 
00035 
00036     char fname[50];
00037     FILE *fout;
00038     double __res[] = {0.5, 1, 2, 4};
00039     std::vector<double> resolutions (__res, __res+sizeof(__res)/sizeof(double));
00040 
00041     struct timeval tv_start,tv_end,tv_reg_start,tv_reg_end;
00042 
00043     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Tout;
00044     //Tout.setIdentity();
00045     Tout =  Eigen::Translation<double,3>(xoffset,yoffset,zoffset)*
00046         Eigen::AngleAxis<double>(roll,Eigen::Vector3d::UnitX()) *
00047         Eigen::AngleAxis<double>(pitch,Eigen::Vector3d::UnitY()) *
00048         Eigen::AngleAxis<double>(yaw,Eigen::Vector3d::UnitZ()) ;
00049 
00050 
00051     gettimeofday(&tv_start,NULL);
00052     //we do a single scan to scan registration
00053     lslgeneric::NDTMap<pcl::PointXYZI> ndmap(new lslgeneric::LazyGrid<pcl::PointXYZI>(0.4)), local_map(new lslgeneric::LazyGrid<pcl::PointXYZI>(0.4));
00054     ndmap.loadFromJFF(argv[7]);
00055     local_map.loadFromJFF(argv[8]);
00056 
00057     //lslgeneric::NDTMatcherD2D_2D<pcl::PointXYZI,pcl::PointXYZI> matcherD2D(false, false, resolutions);
00058     lslgeneric::NDTMatcherD2D<pcl::PointXYZI,pcl::PointXYZI> matcherD2D(false, false, resolutions);
00059     bool ret = matcherD2D.match(ndmap,local_map,Tout,true);
00060 
00061     std::cout<<"Transform: \n"<<Tout.matrix()<<std::endl;
00062 
00063 }


ndt_registration
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Mon Oct 6 2014 03:19:29