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
00041 #include <pcl/filters/impl/passthrough.hpp>
00042
00044 void
00045 pcl::PassThrough<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
00046 {
00047 if (!input_)
00048 {
00049 PCL_ERROR ("[pcl::%s::applyFilter] Input dataset not given!\n", getClassName ().c_str ());
00050 output.width = output.height = 0;
00051 output.data.clear ();
00052 return;
00053 }
00054
00055
00056 if (x_idx_ == -1 || y_idx_ == -1 || z_idx_ == -1)
00057 {
00058 PCL_ERROR ("[pcl::%s::applyFilter] Input dataset doesn't have x-y-z coordinates!\n", getClassName ().c_str ());
00059 output.width = output.height = 0;
00060 output.data.clear ();
00061 return;
00062 }
00063
00064 int nr_points = input_->width * input_->height;
00065
00066
00067 if (keep_organized_)
00068 {
00069 if (filter_field_name_.empty ())
00070 {
00071
00072
00073 output = *input_;
00074 return;
00075 }
00076
00077 output.width = input_->width;
00078 output.height = input_->height;
00079
00080 if (!pcl_isfinite (user_filter_value_))
00081 output.is_dense = false;
00082 else
00083 output.is_dense = input_->is_dense;
00084 }
00085 else
00086 {
00087
00088 output.height = 1;
00089
00090 output.is_dense = true;
00091 }
00092 output.row_step = input_->row_step;
00093 output.point_step = input_->point_step;
00094 output.is_bigendian = input_->is_bigendian;
00095 output.data.resize (input_->data.size ());
00096
00097 removed_indices_->resize (input_->data.size ());
00098
00099 int nr_p = 0;
00100 int nr_removed_p = 0;
00101
00102 Eigen::Array4i xyz_offset (input_->fields[x_idx_].offset, input_->fields[y_idx_].offset,
00103 input_->fields[z_idx_].offset, 0);
00104
00105 Eigen::Vector4f pt = Eigen::Vector4f::Zero ();
00106
00107 if (!filter_field_name_.empty ())
00108 {
00109
00110 int distance_idx = pcl::getFieldIndex (*input_, filter_field_name_);
00111 if (distance_idx == -1)
00112 {
00113 PCL_WARN ("[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().c_str (), distance_idx);
00114 output.width = output.height = 0;
00115 output.data.clear ();
00116 return;
00117 }
00118
00119
00120 if (input_->fields[distance_idx].datatype != pcl::PCLPointField::FLOAT32)
00121 {
00122 PCL_ERROR ("[pcl::%s::downsample] Distance filtering requested, but distances are not float/double in the dataset! Only FLOAT32/FLOAT64 distances are supported right now.\n", getClassName ().c_str ());
00123 output.width = output.height = 0;
00124 output.data.clear ();
00125 return;
00126 }
00127
00128 float badpt = std::numeric_limits<float>::quiet_NaN ();
00129
00130 if (keep_organized_)
00131 {
00132 float distance_value = 0;
00133
00134 for (int cp = 0; cp < nr_points; ++cp, xyz_offset += input_->point_step)
00135 {
00136
00137 memcpy (&output.data[cp * output.point_step], &input_->data[cp * output.point_step], output.point_step);
00138
00139
00140 memcpy (&distance_value, &input_->data[cp * input_->point_step + input_->fields[distance_idx].offset],
00141 sizeof (float));
00142
00143 if (filter_limit_negative_)
00144 {
00145
00146 if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_))
00147 {
00148
00149 memcpy (&output.data[xyz_offset[0]], &badpt, sizeof (float));
00150 memcpy (&output.data[xyz_offset[1]], &badpt, sizeof (float));
00151 memcpy (&output.data[xyz_offset[2]], &badpt, sizeof (float));
00152 continue;
00153 }
00154 else
00155 {
00156 if (extract_removed_indices_)
00157 (*removed_indices_)[nr_removed_p++] = cp;
00158 }
00159 }
00160 else
00161 {
00162
00163 if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_))
00164 {
00165
00166 memcpy (&output.data[xyz_offset[0]], &badpt, sizeof (float));
00167 memcpy (&output.data[xyz_offset[1]], &badpt, sizeof (float));
00168 memcpy (&output.data[xyz_offset[2]], &badpt, sizeof (float));
00169 continue;
00170 }
00171 else
00172 {
00173 if (extract_removed_indices_)
00174 (*removed_indices_)[nr_removed_p++] = cp;
00175 }
00176 }
00177 }
00178 }
00179
00180 else
00181 {
00182
00183 float distance_value = 0;
00184 for (int cp = 0; cp < nr_points; ++cp, xyz_offset += input_->point_step)
00185 {
00186
00187 memcpy (&distance_value, &input_->data[cp * input_->point_step + input_->fields[distance_idx].offset],
00188 sizeof(float));
00189
00190
00191 if (!pcl_isfinite (distance_value))
00192 {
00193 if (extract_removed_indices_)
00194 (*removed_indices_)[nr_removed_p++] = cp;
00195 continue;
00196 }
00197
00198 if (filter_limit_negative_)
00199 {
00200
00201 if (distance_value < filter_limit_max_ && distance_value > filter_limit_min_)
00202 {
00203 if (extract_removed_indices_)
00204 {
00205 (*removed_indices_)[nr_removed_p] = cp;
00206 nr_removed_p++;
00207 }
00208 continue;
00209 }
00210 }
00211 else
00212 {
00213
00214 if (distance_value > filter_limit_max_ || distance_value < filter_limit_min_)
00215 {
00216 if (extract_removed_indices_)
00217 {
00218 (*removed_indices_)[nr_removed_p] = cp;
00219 nr_removed_p++;
00220 }
00221 continue;
00222 }
00223 }
00224
00225
00226 memcpy (&pt[0], &input_->data[xyz_offset[0]], sizeof(float));
00227 memcpy (&pt[1], &input_->data[xyz_offset[1]], sizeof(float));
00228 memcpy (&pt[2], &input_->data[xyz_offset[2]], sizeof(float));
00229
00230
00231 if (!pcl_isfinite (pt[0]) || !pcl_isfinite (pt[1]) || !pcl_isfinite (pt[2]))
00232 {
00233 if (extract_removed_indices_)
00234 {
00235 (*removed_indices_)[nr_removed_p] = cp;
00236 nr_removed_p++;
00237 }
00238 continue;
00239 }
00240
00241
00242 memcpy (&output.data[nr_p * output.point_step], &input_->data[cp * output.point_step], output.point_step);
00243 nr_p++;
00244 }
00245 output.width = nr_p;
00246 }
00247 }
00248
00249 else
00250 {
00251 for (int cp = 0; cp < nr_points; ++cp, xyz_offset += input_->point_step)
00252 {
00253
00254 memcpy (&pt[0], &input_->data[xyz_offset[0]], sizeof(float));
00255 memcpy (&pt[1], &input_->data[xyz_offset[1]], sizeof(float));
00256 memcpy (&pt[2], &input_->data[xyz_offset[2]], sizeof(float));
00257
00258
00259 if (!pcl_isfinite (pt[0]) || !pcl_isfinite (pt[1]) || !pcl_isfinite (pt[2]))
00260 {
00261 if (extract_removed_indices_)
00262 {
00263 (*removed_indices_)[nr_removed_p] = cp;
00264 nr_removed_p++;
00265 }
00266 continue;
00267 }
00268
00269
00270 memcpy (&output.data[nr_p * output.point_step], &input_->data[cp * output.point_step], output.point_step);
00271 nr_p++;
00272 }
00273 output.width = nr_p;
00274 }
00275
00276 output.row_step = output.point_step * output.width;
00277 output.data.resize (output.width * output.height * output.point_step);
00278
00279 removed_indices_->resize (nr_removed_p);
00280 }
00281
00282 #ifndef PCL_NO_PRECOMPILE
00283 #include <pcl/impl/instantiate.hpp>
00284 #include <pcl/point_types.h>
00285
00286
00287 PCL_INSTANTIATE(PassThrough, PCL_XYZ_POINT_TYPES)
00288
00289 #endif // PCL_NO_PRECOMPILE
00290