00001 #include <ndt_map/ndt_map.h> 00002 #include <ndt_map/lazy_grid.h> 00003 #include <pointcloud_vrml/pointcloud_utils.h> 00004 00005 #include "pcl/point_cloud.h" 00006 #include "pcl/io/pcd_io.h" 00007 #include "pcl/features/feature.h" 00008 #include <cstdio> 00009 #include <cstring> 00010 00011 // #include <opencv/cv.h> 00012 // #include <opencv/highgui.h> 00013 // #include <boost/shared_ptr.hpp> 00014 // #include <boost/thread/mutex.hpp> 00015 // #include <cv_bridge/CvBridge.h> 00016 // #include <cv_bridge/cv_bridge.h> 00017 // #include <sensor_msgs/Image.h> 00018 00019 #include <Eigen/Eigen> 00020 00021 using namespace std; 00022 00023 int main (int argc, char** argv) 00024 { 00025 if(argc < 1) 00026 { 00027 cout << "[ USAGE ] jffLoadTest \n"; 00028 exit(1); 00029 } 00030 00031 cout << "Started loadTest\n"; 00032 00033 char fname[] = "test_jff.wrl"; 00034 00035 lslgeneric::NDTMap<pcl::PointXYZ> nd(new lslgeneric::LazyGrid<pcl::PointXYZ>(0.2)); 00036 if (nd.loadFromJFF("LazyGrid.jff") < 0) 00037 cout << "loading from jff failed\n"; 00038 nd.writeToVRML(fname); 00039 00040 cout << "Finished loadTest\n"; 00041 00042 return 0; 00043 } 00044