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->setMargin(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::CloudViewer::setFrustumShown
void setFrustumShown(bool shown)
Definition: CloudViewer.cpp:2347
rtabmap::ProgressDialog::incrementStep
void incrementStep(int steps=1)
Definition: ProgressDialog.cpp:138
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::CameraModel::imageHeight
int imageHeight() const
Definition: CameraModel.h:121
rtabmap::ProgressDialog::setAutoClose
void setAutoClose(bool on, int delayedClosingTimeSec=-1)
Definition: ProgressDialog.cpp:88
rtabmap::CameraModel::scaled
CameraModel scaled(double scale) const
Definition: CameraModel.cpp:609
rtabmap::ProgressDialog::setCancelButtonVisible
void setCancelButtonVisible(bool visible)
Definition: ProgressDialog.cpp:97
rtabmap::util3d::transformPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
Definition: util3d_transforms.cpp:107
rtabmap::DepthCalibrationDialog::saveSettings
void saveSettings(QSettings &settings, const QString &group="") const
Definition: DepthCalibrationDialog.cpp:100
data
data
uCvMat2QImage
QImage uCvMat2QImage(const cv::Mat &image, bool isBgr=true, uCvQtDepthColorMap colorMap=uCvQtDepthWhiteToBlack, float depthMin=0, float depthMax=0)
Definition: UCv2Qt.h:47
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::ImageView::setImage
void setImage(const QImage &image)
Definition: ImageView.cpp:1143
rtabmap::util3d::laserScanToPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2197
rtabmap::DepthCalibrationDialog::loadSettings
void loadSettings(QSettings &settings, const QString &group="")
Definition: DepthCalibrationDialog.cpp:127
UASSERT
#define UASSERT(condition)
rtabmap::SensorData::id
int id() const
Definition: SensorData.h:174
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:236
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::_ui
Ui_DepthCalibrationDialog * _ui
Definition: DepthCalibrationDialog.h:76
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
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
name
string name
slam_calibrator.h
util3d_filtering.h
clams::DiscreteDepthDistortionModel::visualize
cv::Mat visualize(const std::string &path="") const
Definition: discrete_depth_distortion_model_helpers.cpp:42
rtabmap::ProgressDialog::setMaximumSteps
void setMaximumSteps(int steps)
Definition: ProgressDialog.cpp:133
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:2007
ULogger::level
static ULogger::Level level()
Definition: ULogger.h:340
util3d_transforms.h
rtabmap::CloudViewer::setCameraLockZ
void setCameraLockZ(bool enabled=true)
Definition: CloudViewer.cpp:3175
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:734
false
#define false
Definition: ConvertUTF.c:56
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
rtabmap::util3d::cloudFromSensorData
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP 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:853
frame_projector.h
clams::DiscreteDepthDistortionModel::accumulate
size_t accumulate(const cv::Mat &ground_truth, const cv::Mat &measurement)
Definition: discrete_depth_distortion_model.cpp:318
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:137
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
rtabmap::Signature::id
int id() const
Definition: Signature.h:70
trace.model
model
Definition: trace.py:4
rtabmap::CameraModel::imageWidth
int imageWidth() const
Definition: CameraModel.h:120
rtabmap::ProgressDialog::appendText
void appendText(const QString &text, const QColor &color=Qt::black)
Definition: ProgressDialog.cpp:102
rtabmap::util3d::voxelize
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
Definition: util3d_filtering.cpp:603
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap::SensorData::uncompressDataConst
void uncompressDataConst(cv::Mat *imageRaw, cv::Mat *depthOrRightRaw, LaserScan *laserScanRaw=0, cv::Mat *userDataRaw=0, cv::Mat *groundCellsRaw=0, cv::Mat *obstacleCellsRaw=0, cv::Mat *emptyCellsRaw=0) const
Definition: SensorData.cpp:623
rtabmap::DepthCalibrationDialog::_model
clams::DiscreteDepthDistortionModel * _model
Definition: DepthCalibrationDialog.h:79

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:28