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 81 template <
typename Po
intT>
void 91 template <
typename Po
intT>
void 101 template <
typename Po
intT>
void 110 template <
typename Po
intT>
void 121 template <
typename Po
intT>
void 123 float sr,
float sg,
float sb,
136 template <
typename Po
intT>
void 138 float snx,
float sny,
float snz,
152 template <
typename Po
intT>
void 201 template <
typename Po
intT>
void 258 std::vector< pcl::PCLPointField > fields;
260 bool color_data =
false;
263 if (rgba_index == -1)
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);
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 );
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;
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;
435 unary.resize (N * 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;
529 std::cout <<
"create dense CRF - DONE" << std::endl;
537 std::cout <<
"add smoothness kernel - DONE" << std::endl;
547 std::cout <<
"add appearance kernel - DONE" << std::endl;
557 std::cout <<
"add surface kernel - DONE" << std::endl;
560 std::vector<int> r (N);
571 std::cout <<
"Map inference - DONE" << std::endl;
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_ float smoothness_kernel_param_[4]
smoothness kernel parameters [0] = standard deviation x [1] = standard deviation y [2] = standard dev...
~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.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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)
void setInputCloud(typename pcl::PointCloud< PointT >::Ptr input_cloud)
This method sets the input cloud.
void setUnaryEnergy(const std::vector< float > unary)
pcl::PointCloud< PointT >::Ptr input_cloud_
input cloud that will be segmented.
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.
pcl::VoxelGrid< PointT > voxel_grid_
Voxel grid to discretize the scene.
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...
std::vector< Eigen::Vector3i, Eigen::aligned_allocator< Eigen::Vector3i > > color_
void segmentPoints(pcl::PointCloud< pcl::PointXYZRGBL > &output)
This method simply launches the segmentation algorithm.
float appearance_kernel_param_[7]
appearance kernel parameters [0] = standard deviation x [1] = standard deviation y [2] = standard dev...
Eigen::Vector4f voxel_grid_leaf_size_
indices of the filtered cloud.
pcl::PointCloud< PointT >::Ptr filtered_cloud_
voxel grid filtered cloud.
boost::shared_ptr< PointCloud< PointT > > Ptr
std::vector< Eigen::Vector3i, Eigen::aligned_allocator< Eigen::Vector3i > > data_
voxel grid data points packing order [x0y0z0, x1y0z0,x2y0z0,...,x0y1z0,x1y1z0,...,x0y0z1,x1y0z1,...]
unsigned int n_iterations_
void createDataVectorFromVoxelGrid()
Get the data from the voxel grid and convert it into a vector.
pcl::PointCloud< pcl::PointNormal >::Ptr normal_cloud_
void setColorVector(const std::vector< Eigen::Vector3i, Eigen::aligned_allocator< Eigen::Vector3i > > color)
The associated color of the data.
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > normal_
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void createUnaryPotentials(std::vector< float > &unary, std::vector< int > &colors, unsigned int n_labels)
A point structure representing Euclidean xyz coordinates, together with normal coordinates and the su...
pcl::PointCloud< pcl::PointNormal >::Ptr filtered_normal_
void setSurfaceKernelParameters(float sx, float sy, float sz, float snx, float sny, float snz, float w)
float surface_kernel_param_[7]
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
pcl::PointCloud< pcl::PointXYZRGBL >::Ptr anno_cloud_
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)
pcl::PointCloud< pcl::PointXYZRGBL >::Ptr filtered_anno_
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
void setNormalCloud(typename pcl::PointCloud< pcl::PointNormal >::Ptr normal_cloud)