00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "rtabmap/gui/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_->comboBox_advanced, SIGNAL(currentIndexChanged(int)), this, SLOT(updateStereoView()));
00064 connect(ui_->checkBox_stereo, SIGNAL(stateChanged(int)), this, SLOT(updateSaveStatus()));
00065 connect(ui_->comboBox_advanced, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSaveStatus()));
00066
00067 connect(ui_->doubleSpinBox_fx, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
00068 connect(ui_->doubleSpinBox_fy, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
00069
00070 connect(ui_->doubleSpinBox_fx_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
00071 connect(ui_->doubleSpinBox_fy_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
00072 connect(ui_->doubleSpinBox_cx_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
00073 connect(ui_->doubleSpinBox_cy_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
00074 connect(ui_->lineEdit_D_l, SIGNAL(textEdited(const QString &)), this, SLOT(updateSaveStatus()));
00075
00076 connect(ui_->doubleSpinBox_fx_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
00077 connect(ui_->doubleSpinBox_fy_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
00078 connect(ui_->doubleSpinBox_cx_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
00079 connect(ui_->doubleSpinBox_cy_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
00080 connect(ui_->lineEdit_D_r, SIGNAL(textEdited(const QString &)), this, SLOT(updateSaveStatus()));
00081
00082 connect(ui_->spinBox_width, SIGNAL(valueChanged(int)), this, SLOT(updateSaveStatus()));
00083 connect(ui_->spinBox_height, SIGNAL(valueChanged(int)), this, SLOT(updateSaveStatus()));
00084
00085 connect(ui_->doubleSpinBox_baseline, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
00086 connect(ui_->lineEdit_RT, SIGNAL(textEdited(const QString &)), this, SLOT(updateSaveStatus()));
00087
00088 ui_->stackedWidget->setCurrentIndex(ui_->comboBox_advanced->currentIndex());
00089
00090 ui_->buttonBox->button(QDialogButtonBox::Save)->setEnabled(false);
00091
00092 updateStereoView();
00093 }
00094
00095 CreateSimpleCalibrationDialog::~CreateSimpleCalibrationDialog()
00096 {
00097 delete ui_;
00098 }
00099
00100 void CreateSimpleCalibrationDialog::updateStereoView()
00101 {
00102 bool checked = ui_->checkBox_stereo->isChecked();
00103 ui_->doubleSpinBox_baseline->setVisible(checked);
00104 ui_->label_baseline->setVisible(checked);
00105 ui_->label_right->setVisible(checked);
00106 ui_->doubleSpinBox_fx_r->setVisible(checked);
00107 ui_->doubleSpinBox_fy_r->setVisible(checked);
00108 ui_->doubleSpinBox_cx_r->setVisible(checked);
00109 ui_->doubleSpinBox_cy_r->setVisible(checked);
00110 ui_->lineEdit_D_r->setVisible(checked);
00111 ui_->groupBox_stereo_extrinsics->setVisible(ui_->comboBox_advanced->currentIndex() == 1 && checked);
00112 ui_->label_left->setVisible(checked);
00113 }
00114
00115 void CreateSimpleCalibrationDialog::updateSaveStatus()
00116 {
00117 bool valid = false;
00118 if(ui_->comboBox_advanced->currentIndex() == 0 &&
00119 ui_->doubleSpinBox_fx->value() > 0.0 &&
00120 ui_->doubleSpinBox_fy->value() > 0.0 &&
00121 ui_->spinBox_width->value() > 0 &&
00122 ui_->spinBox_height->value() > 0 &&
00123 (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_baseline->value() != 0.0))
00124 {
00125
00126 valid = true;
00127 }
00128 else if(ui_->comboBox_advanced->currentIndex() == 1 &&
00129 ui_->doubleSpinBox_fx_l->value() > 0.0 &&
00130 ui_->doubleSpinBox_fy_l->value() > 0.0 &&
00131 ui_->doubleSpinBox_cx_l->value() > 0.0 &&
00132 ui_->doubleSpinBox_cy_l->value() > 0.0 &&
00133 (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_fx_r->value() > 0.0) &&
00134 (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_fy_r->value() > 0.0) &&
00135 (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_cx_r->value() > 0.0) &&
00136 (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_cy_r->value() > 0.0) &&
00137 ui_->spinBox_width->value() > 0 &&
00138 ui_->spinBox_height->value() > 0 &&
00139 !ui_->lineEdit_D_l->text().isEmpty() &&
00140 (!ui_->checkBox_stereo->isChecked() || !ui_->lineEdit_D_r->text().isEmpty()) &&
00141 (!ui_->checkBox_stereo->isChecked() || !ui_->lineEdit_RT->text().isEmpty()))
00142 {
00143
00144 QStringList distorsionsStrListL = ui_->lineEdit_D_l->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().split(' ');
00145 QStringList distorsionsStrListR = ui_->lineEdit_D_r->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().split(' ');
00146 std::string RT = ui_->lineEdit_RT->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().toStdString();
00147
00148 if((distorsionsStrListL.size() == 4 || distorsionsStrListL.size() == 5 || distorsionsStrListL.size() == 8) &&
00149 (!ui_->checkBox_stereo->isChecked() || (distorsionsStrListR.size() == 4 || distorsionsStrListR.size() == 5 || distorsionsStrListR.size() == 8)) &&
00150 (!ui_->checkBox_stereo->isChecked() || (!RT.empty() && Transform::canParseString(RT))))
00151 {
00152 valid = true;
00153 }
00154 }
00155 ui_->buttonBox->button(QDialogButtonBox::Save)->setEnabled(valid);
00156 }
00157
00158 void CreateSimpleCalibrationDialog::saveCalibration()
00159 {
00160 QString filePath = QFileDialog::getSaveFileName(this, tr("Save"), savingFolder_+"/"+cameraName_+".yaml", "*.yaml");
00161 QString name = QFileInfo(filePath).baseName();
00162 QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
00163 if(!filePath.isEmpty())
00164 {
00165 cameraName_ = name;
00166 CameraModel modelLeft;
00167 float width = ui_->spinBox_width->value();
00168 float height = ui_->spinBox_height->value();
00169 if(ui_->comboBox_advanced->currentIndex() == 0)
00170 {
00171
00172 modelLeft = CameraModel(
00173 name.toStdString(),
00174 ui_->doubleSpinBox_fx->value(),
00175 ui_->doubleSpinBox_fy->value(),
00176 ui_->doubleSpinBox_cx->value(),
00177 ui_->doubleSpinBox_cy->value(),
00178 Transform::getIdentity(),
00179 0,
00180 cv::Size(width, height));
00181 UASSERT(modelLeft.isValidForProjection());
00182 }
00183 else
00184 {
00185
00186 cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
00187 K.at<double>(0,0) = ui_->doubleSpinBox_fx_l->value();
00188 K.at<double>(1,1) = ui_->doubleSpinBox_fy_l->value();
00189 K.at<double>(0,2) = ui_->doubleSpinBox_cx_l->value();
00190 K.at<double>(1,2) = ui_->doubleSpinBox_cy_l->value();
00191 cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
00192 cv::Mat P = cv::Mat::zeros(3, 4, CV_64FC1);
00193 K.copyTo(cv::Mat(P, cv::Range(0,3), cv::Range(0,3)));
00194
00195 QStringList distorsionCoeffs = ui_->lineEdit_D_l->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().split(' ');
00196 UASSERT(distorsionCoeffs.size() == 4 || distorsionCoeffs.size() == 5 || distorsionCoeffs.size() == 8);
00197 cv::Mat D = cv::Mat::zeros(1, distorsionCoeffs.size(), CV_64FC1);
00198 bool ok;
00199 for(int i=0; i<distorsionCoeffs.size(); ++i)
00200 {
00201 D.at<double>(i) = distorsionCoeffs.at(i).toDouble(&ok);
00202 if(!ok)
00203 {
00204 QMessageBox::warning(this, tr("Save"), tr("Error parsing left distortion coefficients \"%1\".").arg(ui_->lineEdit_D_l->text()));
00205 return;
00206 }
00207 }
00208
00209 modelLeft = CameraModel(name.toStdString(), cv::Size(width,height), K, D, R, P);
00210 UASSERT(modelLeft.isValidForRectification());
00211 }
00212
00213 if(ui_->checkBox_stereo->isChecked())
00214 {
00215 StereoCameraModel stereoModel;
00216 if(ui_->comboBox_advanced->currentIndex() == 0)
00217 {
00218 CameraModel modelRight(
00219 name.toStdString(),
00220 ui_->doubleSpinBox_fx->value(),
00221 ui_->doubleSpinBox_fy->value(),
00222 ui_->doubleSpinBox_cx->value(),
00223 ui_->doubleSpinBox_cy->value(),
00224 Transform::getIdentity(),
00225 ui_->doubleSpinBox_baseline->value()*-ui_->doubleSpinBox_fx->value(),
00226 cv::Size(width, height));
00227 UASSERT(modelRight.isValidForProjection());
00228 stereoModel = StereoCameraModel(name.toStdString(), modelLeft, modelRight, Transform());
00229 UASSERT(stereoModel.isValidForProjection());
00230 }
00231 else if(ui_->comboBox_advanced->currentIndex() == 1)
00232 {
00233 CameraModel modelRight;
00234 cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
00235 K.at<double>(0,0) = ui_->doubleSpinBox_fx_r->value();
00236 K.at<double>(1,1) = ui_->doubleSpinBox_fy_r->value();
00237 K.at<double>(0,2) = ui_->doubleSpinBox_cx_r->value();
00238 K.at<double>(1,2) = ui_->doubleSpinBox_cy_r->value();
00239 cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
00240 cv::Mat P = cv::Mat::zeros(3, 4, CV_64FC1);
00241 K.copyTo(cv::Mat(P, cv::Range(0,3), cv::Range(0,3)));
00242
00243 QStringList distorsionCoeffs = ui_->lineEdit_D_r->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().split(' ');
00244 UASSERT(distorsionCoeffs.size() == 4 || distorsionCoeffs.size() == 5 || distorsionCoeffs.size() == 8);
00245 cv::Mat D = cv::Mat::zeros(1, distorsionCoeffs.size(), CV_64FC1);
00246 bool ok;
00247 for(int i=0; i<distorsionCoeffs.size(); ++i)
00248 {
00249 D.at<double>(i) = distorsionCoeffs.at(i).toDouble(&ok);
00250 if(!ok)
00251 {
00252 QMessageBox::warning(this, tr("Save"), tr("Error parsing right distortion coefficients \"%1\".").arg(ui_->lineEdit_D_r->text()));
00253 return;
00254 }
00255 }
00256
00257 modelRight = CameraModel(name.toStdString(), cv::Size(width,height), K, D, R, P);
00258 UASSERT(modelRight.isValidForRectification());
00259
00260 UASSERT(Transform::canParseString(ui_->lineEdit_RT->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().toStdString()));
00261 stereoModel = StereoCameraModel(name.toStdString(), modelLeft, modelRight, Transform::fromString(ui_->lineEdit_RT->text().remove('[').remove(']').replace(',',' ').replace(';',' ').trimmed().toStdString()));
00262 if(stereoModel.baseline() < 0)
00263 {
00264 QMessageBox::warning(this, tr("Save"), tr("Error parsing the extrinsics \"%1\", resulting baseline (%f) is negative!").arg(ui_->lineEdit_RT->text()).arg(stereoModel.baseline()));
00265 return;
00266 }
00267 UASSERT(stereoModel.isValidForRectification());
00268 }
00269
00270 std::string base = (dir+QDir::separator()+name).toStdString();
00271 std::string leftPath = base+"_left.yaml";
00272 std::string rightPath = base+"_right.yaml";
00273
00274 if(stereoModel.save(dir.toStdString(), ui_->comboBox_advanced->currentIndex() != 1))
00275 {
00276 if(ui_->comboBox_advanced->currentIndex() == 0)
00277 {
00278 QMessageBox::information(this, tr("Save"), tr("Calibration files saved:\n \"%1\"\n \"%2\".").
00279 arg(leftPath.c_str()).arg(rightPath.c_str()));
00280 }
00281 else
00282 {
00283 std::string posePath = base+"_pose.yaml";
00284 QMessageBox::information(this, tr("Save"), tr("Calibration files saved:\n \"%1\"\n \"%2\"\n \"%3\".").
00285 arg(leftPath.c_str()).arg(rightPath.c_str()).arg(posePath.c_str()));
00286 }
00287 this->accept();
00288 }
00289 else
00290 {
00291 QMessageBox::warning(this, tr("Save"), tr("Error saving \"%1\" and \"%2\"").arg(leftPath.c_str()).arg(rightPath.c_str()));
00292 }
00293 }
00294 else
00295 {
00296 if(modelLeft.save(dir.toStdString()))
00297 {
00298 QMessageBox::information(this, tr("Save"), tr("Calibration file saved to \"%1\".").arg(filePath));
00299 this->accept();
00300 }
00301 else
00302 {
00303 QMessageBox::warning(this, tr("Save"), tr("Error saving \"%1\"").arg(filePath));
00304 }
00305 }
00306 }
00307 }
00308
00309 }