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
00036
00037
00038
00039
00040 #include <pcl/impl/instantiate.hpp>
00041 #include <pcl/point_types.h>
00042 #include <pcl/filters/extract_indices.h>
00043 #include <pcl/filters/impl/extract_indices.hpp>
00044
00046 void
00047 pcl::ExtractIndices<sensor_msgs::PointCloud2>::applyFilter (PointCloud2 &output)
00048 {
00049
00050 if (indices_->empty () || (input_->width * input_->height == 0))
00051 {
00052 output.width = output.height = 0;
00053 output.data.clear ();
00054
00055 if (negative_)
00056 output = *input_;
00057 return;
00058 }
00059 if (indices_->size () == (input_->width * input_->height))
00060 {
00061
00062 if (negative_)
00063 {
00064 output.width = output.height = 0;
00065 output.data.clear ();
00066 }
00067
00068 else
00069 output = *input_;
00070 return;
00071 }
00072
00073
00074 output.is_bigendian = input_->is_bigendian;
00075 output.point_step = input_->point_step;
00076 output.height = 1;
00077
00078 output.is_dense = false;
00079
00080 if (negative_)
00081 {
00082
00083 std::vector<int> all_indices (input_->width * input_->height);
00084 for (int i = 0; i < static_cast<int>(all_indices.size ()); ++i)
00085 all_indices[i] = i;
00086
00087 std::vector<int> indices = *indices_;
00088 std::sort (indices.begin (), indices.end ());
00089
00090
00091 std::vector<int> remaining_indices;
00092 set_difference (all_indices.begin (), all_indices.end (), indices.begin (), indices.end (),
00093 inserter (remaining_indices, remaining_indices.begin ()));
00094
00095
00096 output.width = static_cast<uint32_t> (remaining_indices.size ());
00097 output.data.resize (remaining_indices.size () * output.point_step);
00098 for (size_t i = 0; i < remaining_indices.size (); ++i)
00099 memcpy (&output.data[i * output.point_step], &input_->data[remaining_indices[i] * output.point_step], output.point_step);
00100 }
00101 else
00102 {
00103
00104 output.width = static_cast<uint32_t> (indices_->size ());
00105 output.data.resize (indices_->size () * output.point_step);
00106 for (size_t i = 0; i < indices_->size (); ++i)
00107 memcpy (&output.data[i * output.point_step], &input_->data[(*indices_)[i] * output.point_step], output.point_step);
00108 }
00109 output.row_step = output.point_step * output.width;
00110 }
00111
00113 void
00114 pcl::ExtractIndices<sensor_msgs::PointCloud2>::applyFilter (std::vector<int> &indices)
00115 {
00116 if (negative_)
00117 {
00118
00119 if (indices_->size () == (input_->width * input_->height))
00120 {
00121
00122 indices.clear ();
00123 return;
00124 }
00125
00126
00127 std::vector<int> indices_fullset (input_->width * input_->height);
00128 for (int p_it = 0; p_it < static_cast<int> (indices_fullset.size ()); ++p_it)
00129 indices_fullset[p_it] = p_it;
00130
00131
00132 if (indices_->empty () || (input_->width * input_->height == 0))
00133 {
00134
00135 indices = indices_fullset;
00136 return;
00137 }
00138
00139
00140
00141 std::vector<int> indices_subset = *indices_;
00142 std::sort (indices_subset.begin (), indices_subset.end ());
00143
00144
00145 set_difference (indices_fullset.begin (), indices_fullset.end (), indices_subset.begin (), indices_subset.end (), inserter (indices, indices.begin ()));
00146 }
00147 else
00148 indices = *indices_;
00149 }
00150
00151 #ifdef PCL_ONLY_CORE_POINT_TYPES
00152 PCL_INSTANTIATE(ExtractIndices, (pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGB)(pcl::PointXYZRGBA)(pcl::PointXYZRGBNormal))
00153 #else
00154 PCL_INSTANTIATE(ExtractIndices, PCL_POINT_TYPES)
00155 #endif
00156