Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include "jsk_pcl_ros/cluster_point_indices_decomposer_z_axis.h"
00036 #include <pluginlib/class_list_macros.h>
00037 #include <pcl/filters/extract_indices.h>
00038
00039 #include <pcl/common/centroid.h>
00040
00041 namespace jsk_pcl_ros
00042 {
00043 void ClusterPointIndicesDecomposerZAxis::sortIndicesOrder
00044 (pcl::PointCloud<pcl::PointXYZ>::Ptr input,
00045 std::vector<pcl::IndicesPtr> indices_array,
00046 std::vector<pcl::IndicesPtr> &output_array)
00047 {
00048 output_array.resize(indices_array.size());
00049 std::vector<double> z_values;
00050 pcl::ExtractIndices<pcl::PointXYZ> ex;
00051 ex.setInputCloud(input);
00052 for (size_t i = 0; i < indices_array.size(); i++)
00053 {
00054 Eigen::Vector4f center;
00055 ex.setIndices(indices_array[i]);
00056 pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
00057 ex.filter(*tmp);
00058 pcl::compute3DCentroid(*tmp, center);
00059 z_values.push_back(center[2]);
00060 }
00061
00062
00063 for (size_t i = 0; i < indices_array.size(); i++)
00064 {
00065 size_t minimum_index = 0;
00066 double minimum_value = DBL_MAX;
00067 for (size_t j = 0; j < indices_array.size(); j++)
00068 {
00069 if (z_values[j] < minimum_value)
00070 {
00071 minimum_value = z_values[j];
00072 minimum_index = j;
00073 }
00074 }
00075
00076 output_array[i] = indices_array[minimum_index];
00077 z_values[minimum_index] = DBL_MAX;
00078 }
00079 }
00080 }
00081
00082
00083 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::ClusterPointIndicesDecomposerZAxis,
00084 nodelet::Nodelet);