40 #ifndef PCL_CRF_SEGMENTATION_HPP_
41 #define PCL_CRF_SEGMENTATION_HPP_
43 #include <pcl/segmentation/crf_segmentation.h>
45 #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 if (input_cloud_ != NULL)
75 input_cloud_.reset ();
77 input_cloud_ = input_cloud;
81 template <
typename Po
intT>
void
84 if (anno_cloud_ != NULL)
87 anno_cloud_ = anno_cloud;
91 template <
typename Po
intT>
void
94 if (normal_cloud_ != NULL)
95 normal_cloud_.reset ();
97 normal_cloud_ = normal_cloud;
101 template <
typename Po
intT>
void
104 voxel_grid_leaf_size_.x () = x;
105 voxel_grid_leaf_size_.y () = y;
106 voxel_grid_leaf_size_.z () = z;
110 template <
typename Po
intT>
void
114 smoothness_kernel_param_[0] = sx;
115 smoothness_kernel_param_[1] = sy;
116 smoothness_kernel_param_[2] = sz;
117 smoothness_kernel_param_[3] = w;
121 template <
typename Po
intT>
void
123 float sr,
float sg,
float sb,
126 appearance_kernel_param_[0] = sx;
127 appearance_kernel_param_[1] = sy;
128 appearance_kernel_param_[2] = sz;
129 appearance_kernel_param_[3] = sr;
130 appearance_kernel_param_[4] = sg;
131 appearance_kernel_param_[5] = sb;
132 appearance_kernel_param_[6] = w;
136 template <
typename Po
intT>
void
138 float snx,
float sny,
float snz,
141 surface_kernel_param_[0] = sx;
142 surface_kernel_param_[1] = sy;
143 surface_kernel_param_[2] = sz;
144 surface_kernel_param_[3] = snx;
145 surface_kernel_param_[4] = sny;
146 surface_kernel_param_[5] = snz;
147 surface_kernel_param_[6] = w;
152 template <
typename Po
intT>
void
157 voxel_grid_.setInputCloud (input_cloud_);
159 voxel_grid_.setLeafSize (voxel_grid_leaf_size_.x (), voxel_grid_leaf_size_.y (), voxel_grid_leaf_size_.z () );
161 voxel_grid_.setDownsampleAllData (
true);
165 voxel_grid_.filter (*filtered_cloud_);
168 if (anno_cloud_->points.size () > 0)
174 vg.
setLeafSize (voxel_grid_leaf_size_.x (), voxel_grid_leaf_size_.y (), voxel_grid_leaf_size_.z () );
180 vg.
filter (*filtered_anno_);
184 if (normal_cloud_->points.size () > 0)
189 vg.
setLeafSize (voxel_grid_leaf_size_.x (), voxel_grid_leaf_size_.y (), voxel_grid_leaf_size_.z () );
195 vg.
filter (*filtered_normal_);
201 template <
typename Po
intT>
void
256 data_.resize (filtered_cloud_->points.size ());
258 std::vector< pcl::PCLPointField > fields;
260 bool color_data =
false;
263 if (rgba_index == -1)
268 color_.resize (filtered_cloud_->points.size ());
285 for (
size_t i = 0; i < filtered_cloud_->points.size (); i++)
287 Eigen::Vector3f p (filtered_anno_->points[i].x,
288 filtered_anno_->points[i].y,
289 filtered_anno_->points[i].z);
290 Eigen::Vector3i c = voxel_grid_.getGridCoordinates (p.x (), p.y (), p.y ());
295 uint32_t rgb = *
reinterpret_cast<int*
>(&filtered_cloud_->points[i].rgba);
296 uint8_t r = (rgb >> 16) & 0x0000ff;
297 uint8_t g = (rgb >> 8) & 0x0000ff;
298 uint8_t b = (rgb) & 0x0000ff;
299 color_[i] = Eigen::Vector3i (r, g, b);
313 normal_.resize (filtered_normal_->points.size ());
314 for (
size_t i = 0; i < filtered_normal_->points.size (); i++)
316 float n_x = filtered_normal_->points[i].normal_x;
317 float n_y = filtered_normal_->points[i].normal_y;
318 float n_z = filtered_normal_->points[i].normal_z;
319 normal_[i] = Eigen::Vector3f (n_x, n_y, n_z);
326 template <
typename Po
intT>
void
328 std::vector<int> &labels,
329 unsigned int n_labels)
332 srand (
static_cast<unsigned int> (time (NULL)) );
336 const float GT_PROB = 0.9f;
337 const float u_energy = -logf ( 1.0f /
static_cast<float> (n_labels) );
338 const float n_energy = -logf ( (1.0f - GT_PROB) /
static_cast<float>(n_labels - 1) );
339 const float p_energy = -logf ( GT_PROB );
341 for (
size_t k = 0; k < filtered_anno_->points.size (); k++)
343 int label = filtered_anno_->points[k].label;
345 if (labels.size () == 0 && label > 0)
346 labels.push_back (label);
350 for (c_idx = 0; c_idx < static_cast<int> (labels.size ()) ; c_idx++)
352 if (labels[c_idx] == label)
355 if (c_idx ==
static_cast<int>(labels.size () -1) && label > 0)
357 if (labels.size () < n_labels)
358 labels.push_back (label);
386 size_t u_idx = k * n_labels;
389 for (
size_t i = 0; i < n_labels; i++)
390 unary[u_idx + i] = n_energy;
391 unary[u_idx + c_idx] = p_energy;
395 const float PROB = 0.2f;
396 const float n_energy2 = -logf ( (1.0f - PROB) /
static_cast<float>(n_labels - 1) );
397 const float p_energy2 = -logf ( PROB );
399 for (
size_t i = 0; i < n_labels; i++)
400 unary[u_idx + i] = n_energy2;
401 unary[u_idx + c_idx] = p_energy2;
407 for (
size_t i = 0; i < n_labels; i++)
408 unary[u_idx + i] = u_energy;
414 template <
typename Po
intT>
void
419 std::cout <<
"create Voxel Grid - DONE" << std::endl;
422 createDataVectorFromVoxelGrid ();
423 std::cout <<
"create Data Vector from Voxel Grid - DONE" << std::endl;
426 const int n_labels = 10;
428 int N =
static_cast<int> (data_.size ());
431 std::vector<int> labels;
432 std::vector<float> unary;
433 if (anno_cloud_->points.size () > 0)
435 unary.resize (N * n_labels);
436 createUnaryPotentials (unary, labels, n_labels);
439 std::cout <<
"labels size: " << labels.size () << std::endl;
440 for (
size_t i = 0; i < labels.size (); i++)
442 std::cout << labels[i] << std::endl;
446 std::cout <<
"create unary potentials - DONE" << std::endl;
515 tmp_cloud = *filtered_anno_;
529 std::cout <<
"create dense CRF - DONE" << std::endl;
534 smoothness_kernel_param_[1],
535 smoothness_kernel_param_[2],
536 smoothness_kernel_param_[3]);
537 std::cout <<
"add smoothness kernel - DONE" << std::endl;
541 appearance_kernel_param_[1],
542 appearance_kernel_param_[2],
543 appearance_kernel_param_[3],
544 appearance_kernel_param_[4],
545 appearance_kernel_param_[5],
546 appearance_kernel_param_[6]);
547 std::cout <<
"add appearance kernel - DONE" << std::endl;
550 surface_kernel_param_[0],
551 surface_kernel_param_[1],
552 surface_kernel_param_[2],
553 surface_kernel_param_[3],
554 surface_kernel_param_[4],
555 surface_kernel_param_[5],
556 surface_kernel_param_[6]);
557 std::cout <<
"add surface kernel - DONE" << std::endl;
560 std::vector<int> r (N);
571 std::cout <<
"Map inference - DONE" << std::endl;
574 output = *filtered_anno_;
576 for (
int i = 0; i < N; i++)
578 output.
points[i].label = labels[r[i]];
631 #define PCL_INSTANTIATE_CrfSegmentation(T) template class pcl::CrfSegmentation<T>;
633 #endif // PCL_CRF_SEGMENTATION_HPP_