Go to the documentation of this file.00001
00059 #include <pcl/point_types.h>
00060 #include <pcl/io/pcd_io.h>
00061
00062 #include "cob_3d_mapping_common/stop_watch.h"
00063 #include "cob_3d_mapping_common/point_types.h"
00064 #include "cob_3d_features/organized_normal_estimation.h"
00065 #include "cob_3d_features/organized_curvature_estimation.h"
00066 #include "cob_3d_features/organized_normal_estimation_omp.h"
00067 #include "cob_3d_features/organized_curvature_estimation_omp.h"
00068
00069 using namespace pcl;
00070
00071 int main(int argc, char** argv)
00072 {
00073 PointCloud<PointXYZRGB>::Ptr p(new PointCloud<PointXYZRGB>);
00074 PointCloud<Normal>::Ptr n(new PointCloud<Normal>);
00075 PointCloud<PointLabel>::Ptr l(new PointCloud<PointLabel>);
00076 PointCloud<PrincipalCurvatures>::Ptr pc(new PointCloud<PrincipalCurvatures>);
00077
00078 PrecisionStopWatch t;
00079 std::string file_ = argv[1];
00080 PCDReader r;
00081 if (r.read(file_, *p) == -1) return(0);
00082
00083 t.precisionStart();
00084 cob_3d_features::OrganizedNormalEstimationOMP<PointXYZRGB, Normal, PointLabel>one;
00085 one.setInputCloud(p);
00086 one.setOutputLabels(l);
00087 one.setPixelSearchRadius(8,1,2);
00088 one.setSkipDistantPointThreshold(12);
00089 one.compute(*n);
00090 std::cout << t.precisionStop() << "s\t for organized curvature estimation" << std::endl;
00091
00092 t.precisionStart();
00093 cob_3d_features::OrganizedCurvatureEstimationOMP<PointXYZRGB,Normal,PointLabel,PrincipalCurvatures>oce;
00094 oce.setInputCloud(p);
00095 oce.setInputNormals(n);
00096 oce.setOutputLabels(l);
00097 oce.setPixelSearchRadius(8,1,2);
00098 oce.setSkipDistantPointThreshold(12);
00099 oce.setEdgeClassThreshold(6);
00100 oce.compute(*pc);
00101 std::cout << t.precisionStop() << "s\t for organized curvature estimation" << std::endl;
00102
00103
00104 return 0;
00105 }