Go to the documentation of this file.00001 #include "obstacle_detection_normals_alg.h"
00002 using namespace std;
00003
00004 ObstacleDetectionNormalsAlgorithm::ObstacleDetectionNormalsAlgorithm(void)
00005 {
00006 }
00007
00008 ObstacleDetectionNormalsAlgorithm::~ObstacleDetectionNormalsAlgorithm(void)
00009 {
00010 }
00011
00012 void ObstacleDetectionNormalsAlgorithm::config_update(Config& new_cfg, uint32_t level)
00013 {
00014 this->lock();
00015
00016
00017 this->config_=new_cfg;
00018
00019 this->unlock();
00020 }
00021
00022
00023
00024 void ObstacleDetectionNormalsAlgorithm::cloud_all(int KSearch, float normal_z,
00025 float normal_y, float normal_x, float min_dist,
00026 const pcl::PointCloud<pcl::PointXYZ>& cloud,
00027 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& cloud_out, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& cloud_obs)
00028 {
00029
00030
00031
00032 pcl::PointCloud<pcl::Normal>::Ptr output (new pcl::PointCloud<pcl::Normal>);
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048 pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00079
00080 std::vector<int> indices;
00081 indices.resize (cloud.points.size());
00082 for (size_t i = 0; i < indices.size (); ++i) { indices[i] = i; }
00083
00084 pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
00085
00086
00087 boost::shared_ptr< vector<int> > indicesptr(new vector<int> (indices));
00088
00089 n.setIndices(indicesptr);
00090 n.setSearchMethod(tree);
00091 n.setKSearch(KSearch);
00092
00093
00094 n.setInputCloud(cloud.makeShared());
00095 n.compute(*output);
00096
00097 pcl::concatenateFields (cloud, *output, *cloud_out);
00098
00099
00100
00101 float e=0.0;
00102
00103 *cloud_obs=*cloud_out;
00104
00105 for (size_t rowIndex=0, pointIndex=0; rowIndex<cloud_out->height; ++rowIndex)
00106 {
00107 for (size_t colIndex=0; colIndex<cloud_out->width; ++colIndex, ++pointIndex)
00108 {
00109
00110 if (cloud_out->points[pointIndex].z > min_dist)
00111 {
00112 if (cloud_out->points[pointIndex].normal_z < normal_z &&
00113 cloud_out->points[pointIndex].normal_y < normal_y &&
00114 cloud_out->points[pointIndex].normal_x < normal_x)
00115 {
00116 cloud_out->points[pointIndex].r=0;
00117 cloud_out->points[pointIndex].g=255;
00118 cloud_out->points[pointIndex].b=0;
00119 cloud_obs->points[pointIndex].x=std::numeric_limits<float>::quiet_NaN();;
00120 cloud_obs->points[pointIndex].y=std::numeric_limits<float>::quiet_NaN();;
00121 cloud_obs->points[pointIndex].z=std::numeric_limits<float>::quiet_NaN();;
00122 }
00123 else
00124 {
00125 cloud_out->points[pointIndex].r=255;
00126 cloud_out->points[pointIndex].g=0;
00127 cloud_out->points[pointIndex].b=0;
00128
00129 e=e+1.0;
00130 }
00131 }
00132 else
00133 {
00134 cloud_out->points[pointIndex].r=0;
00135 cloud_out->points[pointIndex].g=0;
00136 cloud_out->points[pointIndex].b=255;
00137 }
00138 }
00139 }
00140 e=(e/float(cloud_out->width*cloud_out->height))*100;
00141
00142 }
00143