rtabmap: CreateSimpleCalibrationDialog.cpp Source File

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 "CreateSimpleCalibrationDialog.h"
00029 #include "ui_createSimpleCalibrationDialog.h"
00030 
00031 #include "rtabmap/core/CameraModel.h"
00032 #include "rtabmap/core/StereoCameraModel.h"
00033 #include "rtabmap/utilite/ULogger.h"
00034 
00035 #include <QFileDialog>
00036 #include <QPushButton>
00037 #include <QMessageBox>
00038 
00039 namespace rtabmap {
00040 
00041 CreateSimpleCalibrationDialog::CreateSimpleCalibrationDialog(
00042                 const QString & savingFolder,
00043                 const QString & cameraName,
00044                 QWidget * parent) :
00045         QDialog(parent),
00046         savingFolder_(savingFolder),
00047         cameraName_(cameraName)
00048 {
00049         if(cameraName_.isEmpty())
00050         {
00051                 cameraName_ = "calib";
00052         }
00053 
00054         ui_ = new Ui_createSimpleCalibrationDialog();
00055         ui_->setupUi(this);
00056 
00057         connect(ui_->buttonBox->button(QDialogButtonBox::Save), SIGNAL(clicked()), this, SLOT(saveCalibration()));
00058         connect(ui_->buttonBox, SIGNAL(rejected()), this, SLOT(reject()));
00059 
00060         connect(ui_->comboBox_advanced, SIGNAL(currentIndexChanged(int)), ui_->stackedWidget, SLOT(setCurrentIndex(int)));
00061 
00062         connect(ui_->checkBox_stereo, SIGNAL(stateChanged(int)), this, SLOT(updateStereoView()));
00063         connect(ui_->checkBox_stereo, SIGNAL(stateChanged(int)), this, SLOT(updateSaveStatus()));
00064         connect(ui_->comboBox_advanced, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSaveStatus()));
00065 
00066         connect(ui_->doubleSpinBox_fx, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
00067         connect(ui_->doubleSpinBox_fy, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
00068 
00069         connect(ui_->doubleSpinBox_fx_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
00070         connect(ui_->doubleSpinBox_fy_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
00071         connect(ui_->doubleSpinBox_cx_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
00072         connect(ui_->doubleSpinBox_cy_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
00073         connect(ui_->lineEdit_D_l, SIGNAL(textEdited(const QString &)), this, SLOT(updateSaveStatus()));
00074 
00075         connect(ui_->doubleSpinBox_fx_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
00076         connect(ui_->doubleSpinBox_fy_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
00077         connect(ui_->doubleSpinBox_cx_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
00078         connect(ui_->doubleSpinBox_cy_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
00079         connect(ui_->lineEdit_D_r, SIGNAL(textEdited(const QString &)), this, SLOT(updateSaveStatus()));
00080 
00081         connect(ui_->spinBox_width, SIGNAL(valueChanged(int)), this, SLOT(updateSaveStatus()));
00082         connect(ui_->spinBox_height, SIGNAL(valueChanged(int)), this, SLOT(updateSaveStatus()));
00083 
00084         connect(ui_->doubleSpinBox_baseline, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
00085         connect(ui_->lineEdit_RT, SIGNAL(textEdited(const QString &)), this, SLOT(updateSaveStatus()));
00086 
00087         ui_->stackedWidget->setCurrentIndex(ui_->comboBox_advanced->currentIndex());
00088 
00089         ui_->buttonBox->button(QDialogButtonBox::Save)->setEnabled(false);
00090 
00091         updateStereoView();
00092 }
00093 
00094 CreateSimpleCalibrationDialog::~CreateSimpleCalibrationDialog()
00095 {
00096         delete ui_;
00097 }
00098 
00099 void CreateSimpleCalibrationDialog::updateStereoView()
00100 {
00101         bool checked = ui_->checkBox_stereo->isChecked();
00102         ui_->doubleSpinBox_baseline->setVisible(checked);
00103         ui_->label_baseline->setVisible(checked);
00104         ui_->label_right->setVisible(checked);
00105         ui_->doubleSpinBox_fx_r->setVisible(checked);
00106         ui_->doubleSpinBox_fy_r->setVisible(checked);
00107         ui_->doubleSpinBox_cx_r->setVisible(checked);
00108         ui_->doubleSpinBox_cy_r->setVisible(checked);
00109         ui_->lineEdit_D_r->setVisible(checked);
00110         ui_->groupBox_stereo_extrinsics->setVisible(checked);
00111         ui_->label_left->setVisible(checked);
00112 }
00113 
00114 void CreateSimpleCalibrationDialog::updateSaveStatus()
00115 {
00116         bool valid = false;
00117         if(ui_->comboBox_advanced->currentIndex() == 0 &&
00118            ui_->doubleSpinBox_fx->value() > 0.0 &&
00119            ui_->doubleSpinBox_fy->value() > 0.0 &&
00120            (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_baseline->value() != 0.0))
00121         {
00122                 // basic
00123                 valid = true;
00124         }
00125         else if(ui_->comboBox_advanced->currentIndex() == 1 &&
00126                          ui_->doubleSpinBox_fx_l->value() > 0.0 &&
00127                          ui_->doubleSpinBox_fy_l->value() > 0.0 &&
00128                          ui_->doubleSpinBox_cx_l->value() > 0.0 &&
00129                          ui_->doubleSpinBox_cy_l->value() > 0.0 &&
00130                          (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_fx_r->value() > 0.0) &&
00131                          (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_fy_r->value() > 0.0) &&
00132                          (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_cx_r->value() > 0.0) &&
00133                          (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_cy_r->value() > 0.0) &&
00134                          ui_->spinBox_width->value() > 0 &&
00135                          ui_->spinBox_height->value() > 0 &&
00136                          !ui_->lineEdit_D_l->text().isEmpty() &&
00137                          (!ui_->checkBox_stereo->isChecked() || !ui_->lineEdit_D_r->text().isEmpty()) &&
00138                          (!ui_->checkBox_stereo->isChecked() || !ui_->lineEdit_RT->text().isEmpty()))
00139         {
00140                 //advanced
00141                 QStringList distorsionsStrListL = ui_->lineEdit_D_l->text().trimmed().split(' ');
00142                 QStringList distorsionsStrListR = ui_->lineEdit_D_r->text().trimmed().split(' ');
00143 
00144                 if((distorsionsStrListL.size() == 4 || distorsionsStrListL.size() == 5 || distorsionsStrListL.size() == 8) &&
00145                                 (!ui_->checkBox_stereo->isChecked() || distorsionsStrListL.size() == 4 || distorsionsStrListL.size() == 5 || distorsionsStrListL.size() == 8) &&
00146                                 (!ui_->checkBox_stereo->isChecked() || Transform::canParseString(ui_->lineEdit_RT->text().trimmed().toStdString())))
00147                 {
00148                         valid = true;
00149                 }
00150         }
00151         ui_->buttonBox->button(QDialogButtonBox::Save)->setEnabled(valid);
00152 }
00153 
00154 void CreateSimpleCalibrationDialog::saveCalibration()
00155 {
00156         QString filePath = QFileDialog::getSaveFileName(this, tr("Save"), savingFolder_+"/"+cameraName_+".yaml", "*.yaml");
00157         QString name = QFileInfo(filePath).baseName();
00158         QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
00159         if(!filePath.isEmpty())
00160         {
00161                 cameraName_ = name;
00162                 CameraModel modelLeft;
00163                 float width = ui_->spinBox_width->value();
00164                 float height = ui_->spinBox_height->value();
00165                 if(ui_->comboBox_advanced->currentIndex() == 0)
00166                 {
00167                         //basic
00168                         modelLeft = CameraModel(
00169                                         name.toStdString(),
00170                                         ui_->doubleSpinBox_fx->value(),
00171                                         ui_->doubleSpinBox_fy->value(),
00172                                         ui_->doubleSpinBox_cx->value(),
00173                                         ui_->doubleSpinBox_cy->value(),
00174                                         Transform::getIdentity(),
00175                                         0,
00176                                         cv::Size(width, height));
00177                         UASSERT(modelLeft.isValidForProjection());
00178                 }
00179                 else
00180                 {
00181                         //advanced
00182                         cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
00183                         K.at<double>(0,0) = ui_->doubleSpinBox_fx_l->value();
00184                         K.at<double>(1,1) = ui_->doubleSpinBox_fy_l->value();
00185                         K.at<double>(0,2) = ui_->doubleSpinBox_cx_l->value();
00186                         K.at<double>(1,2) = ui_->doubleSpinBox_cy_l->value();
00187                         cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
00188                         cv::Mat P = cv::Mat::zeros(3, 4, CV_64FC1);
00189                         K.copyTo(cv::Mat(P, cv::Range(0,3), cv::Range(0,3)));
00190 
00191                         QStringList distorsionCoeffs = ui_->lineEdit_D_l->text().trimmed().split(' ');
00192                         UASSERT(distorsionCoeffs.size() == 4 || distorsionCoeffs.size() == 5 || distorsionCoeffs.size() == 8);
00193                         cv::Mat D = cv::Mat::zeros(1, distorsionCoeffs.size(), CV_64FC1);
00194                         bool ok;
00195                         for(int i=0; i<distorsionCoeffs.size(); ++i)
00196                         {
00197                                 D.at<double>(i) = distorsionCoeffs.at(i).toDouble(&ok);
00198                                 if(!ok)
00199                                 {
00200                                         QMessageBox::warning(this, tr("Save"), tr("Error parsing left distorsion coefficients \"%1\".").arg(ui_->lineEdit_D_l->text()));
00201                                         return;
00202                                 }
00203                         }
00204 
00205                         modelLeft = CameraModel(name.toStdString(), cv::Size(width,height), K, D, R, P);
00206                         UASSERT(modelLeft.isValidForRectification());
00207                 }
00208 
00209                 if(ui_->checkBox_stereo->isChecked())
00210                 {
00211                         StereoCameraModel stereoModel;
00212                         if(ui_->comboBox_advanced->currentIndex() == 0)
00213                         {
00214                                 CameraModel modelRight(
00215                                                 name.toStdString(),
00216                                                 ui_->doubleSpinBox_fx->value(),
00217                                                 ui_->doubleSpinBox_fy->value(),
00218                                                 ui_->doubleSpinBox_cx->value(),
00219                                                 ui_->doubleSpinBox_cy->value(),
00220                                                 Transform::getIdentity(),
00221                                                 ui_->doubleSpinBox_baseline->value()*-ui_->doubleSpinBox_fx->value(),
00222                                                 cv::Size(width, height));
00223                                 UASSERT(modelRight.isValidForProjection());
00224                                 stereoModel = StereoCameraModel(name.toStdString(), modelLeft, modelRight, Transform());
00225                                 UASSERT(stereoModel.isValidForProjection());
00226                         }
00227                         else if(ui_->comboBox_advanced->currentIndex() == 1)
00228                         {
00229                                 CameraModel modelRight;
00230                                 cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
00231                                 K.at<double>(0,0) = ui_->doubleSpinBox_fx_r->value();
00232                                 K.at<double>(1,1) = ui_->doubleSpinBox_fy_r->value();
00233                                 K.at<double>(0,2) = ui_->doubleSpinBox_cx_r->value();
00234                                 K.at<double>(1,2) = ui_->doubleSpinBox_cy_r->value();
00235                                 cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
00236                                 cv::Mat P = cv::Mat::zeros(3, 4, CV_64FC1);
00237                                 K.copyTo(cv::Mat(P, cv::Range(0,3), cv::Range(0,3)));
00238 
00239                                 QStringList distorsionCoeffs = ui_->lineEdit_D_r->text().trimmed().split(' ');
00240                                 UASSERT(distorsionCoeffs.size() == 4 || distorsionCoeffs.size() == 5 || distorsionCoeffs.size() == 8);
00241                                 cv::Mat D = cv::Mat::zeros(1, distorsionCoeffs.size(), CV_64FC1);
00242                                 bool ok;
00243                                 for(int i=0; i<distorsionCoeffs.size(); ++i)
00244                                 {
00245                                         D.at<double>(i) = distorsionCoeffs.at(i).toDouble(&ok);
00246                                         if(!ok)
00247                                         {
00248                                                 QMessageBox::warning(this, tr("Save"), tr("Error parsing right distorsion coefficients \"%1\".").arg(ui_->lineEdit_D_l->text()));
00249                                                 return;
00250                                         }
00251                                 }
00252 
00253                                 modelRight = CameraModel(name.toStdString(), cv::Size(width,height), K, D, R, P);
00254                                 UASSERT(modelRight.isValidForRectification());
00255 
00256                                 UASSERT(Transform::canParseString(ui_->lineEdit_RT->text().trimmed().toStdString()));
00257                                 stereoModel = StereoCameraModel(name.toStdString(), modelLeft, modelRight, Transform::fromString(ui_->lineEdit_RT->text().toStdString()));
00258                                 UASSERT(stereoModel.isValidForRectification());
00259                         }
00260 
00261                         std::string base = (dir+QDir::separator()+name).toStdString();
00262                         std::string leftPath = base+"_left.yaml";
00263                         std::string rightPath = base+"_right.yaml";
00264 
00265                         if(stereoModel.save(dir.toStdString(), ui_->comboBox_advanced->currentIndex() != 1))
00266                         {
00267                                 if(ui_->comboBox_advanced->currentIndex() == 0)
00268                                 {
00269                                         QMessageBox::information(this, tr("Save"), tr("Calibration files saved:\n  \"%1\"\n  \"%2\".").
00270                                                         arg(leftPath.c_str()).arg(rightPath.c_str()));
00271                                 }
00272                                 else
00273                                 {
00274                                         std::string posePath = base+"_pose.yaml";
00275                                         QMessageBox::information(this, tr("Save"), tr("Calibration files saved:\n  \"%1\"\n  \"%2\"\n  \"%3\".").
00276                                                         arg(leftPath.c_str()).arg(rightPath.c_str()).arg(posePath.c_str()));
00277                                 }
00278                                 this->accept();
00279                         }
00280                         else
00281                         {
00282                                 QMessageBox::warning(this, tr("Save"), tr("Error saving \"%1\" and \"%2\"").arg(leftPath.c_str()).arg(rightPath.c_str()));
00283                         }
00284                 }
00285                 else
00286                 {
00287                         if(modelLeft.save(dir.toStdString()))
00288                         {
00289                                 QMessageBox::information(this, tr("Save"), tr("Calibration file saved to \"%1\".").arg(filePath));
00290                                 this->accept();
00291                         }
00292                         else
00293                         {
00294                                 QMessageBox::warning(this, tr("Save"), tr("Error saving \"%1\"").arg(filePath));
00295                         }
00296                 }
00297         }
00298 }
00299 
00300 }