40 #ifndef PCL_CRF_SEGMENTATION_HPP_
41 #define PCL_CRF_SEGMENTATION_HPP_
43 #include <pcl/filters/voxel_grid_label.h>
44 #include <pcl/segmentation/crf_segmentation.h>
46 #include <pcl/common/io.h>
52 template <
typename Po
intT>
60 voxel_grid_leaf_size_ (
Eigen::Vector4f (0.001f, 0.001f, 0.001f, 0.0f))
65 template <
typename Po
intT>
71 template <
typename Po
intT>
void
74 input_cloud_ = input_cloud;
78 template <
typename Po
intT>
void
81 anno_cloud_ = anno_cloud;
85 template <
typename Po
intT>
void
88 normal_cloud_ = normal_cloud;
92 template <
typename Po
intT>
void
95 voxel_grid_leaf_size_.x () = x;
96 voxel_grid_leaf_size_.y () = y;
97 voxel_grid_leaf_size_.z () = z;
101 template <
typename Po
intT>
void
105 smoothness_kernel_param_[0] = sx;
106 smoothness_kernel_param_[1] = sy;
107 smoothness_kernel_param_[2] = sz;
108 smoothness_kernel_param_[3] = w;
112 template <
typename Po
intT>
void
114 float sr,
float sg,
float sb,
117 appearance_kernel_param_[0] = sx;
118 appearance_kernel_param_[1] = sy;
119 appearance_kernel_param_[2] = sz;
120 appearance_kernel_param_[3] = sr;
121 appearance_kernel_param_[4] = sg;
122 appearance_kernel_param_[5] = sb;
123 appearance_kernel_param_[6] = w;
127 template <
typename Po
intT>
void
129 float snx,
float sny,
float snz,
132 surface_kernel_param_[0] = sx;
133 surface_kernel_param_[1] = sy;
134 surface_kernel_param_[2] = sz;
135 surface_kernel_param_[3] = snx;
136 surface_kernel_param_[4] = sny;
137 surface_kernel_param_[5] = snz;
138 surface_kernel_param_[6] = w;
143 template <
typename Po
intT>
void
148 voxel_grid_.setInputCloud (input_cloud_);
150 voxel_grid_.setLeafSize (voxel_grid_leaf_size_.x (), voxel_grid_leaf_size_.y (), voxel_grid_leaf_size_.z () );
152 voxel_grid_.setDownsampleAllData (
true);
156 voxel_grid_.filter (*filtered_cloud_);
159 if (!anno_cloud_->points.empty ())
165 vg.
setLeafSize (voxel_grid_leaf_size_.x (), voxel_grid_leaf_size_.y (), voxel_grid_leaf_size_.z () );
171 vg.
filter (*filtered_anno_);
175 if (!normal_cloud_->points.empty ())
180 vg.
setLeafSize (voxel_grid_leaf_size_.x (), voxel_grid_leaf_size_.y (), voxel_grid_leaf_size_.z () );
186 vg.
filter (*filtered_normal_);
192 template <
typename Po
intT>
void
247 data_.resize (filtered_cloud_->size ());
249 std::vector< pcl::PCLPointField > fields;
251 bool color_data =
false;
253 rgba_index = pcl::getFieldIndex<PointT> (
"rgb", fields);
254 if (rgba_index == -1)
255 rgba_index = pcl::getFieldIndex<PointT> (
"rgba", fields);
259 color_.resize (filtered_cloud_->size ());
276 for (std::size_t i = 0; i < filtered_cloud_->size (); i++)
278 Eigen::Vector3f p ((*filtered_anno_)[i].x,
279 (*filtered_anno_)[i].y,
280 (*filtered_anno_)[i].z);
281 Eigen::Vector3i c = voxel_grid_.getGridCoordinates (p.x (), p.y (), p.y ());
286 std::uint32_t rgb = *
reinterpret_cast<int*
>(&(*filtered_cloud_)[i].rgba);
287 std::uint8_t r = (rgb >> 16) & 0x0000ff;
288 std::uint8_t g = (rgb >> 8) & 0x0000ff;
289 std::uint8_t b = (rgb) & 0x0000ff;
290 color_[i] = Eigen::Vector3i (r, g, b);
304 normal_.resize (filtered_normal_->size ());
305 for (std::size_t i = 0; i < filtered_normal_->size (); i++)
307 float n_x = (*filtered_normal_)[i].normal_x;
308 float n_y = (*filtered_normal_)[i].normal_y;
309 float n_z = (*filtered_normal_)[i].normal_z;
310 normal_[i] = Eigen::Vector3f (n_x, n_y, n_z);
317 template <
typename Po
intT>
void
319 std::vector<int> &labels,
320 unsigned int n_labels)
323 srand (
static_cast<unsigned int> (time (
nullptr)) );
327 const float GT_PROB = 0.9f;
328 const float u_energy = -std::log ( 1.0f /
static_cast<float> (n_labels) );
329 const float n_energy = -std::log ( (1.0f - GT_PROB) /
static_cast<float>(n_labels - 1) );
330 const float p_energy = -std::log ( GT_PROB );
332 for (std::size_t k = 0; k < filtered_anno_->size (); k++)
334 int label = (*filtered_anno_)[k].label;
336 if (labels.empty () && label > 0)
337 labels.push_back (label);
340 for (
int c_idx = 0; c_idx < static_cast<int> (labels.size ()) ; c_idx++)
342 if (labels[c_idx] == label)
345 if (c_idx ==
static_cast<int>(labels.size () -1) && label > 0)
347 if (labels.size () < n_labels)
348 labels.push_back (label);
355 std::size_t u_idx = k * n_labels;
358 for (std::size_t i = 0; i < n_labels; i++)
359 unary[u_idx + i] = n_energy;
360 unary[u_idx + labels.size ()] = p_energy;
364 const float PROB = 0.2f;
365 const float n_energy2 = -std::log ( (1.0f - PROB) /
static_cast<float>(n_labels - 1) );
366 const float p_energy2 = -std::log ( PROB );
368 for (std::size_t i = 0; i < n_labels; i++)
369 unary[u_idx + i] = n_energy2;
370 unary[u_idx + labels.size ()] = p_energy2;
376 for (std::size_t i = 0; i < n_labels; i++)
377 unary[u_idx + i] = u_energy;
383 template <
typename Po
intT>
void
388 std::cout <<
"create Voxel Grid - DONE" << std::endl;
391 createDataVectorFromVoxelGrid ();
392 std::cout <<
"create Data Vector from Voxel Grid - DONE" << std::endl;
395 const int n_labels = 10;
397 int N =
static_cast<int> (data_.size ());
400 std::vector<int> labels;
401 std::vector<float> unary;
402 if (!anno_cloud_->points.empty ())
404 unary.resize (N * n_labels);
405 createUnaryPotentials (unary, labels, n_labels);
408 std::cout <<
"labels size: " << labels.size () << std::endl;
409 for (
const int &label : labels)
411 std::cout << label << std::endl;
415 std::cout <<
"create unary potentials - DONE" << std::endl;
495 std::cout <<
"create dense CRF - DONE" << std::endl;
500 smoothness_kernel_param_[1],
501 smoothness_kernel_param_[2],
502 smoothness_kernel_param_[3]);
503 std::cout <<
"add smoothness kernel - DONE" << std::endl;
507 appearance_kernel_param_[1],
508 appearance_kernel_param_[2],
509 appearance_kernel_param_[3],
510 appearance_kernel_param_[4],
511 appearance_kernel_param_[5],
512 appearance_kernel_param_[6]);
513 std::cout <<
"add appearance kernel - DONE" << std::endl;
516 surface_kernel_param_[0],
517 surface_kernel_param_[1],
518 surface_kernel_param_[2],
519 surface_kernel_param_[3],
520 surface_kernel_param_[4],
521 surface_kernel_param_[5],
522 surface_kernel_param_[6]);
523 std::cout <<
"add surface kernel - DONE" << std::endl;
526 std::vector<int> r (N);
537 std::cout <<
"Map inference - DONE" << std::endl;
540 output = *filtered_anno_;
542 for (
int i = 0; i < N; i++)
544 output[i].label = labels[r[i]];
600 #define PCL_INSTANTIATE_CrfSegmentation(T) template class pcl::CrfSegmentation<T>;
602 #endif // PCL_CRF_SEGMENTATION_HPP_