octTreeBuilder.cc
Go to the documentation of this file.
00001 #include <ndt_map/oc_tree.h>
00002 
00003 #include "ros/ros.h"
00004 #include "pcl/point_cloud.h"
00005 #include "sensor_msgs/PointCloud2.h"
00006 #include "pcl/io/pcd_io.h"
00007 #include "pcl/features/feature.h"
00008 #include "Eigen/Core"
00009 #include <cstdio>
00010 
00011 static int ctr = 0;
00012 
00013 void octCallback(const sensor_msgs::PointCloud2ConstPtr &msg)
00014 {
00015     //build the oct tree and save to disk, update counter
00016 //    pcl::PointCloud
00017     pcl::PointCloud<pcl::PointXYZ> cloud;
00018     pcl::fromROSMsg(*msg,cloud);
00019 
00020     Eigen::Vector4f centroid;
00021     pcl::compute3DCentroid(cloud,centroid);
00022     pcl::PointXYZ c;
00023     c.x=centroid(0);
00024     c.y=centroid(1);
00025     c.z=centroid(2);
00026 
00027     lslgeneric::OctTree<pcl::PointXYZ> tr(c,10,10,10,new lslgeneric::NDTCell<pcl::PointXYZ>());
00028 
00029     for(unsigned int i=0; i<cloud.points.size(); ++i)
00030     {
00031         tr.addPoint(cloud.points[i]);
00032     }
00033 
00034     char fname[50];
00035     snprintf(fname,49,"oct_tree%05d.wrl",ctr);
00036     FILE* fout = fopen (fname,"w");
00037     if(fout == NULL)
00038     {
00039         return;
00040     }
00041     fprintf(fout,"#VRML V2.0 utf8\n");
00042     tr.writeToVRML(fout);
00043     lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloud);
00044     ctr++;
00045     fclose(fout);
00046 
00047 
00048 }
00049 
00050 int
00051 main (int argc, char** argv)
00052 {
00053 
00054     ros::init(argc, argv, "oct_builder");
00055     ros::NodeHandle n;
00056     ros::Subscriber chatter_sub = n.subscribe("points2_in", 10, octCallback);
00057     ros::spin();
00058 
00059     return (0);
00060 }
00061 
00062 
00063 


ndt_map
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Mon Oct 6 2014 03:18:54