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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:58