CreateSimpleCalibrationDialog.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/CreateSimpleCalibrationDialog.h"
29 #include "ui_createSimpleCalibrationDialog.h"
30 
31 #include "rtabmap/core/CameraModel.h"
32 #include "rtabmap/core/StereoCameraModel.h"
33 #include "rtabmap/utilite/ULogger.h"
34 
35 #include <QFileDialog>
36 #include <QPushButton>
37 #include <QMessageBox>
38 
39 namespace rtabmap {
40 
41 CreateSimpleCalibrationDialog::CreateSimpleCalibrationDialog(
42  const QString & savingFolder,
43  const QString & cameraName,
44  QWidget * parent) :
45  QDialog(parent),
46  savingFolder_(savingFolder),
47  cameraName_(cameraName)
48 {
49  if(cameraName_.isEmpty())
50  {
51  cameraName_ = "calib";
52  }
53 
54  ui_ = new Ui_createSimpleCalibrationDialog();
55  ui_->setupUi(this);
56 
57  connect(ui_->buttonBox->button(QDialogButtonBox::Save), SIGNAL(clicked()), this, SLOT(saveCalibration()));
58  connect(ui_->buttonBox, SIGNAL(rejected()), this, SLOT(reject()));
59 
60  connect(ui_->comboBox_advanced, SIGNAL(currentIndexChanged(int)), ui_->stackedWidget, SLOT(setCurrentIndex(int)));
61 
62  connect(ui_->checkBox_stereo, SIGNAL(stateChanged(int)), this, SLOT(updateStereoView()));
63  connect(ui_->comboBox_advanced, SIGNAL(currentIndexChanged(int)), this, SLOT(updateStereoView()));
64  connect(ui_->checkBox_stereo, SIGNAL(stateChanged(int)), this, SLOT(updateSaveStatus()));
65  connect(ui_->comboBox_advanced, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSaveStatus()));
66 
67  connect(ui_->doubleSpinBox_fx, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
68  connect(ui_->doubleSpinBox_fy, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
69 
70  connect(ui_->doubleSpinBox_fx_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
71  connect(ui_->doubleSpinBox_fy_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
72  connect(ui_->doubleSpinBox_cx_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
73  connect(ui_->doubleSpinBox_cy_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
74  connect(ui_->lineEdit_D_l, SIGNAL(textEdited(const QString &)), this, SLOT(updateSaveStatus()));
75 
76  connect(ui_->doubleSpinBox_fx_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
77  connect(ui_->doubleSpinBox_fy_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
78  connect(ui_->doubleSpinBox_cx_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
79  connect(ui_->doubleSpinBox_cy_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
80  connect(ui_->lineEdit_D_r, SIGNAL(textEdited(const QString &)), this, SLOT(updateSaveStatus()));
81 
82  connect(ui_->spinBox_width, SIGNAL(valueChanged(int)), this, SLOT(updateSaveStatus()));
83  connect(ui_->spinBox_height, SIGNAL(valueChanged(int)), this, SLOT(updateSaveStatus()));
84 
85  connect(ui_->doubleSpinBox_baseline, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
86  connect(ui_->lineEdit_RT, SIGNAL(textEdited(const QString &)), this, SLOT(updateSaveStatus()));
87 
88  ui_->stackedWidget->setCurrentIndex(ui_->comboBox_advanced->currentIndex());
89 
90  ui_->buttonBox->button(QDialogButtonBox::Save)->setEnabled(false);
91 
92  updateStereoView();
93 }
94 
95 CreateSimpleCalibrationDialog::~CreateSimpleCalibrationDialog()
96 {
97  delete ui_;
98 }
99 
100 void CreateSimpleCalibrationDialog::updateStereoView()
101 {
102  bool checked = ui_->checkBox_stereo->isChecked();
103  ui_->doubleSpinBox_baseline->setVisible(checked);
104  ui_->label_baseline->setVisible(checked);
105  ui_->label_right->setVisible(checked);
106  ui_->doubleSpinBox_fx_r->setVisible(checked);
107  ui_->doubleSpinBox_fy_r->setVisible(checked);
108  ui_->doubleSpinBox_cx_r->setVisible(checked);
109  ui_->doubleSpinBox_cy_r->setVisible(checked);
110  ui_->lineEdit_D_r->setVisible(checked);
111  ui_->groupBox_stereo_extrinsics->setVisible(ui_->comboBox_advanced->currentIndex() == 1 && checked);
112  ui_->label_left->setVisible(checked);
113 }
114 
115 void CreateSimpleCalibrationDialog::updateSaveStatus()
116 {
117  bool valid = false;
118  if(ui_->comboBox_advanced->currentIndex() == 0 &&
119  ui_->doubleSpinBox_fx->value() > 0.0 &&
120  ui_->doubleSpinBox_fy->value() > 0.0 &&
121  ui_->spinBox_width->value() > 0 &&
122  ui_->spinBox_height->value() > 0 &&
123  (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_baseline->value() != 0.0))
124  {
125  // basic
126  valid = true;
127  }
128  else if(ui_->comboBox_advanced->currentIndex() == 1 &&
129  ui_->doubleSpinBox_fx_l->value() > 0.0 &&
130  ui_->doubleSpinBox_fy_l->value() > 0.0 &&
131  ui_->doubleSpinBox_cx_l->value() > 0.0 &&
132  ui_->doubleSpinBox_cy_l->value() > 0.0 &&
133  (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_fx_r->value() > 0.0) &&
134  (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_fy_r->value() > 0.0) &&
135  (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_cx_r->value() > 0.0) &&
136  (!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_cy_r->value() > 0.0) &&
137  ui_->spinBox_width->value() > 0 &&
138  ui_->spinBox_height->value() > 0 &&
139  !ui_->lineEdit_D_l->text().isEmpty() &&
140  (!ui_->checkBox_stereo->isChecked() || !ui_->lineEdit_D_r->text().isEmpty()) &&
141  (!ui_->checkBox_stereo->isChecked() || !ui_->lineEdit_RT->text().isEmpty()))
142  {
143  //advanced
144  QStringList distorsionsStrListL = ui_->lineEdit_D_l->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().split(' ');
145  QStringList distorsionsStrListR = ui_->lineEdit_D_r->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().split(' ');
146  std::string RT = ui_->lineEdit_RT->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().toStdString();
147 
148  if((distorsionsStrListL.size() == 4 || distorsionsStrListL.size() == 5 || distorsionsStrListL.size() == 8) &&
149  (!ui_->checkBox_stereo->isChecked() || (distorsionsStrListR.size() == 4 || distorsionsStrListR.size() == 5 || distorsionsStrListR.size() == 8)) &&
150  (!ui_->checkBox_stereo->isChecked() || (!RT.empty() && Transform::canParseString(RT))))
151  {
152  valid = true;
153  }
154  }
155  ui_->buttonBox->button(QDialogButtonBox::Save)->setEnabled(valid);
156 }
157 
158 void CreateSimpleCalibrationDialog::saveCalibration()
159 {
160  QString filePath = QFileDialog::getSaveFileName(this, tr("Save"), savingFolder_+"/"+cameraName_+".yaml", "*.yaml");
161  QString name = QFileInfo(filePath).baseName();
162  QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
163  if(!filePath.isEmpty())
164  {
165  cameraName_ = name;
166  CameraModel modelLeft;
167  float width = ui_->spinBox_width->value();
168  float height = ui_->spinBox_height->value();
169  if(ui_->comboBox_advanced->currentIndex() == 0)
170  {
171  //basic
172  modelLeft = CameraModel(
173  name.toStdString(),
174  ui_->doubleSpinBox_fx->value(),
175  ui_->doubleSpinBox_fy->value(),
176  ui_->doubleSpinBox_cx->value(),
177  ui_->doubleSpinBox_cy->value(),
178  CameraModel::opticalRotation(),
179  0,
180  cv::Size(width, height));
181  UASSERT(modelLeft.isValidForProjection());
182  }
183  else
184  {
185  //advanced
186  cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
187  K.at<double>(0,0) = ui_->doubleSpinBox_fx_l->value();
188  K.at<double>(1,1) = ui_->doubleSpinBox_fy_l->value();
189  K.at<double>(0,2) = ui_->doubleSpinBox_cx_l->value();
190  K.at<double>(1,2) = ui_->doubleSpinBox_cy_l->value();
191  cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
192  cv::Mat P = cv::Mat::zeros(3, 4, CV_64FC1);
193  K.copyTo(cv::Mat(P, cv::Range(0,3), cv::Range(0,3)));
194 
195  QStringList distorsionCoeffs = ui_->lineEdit_D_l->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().split(' ');
196  UASSERT(distorsionCoeffs.size() == 4 || distorsionCoeffs.size() == 5 || distorsionCoeffs.size() == 8);
197  cv::Mat D = cv::Mat::zeros(1, distorsionCoeffs.size(), CV_64FC1);
198  bool ok;
199  for(int i=0; i<distorsionCoeffs.size(); ++i)
200  {
201  D.at<double>(i) = distorsionCoeffs.at(i).toDouble(&ok);
202  if(!ok)
203  {
204  QMessageBox::warning(this, tr("Save"), tr("Error parsing left distortion coefficients \"%1\".").arg(ui_->lineEdit_D_l->text()));
205  return;
206  }
207  }
208 
209  modelLeft = CameraModel(name.toStdString(), cv::Size(width,height), K, D, R, P);
210  UASSERT(modelLeft.isValidForRectification());
211  }
212 
213  if(ui_->checkBox_stereo->isChecked())
214  {
215  StereoCameraModel stereoModel;
216  if(ui_->comboBox_advanced->currentIndex() == 0)
217  {
218  CameraModel modelRight(
219  name.toStdString(),
220  ui_->doubleSpinBox_fx->value(),
221  ui_->doubleSpinBox_fy->value(),
222  ui_->doubleSpinBox_cx->value(),
223  ui_->doubleSpinBox_cy->value(),
224  Transform::getIdentity(),
225  ui_->doubleSpinBox_baseline->value()*-ui_->doubleSpinBox_fx->value(),
226  cv::Size(width, height));
227  UASSERT(modelRight.isValidForProjection());
228  stereoModel = StereoCameraModel(name.toStdString(), modelLeft, modelRight, Transform());
229  UASSERT(stereoModel.isValidForProjection());
230  }
231  else if(ui_->comboBox_advanced->currentIndex() == 1)
232  {
233  CameraModel modelRight;
234  cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
235  K.at<double>(0,0) = ui_->doubleSpinBox_fx_r->value();
236  K.at<double>(1,1) = ui_->doubleSpinBox_fy_r->value();
237  K.at<double>(0,2) = ui_->doubleSpinBox_cx_r->value();
238  K.at<double>(1,2) = ui_->doubleSpinBox_cy_r->value();
239  cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
240  cv::Mat P = cv::Mat::zeros(3, 4, CV_64FC1);
241  K.copyTo(cv::Mat(P, cv::Range(0,3), cv::Range(0,3)));
242 
243  QStringList distorsionCoeffs = ui_->lineEdit_D_r->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().split(' ');
244  UASSERT(distorsionCoeffs.size() == 4 || distorsionCoeffs.size() == 5 || distorsionCoeffs.size() == 8);
245  cv::Mat D = cv::Mat::zeros(1, distorsionCoeffs.size(), CV_64FC1);
246  bool ok;
247  for(int i=0; i<distorsionCoeffs.size(); ++i)
248  {
249  D.at<double>(i) = distorsionCoeffs.at(i).toDouble(&ok);
250  if(!ok)
251  {
252  QMessageBox::warning(this, tr("Save"), tr("Error parsing right distortion coefficients \"%1\".").arg(ui_->lineEdit_D_r->text()));
253  return;
254  }
255  }
256 
257  modelRight = CameraModel(name.toStdString(), cv::Size(width,height), K, D, R, P);
258  UASSERT(modelRight.isValidForRectification());
259 
260  UASSERT(Transform::canParseString(ui_->lineEdit_RT->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().toStdString()));
261  stereoModel = StereoCameraModel(name.toStdString(), modelLeft, modelRight, Transform::fromString(ui_->lineEdit_RT->text().remove('[').remove(']').replace(',',' ').replace(';',' ').trimmed().toStdString()));
262  if(stereoModel.baseline() < 0)
263  {
264  QMessageBox::warning(this, tr("Save"), tr("Error parsing the extrinsics \"%1\", resulting baseline (%f) is negative!").arg(ui_->lineEdit_RT->text()).arg(stereoModel.baseline()));
265  return;
266  }
267  UASSERT(stereoModel.isValidForRectification());
268  }
269 
270  std::string base = (dir+QDir::separator()+name).toStdString();
271  std::string leftPath = base+"_left.yaml";
272  std::string rightPath = base+"_right.yaml";
273 
274  if(stereoModel.save(dir.toStdString(), ui_->comboBox_advanced->currentIndex() != 1))
275  {
276  if(ui_->comboBox_advanced->currentIndex() == 0)
277  {
278  QMessageBox::information(this, tr("Save"), tr("Calibration files saved:\n \"%1\"\n \"%2\".").
279  arg(leftPath.c_str()).arg(rightPath.c_str()));
280  }
281  else
282  {
283  std::string posePath = base+"_pose.yaml";
284  QMessageBox::information(this, tr("Save"), tr("Calibration files saved:\n \"%1\"\n \"%2\"\n \"%3\".").
285  arg(leftPath.c_str()).arg(rightPath.c_str()).arg(posePath.c_str()));
286  }
287  this->accept();
288  }
289  else
290  {
291  QMessageBox::warning(this, tr("Save"), tr("Error saving \"%1\" and \"%2\"").arg(leftPath.c_str()).arg(rightPath.c_str()));
292  }
293  }
294  else
295  {
296  if(modelLeft.save(dir.toStdString()))
297  {
298  QMessageBox::information(this, tr("Save"), tr("Calibration file saved to \"%1\".").arg(filePath));
299  this->accept();
300  }
301  else
302  {
303  QMessageBox::warning(this, tr("Save"), tr("Error saving \"%1\"").arg(filePath));
304  }
305  }
306  }
307 }
308 
309 }
CreateSimpleCalibrationDialog.h
name
rtabmap::StereoCameraModel
Definition: StereoCameraModel.h:35
base
D
MatrixXcd D
arg::arg
constexpr arg(const char *name=nullptr)
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:411
rtabmap::CameraModel
Definition: CameraModel.h:38
rtabmap::CreateSimpleCalibrationDialog::saveCalibration
void saveCalibration()
Definition: CreateSimpleCalibrationDialog.cpp:158
rtabmap::CreateSimpleCalibrationDialog::updateSaveStatus
void updateSaveStatus()
Definition: CreateSimpleCalibrationDialog.cpp:115
rtabmap::StereoCameraModel::baseline
double baseline() const
Definition: StereoCameraModel.h:104
rtabmap::Transform::fromString
static Transform fromString(const std::string &string)
Definition: Transform.cpp:475
rtabmap::CameraModel::save
bool save(const std::string &directory) const
Definition: CameraModel.cpp:388
arg
rtabmap::CameraModel::isValidForProjection
bool isValidForProjection() const
Definition: CameraModel.h:87
rtabmap::CreateSimpleCalibrationDialog::ui_
Ui_createSimpleCalibrationDialog * ui_
Definition: CreateSimpleCalibrationDialog.h:61
UASSERT
#define UASSERT(condition)
StereoCameraModel.h
rtabmap::StereoCameraModel::isValidForProjection
bool isValidForProjection() const
Definition: StereoCameraModel.h:85
rtabmap::StereoCameraModel::save
bool save(const std::string &directory, bool ignoreStereoTransform=true) const
Definition: StereoCameraModel.cpp:334
rtabmap::CreateSimpleCalibrationDialog::savingFolder_
QString savingFolder_
Definition: CreateSimpleCalibrationDialog.h:62
rtabmap::CreateSimpleCalibrationDialog::cameraName_
QString cameraName_
Definition: CreateSimpleCalibrationDialog.h:63
rtabmap::CameraModel::opticalRotation
static Transform opticalRotation()
Definition: CameraModel.h:45
rtabmap::CameraModel::isValidForRectification
bool isValidForRectification() const
Definition: CameraModel.h:89
K
K
rtabmap::StereoCameraModel::isValidForRectification
bool isValidForRectification() const
Definition: StereoCameraModel.h:86
ULogger.h
ULogger class and convenient macros.
rtabmap::Transform
Definition: Transform.h:41
PointMatcherSupport::Parametrizable
rtabmap::CreateSimpleCalibrationDialog::~CreateSimpleCalibrationDialog
virtual ~CreateSimpleCalibrationDialog()
Definition: CreateSimpleCalibrationDialog.cpp:95
rtabmap::CreateSimpleCalibrationDialog::CreateSimpleCalibrationDialog
CreateSimpleCalibrationDialog(const QString &savingFolder=".", const QString &cameraName="", QWidget *parent=0)
Definition: CreateSimpleCalibrationDialog.cpp:41
CameraModel.h
rtabmap::CreateSimpleCalibrationDialog::updateStereoView
void updateStereoView()
Definition: CreateSimpleCalibrationDialog.cpp:100
rtabmap
Definition: CameraARCore.cpp:35
i
int i
rtabmap::Transform::canParseString
static bool canParseString(const std::string &string)
Definition: Transform.cpp:524
R
R

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