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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:41:30