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


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