DenseCRFSegmentationProcessor.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package ROBDEKON::ArmarXObjects::DenseCRFSegmentationProcessor
17  * @author Christoph Pohl ( christoph dot pohl at kit dot edu )
18  * @date 2019
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include "DenseCRFSegmentationProcessor.h"
24 
25 #include <boost/graph/adjacency_list.hpp>
26 #include <boost/graph/graph_concepts.hpp>
27 #include <boost/graph/subgraph.hpp>
28 //#include <boost/bimap.hpp>
29 //#include <boost/graph/copy.hpp>
30 
31 //#include <Eigen/Core>
32 //#include <Eigen/Geometry>
33 
34 #include <algorithm>
35 #include <random>
36 
37 #include <boost/smart_ptr/make_shared.hpp>
38 
39 #include <pcl/filters/approximate_voxel_grid.h>
40 #include <pcl/filters/filter.h>
41 #include <pcl/filters/voxel_grid.h>
42 
43 #include <RobotAPI/libraries/core/Pose.h>
44 
45 #include <VisionX/libraries/PointCloudGraph/PointXYZRGBLNormal.h>
46 
47 #include "DenseCRFFeatureTerms.h"
48 #include "DenseGraphCRF.hpp"
49 #include "include/colormap/colormap.h"
50 
51 
52 using namespace armarx;
53 
54 armarx::PropertyDefinitionsPtr
55 DenseCRFSegmentationProcessor::createPropertyDefinitions()
56 {
57  armarx::PropertyDefinitionsPtr def{
58  new DenseCRFSegmentationProcessorPropertyDefinitions(getConfigIdentifier())};
59  def->optional(config_.getWriteBuffer(), "", "");
60  return def;
61 }
62 
63 void
64 armarx::DenseCRFSegmentationProcessor::onInitPointCloudProcessor()
65 {
66  ARMARX_TRACE;
67  std::unique_lock lock(write_mutex_);
68  config_.commitWrite(); // To commit the property definitions to the buffer
69  usingPointCloudProvider(getProperty<std::string>("ProviderName").getValue());
70  current_graph_ptr_ = GraphPtr(new Graph);
71  persistent_graph_ptr_ = GraphWithTimestampPtr(new GraphWithTimestamp);
72  initial_time_ = TimeUtil::GetTime().toMilliSecondsDouble();
73 }
74 
75 void
76 armarx::DenseCRFSegmentationProcessor::onConnectPointCloudProcessor()
77 {
78  ARMARX_TRACE;
79  // enableResultPointClouds<PointT>();
80  // add a name to provide more than one result point cloud
81  enableResultPointClouds<PointT>("CRFSegmentationResult");
82  enableResultPointClouds<pcl::PointXYZI>("CRFSegmentationConfidence");
83  if (config_.getUpToDateReadBuffer().out.provide_graph_pclouds)
84  {
85  ARMARX_TRACE;
86  enableResultPointClouds<PointT>("Graph0");
87  enableResultPointClouds<PointT>("Graph1");
88  enableResultPointClouds<PointT>("Graph2");
89  enableResultPointClouds<PointT>("Graph3");
90  enableResultPointClouds<PointT>("Graph4");
91  }
92 
93  if (config_.getUpToDateReadBuffer().out.provide_confidence_pclouds)
94  {
95  if (config_.getUpToDateReadBuffer().out.colorize_confidence_pclouds)
96  {
97  ARMARX_TRACE;
98  enableResultPointClouds<PointT>("Confidence0");
99  enableResultPointClouds<PointT>("Confidence1");
100  enableResultPointClouds<PointT>("Confidence2");
101  enableResultPointClouds<PointT>("Confidence3");
102  enableResultPointClouds<PointT>("Confidence4");
103  }
104  else
105  {
106  ARMARX_TRACE;
107  enableResultPointClouds<pcl::PointXYZI>("Confidence0");
108  enableResultPointClouds<pcl::PointXYZI>("Confidence1");
109  enableResultPointClouds<pcl::PointXYZI>("Confidence2");
110  enableResultPointClouds<pcl::PointXYZI>("Confidence3");
111  enableResultPointClouds<pcl::PointXYZI>("Confidence4");
112  }
113  }
114 
115  {
116  ARMARX_TRACE;
117  createOrUpdateRemoteGuiTab(
118  RemoteGui::MakeGuiConfig("Config", config_.getUpToDateReadBuffer()),
119  [this](armarx::RemoteGui::TabProxy& prx)
120  {
121  std::unique_lock lock(write_mutex_);
122  prx.getValue(config_.getWriteBuffer(), "Config");
123  config_.commitWrite();
124  });
125  }
126 }
127 
128 void
129 armarx::DenseCRFSegmentationProcessor::onDisconnectPointCloudProcessor()
130 {
131 }
132 
133 void
134 armarx::DenseCRFSegmentationProcessor::onExitPointCloudProcessor()
135 {
136 }
137 
138 void
139 armarx::DenseCRFSegmentationProcessor::process()
140 {
141  ARMARX_TRACE;
142  pcl::PointCloud<PointT>::Ptr input_cloud_ptr(new pcl::PointCloud<PointT>());
143  pcl::PointCloud<PointT>::Ptr output_cloud_ptr(new pcl::PointCloud<PointT>());
144 
145  if (!waitForPointClouds())
146  {
147  ARMARX_INFO << "Timeout or error while waiting for point cloud data";
148  return;
149  }
150  else
151  {
152  getPointClouds(input_cloud_ptr);
153  }
154  // TODO: there will be a local exception when the number of labels is bigger than the number of classes set
155  // which is hard to decipher
156  std::vector<int> indices;
157  input_cloud_ptr->is_dense = false;
158  pcl::removeNaNFromPointCloud(*input_cloud_ptr, *input_cloud_ptr, indices);
159  relabelPointCloud(input_cloud_ptr);
160  ARMARX_DEBUG << "Point Cloud has " << input_cloud_ptr->size() << " Points before Downsampling";
161  auto config = config_.getUpToDateReadBuffer();
162  if (config.general.use_vertex_only_graph)
163  {
164  ARMARX_TRACE;
165  computeVertexOnlyGraph(input_cloud_ptr);
166  }
167  else
168  {
169  ARMARX_TRACE;
170  computeGraphUsingVoxelGrid(input_cloud_ptr);
171  }
172  // if (not pre_segmented_)
173  // {
174  // computeRandomWalkerSegmentation();
175  // }
176  updatePersistentGraph();
177  PointCloudWithNormalT::Ptr temp_cloud_ptr = pcl::graph::point_cloud(*current_graph_ptr_);
178  // pcl::copyPointCloud(*temp_cloud_ptr, *output_cloud_ptr);
179  copyRGBLNormalToRGBL(*temp_cloud_ptr, *output_cloud_ptr);
180  // generateRandomClassLabels();
181  config = config_.getUpToDateReadBuffer();
182  DenseGraphCRF<GraphWithTimestamp> crf(persistent_graph_ptr_, config.general.num_classes);
183  crf.computeUnaryEnergyFromGraph(config.general.ground_truth_prob);
184  if (config.edge.use_xyz && !config.edge.use_combined)
185  {
186  ARMARX_TRACE;
187  TIMING_START(AddXYZFeature);
188  std::vector<float> xyz_infl(3);
189  std::fill(xyz_infl.begin(), xyz_infl.end(), config.edge.xyz_influence);
190  crf.addPairwiseGaussianFeature<XYZFeature<GraphWithTimestamp>>(xyz_infl,
191  new PottsCompatibility(10));
192  TIMING_END_STREAM(AddXYZFeature, ARMARX_DEBUG);
193  }
194  if (config.edge.use_normals && !config.edge.use_combined)
195  {
196  ARMARX_TRACE;
197  // TODO: use normals causes segfault
198  TIMING_START(AddNormalFeature);
199  std::vector<float> normals_infl(3);
200  std::fill(normals_infl.begin(), normals_infl.end(), config.edge.normals_influence);
201  crf.addPairwiseGaussianFeature<NormalFeature<GraphWithTimestamp>>(
202  normals_infl, new PottsCompatibility(10));
203  TIMING_END_STREAM(AddNormalFeature, ARMARX_DEBUG);
204  }
205  if (config.edge.use_curvature && !config.edge.use_combined)
206  {
207  ARMARX_TRACE;
208  TIMING_START(AddCurvatureFeature);
209  std::vector<float> curv_infl(1);
210  std::fill(curv_infl.begin(), curv_infl.end(), config.edge.curvature_influence);
211  crf.addPairwiseGaussianFeature<CurvatureFeature<GraphWithTimestamp>>(
212  curv_infl, new PottsCompatibility(10));
213  TIMING_END_STREAM(AddCurvatureFeature, ARMARX_DEBUG);
214  }
215  if (config.edge.use_rgb && !config.edge.use_combined)
216  {
217  ARMARX_TRACE;
218  TIMING_START(AddRGBFeature);
219  std::vector<float> rgb_infl(3);
220  std::fill(rgb_infl.begin(), rgb_infl.end(), config.edge.rgb_influence);
221  crf.addPairwiseGaussianFeature<RGBFeature<GraphWithTimestamp>>(rgb_infl,
222  new PottsCompatibility(10));
223  TIMING_END_STREAM(AddRGBFeature, ARMARX_DEBUG);
224  }
225  if (config.edge.use_time && !config.edge.use_combined)
226  {
227  ARMARX_TRACE;
228  TIMING_START(AddTimeFeature);
229  std::vector<float> time_infl(1);
230  std::fill(time_infl.begin(), time_infl.end(), config.edge.time_influence);
231  crf.addPairwiseGaussianFeature<TimeFeature>(time_infl, new PottsCompatibility(1));
232  TIMING_END_STREAM(AddTimeFeature, ARMARX_DEBUG);
233  }
234  if (config.edge.use_combined)
235  {
236  ARMARX_TRACE;
237  TIMING_START(AddCombinedFeature);
238  int num_feat = 0;
239  num_feat += config.edge.use_rgb ? 3 : 0;
240  num_feat += config.edge.use_normals ? 3 : 0;
241  num_feat += config.edge.use_xyz ? 3 : 0;
242  num_feat += config.edge.use_time ? 1 : 0;
243  num_feat += config.edge.use_curvature ? 1 : 0;
244  std::vector<float> comb_infl(num_feat);
245  int j;
246  for (int i = 0; i < 3; i++)
247  {
248  j = 0;
249  if (config.edge.use_rgb)
250  {
251  comb_infl[i + j] = config.edge.rgb_influence;
252  j += 3;
253  }
254  if (config.edge.use_normals)
255  {
256  comb_infl[i + j] = config.edge.normals_influence;
257  j += 3;
258  }
259  if (config.edge.use_xyz)
260  {
261  comb_infl[i + j] = config.edge.xyz_influence;
262  j += 3;
263  }
264  }
265  if (config.edge.use_curvature)
266  {
267  comb_infl[j] = config.edge.curvature_influence;
268  j++;
269  }
270  if (config.edge.use_time)
271  {
272  comb_infl[j] = config.edge.time_influence;
273  }
274  crf.addCombinedGaussianFeature(comb_infl,
275  config.edge.use_rgb,
276  config.edge.use_normals,
277  config.edge.use_xyz,
278  config.edge.use_curvature,
279  config.edge.use_time,
280  new PottsCompatibility(config.edge.potts_compatibilty));
281  TIMING_END_STREAM(AddCombinedFeature, ARMARX_DEBUG);
282  }
283 
284 
285  // VectorXs map = crf.map(map_iterations_);
286  ARMARX_TRACE;
287  TIMING_START(MAP);
288  Eigen::VectorXf confidence = crf.map_and_confidence(config.general.map_iterations);
289  TIMING_END_STREAM(MAP, ARMARX_DEBUG);
290  if (config.out.provide_graph_pclouds)
291  {
292  provideAllGraphs();
293  }
294  if (config.out.provide_confidence_pclouds)
295  {
296  provideAllConfidences();
297  }
298  output_cloud_ptr->points.clear();
299  GraphWithTimestamp outputGraph = retrieveCurrentGraphFromPersistentGraph();
300  // ConfidenceMap cm = boost::get(boost::vertex_confidence_t(), outputGraph.m_graph);
301 
302 
303  temp_cloud_ptr = pcl::graph::point_cloud(outputGraph);
304  // pcl::copyPointCloud(*temp_cloud_ptr, *output_cloud_ptr);
305  copyRGBLNormalToRGBL(*temp_cloud_ptr, *output_cloud_ptr);
306  // FIXME: providePointClouds segfaults when called with pcl::pointXYZRGB
307  outputGraph = retrieveGraphFromPersistentGraph(timestamp_queue_.back());
308  ConfidenceMap cm = boost::get(boost::vertex_confidence_t(), outputGraph.m_graph);
309  temp_cloud_ptr = pcl::graph::point_cloud(outputGraph);
310  pcl::PointCloud<pcl::PointXYZI>::Ptr confidence_cloud_ptr(
311  new pcl::PointCloud<pcl::PointXYZI>());
312  pcl::copyPointCloud(*temp_cloud_ptr, *confidence_cloud_ptr);
313  for (long unsigned int i = 0; i < confidence_cloud_ptr->points.size(); i++)
314  {
315  // float angle = confidence[i] * M_PI;
316  // float red = 255.0 * sin(angle);
317  // float green = 255.0 * sin(angle + 2 * M_PI / 3.); // + 60°
318  // float blue = 255.0 * sin(angle + 4 * M_PI / 3.); // + 120°
319  float conf = cm[i];
320  // colormap::Color c = jet.getColor(conf);
321  // int rgb = int(conf*2147483647);
322  // uint8_t r = (rgb >> 16) & 0x0000ff;
323  // uint8_t g = (rgb >> 8) & 0x0000ff;
324  // uint8_t b = (rgb) & 0x0000ff;
325 
326 
327  // confidence_cloud_ptr->points[i].r = c.r * 255.0;
328  // confidence_cloud_ptr->points[i].g = c.g * 255.0;
329  // confidence_cloud_ptr->points[i].b = c.b * 255.0;
330  confidence_cloud_ptr->points[i].intensity = conf;
331  }
332  ARMARX_TRACE;
333  output_cloud_ptr->header = input_cloud_ptr->header;
334  provideResultPointClouds<decltype(output_cloud_ptr)>(output_cloud_ptr, "CRFSegmentationResult");
335  provideResultPointClouds<decltype(confidence_cloud_ptr)>(confidence_cloud_ptr,
336  "CRFSegmentationConfidence");
337  pcl::PointXYZRGBA i;
338 }
339 
340 void
341 DenseCRFSegmentationProcessor::computeGraphUsingVoxelGrid(const PointCloudT::Ptr inputCloudPtr)
342 {
343  const auto& config = config_.getUpToDateReadBuffer();
344  double r = config.general.voxel_resolution;
345  pcl::graph::VoxelGridGraphBuilder<PointT, Graph> graph_builder(r);
346  graph_builder.setInputCloud(inputCloudPtr);
347  TIMING_START(ComputeGraph);
348  graph_builder.compute(*current_graph_ptr_);
349  TIMING_END_STREAM(ComputeGraph, ARMARX_DEBUG);
350  ARMARX_DEBUG << "Graph has " << static_cast<int>(boost::num_vertices(*current_graph_ptr_))
351  << " Vertices and " << static_cast<int>(boost::num_edges(*current_graph_ptr_))
352  << " Edges";
353  if (config.edge.use_curvature || config.edge.use_normals)
354  {
355  TIMING_START(ComputeNormalsAndCurvatures);
356  pcl::graph::computeNormalsAndCurvatures(*current_graph_ptr_);
357  TIMING_END_STREAM(ComputeNormalsAndCurvatures, ARMARX_DEBUG);
358  TIMING_START(ComputeSignedCurvatures);
359  pcl::graph::computeSignedCurvatures(*current_graph_ptr_);
360  TIMING_END_STREAM(ComputeSignedCurvatures, ARMARX_DEBUG);
361  }
362 }
363 
364 Eigen::MatrixXf
365 DenseCRFSegmentationProcessor::computeRandomUnaryEnergy(int num_points)
366 {
367  const auto& config = config_.getUpToDateReadBuffer();
368  Eigen::MatrixXf energy(config.general.num_classes, num_points);
369  const double u_energy = -log(1.0 / config.general.num_classes);
370  const double n_energy =
371  -log((1.0 - config.general.ground_truth_prob) / (config.general.num_classes - 1));
372  const double p_energy = -log(config.general.ground_truth_prob);
373  energy.fill(u_energy);
374  std::random_device dev;
375  std::mt19937 rng(dev());
376  std::uniform_int_distribution<std::mt19937::result_type> dist(0,
377  config.general.num_classes - 1);
378  for (int k = 0; k < num_points; k++)
379  {
380  // Set the energy
381  int r = dist(rng);
382  if (r >= 0)
383  {
384  energy.col(k).fill(n_energy);
385  energy(r, k) = p_energy;
386  }
387  }
388  return energy;
389 }
390 
391 void
392 DenseCRFSegmentationProcessor::generateRandomClassLabels()
393 {
394 
395  VertexIterator vi, v_end;
396  std::random_device dev;
397  std::mt19937 rng(dev());
398  std::uniform_int_distribution<std::mt19937::result_type> dist(
399  0, config_.getUpToDateReadBuffer().general.num_classes - 1);
400  for (boost::tie(vi, v_end) = boost::vertices(*current_graph_ptr_); vi != v_end; ++vi)
401  {
402  int r = dist(rng);
403  (*current_graph_ptr_)[*vi].label = r;
404  }
405 }
406 
407 //void DenseCRFSegmentationProcessor::computeRandomWalkerSegmentation()
408 //{
409 // computeEdgeWeights();
410 // pcl::segmentation::RandomWalkerSegmentation<PointT> rws;
411 // rws.setInputGraph(current_graph_ptr_);
412 // rws.setSeeds(selectRandomSeeds());
413 // std::vector<pcl::PointIndices> clusters;
414 // rws.segment(clusters);
415 // PointCloudWithNormalT::Ptr tempCloud = pcl::graph::point_cloud(*current_graph_ptr_);
416 // VertexIterator vi, v_end;
417 // int lbl = 0;
418 // for (auto clstr_ptr = clusters.begin(); clstr_ptr != clusters.end(); clstr_ptr++, lbl++)
419 // {
420 // for (auto pt_ind_ptr = clstr_ptr->indices.begin(); pt_ind_ptr != clstr_ptr->indices.end(); pt_ind_ptr++)
421 // {
422 // (*current_graph_ptr_)[static_cast<Graph::vertex_descriptor>(*pt_ind_ptr)].label = lbl;
423 // }
424 // }
425 //}
426 
427 pcl::PointCloud<pcl::PointXYZL>::ConstPtr
428 DenseCRFSegmentationProcessor::selectRandomSeeds()
429 {
430  auto num_classes = config_.getUpToDateReadBuffer().general.num_classes;
431  VertexIterator vi, v_end;
432  std::vector<Graph::vertex_descriptor> v_out;
433  boost::tie(vi, v_end) = boost::vertices(*current_graph_ptr_);
434  // select num_classes_ random samples from the vertices as seeds
435  std::sample(vi,
436  v_end,
437  std::back_inserter(v_out),
438  num_classes - 1,
439  std::mt19937{std::random_device{}()});
440 
441  PointCloudWithNormalT::Ptr tempCloudPtr(new PointCloudWithNormalT(num_classes, 1));
442  int j = 0;
443  for (auto i = v_out.begin(); i != v_out.end(); i++, j++)
444  {
445  tempCloudPtr->points[j] = (*current_graph_ptr_)[*i];
446  // each seed gets an individiual label
447  tempCloudPtr->points[j].label = j;
448  }
449 
450  pcl::PointCloud<pcl::PointXYZL>::Ptr outCloudPtr(new pcl::PointCloud<pcl::PointXYZL>());
451  pcl::copyPointCloud(*tempCloudPtr, *outCloudPtr);
452  return outCloudPtr;
453 }
454 
455 //void DenseCRFSegmentationProcessor::computeEdgeWeights()
456 //{
457 // using namespace pcl::graph;
458 // typedef EdgeWeightComputer<Graph> EWC;
459 // EWC computer;
460 // if (use_xyz_)
461 // {
462 // float influence = xyz_influence_;
463 // float multiplier = 1.0;
464 // computer.addTerm<terms::XYZ>(influence, multiplier, EWC::NORMALIZATION_LOCAL);
465 // }
466 // if (use_normals_)
467 // {
468 // float influence = normals_influence_;
469 // float multiplier = 1.0;
470 // computer.addTerm<terms::Normal>(influence, multiplier);
471 // }
472 // if (use_curvature_)
473 // {
474 // float influence = curvature_influence_;
475 // float multiplier = 1.0;
476 // computer.addTerm<terms::Curvature>(influence, multiplier);
477 // }
478 // if (use_rgb_)
479 // {
480 // float influence = rgb_influence_;
481 // float multiplier = 1.0;
482 // computer.addTerm<terms::RGB>(influence, multiplier, EWC::NORMALIZATION_GLOBAL);
483 // }
484 // computer.setSmallWeightThreshold(1e-5);
485 // computer.setSmallWeightPolicy(EWC::SMALL_WEIGHT_COERCE_TO_THRESHOLD);
486 // computer.compute(*current_graph_ptr_);
487 //}
488 
489 void
490 DenseCRFSegmentationProcessor::updatePersistentGraph()
491 {
492  ARMARX_TRACE;
493  TIMING_START(UpdateRootGraph)
494  double current_ts =
495  static_cast<double>(current_graph_ptr_->m_graph.m_point_cloud->header.stamp / 1000.0) -
496  initial_time_;
497  timestamp_queue_.push_back(current_ts);
498  // graph_queue_.push_back(*current_graph_ptr_);
499  if (timestamp_queue_.size() > 5)
500  {
501  ARMARX_TRACE;
502  double tsToRemove = timestamp_queue_.front();
503  timestamp_queue_.pop_front();
504  removeTimestampFromPersistentGraph(tsToRemove);
505  }
506  addCurrentGraphToPersistentGraph();
507  ARMARX_DEBUG << "Root Graph consisting of " << timestamp_queue_.size() << " Subgraphs has "
508  << static_cast<int>(boost::num_vertices(*persistent_graph_ptr_)) << " Vertices";
509  TIMING_END_STREAM(UpdateRootGraph, ARMARX_DEBUG)
510 }
511 
512 void
513 DenseCRFSegmentationProcessor::addGraphToPersistentGraph(Graph& graph)
514 {
515  TimestampMap tsm = boost::get(boost::vertex_timestamp_t(), persistent_graph_ptr_->m_graph);
516  ConfidenceMap cm = boost::get(boost::vertex_confidence_t(), persistent_graph_ptr_->m_graph);
517  VertexIterator vi, v_end;
518  double current_ts =
519  static_cast<double>(graph.m_graph.m_point_cloud->header.stamp / 1000.0) - initial_time_;
520  for (boost::tie(vi, v_end) = boost::vertices(graph); vi != v_end; ++vi)
521  {
522  VertexWTsId new_vertex = boost::add_vertex(*persistent_graph_ptr_);
523  boost::put(tsm, new_vertex, current_ts);
524  // value of -1.0 signals it has not been set
525  boost::put(cm, new_vertex, -1.0);
526  PointWithNormalT point = graph.m_graph.m_point_cloud->points[*vi];
527  (*persistent_graph_ptr_)[new_vertex] = point;
528  }
529 }
530 
531 void
532 DenseCRFSegmentationProcessor::addCurrentGraphToPersistentGraph()
533 {
534  ARMARX_TRACE;
535  addGraphToPersistentGraph(*current_graph_ptr_);
536 }
537 
538 void
539 DenseCRFSegmentationProcessor::removeGraphFromPersistentGraph(Graph& graph)
540 {
541  ARMARX_TRACE;
542  double graph_ts =
543  static_cast<double>(graph.m_graph.m_point_cloud->header.stamp / 1000.0) - initial_time_;
544  removeTimestampFromPersistentGraph(graph_ts);
545 }
546 
547 void
548 DenseCRFSegmentationProcessor::removeCurrentGraphFromPersistentGraph()
549 {
550  ARMARX_TRACE;
551  removeGraphFromPersistentGraph(*current_graph_ptr_);
552 }
553 
554 void
555 DenseCRFSegmentationProcessor::removeTimestampFromPersistentGraph(double ts)
556 {
557  ARMARX_TRACE;
558  TimestampMap tsm = boost::get(boost::vertex_timestamp_t(), persistent_graph_ptr_->m_graph);
559  typedef vertex_timestamp_unequal_filter<TimestampMap> FilterT;
560  FilterT filter(tsm, ts);
561  GraphWithTimestamp filtered_graph = filterAndCopyPersistentGraph<FilterT>(tsm, filter);
562  GraphWithTimestampPtr temp_graph_ptr = boost::make_shared<GraphWithTimestamp>(filtered_graph);
563  persistent_graph_ptr_.swap(temp_graph_ptr);
564 }
565 
566 GraphWithTimestamp
567 DenseCRFSegmentationProcessor::retrieveGraphFromPersistentGraph(double ts)
568 {
569  ARMARX_TRACE;
570  TimestampMap tsm = boost::get(boost::vertex_timestamp_t(), persistent_graph_ptr_->m_graph);
571  typedef vertex_timestamp_equal_filter<TimestampMap> FilterT;
572  FilterT filter(tsm, ts);
573  GraphWithTimestamp filtered_graph = filterAndCopyPersistentGraph<FilterT>(tsm, filter);
574  return filtered_graph;
575 }
576 
577 GraphWithTimestamp
578 DenseCRFSegmentationProcessor::retrieveCurrentGraphFromPersistentGraph()
579 {
580  ARMARX_TRACE;
581  double current_ts =
582  static_cast<double>(current_graph_ptr_->m_graph.m_point_cloud->header.stamp / 1000.0) -
583  initial_time_;
584  return retrieveGraphFromPersistentGraph(current_ts);
585 }
586 
587 void
588 DenseCRFSegmentationProcessor::provideAllGraphs()
589 {
590  ARMARX_TRACE;
591  for (unsigned int i = 0; i < timestamp_queue_.size(); i++)
592  {
593  double ts = timestamp_queue_.at(i);
594  GraphWithTimestamp graph = retrieveGraphFromPersistentGraph(ts);
595  PointCloudT::Ptr output_cloud_ptr(new PointCloudT());
596  PointCloudWithNormalT::Ptr temp_cloud_ptr(new PointCloudWithNormalT());
597  temp_cloud_ptr = pcl::graph::point_cloud(graph);
598  // pcl::graph::indices (Subgraph& g)
599  // pcl::copyPointCloud(*temp_cloud_ptr, *output_cloud_ptr);
600  // FIXME: providePointClouds segfaults when called with pcl::pointXYZRGB
601  copyRGBLNormalToRGBL(*temp_cloud_ptr, *output_cloud_ptr);
602  provideResultPointClouds(output_cloud_ptr, "Graph" + std::to_string(i));
603  }
604 }
605 
606 void
607 DenseCRFSegmentationProcessor::computeVertexOnlyGraph(PointCloudT::Ptr input_cloud_ptr)
608 {
609  ARMARX_TRACE;
610  const auto& config = config_.getUpToDateReadBuffer();
611  TIMING_START(VoxelDownsample);
612  pcl::PointCloud<PointT>::Ptr temp_cloud_ptr(new pcl::PointCloud<PointT>());
613  ARMARX_DEBUG << input_cloud_ptr->size() << "before downsampling";
614  // TODO: Voxel downsampling produces a cloud with a=0 for every point; Fix this
615  if (config.general.use_approximate_voxels)
616  {
617  ARMARX_TRACE;
618  pcl::ApproximateVoxelGrid<PointT> vg;
619  vg.setInputCloud(input_cloud_ptr);
620  vg.setLeafSize(config.general.voxel_resolution * 100,
621  config.general.voxel_resolution * 100,
622  config.general.voxel_resolution * 100);
623  vg.filter(*temp_cloud_ptr);
624  temp_cloud_ptr.swap(input_cloud_ptr);
625  }
626  else
627  {
628  ARMARX_TRACE;
629  pcl::VoxelGrid<PointT> vg;
630  vg.setInputCloud(input_cloud_ptr);
631  vg.setLeafSize(config.general.voxel_resolution * 100,
632  config.general.voxel_resolution * 100,
633  config.general.voxel_resolution * 100);
634  vg.filter(*temp_cloud_ptr);
635  temp_cloud_ptr.swap(input_cloud_ptr);
636  }
637  ARMARX_TRACE;
638  ARMARX_DEBUG << input_cloud_ptr->size() << "after downsampling";
639  TIMING_END_STREAM(VoxelDownsample, ARMARX_DEBUG);
640  TIMING_START(NormalComputation);
641  pcl::NormalEstimation<PointT, PointWithNormalT> ne;
642  ne.setInputCloud(input_cloud_ptr);
643  pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());
644  ne.setSearchMethod(tree);
645  PointCloudWithNormalT::Ptr cloud_normals(new PointCloudWithNormalT());
646  // pcl::copyPointCloud(*input_cloud_ptr, *cloud_normals);
647  copyRGBLToRGBLNormal(*input_cloud_ptr, *cloud_normals);
648  // Use all neighbors in a sphere of radius 3cm
649  ne.setRadiusSearch(100 * 5 * config.general.voxel_resolution);
650  ARMARX_TRACE;
651  // Compute the features
652  ne.compute(*cloud_normals);
653  TIMING_END_STREAM(NormalComputation, ARMARX_DEBUG);
654  GraphPtr temp_graph_ptr = boost::make_shared<Graph>(Graph(cloud_normals));
655  current_graph_ptr_.swap(temp_graph_ptr);
656  ARMARX_DEBUG << "Graph has " << static_cast<int>(boost::num_vertices(*current_graph_ptr_))
657  << " Vertices and " << static_cast<int>(boost::num_edges(*current_graph_ptr_))
658  << " Edges";
659 }
660 
661 void
662 DenseCRFSegmentationProcessor::copyRGBLToRGBLNormal(PointCloudT& input_cloud,
663  PointCloudWithNormalT& output_cloud)
664 {
665  ARMARX_TRACE;
666  output_cloud.header = input_cloud.header;
667  output_cloud.width = input_cloud.width;
668  output_cloud.height = input_cloud.height;
669  output_cloud.is_dense = input_cloud.is_dense;
670  output_cloud.sensor_orientation_ = input_cloud.sensor_orientation_;
671  output_cloud.sensor_origin_ = input_cloud.sensor_origin_;
672  output_cloud.points.resize(input_cloud.points.size());
673 
674  if (input_cloud.points.size() == 0)
675  {
676  ARMARX_TRACE;
677  return;
678  }
679  ARMARX_TRACE;
680  for (size_t i = 0; i < output_cloud.size(); i++)
681  {
682  output_cloud.points[i].x = input_cloud.points[i].x;
683  output_cloud.points[i].y = input_cloud.points[i].y;
684  output_cloud.points[i].z = input_cloud.points[i].z;
685  output_cloud.points[i].r = input_cloud.points[i].r;
686  output_cloud.points[i].g = input_cloud.points[i].g;
687  output_cloud.points[i].b = input_cloud.points[i].b;
688  output_cloud.points[i].a = uint8_t(255);
689  output_cloud.points[i].label = input_cloud.points[i].label;
690  output_cloud.points[i].normal_x = 0;
691  output_cloud.points[i].normal_y = 0;
692  output_cloud.points[i].normal_z = 0;
693  output_cloud.points[i].curvature = 0;
694  }
695 }
696 
697 void
698 DenseCRFSegmentationProcessor::copyRGBLNormalToRGBL(PointCloudWithNormalT& input_cloud,
699  PointCloudT& output_cloud)
700 {
701  ARMARX_TRACE;
702  output_cloud.header = input_cloud.header;
703  output_cloud.width = input_cloud.width;
704  output_cloud.height = input_cloud.height;
705  output_cloud.is_dense = input_cloud.is_dense;
706  output_cloud.sensor_orientation_ = input_cloud.sensor_orientation_;
707  output_cloud.sensor_origin_ = input_cloud.sensor_origin_;
708  output_cloud.points.resize(input_cloud.points.size());
709 
710  if (input_cloud.points.size() == 0)
711  {
712  return;
713  }
714  ARMARX_TRACE;
715  for (size_t i = 0; i < output_cloud.size(); i++)
716  {
717  output_cloud.points[i].x = input_cloud.points[i].x;
718  output_cloud.points[i].y = input_cloud.points[i].y;
719  output_cloud.points[i].z = input_cloud.points[i].z;
720  output_cloud.points[i].r = input_cloud.points[i].r;
721  output_cloud.points[i].g = input_cloud.points[i].g;
722  output_cloud.points[i].b = input_cloud.points[i].b;
723  output_cloud.points[i].a = uint8_t(255);
724  output_cloud.points[i].label = input_cloud.points[i].label;
725  }
726 }
727 
728 void
729 DenseCRFSegmentationProcessor::relabelPointCloud(pcl::PointCloud<PointT>::Ptr input_cloud_ptr)
730 {
731  ARMARX_TRACE;
732  std::map<uint32_t, uint32_t> labelmap;
733  uint32_t highest_new_label = 1;
734  for (auto& point : input_cloud_ptr->points)
735  {
736  if (labelmap.find(point.label) != labelmap.end())
737  {
738  point.label = labelmap.at(point.label);
739  }
740  else
741  {
742  labelmap.insert(std::make_pair(point.label, highest_new_label));
743  point.label = highest_new_label;
744  highest_new_label++;
745  }
746  }
747 }
748 
749 void
750 DenseCRFSegmentationProcessor::provideAllConfidences()
751 {
752  ARMARX_TRACE;
753  colormap::MATLAB::Jet jet;
754  bool colorize = config_.getUpToDateReadBuffer().out.colorize_confidence_pclouds;
755  for (unsigned int i = 0; i < timestamp_queue_.size(); i++)
756  {
757  double ts = timestamp_queue_.at(i);
758  GraphWithTimestamp graph = retrieveGraphFromPersistentGraph(ts);
759  ConfidenceMap cm = boost::get(boost::vertex_confidence_t(), graph.m_graph);
760  PointCloudWithNormalT::Ptr temp_cloud_ptr(new PointCloudWithNormalT());
761  temp_cloud_ptr = pcl::graph::point_cloud(graph);
762  // pcl::graph::indices (Subgraph& g)
763  // pcl::copyPointCloud(*temp_cloud_ptr, *output_cloud_ptr);
764  // FIXME: providePointClouds segfaults when called with pcl::pointXYZRGB
765  if (colorize)
766  {
767  PointCloudT::Ptr output_cloud_ptr(new PointCloudT());
768  copyRGBLNormalToRGBL(*temp_cloud_ptr, *output_cloud_ptr);
769  for (long unsigned int j = 0; j < output_cloud_ptr->points.size(); j++)
770  {
771  float conf = cm[j];
772  colormap::Color c = jet.getColor(conf);
773  output_cloud_ptr->points[j].r = c.r * 255.0;
774  output_cloud_ptr->points[j].g = c.g * 255.0;
775  output_cloud_ptr->points[j].b = c.b * 255.0;
776  }
777  provideResultPointClouds<decltype(output_cloud_ptr)>(output_cloud_ptr,
778  "Confidence" + std::to_string(i));
779  }
780  else
781  {
782  pcl::PointCloud<pcl::PointXYZI>::Ptr confidence_cloud_ptr(
783  new pcl::PointCloud<pcl::PointXYZI>());
784  pcl::copyPointCloud(*temp_cloud_ptr, *confidence_cloud_ptr);
785  for (long unsigned int j = 0; j < confidence_cloud_ptr->points.size(); j++)
786  {
787  float conf = cm[j];
788  confidence_cloud_ptr->points[j].intensity = conf;
789  }
790  provideResultPointClouds<decltype(confidence_cloud_ptr)>(
791  confidence_cloud_ptr, "Confidence" + std::to_string(i));
792  }
793  }
794 }
795 
796 Vector5f
797 DenseCRFSegmentationProcessor::getCurrentEdgeWeights(const Ice::Current& current)
798 {
799  const auto& config = config_.getUpToDateReadBuffer().edge;
800  Vector5f vec{config.xyz_influence,
801  config.rgb_influence,
802  config.normals_influence,
803  config.curvature_influence,
804  config.time_influence};
805  return vec;
806 }
807 
808 void
809 DenseCRFSegmentationProcessor::setEdgeWeights(float xyz,
810  float rgb,
811  float normals,
812  float curvature,
813  float time,
814  const Ice::Current& current)
815 {
816  // TODO: use mutex
817  std::unique_lock lock(write_mutex_);
818  auto& config = config_.getWriteBuffer().edge;
819  config.xyz_influence = xyz;
820  config.rgb_influence = rgb;
821  config.normals_influence = normals;
822  config.curvature_influence = curvature;
823  config.time_influence = time;
824  config_.commitWrite();
825 }
armarx::PointCloudWithNormalT
pcl::PointCloud< PointWithNormalT > PointCloudWithNormalT
Definition: Common.h:33
armarx::RemoteGui::MakeGuiConfig
RemoteGui::WidgetPtr MakeGuiConfig(const std::string &name, const T &val)
Definition: MakeGuiConfig.h:9
armarx::VertexWTsId
boost::graph_traits< GraphWithTimestamp >::vertex_descriptor VertexWTsId
Definition: Common.h:69
armarx::DenseCRFSegmentationProcessor::getCurrentEdgeWeights
Vector5f getCurrentEdgeWeights(const Ice::Current &current) override
Definition: DenseCRFSegmentationProcessor.cpp:797
TIMING_START
#define TIMING_START(name)
Definition: TimeUtil.h:289
armarx::VertexIterator
boost::graph_traits< Graph >::vertex_iterator VertexIterator
Definition: Common.h:71
armarx::TimeFeature
Definition: DenseCRFFeatureTerms.h:487
boost::vertex_timestamp_t
Definition: Common.h:16
armarx::DenseCRFSegmentationProcessor::onInitPointCloudProcessor
void onInitPointCloudProcessor() override
Definition: DenseCRFSegmentationProcessor.cpp:64
armarx::CurvatureFeature
Definition: DenseCRFFeatureTerms.h:96
armarx::DenseGraphCRF::computeUnaryEnergyFromGraph
void computeUnaryEnergyFromGraph(float gt_prob)
Definition: DenseGraphCRF.hpp:101
armarx::NormalFeature
Definition: DenseCRFFeatureTerms.h:67
boost::shared_ptr< GraphWithTimestamp >
armarx::ConfidenceMap
boost::property_map< CloudGraphWithTimestamp, boost::vertex_confidence_t >::type ConfidenceMap
Definition: Common.h:77
DenseCRFFeatureTerms.h
armarx::WriteBufferedTripleBuffer::commitWrite
void commitWrite()
Definition: TripleBuffer.h:383
armarx::DenseCRFSegmentationProcessor::onExitPointCloudProcessor
void onExitPointCloudProcessor() override
Definition: DenseCRFSegmentationProcessor.cpp:134
pcl::graph::computeNormalsAndCurvatures
void computeNormalsAndCurvatures(Graph &graph, bool neighborhood_1ring=false)
Compute normals and curvatures for all vertices in a graph.
Definition: common.hpp:55
Pose.h
colormap::MATLAB::Jet
Definition: jet.h:13
DenseGraphCRF.hpp
armarx::DenseCRFSegmentationProcessor::onConnectPointCloudProcessor
void onConnectPointCloudProcessor() override
Definition: DenseCRFSegmentationProcessor.cpp:76
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
armarx::DenseCRFSegmentationProcessorPropertyDefinitions
Definition: DenseCRFSegmentationProcessor.h:213
armarx::DenseCRFSegmentationProcessor::setEdgeWeights
void setEdgeWeights(float xyz, float rgb, float normals, float curvature, float time, const Ice::Current &current) override
Definition: DenseCRFSegmentationProcessor.cpp:809
pcl::PointXYZRGBLNormal
Definition: PointXYZRGBLNormal.hpp:55
TIMING_END_STREAM
#define TIMING_END_STREAM(name, os)
Definition: TimeUtil.h:310
armarx::GraphPtr
Agraph_t * GraphPtr
Definition: Layout.h:41
armarx::XYZFeature
Definition: DenseCRFFeatureTerms.h:38
boost::vertex_confidence_t
Definition: Common.h:21
armarx::RemoteGui::TabProxy
Definition: WidgetProxy.h:17
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:77
visionx::PointCloudProcessor::provideResultPointClouds
void provideResultPointClouds(const PointCloudPtrT &pointClouds, std::string providerName="")
sends result PointClouds for visualization
Definition: PointCloudProcessor.h:288
armarx::DenseGraphCRF::addPairwiseGaussianFeature
void addPairwiseGaussianFeature(std::vector< float > sigma, LabelCompatibility *function, KernelType kernel_type=DIAG_KERNEL, NormalizationType normalization_type=NORMALIZE_SYMMETRIC)
Definition: DenseGraphCRF.hpp:56
pcl::graph::indices
pcl::PointIndices::Ptr indices(const PCG &g)
Retrieve the indices of the points of the point cloud stored in a point cloud graph that actually bel...
Definition: point_cloud_graph.h:717
colormap::MATLAB::Jet::getColor
Color getColor(double x) const override
Definition: jet.h:29
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:184
pcl::graph::VoxelGridGraphBuilder
This class builds a BGL graph representing an input dataset by using octree::OctreePointCloud.
Definition: voxel_grid_graph_builder.h:54
armarx::RGBFeature
Definition: DenseCRFFeatureTerms.h:124
pcl::graph::point_cloud
pcl::PointCloud< P >::Ptr point_cloud(PCG &g)
Retrieve the point cloud stored in a point cloud graph.
Definition: point_cloud_graph.h:690
armarx::Graph
boost::subgraph< CloudGraph > Graph
Definition: Common.h:58
armarx::WriteBufferedTripleBuffer::getWriteBuffer
T & getWriteBuffer()
Definition: TripleBuffer.h:325
armarx::TimestampMap
boost::property_map< CloudGraphWithTimestamp, boost::vertex_timestamp_t >::type TimestampMap
Definition: Common.h:75
armarx::PointCloudT
pcl::PointCloud< PointT > PointCloudT
Definition: Common.h:32
armarx::DenseGraphCRF
Definition: DenseGraphCRF.hpp:42
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:41
armarx::TimeUtil::GetTime
static IceUtil::Time GetTime(TimeMode timeMode=TimeMode::VirtualTime)
Get the current time.
Definition: TimeUtil.cpp:42
colormap::Color
Definition: colormap.h:12
armarx::DenseCRFSegmentationProcessor::onDisconnectPointCloudProcessor
void onDisconnectPointCloudProcessor() override
Definition: DenseCRFSegmentationProcessor.cpp:129
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:79
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
armarx::PropertyDefinitionContainer::optional
decltype(auto) optional(PropertyType &setter, const std::string &name, const std::string &description="", PropertyDefinitionBase::PropertyConstness constness=PropertyDefinitionBase::eConstant)
Definition: PropertyDefinitionContainer.h:84
IceUtil::Handle< class PropertyDefinitionContainer >
pcl::graph::computeSignedCurvatures
void computeSignedCurvatures(Graph &graph)
Compute the type of curvature (concave/convex) for each vertex.
Definition: common.hpp:111
colormap::Color::r
double r
Definition: colormap.h:14
armarx::DenseCRFSegmentationProcessor::process
void process() override
Definition: DenseCRFSegmentationProcessor.cpp:139
armarx::DenseGraphCRF::addCombinedGaussianFeature
void addCombinedGaussianFeature(std::vector< float > sigma, bool use_rgb, bool use_norm, bool use_xyz, bool use_curv, bool use_time, LabelCompatibility *function, KernelType kernel_type=DIAG_KERNEL, NormalizationType normalization_type=NORMALIZE_SYMMETRIC)
Definition: DenseGraphCRF.hpp:68
PointXYZRGBLNormal.h
armarx::WriteBufferedTripleBuffer::getUpToDateReadBuffer
const T & getUpToDateReadBuffer() const
Definition: TripleBuffer.h:346
armarx::DenseCRFSegmentationProcessor::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: DenseCRFSegmentationProcessor.cpp:55
armarx::GraphWithTimestamp
boost::subgraph< CloudGraphWithTimestamp > GraphWithTimestamp
Definition: Common.h:59
DenseCRFSegmentationProcessor.h
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
armarx::GraphWithTimestampPtr
boost::shared_ptr< GraphWithTimestamp > GraphWithTimestampPtr
Definition: Common.h:64
armarx::DenseGraphCRF::map_and_confidence
Eigen::VectorXf map_and_confidence(int n_iterations)
Definition: DenseGraphCRF.hpp:141
colormap.h