DepthCalibrationDialog.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <rtabmap/gui/DepthCalibrationDialog.h>
29 #include "ui_depthCalibrationDialog.h"
30 
31 #include "rtabmap/gui/ProgressDialog.h"
32 #include "rtabmap/gui/CloudViewer.h"
33 #include "rtabmap/gui/ImageView.h"
34 #include "rtabmap/utilite/ULogger.h"
35 #include "rtabmap/utilite/UThread.h"
36 #include "rtabmap/utilite/UCv2Qt.h"
37 #include "rtabmap/core/util3d.h"
38 #include "rtabmap/core/util3d_filtering.h"
39 #include "rtabmap/core/util3d_transforms.h"
40 
41 #include "rtabmap/core/clams/slam_calibrator.h"
42 #include "rtabmap/core/clams/frame_projector.h"
43 
44 #include <QPushButton>
45 #include <QDir>
46 #include <QFileInfo>
47 #include <QUrl>
48 #include <QtGui/QDesktopServices>
49 #include <QMessageBox>
50 #include <QFileDialog>
51 
52 namespace rtabmap {
53 
54 DepthCalibrationDialog::DepthCalibrationDialog(QWidget *parent) :
55  QDialog(parent),
56  _canceled(false),
57  _model(0)
58 {
59  _ui = new Ui_DepthCalibrationDialog();
60  _ui->setupUi(this);
61 
62  connect(_ui->buttonBox->button(QDialogButtonBox::RestoreDefaults), SIGNAL(clicked()), this, SLOT(restoreDefaults()));
63  connect(_ui->buttonBox->button(QDialogButtonBox::Save), SIGNAL(clicked()), this, SLOT(saveModel()));
64  connect(_ui->buttonBox->button(QDialogButtonBox::Ok), SIGNAL(clicked()), this, SLOT(accept()));
65  _ui->buttonBox->button(QDialogButtonBox::Ok)->setText("Calibrate");
66 
67  restoreDefaults();
68 
69  connect(_ui->spinBox_decimation, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
70  connect(_ui->doubleSpinBox_maxDepth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
71  connect(_ui->doubleSpinBox_minDepth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
72  connect(_ui->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
73  connect(_ui->doubleSpinBox_coneRadius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
74  connect(_ui->doubleSpinBox_coneStdDevThresh, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
75  connect(_ui->checkBox_laserScan, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
76 
77  connect(_ui->spinBox_bin_width, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
78  connect(_ui->spinBox_bin_height, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
79  connect(_ui->doubleSpinBox_bin_depth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
80  connect(_ui->spinBox_smoothing, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
81  connect(_ui->doubleSpinBox_maxDepthModel, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
82 
83  _ui->buttonBox->button(QDialogButtonBox::Ok)->setFocus();
84 
85  _progressDialog = new ProgressDialog(this);
86  _progressDialog->setVisible(false);
87  _progressDialog->setAutoClose(true, 2);
88  _progressDialog->setMinimumWidth(600);
89  _progressDialog->setCancelButtonVisible(true);
90 
91  connect(_progressDialog, SIGNAL(canceled()), this, SLOT(cancel()));
92 }
93 
94 DepthCalibrationDialog::~DepthCalibrationDialog()
95 {
96  delete _ui;
97  delete _model;
98 }
99 
100 void DepthCalibrationDialog::saveSettings(QSettings & settings, const QString & group) const
101 {
102  if(!group.isEmpty())
103  {
104  settings.beginGroup(group);
105  }
106 
107  settings.setValue("decimation", _ui->spinBox_decimation->value());
108  settings.setValue("max_depth", _ui->doubleSpinBox_maxDepth->value());
109  settings.setValue("min_depth", _ui->doubleSpinBox_minDepth->value());
110  settings.setValue("voxel",_ui->doubleSpinBox_voxelSize->value());
111  settings.setValue("cone_radius",_ui->doubleSpinBox_coneRadius->value());
112  settings.setValue("cone_stddev_thresh",_ui->doubleSpinBox_coneStdDevThresh->value());
113  settings.setValue("laser_scan",_ui->checkBox_laserScan->isChecked());
114 
115  settings.setValue("bin_width",_ui->spinBox_bin_width->value());
116  settings.setValue("bin_height",_ui->spinBox_bin_height->value());
117  settings.setValue("bin_depth",_ui->doubleSpinBox_bin_depth->value());
118  settings.setValue("smoothing",_ui->spinBox_smoothing->value());
119  settings.setValue("max_model_depth",_ui->doubleSpinBox_maxDepthModel->value());
120 
121  if(!group.isEmpty())
122  {
123  settings.endGroup();
124  }
125 }
126 
127 void DepthCalibrationDialog::loadSettings(QSettings & settings, const QString & group)
128 {
129  if(!group.isEmpty())
130  {
131  settings.beginGroup(group);
132  }
133 
134  _ui->spinBox_decimation->setValue(settings.value("decimation", _ui->spinBox_decimation->value()).toInt());
135  _ui->doubleSpinBox_maxDepth->setValue(settings.value("max_depth", _ui->doubleSpinBox_maxDepth->value()).toDouble());
136  _ui->doubleSpinBox_minDepth->setValue(settings.value("min_depth", _ui->doubleSpinBox_minDepth->value()).toDouble());
137  _ui->doubleSpinBox_voxelSize->setValue(settings.value("voxel", _ui->doubleSpinBox_voxelSize->value()).toDouble());
138  _ui->doubleSpinBox_coneRadius->setValue(settings.value("cone_radius", _ui->doubleSpinBox_coneRadius->value()).toDouble());
139  _ui->doubleSpinBox_coneStdDevThresh->setValue(settings.value("cone_stddev_thresh", _ui->doubleSpinBox_coneStdDevThresh->value()).toDouble());
140  _ui->checkBox_laserScan->setChecked(settings.value("laser_scan", _ui->checkBox_laserScan->isChecked()).toBool());
141 
142  _ui->spinBox_bin_width->setValue(settings.value("bin_width", _ui->spinBox_bin_width->value()).toInt());
143  _ui->spinBox_bin_height->setValue(settings.value("bin_height", _ui->spinBox_bin_height->value()).toInt());
144  _ui->doubleSpinBox_bin_depth->setValue(settings.value("bin_depth", _ui->doubleSpinBox_bin_depth->value()).toDouble());
145  _ui->spinBox_smoothing->setValue(settings.value("smoothing", _ui->spinBox_smoothing->value()).toInt());
146  _ui->doubleSpinBox_maxDepthModel->setValue(settings.value("max_model_depth", _ui->doubleSpinBox_maxDepthModel->value()).toDouble());
147 
148  if(!group.isEmpty())
149  {
150  settings.endGroup();
151  }
152 }
153 
154 void DepthCalibrationDialog::restoreDefaults()
155 {
156  _ui->spinBox_decimation->setValue(1);
157  _ui->doubleSpinBox_maxDepth->setValue(3.5);
158  _ui->doubleSpinBox_minDepth->setValue(0);
159  _ui->doubleSpinBox_voxelSize->setValue(0.01);
160  _ui->doubleSpinBox_coneRadius->setValue(0.02);
161  _ui->doubleSpinBox_coneStdDevThresh->setValue(0.1); // 0.03
162  _ui->checkBox_laserScan->setChecked(false);
163  _ui->checkBox_resetModel->setChecked(true);
164 
165  _ui->spinBox_bin_width->setValue(8);
166  _ui->spinBox_bin_height->setValue(6);
167  if(_imageSize.width > 0 && _imageSize.height > 0)
168  {
169  size_t bin_width, bin_height;
170  clams::DiscreteDepthDistortionModel::getBinSize(_imageSize.width, _imageSize.height, bin_width, bin_height);
171  _ui->spinBox_bin_width->setValue(bin_width);
172  _ui->spinBox_bin_height->setValue(bin_height);
173  }
174  _ui->doubleSpinBox_bin_depth->setValue(2.0),
175  _ui->spinBox_smoothing->setValue(1);
176  _ui->doubleSpinBox_maxDepthModel->setValue(10.0);
177 }
178 
179 void DepthCalibrationDialog::saveModel()
180 {
181  if(_model && _model->getTrainingSamples())
182  {
183  QString path = QFileDialog::getSaveFileName(this, tr("Save distortion model to ..."), _workingDirectory+QDir::separator()+"distortion_model.bin", tr("Distortion model (*.bin *.txt)"));
184  if(!path.isEmpty())
185  {
186  //
187  // Save depth calibration
188  //
189  cv::Mat results = _model->visualize(ULogger::level() == ULogger::kDebug?_workingDirectory.toStdString():"");
190  _model->save(path.toStdString());
191 
192  if(!results.empty())
193  {
194  QString name = QString(path).replace(".bin", ".png", Qt::CaseInsensitive).replace(".txt", ".png", Qt::CaseInsensitive);
195  cv::imwrite(name.toStdString(), results);
196  QDesktopServices::openUrl(QUrl::fromLocalFile(name));
197  }
198 
199  QMessageBox::information(this, tr("Depth Calibration"), tr("Distortion model saved to \"%1\"!").arg(path));
200  }
201  }
202 }
203 
204 void DepthCalibrationDialog::cancel()
205 {
206  _canceled = true;
207  _progressDialog->appendText(tr("Canceled!"));
208 }
209 
210 void DepthCalibrationDialog::calibrate(
211  const std::map<int, Transform> & poses,
212  const QMap<int, Signature> & cachedSignatures,
213  const QString & workingDirectory,
214  const ParametersMap & parameters)
215 {
216  _canceled = false;
217  _workingDirectory = workingDirectory;
218  _ui->buttonBox->button(QDialogButtonBox::Save)->setEnabled(_model && _model->getTrainingSamples()>0);
219  if(_model)
220  {
221  _ui->label_trainingSamples->setNum((int)_model->getTrainingSamples());
222  }
223 
224  _ui->label_width->setText("NA");
225  _ui->label_height->setText("NA");
226  _imageSize = cv::Size();
227  CameraModel model;
228  if(cachedSignatures.size())
229  {
230  const Signature & s = cachedSignatures.begin().value();
231  const SensorData & data = s.sensorData();
232  cv::Mat depth;
233  data.uncompressDataConst(0, &depth);
234  if(data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForProjection() && !depth.empty())
235  {
236  // use depth image size
237  _imageSize = depth.size();
238  _ui->label_width->setNum(_imageSize.width);
239  _ui->label_height->setNum(_imageSize.height);
240 
241  if(_imageSize.width % _ui->spinBox_bin_width->value() != 0 ||
242  _imageSize.height % _ui->spinBox_bin_height->value() != 0)
243  {
244  size_t bin_width, bin_height;
245  clams::DiscreteDepthDistortionModel::getBinSize(_imageSize.width, _imageSize.height, bin_width, bin_height);
246  _ui->spinBox_bin_width->setValue(bin_width);
247  _ui->spinBox_bin_height->setValue(bin_height);
248  }
249  }
250  else if(data.cameraModels().size() > 1)
251  {
252  QMessageBox::warning(this, tr("Depth Calibration"),tr("Multi-camera not supported!"));
253  return;
254  }
255  else if(data.cameraModels().size() != 1)
256  {
257  QMessageBox::warning(this, tr("Depth Calibration"), tr("Camera model not found."));
258  return;
259  }
260  else if(data.cameraModels().size() == 1 && !data.cameraModels()[0].isValidForProjection())
261  {
262  QMessageBox::warning(this, tr("Depth Calibration"), tr("Camera model %1 not valid for projection.").arg(s.id()));
263  return;
264  }
265  else
266  {
267  QMessageBox::warning(this, tr("Depth Calibration"), tr("Depth image cannot be found in the cache, make sure to update cache before doing calibration."));
268  return;
269  }
270  }
271  else
272  {
273  QMessageBox::warning(this, tr("Depth Calibration"), tr("No signatures detected! Map is empty!?"));
274  return;
275  }
276 
277  if(this->exec() == QDialog::Accepted)
278  {
279  if(_model && _ui->checkBox_resetModel->isChecked())
280  {
281  delete _model;
282  _model = 0;
283  }
284 
285  if(_ui->doubleSpinBox_maxDepthModel->value() < _ui->doubleSpinBox_bin_depth->value())
286  {
287  QMessageBox::warning(this, tr("Wrong parameter"), tr("Maximum model depth should be higher than bin depth, setting to bin depth x5."));
288  _ui->doubleSpinBox_maxDepthModel->setValue(_ui->doubleSpinBox_bin_depth->value() * 5.0);
289  }
290 
291  _progressDialog->setMaximumSteps(poses.size()*2 + 3);
292  if(_ui->doubleSpinBox_voxelSize->value() > 0.0)
293  {
294  _progressDialog->setMaximumSteps(_progressDialog->maximumSteps()+1);
295  }
296  _progressDialog->resetProgress();
297  _progressDialog->show();
298 
299  std::map<int, rtabmap::SensorData> sequence;
300 
301  // Create the map
302  pcl::PointCloud<pcl::PointXYZ>::Ptr map(new pcl::PointCloud<pcl::PointXYZ>);
303  int index=1;
304  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end() && !_canceled; ++iter, ++index)
305  {
306  int points = 0;
307  if(!iter->second.isNull())
308  {
309  pcl::IndicesPtr indices(new std::vector<int>);
310  if(cachedSignatures.contains(iter->first))
311  {
312  const Signature & s = cachedSignatures.find(iter->first).value();
313  SensorData data = s.sensorData();
314 
315  cv::Mat depth;
316  LaserScan laserScan;
317  data.uncompressData(0, &depth, _ui->checkBox_laserScan->isChecked()?&laserScan:0);
318  if(data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForProjection() && !depth.empty())
319  {
320  UASSERT(iter->first == data.id());
321  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
322 
323  if(_ui->checkBox_laserScan->isChecked())
324  {
325  cloud = util3d::laserScanToPointCloud(laserScan);
326  indices->resize(cloud->size());
327  for(unsigned int i=0; i<indices->size(); ++i)
328  {
329  indices->at(i) = i;
330  }
331  }
332  else
333  {
334  cloud = util3d::cloudFromSensorData(
335  data,
336  _ui->spinBox_decimation->value(),
337  _ui->doubleSpinBox_maxDepth->value(),
338  _ui->doubleSpinBox_minDepth->value(),
339  indices.get(),
340  parameters);
341  }
342 
343  if(indices->size())
344  {
345  if(_ui->doubleSpinBox_voxelSize->value() > 0.0)
346  {
347  cloud = util3d::voxelize(cloud, indices, _ui->doubleSpinBox_voxelSize->value());
348  }
349 
350  cloud = util3d::transformPointCloud(cloud, iter->second);
351 
352  points+=cloud->size();
353 
354  *map += *cloud;
355 
356  sequence.insert(std::make_pair(iter->first, data));
357 
358  cv::Size size = depth.size();
359  if(_model &&
360  (_model->getWidth()!=size.width ||
361  _model->getHeight()!=size.height))
362  {
363  QString msg = tr("Depth images (%1x%2) in the map don't have the "
364  "same size then in the current model (%3x%4). You may want "
365  "to check \"Reset previous model\" before trying again.")
366  .arg(size.width).arg(size.height)
367  .arg(_model->getWidth()).arg(_model->getHeight());
368  QMessageBox::warning(this, tr("Depth Calibration"), msg);
369  _progressDialog->appendText(msg, Qt::darkRed);
370  _progressDialog->setAutoClose(false);
371  return;
372  }
373  }
374  }
375  else
376  {
377  _progressDialog->appendText(tr("Not suitable camera model found for node %1, ignoring this node!").arg(iter->first), Qt::darkYellow);
378  _progressDialog->setAutoClose(false);
379  }
380  }
381  else
382  {
383  UERROR("Cloud %d not found in cache!", iter->first);
384  }
385  }
386  else
387  {
388  UERROR("transform is null!?");
389  }
390 
391  if(points>0)
392  {
393  _progressDialog->appendText(tr("Generated cloud %1 with %2 points (%3/%4).")
394  .arg(iter->first).arg(points).arg(index).arg(poses.size()));
395  }
396  else
397  {
398  _progressDialog->appendText(tr("Ignored cloud %1 (%2/%3).").arg(iter->first).arg(index).arg(poses.size()));
399  }
400  _progressDialog->incrementStep();
401  QApplication::processEvents();
402  }
403 
404  if(!_canceled && map->size() && sequence.size())
405  {
406  if(_ui->doubleSpinBox_voxelSize->value() > 0.0)
407  {
408  _progressDialog->appendText(tr("Voxel filtering (%1 m) of the merged point cloud (%2 points)")
409  .arg(_ui->doubleSpinBox_voxelSize->value())
410  .arg(map->size()));
411  QApplication::processEvents();
412  QApplication::processEvents();
413 
414  map = util3d::voxelize(map, _ui->doubleSpinBox_voxelSize->value());
415  _progressDialog->incrementStep();
416  }
417 
418  if(ULogger::level() == ULogger::kDebug)
419  {
420  //
421  // Show 3D map with frustums
422  //
423  QDialog * window = new QDialog(this->parentWidget()?this->parentWidget():this, Qt::Window);
424  window->setAttribute(Qt::WA_DeleteOnClose, true);
425  window->setWindowTitle(tr("Map"));
426  window->setMinimumWidth(800);
427  window->setMinimumHeight(600);
428 
429  CloudViewer * viewer = new CloudViewer(window);
430  viewer->setCameraLockZ(false);
431  viewer->setFrustumShown(true);
432 
433  QVBoxLayout *layout = new QVBoxLayout();
434  layout->addWidget(viewer);
435  window->setLayout(layout);
436  connect(window, SIGNAL(finished(int)), viewer, SLOT(clear()));
437 
438  window->show();
439 
440  uSleep(500);
441 
442  _progressDialog->appendText(tr("Viewing the cloud (%1 points and %2 poses)...").arg(map->size()).arg(sequence.size()));
443  _progressDialog->incrementStep();
444  viewer->addCloud("map", map);
445  for(std::map<int, SensorData>::iterator iter=sequence.begin(); iter!=sequence.end(); ++iter)
446  {
447  Transform baseToCamera = iter->second.cameraModels()[0].localTransform();
448  viewer->addOrUpdateFrustum(uFormat("frustum%d",iter->first), poses.at(iter->first), baseToCamera, 0.2, QColor(), iter->second.cameraModels()[0].fovX(), iter->second.cameraModels()[0].fovY());
449  }
450  _progressDialog->appendText(tr("Viewing the cloud (%1 points and %2 poses)... done.").arg(map->size()).arg(sequence.size()));
451 
452  viewer->update();
453  }
454 
455  _progressDialog->appendText(tr("CLAMS depth calibration..."));
456  QApplication::processEvents();
457  QApplication::processEvents();
458 
459  QDialog * dialog = new QDialog(this->parentWidget()?this->parentWidget():this, Qt::Window);
460  dialog->setAttribute(Qt::WA_DeleteOnClose, true);
461  dialog->setWindowTitle(tr("Original/Map"));
462  dialog->setMinimumWidth(_imageSize.width);
463  ImageView * imageView1 = new ImageView(dialog);
464  imageView1->setMinimumSize(320, 240);
465  ImageView * imageView2 = new ImageView(dialog);
466  imageView2->setMinimumSize(320, 240);
467  QVBoxLayout * vlayout = new QVBoxLayout();
468  vlayout->setContentsMargins(0,0,0,0);
469  vlayout->addWidget(imageView1, 1);
470  vlayout->addWidget(imageView2, 1);
471  dialog->setLayout(vlayout);
472  if(ULogger::level() == ULogger::kDebug)
473  {
474  dialog->show();
475  }
476 
477  //clams::DiscreteDepthDistortionModel model = clams::calibrate(sequence, poses, map);
478  const cv::Size & imageSize = _imageSize;
479  if(_model == 0)
480  {
481  size_t bin_width = _ui->spinBox_bin_width->value();
482  size_t bin_height = _ui->spinBox_bin_height->value();
483  if(imageSize.width % _ui->spinBox_bin_width->value() != 0 ||
484  imageSize.height % _ui->spinBox_bin_height->value() != 0)
485  {
486  size_t bin_width, bin_height;
487  clams::DiscreteDepthDistortionModel::getBinSize(imageSize.width, imageSize.height, bin_width, bin_height);
488  _ui->spinBox_bin_width->setValue(bin_width);
489  _ui->spinBox_bin_height->setValue(bin_height);
490  }
491  _model = new clams::DiscreteDepthDistortionModel(
492  imageSize.width,
493  imageSize.height,
494  bin_width,
495  bin_height,
496  _ui->doubleSpinBox_bin_depth->value(),
497  _ui->spinBox_smoothing->value(),
498  _ui->doubleSpinBox_maxDepthModel->value());
499  }
500  UASSERT(_model->getWidth() == imageSize.width && _model->getHeight() == imageSize.height);
501 
502  // -- For all selected frames, accumulate training examples
503  // in the distortion model.
504  size_t counts;
505  index = 0;
506  for(std::map<int, rtabmap::Transform>::const_iterator iter = poses.begin(); iter != poses.end() && !_canceled; ++iter)
507  {
508  size_t idx = iter->first;
509  std::map<int, rtabmap::SensorData>::const_iterator ster = sequence.find(idx);
510  if(ster!=sequence.end())
511  {
512  cv::Mat depthImage;
513  ster->second.uncompressDataConst(0, &depthImage);
514 
515  if(ster->second.cameraModels().size() == 1 && ster->second.cameraModels()[0].isValidForProjection() && !depthImage.empty())
516  {
517  cv::Mat mapDepth;
518  CameraModel model = ster->second.cameraModels()[0];
519  if(model.imageWidth() != depthImage.cols)
520  {
521  UASSERT_MSG(model.imageHeight() % depthImage.rows == 0, uFormat("rgb=%d depth=%d", model.imageHeight(), depthImage.rows).c_str());
522  model = model.scaled(double(depthImage.rows) / double(model.imageHeight()));
523  }
524  clams::FrameProjector projector(model);
525  mapDepth = projector.estimateMapDepth(
526  map,
527  iter->second.inverse(),
528  depthImage,
529  _ui->doubleSpinBox_coneRadius->value(),
530  _ui->doubleSpinBox_coneStdDevThresh->value());
531 
532  if(ULogger::level() == ULogger::kDebug)
533  {
534  imageView1->setImage(uCvMat2QImage(depthImage));
535  imageView2->setImage(uCvMat2QImage(mapDepth));
536  }
537 
538  counts = _model->accumulate(mapDepth, depthImage);
539  _progressDialog->appendText(tr("Added %1 training examples from node %2 (%3/%4).").arg(counts).arg(iter->first).arg(++index).arg(sequence.size()));
540  }
541  }
542  _progressDialog->incrementStep();
543  QApplication::processEvents();
544  }
545 
546  _progressDialog->appendText(tr("CLAMS depth calibration... done!"));
547  QApplication::processEvents();
548 
549  if(!_canceled)
550  {
551  this->saveModel();
552  }
553  }
554  else
555  {
556  QMessageBox::warning(this, tr("Depth Calibration"), tr("The resulting map is empty!"));
557  }
558  _progressDialog->setValue(_progressDialog->maximumSteps());
559  }
560 }
561 
562 } /* namespace rtabmap */
rtabmap::SensorData
Definition: SensorData.h:51
rtabmap::CloudViewer::setCameraLockZ
void setCameraLockZ(bool enabled=true)
Definition: CloudViewer.cpp:3304
rtabmap::ProgressDialog
Definition: ProgressDialog.h:43
sequence::size
size_t size() const
rtabmap::DepthCalibrationDialog::_progressDialog
ProgressDialog * _progressDialog
Definition: DepthCalibrationDialog.h:77
rtabmap::DepthCalibrationDialog::DepthCalibrationDialog
DepthCalibrationDialog(QWidget *parent=0)
Definition: DepthCalibrationDialog.cpp:54
name
clams::DiscreteDepthDistortionModel::accumulate
size_t accumulate(const cv::Mat &ground_truth, const cv::Mat &measurement)
Definition: discrete_depth_distortion_model.cpp:318
ImageView.h
clams::DiscreteDepthDistortionModel
Definition: discrete_depth_distortion_model.h:68
arg::arg
constexpr arg(const char *name=nullptr)
clams::DiscreteDepthDistortionModel::getTrainingSamples
size_t getTrainingSamples() const
Definition: discrete_depth_distortion_model.h:108
s
RealScalar s
rtabmap::CloudViewer::addCloud
bool addCloud(const std::string &id, const pcl::PCLPointCloud2Ptr &binaryCloud, const Transform &pose, bool rgb, bool hasNormals, bool hasIntensity, const QColor &color=QColor(), int viewport=1)
Definition: CloudViewer.cpp:745
rtabmap::DepthCalibrationDialog::cancel
void cancel()
Definition: DepthCalibrationDialog.cpp:204
size
Index size
rtabmap::CameraModel
Definition: CameraModel.h:38
rtabmap::DepthCalibrationDialog::restoreDefaults
void restoreDefaults()
Definition: DepthCalibrationDialog.cpp:154
rtabmap::util3d::voxelize
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
Definition: util3d_filtering.cpp:613
rtabmap::DepthCalibrationDialog::_imageSize
cv::Size _imageSize
Definition: DepthCalibrationDialog.h:81
clams::DiscreteDepthDistortionModel::getWidth
int getWidth() const
Definition: discrete_depth_distortion_model.h:106
rtabmap::ProgressDialog::resetProgress
void resetProgress()
Definition: ProgressDialog.cpp:155
rtabmap::ProgressDialog::setValue
void setValue(int value)
Definition: ProgressDialog.cpp:112
ULogger::kDebug
@ kDebug
Definition: ULogger.h:252
rtabmap::ProgressDialog::appendText
void appendText(const QString &text, const QColor &color=Qt::black)
Definition: ProgressDialog.cpp:102
ProgressDialog.h
util3d.h
rtabmap::LaserScan
Definition: LaserScan.h:37
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
slam_calibrator.h
indices
indices
rtabmap::ProgressDialog::setAutoClose
void setAutoClose(bool on, int delayedClosingTimeSec=-1)
Definition: ProgressDialog.cpp:88
rtabmap::CloudViewer
Definition: CloudViewer.h:79
rtabmap::util3d::transformPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
Definition: util3d_transforms.cpp:107
rtabmap::ProgressDialog::setCancelButtonVisible
void setCancelButtonVisible(bool visible)
Definition: ProgressDialog.cpp:97
rtabmap::ImageView::setImage
void setImage(const QImage &image, const std::vector< CameraModel > &models=std::vector< CameraModel >(), const Transform &pose=Transform())
Definition: ImageView.cpp:1235
rtabmap::DepthCalibrationDialog::_model
clams::DiscreteDepthDistortionModel * _model
Definition: DepthCalibrationDialog.h:79
data
int data[]
sequence::begin
detail::sequence_iterator begin() const
rtabmap::CloudViewer::addOrUpdateFrustum
void addOrUpdateFrustum(const std::string &id, const Transform &pose, const Transform &localTransform, double scale, const QColor &color=QColor(), float fovX=1.1, float fovY=0.85)
Definition: CloudViewer.cpp:2146
util3d_transforms.h
sequence
rtabmap::DepthCalibrationDialog::loadSettings
void loadSettings(QSettings &settings, const QString &group="")
Definition: DepthCalibrationDialog.cpp:127
CloudViewer.h
util3d_filtering.h
rtabmap::DepthCalibrationDialog::_workingDirectory
QString _workingDirectory
Definition: DepthCalibrationDialog.h:80
arg
UCv2Qt.h
clams::DiscreteDepthDistortionModel::save
void save(const std::string &path) const
Definition: discrete_depth_distortion_model.cpp:365
rtabmap::DepthCalibrationDialog::saveModel
void saveModel()
Definition: DepthCalibrationDialog.cpp:179
UASSERT
#define UASSERT(condition)
DepthCalibrationDialog.h
rtabmap::DepthCalibrationDialog::_ui
Ui_DepthCalibrationDialog * _ui
Definition: DepthCalibrationDialog.h:76
path
path
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
ULogger::level
static ULogger::Level level()
Definition: ULogger.h:340
ULogger.h
ULogger class and convenient macros.
clams::FrameProjector
Definition: frame_projector.h:62
clams::FrameProjector::estimateMapDepth
cv::Mat estimateMapDepth(const pcl::PointCloud< pcl::PointXYZ >::Ptr &map, const rtabmap::Transform &transform, const cv::Mat &measurement, double coneRadius=0.02, double coneStdevThresh=0.03) const
Definition: frame_projector.cpp:106
rtabmap::Transform
Definition: Transform.h:41
rtabmap::util3d::cloudFromSensorData
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT cloudFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:1040
rtabmap::DepthCalibrationDialog::configChanged
void configChanged()
rtabmap::DepthCalibrationDialog::calibrate
void calibrate(const std::map< int, Transform > &poses, const QMap< int, Signature > &cachedSignatures, const QString &workingDirectory, const ParametersMap &parameters)
Definition: DepthCalibrationDialog.cpp:210
rtabmap::DepthCalibrationDialog::saveSettings
void saveSettings(QSettings &settings, const QString &group="") const
Definition: DepthCalibrationDialog.cpp:100
iter
iterator iter(handle obj)
clams::DiscreteDepthDistortionModel::visualize
cv::Mat visualize(const std::string &path="") const
Definition: discrete_depth_distortion_model_helpers.cpp:42
UThread.h
rtabmap::ProgressDialog::incrementStep
void incrementStep(int steps=1)
Definition: ProgressDialog.cpp:138
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
results
std::map< std::string, Array< float, 1, 8, DontAlign|RowMajor > > results
rtabmap::ImageView
Definition: ImageView.h:49
clams::DiscreteDepthDistortionModel::getHeight
int getHeight() const
Definition: discrete_depth_distortion_model.h:107
rtabmap::util3d::laserScanToPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2269
false
#define false
Definition: ConvertUTF.c:56
uCvMat2QImage
QImage uCvMat2QImage(const cv::Mat &image, bool isBgr=true, uCvQtDepthColorMap colorMap=uCvQtDepthWhiteToBlack, float depthMin=0, float depthMax=0)
Definition: UCv2Qt.h:47
rtabmap::CloudViewer::setFrustumShown
void setFrustumShown(bool shown)
Definition: CloudViewer.cpp:2486
rtabmap::DepthCalibrationDialog::~DepthCalibrationDialog
virtual ~DepthCalibrationDialog()
Definition: DepthCalibrationDialog.cpp:94
rtabmap::ProgressDialog::setMaximumSteps
void setMaximumSteps(int steps)
Definition: ProgressDialog.cpp:133
exec
void exec(const char(&s)[N], object global=globals(), object local=object())
rtabmap
Definition: CameraARCore.cpp:35
UERROR
#define UERROR(...)
trace.model
model
Definition: trace.py:4
i
int i
rtabmap::Signature
Definition: Signature.h:48
sequence::end
detail::sequence_iterator end() const
clams::DiscreteDepthDistortionModel::getBinSize
static void getBinSize(const size_t &width, const size_t &height, size_t &bin_width, size_t &bin_height)
Definition: discrete_depth_distortion_model.cpp:199
rtabmap::ProgressDialog::maximumSteps
int maximumSteps() const
Definition: ProgressDialog.cpp:129
msg
msg
frame_projector.h
rtabmap::DepthCalibrationDialog::_canceled
bool _canceled
Definition: DepthCalibrationDialog.h:78

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:09