PrimitiveExtractionWidgetController.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 VisionX::gui-plugins::PrimitiveExtractionWidgetController
17  * \author Peter Kaiser ( peter dot kaiser at kit dot edu )
18  * \date 2016
19  * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
24 
27 
28 #include <VisionX/interface/core/PointCloudProviderInterface.h>
30 #include <AffordanceKit/primitives/Plane.h>
31 
32 #include <string>
33 
34 #include <QDoubleSpinBox>
35 #include <QCheckBox>
36 #include <QComboBox>
37 
38 #include <pcl/io/pcd_io.h>
39 #include <pcl/point_types.h>
40 #include <pcl/common/transforms.h>
41 
42 #include <memory>
43 
44 #include <filesystem>
45 
46 #define PI 3.141592654
47 
48 namespace armarx
49 {
51  {
52  widget.setupUi(getWidget());
53  }
54 
55 
57  {
58 
59  }
60 
61 
63  {
64  pointCloudProviderName = settings->value("pointCloudProviderName", "").toString().toStdString();
65  pointCloudSegmenterName = settings->value("pointCloudSegmenterName", "").toString().toStdString();
66  primitiveExtractorName = settings->value("primitiveExtractorName", "").toString().toStdString();
67  pipelineVisualizationName = settings->value("pipelineVisualizationName", "").toString().toStdString();
68  workingMemoryName = settings->value("workingMemoryName", "").toString().toStdString();
69  }
70 
72  {
73  settings->setValue("pointCloudProviderName", QString::fromStdString(pointCloudProviderName));
74  settings->setValue("pointCloudSegmenterName", QString::fromStdString(pointCloudSegmenterName));
75  settings->setValue("primitiveExtractorName", QString::fromStdString(primitiveExtractorName));
76  settings->setValue("pipelineVisualizationName", QString::fromStdString(pipelineVisualizationName));
77  settings->setValue("workingMemoryName", QString::fromStdString(workingMemoryName));
78  }
79 
80  QPointer<QDialog> PrimitiveExtractionWidgetController::getConfigDialog(QWidget* parent)
81  {
82  if (!configDialog)
83  {
84  configDialog = new PrimitiveExtractionConfigDialog(parent);
85  }
86 
87  return qobject_cast<PrimitiveExtractionConfigDialog*>(configDialog);
88  }
89 
91  {
92  pointCloudProviderName = configDialog->pointCloudProviderProxyFinder->getSelectedProxyName().toStdString();
93  pointCloudSegmenterName = configDialog->pointCloudSegmenterProxyFinder->getSelectedProxyName().toStdString();
94  primitiveExtractorName = configDialog->primitiveExtractorProxyFinder->getSelectedProxyName().toStdString();
95  pipelineVisualizationName = configDialog->pipelineVisualizationProxyFinder->getSelectedProxyName().toStdString();
96  workingMemoryName = configDialog->pipelineVisualizationProxyFinder->getSelectedProxyName().toStdString();
97  }
98 
100  {
101 
102  }
103 
105  {
106  pointCloudProvider = getProxy<visionx::PointCloudProviderInterfacePrx>(pointCloudProviderName);
107  if (!pointCloudProvider)
108  {
109  ARMARX_ERROR << "Could not obtain point cloud provider proxy";
110  return;
111  }
112 
113  visionx::FakePointCloudProviderInterfacePrx _fakePointCloudProvider = visionx::FakePointCloudProviderInterfacePrx::checkedCast(pointCloudProvider);
114  if (_fakePointCloudProvider)
115  {
116  widget.pushButtonLoadPointCloud->setEnabled(true);
117  }
118  else
119  {
120  widget.pushButtonLoadPointCloud->setEnabled(false);
121  }
122 
123  // The segmenter is optional (it does not exist when processing ground truth segmentations)
124  if (pointCloudSegmenterName != "")
125  {
126  try
127  {
128  pointCloudSegmenter = getProxy<visionx::PointCloudSegmenterInterfacePrx>(pointCloudSegmenterName);
129  }
130  catch (const armarx::UserException& e)
131  {
132  ARMARX_INFO << "No segmenter found, assuming ground truth segmentation";
133  }
134  }
135 
136  primitiveExtractor = getProxy<visionx::PrimitiveMapperInterfacePrx>(primitiveExtractorName);
137  if (!primitiveExtractor)
138  {
139  ARMARX_ERROR << "Could not obtain primitive extractor proxy";
140  return;
141  }
142 
143  pipelineVisualization = getProxy<AffordancePipelineVisualizationInterfacePrx>(pipelineVisualizationName);
144  if (!pipelineVisualization)
145  {
146  ARMARX_ERROR << "Could not obtain affordance pipeline visualization proxy";
147  return;
148  }
149 
150  workingMemory = getProxy<memoryx::WorkingMemoryInterfacePrx>(workingMemoryName);
151  if (!workingMemory)
152  {
153  ARMARX_ERROR << "Could not obtain point working memory proxy";
154  return;
155  }
156 
157  debugDrawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
158  if (!debugDrawer)
159  {
160  ARMARX_ERROR << "Failed to obtain debug drawer proxy";
161  return;
162  }
163 
164  connect(widget.pushButtonLoadPointCloud, SIGNAL(clicked()), this, SLOT(loadPointCloud()));
165  connect(widget.pushButtonRun, SIGNAL(clicked()), this, SLOT(run()));
166  connect(widget.pushButtonSetParam, SIGNAL(clicked()), this, SLOT(setParameters()));
167  connect(widget.pushButtonApplyTransformation, SIGNAL(clicked()), this, SLOT(applyPointCloudTransformation()));
168  connect(widget.pushButtonApplyAutoRotate, SIGNAL(clicked()), this, SLOT(applyAutoRotation()));
169  connect(widget.checkBoxShowPrimitives, SIGNAL(toggled(bool)), this, SLOT(visualizationOptionsChanged()));
170  connect(widget.checkBoxShowRootPose, SIGNAL(toggled(bool)), this, SLOT(visualizationOptionsChanged()));
171  connect(widget.pushButtonImportConfig, SIGNAL(clicked()), this, SLOT(importConfig()));
172  connect(widget.pushButtonExportConfig, SIGNAL(clicked()), this, SLOT(exportConfig()));
173 
174  if (pointCloudSegmenter)
175  {
176  visionx::LccpParameters lccpParameters = pointCloudSegmenter->getLccpParameters();
177  widget.doubleSpinBoxLccpMinSegmentSize->setValue(lccpParameters.minSegmentSize);
178  widget.doubleSpinBoxLccpVoxelResolution->setValue(lccpParameters.voxelResolution);
179  widget.doubleSpinBoxLccpSeedResolution->setValue(lccpParameters.seedResolution);
180  widget.doubleSpinBoxLccpColorImportance->setValue(lccpParameters.colorImportance);
181  widget.doubleSpinBoxLccpSpatialImportance->setValue(lccpParameters.spatialImportance);
182  widget.doubleSpinBoxLccpNormalImportance->setValue(lccpParameters.normalImportance);
183  widget.doubleSpinBoxLccpConcavityThreshold->setValue(lccpParameters.concavityThreshold);
184  widget.doubleSpinBoxLccpSmoothnessThreshold->setValue(lccpParameters.smoothnessThreshold);
185  }
186  else
187  {
188  widget.doubleSpinBoxLccpMinSegmentSize->setEnabled(false);
189  widget.doubleSpinBoxLccpVoxelResolution->setEnabled(false);
190  widget.doubleSpinBoxLccpSeedResolution->setEnabled(false);
191  widget.doubleSpinBoxLccpColorImportance->setEnabled(false);
192  widget.doubleSpinBoxLccpSpatialImportance->setEnabled(false);
193  widget.doubleSpinBoxLccpNormalImportance->setEnabled(false);
194  widget.doubleSpinBoxLccpConcavityThreshold->setEnabled(false);
195  widget.doubleSpinBoxLccpSmoothnessThreshold->setEnabled(false);
196  }
197 
198  visionx::PrimitiveExtractorParameters primtiveParameters = primitiveExtractor->getParameters();
199  ARMARX_LOG << "parameters " << primtiveParameters.minSegmentSize;
200  widget.doubleSpinBoxMinPrimitiveSize->setValue(primtiveParameters.minSegmentSize);
201  widget.doubleSpinBoxMaxPrimitiveSize->setValue(primtiveParameters.maxSegmentSize);
202  widget.doubleSpinBoxEuclideanClusteringTolerance->setValue(primtiveParameters.euclideanClusteringTolerance);
203  widget.doubleSpinBoxOutlierDistanceThreshold->setValue(primtiveParameters.outlierThreshold);
204 
205  widget.doubleSpinBoxPlaneMaxIterations->setValue(primtiveParameters.planeMaxIterations);
206  widget.doubleSpinBoxPlaneDistanceThreshold->setValue(primtiveParameters.planeDistanceThreshold);
207  widget.doubleSpinBoxPlaneNormalDistance->setValue(primtiveParameters.planeNormalDistance);
208  widget.doubleSpinBoxPlaneCircularDistanceThreshold->setValue(primtiveParameters.circularDistanceThreshold);
209 
210  widget.doubleSpinBoxCylinderMaxIterations->setValue(primtiveParameters.cylinderMaxIterations);
211  widget.doubleSpinBoxCylinderDistanceThreshold->setValue(primtiveParameters.cylinderDistanceThreshold);
212  widget.doubleSpinBoxCylinderRadiusLimit->setValue(primtiveParameters.cylinderRadiusLimit);
213 
214  widget.doubleSpinBoxSphereMaxIterations->setValue(primtiveParameters.sphereMaxIterations);
215  widget.doubleSpinBoxSphereDistanceThreshold->setValue(primtiveParameters.sphereDistanceThreshold);
216  widget.doubleSpinBoxSphereNormalDistance->setValue(primtiveParameters.sphereNormalDistance);
217  }
218 
220  {
221  QObject::disconnect(this, SLOT(loadPointCloud()));
222  QObject::disconnect(this, SLOT(run()));
223  QObject::disconnect(this, SLOT(setParameters()));
224  QObject::disconnect(this, SLOT(visualizationOptionsChanged()));
225  QObject::disconnect(this, SLOT(importConfig()));
226  QObject::disconnect(this, SLOT(exportConfig()));
227  }
228 
230  {
231  QString filename = QFileDialog::getOpenFileName(NULL, "Open Point Cloud", "", "Point Cloud Files (*.pcd)");
232  if (filename != "")
233  {
234  widget.lineEditPointCloud->setText(filename);
235  }
236  }
237 
239  {
240  visionx::FakePointCloudProviderInterfacePrx _fakePointCloudProvider = visionx::FakePointCloudProviderInterfacePrx::checkedCast(pointCloudProvider);
241  if (_fakePointCloudProvider)
242  {
243  std::string path = widget.lineEditPointCloud->text().toStdString();
244  _fakePointCloudProvider->setPointCloudFilename(path);
245  }
246 
247  if (pointCloudSegmenter)
248  {
249  visionx::LccpParameters segmenterPrm;
250  segmenterPrm.minSegmentSize = widget.doubleSpinBoxLccpMinSegmentSize->value();
251  segmenterPrm.voxelResolution = widget.doubleSpinBoxLccpVoxelResolution->value();
252  segmenterPrm.seedResolution = widget.doubleSpinBoxLccpSeedResolution->value();
253  segmenterPrm.colorImportance = widget.doubleSpinBoxLccpColorImportance->value();
254  segmenterPrm.spatialImportance = widget.doubleSpinBoxLccpSpatialImportance->value();
255  segmenterPrm.normalImportance = widget.doubleSpinBoxLccpNormalImportance->value();
256  segmenterPrm.concavityThreshold = widget.doubleSpinBoxLccpConcavityThreshold->value();
257  segmenterPrm.smoothnessThreshold = widget.doubleSpinBoxLccpSmoothnessThreshold->value();
258  pointCloudSegmenter->setLccpParameters(segmenterPrm);
259  }
260 
261  visionx::PrimitiveExtractorParameters primitiveExtractionPrm;
262  primitiveExtractionPrm.minSegmentSize = widget.doubleSpinBoxMinPrimitiveSize->value();
263  primitiveExtractionPrm.maxSegmentSize = widget.doubleSpinBoxMaxPrimitiveSize->value();
264  primitiveExtractionPrm.planeMaxIterations = widget.doubleSpinBoxPlaneMaxIterations->value();
265  primitiveExtractionPrm.planeDistanceThreshold = widget.doubleSpinBoxPlaneDistanceThreshold->value();
266  primitiveExtractionPrm.planeNormalDistance = widget.doubleSpinBoxPlaneNormalDistance->value();
267  primitiveExtractionPrm.cylinderMaxIterations = widget.doubleSpinBoxCylinderMaxIterations->value();
268  primitiveExtractionPrm.cylinderDistanceThreshold = widget.doubleSpinBoxCylinderDistanceThreshold->value();
269  primitiveExtractionPrm.cylinderRadiusLimit = widget.doubleSpinBoxCylinderRadiusLimit->value();
270  primitiveExtractionPrm.sphereMaxIterations = widget.doubleSpinBoxSphereMaxIterations->value();
271  primitiveExtractionPrm.sphereDistanceThreshold = widget.doubleSpinBoxSphereDistanceThreshold->value();
272  primitiveExtractionPrm.sphereNormalDistance = widget.doubleSpinBoxSphereNormalDistance->value();
273  primitiveExtractionPrm.euclideanClusteringTolerance = widget.doubleSpinBoxEuclideanClusteringTolerance->value();
274  primitiveExtractionPrm.outlierThreshold = widget.doubleSpinBoxOutlierDistanceThreshold->value();
275  primitiveExtractionPrm.circularDistanceThreshold = widget.doubleSpinBoxPlaneCircularDistanceThreshold->value();
276  primitiveExtractor->setParameters(primitiveExtractionPrm);
277  }
278 
280  {
281  setParameters();
282 
283  visionx::CapturingPointCloudProviderInterfacePrx cx = visionx::CapturingPointCloudProviderInterfacePrx::checkedCast(pointCloudProvider);
284  if (cx)
285  {
286  cx->begin_startCaptureForNumFrames(1);
287  }
288 
289  }
290 
292  {
293  pipelineVisualization->begin_enableVisualization(widget.checkBoxShowPrimitives->isChecked(), false, false);
294 
295  if (widget.checkBoxShowRootPose->isChecked())
296  {
297  debugDrawer->setPoseVisu("PrimitiveExtractorGuiLayer", "RootPose", new Pose(Eigen::Matrix4f::Identity()));
298  }
299  else
300  {
301  debugDrawer->removePoseVisu("PrimitiveExtractorGuiLayer", "RootPose");
302  }
303  }
304 
306  {
307  std::string filename = widget.lineEditPointCloud->text().toStdString();
308 
309  if (!boost::algorithm::ends_with(filename, ".pcd") || !std::filesystem::is_regular_file(filename))
310  {
311  ARMARX_WARNING << "Unable to load point cloud " << filename;
312  return;
313  }
314 
315  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pc(new pcl::PointCloud<pcl::PointXYZRGBA>());
316  if (pcl::io::loadPCDFile<pcl::PointXYZRGBA>(filename.c_str(), *pc) == -1)
317  {
318  ARMARX_WARNING << "Unable to load point cloud " << filename;
319  return;
320  }
321 
322  ARMARX_INFO << "Transforming point cloud: " << filename;
323  Eigen::Affine3f scaling(Eigen::Scaling((float)widget.doubleSpinBoxScaling->value()));
324  pcl::transformPointCloud(*pc, *pc, scaling);
325 
326  std::string out_filename = QFileDialog::getSaveFileName(NULL, "Save transformed PCD", filename.c_str(), "Point Cloud Files (*.pcd)").toStdString();
327  if (filename != "")
328  {
329  pcl::io::savePCDFileASCII(out_filename, *pc);
330  ARMARX_INFO << "Point cloud saved";
331  }
332  }
333 
335  {
336  QString filename = QFileDialog::getOpenFileName(NULL, "Open Pipeline Configuration file", "", "Config file (*.cfg)");
337  if (filename == "")
338  {
339  return;
340  }
341 
342  QSettings config(filename, QSettings::IniFormat);
343 
344  config.beginGroup("LCCP");
345  widget.doubleSpinBoxLccpMinSegmentSize->setValue(config.value("MinSegmentSize", widget.doubleSpinBoxLccpMinSegmentSize->value()).toDouble());
346  widget.doubleSpinBoxLccpVoxelResolution->setValue(config.value("VoxelResolution", widget.doubleSpinBoxLccpVoxelResolution->value()).toDouble());
347  widget.doubleSpinBoxLccpSeedResolution->setValue(config.value("SeedResolution", widget.doubleSpinBoxLccpSeedResolution->value()).toDouble());
348  widget.doubleSpinBoxLccpColorImportance->setValue(config.value("ColorImportance", widget.doubleSpinBoxLccpColorImportance->value()).toDouble());
349  widget.doubleSpinBoxLccpSpatialImportance->setValue(config.value("SpatialImportance", widget.doubleSpinBoxLccpSpatialImportance->value()).toDouble());
350  widget.doubleSpinBoxLccpNormalImportance->setValue(config.value("NormalImportance", widget.doubleSpinBoxLccpNormalImportance->value()).toDouble());
351  widget.doubleSpinBoxLccpConcavityThreshold->setValue(config.value("ConcavityThreshold", widget.doubleSpinBoxLccpConcavityThreshold->value()).toDouble());
352  widget.doubleSpinBoxLccpSmoothnessThreshold->setValue(config.value("SmoothnessThreshold", widget.doubleSpinBoxLccpSmoothnessThreshold->value()).toDouble());
353  config.endGroup();
354 
355  config.beginGroup("PrimitiveExtraction");
356  widget.doubleSpinBoxMinPrimitiveSize->setValue(config.value("MinPrimitiveSize", widget.doubleSpinBoxMinPrimitiveSize->value()).toDouble());
357  widget.doubleSpinBoxMaxPrimitiveSize->setValue(config.value("MaxPrimitiveSize", widget.doubleSpinBoxMaxPrimitiveSize->value()).toDouble());
358  widget.doubleSpinBoxPlaneMaxIterations->setValue(config.value("PlaneMaxIterations", widget.doubleSpinBoxPlaneMaxIterations->value()).toDouble());
359  widget.doubleSpinBoxPlaneDistanceThreshold->setValue(config.value("PlaneDistanceThreshold", widget.doubleSpinBoxPlaneDistanceThreshold->value()).toDouble());
360  widget.doubleSpinBoxPlaneNormalDistance->setValue(config.value("PlaneNormalDistance", widget.doubleSpinBoxPlaneNormalDistance->value()).toDouble());
361  widget.doubleSpinBoxPlaneCircularDistanceThreshold->setValue(config.value("PlaneCircularDistanceThreshold", widget.doubleSpinBoxPlaneCircularDistanceThreshold->value()).toDouble());
362  widget.doubleSpinBoxCylinderMaxIterations->setValue(config.value("CylinderMaxIterations", widget.doubleSpinBoxCylinderMaxIterations->value()).toDouble());
363  widget.doubleSpinBoxCylinderDistanceThreshold->setValue(config.value("CylinderDistanceThreshold", widget.doubleSpinBoxCylinderDistanceThreshold->value()).toDouble());
364  widget.doubleSpinBoxCylinderRadiusLimit->setValue(config.value("CylinderRadiusLimit", widget.doubleSpinBoxSphereMaxIterations->value()).toDouble());
365  widget.doubleSpinBoxSphereMaxIterations->setValue(config.value("SphereMaxIterations", widget.doubleSpinBoxSphereMaxIterations->value()).toDouble());
366  widget.doubleSpinBoxSphereDistanceThreshold->setValue(config.value("SphereDistanceThreshold", widget.doubleSpinBoxSphereDistanceThreshold->value()).toDouble());
367  widget.doubleSpinBoxSphereNormalDistance->setValue(config.value("SphereNormalDistance", widget.doubleSpinBoxSphereNormalDistance->value()).toDouble());
368  widget.doubleSpinBoxEuclideanClusteringTolerance->setValue(config.value("EuclideanClusteringTolerance", widget.doubleSpinBoxEuclideanClusteringTolerance->value()).toDouble());
369  widget.doubleSpinBoxOutlierDistanceThreshold->setValue(config.value("OutlierDistanceThreshold", widget.doubleSpinBoxOutlierDistanceThreshold->value()).toDouble());
370  config.endGroup();
371 
372  ARMARX_INFO << "Current settings read from '" << filename << "'";
373  }
374 
376  {
377  QString filename = QFileDialog::getSaveFileName(NULL, "Save Pipeline Configuration file", "", "Config file (*.cfg)");
378  if (filename == "")
379  {
380  return;
381  }
382 
383  QSettings config(filename, QSettings::IniFormat);
384 
385  config.beginGroup("LCCP");
386  config.setValue("MinSegmentSize", widget.doubleSpinBoxLccpMinSegmentSize->value());
387  config.setValue("VoxelResolution", widget.doubleSpinBoxLccpVoxelResolution->value());
388  config.setValue("SeedResolution", widget.doubleSpinBoxLccpSeedResolution->value());
389  config.setValue("ColorImportance", widget.doubleSpinBoxLccpColorImportance->value());
390  config.setValue("SpatialImportance", widget.doubleSpinBoxLccpSpatialImportance->value());
391  config.setValue("NormalImportance", widget.doubleSpinBoxLccpNormalImportance->value());
392  config.setValue("ConcavityThreshold", widget.doubleSpinBoxLccpConcavityThreshold->value());
393  config.setValue("SmoothnessThreshold", widget.doubleSpinBoxLccpSmoothnessThreshold->value());
394  config.endGroup();
395 
396  config.beginGroup("PrimitiveExtraction");
397  config.setValue("MinPrimitiveSize", widget.doubleSpinBoxMinPrimitiveSize->value());
398  config.setValue("MaxPrimitiveSize", widget.doubleSpinBoxMaxPrimitiveSize->value());
399  config.setValue("PlaneMaxIterations", widget.doubleSpinBoxPlaneMaxIterations->value());
400  config.setValue("PlaneDistanceThreshold", widget.doubleSpinBoxPlaneDistanceThreshold->value());
401  config.setValue("PlaneNormalDistance", widget.doubleSpinBoxPlaneNormalDistance->value());
402  config.setValue("PlaneCircularDistanceThreshold", widget.doubleSpinBoxPlaneCircularDistanceThreshold->value());
403  config.setValue("CylinderMaxIterations", widget.doubleSpinBoxCylinderMaxIterations->value());
404  config.setValue("CylinderDistanceThreshold", widget.doubleSpinBoxCylinderDistanceThreshold->value());
405  config.setValue("CylinderRadiusLimit", widget.doubleSpinBoxSphereMaxIterations->value());
406  config.setValue("SphereMaxIterations", widget.doubleSpinBoxSphereMaxIterations->value());
407  config.setValue("SphereDistanceThreshold", widget.doubleSpinBoxSphereDistanceThreshold->value());
408  config.setValue("SphereNormalDistance", widget.doubleSpinBoxSphereNormalDistance->value());
409  config.setValue("EuclideanClusteringTolerance", widget.doubleSpinBoxEuclideanClusteringTolerance->value());
410  config.setValue("OutlierDistanceThreshold", widget.doubleSpinBoxOutlierDistanceThreshold->value());
411  config.endGroup();
412 
413  config.sync();
414 
415  ARMARX_INFO << "Current settings saved to '" << filename << "'";
416  }
417 
418  // Apply a rotation to the pointCloud corresponding to the inverse of the rotational component of the biggest plane primitive.
419  // The should result in a point cloud in which the ground is fitted exactly into the x/y-plane.
421  {
423  primitives.reset(new AffordanceKitArmarX::PrimitiveSetArmarX(workingMemory->getEnvironmentalPrimitiveSegment()));
424 
425  bool found = false;
426  unsigned int indexOfBiggestPrimitive;
427  unsigned int maxSampleSize = 0;
428 
429  using boost::dynamic_pointer_cast;
430  using std::dynamic_pointer_cast;
431 
432  // Find biggest plane primitive.
433  for (unsigned int i = 0; i < primitives->size(); i++)
434  {
435  AffordanceKit::PrimitivePtr primitive = primitives->at(i);
436  if (!dynamic_pointer_cast<AffordanceKit::Plane>(primitive))
437  {
438  continue;
439  }
440  AffordanceKit::PlanePtr plane = dynamic_pointer_cast<AffordanceKit::Plane>(primitive);
441  plane->sample(20, 1);
442  unsigned int curPlaneSize = plane->getSamplingSize();
443  if (curPlaneSize > maxSampleSize)
444  {
445  found = true;
446  maxSampleSize = curPlaneSize;
447  indexOfBiggestPrimitive = i;
448  }
449  }
450 
451  if (!found)
452  {
453  ARMARX_WARNING << "Did not find any plane-shaped primitives to rotate upon!";
454  return;
455  }
456 
457  // If found: get rotation of biggest plane primitive.
458  AffordanceKit::PrimitivePtr primitive = primitives->at(indexOfBiggestPrimitive);
459  if (!dynamic_pointer_cast<AffordanceKit::Plane>(primitive))
460  {
461  ARMARX_ERROR << "Could not convert to planar primitive";
462  return;
463  }
464  AffordanceKit::PlanePtr plane = dynamic_pointer_cast<AffordanceKit::Plane>(primitive);
465  Eigen::Matrix4f gp = plane->getPose();
466 
467  debugDrawer->setPoseVisu("PrimitiveExtractorGuiLayer", "BiggestPlaneRotation", new Pose(gp));
468 
469  Eigen::Matrix4f gpi = gp.inverse();
470 
471  // Rotate the point cloud around the inverse of the ground's rotation matrix.
472  std::string filename = widget.lineEditPointCloud->text().toStdString();
473  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pc(new pcl::PointCloud<pcl::PointXYZRGBA>());
474  if (pcl::io::loadPCDFile<pcl::PointXYZRGBA>(filename.c_str(), *pc) == -1)
475  {
476  ARMARX_WARNING << "Unable to load point cloud " << filename << " for auto-rotation.";
477  return;
478  }
479 
480  // Apply transformation
481  pcl::transformPointCloud(*pc, *pc, gpi);
482 
483  // Use a simple heuristic to check if z points in the right direction (upwards)
484  int balance = 0;
485  for (unsigned int i = 0; i < pc->points.size(); i++)
486  {
487  balance += (pc->points[i].z >= 0) ? 1 : -1;
488  }
489 
490  ARMARX_INFO << "Z-axis balance after transformation: " << balance;
491  if (balance < 0)
492  {
493  // z seems to point in the wrong direction (most point have negative z-value)
494  Eigen::Affine3f rotation(Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitX()));
495  pcl::transformPointCloud(*pc, *pc, rotation);
496 
497  ARMARX_INFO << "Flipping Z axis";
498 
499  // Update visualization
500  Eigen::Matrix4f T = rotation.matrix();
501  debugDrawer->setPoseVisu("PrimitiveExtractorGuiLayer", "BiggestPlaneRotation", new Pose(T * gp));
502  }
503 
504  std::string out_filename = QFileDialog::getSaveFileName(NULL, "Save transformed PCD", filename.c_str(), "Point Cloud Files (*.pcd)").toStdString();
505  if (filename != "")
506  {
507  pcl::io::savePCDFileASCII(out_filename, *pc);
508  ARMARX_INFO << "Point cloud saved";
509  }
510  }
511 }
RemoteRobot.h
armarx::PrimitiveExtractionWidgetController::onConnectComponent
void onConnectComponent() override
Definition: PrimitiveExtractionWidgetController.cpp:104
armarx::PrimitiveExtractionWidgetController::getConfigDialog
QPointer< QDialog > getConfigDialog(QWidget *parent=0) override
getConfigDialog returns a pointer to the a configuration widget of this controller.
Definition: PrimitiveExtractionWidgetController.cpp:80
armarx::PrimitiveExtractionWidgetController::visualizationOptionsChanged
void visualizationOptionsChanged()
Definition: PrimitiveExtractionWidgetController.cpp:291
armarx::PrimitiveExtractionConfigDialog
Definition: PrimitiveExtractionConfigDialog.h:40
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
PrimitiveExtractionWidgetController.h
armarx::PrimitiveExtractionWidgetController::configured
void configured() override
This function must be implemented by the user, if he supplies a config dialog.
Definition: PrimitiveExtractionWidgetController.cpp:90
armarx::PrimitiveExtractionWidgetController::setParameters
void setParameters()
Definition: PrimitiveExtractionWidgetController.cpp:238
armarx::PrimitiveExtractionWidgetController::run
void run()
Definition: PrimitiveExtractionWidgetController.cpp:279
armarx::PrimitiveExtractionWidgetController::onInitComponent
void onInitComponent() override
Definition: PrimitiveExtractionWidgetController.cpp:99
armarx::PrimitiveExtractionWidgetController::applyAutoRotation
void applyAutoRotation()
Definition: PrimitiveExtractionWidgetController.cpp:420
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
armarx::PrimitiveExtractionWidgetController::importConfig
void importConfig()
Definition: PrimitiveExtractionWidgetController.cpp:334
M_PI
#define M_PI
Definition: MathTools.h:17
FramedPose.h
AffordanceKitArmarX::PrimitiveSetArmarX
Definition: PrimitiveSetArmarX.h:32
filename
std::string filename
Definition: VisualizationRobot.cpp:83
armarx::PrimitiveExtractionWidgetController::PrimitiveExtractionWidgetController
PrimitiveExtractionWidgetController()
Controller Constructor.
Definition: PrimitiveExtractionWidgetController.cpp:50
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
ARMARX_LOG
#define ARMARX_LOG
Definition: Logging.h:163
armarx::PrimitiveExtractionWidgetController::saveSettings
void saveSettings(QSettings *settings) override
Definition: PrimitiveExtractionWidgetController.cpp:71
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::PrimitiveExtractionWidgetController::loadPointCloud
void loadPointCloud()
Definition: PrimitiveExtractionWidgetController.cpp:229
armarx::PrimitiveExtractionWidgetController::exportConfig
void exportConfig()
Definition: PrimitiveExtractionWidgetController.cpp:375
armarx::ends_with
bool ends_with(const std::string &haystack, const std::string &needle)
Definition: StringHelpers.cpp:50
armarx::ArmarXWidgetController::getWidget
virtual QPointer< QWidget > getWidget()
getWidget returns a pointer to the a widget of this controller.
Definition: ArmarXWidgetController.cpp:54
pc
Introduction Thank you for taking interest in our work and downloading this software This library implements the algorithm described in the paper R R R Klein Efficient RANSAC for Point Cloud Shape in Computer Graphics Blackwell June If you use this software you should cite the aforementioned paper in any resulting publication Please send comments or bug reports to Ruwen Roland BUT NOT LIMITED THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY OR CONSEQUENTIAL WHETHER IN STRICT OR EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE Example usage This section shows how to use the library to detect the shapes in a point cloud PointCloud pc
Definition: ReadMe.txt:68
armarx::PrimitiveExtractionWidgetController::onDisconnectComponent
void onDisconnectComponent() override
Hook for subclass.
Definition: PrimitiveExtractionWidgetController.cpp:219
PrimitiveSetArmarX.h
T
float T
Definition: UnscentedKalmanFilterTest.cpp:35
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::PrimitiveExtractionWidgetController::~PrimitiveExtractionWidgetController
virtual ~PrimitiveExtractionWidgetController()
Controller destructor.
Definition: PrimitiveExtractionWidgetController.cpp:56
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::PrimitiveExtractionWidgetController::loadSettings
void loadSettings(QSettings *settings) override
Definition: PrimitiveExtractionWidgetController.cpp:62
armarx::PrimitiveExtractionWidgetController::applyPointCloudTransformation
void applyPointCloudTransformation()
Definition: PrimitiveExtractionWidgetController.cpp:305
AffordanceKitArmarX::PrimitiveSetArmarXPtr
std::shared_ptr< PrimitiveSetArmarX > PrimitiveSetArmarXPtr
Definition: PrimitiveSetArmarX.h:42