Go to the documentation of this file.00001 #include <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
00016
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