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 
47 #include <rtabmap/core/CameraEvent.h>
48 #include <rtabmap/utilite/UCv2Qt.h>
49 
50 #include <rtabmap/utilite/ULogger.h>
51 
52 namespace rtabmap {
53 
54 #define COUNT_MIN 40
55 
56 CalibrationDialog::CalibrationDialog(bool stereo, const QString & savingDirectory, bool switchImages, QWidget * parent) :
57  QDialog(parent),
58  stereo_(stereo),
59  leftSuffix_("left"),
60  rightSuffix_("right"),
61  savingDirectory_(savingDirectory),
62  processingData_(false),
63  savedCalibration_(false)
64 {
65  imagePoints_.resize(2);
66  imageParams_.resize(2);
67  imageSize_.resize(2);
68  stereoImagePoints_.resize(2);
69  models_.resize(2);
70 
71  minIrs_.resize(2);
72  maxIrs_.resize(2);
73  minIrs_[0] = 0x0000;
74  maxIrs_[0] = 0x7fff;
75  minIrs_[1] = 0x0000;
76  maxIrs_[1] = 0x7fff;
77 
78  qRegisterMetaType<cv::Mat>("cv::Mat");
79 
80  ui_ = new Ui_calibrationDialog();
81  ui_->setupUi(this);
82 
83  connect(ui_->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate()));
84  connect(ui_->pushButton_restart, SIGNAL(clicked()), this, SLOT(restart()));
85  connect(ui_->pushButton_save, SIGNAL(clicked()), this, SLOT(save()));
86  connect(ui_->checkBox_switchImages, SIGNAL(stateChanged(int)), this, SLOT(restart()));
87  connect(ui_->checkBox_unlock, SIGNAL(stateChanged(int)), SLOT(unlock()));
88 
89  connect(ui_->spinBox_boardWidth, SIGNAL(valueChanged(int)), this, SLOT(setBoardWidth(int)));
90  connect(ui_->spinBox_boardHeight, SIGNAL(valueChanged(int)), this, SLOT(setBoardHeight(int)));
91  connect(ui_->doubleSpinBox_squareSize, SIGNAL(valueChanged(double)), this, SLOT(setSquareSize(double)));
92  connect(ui_->doubleSpinBox_stereoBaseline, SIGNAL(valueChanged(double)), this, SLOT(setExpectedStereoBaseline(double)));
93  connect(ui_->spinBox_maxScale, SIGNAL(valueChanged(int)), this, SLOT(setMaxScale(int)));
94 
95  connect(ui_->buttonBox, SIGNAL(rejected()), this, SLOT(close()));
96 
97  ui_->image_view->setFocus();
98 
99  ui_->progressBar_count->setMaximum(COUNT_MIN);
100  ui_->progressBar_count->setFormat("%v");
101  ui_->progressBar_count_2->setMaximum(COUNT_MIN);
102  ui_->progressBar_count_2->setFormat("%v");
103 
104  ui_->radioButton_raw->setChecked(true);
105 
106  ui_->checkBox_switchImages->setChecked(switchImages);
107 
108  ui_->checkBox_fisheye->setChecked(false);
109 
110  this->setStereoMode(stereo_);
111 }
112 
113 CalibrationDialog::~CalibrationDialog()
114 {
115  this->unregisterFromEventsManager();
116  delete ui_;
117 }
118 
119 void CalibrationDialog::saveSettings(QSettings & settings, const QString & group) const
120 {
121  if(!group.isEmpty())
122  {
123  settings.beginGroup(group);
124  }
125  settings.setValue("board_width", ui_->spinBox_boardWidth->value());
126  settings.setValue("board_height", ui_->spinBox_boardHeight->value());
127  settings.setValue("board_square_size", ui_->doubleSpinBox_squareSize->value());
128  settings.setValue("max_scale", ui_->spinBox_maxScale->value());
129  settings.setValue("geometry", this->saveGeometry());
130  if(!group.isEmpty())
131  {
132  settings.endGroup();
133  }
134 }
135 
136 void CalibrationDialog::loadSettings(QSettings & settings, const QString & group)
137 {
138  if(!group.isEmpty())
139  {
140  settings.beginGroup(group);
141  }
142  this->setBoardWidth(settings.value("board_width", ui_->spinBox_boardWidth->value()).toInt());
143  this->setBoardHeight(settings.value("board_height", ui_->spinBox_boardHeight->value()).toInt());
144  this->setSquareSize(settings.value("board_square_size", ui_->doubleSpinBox_squareSize->value()).toDouble());
145  this->setMaxScale(settings.value("max_scale", ui_->spinBox_maxScale->value()).toDouble());
146  QByteArray bytes = settings.value("geometry", QByteArray()).toByteArray();
147  if(!bytes.isEmpty())
148  {
149  this->restoreGeometry(bytes);
150  }
151  if(!group.isEmpty())
152  {
153  settings.endGroup();
154  }
155 }
156 
157 void CalibrationDialog::resetSettings()
158 {
159  this->setBoardWidth(8);
160  this->setBoardHeight(6);
161  this->setSquareSize(0.033);
162 }
163 
164 void CalibrationDialog::setCameraName(const QString & name)
165 {
166  cameraName_ = name;
167 }
168 
169 void CalibrationDialog::setProgressVisibility(bool visible)
170 {
171  ui_->groupBox_progress->setVisible(visible);
172 }
173 
174 void CalibrationDialog::setSwitchedImages(bool switched)
175 {
176  ui_->checkBox_switchImages->setChecked(switched);
177 }
178 
179 void CalibrationDialog::setFisheyeImages(bool enabled)
180 {
181  ui_->checkBox_fisheye->setChecked(enabled);
182 }
183 
184 void CalibrationDialog::setStereoMode(bool stereo, const QString & leftSuffix, const QString & rightSuffix)
185 {
186  leftSuffix_ = leftSuffix;
187  rightSuffix_ = rightSuffix;
188  this->restart();
189 
190  ui_->groupBox_progress->setVisible(true);
191  stereo_ = stereo;
192  ui_->progressBar_x_2->setVisible(stereo_);
193  ui_->progressBar_y_2->setVisible(stereo_);
194  ui_->progressBar_size_2->setVisible(stereo_);
195  ui_->progressBar_skew_2->setVisible(stereo_);
196  ui_->progressBar_count_2->setVisible(stereo_);
197  ui_->label_right->setVisible(stereo_);
198  ui_->image_view_2->setVisible(stereo_);
199  ui_->label_fx_2->setVisible(stereo_);
200  ui_->label_fy_2->setVisible(stereo_);
201  ui_->label_cx_2->setVisible(stereo_);
202  ui_->label_cy_2->setVisible(stereo_);
203  ui_->label_error_2->setVisible(stereo_);
204  ui_->label_baseline->setVisible(stereo_);
205  ui_->label_baseline_name->setVisible(stereo_);
206  ui_->lineEdit_K_2->setVisible(stereo_);
207  ui_->lineEdit_D_2->setVisible(stereo_);
208  ui_->lineEdit_R_2->setVisible(stereo_);
209  ui_->lineEdit_P_2->setVisible(stereo_);
210  ui_->radioButton_stereoRectified->setVisible(stereo_);
211  ui_->checkBox_switchImages->setVisible(stereo_);
212  ui_->doubleSpinBox_stereoBaseline->setVisible(stereo_);
213  ui_->label_stereoBaseline->setVisible(stereo_);
214 }
215 
216 int CalibrationDialog::boardWidth() const
217 {
218  return ui_->spinBox_boardWidth->value();
219 }
220 int CalibrationDialog::boardHeight() const
221 {
222  return ui_->spinBox_boardHeight->value();
223 }
224 double CalibrationDialog::squareSize() const
225 {
226  return ui_->doubleSpinBox_squareSize->value();
227 }
228 
229 void CalibrationDialog::setBoardWidth(int width)
230 {
231  if(width != ui_->spinBox_boardWidth->value())
232  {
233  ui_->spinBox_boardWidth->setValue(width);
234  this->restart();
235  }
236 }
237 
238 void CalibrationDialog::setBoardHeight(int height)
239 {
240  if(height != ui_->spinBox_boardHeight->value())
241  {
242  ui_->spinBox_boardHeight->setValue(height);
243  this->restart();
244  }
245 }
246 
247 void CalibrationDialog::setSquareSize(double size)
248 {
249  if(size != ui_->doubleSpinBox_squareSize->value())
250  {
251  ui_->doubleSpinBox_squareSize->setValue(size);
252  this->restart();
253  }
254 }
255 
256 void CalibrationDialog::setExpectedStereoBaseline(double length)
257 {
258  if(length != ui_->doubleSpinBox_stereoBaseline->value())
259  {
260  ui_->doubleSpinBox_stereoBaseline->setValue(length);
261  }
262 }
263 
264 void CalibrationDialog::setMaxScale(int scale)
265 {
266  if(scale != ui_->spinBox_maxScale->value())
267  {
268  ui_->spinBox_maxScale->setValue(scale);
269  }
270 }
271 
272 void CalibrationDialog::closeEvent(QCloseEvent* event)
273 {
274  if(!savedCalibration_ && models_[0].isValidForRectification() &&
275  (!stereo_ ||
276  (stereoModel_.isValidForRectification() &&
277  (!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0))))
278  {
279  QMessageBox::StandardButton b = QMessageBox::question(this, tr("Save calibration?"),
280  tr("The camera is calibrated but you didn't "
281  "save the calibration, do you want to save it?"),
282  QMessageBox::Yes | QMessageBox::Ignore | QMessageBox::Cancel, QMessageBox::Yes);
283  event->ignore();
284  if(b == QMessageBox::Yes)
285  {
286  if(this->save())
287  {
288  event->accept();
289  }
290  }
291  else if(b == QMessageBox::Ignore)
292  {
293  event->accept();
294  }
295  }
296  else
297  {
298  event->accept();
299  }
300  if(event->isAccepted())
301  {
302  this->unregisterFromEventsManager();
303  }
304 }
305 
306 bool CalibrationDialog::handleEvent(UEvent * event)
307 {
308  if(!processingData_)
309  {
310  if(event->getClassName().compare("CameraEvent") == 0)
311  {
312  rtabmap::CameraEvent * e = (rtabmap::CameraEvent *)event;
313  if(e->getCode() == rtabmap::CameraEvent::kCodeData)
314  {
315  processingData_ = true;
316  QMetaObject::invokeMethod(this, "processImages",
317  Q_ARG(cv::Mat, e->data().imageRaw()),
318  Q_ARG(cv::Mat, e->data().depthOrRightRaw()),
319  Q_ARG(QString, QString(e->cameraName().c_str())));
320  }
321  }
322  }
323  return false;
324 }
325 
326 void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat & imageRight, const QString & cameraName)
327 {
328  UDEBUG("Processing images");
329  processingData_ = true;
330  if(cameraName_.isEmpty())
331  {
332  cameraName_ = cameraName;
333  }
334  else if(cameraName.isEmpty())
335  {
336  cameraName_ = "0000";
337  }
338 
339  if(ui_->label_serial->text().compare(cameraName_)!=0)
340  {
341  ui_->label_serial->setText(cameraName_);
342 
343  }
344  std::vector<cv::Mat> inputRawImages(2);
345  if(ui_->checkBox_switchImages->isChecked())
346  {
347  inputRawImages[0] = imageRight;
348  inputRawImages[1] = imageLeft;
349  }
350  else
351  {
352  inputRawImages[0] = imageLeft;
353  inputRawImages[1] = imageRight;
354  }
355 
356  std::vector<cv::Mat> images(2);
357  images[0] = inputRawImages[0];
358  images[1] = inputRawImages[1];
359  imageSize_[0] = images[0].size();
360  imageSize_[1] = images[1].size();
361 
362  bool boardFound[2] = {false};
363  bool boardAccepted[2] = {false};
364  bool readyToCalibrate[2] = {false};
365 
366  std::vector<std::vector<cv::Point2f> > pointBuf(2);
367 
368  bool depthDetected = false;
369  for(int id=0; id<(stereo_?2:1); ++id)
370  {
371  cv::Mat viewGray;
372  if(!images[id].empty())
373  {
374  if(images[id].type() == CV_16UC1)
375  {
376  double min, max;
377  cv::minMaxLoc(images[id], &min, &max);
378  UDEBUG("Camera IR %d: min=%f max=%f", id, min, max);
379  if(minIrs_[id] == 0)
380  {
381  minIrs_[id] = min;
382  }
383  if(maxIrs_[id] == 0x7fff)
384  {
385  maxIrs_[id] = max;
386  }
387 
388  depthDetected = true;
389  //assume IR image: convert to gray scaled
390  const float factor = 255.0f / float((maxIrs_[id] - minIrs_[id]));
391  viewGray = cv::Mat(images[id].rows, images[id].cols, CV_8UC1);
392  for(int i=0; i<images[id].rows; ++i)
393  {
394  for(int j=0; j<images[id].cols; ++j)
395  {
396  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);
397  }
398  }
399  cvtColor(viewGray, images[id], cv::COLOR_GRAY2BGR); // convert to show detected points in color
400  }
401  else if(images[id].channels() == 3)
402  {
403  cvtColor(images[id], viewGray, cv::COLOR_BGR2GRAY);
404  }
405  else
406  {
407  viewGray = images[id];
408  cvtColor(viewGray, images[id], cv::COLOR_GRAY2BGR); // convert to show detected points in color
409  }
410  }
411  else
412  {
413  UERROR("Image %d is empty!! Should not!", id);
414  }
415 
416  minIrs_[id] = 0;
417  maxIrs_[id] = 0x7FFF;
418 
419  //Dot it only if not yet calibrated
420  if(!ui_->pushButton_save->isEnabled())
421  {
422  cv::Size boardSize(ui_->spinBox_boardWidth->value(), ui_->spinBox_boardHeight->value());
423  if(!viewGray.empty())
424  {
425  int flags = CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE;
426 
427  if(!viewGray.empty())
428  {
429  int maxScale = ui_->spinBox_maxScale->value();
430  for( int scale = 1; scale <= maxScale; scale++ )
431  {
432  cv::Mat timg;
433  if( scale == 1 )
434  timg = viewGray;
435  else
436  cv::resize(viewGray, timg, cv::Size(), scale, scale, CV_INTER_CUBIC);
437  boardFound[id] = cv::findChessboardCorners(timg, boardSize, pointBuf[id], flags);
438  if(boardFound[id])
439  {
440  if( scale > 1 )
441  {
442  cv::Mat cornersMat(pointBuf[id]);
443  cornersMat *= 1./scale;
444  }
445  break;
446  }
447  }
448  }
449  }
450 
451  if(boardFound[id]) // If done with success,
452  {
453  // improve the found corners' coordinate accuracy for chessboard
454  float minSquareDistance = -1.0f;
455  for(unsigned int i=0; i<pointBuf[id].size()-1; ++i)
456  {
457  float d = cv::norm(pointBuf[id][i] - pointBuf[id][i+1]);
458  if(minSquareDistance == -1.0f || minSquareDistance > d)
459  {
460  minSquareDistance = d;
461  }
462  }
463  float radius = minSquareDistance/2.0f +0.5f;
464  cv::cornerSubPix( viewGray, pointBuf[id], cv::Size(radius, radius), cv::Size(-1,-1),
465  cv::TermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1 ));
466 
467  // Draw the corners.
468  cv::drawChessboardCorners(images[id], boardSize, cv::Mat(pointBuf[id]), boardFound[id]);
469 
470  std::vector<float> params(4,0);
471  getParams(pointBuf[id], boardSize, imageSize_[id], params[0], params[1], params[2], params[3]);
472 
473  bool addSample = true;
474  for(unsigned int i=0; i<imageParams_[id].size(); ++i)
475  {
476  if(fabs(params[0] - imageParams_[id][i].at(0)) < 0.1 && // x
477  fabs(params[1] - imageParams_[id][i].at(1)) < 0.1 && // y
478  fabs(params[2] - imageParams_[id][i].at(2)) < 0.05 && // size
479  fabs(params[3] - imageParams_[id][i].at(3)) < 0.1) // skew
480  {
481  addSample = false;
482  }
483  }
484  if(addSample)
485  {
486  boardAccepted[id] = true;
487 
488  imagePoints_[id].push_back(pointBuf[id]);
489  imageParams_[id].push_back(params);
490 
491  UINFO("[%d] Added board, total=%d. (x=%f, y=%f, size=%f, skew=%f)", id, (int)imagePoints_[id].size(), params[0], params[1], params[2], params[3]);
492  }
493 
494  // update statistics
495  std::vector<float> xRange(2, imageParams_[id][0].at(0));
496  std::vector<float> yRange(2, imageParams_[id][0].at(1));
497  std::vector<float> sizeRange(2, imageParams_[id][0].at(2));
498  std::vector<float> skewRange(2, imageParams_[id][0].at(3));
499  for(unsigned int i=1; i<imageParams_[id].size(); ++i)
500  {
501  xRange[0] = imageParams_[id][i].at(0) < xRange[0] ? imageParams_[id][i].at(0) : xRange[0];
502  xRange[1] = imageParams_[id][i].at(0) > xRange[1] ? imageParams_[id][i].at(0) : xRange[1];
503  yRange[0] = imageParams_[id][i].at(1) < yRange[0] ? imageParams_[id][i].at(1) : yRange[0];
504  yRange[1] = imageParams_[id][i].at(1) > yRange[1] ? imageParams_[id][i].at(1) : yRange[1];
505  sizeRange[0] = imageParams_[id][i].at(2) < sizeRange[0] ? imageParams_[id][i].at(2) : sizeRange[0];
506  sizeRange[1] = imageParams_[id][i].at(2) > sizeRange[1] ? imageParams_[id][i].at(2) : sizeRange[1];
507  skewRange[0] = imageParams_[id][i].at(3) < skewRange[0] ? imageParams_[id][i].at(3) : skewRange[0];
508  skewRange[1] = imageParams_[id][i].at(3) > skewRange[1] ? imageParams_[id][i].at(3) : skewRange[1];
509  }
510  //UINFO("Stats [%d]:", id);
511  //UINFO(" Count = %d", (int)imagePoints_[id].size());
512  //UINFO(" x = [%f -> %f]", xRange[0], xRange[1]);
513  //UINFO(" y = [%f -> %f]", yRange[0], yRange[1]);
514  //UINFO(" size = [%f -> %f]", sizeRange[0], sizeRange[1]);
515  //UINFO(" skew = [%f -> %f]", skewRange[0], skewRange[1]);
516 
517  float xGood = xRange[1] - xRange[0];
518  float yGood = yRange[1] - yRange[0];
519  float sizeGood = sizeRange[1] - sizeRange[0];
520  float skewGood = skewRange[1] - skewRange[0];
521 
522  if(id == 0)
523  {
524  ui_->progressBar_x->setValue(xGood*100);
525  ui_->progressBar_y->setValue(yGood*100);
526  ui_->progressBar_size->setValue(sizeGood*100);
527  ui_->progressBar_skew->setValue(skewGood*100);
528  if((int)imagePoints_[id].size() > ui_->progressBar_count->maximum())
529  {
530  ui_->progressBar_count->setMaximum((int)imagePoints_[id].size());
531  }
532  ui_->progressBar_count->setValue((int)imagePoints_[id].size());
533  }
534  else
535  {
536  ui_->progressBar_x_2->setValue(xGood*100);
537  ui_->progressBar_y_2->setValue(yGood*100);
538  ui_->progressBar_size_2->setValue(sizeGood*100);
539  ui_->progressBar_skew_2->setValue(skewGood*100);
540 
541  if((int)imagePoints_[id].size() > ui_->progressBar_count_2->maximum())
542  {
543  ui_->progressBar_count_2->setMaximum((int)imagePoints_[id].size());
544  }
545  ui_->progressBar_count_2->setValue((int)imagePoints_[id].size());
546  }
547 
548  if(imagePoints_[id].size() >= COUNT_MIN &&
549  xGood > 0.5 &&
550  yGood > 0.5 &&
551  (sizeGood > 0.4 || (ui_->checkBox_fisheye->isChecked() && sizeGood > 0.25)) &&
552  skewGood > 0.5)
553  {
554  readyToCalibrate[id] = true;
555  }
556 
557  //update IR values
558  if(inputRawImages[id].type() == CV_16UC1)
559  {
560  //update min max IR if the chessboard was found
561  minIrs_[id] = 0xFFFF;
562  maxIrs_[id] = 0;
563  for(size_t i = 0; i < pointBuf[id].size(); ++i)
564  {
565  const cv::Point2f &p = pointBuf[id][i];
566  cv::Rect roi(std::max(0, (int)p.x - 3), std::max(0, (int)p.y - 3), 6, 6);
567 
568  roi.width = std::min(roi.width, inputRawImages[id].cols - roi.x);
569  roi.height = std::min(roi.height, inputRawImages[id].rows - roi.y);
570 
571  //find minMax in the roi
572  double min, max;
573  cv::minMaxLoc(inputRawImages[id](roi), &min, &max);
574  if(min < minIrs_[id])
575  {
576  minIrs_[id] = min;
577  }
578  if(max > maxIrs_[id])
579  {
580  maxIrs_[id] = max;
581  }
582  }
583  }
584  }
585  }
586  }
587  ui_->label_baseline->setVisible(!depthDetected);
588  ui_->label_baseline_name->setVisible(!depthDetected);
589 
590  if(stereo_ && ((boardAccepted[0] && boardFound[1]) || (boardAccepted[1] && boardFound[0])))
591  {
592  stereoImagePoints_[0].push_back(pointBuf[0]);
593  stereoImagePoints_[1].push_back(pointBuf[1]);
594  UINFO("Add stereo image points (size=%d)", (int)stereoImagePoints_[0].size());
595  }
596 
597  if(!stereo_ && readyToCalibrate[0])
598  {
599  unlock();
600  }
601  else if(stereo_ && readyToCalibrate[0] && readyToCalibrate[1] && stereoImagePoints_[0].size())
602  {
603  unlock();
604  }
605 
606  if(ui_->radioButton_rectified->isChecked())
607  {
608  if(models_[0].isValidForRectification())
609  {
610  images[0] = models_[0].rectifyImage(images[0]);
611  }
612  if(models_[1].isValidForRectification())
613  {
614  images[1] = models_[1].rectifyImage(images[1]);
615  }
616  }
617  else if(ui_->radioButton_stereoRectified->isChecked() &&
618  (stereoModel_.left().isValidForRectification() &&
619  stereoModel_.right().isValidForRectification()&&
620  (!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0)))
621  {
622  images[0] = stereoModel_.left().rectifyImage(images[0]);
623  images[1] = stereoModel_.right().rectifyImage(images[1]);
624  }
625 
626  if(ui_->checkBox_showHorizontalLines->isChecked())
627  {
628  for(int id=0; id<(stereo_?2:1); ++id)
629  {
630  int step = imageSize_[id].height/16;
631  for(int i=step; i<imageSize_[id].height; i+=step)
632  {
633  cv::line(images[id], cv::Point(0, i), cv::Point(imageSize_[id].width, i), CV_RGB(0,255,0));
634  }
635  }
636  }
637 
638  ui_->label_left->setText(tr("%1x%2").arg(images[0].cols).arg(images[0].rows));
639 
640  //show frame
641  ui_->image_view->setImage(uCvMat2QImage(images[0]).mirrored(ui_->checkBox_mirror->isChecked(), false));
642  if(stereo_)
643  {
644  ui_->label_right->setText(tr("%1x%2").arg(images[1].cols).arg(images[1].rows));
645  ui_->image_view_2->setImage(uCvMat2QImage(images[1]).mirrored(ui_->checkBox_mirror->isChecked(), false));
646  }
647  processingData_ = false;
648 }
649 
650 void CalibrationDialog::restart()
651 {
652  // restart
653  savedCalibration_ = false;
654  imagePoints_[0].clear();
655  imagePoints_[1].clear();
656  imageParams_[0].clear();
657  imageParams_[1].clear();
658  stereoImagePoints_[0].clear();
659  stereoImagePoints_[1].clear();
660  models_[0] = CameraModel();
661  models_[1] = CameraModel();
662  stereoModel_ = StereoCameraModel();
663  minIrs_[0] = 0x0000;
664  maxIrs_[0] = 0x7fff;
665  minIrs_[1] = 0x0000;
666  maxIrs_[1] = 0x7fff;
667 
668  ui_->pushButton_calibrate->setEnabled(ui_->checkBox_unlock->isChecked());
669  ui_->pushButton_save->setEnabled(false);
670  ui_->radioButton_raw->setChecked(true);
671  ui_->radioButton_rectified->setEnabled(false);
672  ui_->radioButton_stereoRectified->setEnabled(false);
673 
674  ui_->progressBar_count->reset();
675  ui_->progressBar_count->setMaximum(COUNT_MIN);
676  ui_->progressBar_x->reset();
677  ui_->progressBar_y->reset();
678  ui_->progressBar_size->reset();
679  ui_->progressBar_skew->reset();
680 
681  ui_->progressBar_count_2->reset();
682  ui_->progressBar_count_2->setMaximum(COUNT_MIN);
683  ui_->progressBar_x_2->reset();
684  ui_->progressBar_y_2->reset();
685  ui_->progressBar_size_2->reset();
686  ui_->progressBar_skew_2->reset();
687 
688  ui_->label_serial->clear();
689  ui_->label_fx->setNum(0);
690  ui_->label_fy->setNum(0);
691  ui_->label_cx->setNum(0);
692  ui_->label_cy->setNum(0);
693  ui_->label_baseline->setNum(0);
694  ui_->label_error->setNum(0);
695  ui_->lineEdit_K->clear();
696  ui_->lineEdit_D->clear();
697  ui_->lineEdit_R->clear();
698  ui_->lineEdit_P->clear();
699  ui_->label_fx_2->setNum(0);
700  ui_->label_fy_2->setNum(0);
701  ui_->label_cx_2->setNum(0);
702  ui_->label_cy_2->setNum(0);
703  ui_->lineEdit_K_2->clear();
704  ui_->lineEdit_D_2->clear();
705  ui_->lineEdit_R_2->clear();
706  ui_->lineEdit_P_2->clear();
707 }
708 
709 void CalibrationDialog::unlock()
710 {
711  ui_->pushButton_calibrate->setEnabled(true);
712 }
713 
714 void CalibrationDialog::calibrate()
715 {
716  processingData_ = true;
717  savedCalibration_ = false;
718 
719  QMessageBox mb(QMessageBox::Information,
720  tr("Calibrating..."),
721  tr("Operation in progress..."));
722  mb.show();
723  QApplication::processEvents();
724  uSleep(100); // hack make sure the text in the QMessageBox is shown...
725  QApplication::processEvents();
726 
727  std::vector<std::vector<cv::Point3f> > objectPoints(1);
728  cv::Size boardSize(ui_->spinBox_boardWidth->value(), ui_->spinBox_boardHeight->value());
729  float squareSize = ui_->doubleSpinBox_squareSize->value();
730  // compute board corner positions
731  for( int i = 0; i < boardSize.height; ++i )
732  for( int j = 0; j < boardSize.width; ++j )
733  objectPoints[0].push_back(cv::Point3f(float( j*squareSize ), float( i*squareSize ), 0));
734 
735  for(int id=0; id<(stereo_?2:1); ++id)
736  {
737  UINFO("Calibrating camera %d (samples=%d)", id, (int)imagePoints_[id].size());
738 
739  objectPoints.resize(imagePoints_[id].size(), objectPoints[0]);
740 
741  //calibrate
742  std::vector<cv::Mat> rvecs, tvecs;
743  std::vector<float> reprojErrs;
744  cv::Mat K, D;
745  K = cv::Mat::eye(3,3,CV_64FC1);
746  UINFO("calibrate!");
747  //Find intrinsic and extrinsic camera parameters
748  double rms = 0.0;
749 #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)))
750  bool fishEye = ui_->checkBox_fisheye->isChecked();
751 
752  if(fishEye)
753  {
754  try
755  {
756  rms = cv::fisheye::calibrate(objectPoints,
757  imagePoints_[id],
758  imageSize_[id],
759  K,
760  D,
761  rvecs,
762  tvecs,
763  cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC |
764  cv::fisheye::CALIB_CHECK_COND |
765  cv::fisheye::CALIB_FIX_SKEW);
766  }
767  catch(const cv::Exception & e)
768  {
769  UERROR("Error: %s (try restarting the calibration)", e.what());
770  QMessageBox::warning(this, tr("Calibration failed!"), tr("Error: %1 (try restarting the calibration)").arg(e.what()));
771  processingData_ = false;
772  return;
773  }
774  }
775  else
776 #endif
777  {
778  rms = cv::calibrateCamera(objectPoints,
779  imagePoints_[id],
780  imageSize_[id],
781  K,
782  D,
783  rvecs,
784  tvecs);
785  }
786 
787  UINFO("Re-projection error reported by calibrateCamera: %f", rms);
788 
789  // compute reprojection errors
790  std::vector<cv::Point2f> imagePoints2;
791  int i, totalPoints = 0;
792  double totalErr = 0, err;
793  reprojErrs.resize(objectPoints.size());
794 
795  for( i = 0; i < (int)objectPoints.size(); ++i )
796  {
797 #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)))
798  if(fishEye)
799  {
800  cv::fisheye::projectPoints( cv::Mat(objectPoints[i]), imagePoints2, rvecs[i], tvecs[i], K, D);
801  }
802  else
803 #endif
804  {
805  cv::projectPoints( cv::Mat(objectPoints[i]), rvecs[i], tvecs[i], K, D, imagePoints2);
806  }
807  err = cv::norm(cv::Mat(imagePoints_[id][i]), cv::Mat(imagePoints2), CV_L2);
808 
809  int n = (int)objectPoints[i].size();
810  reprojErrs[i] = (float) std::sqrt(err*err/n);
811  totalErr += err*err;
812  totalPoints += n;
813  }
814 
815  double totalAvgErr = std::sqrt(totalErr/totalPoints);
816 
817  UINFO("avg re projection error = %f", totalAvgErr);
818 
819  cv::Mat P(3,4,CV_64FC1);
820  P.at<double>(2,3) = 1;
821  K.copyTo(P.colRange(0,3).rowRange(0,3));
822 
823 #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)))
824  if(fishEye)
825  {
826  // Convert to unified distortion model (k1,k2,p1,p2,k3,k4)
827  cv::Mat newD = cv::Mat::zeros(1,6,CV_64FC1);
828  newD.at<double>(0,0) = D.at<double>(0,0);
829  newD.at<double>(0,1) = D.at<double>(0,1);
830  newD.at<double>(0,4) = D.at<double>(0,2);
831  newD.at<double>(0,5) = D.at<double>(0,3);
832  D = newD;
833  }
834 #endif
835 
836  std::cout << "K = " << K << std::endl;
837  std::cout << "D = " << D << std::endl;
838  std::cout << "width = " << imageSize_[id].width << std::endl;
839  std::cout << "height = " << imageSize_[id].height << std::endl;
840 
841  models_[id] = CameraModel(cameraName_.toStdString(), imageSize_[id], K, D, cv::Mat::eye(3,3,CV_64FC1), P);
842 
843  if(id == 0)
844  {
845  ui_->label_fx->setNum(models_[id].fx());
846  ui_->label_fy->setNum(models_[id].fy());
847  ui_->label_cx->setNum(models_[id].cx());
848  ui_->label_cy->setNum(models_[id].cy());
849  ui_->label_error->setNum(totalAvgErr);
850 
851  std::stringstream strK, strD, strR, strP;
852  strK << models_[id].K_raw();
853  strD << models_[id].D_raw();
854  strR << models_[id].R();
855  strP << models_[id].P();
856  ui_->lineEdit_K->setText(strK.str().c_str());
857  ui_->lineEdit_D->setText(strD.str().c_str());
858  ui_->lineEdit_R->setText(strR.str().c_str());
859  ui_->lineEdit_P->setText(strP.str().c_str());
860  }
861  else
862  {
863  ui_->label_fx_2->setNum(models_[id].fx());
864  ui_->label_fy_2->setNum(models_[id].fy());
865  ui_->label_cx_2->setNum(models_[id].cx());
866  ui_->label_cy_2->setNum(models_[id].cy());
867  ui_->label_error_2->setNum(totalAvgErr);
868 
869  std::stringstream strK, strD, strR, strP;
870  strK << models_[id].K_raw();
871  strD << models_[id].D_raw();
872  strR << models_[id].R();
873  strP << models_[id].P();
874  ui_->lineEdit_K_2->setText(strK.str().c_str());
875  ui_->lineEdit_D_2->setText(strD.str().c_str());
876  ui_->lineEdit_R_2->setText(strR.str().c_str());
877  ui_->lineEdit_P_2->setText(strP.str().c_str());
878  }
879  }
880 
881  if(stereo_ && models_[0].isValidForRectification() && models_[1].isValidForRectification())
882  {
883  stereoModel_ = stereoCalibration(models_[0], models_[1], false);
884 
885  if(stereoModel_.isValidForProjection() &&
886  ui_->doubleSpinBox_stereoBaseline->value() > 0 &&
887  stereoModel_.baseline() != ui_->doubleSpinBox_stereoBaseline->value())
888  {
889  UWARN("Expected stereo baseline is set to %f m, but computed baseline is %f m. Rescaling baseline...",
890  stereoModel_.baseline(), ui_->doubleSpinBox_stereoBaseline->value());
891  cv::Mat P = stereoModel_.right().P().clone();
892  P.at<double>(0,3) = -P.at<double>(0,0)*ui_->doubleSpinBox_stereoBaseline->value();
893  double scale = ui_->doubleSpinBox_stereoBaseline->value() / stereoModel_.baseline();
894  UWARN("Scale %f (setting square size from %f to %f)", scale, ui_->doubleSpinBox_squareSize->value(), ui_->doubleSpinBox_squareSize->value()*scale);
895  ui_->doubleSpinBox_squareSize->setValue(ui_->doubleSpinBox_squareSize->value()*scale);
896  stereoModel_ = StereoCameraModel(
897  stereoModel_.name(),
898  stereoModel_.left().imageSize(),stereoModel_.left().K_raw(), stereoModel_.left().D_raw(), stereoModel_.left().R(), stereoModel_.left().P(),
899  stereoModel_.right().imageSize(), stereoModel_.right().K_raw(), stereoModel_.right().D_raw(), stereoModel_.right().R(), P,
900  stereoModel_.R(), stereoModel_.T()*scale, stereoModel_.E(), stereoModel_.F(), stereoModel_.localTransform());
901  }
902 
903  std::stringstream strR1, strP1, strR2, strP2;
904  strR1 << stereoModel_.left().R();
905  strP1 << stereoModel_.left().P();
906  strR2 << stereoModel_.right().R();
907  strP2 << stereoModel_.right().P();
908  ui_->lineEdit_R->setText(strR1.str().c_str());
909  ui_->lineEdit_P->setText(strP1.str().c_str());
910  ui_->lineEdit_R_2->setText(strR2.str().c_str());
911  ui_->lineEdit_P_2->setText(strP2.str().c_str());
912 
913  ui_->label_baseline->setNum(stereoModel_.baseline());
914  //ui_->label_error_stereo->setNum(totalAvgErr);
915 
916  if(!stereoModel_.left().P().empty() &&
917  !stereoModel_.left().K_raw().empty() &&
918  fabs(stereoModel_.left().P().at<double>(0,0) - stereoModel_.left().K_raw().at<double>(0,0)) > 5)
919  {
920  QMessageBox::warning(this, tr("Stereo Calibration"),
921  tr("\"fx\" after stereo rectification (%1) is not close from original "
922  "calibration (%2), the difference would have to be under 5. You may "
923  "restart the calibration or keep it as is.")
924  .arg(stereoModel_.left().P().at<double>(0,0))
925  .arg(stereoModel_.left().K_raw().at<double>(0,0)));
926  }
927  }
928 
929  if(stereo_)
930  {
931  if(models_[0].isValidForRectification())
932  {
933  models_[0].initRectificationMap();
934  }
935  if(models_[1].isValidForRectification())
936  {
937  models_[1].initRectificationMap();
938  }
939  if(models_[0].isValidForRectification() || models_[1].isValidForRectification())
940  {
941  ui_->radioButton_rectified->setEnabled(true);
942  }
943  if(stereoModel_.isValidForRectification())
944  {
945  stereoModel_.initRectificationMap();
946  ui_->radioButton_stereoRectified->setEnabled(true);
947  ui_->radioButton_stereoRectified->setChecked(true);
948  ui_->pushButton_save->setEnabled(true);
949  }
950  else
951  {
952  ui_->radioButton_rectified->setChecked(ui_->radioButton_rectified->isEnabled());
953  }
954  }
955  else if(models_[0].isValidForRectification())
956  {
957  models_[0].initRectificationMap();
958  ui_->radioButton_rectified->setEnabled(true);
959  ui_->radioButton_rectified->setChecked(true);
960  ui_->pushButton_save->setEnabled(true);
961  }
962 
963  UINFO("End calibration");
964  processingData_ = false;
965 }
966 
967 StereoCameraModel CalibrationDialog::stereoCalibration(const CameraModel & left, const CameraModel & right, bool ignoreStereoRectification) const
968 {
969  StereoCameraModel output;
970  if (stereoImagePoints_[0].empty())
971  {
972  UERROR("No stereo correspondences!");
973  return output;
974  }
975  UINFO("stereo calibration (samples=%d)...", (int)stereoImagePoints_[0].size());
976 
977  if (left.K_raw().empty() || left.D_raw().empty())
978  {
979  UERROR("Empty intrinsic parameters (K, D) for the %s camera! Aborting stereo calibration...", leftSuffix_.toStdString().c_str());
980  return output;
981  }
982  if (right.K_raw().empty() || right.D_raw().empty())
983  {
984  UERROR("Empty intrinsic parameters (K, D) for the %s camera! Aborting stereo calibration...", rightSuffix_.toStdString().c_str());
985  return output;
986  }
987 
988  if (left.imageSize() != imageSize_[0])
989  {
990  UERROR("left model (%dx%d) has not the same size as the processed images (%dx%d)",
991  left.imageSize().width, left.imageSize().height,
992  imageSize_[0].width, imageSize_[0].height);
993  return output;
994  }
995  if (right.imageSize() != imageSize_[1])
996  {
997  UERROR("right model (%dx%d) has not the same size as the processed images (%dx%d)",
998  right.imageSize().width, right.imageSize().height,
999  imageSize_[1].width, imageSize_[1].height);
1000  return output;
1001  }
1002 
1003  cv::Size imageSize = imageSize_[0].width > imageSize_[1].width ? imageSize_[0] : imageSize_[1];
1004  cv::Mat R, T, E, F;
1005 
1006  cv::Size boardSize(ui_->spinBox_boardWidth->value(), ui_->spinBox_boardHeight->value());
1007  float squareSize = ui_->doubleSpinBox_squareSize->value();
1008 
1009  double rms = 0.0;
1010 #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)))
1011  bool fishEye = left.D_raw().cols == 6;
1012  // calibrate extrinsic
1013  if(fishEye)
1014  {
1015  // compute board corner positions
1016  std::vector<std::vector<cv::Point3d> > objectPoints(1);
1017  for (int i = 0; i < boardSize.height; ++i)
1018  for (int j = 0; j < boardSize.width; ++j)
1019  objectPoints[0].push_back(cv::Point3d(double(j*squareSize), double(i*squareSize), 0));
1020  objectPoints.resize(stereoImagePoints_[0].size(), objectPoints[0]);
1021 
1022  cv::Vec3d Tvec;
1023  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));
1024  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));
1025 
1026  UASSERT(stereoImagePoints_[0].size() == stereoImagePoints_[1].size());
1027  std::vector<std::vector<cv::Point2d> > leftPoints(stereoImagePoints_[0].size());
1028  std::vector<std::vector<cv::Point2d> > rightPoints(stereoImagePoints_[1].size());
1029  for(unsigned int i =0; i<stereoImagePoints_[0].size(); ++i)
1030  {
1031  UASSERT(stereoImagePoints_[0][i].size() == stereoImagePoints_[1][i].size());
1032  leftPoints[i].resize(stereoImagePoints_[0][i].size());
1033  rightPoints[i].resize(stereoImagePoints_[1][i].size());
1034  for(unsigned int j =0; j<stereoImagePoints_[0][i].size(); ++j)
1035  {
1036  leftPoints[i][j].x = stereoImagePoints_[0][i][j].x;
1037  leftPoints[i][j].y = stereoImagePoints_[0][i][j].y;
1038  rightPoints[i][j].x = stereoImagePoints_[1][i][j].x;
1039  rightPoints[i][j].y = stereoImagePoints_[1][i][j].y;
1040  }
1041  }
1042 
1043  try
1044  {
1045  rms = cv::fisheye::stereoCalibrate(
1046  objectPoints,
1047  leftPoints,
1048  rightPoints,
1049  left.K_raw(), D_left, right.K_raw(), D_right,
1050  imageSize, R, Tvec,
1051  cv::fisheye::CALIB_FIX_INTRINSIC,
1052  cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
1053  UINFO("stereo calibration... done with RMS error=%f", rms);
1054  }
1055  catch(const cv::Exception & e)
1056  {
1057  UERROR("Error: %s (try restarting the calibration)", e.what());
1058  return output;
1059  }
1060 
1061  std::cout << "R = " << R << std::endl;
1062  std::cout << "T = " << Tvec << std::endl;
1063 
1064  if(imageSize_[0] == imageSize_[1] && !ignoreStereoRectification)
1065  {
1066  UINFO("Compute stereo rectification");
1067 
1068  cv::Mat R1, R2, P1, P2, Q;
1069  stereoRectifyFisheye(
1070  left.K_raw(), D_left,
1071  right.K_raw(), D_right,
1072  imageSize, R, Tvec, R1, R2, P1, P2, Q,
1073  cv::CALIB_ZERO_DISPARITY, 0, imageSize);
1074 
1075  // Very hard to get good results with this one:
1076  /*double balance = 0.0, fov_scale = 1.0;
1077  cv::fisheye::stereoRectify(
1078  left.K_raw(), D_left,
1079  right.K_raw(), D_right,
1080  imageSize, R, Tvec, R1, R2, P1, P2, Q,
1081  cv::CALIB_ZERO_DISPARITY, imageSize, balance, fov_scale);*/
1082 
1083  std::cout << "R1 = " << R1 << std::endl;
1084  std::cout << "R2 = " << R2 << std::endl;
1085  std::cout << "P1 = " << P1 << std::endl;
1086  std::cout << "P2 = " << P2 << std::endl;
1087 
1088  // Re-zoom to original focal distance
1089  if(P1.at<double>(0,0) < 0)
1090  {
1091  P1.at<double>(0,0) *= -1;
1092  P1.at<double>(1,1) *= -1;
1093  }
1094  if(P2.at<double>(0,0) < 0)
1095  {
1096  P2.at<double>(0,0) *= -1;
1097  P2.at<double>(1,1) *= -1;
1098  }
1099  if(P2.at<double>(0,3) > 0)
1100  {
1101  P2.at<double>(0,3) *= -1;
1102  }
1103  P2.at<double>(0,3) = P2.at<double>(0,3) * left.K_raw().at<double>(0,0) / P2.at<double>(0,0);
1104  P1.at<double>(0,0) = P1.at<double>(1,1) = left.K_raw().at<double>(0,0);
1105  P2.at<double>(0,0) = P2.at<double>(1,1) = left.K_raw().at<double>(0,0);
1106 
1107  std::cout << "P1n = " << P1 << std::endl;
1108  std::cout << "P2n = " << P2 << std::endl;
1109 
1110 
1111  cv::Mat T(3,1,CV_64FC1);
1112  T.at <double>(0,0) = Tvec[0];
1113  T.at <double>(1,0) = Tvec[1];
1114  T.at <double>(2,0) = Tvec[2];
1115  output = StereoCameraModel(
1116  cameraName_.toStdString(),
1117  imageSize_[0], left.K_raw(), left.D_raw(), R1, P1,
1118  imageSize_[1], right.K_raw(), right.D_raw(), R2, P2,
1119  R, T, E, F);
1120  }
1121  else
1122  {
1123  UDEBUG("%s", cameraName_.toStdString().c_str());
1124  //Kinect, ignore the stereo rectification
1125  output = StereoCameraModel(
1126  cameraName_.toStdString(),
1127  imageSize_[0], left.K_raw(), left.D_raw(), left.R(), left.P(),
1128  imageSize_[1], right.K_raw(), right.D_raw(), right.R(), right.P(),
1129  R, T, E, F);
1130  }
1131  }
1132  else
1133 #endif
1134  {
1135  // compute board corner positions
1136  std::vector<std::vector<cv::Point3f> > objectPoints(1);
1137  for (int i = 0; i < boardSize.height; ++i)
1138  for (int j = 0; j < boardSize.width; ++j)
1139  objectPoints[0].push_back(cv::Point3f(float(j*squareSize), float(i*squareSize), 0));
1140  objectPoints.resize(stereoImagePoints_[0].size(), objectPoints[0]);
1141 
1142 #if CV_MAJOR_VERSION < 3
1143  rms = cv::stereoCalibrate(
1144  objectPoints,
1145  stereoImagePoints_[0],
1146  stereoImagePoints_[1],
1147  left.K_raw(), left.D_raw(),
1148  right.K_raw(), right.D_raw(),
1149  imageSize, R, T, E, F,
1150  cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5),
1151  cv::CALIB_FIX_INTRINSIC);
1152 #else
1153  rms = cv::stereoCalibrate(
1154  objectPoints,
1155  stereoImagePoints_[0],
1156  stereoImagePoints_[1],
1157  left.K_raw(), left.D_raw(),
1158  right.K_raw(), right.D_raw(),
1159  imageSize, R, T, E, F,
1160  cv::CALIB_FIX_INTRINSIC,
1161  cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
1162 #endif
1163  UINFO("stereo calibration... done with RMS error=%f", rms);
1164 
1165  std::cout << "R = " << R << std::endl;
1166  std::cout << "T = " << T << std::endl;
1167  std::cout << "E = " << E << std::endl;
1168  std::cout << "F = " << F << std::endl;
1169 
1170  if(imageSize_[0] == imageSize_[1] && !ignoreStereoRectification)
1171  {
1172  UINFO("Compute stereo rectification");
1173 
1174  cv::Mat R1, R2, P1, P2, Q;
1175  cv::stereoRectify(left.K_raw(), left.D_raw(),
1176  right.K_raw(), right.D_raw(),
1177  imageSize, R, T, R1, R2, P1, P2, Q,
1178  cv::CALIB_ZERO_DISPARITY, 0, imageSize);
1179 
1180  std::cout << "R1 = " << R1 << std::endl;
1181  std::cout << "P1 = " << P1 << std::endl;
1182  std::cout << "R2 = " << R2 << std::endl;
1183  std::cout << "P2 = " << P2 << std::endl;
1184 
1185  double err = 0;
1186  int npoints = 0;
1187  std::vector<cv::Vec3f> lines[2];
1188  UINFO("Computing avg re-projection error...");
1189  for(unsigned int i = 0; i < stereoImagePoints_[0].size(); i++ )
1190  {
1191  int npt = (int)stereoImagePoints_[0][i].size();
1192 
1193  cv::Mat imgpt0 = cv::Mat(stereoImagePoints_[0][i]);
1194  cv::Mat imgpt1 = cv::Mat(stereoImagePoints_[1][i]);
1195  cv::undistortPoints(imgpt0, imgpt0, left.K_raw(), left.D_raw(), R1, P1);
1196  cv::undistortPoints(imgpt1, imgpt1, right.K_raw(), right.D_raw(), R2, P2);
1197  computeCorrespondEpilines(imgpt0, 1, F, lines[0]);
1198  computeCorrespondEpilines(imgpt1, 2, F, lines[1]);
1199 
1200  for(int j = 0; j < npt; j++ )
1201  {
1202  double errij = fabs(stereoImagePoints_[0][i][j].x*lines[1][j][0] +
1203  stereoImagePoints_[0][i][j].y*lines[1][j][1] + lines[1][j][2]) +
1204  fabs(stereoImagePoints_[1][i][j].x*lines[0][j][0] +
1205  stereoImagePoints_[1][i][j].y*lines[0][j][1] + lines[0][j][2]);
1206  err += errij;
1207  }
1208  npoints += npt;
1209  }
1210  double totalAvgErr = err/(double)npoints;
1211  UINFO("stereo avg re projection error = %f", totalAvgErr);
1212 
1213  output = StereoCameraModel(
1214  cameraName_.toStdString(),
1215  imageSize_[0], left.K_raw(), left.D_raw(), R1, P1,
1216  imageSize_[1], right.K_raw(), right.D_raw(), R2, P2,
1217  R, T, E, F);
1218  }
1219  else
1220  {
1221  UDEBUG("%s", cameraName_.toStdString().c_str());
1222  //Kinect, ignore the stereo rectification
1223  output = StereoCameraModel(
1224  cameraName_.toStdString(),
1225  imageSize_[0], left.K_raw(), left.D_raw(), left.R(), left.P(),
1226  imageSize_[1], right.K_raw(), right.D_raw(), right.R(), right.P(),
1227  R, T, E, F);
1228  }
1229  }
1230  return output;
1231 }
1232 
1233 bool CalibrationDialog::save()
1234 {
1235  bool saved = false;
1236  processingData_ = true;
1237  if(!stereo_)
1238  {
1239  UASSERT(models_[0].isValidForRectification());
1240  QString cameraName = models_[0].name().c_str();
1241  QString filePath = QFileDialog::getSaveFileName(this, tr("Export"), savingDirectory_+"/"+cameraName+".yaml", "*.yaml");
1242 
1243  if(!filePath.isEmpty())
1244  {
1245  QString name = QFileInfo(filePath).baseName();
1246  QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
1247  models_[0].setName(name.toStdString());
1248  if(models_[0].save(dir.toStdString()))
1249  {
1250  QMessageBox::information(this, tr("Export"), tr("Calibration file saved to \"%1\".").arg(filePath));
1251  UINFO("Saved \"%s\"!", filePath.toStdString().c_str());
1252  savedCalibration_ = true;
1253  saved = true;
1254  }
1255  else
1256  {
1257  UERROR("Error saving \"%s\"", filePath.toStdString().c_str());
1258  }
1259  }
1260  }
1261  else
1262  {
1263  UASSERT(stereoModel_.left().isValidForRectification() &&
1264  stereoModel_.right().isValidForRectification());
1265  QString cameraName = stereoModel_.name().c_str();
1266  QString filePath = QFileDialog::getSaveFileName(this, tr("Export"), savingDirectory_ + "/" + cameraName, "*.yaml");
1267  QString name = QFileInfo(filePath).baseName();
1268  QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
1269  if(!name.isEmpty())
1270  {
1271  bool switched = ui_->checkBox_switchImages->isChecked();
1272  stereoModel_.setName(name.toStdString(), switched?rightSuffix_.toStdString():leftSuffix_.toStdString(), switched?leftSuffix_.toStdString():rightSuffix_.toStdString());
1273  std::string base = (dir+QDir::separator()+name).toStdString();
1274  std::string leftPath = base+"_"+stereoModel_.getLeftSuffix()+".yaml";
1275  std::string rightPath = base+"_"+stereoModel_.getRightSuffix()+".yaml";
1276  std::string posePath = base+"_pose.yaml";
1277  if(stereoModel_.save(dir.toStdString(), false))
1278  {
1279  QMessageBox::information(this, tr("Export"), tr("Calibration files saved:\n \"%1\"\n \"%2\"\n \"%3\".").
1280  arg(leftPath.c_str()).arg(rightPath.c_str()).arg(posePath.c_str()));
1281  UINFO("Saved \"%s\" and \"%s\"!", leftPath.c_str(), rightPath.c_str());
1282  savedCalibration_ = true;
1283  saved = true;
1284  }
1285  else
1286  {
1287  UERROR("Error saving \"%s\" and \"%s\"", leftPath.c_str(), rightPath.c_str());
1288  }
1289  }
1290  }
1291  processingData_ = false;
1292  return saved;
1293 }
1294 
1295 float CalibrationDialog::getArea(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize)
1296 {
1297  //Get 2d image area of the detected checkerboard.
1298  //The projected checkerboard is assumed to be a convex quadrilateral, and the area computed as
1299  //|p X q|/2; see http://mathworld.wolfram.com/Quadrilateral.html.
1300 
1301  cv::Point2f up_left = corners[0];
1302  cv::Point2f up_right = corners[boardSize.width-1];
1303  cv::Point2f down_right = corners[corners.size()-1];
1304  cv::Point2f down_left = corners[corners.size()-boardSize.width];
1305  cv::Point2f a = up_right - up_left;
1306  cv::Point2f b = down_right - up_right;
1307  cv::Point2f c = down_left - down_right;
1308  cv::Point2f p = b + c;
1309  cv::Point2f q = a + b;
1310  return std::fabs(p.x*q.y - p.y*q.x) / 2.0f;
1311 }
1312 
1313 float CalibrationDialog::getSkew(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize)
1314 {
1315  // Get skew for given checkerboard detection.
1316  // Scaled to [0,1], which 0 = no skew, 1 = high skew
1317  // Skew is proportional to the divergence of three outside corners from 90 degrees.
1318 
1319  cv::Point2f up_left = corners[0];
1320  cv::Point2f up_right = corners[boardSize.width-1];
1321  cv::Point2f down_right = corners[corners.size()-1];
1322 
1323 
1324  // Return angle between lines ab, bc
1325  cv::Point2f ab = up_left - up_right;
1326  cv::Point2f cb = down_right - up_right;
1327  float angle = std::acos(ab.dot(cb) / (cv::norm(ab) * cv::norm(cb)));
1328 
1329  float r = 2.0f * std::fabs((CV_PI / 2.0f) - angle);
1330  return r > 1.0f?1.0f:r;
1331 }
1332 
1333 // x -> [0, 1] (left, right)
1334 // y -> [0, 1] (top, bottom)
1335 // size -> [0, 1] (small -> big)
1336 // skew -> [0, 1] (low, high)
1337 void CalibrationDialog::getParams(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize, const cv::Size & imageSize,
1338  float & x, float & y, float & size, float & skew)
1339 {
1340  float area = getArea(corners, boardSize);
1341  size = std::sqrt(area / (imageSize.width * imageSize.height));
1342  skew = getSkew(corners, boardSize);
1343  float meanX = 0.0f;
1344  float meanY = 0.0f;
1345  for(unsigned int i=0; i<corners.size(); ++i)
1346  {
1347  meanX += corners[i].x;
1348  meanY += corners[i].y;
1349  }
1350  meanX /= corners.size();
1351  meanY /= corners.size();
1352  x = meanX / imageSize.width;
1353  y = meanY / imageSize.height;
1354 }
1355 
1356 } /* namespace rtabmap */
d
d
rtabmap::StereoCameraModel::save
bool save(const std::string &directory, bool ignoreStereoTransform=true) const
Definition: StereoCameraModel.cpp:334
rtabmap::CameraModel::R
cv::Mat R() const
Definition: CameraModel.h:112
rtabmap::CalibrationDialog::processImages
void processImages(const cv::Mat &imageLeft, const cv::Mat &imageRight, const QString &cameraName)
Definition: CalibrationDialog.cpp:326
rtabmap::CameraModel::imageSize
const cv::Size & imageSize() const
Definition: CameraModel.h:119
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
UEvent
Definition: UEvent.h:57
COUNT_MIN
#define COUNT_MIN
Definition: CalibrationDialog.cpp:54
rtabmap::CalibrationDialog::stereoCalibration
StereoCameraModel stereoCalibration(const CameraModel &left, const CameraModel &right, bool ignoreStereoRectification) const
Definition: CalibrationDialog.cpp:967
f
f
rtabmap::CalibrationDialog::saveSettings
void saveSettings(QSettings &settings, const QString &group="") const
Definition: CalibrationDialog.cpp:119
x
x
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
rtabmap::CalibrationDialog::stereoImagePoints_
std::vector< std::vector< std::vector< cv::Point2f > > > stereoImagePoints_
Definition: CalibrationDialog.h:120
T
T
rtabmap::CalibrationDialog::loadSettings
void loadSettings(QSettings &settings, const QString &group="")
Definition: CalibrationDialog.cpp:136
rtabmap::SensorData::depthOrRightRaw
const cv::Mat & depthOrRightRaw() const
Definition: SensorData.h:184
uCvMat2QImage
QImage uCvMat2QImage(const cv::Mat &image, bool isBgr=true, uCvQtDepthColorMap colorMap=uCvQtDepthWhiteToBlack, float depthMin=0, float depthMax=0)
Definition: UCv2Qt.h:47
glm::e
GLM_FUNC_DECL genType e()
glm::scale
GLM_FUNC_DECL detail::tmat4x4< T, P > scale(detail::tmat4x4< T, P > const &m, detail::tvec3< T, P > const &v)
rtabmap::CalibrationDialog::minIrs_
std::vector< unsigned short > minIrs_
Definition: CalibrationDialog.h:124
glm::angle
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
rtabmap::CalibrationDialog::maxIrs_
std::vector< unsigned short > maxIrs_
Definition: CalibrationDialog.h:125
rtabmap::CalibrationDialog::setSwitchedImages
void setSwitchedImages(bool switched)
Definition: CalibrationDialog.cpp:174
UEvent::getCode
int getCode() const
Definition: UEvent.h:74
rtabmap::StereoCameraModel::T
const cv::Mat & T() const
Definition: StereoCameraModel.h:111
glm::step
GLM_FUNC_DECL genType step(genType const &edge, genType const &x)
rtabmap::StereoCameraModel::name
const std::string & name() const
Definition: StereoCameraModel.h:92
rtabmap::CalibrationDialog::stereoModel_
rtabmap::StereoCameraModel stereoModel_
Definition: CalibrationDialog.h:123
UASSERT
#define UASSERT(condition)
rtabmap::StereoCameraModel::F
const cv::Mat & F() const
Definition: StereoCameraModel.h:113
rtabmap::CalibrationDialog::closeEvent
virtual void closeEvent(QCloseEvent *event)
Definition: CalibrationDialog.cpp:272
rtabmap::CalibrationDialog::imagePoints_
std::vector< std::vector< std::vector< cv::Point2f > > > imagePoints_
Definition: CalibrationDialog.h:118
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:1337
rtabmap::CameraModel::isValidForRectification
bool isValidForRectification() const
Definition: CameraModel.h:89
cvtColor
CvImagePtr cvtColor(const CvImageConstPtr &source, const std::string &encoding)
name
string name
UEvent::getClassName
virtual std::string getClassName() const =0
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:183
rtabmap::CameraModel::D_raw
cv::Mat D_raw() const
Definition: CameraModel.h:109
rtabmap::StereoCameraModel::getLeftSuffix
const std::string & getLeftSuffix() const
Definition: StereoCameraModel.h:125
rtabmap::CameraModel::K_raw
cv::Mat K_raw() const
Definition: CameraModel.h:108
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::CalibrationDialog::models_
std::vector< rtabmap::CameraModel > models_
Definition: CalibrationDialog.h:122
clams::calibrate
DiscreteDepthDistortionModel RTABMAP_EXP 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)
Definition: slam_calibrator.cpp:40
rtabmap::StereoCameraModel::R
const cv::Mat & R() const
Definition: StereoCameraModel.h:110
params
params
false
#define false
Definition: ConvertUTF.c:56
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
rtabmap::CalibrationDialog::CalibrationDialog
CalibrationDialog(bool stereo=false, const QString &savingDirectory=".", bool switchImages=false, QWidget *parent=0)
Definition: CalibrationDialog.cpp:56
rtabmap::StereoCameraModel::localTransform
const Transform & localTransform() const
Definition: StereoCameraModel.h:119
rtabmap::StereoCameraModel::getRightSuffix
const std::string & getRightSuffix() const
Definition: StereoCameraModel.h:126
rtabmap::CalibrationDialog::setStereoMode
void setStereoMode(bool stereo, const QString &leftSuffix="left", const QString &rightSuffix="right")
Definition: CalibrationDialog.cpp:184
rtabmap::CalibrationDialog::setProgressVisibility
void setProgressVisibility(bool visible)
Definition: CalibrationDialog.cpp:169
UDEBUG
#define UDEBUG(...)
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
rtabmap::CameraModel::rectifyImage
cv::Mat rectifyImage(const cv::Mat &raw, int interpolation=cv::INTER_LINEAR) const
Definition: CameraModel.cpp:695
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
glm::acos
GLM_FUNC_DECL genType acos(genType const &x)
UWARN
#define UWARN(...)
rtabmap::CameraModel::P
cv::Mat P() const
Definition: CameraModel.h:113
rtabmap::StereoCameraModel::setName
void setName(const std::string &name, const std::string &leftSuffix="left", const std::string &rightSuffix="right")
Definition: StereoCameraModel.cpp:163
rtabmap::CalibrationDialog::imageSize_
std::vector< cv::Size > imageSize_
Definition: CalibrationDialog.h:121
rtabmap::CalibrationDialog::getArea
float getArea(const std::vector< cv::Point2f > &corners, const cv::Size &boardSize)
Definition: CalibrationDialog.cpp:1295
rtabmap::StereoCameraModel::E
const cv::Mat & E() const
Definition: StereoCameraModel.h:112
rtabmap::CalibrationDialog::imageParams_
std::vector< std::vector< std::vector< float > > > imageParams_
Definition: CalibrationDialog.h:119
rtabmap::CalibrationDialog::ui_
Ui_calibrationDialog * ui_
Definition: CalibrationDialog.h:127
rtabmap::CalibrationDialog::handleEvent
virtual bool handleEvent(UEvent *event)
Definition: CalibrationDialog.cpp:306
UEventsHandler::unregisterFromEventsManager
void unregisterFromEventsManager()
Definition: UEventsHandler.cpp:33
glm::length
GLM_FUNC_DECL genType::value_type length(genType const &x)
rtabmap::CalibrationDialog::setExpectedStereoBaseline
void setExpectedStereoBaseline(double length)
Definition: CalibrationDialog.cpp:256
rtabmap::CalibrationDialog::setCameraName
void setCameraName(const QString &name)
Definition: CalibrationDialog.cpp:164
rtabmap::CalibrationDialog::getSkew
float getSkew(const std::vector< cv::Point2f > &corners, const cv::Size &boardSize)
Definition: CalibrationDialog.cpp:1313
rtabmap::CameraEvent::cameraName
const std::string & cameraName() const
Definition: CameraEvent.h:80
rtabmap::StereoCameraModel::right
const CameraModel & right() const
Definition: StereoCameraModel.h:123
rtabmap::CalibrationDialog::setFisheyeImages
void setFisheyeImages(bool enabled)
Definition: CalibrationDialog.cpp:179
q
q
rtabmap::CameraEvent::data
const SensorData & data() const
Definition: CameraEvent.h:79
rtabmap::StereoCameraModel::left
const CameraModel & left() const
Definition: StereoCameraModel.h:122
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:27