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