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

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