Point Cloud Library (PCL) 1.13.0
Loading...
Searching...
No Matches
crf_segmentation.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 *
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of the copyright holder(s) nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 * Author : Christian Potthast
36 * Email : potthast@usc.edu
37 *
38 */
39
40#ifndef PCL_CRF_SEGMENTATION_HPP_
41#define PCL_CRF_SEGMENTATION_HPP_
42
43#include <pcl/filters/voxel_grid_label.h> // for VoxelGridLabel
44#include <pcl/segmentation/crf_segmentation.h>
45
46#include <pcl/common/io.h>
47
48#include <cstdlib>
49#include <ctime>
50
51//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
52template <typename PointT>
54 voxel_grid_ (),
55 input_cloud_ (new pcl::PointCloud<PointT>),
56 normal_cloud_ (new pcl::PointCloud<pcl::PointNormal>),
57 filtered_cloud_ (new pcl::PointCloud<PointT>),
58 filtered_anno_ (new pcl::PointCloud<pcl::PointXYZRGBL>),
59 filtered_normal_ (new pcl::PointCloud<pcl::PointNormal>),
60 voxel_grid_leaf_size_ (Eigen::Vector4f (0.001f, 0.001f, 0.001f, 0.0f))
61{
62}
63
64//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
65template <typename PointT>
67
68//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
69template <typename PointT> void
71{
72 input_cloud_ = input_cloud;
73}
74
75//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
76template <typename PointT> void
78{
79 anno_cloud_ = anno_cloud;
80}
81
82//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
83template <typename PointT> void
85{
86 normal_cloud_ = normal_cloud;
87}
88
89//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
90template <typename PointT> void
91pcl::CrfSegmentation<PointT>::setVoxelGridLeafSize (const float x, const float y, const float z)
92{
93 voxel_grid_leaf_size_.x () = x;
94 voxel_grid_leaf_size_.y () = y;
95 voxel_grid_leaf_size_.z () = z;
96}
97
98//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
99template <typename PointT> void
100pcl::CrfSegmentation<PointT>::setSmoothnessKernelParameters (const float sx, const float sy, const float sz,
101 const float w)
102{
103 smoothness_kernel_param_[0] = sx;
104 smoothness_kernel_param_[1] = sy;
105 smoothness_kernel_param_[2] = sz;
106 smoothness_kernel_param_[3] = w;
107}
108
109//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
110template <typename PointT> void
112 float sr, float sg, float sb,
113 float w)
114{
115 appearance_kernel_param_[0] = sx;
116 appearance_kernel_param_[1] = sy;
117 appearance_kernel_param_[2] = sz;
118 appearance_kernel_param_[3] = sr;
119 appearance_kernel_param_[4] = sg;
120 appearance_kernel_param_[5] = sb;
121 appearance_kernel_param_[6] = w;
122}
123
124//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
125template <typename PointT> void
127 float snx, float sny, float snz,
128 float w)
129{
130 surface_kernel_param_[0] = sx;
131 surface_kernel_param_[1] = sy;
132 surface_kernel_param_[2] = sz;
133 surface_kernel_param_[3] = snx;
134 surface_kernel_param_[4] = sny;
135 surface_kernel_param_[5] = snz;
136 surface_kernel_param_[6] = w;
137}
138
139
140//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
141template <typename PointT> void
143{
144 // Filter the input cloud
145 // Set the voxel grid input cloud
146 voxel_grid_.setInputCloud (input_cloud_);
147 // Set the voxel grid leaf size
148 voxel_grid_.setLeafSize (voxel_grid_leaf_size_.x (), voxel_grid_leaf_size_.y (), voxel_grid_leaf_size_.z () );
149 // Only downsample XYZ (if this is set to false, color values set to 0)
150 voxel_grid_.setDownsampleAllData (true);
151 // Save leaf information
152 //voxel_grid_.setSaveLeafLayout (true);
153 // apply the filter
154 voxel_grid_.filter (*filtered_cloud_);
155
156 // Filter the annotated cloud
157 if (!anno_cloud_->points.empty ())
158 {
160
161 vg.setInputCloud (anno_cloud_);
162 // Set the voxel grid leaf size
163 vg.setLeafSize (voxel_grid_leaf_size_.x (), voxel_grid_leaf_size_.y (), voxel_grid_leaf_size_.z () );
164 // Only downsample XYZ
165 vg.setDownsampleAllData (true);
166 // Save leaf information
167 //vg.setSaveLeafLayout (false);
168 // apply the filter
169 vg.filter (*filtered_anno_);
170 }
171
172 // Filter the annotated cloud
173 if (!normal_cloud_->points.empty ())
174 {
176 vg.setInputCloud (normal_cloud_);
177 // Set the voxel grid leaf size
178 vg.setLeafSize (voxel_grid_leaf_size_.x (), voxel_grid_leaf_size_.y (), voxel_grid_leaf_size_.z () );
179 // Only downsample XYZ
180 vg.setDownsampleAllData (true);
181 // Save leaf information
182 //vg.setSaveLeafLayout (false);
183 // apply the filter
184 vg.filter (*filtered_normal_);
185 }
186
187}
188
189//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
190template <typename PointT> void
192{
193 // get the dimension of the voxel grid
194 //Eigen::Vector3i min_b, max_b;
195 //min_b = voxel_grid_.getMinBoxCoordinates ();
196 //max_b = voxel_grid_.getMaxBoxCoordinates ();
197
198 //std::cout << "min_b: " << min_b.x () << " " << min_b.y () << " " << min_b.z () << std::endl;
199 //std::cout << "max_b: " << max_b.x () << " " << max_b.y () << " " << max_b.z () << std::endl;
200
201 // compute the voxel grid dimensions
202 //dim_.x () = std::abs (max_b.x () - min_b.x ());
203 //dim_.y () = std::abs (max_b.y () - min_b.y ());
204 //dim_.z () = std::abs (max_b.z () - min_b.z ());
205
206 //std::cout << dim_.x () * dim_.y () * dim_.z () << std::endl;
207
208 // reserve the space for the data vector
209 //data_.reserve (dim_.x () * dim_.y () * dim_.z ());
210
211/*
212 std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > data;
213 std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > color;
214 // fill the data vector
215 for (int kk = min_b.z (), k = 0; kk <= max_b.z (); kk++, k++)
216 {
217 for (int jj = min_b.y (), j = 0; jj <= max_b.y (); jj++, j++)
218 {
219 for (int ii = min_b.x (), i = 0; ii <= max_b.x (); ii++, i++)
220 {
221 Eigen::Vector3i ijk (ii, jj, kk);
222 int index = voxel_grid_.getCentroidIndexAt (ijk);
223 if (index != -1)
224 {
225 data_.push_back (Eigen::Vector3i (i, j, k));
226 color_.push_back ((*input_cloud_)[index].getRGBVector3i ());
227 }
228 }
229 }
230 }
231*/
232
233
234/*
235 // get the size of the input fields
236 std::vector< pcl::PCLPointField > fields;
237 pcl::getFields (*input_cloud_, fields);
238
239 for (int i = 0; i < fields.size (); i++)
240 std::cout << fields[i] << std::endl;
241*/
242
243
244 // reserve space for the data vector
245 data_.resize (filtered_cloud_->size ());
246
247 std::vector< pcl::PCLPointField > fields;
248 // check if we have color data
249 bool color_data = false;
250 int rgba_index = -1;
251 rgba_index = pcl::getFieldIndex<PointT> ("rgb", fields);
252 if (rgba_index == -1)
253 rgba_index = pcl::getFieldIndex<PointT> ("rgba", fields);
254 if (rgba_index >= 0)
255 {
256 color_data = true;
257 color_.resize (filtered_cloud_->size ());
258 }
259
260
261/*
262 // check if we have normal data
263 bool normal_data = false;
264 int normal_index = -1;
265 rgba_index = pcl::getFieldIndex<PointT> ("normal_x", fields);
266 if (rgba_index >= 0)
267 {
268 normal_data = true;
269 normal_.resize (filtered_cloud_->size ());
270 }
271*/
272
273 // fill the data vector
274 for (std::size_t i = 0; i < filtered_cloud_->size (); i++)
275 {
276 Eigen::Vector3f p ((*filtered_anno_)[i].x,
277 (*filtered_anno_)[i].y,
278 (*filtered_anno_)[i].z);
279 Eigen::Vector3i c = voxel_grid_.getGridCoordinates (p.x (), p.y (), p.y ());
280 data_[i] = c;
281
282 if (color_data)
283 {
284 std::uint32_t rgb = *reinterpret_cast<int*>(&(*filtered_cloud_)[i].rgba);
285 std::uint8_t r = (rgb >> 16) & 0x0000ff;
286 std::uint8_t g = (rgb >> 8) & 0x0000ff;
287 std::uint8_t b = (rgb) & 0x0000ff;
288 color_[i] = Eigen::Vector3i (r, g, b);
289 }
290
291/*
292 if (normal_data)
293 {
294 float n_x = (*filtered_cloud_)[i].normal_x;
295 float n_y = (*filtered_cloud_)[i].normal_y;
296 float n_z = (*filtered_cloud_)[i].normal_z;
297 normal_[i] = Eigen::Vector3f (n_x, n_y, n_z);
298 }
299*/
300 }
301
302 normal_.resize (filtered_normal_->size ());
303 for (std::size_t i = 0; i < filtered_normal_->size (); i++)
304 {
305 float n_x = (*filtered_normal_)[i].normal_x;
306 float n_y = (*filtered_normal_)[i].normal_y;
307 float n_z = (*filtered_normal_)[i].normal_z;
308 normal_[i] = Eigen::Vector3f (n_x, n_y, n_z);
309 }
310
311
312}
313
314//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
315template <typename PointT> void
317 std::vector<int> &labels,
318 unsigned int n_labels)
319{
320 /* initialize random seed: */
321 srand ( static_cast<unsigned int> (time (nullptr)) );
322 //srand ( time (NULL) );
323
324 // Certainty that the groundtruth is correct
325 const float GT_PROB = 0.9f;
326 const float u_energy = -std::log ( 1.0f / static_cast<float> (n_labels) );
327 const float n_energy = -std::log ( (1.0f - GT_PROB) / static_cast<float>(n_labels - 1) );
328 const float p_energy = -std::log ( GT_PROB );
329
330 for (std::size_t k = 0; k < filtered_anno_->size (); k++)
331 {
332 int label = (*filtered_anno_)[k].label;
333
334 if (labels.empty () && label > 0)
335 labels.push_back (label);
336
337 // add color to the color vector if not added yet
338 for (int c_idx = 0; c_idx < static_cast<int> (labels.size ()) ; c_idx++)
339 {
340 if (labels[c_idx] == label)
341 break;
342
343 if (c_idx == static_cast<int>(labels.size () -1) && label > 0)
344 {
345 if (labels.size () < n_labels)
346 labels.push_back (label);
347 else
348 label = 0;
349 }
350 }
351
352 // set the engeries for the labels
353 std::size_t u_idx = k * n_labels;
354 if (label > 0)
355 {
356 for (std::size_t i = 0; i < n_labels; i++)
357 unary[u_idx + i] = n_energy;
358 unary[u_idx + labels.size ()] = p_energy;
359
360 if (label == 1)
361 {
362 const float PROB = 0.2f;
363 const float n_energy2 = -std::log ( (1.0f - PROB) / static_cast<float>(n_labels - 1) );
364 const float p_energy2 = -std::log ( PROB );
365
366 for (std::size_t i = 0; i < n_labels; i++)
367 unary[u_idx + i] = n_energy2;
368 unary[u_idx + labels.size ()] = p_energy2;
369 }
370
371 }
372 else
373 {
374 for (std::size_t i = 0; i < n_labels; i++)
375 unary[u_idx + i] = u_energy;
376 }
377 }
378}
379
380//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
381template <typename PointT> void
383{
384 // create the voxel grid
385 createVoxelGrid ();
386 std::cout << "create Voxel Grid - DONE" << std::endl;
387
388 // create the data Vector
389 createDataVectorFromVoxelGrid ();
390 std::cout << "create Data Vector from Voxel Grid - DONE" << std::endl;
391
392 // number of labels
393 const int n_labels = 10;
394 // number of data points
395 int N = static_cast<int> (data_.size ());
396
397 // create unary potentials
398 std::vector<int> labels;
399 std::vector<float> unary;
400 if (!anno_cloud_->points.empty ())
401 {
402 unary.resize (N * n_labels);
403 createUnaryPotentials (unary, labels, n_labels);
404
405
406 std::cout << "labels size: " << labels.size () << std::endl;
407 for (const int &label : labels)
408 {
409 std::cout << label << std::endl;
410 }
411
412 }
413 std::cout << "create unary potentials - DONE" << std::endl;
414
415
416/*
417 pcl::PointCloud<pcl::PointXYZRGBL> tmp_cloud_OLD;
418 tmp_cloud_OLD = *filtered_anno_;
419
420 // Setup the CRF model
421 DenseCRF2D crfOLD(N, 1, n_labels);
422
423 float * unaryORI = new float[N*n_labels];
424 for (int i = 0; i < N*n_labels; i++)
425 unaryORI[i] = unary[i];
426 crfOLD.setUnaryEnergy( unaryORI );
427
428 float * pos = new float[N*3];
429 for (int i = 0; i < N; i++)
430 {
431 pos[i * 3] = data_[i].x ();
432 pos[i * 3 +1] = data_[i].y ();
433 pos[i * 3 +2] = data_[i].z ();
434 }
435 crfOLD.addPairwiseGaussian( pos, 3, 3, 3, 2.0 );
436
437 float * col = new float[N*3];
438 for (int i = 0; i < N; i++)
439 {
440 col[i * 3] = color_[i].x ();
441 col[i * 3 +1] = color_[i].y ();
442 col[i * 3 +2] = color_[i].z ();
443 }
444 crfOLD.addPairwiseBilateral(pos, col, 20, 20, 20, 10, 10, 10, 1.3 );
445
446 short * map = new short[N];
447 crfOLD.map(10, map);
448
449 for (std::size_t i = 0; i < N; i++)
450 {
451 tmp_cloud_OLD[i].label = map[i];
452 }
453
454
455*/
456
457 //float * resultOLD = new float[N*n_labels];
458 //crfOLD.inference (10, resultOLD);
459
460 //std::vector<float> baryOLD;
461 //crfOLD.getBarycentric (0, baryOLD);
462 //std::vector<float> featuresOLD;
463 //crfOLD.getFeature (1, featuresOLD);
464
465/*
466 for(int i = 0; i < 25; i++)
467 {
468 std::cout << baryOLD[i] << std::endl;
469 }
470*/
471
472
473 // create the output cloud
474 //output = *filtered_anno_;
475
476
477
478 // ----------------------------------//
479 // -------- -------------------//
480
481 // create dense CRF
482 DenseCrf crf (N, n_labels);
483
484 // set the unary potentials
485 crf.setUnaryEnergy (unary);
486
487 // set the data vector
488 crf.setDataVector (data_);
489
490 // set the color vector
491 crf.setColorVector (color_);
492
493 std::cout << "create dense CRF - DONE" << std::endl;
494
495
496 // add the smoothness kernel
497 crf.addPairwiseGaussian (smoothness_kernel_param_[0],
498 smoothness_kernel_param_[1],
499 smoothness_kernel_param_[2],
500 smoothness_kernel_param_[3]);
501 std::cout << "add smoothness kernel - DONE" << std::endl;
502
503 // add the appearance kernel
504 crf.addPairwiseBilateral (appearance_kernel_param_[0],
505 appearance_kernel_param_[1],
506 appearance_kernel_param_[2],
507 appearance_kernel_param_[3],
508 appearance_kernel_param_[4],
509 appearance_kernel_param_[5],
510 appearance_kernel_param_[6]);
511 std::cout << "add appearance kernel - DONE" << std::endl;
512
513 crf.addPairwiseNormals (data_, normal_,
514 surface_kernel_param_[0],
515 surface_kernel_param_[1],
516 surface_kernel_param_[2],
517 surface_kernel_param_[3],
518 surface_kernel_param_[4],
519 surface_kernel_param_[5],
520 surface_kernel_param_[6]);
521 std::cout << "add surface kernel - DONE" << std::endl;
522
523 // map inference
524 std::vector<int> r (N);
525 crf.mapInference (n_iterations_, r);
526
527 //std::vector<float> result (N*n_labels);
528 //crf.inference (n_iterations_, result);
529
530 //std::vector<float> bary;
531 //crf.getBarycentric (0, bary);
532 //std::vector<float> features;
533 //crf.getFeatures (1, features);
534
535 std::cout << "Map inference - DONE" << std::endl;
536
537 // create the output cloud
538 output = *filtered_anno_;
539
540 for (int i = 0; i < N; i++)
541 {
542 output[i].label = labels[r[i]];
543 }
544
545
546/*
547 pcl::PointCloud<pcl::PointXYZRGBL> tmp_cloud;
548 tmp_cloud = *filtered_anno_;
549
550 bool c = true;
551 for (std::size_t i = 0; i < tmp_cloud.size (); i++)
552 {
553 if (tmp_cloud[i].label != tmp_cloud_OLD[i].label)
554 {
555
556 std::cout << "idx: " << i << " = " <<tmp_cloud[i].label << " | " << tmp_cloud_OLD[i].label << std::endl;
557 c = false;
558 break;
559 }
560 }
561
562 if (c)
563 std::cout << "DEBUG - OUTPUT - OK" << std::endl;
564 else
565 std::cout << "DEBUG - OUTPUT - ERROR" << std::endl;
566*/
567
568
569
570/*
571 for (std::size_t i = 0; i < 25; i++)
572 {
573 std::cout << result[i] << " | " << resultOLD[i] << std::endl;
574 }
575
576
577 c = true;
578 for (std::size_t i = 0; i < result.size (); i++)
579 {
580 if (result[i] != resultOLD[i])
581 {
582 std::cout << result[i] << " | " << resultOLD[i] << std::endl;
583
584 c = false;
585 break;
586 }
587 }
588
589 if (c)
590 std::cout << "DEBUG - OUTPUT - OK" << std::endl;
591 else
592 std::cout << "DEBUG - OUTPUT - ERROR" << std::endl;
593*/
594
595
596}
597
598#define PCL_INSTANTIATE_CrfSegmentation(T) template class pcl::CrfSegmentation<T>;
599
600#endif // PCL_CRF_SEGMENTATION_HPP_
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 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 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 mapInference(int n_iterations, std::vector< int > &result, float relax=1.0f)
void setColorVector(const std::vector< Eigen::Vector3i, Eigen::aligned_allocator< Eigen::Vector3i > > color)
The associated color of the data.
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
Definition filter.h:121
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition pcl_base.hpp:65
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.
Definition voxel_grid.h:177
void setDownsampleAllData(bool downsample)
Set to true if all fields need to be downsampled, or false if just XYZ.
Definition voxel_grid.h:255
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
Definition voxel_grid.h:221
Definition bfgs.h:10
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.