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 <iostream>
00042 #include <pcl/common/io.h>
00043 #include <pcl/filters/impl/voxel_grid.hpp>
00044
00045 typedef Eigen::Array<size_t, 4, 1> Array4size_t;
00046
00048 void
00049 pcl::getMinMax3D (const pcl::PCLPointCloud2ConstPtr &cloud, int x_idx, int y_idx, int z_idx,
00050 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
00051 {
00052
00053 if (cloud->fields[x_idx].datatype != pcl::PCLPointField::FLOAT32 ||
00054 cloud->fields[y_idx].datatype != pcl::PCLPointField::FLOAT32 ||
00055 cloud->fields[z_idx].datatype != pcl::PCLPointField::FLOAT32)
00056 {
00057 PCL_ERROR ("[pcl::getMinMax3D] XYZ dimensions are not float type!\n");
00058 return;
00059 }
00060
00061 Eigen::Array4f min_p, max_p;
00062 min_p.setConstant (FLT_MAX);
00063 max_p.setConstant (-FLT_MAX);
00064
00065 size_t nr_points = cloud->width * cloud->height;
00066
00067 Eigen::Array4f pt = Eigen::Array4f::Zero ();
00068 Array4size_t xyz_offset (cloud->fields[x_idx].offset, cloud->fields[y_idx].offset, cloud->fields[z_idx].offset, 0);
00069
00070 for (size_t cp = 0; cp < nr_points; ++cp)
00071 {
00072
00073 memcpy (&pt[0], &cloud->data[xyz_offset[0]], sizeof (float));
00074 memcpy (&pt[1], &cloud->data[xyz_offset[1]], sizeof (float));
00075 memcpy (&pt[2], &cloud->data[xyz_offset[2]], sizeof (float));
00076
00077 if (!pcl_isfinite (pt[0]) ||
00078 !pcl_isfinite (pt[1]) ||
00079 !pcl_isfinite (pt[2]))
00080 {
00081 xyz_offset += cloud->point_step;
00082 continue;
00083 }
00084 xyz_offset += cloud->point_step;
00085 min_p = (min_p.min) (pt);
00086 max_p = (max_p.max) (pt);
00087 }
00088 min_pt = min_p;
00089 max_pt = max_p;
00090 }
00091
00093 void
00094 pcl::getMinMax3D (const pcl::PCLPointCloud2ConstPtr &cloud, int x_idx, int y_idx, int z_idx,
00095 const std::string &distance_field_name, float min_distance, float max_distance,
00096 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative)
00097 {
00098
00099 if (cloud->fields[x_idx].datatype != pcl::PCLPointField::FLOAT32 ||
00100 cloud->fields[y_idx].datatype != pcl::PCLPointField::FLOAT32 ||
00101 cloud->fields[z_idx].datatype != pcl::PCLPointField::FLOAT32)
00102 {
00103 PCL_ERROR ("[pcl::getMinMax3D] XYZ dimensions are not float type!\n");
00104 return;
00105 }
00106
00107 Eigen::Array4f min_p, max_p;
00108 min_p.setConstant (FLT_MAX);
00109 max_p.setConstant (-FLT_MAX);
00110
00111
00112 int distance_idx = pcl::getFieldIndex (*cloud, distance_field_name);
00113
00114
00115 if (cloud->fields[distance_idx].datatype != pcl::PCLPointField::FLOAT32)
00116 {
00117 PCL_ERROR ("[pcl::getMinMax3D] Filtering dimensions is not float type!\n");
00118 return;
00119 }
00120
00121 size_t nr_points = cloud->width * cloud->height;
00122
00123 Eigen::Array4f pt = Eigen::Array4f::Zero ();
00124 Array4size_t xyz_offset (cloud->fields[x_idx].offset,
00125 cloud->fields[y_idx].offset,
00126 cloud->fields[z_idx].offset,
00127 0);
00128 float distance_value = 0;
00129 for (size_t cp = 0; cp < nr_points; ++cp)
00130 {
00131 size_t point_offset = cp * cloud->point_step;
00132
00133
00134 memcpy (&distance_value, &cloud->data[point_offset + cloud->fields[distance_idx].offset], sizeof (float));
00135
00136 if (limit_negative)
00137 {
00138
00139 if ((distance_value < max_distance) && (distance_value > min_distance))
00140 {
00141 xyz_offset += cloud->point_step;
00142 continue;
00143 }
00144 }
00145 else
00146 {
00147
00148 if ((distance_value > max_distance) || (distance_value < min_distance))
00149 {
00150 xyz_offset += cloud->point_step;
00151 continue;
00152 }
00153 }
00154
00155
00156 memcpy (&pt[0], &cloud->data[xyz_offset[0]], sizeof (float));
00157 memcpy (&pt[1], &cloud->data[xyz_offset[1]], sizeof (float));
00158 memcpy (&pt[2], &cloud->data[xyz_offset[2]], sizeof (float));
00159
00160 if (!pcl_isfinite (pt[0]) ||
00161 !pcl_isfinite (pt[1]) ||
00162 !pcl_isfinite (pt[2]))
00163 {
00164 xyz_offset += cloud->point_step;
00165 continue;
00166 }
00167 xyz_offset += cloud->point_step;
00168 min_p = (min_p.min) (pt);
00169 max_p = (max_p.max) (pt);
00170 }
00171 min_pt = min_p;
00172 max_pt = max_p;
00173 }
00174
00176 void
00177 pcl::VoxelGrid<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
00178 {
00179
00180 if (x_idx_ == -1 || y_idx_ == -1 || z_idx_ == -1)
00181 {
00182 PCL_ERROR ("[pcl::%s::applyFilter] Input dataset doesn't have x-y-z coordinates!\n", getClassName ().c_str ());
00183 output.width = output.height = 0;
00184 output.data.clear ();
00185 return;
00186 }
00187 size_t nr_points = input_->width * input_->height;
00188
00189
00190 output.height = 1;
00191 if (downsample_all_data_)
00192 {
00193 output.fields = input_->fields;
00194 output.point_step = input_->point_step;
00195 }
00196 else
00197 {
00198 output.fields.resize (4);
00199
00200 output.fields[0] = input_->fields[x_idx_];
00201 output.fields[0].offset = 0;
00202
00203 output.fields[1] = input_->fields[y_idx_];
00204 output.fields[1].offset = 4;
00205
00206 output.fields[2] = input_->fields[z_idx_];
00207 output.fields[2].offset = 8;
00208
00209 output.point_step = 12;
00210 }
00211 output.is_bigendian = input_->is_bigendian;
00212 output.row_step = input_->row_step;
00213 output.is_dense = true;
00214
00215 Eigen::Vector4f min_p, max_p;
00216
00217 if (!filter_field_name_.empty ())
00218 getMinMax3D (input_, x_idx_, y_idx_, z_idx_, filter_field_name_,
00219 static_cast<float> (filter_limit_min_),
00220 static_cast<float> (filter_limit_max_), min_p, max_p, filter_limit_negative_);
00221 else
00222 getMinMax3D (input_, x_idx_, y_idx_, z_idx_, min_p, max_p);
00223
00224
00225 int64_t dx = static_cast<int64_t>((max_p[0] - min_p[0]) * inverse_leaf_size_[0])+1;
00226 int64_t dy = static_cast<int64_t>((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1;
00227 int64_t dz = static_cast<int64_t>((max_p[2] - min_p[2]) * inverse_leaf_size_[2])+1;
00228
00229 if( (dx*dy*dz) > static_cast<int64_t>(std::numeric_limits<int32_t>::max()) )
00230 {
00231 PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName().c_str());
00232
00233
00234
00235 }
00236
00237
00238 min_b_[0] = static_cast<int> (floor (min_p[0] * inverse_leaf_size_[0]));
00239 max_b_[0] = static_cast<int> (floor (max_p[0] * inverse_leaf_size_[0]));
00240 min_b_[1] = static_cast<int> (floor (min_p[1] * inverse_leaf_size_[1]));
00241 max_b_[1] = static_cast<int> (floor (max_p[1] * inverse_leaf_size_[1]));
00242 min_b_[2] = static_cast<int> (floor (min_p[2] * inverse_leaf_size_[2]));
00243 max_b_[2] = static_cast<int> (floor (max_p[2] * inverse_leaf_size_[2]));
00244
00245
00246 div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones ();
00247 div_b_[3] = 0;
00248
00249 std::vector<cloud_point_index_idx> index_vector;
00250 index_vector.reserve (nr_points);
00251
00252
00253 Array4size_t xyz_offset (input_->fields[x_idx_].offset,
00254 input_->fields[y_idx_].offset,
00255 input_->fields[z_idx_].offset,
00256 0);
00257 divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);
00258 Eigen::Vector4f pt = Eigen::Vector4f::Zero ();
00259
00260 int centroid_size = 4;
00261 if (downsample_all_data_)
00262 centroid_size = static_cast<int> (input_->fields.size ());
00263
00264 int rgba_index = -1;
00265
00266
00267
00268 for (int d = 0; d < centroid_size; ++d)
00269 {
00270 if (input_->fields[d].name == std::string ("rgba") || input_->fields[d].name == std::string ("rgb"))
00271 {
00272 rgba_index = d;
00273 centroid_size += 3;
00274 break;
00275 }
00276 }
00277
00278
00279 if (!filter_field_name_.empty ())
00280 {
00281
00282 int distance_idx = pcl::getFieldIndex (*input_, filter_field_name_);
00283
00284
00285 if (input_->fields[distance_idx].datatype != pcl::PCLPointField::FLOAT32)
00286 {
00287 PCL_ERROR ("[pcl::%s::applyFilter] Distance filtering requested, but distances are not float/double in the dataset! Only FLOAT32/FLOAT64 distances are supported right now.\n", getClassName ().c_str ());
00288 output.width = output.height = 0;
00289 output.data.clear ();
00290 return;
00291 }
00292
00293
00294
00295
00296 float distance_value = 0;
00297 for (size_t cp = 0; cp < nr_points; ++cp)
00298 {
00299 size_t point_offset = cp * input_->point_step;
00300
00301 memcpy (&distance_value, &input_->data[point_offset + input_->fields[distance_idx].offset], sizeof (float));
00302
00303 if (filter_limit_negative_)
00304 {
00305
00306 if (distance_value < filter_limit_max_ && distance_value > filter_limit_min_)
00307 {
00308 xyz_offset += input_->point_step;
00309 continue;
00310 }
00311 }
00312 else
00313 {
00314
00315 if (distance_value > filter_limit_max_ || distance_value < filter_limit_min_)
00316 {
00317 xyz_offset += input_->point_step;
00318 continue;
00319 }
00320 }
00321
00322
00323 memcpy (&pt[0], &input_->data[xyz_offset[0]], sizeof (float));
00324 memcpy (&pt[1], &input_->data[xyz_offset[1]], sizeof (float));
00325 memcpy (&pt[2], &input_->data[xyz_offset[2]], sizeof (float));
00326
00327
00328 if (!pcl_isfinite (pt[0]) ||
00329 !pcl_isfinite (pt[1]) ||
00330 !pcl_isfinite (pt[2]))
00331 {
00332 xyz_offset += input_->point_step;
00333 continue;
00334 }
00335
00336 int ijk0 = static_cast<int> (floor (pt[0] * inverse_leaf_size_[0]) - min_b_[0]);
00337 int ijk1 = static_cast<int> (floor (pt[1] * inverse_leaf_size_[1]) - min_b_[1]);
00338 int ijk2 = static_cast<int> (floor (pt[2] * inverse_leaf_size_[2]) - min_b_[2]);
00339
00340 int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
00341 index_vector.push_back (cloud_point_index_idx (idx, static_cast<unsigned int> (cp)));
00342
00343 xyz_offset += input_->point_step;
00344 }
00345 }
00346
00347 else
00348 {
00349
00350 for (size_t cp = 0; cp < nr_points; ++cp)
00351 {
00352
00353 memcpy (&pt[0], &input_->data[xyz_offset[0]], sizeof (float));
00354 memcpy (&pt[1], &input_->data[xyz_offset[1]], sizeof (float));
00355 memcpy (&pt[2], &input_->data[xyz_offset[2]], sizeof (float));
00356
00357
00358 if (!pcl_isfinite (pt[0]) ||
00359 !pcl_isfinite (pt[1]) ||
00360 !pcl_isfinite (pt[2]))
00361 {
00362 xyz_offset += input_->point_step;
00363 continue;
00364 }
00365
00366 int ijk0 = static_cast<int> (floor (pt[0] * inverse_leaf_size_[0]) - min_b_[0]);
00367 int ijk1 = static_cast<int> (floor (pt[1] * inverse_leaf_size_[1]) - min_b_[1]);
00368 int ijk2 = static_cast<int> (floor (pt[2] * inverse_leaf_size_[2]) - min_b_[2]);
00369
00370 int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
00371 index_vector.push_back (cloud_point_index_idx (idx, static_cast<unsigned int> (cp)));
00372 xyz_offset += input_->point_step;
00373 }
00374 }
00375
00376
00377
00378 std::sort (index_vector.begin (), index_vector.end (), std::less<cloud_point_index_idx> ());
00379
00380
00381
00382 size_t total = 0;
00383 size_t index = 0;
00384 while (index < index_vector.size ())
00385 {
00386 size_t i = index + 1;
00387 while (i < index_vector.size () && index_vector[i].idx == index_vector[index].idx)
00388 ++i;
00389 ++total;
00390 index = i;
00391 }
00392
00393
00394 output.width = uint32_t (total);
00395 output.row_step = output.point_step * output.width;
00396 output.data.resize (output.width * output.point_step);
00397
00398 if (save_leaf_layout_)
00399 {
00400 try
00401 {
00402
00403 uint32_t new_layout_size = div_b_[0]*div_b_[1]*div_b_[2];
00404
00405 uint32_t reinit_size = std::min (static_cast<unsigned int> (new_layout_size), static_cast<unsigned int> (leaf_layout_.size()));
00406 for (uint32_t i = 0; i < reinit_size; i++)
00407 {
00408 leaf_layout_[i] = -1;
00409 }
00410 leaf_layout_.resize (new_layout_size, -1);
00411 }
00412 catch (std::bad_alloc&)
00413 {
00414 throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout",
00415 "voxel_grid.cpp", "applyFilter");
00416 }
00417 catch (std::length_error&)
00418 {
00419 throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout",
00420 "voxel_grid.cpp", "applyFilter");
00421 }
00422 }
00423
00424
00425 if (downsample_all_data_)
00426 xyz_offset = Array4size_t (output.fields[x_idx_].offset,
00427 output.fields[y_idx_].offset,
00428 output.fields[z_idx_].offset,
00429 0);
00430 else
00431
00432 xyz_offset = Array4size_t (0, 4, 8, 12);
00433
00434 index=0;
00435 Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
00436 Eigen::VectorXf temporary = Eigen::VectorXf::Zero (centroid_size);
00437
00438 for (size_t cp = 0; cp < index_vector.size ();)
00439 {
00440 size_t point_offset = index_vector[cp].cloud_point_index * input_->point_step;
00441
00442 if (!downsample_all_data_)
00443 {
00444 memcpy (&pt[0], &input_->data[point_offset+input_->fields[x_idx_].offset], sizeof (float));
00445 memcpy (&pt[1], &input_->data[point_offset+input_->fields[y_idx_].offset], sizeof (float));
00446 memcpy (&pt[2], &input_->data[point_offset+input_->fields[z_idx_].offset], sizeof (float));
00447 centroid[0] = pt[0];
00448 centroid[1] = pt[1];
00449 centroid[2] = pt[2];
00450 centroid[3] = 0;
00451 }
00452 else
00453 {
00454
00455
00456 if (rgba_index >= 0)
00457 {
00458 pcl::RGB rgb;
00459 memcpy (&rgb, &input_->data[point_offset + input_->fields[rgba_index].offset], sizeof (RGB));
00460 centroid[centroid_size-3] = rgb.r;
00461 centroid[centroid_size-2] = rgb.g;
00462 centroid[centroid_size-1] = rgb.b;
00463 }
00464
00465 for (size_t d = 0; d < input_->fields.size (); ++d)
00466 memcpy (¢roid[d], &input_->data[point_offset + input_->fields[d].offset], field_sizes_[d]);
00467 }
00468
00469 size_t i = cp + 1;
00470 while (i < index_vector.size () && index_vector[i].idx == index_vector[cp].idx)
00471 {
00472 size_t point_offset = index_vector[i].cloud_point_index * input_->point_step;
00473 if (!downsample_all_data_)
00474 {
00475 memcpy (&pt[0], &input_->data[point_offset+input_->fields[x_idx_].offset], sizeof (float));
00476 memcpy (&pt[1], &input_->data[point_offset+input_->fields[y_idx_].offset], sizeof (float));
00477 memcpy (&pt[2], &input_->data[point_offset+input_->fields[z_idx_].offset], sizeof (float));
00478 centroid[0] += pt[0];
00479 centroid[1] += pt[1];
00480 centroid[2] += pt[2];
00481 }
00482 else
00483 {
00484
00485
00486 if (rgba_index >= 0)
00487 {
00488 pcl::RGB rgb;
00489 memcpy (&rgb, &input_->data[point_offset + input_->fields[rgba_index].offset], sizeof (RGB));
00490 temporary[centroid_size-3] = rgb.r;
00491 temporary[centroid_size-2] = rgb.g;
00492 temporary[centroid_size-1] = rgb.b;
00493 }
00494
00495 for (size_t d = 0; d < input_->fields.size (); ++d)
00496 memcpy (&temporary[d], &input_->data[point_offset + input_->fields[d].offset], field_sizes_[d]);
00497 centroid += temporary;
00498 }
00499 ++i;
00500 }
00501
00502
00503 if (save_leaf_layout_)
00504 leaf_layout_[index_vector[cp].idx] = static_cast<int> (index);
00505
00506
00507 centroid /= static_cast<float> (i - cp);
00508
00509
00510 if (!downsample_all_data_)
00511 {
00512
00513 memcpy (&output.data[xyz_offset[0]], ¢roid[0], sizeof (float));
00514 memcpy (&output.data[xyz_offset[1]], ¢roid[1], sizeof (float));
00515 memcpy (&output.data[xyz_offset[2]], ¢roid[2], sizeof (float));
00516 xyz_offset += output.point_step;
00517 }
00518 else
00519 {
00520 size_t point_offset = index * output.point_step;
00521
00522 for (size_t d = 0; d < output.fields.size (); ++d)
00523 memcpy (&output.data[point_offset + output.fields[d].offset], ¢roid[d], field_sizes_[d]);
00524
00525
00526
00527 if (rgba_index >= 0)
00528 {
00529 float r = centroid[centroid_size-3], g = centroid[centroid_size-2], b = centroid[centroid_size-1];
00530 int rgb = (static_cast<int> (r) << 16) | (static_cast<int> (g) << 8) | static_cast<int> (b);
00531 memcpy (&output.data[point_offset + output.fields[rgba_index].offset], &rgb, sizeof (float));
00532 }
00533 }
00534 cp = i;
00535 ++index;
00536 }
00537 }
00538
00539 #ifndef PCL_NO_PRECOMPILE
00540 #include <pcl/impl/instantiate.hpp>
00541 #include <pcl/point_types.h>
00542
00543
00544 PCL_INSTANTIATE(getMinMax3D, PCL_XYZ_POINT_TYPES)
00545 PCL_INSTANTIATE(VoxelGrid, PCL_XYZ_POINT_TYPES)
00546
00547 #endif // PCL_NO_PRECOMPILE
00548