Point Cloud Library (PCL) 1.12.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}
69
70//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
71template <typename PointT> void
73{
74 input_cloud_ = input_cloud;
75}
76
77//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
78template <typename PointT> void
80{
81 anno_cloud_ = anno_cloud;
82}
83
84//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
85template <typename PointT> void
87{
88 normal_cloud_ = normal_cloud;
89}
90
91//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
92template <typename PointT> void
93pcl::CrfSegmentation<PointT>::setVoxelGridLeafSize (const float x, const float y, const float z)
94{
95 voxel_grid_leaf_size_.x () = x;
96 voxel_grid_leaf_size_.y () = y;
97 voxel_grid_leaf_size_.z () = z;
98}
99
100//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
101template <typename PointT> void
102pcl::CrfSegmentation<PointT>::setSmoothnessKernelParameters (const float sx, const float sy, const float sz,
103 const float w)
104{
105 smoothness_kernel_param_[0] = sx;
106 smoothness_kernel_param_[1] = sy;
107 smoothness_kernel_param_[2] = sz;
108 smoothness_kernel_param_[3] = w;
109}
110
111//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
112template <typename PointT> void
114 float sr, float sg, float sb,
115 float w)
116{
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;
124}
125
126//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
127template <typename PointT> void
129 float snx, float sny, float snz,
130 float w)
131{
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;
139}
140
141
142//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
143template <typename PointT> void
145{
146 // Filter the input cloud
147 // Set the voxel grid input cloud
148 voxel_grid_.setInputCloud (input_cloud_);
149 // Set the voxel grid leaf size
150 voxel_grid_.setLeafSize (voxel_grid_leaf_size_.x (), voxel_grid_leaf_size_.y (), voxel_grid_leaf_size_.z () );
151 // Only downsample XYZ (if this is set to false, color values set to 0)
152 voxel_grid_.setDownsampleAllData (true);
153 // Save leaf information
154 //voxel_grid_.setSaveLeafLayout (true);
155 // apply the filter
156 voxel_grid_.filter (*filtered_cloud_);
157
158 // Filter the annotated cloud
159 if (!anno_cloud_->points.empty ())
160 {
162
163 vg.setInputCloud (anno_cloud_);
164 // Set the voxel grid leaf size
165 vg.setLeafSize (voxel_grid_leaf_size_.x (), voxel_grid_leaf_size_.y (), voxel_grid_leaf_size_.z () );
166 // Only downsample XYZ
167 vg.setDownsampleAllData (true);
168 // Save leaf information
169 //vg.setSaveLeafLayout (false);
170 // apply the filter
171 vg.filter (*filtered_anno_);
172 }
173
174 // Filter the annotated cloud
175 if (!normal_cloud_->points.empty ())
176 {
178 vg.setInputCloud (normal_cloud_);
179 // Set the voxel grid leaf size
180 vg.setLeafSize (voxel_grid_leaf_size_.x (), voxel_grid_leaf_size_.y (), voxel_grid_leaf_size_.z () );
181 // Only downsample XYZ
182 vg.setDownsampleAllData (true);
183 // Save leaf information
184 //vg.setSaveLeafLayout (false);
185 // apply the filter
186 vg.filter (*filtered_normal_);
187 }
188
189}
190
191//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
192template <typename PointT> void
194{
195 // get the dimension of the voxel grid
196 //Eigen::Vector3i min_b, max_b;
197 //min_b = voxel_grid_.getMinBoxCoordinates ();
198 //max_b = voxel_grid_.getMaxBoxCoordinates ();
199
200 //std::cout << "min_b: " << min_b.x () << " " << min_b.y () << " " << min_b.z () << std::endl;
201 //std::cout << "max_b: " << max_b.x () << " " << max_b.y () << " " << max_b.z () << std::endl;
202
203 // compute the voxel grid dimensions
204 //dim_.x () = std::abs (max_b.x () - min_b.x ());
205 //dim_.y () = std::abs (max_b.y () - min_b.y ());
206 //dim_.z () = std::abs (max_b.z () - min_b.z ());
207
208 //std::cout << dim_.x () * dim_.y () * dim_.z () << std::endl;
209
210 // reserve the space for the data vector
211 //data_.reserve (dim_.x () * dim_.y () * dim_.z ());
212
213/*
214 std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > data;
215 std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > color;
216 // fill the data vector
217 for (int kk = min_b.z (), k = 0; kk <= max_b.z (); kk++, k++)
218 {
219 for (int jj = min_b.y (), j = 0; jj <= max_b.y (); jj++, j++)
220 {
221 for (int ii = min_b.x (), i = 0; ii <= max_b.x (); ii++, i++)
222 {
223 Eigen::Vector3i ijk (ii, jj, kk);
224 int index = voxel_grid_.getCentroidIndexAt (ijk);
225 if (index != -1)
226 {
227 data_.push_back (Eigen::Vector3i (i, j, k));
228 color_.push_back ((*input_cloud_)[index].getRGBVector3i ());
229 }
230 }
231 }
232 }
233*/
234
235
236/*
237 // get the size of the input fields
238 std::vector< pcl::PCLPointField > fields;
239 pcl::getFields (*input_cloud_, fields);
240
241 for (int i = 0; i < fields.size (); i++)
242 std::cout << fields[i] << std::endl;
243*/
244
245
246 // reserve space for the data vector
247 data_.resize (filtered_cloud_->size ());
248
249 std::vector< pcl::PCLPointField > fields;
250 // check if we have color data
251 bool color_data = false;
252 int rgba_index = -1;
253 rgba_index = pcl::getFieldIndex<PointT> ("rgb", fields);
254 if (rgba_index == -1)
255 rgba_index = pcl::getFieldIndex<PointT> ("rgba", fields);
256 if (rgba_index >= 0)
257 {
258 color_data = true;
259 color_.resize (filtered_cloud_->size ());
260 }
261
262
263/*
264 // check if we have normal data
265 bool normal_data = false;
266 int normal_index = -1;
267 rgba_index = pcl::getFieldIndex<PointT> ("normal_x", fields);
268 if (rgba_index >= 0)
269 {
270 normal_data = true;
271 normal_.resize (filtered_cloud_->size ());
272 }
273*/
274
275 // fill the data vector
276 for (std::size_t i = 0; i < filtered_cloud_->size (); i++)
277 {
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 ());
282 data_[i] = c;
283
284 if (color_data)
285 {
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);
291 }
292
293/*
294 if (normal_data)
295 {
296 float n_x = (*filtered_cloud_)[i].normal_x;
297 float n_y = (*filtered_cloud_)[i].normal_y;
298 float n_z = (*filtered_cloud_)[i].normal_z;
299 normal_[i] = Eigen::Vector3f (n_x, n_y, n_z);
300 }
301*/
302 }
303
304 normal_.resize (filtered_normal_->size ());
305 for (std::size_t i = 0; i < filtered_normal_->size (); i++)
306 {
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);
311 }
312
313
314}
315
316//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
317template <typename PointT> void
319 std::vector<int> &labels,
320 unsigned int n_labels)
321{
322 /* initialize random seed: */
323 srand ( static_cast<unsigned int> (time (nullptr)) );
324 //srand ( time (NULL) );
325
326 // Certainty that the groundtruth is correct
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 );
331
332 for (std::size_t k = 0; k < filtered_anno_->size (); k++)
333 {
334 int label = (*filtered_anno_)[k].label;
335
336 if (labels.empty () && label > 0)
337 labels.push_back (label);
338
339 // add color to the color vector if not added yet
340 for (int c_idx = 0; c_idx < static_cast<int> (labels.size ()) ; c_idx++)
341 {
342 if (labels[c_idx] == label)
343 break;
344
345 if (c_idx == static_cast<int>(labels.size () -1) && label > 0)
346 {
347 if (labels.size () < n_labels)
348 labels.push_back (label);
349 else
350 label = 0;
351 }
352 }
353
354 // set the engeries for the labels
355 std::size_t u_idx = k * n_labels;
356 if (label > 0)
357 {
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;
361
362 if (label == 1)
363 {
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 );
367
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;
371 }
372
373 }
374 else
375 {
376 for (std::size_t i = 0; i < n_labels; i++)
377 unary[u_idx + i] = u_energy;
378 }
379 }
380}
381
382//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
383template <typename PointT> void
385{
386 // create the voxel grid
387 createVoxelGrid ();
388 std::cout << "create Voxel Grid - DONE" << std::endl;
389
390 // create the data Vector
391 createDataVectorFromVoxelGrid ();
392 std::cout << "create Data Vector from Voxel Grid - DONE" << std::endl;
393
394 // number of labels
395 const int n_labels = 10;
396 // number of data points
397 int N = static_cast<int> (data_.size ());
398
399 // create unary potentials
400 std::vector<int> labels;
401 std::vector<float> unary;
402 if (!anno_cloud_->points.empty ())
403 {
404 unary.resize (N * n_labels);
405 createUnaryPotentials (unary, labels, n_labels);
406
407
408 std::cout << "labels size: " << labels.size () << std::endl;
409 for (const int &label : labels)
410 {
411 std::cout << label << std::endl;
412 }
413
414 }
415 std::cout << "create unary potentials - DONE" << std::endl;
416
417
418/*
419 pcl::PointCloud<pcl::PointXYZRGBL> tmp_cloud_OLD;
420 tmp_cloud_OLD = *filtered_anno_;
421
422 // Setup the CRF model
423 DenseCRF2D crfOLD(N, 1, n_labels);
424
425 float * unaryORI = new float[N*n_labels];
426 for (int i = 0; i < N*n_labels; i++)
427 unaryORI[i] = unary[i];
428 crfOLD.setUnaryEnergy( unaryORI );
429
430 float * pos = new float[N*3];
431 for (int i = 0; i < N; i++)
432 {
433 pos[i * 3] = data_[i].x ();
434 pos[i * 3 +1] = data_[i].y ();
435 pos[i * 3 +2] = data_[i].z ();
436 }
437 crfOLD.addPairwiseGaussian( pos, 3, 3, 3, 2.0 );
438
439 float * col = new float[N*3];
440 for (int i = 0; i < N; i++)
441 {
442 col[i * 3] = color_[i].x ();
443 col[i * 3 +1] = color_[i].y ();
444 col[i * 3 +2] = color_[i].z ();
445 }
446 crfOLD.addPairwiseBilateral(pos, col, 20, 20, 20, 10, 10, 10, 1.3 );
447
448 short * map = new short[N];
449 crfOLD.map(10, map);
450
451 for (std::size_t i = 0; i < N; i++)
452 {
453 tmp_cloud_OLD[i].label = map[i];
454 }
455
456
457*/
458
459 //float * resultOLD = new float[N*n_labels];
460 //crfOLD.inference (10, resultOLD);
461
462 //std::vector<float> baryOLD;
463 //crfOLD.getBarycentric (0, baryOLD);
464 //std::vector<float> featuresOLD;
465 //crfOLD.getFeature (1, featuresOLD);
466
467/*
468 for(int i = 0; i < 25; i++)
469 {
470 std::cout << baryOLD[i] << std::endl;
471 }
472*/
473
474
475 // create the output cloud
476 //output = *filtered_anno_;
477
478
479
480 // ----------------------------------//
481 // -------- -------------------//
482
483 // create dense CRF
484 DenseCrf crf (N, n_labels);
485
486 // set the unary potentials
487 crf.setUnaryEnergy (unary);
488
489 // set the data vector
490 crf.setDataVector (data_);
491
492 // set the color vector
493 crf.setColorVector (color_);
494
495 std::cout << "create dense CRF - DONE" << std::endl;
496
497
498 // add the smoothness kernel
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;
504
505 // add the appearance kernel
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;
514
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;
524
525 // map inference
526 std::vector<int> r (N);
527 crf.mapInference (n_iterations_, r);
528
529 //std::vector<float> result (N*n_labels);
530 //crf.inference (n_iterations_, result);
531
532 //std::vector<float> bary;
533 //crf.getBarycentric (0, bary);
534 //std::vector<float> features;
535 //crf.getFeatures (1, features);
536
537 std::cout << "Map inference - DONE" << std::endl;
538
539 // create the output cloud
540 output = *filtered_anno_;
541
542 for (int i = 0; i < N; i++)
543 {
544 output[i].label = labels[r[i]];
545 }
546
547
548/*
549 pcl::PointCloud<pcl::PointXYZRGBL> tmp_cloud;
550 tmp_cloud = *filtered_anno_;
551
552 bool c = true;
553 for (std::size_t i = 0; i < tmp_cloud.size (); i++)
554 {
555 if (tmp_cloud[i].label != tmp_cloud_OLD[i].label)
556 {
557
558 std::cout << "idx: " << i << " = " <<tmp_cloud[i].label << " | " << tmp_cloud_OLD[i].label << std::endl;
559 c = false;
560 break;
561 }
562 }
563
564 if (c)
565 std::cout << "DEBUG - OUTPUT - OK" << std::endl;
566 else
567 std::cout << "DEBUG - OUTPUT - ERROR" << std::endl;
568*/
569
570
571
572/*
573 for (std::size_t i = 0; i < 25; i++)
574 {
575 std::cout << result[i] << " | " << resultOLD[i] << std::endl;
576 }
577
578
579 c = true;
580 for (std::size_t i = 0; i < result.size (); i++)
581 {
582 if (result[i] != resultOLD[i])
583 {
584 std::cout << result[i] << " | " << resultOLD[i] << std::endl;
585
586 c = false;
587 break;
588 }
589 }
590
591 if (c)
592 std::cout << "DEBUG - OUTPUT - OK" << std::endl;
593 else
594 std::cout << "DEBUG - OUTPUT - ERROR" << std::endl;
595*/
596
597
598}
599
600#define PCL_INSTANTIATE_CrfSegmentation(T) template class pcl::CrfSegmentation<T>;
601
602#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.