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