DepthCalibrationDialog.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include <rtabmap/gui/DepthCalibrationDialog.h>
00029 #include "ui_depthCalibrationDialog.h"
00030 
00031 #include "rtabmap/gui/ProgressDialog.h"
00032 #include "rtabmap/gui/CloudViewer.h"
00033 #include "rtabmap/gui/ImageView.h"
00034 #include "rtabmap/utilite/ULogger.h"
00035 #include "rtabmap/utilite/UThread.h"
00036 #include "rtabmap/utilite/UCv2Qt.h"
00037 #include "rtabmap/core/util3d.h"
00038 #include "rtabmap/core/util3d_filtering.h"
00039 #include "rtabmap/core/util3d_transforms.h"
00040 
00041 #include "rtabmap/core/clams/slam_calibrator.h"
00042 #include "rtabmap/core/clams/frame_projector.h"
00043 
00044 #include <QPushButton>
00045 #include <QDir>
00046 #include <QFileInfo>
00047 #include <QUrl>
00048 #include <QtGui/QDesktopServices>
00049 #include <QMessageBox>
00050 #include <QFileDialog>
00051 
00052 namespace rtabmap {
00053 
00054 DepthCalibrationDialog::DepthCalibrationDialog(QWidget *parent) :
00055                         QDialog(parent),
00056                         _canceled(false),
00057                         _model(0)
00058 {
00059         _ui = new Ui_DepthCalibrationDialog();
00060         _ui->setupUi(this);
00061 
00062         connect(_ui->buttonBox->button(QDialogButtonBox::RestoreDefaults), SIGNAL(clicked()), this, SLOT(restoreDefaults()));
00063         connect(_ui->buttonBox->button(QDialogButtonBox::Save), SIGNAL(clicked()), this, SLOT(saveModel()));
00064         connect(_ui->buttonBox->button(QDialogButtonBox::Ok), SIGNAL(clicked()), this, SLOT(accept()));
00065         _ui->buttonBox->button(QDialogButtonBox::Ok)->setText("Calibrate");
00066 
00067         restoreDefaults();
00068 
00069         connect(_ui->spinBox_decimation, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
00070         connect(_ui->doubleSpinBox_maxDepth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
00071         connect(_ui->doubleSpinBox_minDepth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
00072         connect(_ui->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
00073         connect(_ui->doubleSpinBox_coneRadius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
00074         connect(_ui->doubleSpinBox_coneStdDevThresh, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
00075         connect(_ui->checkBox_laserScan, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
00076 
00077         connect(_ui->spinBox_bin_width, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
00078         connect(_ui->spinBox_bin_height, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
00079         connect(_ui->doubleSpinBox_bin_depth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
00080         connect(_ui->spinBox_smoothing, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
00081         connect(_ui->doubleSpinBox_maxDepthModel, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
00082 
00083         _ui->buttonBox->button(QDialogButtonBox::Ok)->setFocus();
00084 
00085         _progressDialog = new ProgressDialog(this);
00086         _progressDialog->setVisible(false);
00087         _progressDialog->setAutoClose(true, 2);
00088         _progressDialog->setMinimumWidth(600);
00089         _progressDialog->setCancelButtonVisible(true);
00090 
00091         connect(_progressDialog, SIGNAL(canceled()), this, SLOT(cancel()));
00092 }
00093 
00094 DepthCalibrationDialog::~DepthCalibrationDialog()
00095 {
00096         delete _ui;
00097         delete _model;
00098 }
00099 
00100 void DepthCalibrationDialog::saveSettings(QSettings & settings, const QString & group) const
00101 {
00102         if(!group.isEmpty())
00103         {
00104                 settings.beginGroup(group);
00105         }
00106 
00107         settings.setValue("decimation", _ui->spinBox_decimation->value());
00108         settings.setValue("max_depth", _ui->doubleSpinBox_maxDepth->value());
00109         settings.setValue("min_depth", _ui->doubleSpinBox_minDepth->value());
00110         settings.setValue("voxel",_ui->doubleSpinBox_voxelSize->value());
00111         settings.setValue("cone_radius",_ui->doubleSpinBox_coneRadius->value());
00112         settings.setValue("cone_stddev_thresh",_ui->doubleSpinBox_coneStdDevThresh->value());
00113         settings.setValue("laser_scan",_ui->checkBox_laserScan->isChecked());
00114 
00115         settings.setValue("bin_width",_ui->spinBox_bin_width->value());
00116         settings.setValue("bin_height",_ui->spinBox_bin_height->value());
00117         settings.setValue("bin_depth",_ui->doubleSpinBox_bin_depth->value());
00118         settings.setValue("smoothing",_ui->spinBox_smoothing->value());
00119         settings.setValue("max_model_depth",_ui->doubleSpinBox_maxDepthModel->value());
00120 
00121         if(!group.isEmpty())
00122         {
00123                 settings.endGroup();
00124         }
00125 }
00126 
00127 void DepthCalibrationDialog::loadSettings(QSettings & settings, const QString & group)
00128 {
00129         if(!group.isEmpty())
00130         {
00131                 settings.beginGroup(group);
00132         }
00133 
00134         _ui->spinBox_decimation->setValue(settings.value("decimation", _ui->spinBox_decimation->value()).toInt());
00135         _ui->doubleSpinBox_maxDepth->setValue(settings.value("max_depth", _ui->doubleSpinBox_maxDepth->value()).toDouble());
00136         _ui->doubleSpinBox_minDepth->setValue(settings.value("min_depth", _ui->doubleSpinBox_minDepth->value()).toDouble());
00137         _ui->doubleSpinBox_voxelSize->setValue(settings.value("voxel", _ui->doubleSpinBox_voxelSize->value()).toDouble());
00138         _ui->doubleSpinBox_coneRadius->setValue(settings.value("cone_radius", _ui->doubleSpinBox_coneRadius->value()).toDouble());
00139         _ui->doubleSpinBox_coneStdDevThresh->setValue(settings.value("cone_stddev_thresh", _ui->doubleSpinBox_coneStdDevThresh->value()).toDouble());
00140         _ui->checkBox_laserScan->setChecked(settings.value("laser_scan", _ui->checkBox_laserScan->isChecked()).toBool());
00141 
00142         _ui->spinBox_bin_width->setValue(settings.value("bin_width", _ui->spinBox_bin_width->value()).toInt());
00143         _ui->spinBox_bin_height->setValue(settings.value("bin_height", _ui->spinBox_bin_height->value()).toInt());
00144         _ui->doubleSpinBox_bin_depth->setValue(settings.value("bin_depth", _ui->doubleSpinBox_bin_depth->value()).toDouble());
00145         _ui->spinBox_smoothing->setValue(settings.value("smoothing", _ui->spinBox_smoothing->value()).toInt());
00146         _ui->doubleSpinBox_maxDepthModel->setValue(settings.value("max_model_depth", _ui->doubleSpinBox_maxDepthModel->value()).toDouble());
00147 
00148         if(!group.isEmpty())
00149         {
00150                 settings.endGroup();
00151         }
00152 }
00153 
00154 void DepthCalibrationDialog::restoreDefaults()
00155 {
00156         _ui->spinBox_decimation->setValue(1);
00157         _ui->doubleSpinBox_maxDepth->setValue(3.5);
00158         _ui->doubleSpinBox_minDepth->setValue(0);
00159         _ui->doubleSpinBox_voxelSize->setValue(0.01);
00160         _ui->doubleSpinBox_coneRadius->setValue(0.02);
00161         _ui->doubleSpinBox_coneStdDevThresh->setValue(0.1); // 0.03
00162         _ui->checkBox_laserScan->setChecked(false);
00163         _ui->checkBox_resetModel->setChecked(true);
00164 
00165         _ui->spinBox_bin_width->setValue(8);
00166         _ui->spinBox_bin_height->setValue(6);
00167         if(_imageSize.width > 0 && _imageSize.height > 0)
00168         {
00169                 size_t bin_width, bin_height;
00170                 clams::DiscreteDepthDistortionModel::getBinSize(_imageSize.width, _imageSize.height, bin_width, bin_height);
00171                 _ui->spinBox_bin_width->setValue(bin_width);
00172                 _ui->spinBox_bin_height->setValue(bin_height);
00173         }
00174         _ui->doubleSpinBox_bin_depth->setValue(2.0),
00175         _ui->spinBox_smoothing->setValue(1);
00176         _ui->doubleSpinBox_maxDepthModel->setValue(10.0);
00177 }
00178 
00179 void DepthCalibrationDialog::saveModel()
00180 {
00181         if(_model && _model->getTrainingSamples())
00182         {
00183                 QString path = QFileDialog::getSaveFileName(this, tr("Save distortion model to ..."), _workingDirectory+QDir::separator()+"distortion_model.bin", tr("Distortion model (*.bin *.txt)"));
00184                 if(!path.isEmpty())
00185                 {
00186                         //
00187                         // Save depth calibration
00188                         //
00189                         cv::Mat results = _model->visualize(ULogger::level() == ULogger::kDebug?_workingDirectory.toStdString():"");
00190                         _model->save(path.toStdString());
00191 
00192                         if(!results.empty())
00193                         {
00194                                 QString name = QString(path).replace(".bin", ".png", Qt::CaseInsensitive).replace(".txt", ".png", Qt::CaseInsensitive);
00195                                 cv::imwrite(name.toStdString(), results);
00196                                 QDesktopServices::openUrl(QUrl::fromLocalFile(name));
00197                         }
00198 
00199                         QMessageBox::information(this, tr("Depth Calibration"), tr("Distortion model saved to \"%1\"!").arg(path));
00200                 }
00201         }
00202 }
00203 
00204 void DepthCalibrationDialog::cancel()
00205 {
00206         _canceled = true;
00207         _progressDialog->appendText(tr("Canceled!"));
00208 }
00209 
00210 void DepthCalibrationDialog::calibrate(
00211                 const std::map<int, Transform> & poses,
00212                         const QMap<int, Signature> & cachedSignatures,
00213                         const QString & workingDirectory,
00214                         const ParametersMap & parameters)
00215 {
00216         _canceled = false;
00217         _workingDirectory = workingDirectory;
00218         _ui->buttonBox->button(QDialogButtonBox::Save)->setEnabled(_model && _model->getTrainingSamples()>0);
00219         if(_model)
00220         {
00221                 _ui->label_trainingSamples->setNum((int)_model->getTrainingSamples());
00222         }
00223 
00224         _ui->label_width->setText("NA");
00225         _ui->label_height->setText("NA");
00226         _imageSize = cv::Size();
00227         CameraModel model;
00228         if(cachedSignatures.size())
00229         {
00230                 const Signature & s = cachedSignatures.begin().value();
00231                 const SensorData & data = s.sensorData();
00232                 cv::Mat depth;
00233                 data.uncompressDataConst(0, &depth);
00234                 if(data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForProjection() && !depth.empty())
00235                 {
00236                         // use depth image size
00237                         _imageSize = depth.size();
00238                         _ui->label_width->setNum(_imageSize.width);
00239                         _ui->label_height->setNum(_imageSize.height);
00240 
00241                         if(_imageSize.width % _ui->spinBox_bin_width->value() != 0 ||
00242                                 _imageSize.height % _ui->spinBox_bin_height->value() != 0)
00243                         {
00244                                 size_t bin_width, bin_height;
00245                                 clams::DiscreteDepthDistortionModel::getBinSize(_imageSize.width, _imageSize.height, bin_width, bin_height);
00246                                 _ui->spinBox_bin_width->setValue(bin_width);
00247                                 _ui->spinBox_bin_height->setValue(bin_height);
00248                         }
00249                 }
00250                 else if(data.cameraModels().size() > 1)
00251                 {
00252                         QMessageBox::warning(this, tr("Depth Calibration"),tr("Multi-camera not supported!"));
00253                         return;
00254                 }
00255                 else if(data.cameraModels().size() != 1)
00256                 {
00257                         QMessageBox::warning(this, tr("Depth Calibration"), tr("Camera model not found."));
00258                         return;
00259                 }
00260                 else if(data.cameraModels().size() == 1 && !data.cameraModels()[0].isValidForProjection())
00261                 {
00262                         QMessageBox::warning(this, tr("Depth Calibration"), tr("Camera model %1 not valid for projection.").arg(s.id()));
00263                         return;
00264                 }
00265                 else
00266                 {
00267                         QMessageBox::warning(this, tr("Depth Calibration"), tr("Depth image cannot be found in the cache, make sure to update cache before doing calibration."));
00268                         return;
00269                 }
00270         }
00271         else
00272         {
00273                 QMessageBox::warning(this, tr("Depth Calibration"), tr("No signatures detected! Map is empty!?"));
00274                 return;
00275         }
00276 
00277         if(this->exec() == QDialog::Accepted)
00278         {
00279                 if(_model && _ui->checkBox_resetModel->isChecked())
00280                 {
00281                         delete _model;
00282                         _model = 0;
00283                 }
00284 
00285                 if(_ui->doubleSpinBox_maxDepthModel->value() < _ui->doubleSpinBox_bin_depth->value())
00286                 {
00287                         QMessageBox::warning(this, tr("Wrong parameter"), tr("Maximum model depth should be higher than bin depth, setting to bin depth x5."));
00288                         _ui->doubleSpinBox_maxDepthModel->setValue(_ui->doubleSpinBox_bin_depth->value() * 5.0);
00289                 }
00290 
00291                 _progressDialog->setMaximumSteps(poses.size()*2 + 3);
00292                 if(_ui->doubleSpinBox_voxelSize->value() > 0.0)
00293                 {
00294                         _progressDialog->setMaximumSteps(_progressDialog->maximumSteps()+1);
00295                 }
00296                 _progressDialog->resetProgress();
00297                 _progressDialog->show();
00298 
00299                 std::map<int, rtabmap::SensorData> sequence;
00300 
00301                 // Create the map
00302                 pcl::PointCloud<pcl::PointXYZ>::Ptr  map(new pcl::PointCloud<pcl::PointXYZ>);
00303                 int index=1;
00304                 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end() && !_canceled; ++iter, ++index)
00305                 {
00306                         int points = 0;
00307                         if(!iter->second.isNull())
00308                         {
00309                                 pcl::IndicesPtr indices(new std::vector<int>);
00310                                 if(cachedSignatures.contains(iter->first))
00311                                 {
00312                                         const Signature & s = cachedSignatures.find(iter->first).value();
00313                                         SensorData data = s.sensorData();
00314 
00315                                         cv::Mat  depth;
00316                                         LaserScan laserScan;
00317                                         data.uncompressData(0, &depth, _ui->checkBox_laserScan->isChecked()?&laserScan:0);
00318                                         if(data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForProjection() && !depth.empty())
00319                                         {
00320                                                 UASSERT(iter->first == data.id());
00321                                                 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
00322 
00323                                                 if(_ui->checkBox_laserScan->isChecked())
00324                                                 {
00325                                                         cloud = util3d::laserScanToPointCloud(laserScan);
00326                                                         indices->resize(cloud->size());
00327                                                         for(unsigned int i=0; i<indices->size(); ++i)
00328                                                         {
00329                                                                 indices->at(i) = i;
00330                                                         }
00331                                                 }
00332                                                 else
00333                                                 {
00334                                                         cloud = util3d::cloudFromSensorData(
00335                                                                         data,
00336                                                                         _ui->spinBox_decimation->value(),
00337                                                                         _ui->doubleSpinBox_maxDepth->value(),
00338                                                                         _ui->doubleSpinBox_minDepth->value(),
00339                                                                         indices.get(),
00340                                                                         parameters);
00341                                                 }
00342 
00343                                                 if(indices->size())
00344                                                 {
00345                                                         if(_ui->doubleSpinBox_voxelSize->value() > 0.0)
00346                                                         {
00347                                                                 cloud = util3d::voxelize(cloud, indices, _ui->doubleSpinBox_voxelSize->value());
00348                                                         }
00349 
00350                                                         cloud = util3d::transformPointCloud(cloud, iter->second);
00351 
00352                                                         points+=cloud->size();
00353 
00354                                                         *map += *cloud;
00355 
00356                                                         sequence.insert(std::make_pair(iter->first, data));
00357 
00358                                                         cv::Size size = depth.size();
00359                                                         if(_model &&
00360                                                                 (_model->getWidth()!=size.width ||
00361                                                                  _model->getHeight()!=size.height))
00362                                                         {
00363                                                                 QString msg = tr("Depth images (%1x%2) in the map don't have the "
00364                                                                                    "same size then in the current model (%3x%4). You may want "
00365                                                                                    "to check \"Reset previous model\" before trying again.")
00366                                                                                    .arg(size.width).arg(size.height)
00367                                                                                    .arg(_model->getWidth()).arg(_model->getHeight());
00368                                                                 QMessageBox::warning(this, tr("Depth Calibration"), msg);
00369                                                                 _progressDialog->appendText(msg, Qt::darkRed);
00370                                                                 _progressDialog->setAutoClose(false);
00371                                                                 return;
00372                                                         }
00373                                                 }
00374                                         }
00375                                         else
00376                                         {
00377                                                 _progressDialog->appendText(tr("Not suitable camera model found for node %1, ignoring this node!").arg(iter->first), Qt::darkYellow);
00378                                                 _progressDialog->setAutoClose(false);
00379                                         }
00380                                 }
00381                                 else
00382                                 {
00383                                         UERROR("Cloud %d not found in cache!", iter->first);
00384                                 }
00385                         }
00386                         else
00387                         {
00388                                 UERROR("transform is null!?");
00389                         }
00390 
00391                         if(points>0)
00392                         {
00393                                 _progressDialog->appendText(tr("Generated cloud %1 with %2 points (%3/%4).")
00394                                                 .arg(iter->first).arg(points).arg(index).arg(poses.size()));
00395                         }
00396                         else
00397                         {
00398                                 _progressDialog->appendText(tr("Ignored cloud %1 (%2/%3).").arg(iter->first).arg(index).arg(poses.size()));
00399                         }
00400                         _progressDialog->incrementStep();
00401                         QApplication::processEvents();
00402                 }
00403 
00404                 if(!_canceled && map->size() && sequence.size())
00405                 {
00406                         if(_ui->doubleSpinBox_voxelSize->value() > 0.0)
00407                         {
00408                                 _progressDialog->appendText(tr("Voxel filtering (%1 m) of the merged point cloud (%2 points)")
00409                                                 .arg(_ui->doubleSpinBox_voxelSize->value())
00410                                                 .arg(map->size()));
00411                                 QApplication::processEvents();
00412                                 QApplication::processEvents();
00413 
00414                                 map = util3d::voxelize(map, _ui->doubleSpinBox_voxelSize->value());
00415                                 _progressDialog->incrementStep();
00416                         }
00417 
00418                         if(ULogger::level() == ULogger::kDebug)
00419                         {
00420                                 //
00421                                 // Show 3D map with frustums
00422                                 //
00423                                 QDialog * window = new QDialog(this->parentWidget()?this->parentWidget():this, Qt::Window);
00424                                 window->setAttribute(Qt::WA_DeleteOnClose, true);
00425                                 window->setWindowTitle(tr("Map"));
00426                                 window->setMinimumWidth(800);
00427                                 window->setMinimumHeight(600);
00428 
00429                                 CloudViewer * viewer = new CloudViewer(window);
00430                                 viewer->setCameraLockZ(false);
00431                                 viewer->setFrustumShown(true);
00432 
00433                                 QVBoxLayout *layout = new QVBoxLayout();
00434                                 layout->addWidget(viewer);
00435                                 window->setLayout(layout);
00436                                 connect(window, SIGNAL(finished(int)), viewer, SLOT(clear()));
00437 
00438                                 window->show();
00439 
00440                                 uSleep(500);
00441 
00442                                 _progressDialog->appendText(tr("Viewing the cloud (%1 points and %2 poses)...").arg(map->size()).arg(sequence.size()));
00443                                 _progressDialog->incrementStep();
00444                                 viewer->addCloud("map", map);
00445                                 for(std::map<int, SensorData>::iterator iter=sequence.begin(); iter!=sequence.end(); ++iter)
00446                                 {
00447                                         Transform baseToCamera = iter->second.cameraModels()[0].localTransform();
00448                                         viewer->addOrUpdateFrustum(uFormat("frustum%d",iter->first), poses.at(iter->first), baseToCamera, 0.2);
00449                                 }
00450                                 _progressDialog->appendText(tr("Viewing the cloud (%1 points and %2 poses)... done.").arg(map->size()).arg(sequence.size()));
00451 
00452                                 viewer->update();
00453                         }
00454 
00455                         _progressDialog->appendText(tr("CLAMS depth calibration..."));
00456                         QApplication::processEvents();
00457                         QApplication::processEvents();
00458 
00459                         QDialog * dialog = new QDialog(this->parentWidget()?this->parentWidget():this, Qt::Window);
00460                         dialog->setAttribute(Qt::WA_DeleteOnClose, true);
00461                         dialog->setWindowTitle(tr("Original/Map"));
00462                         dialog->setMinimumWidth(_imageSize.width);
00463                         ImageView * imageView1 = new ImageView(dialog);
00464                         imageView1->setMinimumSize(320, 240);
00465                         ImageView * imageView2 = new ImageView(dialog);
00466                         imageView2->setMinimumSize(320, 240);
00467                         QVBoxLayout * vlayout = new QVBoxLayout();
00468                         vlayout->setMargin(0);
00469                         vlayout->addWidget(imageView1, 1);
00470                         vlayout->addWidget(imageView2, 1);
00471                         dialog->setLayout(vlayout);
00472                         if(ULogger::level() == ULogger::kDebug)
00473                         {
00474                                 dialog->show();
00475                         }
00476 
00477                         //clams::DiscreteDepthDistortionModel model = clams::calibrate(sequence, poses, map);
00478                         const cv::Size & imageSize = _imageSize;
00479                         if(_model == 0)
00480                         {
00481                                 size_t bin_width = _ui->spinBox_bin_width->value();
00482                                 size_t bin_height = _ui->spinBox_bin_height->value();
00483                                 if(imageSize.width % _ui->spinBox_bin_width->value() != 0 ||
00484                                    imageSize.height % _ui->spinBox_bin_height->value() != 0)
00485                                 {
00486                                         size_t bin_width, bin_height;
00487                                         clams::DiscreteDepthDistortionModel::getBinSize(imageSize.width, imageSize.height, bin_width, bin_height);
00488                                         _ui->spinBox_bin_width->setValue(bin_width);
00489                                         _ui->spinBox_bin_height->setValue(bin_height);
00490                                 }
00491                                 _model = new clams::DiscreteDepthDistortionModel(
00492                                                 imageSize.width,
00493                                                 imageSize.height,
00494                                                 bin_width,
00495                                                 bin_height,
00496                                                 _ui->doubleSpinBox_bin_depth->value(),
00497                                                 _ui->spinBox_smoothing->value(),
00498                                                 _ui->doubleSpinBox_maxDepthModel->value());
00499                         }
00500                         UASSERT(_model->getWidth() == imageSize.width && _model->getHeight() == imageSize.height);
00501 
00502                         // -- For all selected frames, accumulate training examples
00503                         //    in the distortion model.
00504                         size_t counts;
00505                         index = 0;
00506                         for(std::map<int, rtabmap::Transform>::const_iterator iter = poses.begin(); iter != poses.end() && !_canceled; ++iter)
00507                         {
00508                           size_t idx = iter->first;
00509                           std::map<int, rtabmap::SensorData>::const_iterator ster = sequence.find(idx);
00510                           if(ster!=sequence.end())
00511                           {
00512                                   cv::Mat depthImage;
00513                                   ster->second.uncompressDataConst(0, &depthImage);
00514 
00515                                   if(ster->second.cameraModels().size() == 1 && ster->second.cameraModels()[0].isValidForProjection() && !depthImage.empty())
00516                                   {
00517                                           cv::Mat mapDepth;
00518                                           CameraModel model = ster->second.cameraModels()[0];
00519                                           if(model.imageWidth() != depthImage.cols)
00520                                           {
00521                                                   UASSERT_MSG(model.imageHeight() % depthImage.rows == 0, uFormat("rgb=%d depth=%d", model.imageHeight(), depthImage.rows).c_str());
00522                                                   model = model.scaled(double(depthImage.rows) / double(model.imageHeight()));
00523                                           }
00524                                           clams::FrameProjector projector(model);
00525                                           mapDepth = projector.estimateMapDepth(
00526                                                           map,
00527                                                           iter->second.inverse(),
00528                                                           depthImage,
00529                                                          _ui->doubleSpinBox_coneRadius->value(),
00530                                                          _ui->doubleSpinBox_coneStdDevThresh->value());
00531 
00532                                           if(ULogger::level() == ULogger::kDebug)
00533                                           {
00534                                                   imageView1->setImage(uCvMat2QImage(depthImage));
00535                                                   imageView2->setImage(uCvMat2QImage(mapDepth));
00536                                           }
00537 
00538                                           counts = _model->accumulate(mapDepth, depthImage);
00539                                           _progressDialog->appendText(tr("Added %1 training examples from node %2 (%3/%4).").arg(counts).arg(iter->first).arg(++index).arg(sequence.size()));
00540                                   }
00541                                  }
00542                           _progressDialog->incrementStep();
00543                           QApplication::processEvents();
00544                         }
00545 
00546                         _progressDialog->appendText(tr("CLAMS depth calibration... done!"));
00547                         QApplication::processEvents();
00548 
00549                         if(!_canceled)
00550                         {
00551                                 this->saveModel();
00552                         }
00553                 }
00554                 else
00555                 {
00556                         QMessageBox::warning(this, tr("Depth Calibration"), tr("The resulting map is empty!"));
00557                 }
00558                 _progressDialog->setValue(_progressDialog->maximumSteps());
00559         }
00560 }
00561 
00562 } /* namespace rtabmap */


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:59:19