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 pcl::PointCloud<pcl::PointXYZ> cloud, cloud_offset;
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
00046 if(argc == 9)
00047 {
00048
00049 gettimeofday(&tv_start,NULL);
00050
00051 cloud = lslgeneric::readVRML<pcl::PointXYZ>(argv[7]);
00052 cloud_offset = lslgeneric::readVRML<pcl::PointXYZ>(argv[8]);
00053
00054 Tout = Eigen::Translation<double,3>(xoffset,yoffset,zoffset)*
00055 Eigen::AngleAxis<double>(roll,Eigen::Vector3d::UnitX()) *
00056 Eigen::AngleAxis<double>(pitch,Eigen::Vector3d::UnitY()) *
00057 Eigen::AngleAxis<double>(yaw,Eigen::Vector3d::UnitZ()) ;
00058
00059
00060 lslgeneric::NDTMatcherD2D<pcl::PointXYZ,pcl::PointXYZ> matcherD2D(false, false, resolutions);
00061 bool ret = matcherD2D.match(cloud,cloud_offset,Tout,true);
00062
00063 std::cout<<"Transform: \n"<<Tout.matrix()<<std::endl;
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076 snprintf(fname,49,"c_offset.wrl");
00077 fout = fopen(fname,"w");
00078 fprintf(fout,"#VRML V2.0 utf8\n");
00079 lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloud,Eigen::Vector3d(1,0,0));
00080 lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloud_offset,Eigen::Vector3d(1,1,1));
00081
00082 lslgeneric::transformPointCloudInPlace<pcl::PointXYZ>(Tout,cloud_offset);
00083 lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloud_offset,Eigen::Vector3d(0,1,0));
00084 fclose(fout);
00085 }
00086 }