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>;
void createUnaryPotentials(std::vector< float > &unary, std::vector< int > &colors, unsigned int n_labels)
void createVoxelGrid()
Create a voxel grid to discretize the scene.
void setAppearanceKernelParameters(float sx, float sy, float sz, float sr, float sg, float sb, float w)
Set the appearanche kernel parameters.
void createDataVectorFromVoxelGrid()
Get the data from the voxel grid and convert it into a vector.
void segmentPoints(pcl::PointCloud< pcl::PointXYZRGBL > &output)
This method simply launches the segmentation algorithm.
void setVoxelGridLeafSize(const float x, const float y, const float z)
Set the leaf size for the voxel grid.
~CrfSegmentation()
This destructor destroys the cloud...
void setAnnotatedCloud(typename pcl::PointCloud< pcl::PointXYZRGBL >::Ptr anno_cloud)
void setSmoothnessKernelParameters(const float sx, const float sy, const float sz, const float w)
Set the smoothness kernel parameters.
void setNormalCloud(typename pcl::PointCloud< pcl::PointNormal >::Ptr normal_cloud)
CrfSegmentation()
Constructor that sets default values for member variables.
void setSurfaceKernelParameters(float sx, float sy, float sz, float snx, float sny, float snz, float w)
void setInputCloud(typename pcl::PointCloud< PointT >::Ptr input_cloud)
This method sets the input cloud.
void setColorVector(const std::vector< Eigen::Vector3i, Eigen::aligned_allocator< Eigen::Vector3i >> color)
The associated color of the data.
void setDataVector(const std::vector< Eigen::Vector3i, Eigen::aligned_allocator< Eigen::Vector3i >> data)
Set the input data vector.
void addPairwiseGaussian(float sx, float sy, float sz, float w)
Add a pairwise gaussian kernel.
void setUnaryEnergy(const std::vector< float > unary)
void addPairwiseBilateral(float sx, float sy, float sz, float sr, float sg, float sb, float w)
Add a bilateral gaussian kernel.
void mapInference(int n_iterations, std::vector< int > &result, float relax=1.0f)
void addPairwiseNormals(std::vector< Eigen::Vector3i, Eigen::aligned_allocator< Eigen::Vector3i >> &coord, std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f >> &normals, float sx, float sy, float sz, float snx, float sny, float snz, float w)
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< PointCloud< PointT > > Ptr
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
void setDownsampleAllData(bool downsample)
Set to true if all fields need to be downsampled, or false if just XYZ.
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
A point structure representing Euclidean xyz coordinates, together with normal coordinates and the su...
A point structure representing Euclidean xyz coordinates, and the RGB color.