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;
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;
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 ())
163 vg.setInputCloud (anno_cloud_);
165 vg.setLeafSize (voxel_grid_leaf_size_.x (), voxel_grid_leaf_size_.y (), voxel_grid_leaf_size_.z () );
167 vg.setDownsampleAllData (
true);
171 vg.filter (*filtered_anno_);
175 if (!normal_cloud_->points.empty ())
178 vg.setInputCloud (normal_cloud_);
180 vg.setLeafSize (voxel_grid_leaf_size_.x (), voxel_grid_leaf_size_.y (), voxel_grid_leaf_size_.z () );
182 vg.setDownsampleAllData (
true);
186 vg.filter (*filtered_normal_);
247 data_.resize (filtered_cloud_->size ());
249 std::vector< pcl::PCLPointField > 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);
319 std::vector<int> &labels,
323 srand (
static_cast<unsigned int> (time (
nullptr)) );
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);
342 if (labels[
c_idx] == label)
345 if (
c_idx ==
static_cast<int>(labels.size () -1) && label > 0)
348 labels.push_back (label);
358 for (std::size_t i = 0; i <
n_labels; i++)
364 const float PROB = 0.2f;
368 for (std::size_t i = 0; i <
n_labels; i++)
376 for (std::size_t i = 0; i <
n_labels; i++)
388 std::cout <<
"create Voxel Grid - DONE" << std::endl;
391 createDataVectorFromVoxelGrid ();
392 std::cout <<
"create Data Vector from Voxel Grid - DONE" << std::endl;
397 int N =
static_cast<int> (data_.size ());
400 std::vector<int> labels;
401 std::vector<float>
unary;
402 if (!anno_cloud_->points.empty ())
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;
490 crf.setDataVector (data_);
493 crf.setColorVector (color_);
495 std::cout <<
"create dense CRF - DONE" << std::endl;
499 crf.addPairwiseGaussian (smoothness_kernel_param_[0],
500 smoothness_kernel_param_[1],
501 smoothness_kernel_param_[2],
502 smoothness_kernel_param_[3]);
503 std::cout <<
"add smoothness kernel - DONE" << std::endl;
506 crf.addPairwiseBilateral (appearance_kernel_param_[0],
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;
515 crf.addPairwiseNormals (data_, normal_,
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);
527 crf.mapInference (n_iterations_, r);
537 std::cout <<
"Map inference - DONE" << std::endl;
542 for (
int i = 0; i < N; i++)
544 output[i].label = labels[r[i]];
void setAppearanceKernelParameters(float sx, float sy, float sz, float sr, float sg, float sb, float w)
Set the appearanche kernel parameters.