CalibrationDialog.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/CalibrationDialog.h"
29 #include "ui_calibrationDialog.h"
30 
31 #include <opencv2/core/core.hpp>
32 #include <opencv2/imgproc/imgproc.hpp>
33 #include <opencv2/imgproc/imgproc_c.h>
34 #include <opencv2/calib3d/calib3d.hpp>
35 #if CV_MAJOR_VERSION >= 3
36 #include <opencv2/calib3d/calib3d_c.h>
37 #endif
38 #include <opencv2/highgui/highgui.hpp>
39 #if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
40 #include <rtabmap/core/stereo/stereoRectifyFisheye.h>
41 #endif
42 
43 #include <QFileDialog>
44 #include <QMessageBox>
45 #include <QCloseEvent>
46 #include <QDateTime>
47 #include <QTextStream>
48 #include <QtGlobal>
49 
50 #if QT_VERSION >= QT_VERSION_CHECK(5,14,0)
51 #define ENDL Qt::endl
52 #else
53 #define ENDL endl
54 #endif
55 
56 #include <rtabmap/core/SensorEvent.h>
57 #include <rtabmap/utilite/UCv2Qt.h>
58 
59 #include <rtabmap/utilite/ULogger.h>
60 
61 #ifdef HAVE_CHARUCO
62 #define kArucoDictNameSize 21
63 static const char * kArucoDictNames[kArucoDictNameSize] = {
64 "4X4_50",
65 "4X4_100",
66 "4X4_250",
67 "4X4_1000",
68 "5X5_50",
69 "5X5_100",
70 "5X5_250",
71 "5X5_1000",
72 "6X6_50",
73 "6X6_100",
74 "6X6_250",
75 "6X6_1000",
76 "7X7_50",
77 "7X7_100",
78 "7X7_250",
79 "7X7_1000",
80 "ARUCO_ORIGINAL",
81 "APRILTAG_16h5",
82 "APRILTAG_25h9",
83 "APRILTAG_36h10",
84 "APRILTAG_36h11"
85 };
86 #endif
87 
88 
89 namespace rtabmap {
90 
91 #define COUNT_MIN 70
92 
93 CalibrationDialog::CalibrationDialog(bool stereo, const QString & savingDirectory, bool switchImages, QWidget * parent) :
94  QDialog(parent),
95  stereo_(stereo),
96  leftSuffix_("left"),
97  rightSuffix_("right"),
98  savingDirectory_(savingDirectory),
99  processingData_(false),
100  savedCalibration_(false),
101  currentId_(0)
102 {
103  imagePoints_.resize(2);
104  objectPoints_.resize(2);
105  imageParams_.resize(2);
106  imageIds_.resize(2);
107  imageSize_.resize(2);
108  stereoImagePoints_.resize(2);
109  models_.resize(2);
110 
111  minIrs_.resize(2);
112  maxIrs_.resize(2);
113  minIrs_[0] = 0x0000;
114  maxIrs_[0] = 0x7fff;
115  minIrs_[1] = 0x0000;
116  maxIrs_[1] = 0x7fff;
117 
118  qRegisterMetaType<cv::Mat>("cv::Mat");
119 
120  ui_ = new Ui_calibrationDialog();
121  ui_->setupUi(this);
122 
123  connect(ui_->toolButton_generateBoard, SIGNAL(clicked()), this, SLOT(generateBoard()));
124  connect(ui_->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate()));
125  connect(ui_->pushButton_restart, SIGNAL(clicked()), this, SLOT(restart()));
126  connect(ui_->pushButton_save, SIGNAL(clicked()), this, SLOT(save()));
127  connect(ui_->checkBox_switchImages, SIGNAL(stateChanged(int)), this, SLOT(restart()));
128  connect(ui_->checkBox_unlock, SIGNAL(stateChanged(int)), SLOT(unlock()));
129 
130  connect(ui_->comboBox_board_type, SIGNAL(currentIndexChanged(int)), this, SLOT(setBoardType(int)));
131  connect(ui_->comboBox_marker_dictionary, SIGNAL(currentIndexChanged(int)), this, SLOT(setMarkerDictionary(int)));
132  connect(ui_->spinBox_boardWidth, SIGNAL(valueChanged(int)), this, SLOT(setBoardWidth(int)));
133  connect(ui_->spinBox_boardHeight, SIGNAL(valueChanged(int)), this, SLOT(setBoardHeight(int)));
134  connect(ui_->doubleSpinBox_squareSize, SIGNAL(valueChanged(double)), this, SLOT(setSquareSize(double)));
135  connect(ui_->doubleSpinBox_markerLength, SIGNAL(valueChanged(double)), this, SLOT(setMarkerLength(double)));
136  connect(ui_->doubleSpinBox_subpixel_error, SIGNAL(valueChanged(double)), this, SLOT(setSubpixelMaxError(double)));
137  connect(ui_->checkBox_subpixel_refinement, SIGNAL(toggled(bool)), this, SLOT(setSubpixelRefinement(bool)));
138  connect(ui_->checkBox_saveCalibrationData, SIGNAL(toggled(bool)), this, SLOT(setCalibrationDataSaved(bool)));
139  connect(ui_->doubleSpinBox_stereoBaseline, SIGNAL(valueChanged(double)), this, SLOT(setExpectedStereoBaseline(double)));
140  connect(ui_->spinBox_maxScale, SIGNAL(valueChanged(int)), this, SLOT(setMaxScale(int)));
141 
142  connect(ui_->buttonBox, SIGNAL(rejected()), this, SLOT(close()));
143 
144  ui_->image_view->setFocus();
145 
146  ui_->progressBar_count->setMaximum(COUNT_MIN);
147  ui_->progressBar_count->setFormat("%v");
148  ui_->progressBar_count_2->setMaximum(COUNT_MIN);
149  ui_->progressBar_count_2->setFormat("%v");
150 
151  ui_->radioButton_raw->setChecked(true);
152 
153  ui_->checkBox_switchImages->setChecked(switchImages);
154 
155  ui_->comboBox_calib_model->setCurrentIndex(1);
156 
157  this->setStereoMode(stereo_);
158 
159  timestamp_ = QDateTime::currentDateTime().toString("yyyyMMddhhmmss");
160 
161 #ifndef HAVE_CHARUCO
162  ui_->comboBox_board_type->setItemData(1, 0, Qt::UserRole - 1);
163  ui_->comboBox_board_type->setItemData(2, 0, Qt::UserRole - 1);
164 #elif CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION < 6)
165  ui_->comboBox_board_type->setItemData(2, 0, Qt::UserRole - 1);
166 #elif CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION < 8
167  ui_->comboBox_board_type->setItemData(1, 0, Qt::UserRole - 1);
168 #endif
169 }
170 
171 CalibrationDialog::~CalibrationDialog()
172 {
173  this->unregisterFromEventsManager();
174  delete ui_;
175 }
176 
177 void CalibrationDialog::saveSettings(QSettings & settings, const QString & group) const
178 {
179  if(!group.isEmpty())
180  {
181  settings.beginGroup(group);
182  }
183  settings.setValue("board_type", ui_->comboBox_board_type->currentIndex());
184  settings.setValue("board_width", ui_->spinBox_boardWidth->value());
185  settings.setValue("board_height", ui_->spinBox_boardHeight->value());
186  settings.setValue("board_square_size", ui_->doubleSpinBox_squareSize->value());
187  settings.setValue("marker_type", ui_->comboBox_marker_dictionary->currentIndex());
188  settings.setValue("marker_length", ui_->doubleSpinBox_markerLength->value());
189  settings.setValue("subpixel_refinement", ui_->checkBox_subpixel_refinement->isChecked());
190  settings.setValue("subpixel_max_error", ui_->doubleSpinBox_subpixel_error->value());
191  settings.setValue("calibration_data_saved", ui_->checkBox_saveCalibrationData->isChecked());
192  settings.setValue("max_scale", ui_->spinBox_maxScale->value());
193  settings.setValue("geometry", this->saveGeometry());
194  settings.setValue("calibration_model", ui_->comboBox_calib_model->currentIndex());
195 
196  if(!group.isEmpty())
197  {
198  settings.endGroup();
199  }
200 }
201 
202 void CalibrationDialog::loadSettings(QSettings & settings, const QString & group)
203 {
204  if(!group.isEmpty())
205  {
206  settings.beginGroup(group);
207  }
208  this->setBoardType(settings.value("board_type", ui_->comboBox_board_type->currentIndex()).toInt());
209  this->setBoardWidth(settings.value("board_width", ui_->spinBox_boardWidth->value()).toInt());
210  this->setBoardHeight(settings.value("board_height", ui_->spinBox_boardHeight->value()).toInt());
211  this->setSquareSize(settings.value("board_square_size", ui_->doubleSpinBox_squareSize->value()).toDouble());
212  this->setMarkerDictionary(settings.value("marker_type", ui_->comboBox_marker_dictionary->currentIndex()).toInt());
213  this->setMarkerLength(settings.value("marker_length", ui_->doubleSpinBox_markerLength->value()).toDouble());
214  this->setSubpixelRefinement(settings.value("subpixel_refinement", ui_->checkBox_subpixel_refinement->isChecked()).toBool());
215  this->setSubpixelMaxError(settings.value("subpixel_max_error", ui_->doubleSpinBox_subpixel_error->value()).toDouble());
216  this->setCalibrationDataSaved(settings.value("calibration_data_saved", ui_->checkBox_saveCalibrationData->isChecked()).toBool());
217  this->setMaxScale(settings.value("max_scale", ui_->spinBox_maxScale->value()).toDouble());
218  int model = settings.value("calibration_model", ui_->comboBox_calib_model->currentIndex()).toInt();
219  if(model == 0)
220  {
221  this->setFisheyeModel();
222  }
223  else if(model ==2)
224  {
225  this->setRationalModel();
226  }
227  else
228  {
229  this->setPlumbobModel();
230  }
231  QByteArray bytes = settings.value("geometry", QByteArray()).toByteArray();
232  if(!bytes.isEmpty())
233  {
234  this->restoreGeometry(bytes);
235  }
236  if(!group.isEmpty())
237  {
238  settings.endGroup();
239  }
240 }
241 
242 void CalibrationDialog::resetSettings()
243 {
244  this->setBoardType(0);
245  this->setBoardWidth(8);
246  this->setBoardHeight(6);
247  this->setSquareSize(0.033);
248  this->setMarkerLength(0.02475);
249 }
250 
251 cv::Mat drawChessboard(int squareSize, int boardWidth, int boardHeight, int borderSize)
252 {
253  int imageWidth = squareSize*boardWidth + borderSize;
254  int imageHeight = squareSize*boardHeight + borderSize;
255  cv::Mat chessboard(imageWidth, imageHeight, CV_8UC1, 255);
256  unsigned char color = 0;
257  for(int i=borderSize;i<imageHeight-borderSize; i=i+squareSize) {
258  color=~color;
259  for(int j=borderSize;j<imageWidth-borderSize;j=j+squareSize) {
260  cv::Mat roi=chessboard(cv::Rect(i,j,squareSize,squareSize));
261  roi.setTo(color);
262  color=~color;
263  }
264  }
265  return chessboard;
266 }
267 
268 void CalibrationDialog::generateBoard()
269 {
270  int squareSizeInPixels = 200;
271  cv::Mat image;
272  QString filename;
273  QTextStream stream(&filename);
274 #ifdef HAVE_CHARUCO
275  if(ui_->comboBox_board_type->currentIndex() >= 1 )
276  {
277  try {
278 #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
279  charucoBoard_->generateImage(
280  cv::Size(squareSizeInPixels*ui_->spinBox_boardWidth->value(),
281  squareSizeInPixels*ui_->spinBox_boardHeight->value()),
282  image,
283  squareSizeInPixels/4, 1);
284 #else
285  charucoBoard_->draw(
286  cv::Size(squareSizeInPixels*ui_->spinBox_boardWidth->value(),
287  squareSizeInPixels*ui_->spinBox_boardHeight->value()),
288  image,
289  squareSizeInPixels/4, 1);
290 #endif
291 
292  int arucoDict = ui_->comboBox_marker_dictionary->currentIndex();
293  stream << "charuco_" << (arucoDict<kArucoDictNameSize?kArucoDictNames[arucoDict]:"NA") << "_"
294  << ui_->spinBox_boardWidth->value() << "x" << ui_->spinBox_boardHeight->value()
295  << "_ratio" << float(ui_->doubleSpinBox_markerLength->value())/float(ui_->doubleSpinBox_squareSize->value());
296  }
297  catch(const cv::Exception & e)
298  {
299  UERROR("%f", e.what());
300  QMessageBox::critical(this, tr("Generating Board"),
301  tr("Cannot generate the board. Make sure the dictionary "
302  "selected is big enough for the board size. Error:\"%1\"").arg(e.what()));
303  return;
304  }
305  }
306  else
307 #endif
308  {
309  image = drawChessboard(
310  squareSizeInPixels,
311  ui_->spinBox_boardWidth->value(),
312  ui_->spinBox_boardHeight->value(),
313  squareSizeInPixels/4);
314 
315  stream << "/chessboard_" << ui_->spinBox_boardWidth->value() << "x" << ui_->spinBox_boardHeight->value();
316 
317  }
318  QString filePath = QFileDialog::getSaveFileName(this, tr("Save"), savingDirectory_+"/"+filename+".png", "*.png");
319  if(!filePath.isEmpty())
320  {
321  cv::imwrite(filePath.toStdString(), image);
322  }
323 }
324 
325 void CalibrationDialog::setCameraName(const QString & name)
326 {
327  cameraName_ = name;
328 }
329 
330 void CalibrationDialog::setProgressVisibility(bool visible)
331 {
332  ui_->groupBox_progress->setVisible(visible);
333 }
334 
335 void CalibrationDialog::setSwitchedImages(bool switched)
336 {
337  ui_->checkBox_switchImages->setChecked(switched);
338 }
339 
340 void CalibrationDialog::setFisheyeModel()
341 {
342  ui_->comboBox_calib_model->setCurrentIndex(0);
343 }
344 
345 void CalibrationDialog::setPlumbobModel()
346 {
347  ui_->comboBox_calib_model->setCurrentIndex(1);
348 }
349 
350 void CalibrationDialog::setRationalModel()
351 {
352  ui_->comboBox_calib_model->setCurrentIndex(2);
353 }
354 
355 void CalibrationDialog::setStereoMode(bool stereo, const QString & leftSuffix, const QString & rightSuffix)
356 {
357  leftSuffix_ = leftSuffix;
358  rightSuffix_ = rightSuffix;
359  this->restart();
360 
361  ui_->groupBox_progress->setVisible(true);
362  stereo_ = stereo;
363  ui_->progressBar_x_2->setVisible(stereo_);
364  ui_->progressBar_y_2->setVisible(stereo_);
365  ui_->progressBar_size_2->setVisible(stereo_);
366  ui_->progressBar_skew_2->setVisible(stereo_);
367  ui_->progressBar_count_2->setVisible(stereo_);
368  ui_->label_right->setVisible(stereo_);
369  ui_->image_view_2->setVisible(stereo_);
370  ui_->label_fx_2->setVisible(stereo_);
371  ui_->label_fy_2->setVisible(stereo_);
372  ui_->label_cx_2->setVisible(stereo_);
373  ui_->label_cy_2->setVisible(stereo_);
374  ui_->label_fovx_2->setVisible(stereo_);
375  ui_->label_fovy_2->setVisible(stereo_);
376  ui_->label_error_2->setVisible(stereo_);
377  ui_->label_baseline->setVisible(stereo_);
378  ui_->label_baseline_name->setVisible(stereo_);
379  ui_->label_stereoError->setVisible(stereo_);
380  ui_->lineEdit_K_2->setVisible(stereo_);
381  ui_->lineEdit_D_2->setVisible(stereo_);
382  ui_->lineEdit_R_2->setVisible(stereo_);
383  ui_->lineEdit_P_2->setVisible(stereo_);
384  ui_->radioButton_stereoRectified->setVisible(stereo_);
385  ui_->checkBox_switchImages->setVisible(stereo_);
386  ui_->doubleSpinBox_stereoBaseline->setVisible(stereo_);
387  ui_->label_stereoBaseline->setVisible(stereo_);
388 }
389 
390 int CalibrationDialog::boardWidth() const
391 {
392  return ui_->spinBox_boardWidth->value();
393 }
394 int CalibrationDialog::boardHeight() const
395 {
396  return ui_->spinBox_boardHeight->value();
397 }
398 double CalibrationDialog::squareSize() const
399 {
400  return ui_->doubleSpinBox_squareSize->value();
401 }
402 double CalibrationDialog::markerLength() const
403 {
404  return ui_->doubleSpinBox_markerLength->value();
405 }
406 
407 void CalibrationDialog::setBoardType(int type)
408 {
409  if(type != ui_->comboBox_board_type->currentIndex())
410  {
411  ui_->comboBox_board_type->setCurrentIndex(type);
412  }
413  this->restart();
414 }
415 
416 void CalibrationDialog::setMarkerDictionary(int dictionary)
417 {
418  if(dictionary != ui_->comboBox_marker_dictionary->currentIndex())
419  {
420  ui_->comboBox_marker_dictionary->setCurrentIndex(dictionary);
421  }
422  this->restart();
423 }
424 
425 void CalibrationDialog::setBoardWidth(int width)
426 {
427  if(width != ui_->spinBox_boardWidth->value())
428  {
429  ui_->spinBox_boardWidth->setValue(width);
430  }
431  this->restart();
432 }
433 
434 void CalibrationDialog::setBoardHeight(int height)
435 {
436  if(height != ui_->spinBox_boardHeight->value())
437  {
438  ui_->spinBox_boardHeight->setValue(height);
439  }
440  this->restart();
441 }
442 
443 void CalibrationDialog::setSquareSize(double size)
444 {
445  if(size != ui_->doubleSpinBox_squareSize->value())
446  {
447  ui_->doubleSpinBox_squareSize->setValue(size);
448  }
449  if(ui_->doubleSpinBox_markerLength->value() >= ui_->doubleSpinBox_squareSize->value())
450  {
451  if(ui_->comboBox_board_type->currentIndex()==0)
452  {
453  ui_->doubleSpinBox_markerLength->setValue(ui_->doubleSpinBox_squareSize->value()-0.000001);
454  }
455  else
456  {
457  UWARN("Marker length (%f) cannot be larger than square size (%f), setting square size to %f. Decrease marker length first.",
458  ui_->doubleSpinBox_markerLength->value(),
459  ui_->doubleSpinBox_squareSize->value(),
460  ui_->doubleSpinBox_squareSize->value()+0.000001);
461  ui_->doubleSpinBox_squareSize->setValue(ui_->doubleSpinBox_markerLength->value()+0.000001);
462  }
463  }
464  this->restart();
465 }
466 
467 void CalibrationDialog::setMarkerLength(double length)
468 {
469  if(length != ui_->doubleSpinBox_markerLength->value())
470  {
471  ui_->doubleSpinBox_markerLength->setValue(length);
472  }
473  if(ui_->doubleSpinBox_markerLength->value() >= ui_->doubleSpinBox_squareSize->value())
474  {
475  UWARN("Marker length (%f) cannot be larger than square size (%f), setting marker length to %f. Increase square size first.",
476  ui_->doubleSpinBox_markerLength->value(),
477  ui_->doubleSpinBox_squareSize->value(),
478  ui_->doubleSpinBox_markerLength->value()-0.000001);
479  ui_->doubleSpinBox_markerLength->setValue(ui_->doubleSpinBox_squareSize->value()-0.000001);
480  }
481  this->restart();
482 }
483 
484 void CalibrationDialog::setSubpixelRefinement(bool enabled)
485 {
486  if(enabled != ui_->checkBox_subpixel_refinement->isChecked())
487  {
488  ui_->checkBox_subpixel_refinement->setChecked(enabled);
489  }
490  this->restart();
491 }
492 
493 void CalibrationDialog::setSubpixelMaxError(double value)
494 {
495  if(value != ui_->doubleSpinBox_subpixel_error->value())
496  {
497  ui_->doubleSpinBox_subpixel_error->setValue(value);
498  }
499  this->restart();
500 }
501 
502 void CalibrationDialog::setCalibrationDataSaved(bool enabled)
503 {
504  if(enabled != ui_->checkBox_saveCalibrationData->isChecked())
505  {
506  ui_->checkBox_saveCalibrationData->setChecked(enabled);
507  }
508  this->restart();
509 }
510 
511 void CalibrationDialog::setExpectedStereoBaseline(double length)
512 {
513  if(length != ui_->doubleSpinBox_stereoBaseline->value())
514  {
515  ui_->doubleSpinBox_stereoBaseline->setValue(length);
516  }
517 }
518 
519 void CalibrationDialog::setMaxScale(int scale)
520 {
521  if(scale != ui_->spinBox_maxScale->value())
522  {
523  ui_->spinBox_maxScale->setValue(scale);
524  }
525 }
526 
527 void CalibrationDialog::closeEvent(QCloseEvent* event)
528 {
529  if(!savedCalibration_ && models_[0].isValidForRectification() &&
530  (!stereo_ ||
531  (stereoModel_.isValidForRectification() &&
532  (!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0))))
533  {
534  QMessageBox::StandardButton b = QMessageBox::question(this, tr("Save calibration?"),
535  tr("The camera is calibrated but you didn't "
536  "save the calibration, do you want to save it?"),
537  QMessageBox::Yes | QMessageBox::Ignore | QMessageBox::Cancel, QMessageBox::Yes);
538  event->ignore();
539  if(b == QMessageBox::Yes)
540  {
541  if(this->save())
542  {
543  event->accept();
544  }
545  }
546  else if(b == QMessageBox::Ignore)
547  {
548  event->accept();
549  }
550  }
551  else
552  {
553  event->accept();
554  }
555  if(event->isAccepted())
556  {
557  this->unregisterFromEventsManager();
558  }
559  cameraName_.clear();
560 }
561 
562 bool CalibrationDialog::handleEvent(UEvent * event)
563 {
564  if(!processingData_)
565  {
566  if(event->getClassName().compare("SensorEvent") == 0)
567  {
568  rtabmap::SensorEvent * e = (rtabmap::SensorEvent *)event;
569  if(e->getCode() == rtabmap::SensorEvent::kCodeData)
570  {
571  processingData_ = true;
572  QMetaObject::invokeMethod(this, "processImages",
573  Q_ARG(cv::Mat, e->data().imageRaw()),
574  Q_ARG(cv::Mat, e->data().depthOrRightRaw()),
575  Q_ARG(QString, QString(e->cameraName().c_str())));
576  }
577  }
578  }
579  return false;
580 }
581 
582 #ifdef HAVE_CHARUCO
583 void matchCharucoImagePoints(
584  const cv::aruco::CharucoBoard &board,
585  const std::vector< cv::Point2f > & detectedCorners,
586  const std::vector< int > & detectedIds,
587  std::vector< cv::Point3f > & objectPoints)
588 {
589  UASSERT(detectedIds.size() == detectedCorners.size());
590  objectPoints.clear();
591 
592 #if CV_MAJOR_VERSION < 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION < 7)
593  objectPoints.reserve(detectedIds.size());
594 
595  // look for detected markers that belong to the board and get their information
596  for(size_t i = 0; i < detectedIds.size(); i++) {
597  int pointId = detectedIds[i];
598  UASSERT(pointId >= 0 && pointId < (int)board.chessboardCorners.size());
599  objectPoints.push_back(board.chessboardCorners[pointId]);
600  }
601 #else
602  cv::Mat imgPts;
603  board.matchImagePoints(detectedCorners, detectedIds, objectPoints, imgPts);
604 #endif
605 }
606 #endif
607 
608 // Modified from original versoin in opencv_contrib to remove "id="
609 void drawDetectedCornersCharuco(cv::InputOutputArray image, cv::InputArray charucoCorners,
610  cv::InputArray charucoIds = cv::noArray(),
611  cv::Scalar cornerColor = cv::Scalar(255, 0, 0)) {
612 
613  CV_Assert(image.getMat().total() != 0 &&
614  (image.getMat().channels() == 1 || image.getMat().channels() == 3));
615  CV_Assert((charucoCorners.getMat().total() == charucoIds.getMat().total()) ||
616  charucoIds.getMat().total() == 0);
617 
618  unsigned int nCorners = (unsigned int)charucoCorners.getMat().total();
619  for(unsigned int i = 0; i < nCorners; i++) {
620  cv::Point2f corner = charucoCorners.getMat().at< cv::Point2f >(i);
621 
622  // draw first corner mark
623  cv::rectangle(image, corner - cv::Point2f(3, 3), corner + cv::Point2f(3, 3), cornerColor, 1, cv::LINE_AA);
624 
625  // draw ID
626  if(charucoIds.total() != 0) {
627  int id = charucoIds.getMat().at< int >(i);
628  std::stringstream s;
629  s << id;
630  cv::putText(image, s.str(), corner + cv::Point2f(5, -5), cv::FONT_HERSHEY_SIMPLEX, 0.5,
631  cornerColor, 2);
632  }
633  }
634 }
635 
636 void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat & imageRight, const QString & cameraName)
637 {
638  UDEBUG("Processing images");
639  processingData_ = true;
640  if(cameraName_.isEmpty() && !cameraName.isEmpty())
641  {
642  cameraName_ = cameraName;
643  }
644  else if(cameraName.isEmpty())
645  {
646  cameraName_ = "0000";
647  }
648 
649  if(ui_->label_serial->text().compare(cameraName_)!=0)
650  {
651  ui_->label_serial->setText(cameraName_);
652  }
653 
654  std::vector<cv::Mat> inputRawImages(2);
655  if(ui_->checkBox_switchImages->isChecked())
656  {
657  inputRawImages[0] = imageRight;
658  inputRawImages[1] = imageLeft;
659  }
660  else
661  {
662  inputRawImages[0] = imageLeft;
663  inputRawImages[1] = imageRight;
664  }
665 
666  std::vector<cv::Mat> images(2);
667  images[0] = inputRawImages[0];
668  images[1] = inputRawImages[1];
669  imageSize_[0] = images[0].size();
670  imageSize_[1] = images[1].size();
671 
672  bool boardFound[2] = {false};
673  bool boardAccepted[2] = {false};
674  bool readyToCalibrate[2] = {false};
675 
676  std::vector<std::vector<cv::Point2f> > pointBuf(2);
677  std::vector<std::vector<cv::Point3f> > objectBuf(2);
678  std::vector<std::vector< int > > pointIds(2);
679 
680  bool depthDetected = false;
681  for(int id=0; id<(stereo_?2:1); ++id)
682  {
683  cv::Mat viewGray;
684  if(!images[id].empty())
685  {
686  if(images[id].type() == CV_16UC1)
687  {
688  double min, max;
689  cv::minMaxLoc(images[id], &min, &max);
690  UDEBUG("Camera IR %d: min=%f max=%f", id, min, max);
691  if(minIrs_[id] == 0)
692  {
693  minIrs_[id] = min;
694  }
695  if(maxIrs_[id] == 0x7fff)
696  {
697  maxIrs_[id] = max;
698  }
699 
700  depthDetected = true;
701  //assume IR image: convert to gray scaled
702  const float factor = 255.0f / float((maxIrs_[id] - minIrs_[id]));
703  viewGray = cv::Mat(images[id].rows, images[id].cols, CV_8UC1);
704  for(int i=0; i<images[id].rows; ++i)
705  {
706  for(int j=0; j<images[id].cols; ++j)
707  {
708  viewGray.at<unsigned char>(i, j) = (unsigned char)std::min(float(std::max(images[id].at<unsigned short>(i,j) - minIrs_[id], 0)) * factor, 255.0f);
709  }
710  }
711  cvtColor(viewGray, images[id], cv::COLOR_GRAY2BGR); // convert to show detected points in color
712  }
713  else if(images[id].channels() == 3)
714  {
715  cvtColor(images[id], viewGray, cv::COLOR_BGR2GRAY);
716  }
717  else
718  {
719  viewGray = images[id];
720  cvtColor(viewGray, images[id], cv::COLOR_GRAY2BGR); // convert to show detected points in color
721  }
722  }
723  else
724  {
725  UERROR("Image %d is empty!! Should not!", id);
726  }
727 
728  minIrs_[id] = 0;
729  maxIrs_[id] = 0x7FFF;
730 
731  //Dot it only if not yet calibrated
732  if(!ui_->pushButton_save->isEnabled())
733  {
734  std::vector< int > markerIds;
735  std::vector< std::vector< cv::Point2f > > markerCorners;
736  cv::Size boardSize(ui_->spinBox_boardWidth->value(), ui_->spinBox_boardHeight->value());
737  if(!viewGray.empty())
738  {
739  int flags = CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE;
740 
741  if(!viewGray.empty())
742  {
743  int maxScale = ui_->spinBox_maxScale->value();
744  for( int scale = 1; scale <= maxScale; scale++ )
745  {
746  cv::Mat timg;
747  if( scale == 1 )
748  timg = viewGray;
749  else
750  cv::resize(viewGray, timg, cv::Size(), scale, scale, CV_INTER_CUBIC);
751 
752 #ifdef HAVE_CHARUCO
753  if(ui_->comboBox_board_type->currentIndex() >= 1 )
754  {
755  std::vector< std::vector< cv::Point2f > > rejected;
756  UASSERT(charucoBoard_.get());
757 
758  // detect markers
759  UDEBUG("Detecting aruco markers...");
760 #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
761  UASSERT(arucoDetector_.get());
762  arucoDetector_->detectMarkers(timg, markerCorners, markerIds, rejected);
763 #else
764  cv::aruco::detectMarkers(timg, markerDictionary_, markerCorners, markerIds, arucoDetectorParams_, rejected);
765 #endif
766  // refine strategy to detect more markers
767  UDEBUG("Refining aruco markers...");
768 #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
769  arucoDetector_->refineDetectedMarkers(timg, *charucoBoard_, markerCorners, markerIds, rejected);
770 #else
771  cv::aruco::refineDetectedMarkers(timg, charucoBoard_, markerCorners, markerIds, rejected);
772 #endif
773  // interpolate charuco corners
774  UDEBUG("Finding charuco corners (markers=%ld)...", markerCorners.size());
775  if(markerIds.size() > 0)
776  {
777  UASSERT(markerIds.size() == markerCorners.size());
778 #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
779  UASSERT(charucoDetector_.get());
780  charucoDetector_->detectBoard(timg, pointBuf[id], pointIds[id], markerCorners, markerIds);
781 #else
782  cv::aruco::interpolateCornersCharuco(markerCorners, markerIds, timg, charucoBoard_, pointBuf[id], pointIds[id], cv::noArray(), cv::noArray(), 1);
783 #endif
784  UDEBUG("Found %ld charuco corners (requires 12)", pointBuf[id].size());
785  if(pointBuf[id].size() >= 12) {
786  // Match image points
787  matchCharucoImagePoints(*charucoBoard_, pointBuf[id], pointIds[id], objectBuf[id]);
788  boardFound[id] = !objectBuf[id].empty() && objectBuf[id].size() == pointBuf[id].size();
789  }
790  }
791  }
792  else // standard checkerboard
793 #endif
794  {
795  boardFound[id] = cv::findChessboardCorners(timg, boardSize, pointBuf[id], flags);
796  objectBuf[id] = chessboardPoints_;
797  pointIds[id] = chessboardPointIds_;
798  }
799 
800  if(boardFound[id])
801  {
802  if( scale > 1 )
803  {
804  cv::Mat cornersMat(pointBuf[id]);
805  cornersMat *= 1./scale;
806  }
807 
808  break;
809  }
810  }
811  }
812  }
813 
814  if(boardFound[id]) // If done with success,
815  {
816  // refine corners?
817  std::vector<cv::Point2f> originalPoints = pointBuf[id];
818  std::vector<cv::Point2f> rejectedPoints;
819  std::vector<int> rejectedPointIds;
820  if(ui_->checkBox_subpixel_refinement->isChecked())
821  {
822  // improve the found corners' coordinate accuracy
823  float minSquareDistance = -1.0f;
824  for(unsigned int i=0; i<pointBuf[id].size()-1; ++i)
825  {
826  float d = cv::norm(pointBuf[id][i] - pointBuf[id][i+1]);
827  if(minSquareDistance == -1.0f || minSquareDistance > d)
828  {
829  minSquareDistance = d;
830  }
831  }
832  float ratio = ui_->comboBox_board_type->currentIndex() >= 1 ?6.0f:2.0f;
833  float radius = minSquareDistance==-1.0f?5.0f:(minSquareDistance/ratio);
834  cv::cornerSubPix( viewGray, pointBuf[id], cv::Size(radius, radius), cv::Size(-1,-1),
835  cv::TermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1 ));
836 
837  // Filter points that drifted to far (caused by reflection or bad subpixel gradient)
838  float threshold = ui_->doubleSpinBox_subpixel_error->value();
839  if(threshold>0)
840  {
841  std::vector<cv::Point3f> filteredObjectPts;
842  std::vector<cv::Point2f> filteredPoints;
843  std::vector<int> filteredPointIds;
844  for(size_t i=0; i<pointBuf[id].size(); ++i)
845  {
846  float d = cv::norm(pointBuf[id][i] - originalPoints[i]);
847  if(d<threshold)
848  {
849  filteredObjectPts.push_back(objectBuf[id][i]);
850  filteredPoints.push_back(pointBuf[id][i]);
851  filteredPointIds.push_back(pointIds[id][i]);
852  }
853  else
854  {
855  UWARN("Filtered point: subpix error for image=%d cam=%d pt=%d radius=%f: %f > %f", currentId_, id, pointIds[id][i], radius, d, threshold);
856  rejectedPoints.push_back(pointBuf[id][i]);
857  rejectedPointIds.push_back(pointIds[id][i]);
858  }
859  }
860  objectBuf[id] = filteredObjectPts;
861  pointBuf[id] = filteredPoints;
862  pointIds[id] = filteredPointIds;
863  }
864  }
865 
866  // Draw the corners.
867  images[id] = images[id].clone();
868 #ifdef HAVE_CHARUCO
869  if(ui_->comboBox_board_type->currentIndex() >= 1 ) {
870  if(markerIds.size() > 0)
871  cv::aruco::drawDetectedMarkers(images[id], markerCorners, cv::noArray(), cv::Scalar(255,0,0));
872  }
873 #endif
874  if(pointBuf[id].size() > 0)
875  drawDetectedCornersCharuco(images[id], pointBuf[id], pointIds[id], cv::Scalar(0,255,0)); // Accepted Green
876  if(rejectedPoints.size() > 0)
877  drawDetectedCornersCharuco(images[id], rejectedPoints, rejectedPointIds, cv::Scalar(0,0,255)); // Rejected Red
878 
879  if(pointBuf[id].size() < rejectedPoints.size())
880  {
881  // don't add if more than 50% of valid points were filtered
882  UWARN("Ignoring whole board of image %d cam=%d because too many points were filtered.", currentId_, id);
883  boardFound[id] = false;
884  }
885  else
886  {
887  std::vector<float> params(4,0);
888  getParams(originalPoints, boardSize, imageSize_[id], params[0], params[1], params[2], params[3]);
889  if(ui_->comboBox_board_type->currentIndex() >= 1 )
890  {
891  //params[2] = float(pointBuf[id].size()) / float(boardSize.width * boardSize.height); // number of markers seen
892  float area = getArea(markerCorners[markerCorners.size()/2], cv::Size(4,4)) * (boardSize.width*boardSize.height);
893  params[2] = std::sqrt(area / (imageSize_[id].width * imageSize_[id].height));
894  params[2] = params[2]>1?1:params[2];
895  params[3] = getSkew(markerCorners[markerCorners.size()/2]);
896  }
897 
898  bool addSample = true;
899  if(!ui_->checkBox_keep_all->isChecked())
900  {
901  for(unsigned int i=0; i<imageParams_[id].size(); ++i)
902  {
903  if(fabs(params[0] - imageParams_[id][i].at(0)) < (ui_->comboBox_board_type->currentIndex() >= 1 ?0.2:0.1)*ui_->doubleSpinBox_sample_factor->value() && // x
904  fabs(params[1] - imageParams_[id][i].at(1)) < (ui_->comboBox_board_type->currentIndex() >= 1 ?0.2:0.1)*ui_->doubleSpinBox_sample_factor->value() && // y
905  fabs(params[2] - imageParams_[id][i].at(2)) < 0.05*ui_->doubleSpinBox_sample_factor->value() && // size
906  (params[3]==0 || params[3]==1.0f || imageParams_[id][i].at(3) == 0 || imageParams_[id][i].at(3) == 1.0f || fabs(params[3] - imageParams_[id][i].at(3)) < 0.1*ui_->doubleSpinBox_sample_factor->value())) // skew
907  {
908  addSample = false;
909  break;
910  }
911  }
912  }
913  if(addSample)
914  {
915  boardAccepted[id] = true;
916  imageIds_[id].push_back(currentId_);
917  imagePoints_[id].push_back(pointBuf[id]);
918  imageParams_[id].push_back(params);
919  objectPoints_[id].push_back(objectBuf[id]);
920  UINFO("[%d] Added board %d, total=%d. (x=%f, y=%f, size=%f, skew=%f)", id, currentId_, (int)imagePoints_[id].size(), params[0], params[1], params[2], params[3]);
921 
922  // update statistics
923  std::vector<float> xRange(2, imageParams_[id][0].at(0));
924  std::vector<float> yRange(2, imageParams_[id][0].at(1));
925  std::vector<float> sizeRange(2, imageParams_[id][0].at(2));
926  std::vector<float> skewRange(2, imageParams_[id][0].at(3));
927  for(unsigned int i=1; i<imageParams_[id].size(); ++i)
928  {
929  xRange[0] = imageParams_[id][i].at(0) < xRange[0] ? imageParams_[id][i].at(0) : xRange[0];
930  xRange[1] = imageParams_[id][i].at(0) > xRange[1] ? imageParams_[id][i].at(0) : xRange[1];
931  yRange[0] = imageParams_[id][i].at(1) < yRange[0] ? imageParams_[id][i].at(1) : yRange[0];
932  yRange[1] = imageParams_[id][i].at(1) > yRange[1] ? imageParams_[id][i].at(1) : yRange[1];
933  sizeRange[0] = imageParams_[id][i].at(2) < sizeRange[0] ? imageParams_[id][i].at(2) : sizeRange[0];
934  sizeRange[1] = imageParams_[id][i].at(2) > sizeRange[1] ? imageParams_[id][i].at(2) : sizeRange[1];
935  if(imageParams_[id][i].at(3) != 0 && imageParams_[id][i].at(3) != 1) {
936  if(skewRange[0] == 0 || skewRange[0] == 1)
937  {
938  skewRange[0] = imageParams_[id][i].at(3);
939  skewRange[1] = imageParams_[id][i].at(3);
940  }
941  else
942  {
943  skewRange[0] = imageParams_[id][i].at(3) < skewRange[0] ? imageParams_[id][i].at(3) : skewRange[0];
944  skewRange[1] = imageParams_[id][i].at(3) > skewRange[1] ? imageParams_[id][i].at(3) : skewRange[1];
945  }
946  }
947  }
948  //UINFO("Stats [%d]:", id);
949  //UINFO(" Count = %d", (int)imagePoints_[id].size());
950  //UINFO(" x = [%f -> %f]", xRange[0], xRange[1]);
951  //UINFO(" y = [%f -> %f]", yRange[0], yRange[1]);
952  //UINFO(" size = [%f -> %f]", sizeRange[0], sizeRange[1]);
953  //UINFO(" skew = [%f -> %f]", skewRange[0], skewRange[1]);
954 
955  float xGood = xRange[1] - xRange[0];
956  float yGood = yRange[1] - yRange[0];
957  float sizeGood = sizeRange[1] - sizeRange[0];
958  float skewGood = skewRange[1] - skewRange[0];
959 
960  if(id == 0)
961  {
962  ui_->progressBar_x->setValue(xGood*100);
963  ui_->progressBar_y->setValue(yGood*100);
964  ui_->progressBar_size->setValue(sizeGood*100);
965  ui_->progressBar_skew->setValue(skewGood*100);
966  if((int)imagePoints_[id].size() > ui_->progressBar_count->maximum())
967  {
968  ui_->progressBar_count->setMaximum((int)imagePoints_[id].size());
969  }
970  ui_->progressBar_count->setValue((int)imagePoints_[id].size());
971  }
972  else
973  {
974  ui_->progressBar_x_2->setValue(xGood*100);
975  ui_->progressBar_y_2->setValue(yGood*100);
976  ui_->progressBar_size_2->setValue(sizeGood*100);
977  ui_->progressBar_skew_2->setValue(skewGood*100);
978 
979  if((int)imagePoints_[id].size() > ui_->progressBar_count_2->maximum())
980  {
981  ui_->progressBar_count_2->setMaximum((int)imagePoints_[id].size());
982  }
983  ui_->progressBar_count_2->setValue((int)imagePoints_[id].size());
984  }
985 
986  if(imagePoints_[id].size() >= COUNT_MIN/2 &&
987  xGood > 0.5 &&
988  yGood > 0.5 &&
989  (sizeGood > 0.4 || (ui_->comboBox_calib_model->currentIndex()==0 && sizeGood > 0.25)) &&
990  skewGood > 0.5)
991  {
992  readyToCalibrate[id] = true;
993  }
994 
995  //update IR values
996  if(inputRawImages[id].type() == CV_16UC1)
997  {
998  //update min max IR if the chessboard was found
999  minIrs_[id] = 0xFFFF;
1000  maxIrs_[id] = 0;
1001  for(size_t i = 0; i < pointBuf[id].size(); ++i)
1002  {
1003  const cv::Point2f &p = pointBuf[id][i];
1004  cv::Rect roi(std::max(0, (int)p.x - 3), std::max(0, (int)p.y - 3), 6, 6);
1005 
1006  roi.width = std::min(roi.width, inputRawImages[id].cols - roi.x);
1007  roi.height = std::min(roi.height, inputRawImages[id].rows - roi.y);
1008 
1009  //find minMax in the roi
1010  double min, max;
1011  cv::minMaxLoc(inputRawImages[id](roi), &min, &max);
1012  if(min < minIrs_[id])
1013  {
1014  minIrs_[id] = min;
1015  }
1016  if(max > maxIrs_[id])
1017  {
1018  maxIrs_[id] = max;
1019  }
1020  }
1021  }
1022  }
1023  else
1024  {
1025  //break;
1026  }
1027  }
1028  }
1029  }
1030  }
1031  ui_->label_baseline->setVisible(!depthDetected);
1032  ui_->label_baseline_name->setVisible(!depthDetected);
1033  ui_->label_stereoError->setVisible(!depthDetected);
1034 
1035  if(ui_->checkBox_saveCalibrationData->isChecked() && (boardAccepted[0] || boardAccepted[1]))
1036  {
1037  for(int id=0; id<(stereo_?2:1); ++id)
1038  {
1039  QString rawImagesDir = savingDirectory_+"/"+cameraName_+"_"+timestamp_+(stereo_?"/"+(id==0?leftSuffix_:rightSuffix_):"images");
1040  QString imagesWithBoardDir = rawImagesDir+"_board_detection";
1041  if(!QDir(rawImagesDir).exists())
1042  {
1043  UINFO("Creating dir %s", rawImagesDir.toStdString().c_str());
1044  QDir().mkpath(rawImagesDir);
1045  }
1046  if(!QDir(imagesWithBoardDir).exists())
1047  {
1048  UINFO("Creating dir %s", imagesWithBoardDir.toStdString().c_str());
1049  QDir().mkpath(imagesWithBoardDir);
1050  }
1051  cv::imwrite((rawImagesDir+"/"+QString::number(currentId_)+".png").toStdString(), inputRawImages[id]);
1052  cv::imwrite((imagesWithBoardDir+"/"+QString::number(currentId_)+".jpg").toStdString(), images[id]);
1053  }
1054  }
1055 
1056  if(stereo_ && boardFound[0] && boardFound[1] && (boardAccepted[0] || boardAccepted[1]))
1057  {
1058  // Find same corners detected in both boards
1059  std::vector< int > combinedIds;
1060  std::vector<cv::Point2f> leftCorners;
1061  std::vector<cv::Point2f> rightCorners;
1062  std::vector<cv::Point3f> objectPoints;
1063  for(size_t i=0; i<pointIds[0].size(); ++i)
1064  {
1065  for(size_t j=0; j<pointIds[1].size(); ++j)
1066  {
1067  if(pointIds[0][i] == pointIds[1][j])
1068  {
1069  leftCorners.push_back(pointBuf[0][i]);
1070  rightCorners.push_back(pointBuf[1][j]);
1071  objectPoints.push_back(objectBuf[0][i]);
1072  combinedIds.push_back(pointIds[0][i]);
1073  break;
1074  }
1075  }
1076  }
1077  if(objectPoints.size() >=6)
1078  {
1079  stereoImagePoints_[0].push_back(leftCorners);
1080  stereoImagePoints_[1].push_back(rightCorners);
1081  stereoObjectPoints_.push_back(objectPoints);
1082 
1083  stereoImageIds_.push_back(currentId_);
1084  UINFO("Added board %d for stereo image points (size=%d)", currentId_, (int)stereoImagePoints_[0].size());
1085  }
1086  }
1087 
1088  if(!stereo_ && readyToCalibrate[0])
1089  {
1090  unlock();
1091  }
1092  else if(stereo_ && readyToCalibrate[0] && readyToCalibrate[1] && stereoImagePoints_[0].size())
1093  {
1094  unlock();
1095  }
1096 
1097  if(ui_->radioButton_rectified->isChecked())
1098  {
1099  if(models_[0].isValidForRectification())
1100  {
1101  images[0] = models_[0].rectifyImage(images[0]);
1102  }
1103  if(models_[1].isValidForRectification())
1104  {
1105  images[1] = models_[1].rectifyImage(images[1]);
1106  }
1107  }
1108  else if(ui_->radioButton_stereoRectified->isChecked() &&
1109  (stereoModel_.left().isValidForRectification() &&
1110  stereoModel_.right().isValidForRectification()&&
1111  (!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0)))
1112  {
1113  images[0] = stereoModel_.left().rectifyImage(images[0]);
1114  images[1] = stereoModel_.right().rectifyImage(images[1]);
1115  }
1116 
1117  if(ui_->checkBox_showHorizontalLines->isChecked())
1118  {
1119  for(int id=0; id<(stereo_?2:1); ++id)
1120  {
1121  int step = imageSize_[id].height/16;
1122  for(int i=step; i<imageSize_[id].height; i+=step)
1123  {
1124  cv::line(images[id], cv::Point(0, i), cv::Point(imageSize_[id].width, i), CV_RGB(0,255,0));
1125  }
1126  }
1127  }
1128 
1129  ui_->label_left->setText(tr("%1x%2").arg(images[0].cols).arg(images[0].rows));
1130 
1131  //show frame
1132  ui_->image_view->setImage(uCvMat2QImage(images[0]).mirrored(ui_->checkBox_mirror->isChecked(), false));
1133  if(stereo_)
1134  {
1135  ui_->label_right->setText(tr("%1x%2").arg(images[1].cols).arg(images[1].rows));
1136  ui_->image_view_2->setImage(uCvMat2QImage(images[1]).mirrored(ui_->checkBox_mirror->isChecked(), false));
1137  }
1138  processingData_ = false;
1139  ++currentId_;
1140 }
1141 
1142 void CalibrationDialog::restart()
1143 {
1144  // restart
1145  if(!savingDirectory_.isEmpty() && !cameraName_.isEmpty() && !savedCalibration_ && ui_->comboBox_board_type->isEnabled())
1146  {
1147  //overwrite previous data not used.
1148  QDir(savingDirectory_+"/"+cameraName_+"_"+timestamp_).removeRecursively();
1149  }
1150  else
1151  {
1152  timestamp_ = QDateTime::currentDateTime().toString("yyyyMMddhhmmss");
1153  }
1154  savedCalibration_ = false;
1155  currentId_ = 0;
1156  imagePoints_[0].clear();
1157  imagePoints_[1].clear();
1158  objectPoints_[0].clear();
1159  objectPoints_[1].clear();
1160  imageParams_[0].clear();
1161  imageParams_[1].clear();
1162  imageIds_[0].clear();
1163  imageIds_[1].clear();
1164  stereoImagePoints_[0].clear();
1165  stereoImagePoints_[1].clear();
1166  stereoImageIds_.clear();
1167  stereoObjectPoints_.clear();
1168  models_[0] = CameraModel();
1169  models_[1] = CameraModel();
1170  stereoModel_ = StereoCameraModel();
1171  minIrs_[0] = 0x0000;
1172  maxIrs_[0] = 0x7fff;
1173  minIrs_[1] = 0x0000;
1174  maxIrs_[1] = 0x7fff;
1175 
1176  ui_->comboBox_board_type->setEnabled(true);
1177  ui_->comboBox_marker_dictionary->setEnabled(true);
1178  ui_->spinBox_boardWidth->setEnabled(true);
1179  ui_->spinBox_boardHeight->setEnabled(true);
1180  ui_->doubleSpinBox_squareSize->setEnabled(true);
1181  ui_->doubleSpinBox_markerLength->setEnabled(true);
1182  ui_->checkBox_subpixel_refinement->setEnabled(true);
1183  ui_->doubleSpinBox_subpixel_error->setEnabled(true);
1184  ui_->checkBox_saveCalibrationData->setEnabled(true);
1185 
1186  ui_->pushButton_calibrate->setEnabled(ui_->checkBox_unlock->isChecked());
1187  ui_->pushButton_save->setEnabled(false);
1188  ui_->radioButton_raw->setChecked(true);
1189  ui_->radioButton_rectified->setEnabled(false);
1190  ui_->radioButton_stereoRectified->setEnabled(false);
1191 
1192  ui_->progressBar_count->reset();
1193  ui_->progressBar_count->setMaximum(COUNT_MIN);
1194  ui_->progressBar_x->reset();
1195  ui_->progressBar_y->reset();
1196  ui_->progressBar_size->reset();
1197  ui_->progressBar_skew->reset();
1198 
1199  ui_->progressBar_count_2->reset();
1200  ui_->progressBar_count_2->setMaximum(COUNT_MIN);
1201  ui_->progressBar_x_2->reset();
1202  ui_->progressBar_y_2->reset();
1203  ui_->progressBar_size_2->reset();
1204  ui_->progressBar_skew_2->reset();
1205 
1206  ui_->label_serial->clear();
1207  ui_->label_fx->setNum(0);
1208  ui_->label_fy->setNum(0);
1209  ui_->label_cx->setNum(0);
1210  ui_->label_cy->setNum(0);
1211  ui_->label_fovx->setNum(0);
1212  ui_->label_fovy->setNum(0);
1213  ui_->label_baseline->setNum(0);
1214  ui_->label_stereoError->setNum(0);
1215  ui_->label_error->setNum(0);
1216  ui_->label_error_2->setNum(0);
1217  ui_->lineEdit_K->clear();
1218  ui_->lineEdit_D->clear();
1219  ui_->lineEdit_R->clear();
1220  ui_->lineEdit_P->clear();
1221  ui_->label_fx_2->setNum(0);
1222  ui_->label_fy_2->setNum(0);
1223  ui_->label_cx_2->setNum(0);
1224  ui_->label_cy_2->setNum(0);
1225  ui_->label_fovx_2->setNum(0);
1226  ui_->label_fovy_2->setNum(0);
1227  ui_->lineEdit_K_2->clear();
1228  ui_->lineEdit_D_2->clear();
1229  ui_->lineEdit_R_2->clear();
1230  ui_->lineEdit_P_2->clear();
1231 
1232  chessboardPoints_.clear();
1233  chessboardPointIds_.clear();
1234 #ifdef HAVE_CHARUCO
1235  markerDictionary_.release();
1236  arucoDetectorParams_.release();
1237  charucoBoard_.release();
1238  if(ui_->comboBox_board_type->currentIndex() >= 1 )
1239  {
1240 #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
1241  arucoDetectorParams_.reset(new cv::aruco::DetectorParameters());
1242 #else
1243  arucoDetectorParams_ = cv::aruco::DetectorParameters::create();
1244 #endif
1245 
1246 #if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3)
1247  arucoDetectorParams_->cornerRefinementMethod = cv::aruco::CORNER_REFINE_CONTOUR;
1248 #else
1249  arucoDetectorParams_->doCornerRefinement = true;
1250 #endif
1251 
1252  int arucoDictionary = ui_->comboBox_marker_dictionary->currentIndex();
1253  if(arucoDictionary >= 17)
1254  {
1255 #if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2)))
1256  UERROR("Cannot set AprilTag dictionary. OpenCV version should be at least 3.4.2, "
1257  "current version is %s.", CV_VERSION);
1258 
1259  // Dictionary to use:
1260  // DICT_ARUCO_4X4_50=0, DICT_ARUCO_4X4_100=1, DICT_ARUCO_4X4_250=2, DICT_ARUCO_4X4_1000=3,
1261  // DICT_ARUCO_5X5_50=4, DICT_ARUCO_5X5_100=5, DICT_ARUCO_5X5_250=6, DICT_ARUCO_5X5_1000=7,
1262  // DICT_ARUCO_6X6_50=8, DICT_ARUCO_6X6_100=9, DICT_ARUCO_6X6_250=10, DICT_ARUCO_6X6_1000=11,
1263  // DICT_ARUCO_7X7_50=12, DICT_ARUCO_7X7_100=13, DICT_ARUCO_7X7_250=14, DICT_ARUCO_7X7_1000=15,
1264  // DICT_ARUCO_ORIGINAL = 16, DICT_APRILTAG_16h5=17, DICT_APRILTAG_25h9=18, DICT_APRILTAG_36h10=19,
1265  // DICT_APRILTAG_36h11=20
1266  //
1267  arucoDictionary = 0;
1268 #else
1269  arucoDetectorParams_->cornerRefinementMethod = cv::aruco::CORNER_REFINE_APRILTAG;
1270 #endif
1271  }
1272 
1273 #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
1274  markerDictionary_.reset(new cv::aruco::Dictionary());
1275  *markerDictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PredefinedDictionaryType(arucoDictionary));
1276 #elif CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
1277  markerDictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PREDEFINED_DICTIONARY_NAME(arucoDictionary));
1278 #else
1279  markerDictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::PREDEFINED_DICTIONARY_NAME(arucoDictionary));
1280 #endif
1281  UDEBUG("Creating charuco board: %dx%d square=%f marker=%f aruco dict=%d",
1282  ui_->spinBox_boardWidth->value(),
1283  ui_->spinBox_boardHeight->value(),
1284  ui_->doubleSpinBox_squareSize->value(),
1285  ui_->doubleSpinBox_markerLength->value(),
1286  arucoDictionary);
1287 
1288 #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
1289  charucoBoard_.reset(new cv::aruco::CharucoBoard(
1290  cv::Size(ui_->spinBox_boardWidth->value(), ui_->spinBox_boardHeight->value()),
1291  ui_->doubleSpinBox_squareSize->value(),
1292  ui_->doubleSpinBox_markerLength->value(),
1293  *markerDictionary_));
1294 #if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 8)
1295  charucoBoard_->setLegacyPattern(ui_->comboBox_board_type->currentIndex()==1);
1296 #endif
1297  arucoDetector_.reset(new cv::aruco::ArucoDetector(*markerDictionary_, *arucoDetectorParams_));
1298  charucoDetector_.reset(new cv::aruco::CharucoDetector(*charucoBoard_, cv::aruco::CharucoParameters(), *arucoDetectorParams_));
1299 #else
1300  charucoBoard_ = cv::aruco::CharucoBoard::create(
1301  ui_->spinBox_boardWidth->value(),
1302  ui_->spinBox_boardHeight->value(),
1303  ui_->doubleSpinBox_squareSize->value(),
1304  ui_->doubleSpinBox_markerLength->value(),
1305  markerDictionary_);
1306 #endif
1307  }
1308  else //checkerboard
1309 #endif
1310  {
1311  for( int i = 0; i < ui_->spinBox_boardHeight->value(); ++i ) {
1312  for( int j = 0; j < ui_->spinBox_boardWidth->value(); ++j ) {
1313  chessboardPoints_.push_back(cv::Point3f(float( j*ui_->doubleSpinBox_squareSize->value() ), float( i*ui_->doubleSpinBox_squareSize->value() ), 0));
1314  chessboardPointIds_.push_back(i*ui_->spinBox_boardWidth->value() + j);
1315  }
1316  }
1317  }
1318 
1319  ui_->comboBox_marker_dictionary->setVisible(ui_->comboBox_board_type->currentIndex() >= 1 );
1320  ui_->doubleSpinBox_markerLength->setVisible(ui_->comboBox_board_type->currentIndex() >= 1 );
1321  ui_->label_markerDictionary->setVisible(ui_->comboBox_board_type->currentIndex() >= 1 );
1322  ui_->label_markerLength->setVisible(ui_->comboBox_board_type->currentIndex() >= 1 );
1323 }
1324 
1325 void CalibrationDialog::unlock()
1326 {
1327  ui_->pushButton_calibrate->setEnabled(true);
1328 }
1329 
1330 void CalibrationDialog::calibrate()
1331 {
1332  processingData_ = true;
1333  savedCalibration_ = false;
1334 
1335  ui_->comboBox_board_type->setEnabled(false);
1336  ui_->comboBox_marker_dictionary->setEnabled(false);
1337  ui_->spinBox_boardWidth->setEnabled(false);
1338  ui_->spinBox_boardHeight->setEnabled(false);
1339  ui_->doubleSpinBox_squareSize->setEnabled(false);
1340  ui_->doubleSpinBox_markerLength->setEnabled(false);
1341  ui_->checkBox_subpixel_refinement->setEnabled(false);
1342  ui_->doubleSpinBox_subpixel_error->setEnabled(false);
1343  ui_->checkBox_saveCalibrationData->setEnabled(false);
1344 
1345  QMessageBox mb(QMessageBox::Information,
1346  tr("Calibrating..."),
1347  tr("Operation in progress..."));
1348  mb.show();
1349  QApplication::processEvents();
1350  uSleep(100); // hack make sure the text in the QMessageBox is shown...
1351  QApplication::processEvents();
1352 
1353  // Logging
1354  QFile logFile;
1355  QString dummyOutput;
1356  QTextStream logStream(&dummyOutput);
1357  if(ui_->checkBox_saveCalibrationData->isChecked())
1358  {
1359  logFile.setFileName(savingDirectory_+"/"+cameraName_+"_"+timestamp_+"/"+"log.txt");
1360  if (logFile.open(QIODevice::WriteOnly | QIODevice::Text)) {
1361  logStream.setDevice(&logFile);
1362  }
1363  }
1364 
1365  std::cout << "Board type = " << ui_->comboBox_board_type->currentIndex() << std::endl;
1366  std::cout << "Board width = " << ui_->spinBox_boardWidth->value() << std::endl;
1367  std::cout << "Board height = " << ui_->spinBox_boardHeight->value() << std::endl;
1368  std::cout << "Square size = " << ui_->doubleSpinBox_squareSize->value() << std::endl;
1369  std::cout << "Subpixel refinement = " << ui_->checkBox_subpixel_refinement->isChecked() << std::endl;
1370  std::cout << "Subpixel max error = " << ui_->doubleSpinBox_subpixel_error->value() << std::endl;
1371  logStream << "Board type = " << ui_->comboBox_board_type->currentIndex() << ENDL;
1372  logStream << "Board width = " << ui_->spinBox_boardWidth->value() << ENDL;
1373  logStream << "Board height = " << ui_->spinBox_boardHeight->value() << ENDL;
1374  logStream << "Square size = " << ui_->doubleSpinBox_squareSize->value() << ENDL;
1375  logStream << "Subpixel refinement = " << ui_->checkBox_subpixel_refinement->isChecked() << ENDL;
1376  logStream << "Subpixel max error = " << ui_->doubleSpinBox_subpixel_error->value() << ENDL;
1377  if(ui_->comboBox_board_type->currentIndex() >= 1 )
1378  {
1379  std::cout << "Marker dictionary = " << ui_->comboBox_marker_dictionary->currentIndex() << std::endl;
1380  std::cout << "Marker length = " << ui_->doubleSpinBox_markerLength->value() << std::endl;
1381  logStream << "Marker dictionary = " << ui_->comboBox_marker_dictionary->currentIndex() << ENDL;
1382  logStream << "Marker length = " << ui_->doubleSpinBox_markerLength->value() << ENDL;
1383  }
1384 
1385  for(int id=0; id<(stereo_?2:1); ++id)
1386  {
1387  UINFO("Calibrating camera %d (samples=%d)", id, (int)imagePoints_[id].size());
1388  logStream << "Calibrating camera " << id << " (samples=" << imagePoints_[id].size() << ")" << ENDL;
1389 
1390  //calibrate
1391  std::vector<cv::Mat> rvecs, tvecs;
1392  std::vector<float> reprojErrs;
1393  cv::Mat K, D;
1394  K = cv::Mat::eye(3,3,CV_64FC1);
1395  UINFO("calibrate!");
1396  //Find intrinsic and extrinsic camera parameters
1397  double rms = 0.0;
1398 #if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
1399  bool fishEye = ui_->comboBox_calib_model->currentIndex()==0;
1400 
1401  if(fishEye)
1402  {
1403  try
1404  {
1405  rms = cv::fisheye::calibrate(
1406  objectPoints_[id],
1407  imagePoints_[id],
1408  imageSize_[id],
1409  K,
1410  D,
1411  rvecs,
1412  tvecs,
1413  cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC |
1414  cv::fisheye::CALIB_CHECK_COND |
1415  cv::fisheye::CALIB_FIX_SKEW);
1416  }
1417  catch(const cv::Exception & e)
1418  {
1419  UERROR("Error: %s (try restarting the calibration)", e.what());
1420  QMessageBox::warning(this, tr("Calibration failed!"), tr("Error: %1 (try restarting the calibration)").arg(e.what()));
1421  processingData_ = false;
1422  return;
1423  }
1424  }
1425  else
1426 #endif
1427  {
1428  cv::Mat stdDevsMatInt, stdDevsMatExt;
1429  cv::Mat perViewErrorsMat;
1430  rms = cv::calibrateCamera(
1431  objectPoints_[id],
1432  imagePoints_[id],
1433  imageSize_[id],
1434  K,
1435  D,
1436  rvecs,
1437  tvecs,
1438  stdDevsMatInt,
1439  stdDevsMatExt,
1440  perViewErrorsMat,
1441  ui_->comboBox_calib_model->currentIndex()==2?cv::CALIB_RATIONAL_MODEL:0);
1442  if((int)imageIds_[id].size() == perViewErrorsMat.rows)
1443  {
1444  UINFO("Per view errors:");
1445  logStream << "Per view errors:" << ENDL;
1446  for(int i=0; i<perViewErrorsMat.rows; ++i)
1447  {
1448  UINFO("Image %d: %f", imageIds_[id][i], perViewErrorsMat.at<double>(i,0));
1449  logStream << "Image " << imageIds_[id][i] << ": " << perViewErrorsMat.at<double>(i,0) << ENDL;
1450  }
1451  }
1452  }
1453 
1454  UINFO("Re-projection error reported by calibrateCamera: %f", rms);
1455  logStream << "Re-projection error reported by calibrateCamera: " << rms << ENDL;
1456 
1457  // compute reprojection errors
1458  std::vector<cv::Point2f> imagePoints2;
1459  int i, totalPoints = 0;
1460  double totalErr = 0, err;
1461  reprojErrs.resize(objectPoints_[id].size());
1462 
1463  for( i = 0; i < (int)objectPoints_[id].size(); ++i )
1464  {
1465 #if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
1466  if(fishEye)
1467  {
1468  cv::fisheye::projectPoints( cv::Mat(objectPoints_[id][i]), imagePoints2, rvecs[i], tvecs[i], K, D);
1469  }
1470  else
1471 #endif
1472  {
1473  cv::projectPoints( cv::Mat(objectPoints_[id][i]), rvecs[i], tvecs[i], K, D, imagePoints2);
1474  }
1475  err = cv::norm(cv::Mat(imagePoints_[id][i]), cv::Mat(imagePoints2), CV_L2);
1476 
1477  int n = (int)objectPoints_[id][i].size();
1478  reprojErrs[i] = (float) std::sqrt(err*err/n);
1479  totalErr += err*err;
1480  totalPoints += n;
1481  }
1482 
1483  double totalAvgErr = std::sqrt(totalErr/totalPoints);
1484 
1485  UINFO("avg re projection error = %f", totalAvgErr);
1486  logStream << "avg re projection error = " << totalAvgErr << ENDL;
1487 
1488  cv::Mat P(3,4,CV_64FC1);
1489  P.at<double>(2,3) = 1;
1490  K.copyTo(P.colRange(0,3).rowRange(0,3));
1491 
1492 #if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
1493  if(fishEye)
1494  {
1495  // Convert to unified distortion model (k1,k2,p1,p2,k3,k4)
1496  cv::Mat newD = cv::Mat::zeros(1,6,CV_64FC1);
1497  newD.at<double>(0,0) = D.at<double>(0,0);
1498  newD.at<double>(0,1) = D.at<double>(0,1);
1499  newD.at<double>(0,4) = D.at<double>(0,2);
1500  newD.at<double>(0,5) = D.at<double>(0,3);
1501  D = newD;
1502  }
1503 #endif
1504 
1505  models_[id] = CameraModel(cameraName_.toStdString(), imageSize_[id], K, D, cv::Mat::eye(3,3,CV_64FC1), P);
1506 
1507  std::cout << "K = " << K << std::endl;
1508  std::cout << "D = " << D << std::endl;
1509  std::cout << "width = " << imageSize_[id].width << std::endl;
1510  std::cout << "height = " << imageSize_[id].height << std::endl;
1511  UINFO("FOV horizontal=%f vertical=%f", models_[id].horizontalFOV(), models_[id].verticalFOV());
1512 
1513 #if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION > 2)
1514  std::string strStream;
1515  logStream << "K = " << (strStream << K).c_str() << ENDL;
1516  strStream.clear();
1517  logStream << "D = " << (strStream << D).c_str() << ENDL;
1518 #endif
1519  logStream << "width = " << imageSize_[id].width << ENDL;
1520  logStream << "height = " << imageSize_[id].height << ENDL;
1521  logStream << "FOV horizontal=" << models_[id].horizontalFOV() << " vertical=" << models_[id].verticalFOV() << ENDL;
1522 
1523  if(id == 0)
1524  {
1525  ui_->label_fx->setNum(models_[id].fx());
1526  ui_->label_fy->setNum(models_[id].fy());
1527  ui_->label_cx->setNum(models_[id].cx());
1528  ui_->label_cy->setNum(models_[id].cy());
1529  ui_->label_fovx->setNum(models_[id].horizontalFOV());
1530  ui_->label_fovy->setNum(models_[id].verticalFOV());
1531  ui_->label_error->setNum(totalAvgErr);
1532 
1533  std::stringstream strK, strD, strR, strP;
1534  strK << models_[id].K_raw();
1535  strD << models_[id].D_raw();
1536  strR << models_[id].R();
1537  strP << models_[id].P();
1538  ui_->lineEdit_K->setText(strK.str().c_str());
1539  ui_->lineEdit_D->setText(strD.str().c_str());
1540  ui_->lineEdit_R->setText(strR.str().c_str());
1541  ui_->lineEdit_P->setText(strP.str().c_str());
1542  }
1543  else
1544  {
1545  ui_->label_fx_2->setNum(models_[id].fx());
1546  ui_->label_fy_2->setNum(models_[id].fy());
1547  ui_->label_cx_2->setNum(models_[id].cx());
1548  ui_->label_cy_2->setNum(models_[id].cy());
1549  ui_->label_fovx_2->setNum(models_[id].horizontalFOV());
1550  ui_->label_fovy_2->setNum(models_[id].verticalFOV());
1551  ui_->label_error_2->setNum(totalAvgErr);
1552 
1553  std::stringstream strK, strD, strR, strP;
1554  strK << models_[id].K_raw();
1555  strD << models_[id].D_raw();
1556  strR << models_[id].R();
1557  strP << models_[id].P();
1558  ui_->lineEdit_K_2->setText(strK.str().c_str());
1559  ui_->lineEdit_D_2->setText(strD.str().c_str());
1560  ui_->lineEdit_R_2->setText(strR.str().c_str());
1561  ui_->lineEdit_P_2->setText(strP.str().c_str());
1562  }
1563  }
1564 
1565  if(stereo_ && models_[0].isValidForRectification() && models_[1].isValidForRectification())
1566  {
1567  stereoModel_ = stereoCalibration(models_[0], models_[1], false, &logStream);
1568 
1569  if(stereoModel_.isValidForProjection() &&
1570  ui_->doubleSpinBox_stereoBaseline->value() > 0 &&
1571  stereoModel_.baseline() != ui_->doubleSpinBox_stereoBaseline->value())
1572  {
1573  UWARN("Expected stereo baseline is set to %f m, but computed baseline is %f m. Rescaling baseline...",
1574  ui_->doubleSpinBox_stereoBaseline->value(), stereoModel_.baseline());
1575  cv::Mat P = stereoModel_.right().P().clone();
1576  P.at<double>(0,3) = -P.at<double>(0,0)*ui_->doubleSpinBox_stereoBaseline->value();
1577  double scale = ui_->doubleSpinBox_stereoBaseline->value() / stereoModel_.baseline();
1578  UWARN("Scale %f (setting square size from %f to %f)", scale, ui_->doubleSpinBox_squareSize->value(), ui_->doubleSpinBox_squareSize->value()*scale);
1579  logStream << "Baseline rescaled from " << stereoModel_.baseline() << " to " << ui_->doubleSpinBox_stereoBaseline->value() << " scale=" << scale << ENDL;
1580  ui_->doubleSpinBox_squareSize->setValue(ui_->doubleSpinBox_squareSize->value()*scale);
1581  stereoModel_ = StereoCameraModel(
1582  stereoModel_.name(),
1583  stereoModel_.left().imageSize(),stereoModel_.left().K_raw(), stereoModel_.left().D_raw(), stereoModel_.left().R(), stereoModel_.left().P(),
1584  stereoModel_.right().imageSize(), stereoModel_.right().K_raw(), stereoModel_.right().D_raw(), stereoModel_.right().R(), P,
1585  stereoModel_.R(), stereoModel_.T()*scale, stereoModel_.E(), stereoModel_.F(), stereoModel_.localTransform());
1586  }
1587 
1588  std::stringstream strR1, strP1, strR2, strP2;
1589  strR1 << stereoModel_.left().R();
1590  strP1 << stereoModel_.left().P();
1591  strR2 << stereoModel_.right().R();
1592  strP2 << stereoModel_.right().P();
1593  ui_->lineEdit_R->setText(strR1.str().c_str());
1594  ui_->lineEdit_P->setText(strP1.str().c_str());
1595  ui_->lineEdit_R_2->setText(strR2.str().c_str());
1596  ui_->lineEdit_P_2->setText(strP2.str().c_str());
1597 
1598  ui_->label_fovx->setNum(stereoModel_.left().horizontalFOV());
1599  ui_->label_fovx_2->setNum(stereoModel_.right().horizontalFOV());
1600  ui_->label_fovy->setNum(stereoModel_.left().verticalFOV());
1601  ui_->label_fovy_2->setNum(stereoModel_.right().verticalFOV());
1602  ui_->label_baseline->setNum(stereoModel_.baseline());
1603  //ui_->label_error_stereo->setNum(totalAvgErr);
1604  UINFO("Baseline=%f FOV horizontal=%f vertical=%f", stereoModel_.baseline(), stereoModel_.left().horizontalFOV(), stereoModel_.left().verticalFOV());
1605  logStream << "Baseline = " << stereoModel_.baseline() << ENDL;
1606  logStream << "Stereo horizontal FOV = " << stereoModel_.left().horizontalFOV() << ENDL;
1607  logStream << "Stereo vertical FOV = " << stereoModel_.left().verticalFOV() << ENDL;
1608  }
1609 
1610  if(stereo_)
1611  {
1612  if(models_[0].isValidForRectification())
1613  {
1614  models_[0].initRectificationMap();
1615  }
1616  if(models_[1].isValidForRectification())
1617  {
1618  models_[1].initRectificationMap();
1619  }
1620  if(models_[0].isValidForRectification() || models_[1].isValidForRectification())
1621  {
1622  ui_->radioButton_rectified->setEnabled(true);
1623  }
1624  if(stereoModel_.isValidForRectification())
1625  {
1626  stereoModel_.initRectificationMap();
1627  ui_->radioButton_stereoRectified->setEnabled(true);
1628  ui_->radioButton_stereoRectified->setChecked(true);
1629  ui_->pushButton_save->setEnabled(true);
1630 
1631  if(ui_->checkBox_saveCalibrationData->isChecked())
1632  {
1633  stereoModel_.save((savingDirectory_+"/"+cameraName_+"_"+timestamp_).toStdString(), false);
1634  }
1635  }
1636  else
1637  {
1638  ui_->radioButton_rectified->setChecked(ui_->radioButton_rectified->isEnabled());
1639  }
1640  }
1641  else if(models_[0].isValidForRectification())
1642  {
1643  models_[0].initRectificationMap();
1644  ui_->radioButton_rectified->setEnabled(true);
1645  ui_->radioButton_rectified->setChecked(true);
1646  ui_->pushButton_save->setEnabled(true);
1647 
1648  if(ui_->checkBox_saveCalibrationData->isChecked())
1649  {
1650  models_[0].save((savingDirectory_+"/"+cameraName_+"_"+timestamp_).toStdString());
1651  }
1652  }
1653 
1654  UINFO("End calibration");
1655  processingData_ = false;
1656  logFile.close();
1657 }
1658 
1659 StereoCameraModel CalibrationDialog::stereoCalibration(const CameraModel & left, const CameraModel & right, bool ignoreStereoRectification, QTextStream * logStream) const
1660 {
1661  StereoCameraModel output;
1662  if (stereoImagePoints_[0].empty())
1663  {
1664  UERROR("No stereo correspondences!");
1665  return output;
1666  }
1667  UINFO("stereo calibration (samples=%d)...", (int)stereoImagePoints_[0].size());
1668  if(logStream) (*logStream) << "stereo calibration (samples=" << stereoImagePoints_[0].size() <<")..." << ENDL;
1669 
1670  if (left.K_raw().empty() || left.D_raw().empty())
1671  {
1672  UERROR("Empty intrinsic parameters (K, D) for the %s camera! Aborting stereo calibration...", leftSuffix_.toStdString().c_str());
1673  return output;
1674  }
1675  if (right.K_raw().empty() || right.D_raw().empty())
1676  {
1677  UERROR("Empty intrinsic parameters (K, D) for the %s camera! Aborting stereo calibration...", rightSuffix_.toStdString().c_str());
1678  return output;
1679  }
1680 
1681  if (left.imageSize() != imageSize_[0])
1682  {
1683  UERROR("left model (%dx%d) has not the same size as the processed images (%dx%d)",
1684  left.imageSize().width, left.imageSize().height,
1685  imageSize_[0].width, imageSize_[0].height);
1686  return output;
1687  }
1688  if (right.imageSize() != imageSize_[1])
1689  {
1690  UERROR("right model (%dx%d) has not the same size as the processed images (%dx%d)",
1691  right.imageSize().width, right.imageSize().height,
1692  imageSize_[1].width, imageSize_[1].height);
1693  return output;
1694  }
1695 
1696  cv::Size imageSize = imageSize_[0].width > imageSize_[1].width ? imageSize_[0] : imageSize_[1];
1697  cv::Mat R, T, E, F;
1698 
1699  double rms = 0.0;
1700 #if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
1701  bool fishEye = left.D_raw().cols == 6;
1702  // calibrate extrinsic
1703  if(fishEye)
1704  {
1705  cv::Vec3d Tvec;
1706  cv::Vec4d D_left(left.D_raw().at<double>(0,0), left.D_raw().at<double>(0,1), left.D_raw().at<double>(0,4), left.D_raw().at<double>(0,5));
1707  cv::Vec4d D_right(right.D_raw().at<double>(0,0), right.D_raw().at<double>(0,1), right.D_raw().at<double>(0,4), right.D_raw().at<double>(0,5));
1708 
1709  UASSERT(stereoImagePoints_[0].size() == stereoImagePoints_[1].size());
1710  std::vector<std::vector<cv::Point2d> > leftPoints(stereoImagePoints_[0].size());
1711  std::vector<std::vector<cv::Point2d> > rightPoints(stereoImagePoints_[1].size());
1712  for(unsigned int i =0; i<stereoImagePoints_[0].size(); ++i)
1713  {
1714  UASSERT(stereoImagePoints_[0][i].size() == stereoImagePoints_[1][i].size());
1715  leftPoints[i].resize(stereoImagePoints_[0][i].size());
1716  rightPoints[i].resize(stereoImagePoints_[1][i].size());
1717  for(unsigned int j =0; j<stereoImagePoints_[0][i].size(); ++j)
1718  {
1719  leftPoints[i][j].x = stereoImagePoints_[0][i][j].x;
1720  leftPoints[i][j].y = stereoImagePoints_[0][i][j].y;
1721  rightPoints[i][j].x = stereoImagePoints_[1][i][j].x;
1722  rightPoints[i][j].y = stereoImagePoints_[1][i][j].y;
1723  }
1724  }
1725 
1726  try
1727  {
1728  rms = cv::fisheye::stereoCalibrate(
1729  stereoObjectPoints_,
1730  leftPoints,
1731  rightPoints,
1732  left.K_raw(), D_left, right.K_raw(), D_right,
1733  imageSize, R, Tvec,
1734  cv::fisheye::CALIB_FIX_INTRINSIC,
1735  cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
1736  UINFO("stereo calibration... done with RMS error=%f", rms);
1737  }
1738  catch(const cv::Exception & e)
1739  {
1740  UERROR("Error: %s (try restarting the calibration)", e.what());
1741  return output;
1742  }
1743 
1744  std::cout << "R = " << R << std::endl;
1745  std::cout << "T = " << Tvec << std::endl;
1746 
1747  if(imageSize_[0] == imageSize_[1] && !ignoreStereoRectification)
1748  {
1749  UINFO("Compute stereo rectification");
1750 
1751  cv::Mat R1, R2, P1, P2, Q;
1752  stereoRectifyFisheye(
1753  left.K_raw(), D_left,
1754  right.K_raw(), D_right,
1755  imageSize, R, Tvec, R1, R2, P1, P2, Q,
1756  cv::CALIB_ZERO_DISPARITY, 0, imageSize);
1757 
1758  // Very hard to get good results with this one:
1759  /*double balance = 0.0, fov_scale = 1.0;
1760  cv::fisheye::stereoRectify(
1761  left.K_raw(), D_left,
1762  right.K_raw(), D_right,
1763  imageSize, R, Tvec, R1, R2, P1, P2, Q,
1764  cv::CALIB_ZERO_DISPARITY, imageSize, balance, fov_scale);*/
1765 
1766  std::cout << "R1 = " << R1 << std::endl;
1767  std::cout << "R2 = " << R2 << std::endl;
1768  std::cout << "P1 = " << P1 << std::endl;
1769  std::cout << "P2 = " << P2 << std::endl;
1770 
1771  // Re-zoom to original focal distance
1772  if(P1.at<double>(0,0) < 0)
1773  {
1774  P1.at<double>(0,0) *= -1;
1775  P1.at<double>(1,1) *= -1;
1776  }
1777  if(P2.at<double>(0,0) < 0)
1778  {
1779  P2.at<double>(0,0) *= -1;
1780  P2.at<double>(1,1) *= -1;
1781  }
1782  if(P2.at<double>(0,3) > 0)
1783  {
1784  P2.at<double>(0,3) *= -1;
1785  }
1786  P2.at<double>(0,3) = P2.at<double>(0,3) * left.K_raw().at<double>(0,0) / P2.at<double>(0,0);
1787  P1.at<double>(0,0) = P1.at<double>(1,1) = left.K_raw().at<double>(0,0);
1788  P2.at<double>(0,0) = P2.at<double>(1,1) = left.K_raw().at<double>(0,0);
1789 
1790  std::cout << "P1n = " << P1 << std::endl;
1791  std::cout << "P2n = " << P2 << std::endl;
1792 
1793 
1794  cv::Mat T(3,1,CV_64FC1);
1795  T.at <double>(0,0) = Tvec[0];
1796  T.at <double>(1,0) = Tvec[1];
1797  T.at <double>(2,0) = Tvec[2];
1798  output = StereoCameraModel(
1799  cameraName_.toStdString(),
1800  imageSize_[0], left.K_raw(), left.D_raw(), R1, P1,
1801  imageSize_[1], right.K_raw(), right.D_raw(), R2, P2,
1802  R, T, E, F);
1803  }
1804  else
1805  {
1806  UDEBUG("%s", cameraName_.toStdString().c_str());
1807  //Kinect, ignore the stereo rectification
1808  output = StereoCameraModel(
1809  cameraName_.toStdString(),
1810  imageSize_[0], left.K_raw(), left.D_raw(), left.R(), left.P(),
1811  imageSize_[1], right.K_raw(), right.D_raw(), right.R(), right.P(),
1812  R, T, E, F);
1813  }
1814  }
1815  else
1816 #endif
1817  {
1818 #if CV_MAJOR_VERSION < 3
1819  rms = cv::stereoCalibrate(
1820  stereoObjectPoints_,
1821  stereoImagePoints_[0],
1822  stereoImagePoints_[1],
1823  left.K_raw(), left.D_raw(),
1824  right.K_raw(), right.D_raw(),
1825  imageSize, R, T, E, F,
1826  cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5),
1827  cv::CALIB_FIX_INTRINSIC | (ui_->comboBox_calib_model->currentIndex()==2?cv::CALIB_RATIONAL_MODEL:0));
1828 #elif CV_MAJOR_VERSION == 3 and (CV_MINOR_VERSION < 4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION < 1))
1829  //OpenCV < 3.4.1
1830  rms = cv::stereoCalibrate(
1831  stereoObjectPoints_,
1832  stereoImagePoints_[0],
1833  stereoImagePoints_[1],
1834  left.K_raw(), left.D_raw(),
1835  right.K_raw(), right.D_raw(),
1836  imageSize, R, T, E, F,
1837  cv::CALIB_FIX_INTRINSIC | (ui_->comboBox_calib_model->currentIndex()==2?cv::CALIB_RATIONAL_MODEL:0),
1838  cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
1839 #else
1840  cv::Mat perViewErrorsMat;
1841  rms = cv::stereoCalibrate(
1842  stereoObjectPoints_,
1843  stereoImagePoints_[0],
1844  stereoImagePoints_[1],
1845  left.K_raw(), left.D_raw(),
1846  right.K_raw(), right.D_raw(),
1847  imageSize, R, T, E, F,
1848  perViewErrorsMat,
1849  cv::CALIB_FIX_INTRINSIC | (ui_->comboBox_calib_model->currentIndex()==2?cv::CALIB_RATIONAL_MODEL:0),
1850  cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
1851  if((int)stereoImageIds_.size() == perViewErrorsMat.rows)
1852  {
1853  UINFO("Per stereo view errors: %dx%d", perViewErrorsMat.rows, perViewErrorsMat.cols);
1854  if(logStream) (*logStream) << "Per stereo view errors:" << ENDL;
1855  for(int i=0; i<perViewErrorsMat.rows; ++i)
1856  {
1857  UINFO("Image %d: %f <-> %f", stereoImageIds_[i], perViewErrorsMat.at<double>(i,0), perViewErrorsMat.at<double>(i,1));
1858  if(logStream) (*logStream) << "Image " << stereoImageIds_[i] << ": " << perViewErrorsMat.at<double>(i,0) << " <-> " << perViewErrorsMat.at<double>(i,0) << ENDL;
1859  }
1860  }
1861 #endif
1862  UINFO("stereo calibration... done with RMS error=%f", rms);
1863  if(logStream) (*logStream) << "stereo calibration... done with RMS error=" << rms << ENDL;
1864  ui_->label_stereoError->setNum(rms);
1865 
1866  std::cout << "R = " << R << std::endl;
1867  std::cout << "T = " << T << std::endl;
1868  std::cout << "E = " << E << std::endl;
1869  std::cout << "F = " << F << std::endl;
1870 
1871 #if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION > 2)
1872  std::string strStream;
1873  if(logStream) (*logStream) << "R = " << (strStream<<R).c_str() << ENDL;
1874  strStream.clear();
1875  if(logStream) (*logStream) << "T = " << (strStream<<T).c_str() << ENDL;
1876  strStream.clear();
1877  if(logStream) (*logStream) << "E = " << (strStream<<E).c_str() << ENDL;
1878  strStream.clear();
1879  if(logStream) (*logStream) << "F = " << (strStream<<F).c_str() << ENDL;
1880  strStream.clear();
1881 #endif
1882 
1883  if(imageSize_[0] == imageSize_[1] && !ignoreStereoRectification)
1884  {
1885  UINFO("Compute stereo rectification");
1886 
1887  cv::Mat R1, R2, P1, P2, Q;
1888  cv::stereoRectify(left.K_raw(), left.D_raw(),
1889  right.K_raw(), right.D_raw(),
1890  imageSize, R, T, R1, R2, P1, P2, Q,
1891  cv::CALIB_ZERO_DISPARITY, 0, imageSize);
1892 
1893  std::cout << "R1 = " << R1 << std::endl;
1894  std::cout << "P1 = " << P1 << std::endl;
1895  std::cout << "R2 = " << R2 << std::endl;
1896  std::cout << "P2 = " << P2 << std::endl;
1897 
1898 #if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION > 2)
1899  if(logStream) (*logStream) << "R1 = " << (strStream<<R1).c_str() << ENDL;
1900  strStream.clear();
1901  if(logStream) (*logStream) << "P1 = " << (strStream<<P1).c_str() << ENDL;
1902  strStream.clear();
1903  if(logStream) (*logStream) << "R2 = " << (strStream<<R2).c_str() << ENDL;
1904  strStream.clear();
1905  if(logStream) (*logStream) << "P2 = " << (strStream<<P2).c_str() << ENDL;
1906 #endif
1907 
1908  double err = 0;
1909  int npoints = 0;
1910  std::vector<cv::Vec3f> lines[2];
1911  UINFO("Computing re-projection errors...");
1912  if(logStream) (*logStream) << "Computing re-projection epipolar errors..." << ENDL;
1913  for(unsigned int i = 0; i < stereoImagePoints_[0].size(); i++ )
1914  {
1915  int npt = (int)stereoImagePoints_[0][i].size();
1916 
1917  cv::Mat imgpt0 = cv::Mat(stereoImagePoints_[0][i]);
1918  cv::Mat imgpt1 = cv::Mat(stereoImagePoints_[1][i]);
1919  cv::undistortPoints(imgpt0, imgpt0, left.K_raw(), left.D_raw(), R1, P1);
1920  cv::undistortPoints(imgpt1, imgpt1, right.K_raw(), right.D_raw(), R2, P2);
1921  computeCorrespondEpilines(imgpt0, 1, F, lines[0]);
1922  computeCorrespondEpilines(imgpt1, 2, F, lines[1]);
1923 
1924  double sampleErr = 0.0;
1925  for(int j = 0; j < npt; j++ )
1926  {
1927  double errij = fabs(stereoImagePoints_[0][i][j].x*lines[1][j][0] +
1928  stereoImagePoints_[0][i][j].y*lines[1][j][1] + lines[1][j][2]) +
1929  fabs(stereoImagePoints_[1][i][j].x*lines[0][j][0] +
1930  stereoImagePoints_[1][i][j].y*lines[0][j][1] + lines[0][j][2]);
1931  sampleErr += errij;
1932  }
1933  UINFO("Stereo image %d: %f", stereoImageIds_[i], sampleErr/npt);
1934  if(logStream) (*logStream) << "Stereo image " << stereoImageIds_[i] << ": " << sampleErr/npt << ENDL;
1935  err += sampleErr;
1936  npoints += npt;
1937  }
1938  double totalAvgErr = err/(double)npoints;
1939  UINFO("stereo avg re projection error = %f", totalAvgErr);
1940  if(logStream) (*logStream) << "stereo avg re projection error = " << totalAvgErr << ENDL;
1941 
1942  output = StereoCameraModel(
1943  cameraName_.toStdString(),
1944  imageSize_[0], left.K_raw(), left.D_raw(), R1, P1,
1945  imageSize_[1], right.K_raw(), right.D_raw(), R2, P2,
1946  R, T, E, F);
1947  }
1948  else
1949  {
1950  UDEBUG("%s", cameraName_.toStdString().c_str());
1951  //Kinect, ignore the stereo rectification
1952  output = StereoCameraModel(
1953  cameraName_.toStdString(),
1954  imageSize_[0], left.K_raw(), left.D_raw(), left.R(), left.P(),
1955  imageSize_[1], right.K_raw(), right.D_raw(), right.R(), right.P(),
1956  R, T, E, F);
1957  }
1958  }
1959  return output;
1960 }
1961 
1962 bool CalibrationDialog::save()
1963 {
1964  bool saved = false;
1965  processingData_ = true;
1966  if(!stereo_)
1967  {
1968  UASSERT(models_[0].isValidForRectification());
1969  QString cameraName = models_[0].name().c_str();
1970  QString filePath = QFileDialog::getSaveFileName(this, tr("Export"), savingDirectory_+"/"+cameraName+".yaml", "*.yaml");
1971 
1972  if(!filePath.isEmpty())
1973  {
1974  QString name = QFileInfo(filePath).baseName();
1975  QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
1976  models_[0].setName(name.toStdString());
1977  if(models_[0].save(dir.toStdString()))
1978  {
1979  QMessageBox::information(this, tr("Export"), tr("Calibration file saved to \"%1\".").arg(filePath));
1980  UINFO("Saved \"%s\"!", filePath.toStdString().c_str());
1981  savedCalibration_ = true;
1982  saved = true;
1983  }
1984  else
1985  {
1986  UERROR("Error saving \"%s\"", filePath.toStdString().c_str());
1987  }
1988  }
1989  }
1990  else
1991  {
1992  UASSERT(stereoModel_.left().isValidForRectification() &&
1993  stereoModel_.right().isValidForRectification());
1994  QString cameraName = stereoModel_.name().c_str();
1995  QString filePath = QFileDialog::getSaveFileName(this, tr("Export"), savingDirectory_ + "/" + cameraName, "*.yaml");
1996  QString name = QFileInfo(filePath).baseName();
1997  QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
1998  if(!name.isEmpty())
1999  {
2000  bool switched = ui_->checkBox_switchImages->isChecked();
2001  stereoModel_.setName(name.toStdString(), switched?rightSuffix_.toStdString():leftSuffix_.toStdString(), switched?leftSuffix_.toStdString():rightSuffix_.toStdString());
2002  std::string base = (dir+QDir::separator()+name).toStdString();
2003  std::string leftPath = base+"_"+stereoModel_.getLeftSuffix()+".yaml";
2004  std::string rightPath = base+"_"+stereoModel_.getRightSuffix()+".yaml";
2005  std::string posePath = base+"_pose.yaml";
2006  if(stereoModel_.save(dir.toStdString(), false))
2007  {
2008  QMessageBox::information(this, tr("Export"), tr("Calibration files saved:\n \"%1\"\n \"%2\"\n \"%3\".").
2009  arg(leftPath.c_str()).arg(rightPath.c_str()).arg(posePath.c_str()));
2010  UINFO("Saved \"%s\" and \"%s\"!", leftPath.c_str(), rightPath.c_str());
2011  savedCalibration_ = true;
2012  saved = true;
2013  }
2014  else
2015  {
2016  UERROR("Error saving \"%s\" and \"%s\"", leftPath.c_str(), rightPath.c_str());
2017  }
2018  }
2019  }
2020  processingData_ = false;
2021  return saved;
2022 }
2023 
2024 float CalibrationDialog::getArea(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize)
2025 {
2026  //Get 2d image area of the detected checkerboard.
2027  //The projected checkerboard is assumed to be a convex quadrilateral, and the area computed as
2028  //|p X q|/2; see http://mathworld.wolfram.com/Quadrilateral.html.
2029 
2030  cv::Point2f up_left;
2031  cv::Point2f up_right;
2032  cv::Point2f down_right;
2033  cv::Point2f down_left;
2034  if((int)corners.size() == (boardSize.width * boardSize.height))
2035  {
2036  up_left = corners[0];
2037  up_right = corners[boardSize.width-1];
2038  down_right = corners[corners.size()-1];
2039  down_left = corners[corners.size()-boardSize.width];
2040  }
2041  else
2042  {
2043  cv::Rect rect = cv::boundingRect(corners);
2044  up_left = cv::Point2f(rect.x, rect.y);
2045  up_right = cv::Point2f(rect.x+rect.width, rect.y);
2046  down_right = cv::Point2f(rect.x+rect.width, rect.y+rect.height);
2047  down_left = cv::Point2f(rect.x, rect.y+rect.height);
2048  }
2049  cv::Point2f a = up_right - up_left;
2050  cv::Point2f b = down_right - up_right;
2051  cv::Point2f c = down_left - down_right;
2052  cv::Point2f p = b + c;
2053  cv::Point2f q = a + b;
2054  return std::fabs(p.x*q.y - p.y*q.x) / 2.0f;
2055 }
2056 
2057 float CalibrationDialog::getSkew(const std::vector<cv::Point2f> & fourCorners)
2058 {
2059  UASSERT(fourCorners.size() == 4);
2060  std::vector<cv::Point2f> corners = fourCorners;
2061  corners.resize(3);
2062  return getSkew(corners, cv::Size(2,1));
2063 }
2064 
2065 float CalibrationDialog::getSkew(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize)
2066 {
2067  // Get skew for given checkerboard detection.
2068  // Scaled to [0,1], which 0 = no skew, 1 = high skew
2069  // Skew is proportional to the divergence of three outside corners from 90 degrees.
2070 
2071  cv::Point2f up_left = corners[0];
2072  cv::Point2f up_right = corners[boardSize.width-1];
2073  cv::Point2f down_right = corners[corners.size()-1];
2074 
2075  // Return angle between lines ab, bc
2076  cv::Point2f ab = up_left - up_right;
2077  cv::Point2f cb = down_right - up_right;
2078  float angle = std::acos(ab.dot(cb) / (cv::norm(ab) * cv::norm(cb)));
2079 
2080  float r = 2.0f * std::fabs((CV_PI / 2.0f) - angle);
2081  return r > 1.0f?1.0f:r;
2082 }
2083 
2084 // x -> [0, 1] (left, right)
2085 // y -> [0, 1] (top, bottom)
2086 // size -> [0, 1] (small -> big)
2087 // skew -> [0, 1] (low, high)
2088 void CalibrationDialog::getParams(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize, const cv::Size & imageSize,
2089  float & x, float & y, float & size, float & skew)
2090 {
2091  float area = getArea(corners, boardSize);
2092  size = std::sqrt(area / (imageSize.width * imageSize.height));
2093  skew = getSkew(corners, boardSize);
2094  float meanX = 0.0f;
2095  float meanY = 0.0f;
2096  for(unsigned int i=0; i<corners.size(); ++i)
2097  {
2098  meanX += corners[i].x;
2099  meanY += corners[i].y;
2100  }
2101  meanX /= corners.size();
2102  meanY /= corners.size();
2103  x = meanX / imageSize.width;
2104  y = meanY / imageSize.height;
2105 }
2106 
2107 } /* namespace rtabmap */
rtabmap::CalibrationDialog::setProgressVisibility
void setProgressVisibility(bool visible)
Definition: CalibrationDialog.cpp:330
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
int
int
rtabmap::CalibrationDialog::handleEvent
virtual bool handleEvent(UEvent *event)
Definition: CalibrationDialog.cpp:562
rtabmap::CalibrationDialog::loadSettings
void loadSettings(QSettings &settings, const QString &group="")
Definition: CalibrationDialog.cpp:202
rtabmap::CalibrationDialog::~CalibrationDialog
virtual ~CalibrationDialog()
Definition: CalibrationDialog.cpp:171
rtabmap::CalibrationDialog::cameraName_
QString cameraName_
Definition: CalibrationDialog.h:135
rtabmap::CalibrationDialog::setSwitchedImages
void setSwitchedImages(bool switched)
Definition: CalibrationDialog.cpp:335
stereoRectifyFisheye.h
UINFO
#define UINFO(...)
rtabmap::CalibrationDialog::setRationalModel
void setRationalModel()
Definition: CalibrationDialog.cpp:350
name
rtabmap::StereoCameraModel
Definition: StereoCameraModel.h:35
rtabmap::CalibrationDialog::setMarkerLength
void setMarkerLength(double length)
Definition: CalibrationDialog.cpp:467
rtabmap::CameraModel::D_raw
cv::Mat D_raw() const
Definition: CameraModel.h:109
base
glm::length
GLM_FUNC_DECL genType::value_type length(genType const &x)
cy
const double cy
rtabmap::StereoCameraModel::getLeftSuffix
const std::string & getLeftSuffix() const
Definition: StereoCameraModel.h:125
rtabmap::CalibrationDialog::minIrs_
std::vector< unsigned short > minIrs_
Definition: CalibrationDialog.h:164
D
MatrixXcd D
bytes
arg::arg
constexpr arg(const char *name=nullptr)
rtabmap::CalibrationDialog::getArea
float getArea(const std::vector< cv::Point2f > &corners, const cv::Size &boardSize)
Definition: CalibrationDialog.cpp:2024
K
K
rtabmap::CalibrationDialog::getParams
void getParams(const std::vector< cv::Point2f > &corners, const cv::Size &boardSize, const cv::Size &imageSize, float &x, float &y, float &size, float &skew)
Definition: CalibrationDialog.cpp:2088
rtabmap::CalibrationDialog::objectPoints_
std::vector< std::vector< std::vector< cv::Point3f > > > objectPoints_
Definition: CalibrationDialog.h:155
s
RealScalar s
b
Array< int, 3, 1 > b
stream
stream
rtabmap::CalibrationDialog::leftSuffix_
QString leftSuffix_
Definition: CalibrationDialog.h:131
fx
const double fx
c
Scalar Scalar * c
rtabmap::CalibrationDialog::CalibrationDialog
CalibrationDialog(bool stereo=false, const QString &savingDirectory=".", bool switchImages=false, QWidget *parent=0)
Definition: CalibrationDialog.cpp:93
rtabmap::CalibrationDialog::currentId_
int currentId_
Definition: CalibrationDialog.h:138
rtabmap::CalibrationDialog::setSubpixelRefinement
void setSubpixelRefinement(bool enabled)
Definition: CalibrationDialog.cpp:484
size
Index size
clams::calibrate
DiscreteDepthDistortionModel RTABMAP_CORE_EXPORT calibrate(const std::map< int, rtabmap::SensorData > &sequence, const std::map< int, rtabmap::Transform > &trajectory, const pcl::PointCloud< pcl::PointXYZ >::Ptr &map, double coneRadius=0.02, double coneStdevThresh=0.03)
rtabmap::CameraModel
Definition: CameraModel.h:38
rtabmap::CameraModel::P
cv::Mat P() const
Definition: CameraModel.h:113
rtabmap::CalibrationDialog::boardHeight
int boardHeight() const
Definition: CalibrationDialog.cpp:394
rtabmap::StereoCameraModel::E
const cv::Mat & E() const
Definition: StereoCameraModel.h:112
rtabmap::CalibrationDialog::imageSize_
std::vector< cv::Size > imageSize_
Definition: CalibrationDialog.h:161
rtabmap::CalibrationDialog::imageParams_
std::vector< std::vector< std::vector< float > > > imageParams_
Definition: CalibrationDialog.h:156
rtabmap::CalibrationDialog::rightSuffix_
QString rightSuffix_
Definition: CalibrationDialog.h:132
type
E
int E
rtabmap::CalibrationDialog::boardWidth
int boardWidth() const
Definition: CalibrationDialog.cpp:390
rtabmap::StereoCameraModel::F
const cv::Mat & F() const
Definition: StereoCameraModel.h:113
rtabmap::StereoCameraModel::baseline
double baseline() const
Definition: StereoCameraModel.h:104
y
Matrix3f y
rtabmap::StereoCameraModel::getRightSuffix
const std::string & getRightSuffix() const
Definition: StereoCameraModel.h:126
corners
void corners(const MatrixType &m)
rtabmap::CalibrationDialog::getSkew
float getSkew(const std::vector< cv::Point2f > &fourCorners)
Definition: CalibrationDialog.cpp:2057
rows
int rows
rtabmap::CalibrationDialog::setBoardType
void setBoardType(int type)
Definition: CalibrationDialog.cpp:407
rtabmap::drawChessboard
cv::Mat drawChessboard(int squareSize, int boardWidth, int boardHeight, int borderSize)
Definition: CalibrationDialog.cpp:251
rtabmap::CalibrationDialog::stereoImagePoints_
std::vector< std::vector< std::vector< cv::Point2f > > > stereoImagePoints_
Definition: CalibrationDialog.h:158
glm::acos
GLM_FUNC_DECL genType acos(genType const &x)
R1
SO3 R1
fabs
Real fabs(const Real &a)
UEvent
Definition: UEvent.h:57
n
int n
rtabmap::CameraModel::K_raw
cv::Mat K_raw() const
Definition: CameraModel.h:108
rtabmap::CalibrationDialog::ui_
Ui_calibrationDialog * ui_
Definition: CalibrationDialog.h:167
rtabmap::StereoCameraModel::right
const CameraModel & right() const
Definition: StereoCameraModel.h:123
rtabmap::CalibrationDialog::models_
std::vector< rtabmap::CameraModel > models_
Definition: CalibrationDialog.h:162
UEvent::getClassName
virtual std::string getClassName() const =0
scale
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics scale
j
std::ptrdiff_t j
rtabmap::CalibrationDialog::setMarkerDictionary
void setMarkerDictionary(int dictionary)
Definition: CalibrationDialog.cpp:416
q
EIGEN_DEVICE_FUNC const Scalar & q
CalibrationDialog.h
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
filename
filename
rtabmap::CameraModel::R
cv::Mat R() const
Definition: CameraModel.h:112
rtabmap::CalibrationDialog::setFisheyeModel
void setFisheyeModel()
Definition: CalibrationDialog.cpp:340
rtabmap::CalibrationDialog::stereoImageIds_
std::vector< int > stereoImageIds_
Definition: CalibrationDialog.h:160
rtabmap::CalibrationDialog::processImages
void processImages(const cv::Mat &imageLeft, const cv::Mat &imageRight, const QString &cameraName)
Definition: CalibrationDialog.cpp:636
rtabmap::CameraModel::rectifyImage
cv::Mat rectifyImage(const cv::Mat &raw, int interpolation=cv::INTER_LINEAR) const
Definition: CameraModel.cpp:695
rtabmap::drawDetectedCornersCharuco
void drawDetectedCornersCharuco(cv::InputOutputArray image, cv::InputArray charucoCorners, cv::InputArray charucoIds=cv::noArray(), cv::Scalar cornerColor=cv::Scalar(255, 0, 0))
Definition: CalibrationDialog.cpp:609
arg
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
UCv2Qt.h
rtabmap::SensorEvent::kCodeData
@ kCodeData
Definition: SensorEvent.h:42
rtabmap::CalibrationDialog::setExpectedStereoBaseline
void setExpectedStereoBaseline(double length)
Definition: CalibrationDialog.cpp:511
SensorEvent.h
step
def step(data, isam, result, truth, currPoseIndex, isamArgs=())
rtabmap::CalibrationDialog::stereoCalibration
StereoCameraModel stereoCalibration(const CameraModel &left, const CameraModel &right, bool ignoreStereoRectification, QTextStream *logStream=0) const
Definition: CalibrationDialog.cpp:1659
UEventsHandler::unregisterFromEventsManager
void unregisterFromEventsManager()
Definition: UEventsHandler.cpp:33
rtabmap::CalibrationDialog::setSquareSize
void setSquareSize(double size)
Definition: CalibrationDialog.cpp:443
rtabmap::StereoCameraModel::setName
void setName(const std::string &name, const std::string &leftSuffix="left", const std::string &rightSuffix="right")
Definition: StereoCameraModel.cpp:163
rtabmap::StereoCameraModel::localTransform
const Transform & localTransform() const
Definition: StereoCameraModel.h:119
rtabmap::CalibrationDialog::unlock
void unlock()
Definition: CalibrationDialog.cpp:1325
rtabmap::StereoCameraModel::left
const CameraModel & left() const
Definition: StereoCameraModel.h:122
UASSERT
#define UASSERT(condition)
d
d
params
SmartProjectionParams params(gtsam::HESSIAN, gtsam::ZERO_ON_DEGENERACY)
rtabmap::CalibrationDialog::setStereoMode
void setStereoMode(bool stereo, const QString &leftSuffix="left", const QString &rightSuffix="right")
Definition: CalibrationDialog.cpp:355
x
x
rtabmap::CalibrationDialog::setPlumbobModel
void setPlumbobModel()
Definition: CalibrationDialog.cpp:345
p
Point3_ p(2)
Eigen::Triplet< double >
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
glm::angle
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
lines
void lines()
rtabmap::CalibrationDialog::closeEvent
virtual void closeEvent(QCloseEvent *event)
Definition: CalibrationDialog.cpp:527
rtabmap::CalibrationDialog::setMaxScale
void setMaxScale(int scale)
Definition: CalibrationDialog.cpp:519
F
Key F(std::uint64_t j)
rtabmap::CalibrationDialog::setBoardHeight
void setBoardHeight(int height)
Definition: CalibrationDialog.cpp:434
rtabmap::CalibrationDialog::savedCalibration_
bool savedCalibration_
Definition: CalibrationDialog.h:137
rtabmap::stereoRectifyFisheye
void stereoRectifyFisheye(cv::InputArray _cameraMatrix1, cv::InputArray _distCoeffs1, cv::InputArray _cameraMatrix2, cv::InputArray _distCoeffs2, cv::Size imageSize, cv::InputArray _Rmat, cv::InputArray _Tmat, cv::OutputArray _Rmat1, cv::OutputArray _Rmat2, cv::OutputArray _Pmat1, cv::OutputArray _Pmat2, cv::OutputArray _Qmat, int flags, double alpha, cv::Size newImageSize)
Definition: stereoRectifyFisheye.h:1181
rtabmap::CameraModel::isValidForRectification
bool isValidForRectification() const
Definition: CameraModel.h:89
UWARN
#define UWARN(...)
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
rtabmap::CalibrationDialog::markerLength
double markerLength() const
Definition: CalibrationDialog.cpp:402
rtabmap::CalibrationDialog::stereo_
bool stereo_
Definition: CalibrationDialog.h:130
rtabmap::StereoCameraModel::isValidForRectification
bool isValidForRectification() const
Definition: StereoCameraModel.h:86
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
rtabmap::CalibrationDialog::saveSettings
void saveSettings(QSettings &settings, const QString &group="") const
Definition: CalibrationDialog.cpp:177
Eigen::Quaternion
ULogger.h
ULogger class and convenient macros.
a
ArrayXXi a
R2
SO3 R2
empty
rtabmap::CalibrationDialog::calibrate
void calibrate()
Definition: CalibrationDialog.cpp:1330
rtabmap::CalibrationDialog::setCalibrationDataSaved
void setCalibrationDataSaved(bool enabled)
Definition: CalibrationDialog.cpp:502
rtabmap::CalibrationDialog::imagePoints_
std::vector< std::vector< std::vector< cv::Point2f > > > imagePoints_
Definition: CalibrationDialog.h:154
rtabmap::CalibrationDialog::restart
void restart()
Definition: CalibrationDialog.cpp:1142
rtabmap::CalibrationDialog::generateBoard
void generateBoard()
Definition: CalibrationDialog.cpp:268
rtabmap::CalibrationDialog::imageIds_
std::vector< std::vector< int > > imageIds_
Definition: CalibrationDialog.h:157
rtabmap::CalibrationDialog::setCameraName
void setCameraName(const QString &name)
Definition: CalibrationDialog.cpp:325
PointMatcherSupport::Parametrizable
ENDL
#define ENDL
Definition: CalibrationDialog.cpp:51
id
id
c_str
const char * c_str(Args &&...args)
ratio
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size ratio
UDEBUG
#define UDEBUG(...)
rtabmap::CalibrationDialog::squareSize
double squareSize() const
Definition: CalibrationDialog.cpp:398
rtabmap::StereoCameraModel::R
const cv::Mat & R() const
Definition: StereoCameraModel.h:110
rtabmap::CameraModel::imageSize
const cv::Size & imageSize() const
Definition: CameraModel.h:119
rtabmap::CameraModel::verticalFOV
double verticalFOV() const
Definition: CameraModel.cpp:690
rtabmap::CalibrationDialog::resetSettings
void resetSettings()
Definition: CalibrationDialog.cpp:242
rtabmap::CalibrationDialog::stereoObjectPoints_
std::vector< std::vector< cv::Point3f > > stereoObjectPoints_
Definition: CalibrationDialog.h:159
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
rtabmap::StereoCameraModel::initRectificationMap
void initRectificationMap()
Definition: StereoCameraModel.h:88
rtabmap::CalibrationDialog::processingData_
bool processingData_
Definition: CalibrationDialog.h:136
rtabmap::StereoCameraModel::name
const std::string & name() const
Definition: StereoCameraModel.h:92
float
float
rtabmap::CalibrationDialog::stereoModel_
rtabmap::StereoCameraModel stereoModel_
Definition: CalibrationDialog.h:163
false
#define false
Definition: ConvertUTF.c:56
rtabmap::CalibrationDialog::setBoardWidth
void setBoardWidth(int width)
Definition: CalibrationDialog.cpp:425
uCvMat2QImage
QImage uCvMat2QImage(const cv::Mat &image, bool isBgr=true, uCvQtDepthColorMap colorMap=uCvQtDepthWhiteToBlack, float depthMin=0, float depthMax=0)
Definition: UCv2Qt.h:47
cx
const double cx
cols
int cols
rtabmap::CalibrationDialog::chessboardPointIds_
std::vector< int > chessboardPointIds_
Definition: CalibrationDialog.h:142
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::CameraModel::horizontalFOV
double horizontalFOV() const
Definition: CameraModel.cpp:685
rtabmap::CalibrationDialog::savingDirectory_
QString savingDirectory_
Definition: CalibrationDialog.h:133
rtabmap::CalibrationDialog::save
bool save()
Definition: CalibrationDialog.cpp:1962
UERROR
#define UERROR(...)
trace.model
model
Definition: trace.py:4
rtabmap::CalibrationDialog::chessboardPoints_
std::vector< cv::Point3f > chessboardPoints_
Definition: CalibrationDialog.h:141
rtabmap::CalibrationDialog::maxIrs_
std::vector< unsigned short > maxIrs_
Definition: CalibrationDialog.h:165
value
value
i
int i
rtabmap::SensorEvent
Definition: SensorEvent.h:37
COUNT_MIN
#define COUNT_MIN
Definition: CalibrationDialog.cpp:91
rtabmap::CalibrationDialog::timestamp_
QString timestamp_
Definition: CalibrationDialog.h:139
rtabmap::StereoCameraModel::T
const cv::Mat & T() const
Definition: StereoCameraModel.h:111
fy
const double fy
rtabmap::CalibrationDialog::setSubpixelMaxError
void setSubpixelMaxError(double value)
Definition: CalibrationDialog.cpp:493
cvtColor
CvImagePtr cvtColor(const CvImageConstPtr &source, const std::string &encoding)
R
R

rtabmap
Author(s): Mathieu Labbe
autogenerated on Sun Dec 1 2024 03:42:41