00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "rtabmap/gui/CalibrationDialog.h"
00029 #include "ui_calibrationDialog.h"
00030
00031 #include <opencv2/core/core.hpp>
00032 #include <opencv2/imgproc/imgproc.hpp>
00033 #include <opencv2/imgproc/imgproc_c.h>
00034 #include <opencv2/calib3d/calib3d.hpp>
00035 #if CV_MAJOR_VERSION >= 3
00036 #include <opencv2/calib3d/calib3d_c.h>
00037 #endif
00038 #include <opencv2/highgui/highgui.hpp>
00039
00040 #include <QFileDialog>
00041 #include <QMessageBox>
00042 #include <QCloseEvent>
00043
00044 #include <rtabmap/core/CameraEvent.h>
00045 #include <rtabmap/utilite/UCv2Qt.h>
00046
00047 #include <rtabmap/utilite/ULogger.h>
00048
00049 namespace rtabmap {
00050
00051 #define COUNT_MIN 40
00052
00053 CalibrationDialog::CalibrationDialog(bool stereo, const QString & savingDirectory, bool switchImages, QWidget * parent) :
00054 QDialog(parent),
00055 stereo_(stereo),
00056 leftSuffix_("left"),
00057 rightSuffix_("right"),
00058 savingDirectory_(savingDirectory),
00059 processingData_(false),
00060 savedCalibration_(false)
00061 {
00062 imagePoints_.resize(2);
00063 imageParams_.resize(2);
00064 imageSize_.resize(2);
00065 stereoImagePoints_.resize(2);
00066 models_.resize(2);
00067
00068 minIrs_.resize(2);
00069 maxIrs_.resize(2);
00070 minIrs_[0] = 0x0000;
00071 maxIrs_[0] = 0x7fff;
00072 minIrs_[1] = 0x0000;
00073 maxIrs_[1] = 0x7fff;
00074
00075 qRegisterMetaType<cv::Mat>("cv::Mat");
00076
00077 ui_ = new Ui_calibrationDialog();
00078 ui_->setupUi(this);
00079
00080 connect(ui_->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate()));
00081 connect(ui_->pushButton_restart, SIGNAL(clicked()), this, SLOT(restart()));
00082 connect(ui_->pushButton_save, SIGNAL(clicked()), this, SLOT(save()));
00083 connect(ui_->checkBox_switchImages, SIGNAL(stateChanged(int)), this, SLOT(restart()));
00084 connect(ui_->checkBox_unlock, SIGNAL(stateChanged(int)), SLOT(unlock()));
00085
00086 connect(ui_->spinBox_boardWidth, SIGNAL(valueChanged(int)), this, SLOT(setBoardWidth(int)));
00087 connect(ui_->spinBox_boardHeight, SIGNAL(valueChanged(int)), this, SLOT(setBoardHeight(int)));
00088 connect(ui_->doubleSpinBox_squareSize, SIGNAL(valueChanged(double)), this, SLOT(setSquareSize(double)));
00089 connect(ui_->spinBox_maxScale, SIGNAL(valueChanged(int)), this, SLOT(setMaxScale(int)));
00090
00091 connect(ui_->buttonBox, SIGNAL(rejected()), this, SLOT(close()));
00092
00093 ui_->image_view->setFocus();
00094
00095 ui_->progressBar_count->setMaximum(COUNT_MIN);
00096 ui_->progressBar_count->setFormat("%v");
00097 ui_->progressBar_count_2->setMaximum(COUNT_MIN);
00098 ui_->progressBar_count_2->setFormat("%v");
00099
00100 ui_->radioButton_raw->setChecked(true);
00101
00102 ui_->checkBox_switchImages->setChecked(switchImages);
00103
00104 ui_->checkBox_fisheye->setChecked(false);
00105 ui_->checkBox_fisheye->setEnabled(false);
00106
00107 this->setStereoMode(stereo_);
00108 }
00109
00110 CalibrationDialog::~CalibrationDialog()
00111 {
00112 this->unregisterFromEventsManager();
00113 delete ui_;
00114 }
00115
00116 void CalibrationDialog::saveSettings(QSettings & settings, const QString & group) const
00117 {
00118 if(!group.isEmpty())
00119 {
00120 settings.beginGroup(group);
00121 }
00122 settings.setValue("board_width", ui_->spinBox_boardWidth->value());
00123 settings.setValue("board_height", ui_->spinBox_boardHeight->value());
00124 settings.setValue("board_square_size", ui_->doubleSpinBox_squareSize->value());
00125 settings.setValue("max_scale", ui_->spinBox_maxScale->value());
00126 settings.setValue("geometry", this->saveGeometry());
00127 if(!group.isEmpty())
00128 {
00129 settings.endGroup();
00130 }
00131 }
00132
00133 void CalibrationDialog::loadSettings(QSettings & settings, const QString & group)
00134 {
00135 if(!group.isEmpty())
00136 {
00137 settings.beginGroup(group);
00138 }
00139 this->setBoardWidth(settings.value("board_width", ui_->spinBox_boardWidth->value()).toInt());
00140 this->setBoardHeight(settings.value("board_height", ui_->spinBox_boardHeight->value()).toInt());
00141 this->setSquareSize(settings.value("board_square_size", ui_->doubleSpinBox_squareSize->value()).toDouble());
00142 this->setMaxScale(settings.value("max_scale", ui_->spinBox_maxScale->value()).toDouble());
00143 QByteArray bytes = settings.value("geometry", QByteArray()).toByteArray();
00144 if(!bytes.isEmpty())
00145 {
00146 this->restoreGeometry(bytes);
00147 }
00148 if(!group.isEmpty())
00149 {
00150 settings.endGroup();
00151 }
00152 }
00153
00154 void CalibrationDialog::resetSettings()
00155 {
00156 this->setBoardWidth(8);
00157 this->setBoardHeight(6);
00158 this->setSquareSize(0.033);
00159 }
00160
00161 void CalibrationDialog::setCameraName(const QString & name)
00162 {
00163 cameraName_ = name;
00164 }
00165
00166 void CalibrationDialog::setProgressVisibility(bool visible)
00167 {
00168 ui_->groupBox_progress->setVisible(visible);
00169 }
00170
00171 void CalibrationDialog::setSwitchedImages(bool switched)
00172 {
00173 ui_->checkBox_switchImages->setChecked(switched);
00174 }
00175
00176 void CalibrationDialog::setStereoMode(bool stereo, const QString & leftSuffix, const QString & rightSuffix)
00177 {
00178 leftSuffix_ = leftSuffix;
00179 rightSuffix_ = rightSuffix;
00180 this->restart();
00181
00182 ui_->groupBox_progress->setVisible(true);
00183 stereo_ = stereo;
00184 ui_->progressBar_x_2->setVisible(stereo_);
00185 ui_->progressBar_y_2->setVisible(stereo_);
00186 ui_->progressBar_size_2->setVisible(stereo_);
00187 ui_->progressBar_skew_2->setVisible(stereo_);
00188 ui_->progressBar_count_2->setVisible(stereo_);
00189 ui_->label_right->setVisible(stereo_);
00190 ui_->image_view_2->setVisible(stereo_);
00191 ui_->label_fx_2->setVisible(stereo_);
00192 ui_->label_fy_2->setVisible(stereo_);
00193 ui_->label_cx_2->setVisible(stereo_);
00194 ui_->label_cy_2->setVisible(stereo_);
00195 ui_->label_error_2->setVisible(stereo_);
00196 ui_->label_baseline->setVisible(stereo_);
00197 ui_->label_baseline_name->setVisible(stereo_);
00198 ui_->lineEdit_K_2->setVisible(stereo_);
00199 ui_->lineEdit_D_2->setVisible(stereo_);
00200 ui_->lineEdit_R_2->setVisible(stereo_);
00201 ui_->lineEdit_P_2->setVisible(stereo_);
00202 ui_->radioButton_stereoRectified->setVisible(stereo_);
00203 ui_->checkBox_switchImages->setVisible(stereo_);
00204 }
00205
00206 void CalibrationDialog::setBoardWidth(int width)
00207 {
00208 if(width != ui_->spinBox_boardWidth->value())
00209 {
00210 ui_->spinBox_boardWidth->setValue(width);
00211 this->restart();
00212 }
00213 }
00214
00215 void CalibrationDialog::setBoardHeight(int height)
00216 {
00217 if(height != ui_->spinBox_boardHeight->value())
00218 {
00219 ui_->spinBox_boardHeight->setValue(height);
00220 this->restart();
00221 }
00222 }
00223
00224 void CalibrationDialog::setSquareSize(double size)
00225 {
00226 if(size != ui_->doubleSpinBox_squareSize->value())
00227 {
00228 ui_->doubleSpinBox_squareSize->setValue(size);
00229 this->restart();
00230 }
00231 }
00232
00233 void CalibrationDialog::setMaxScale(int scale)
00234 {
00235 if(scale != ui_->spinBox_maxScale->value())
00236 {
00237 ui_->spinBox_maxScale->setValue(scale);
00238 }
00239 }
00240
00241 void CalibrationDialog::closeEvent(QCloseEvent* event)
00242 {
00243 if(!savedCalibration_ && models_[0].isValidForRectification() &&
00244 (!stereo_ ||
00245 (stereoModel_.isValidForRectification() &&
00246 (!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0))))
00247 {
00248 QMessageBox::StandardButton b = QMessageBox::question(this, tr("Save calibration?"),
00249 tr("The camera is calibrated but you didn't "
00250 "save the calibration, do you want to save it?"),
00251 QMessageBox::Yes | QMessageBox::Ignore | QMessageBox::Cancel, QMessageBox::Yes);
00252 event->ignore();
00253 if(b == QMessageBox::Yes)
00254 {
00255 if(this->save())
00256 {
00257 event->accept();
00258 }
00259 }
00260 else if(b == QMessageBox::Ignore)
00261 {
00262 event->accept();
00263 }
00264 }
00265 else
00266 {
00267 event->accept();
00268 }
00269 if(event->isAccepted())
00270 {
00271 this->unregisterFromEventsManager();
00272 }
00273 }
00274
00275 bool CalibrationDialog::handleEvent(UEvent * event)
00276 {
00277 if(!processingData_)
00278 {
00279 if(event->getClassName().compare("CameraEvent") == 0)
00280 {
00281 rtabmap::CameraEvent * e = (rtabmap::CameraEvent *)event;
00282 if(e->getCode() == rtabmap::CameraEvent::kCodeData)
00283 {
00284 processingData_ = true;
00285 QMetaObject::invokeMethod(this, "processImages",
00286 Q_ARG(cv::Mat, e->data().imageRaw()),
00287 Q_ARG(cv::Mat, e->data().depthOrRightRaw()),
00288 Q_ARG(QString, QString(e->cameraName().c_str())));
00289 }
00290 }
00291 }
00292 return false;
00293 }
00294
00295 void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat & imageRight, const QString & cameraName)
00296 {
00297 UDEBUG("Processing images");
00298 processingData_ = true;
00299 if(cameraName_.isEmpty())
00300 {
00301 cameraName_ = "0000";
00302 if(!cameraName.isEmpty())
00303 {
00304 cameraName_ = cameraName;
00305 }
00306 }
00307 if(ui_->label_serial->text().isEmpty())
00308 {
00309 ui_->label_serial->setText(cameraName_);
00310
00311 }
00312 std::vector<cv::Mat> inputRawImages(2);
00313 if(ui_->checkBox_switchImages->isChecked())
00314 {
00315 inputRawImages[0] = imageRight;
00316 inputRawImages[1] = imageLeft;
00317 }
00318 else
00319 {
00320 inputRawImages[0] = imageLeft;
00321 inputRawImages[1] = imageRight;
00322 }
00323
00324 std::vector<cv::Mat> images(2);
00325 images[0] = inputRawImages[0];
00326 images[1] = inputRawImages[1];
00327 imageSize_[0] = images[0].size();
00328 imageSize_[1] = images[1].size();
00329
00330 bool boardFound[2] = {false};
00331 bool boardAccepted[2] = {false};
00332 bool readyToCalibrate[2] = {false};
00333
00334 std::vector<std::vector<cv::Point2f> > pointBuf(2);
00335
00336 bool depthDetected = false;
00337 for(int id=0; id<(stereo_?2:1); ++id)
00338 {
00339 cv::Mat viewGray;
00340 if(!images[id].empty())
00341 {
00342 if(images[id].type() == CV_16UC1)
00343 {
00344 double min, max;
00345 cv::minMaxLoc(images[id], &min, &max);
00346 UDEBUG("Camera IR %d: min=%f max=%f", id, min, max);
00347 if(minIrs_[id] == 0)
00348 {
00349 minIrs_[id] = min;
00350 }
00351 if(maxIrs_[id] == 0x7fff)
00352 {
00353 maxIrs_[id] = max;
00354 }
00355
00356 depthDetected = true;
00357
00358 const float factor = 255.0f / float((maxIrs_[id] - minIrs_[id]));
00359 viewGray = cv::Mat(images[id].rows, images[id].cols, CV_8UC1);
00360 for(int i=0; i<images[id].rows; ++i)
00361 {
00362 for(int j=0; j<images[id].cols; ++j)
00363 {
00364 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);
00365 }
00366 }
00367 cvtColor(viewGray, images[id], cv::COLOR_GRAY2BGR);
00368 }
00369 else if(images[id].channels() == 3)
00370 {
00371 cvtColor(images[id], viewGray, cv::COLOR_BGR2GRAY);
00372 }
00373 else
00374 {
00375 viewGray = images[id];
00376 cvtColor(viewGray, images[id], cv::COLOR_GRAY2BGR);
00377 }
00378 }
00379 else
00380 {
00381 UERROR("Image %d is empty!! Should not!", id);
00382 }
00383
00384 minIrs_[id] = 0;
00385 maxIrs_[id] = 0x7FFF;
00386
00387
00388 if(!ui_->pushButton_save->isEnabled())
00389 {
00390 cv::Size boardSize(ui_->spinBox_boardWidth->value(), ui_->spinBox_boardHeight->value());
00391 if(!viewGray.empty())
00392 {
00393 int flags = CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE;
00394
00395 if(!viewGray.empty())
00396 {
00397 int maxScale = ui_->spinBox_maxScale->value();
00398 for( int scale = 1; scale <= maxScale; scale++ )
00399 {
00400 cv::Mat timg;
00401 if( scale == 1 )
00402 timg = viewGray;
00403 else
00404 cv::resize(viewGray, timg, cv::Size(), scale, scale, CV_INTER_CUBIC);
00405 boardFound[id] = cv::findChessboardCorners(timg, boardSize, pointBuf[id], flags);
00406 if(boardFound[id])
00407 {
00408 if( scale > 1 )
00409 {
00410 cv::Mat cornersMat(pointBuf[id]);
00411 cornersMat *= 1./scale;
00412 }
00413 break;
00414 }
00415 }
00416 }
00417 }
00418
00419 if(boardFound[id])
00420 {
00421
00422 float minSquareDistance = -1.0f;
00423 for(unsigned int i=0; i<pointBuf[id].size()-1; ++i)
00424 {
00425 float d = cv::norm(pointBuf[id][i] - pointBuf[id][i+1]);
00426 if(minSquareDistance == -1.0f || minSquareDistance > d)
00427 {
00428 minSquareDistance = d;
00429 }
00430 }
00431 float radius = minSquareDistance/2.0f +0.5f;
00432 cv::cornerSubPix( viewGray, pointBuf[id], cv::Size(radius, radius), cv::Size(-1,-1),
00433 cv::TermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1 ));
00434
00435
00436 cv::drawChessboardCorners(images[id], boardSize, cv::Mat(pointBuf[id]), boardFound[id]);
00437
00438 std::vector<float> params(4,0);
00439 getParams(pointBuf[id], boardSize, imageSize_[id], params[0], params[1], params[2], params[3]);
00440
00441 bool addSample = true;
00442 for(unsigned int i=0; i<imageParams_[id].size(); ++i)
00443 {
00444 if(fabs(params[0] - imageParams_[id][i].at(0)) < 0.1 &&
00445 fabs(params[1] - imageParams_[id][i].at(1)) < 0.1 &&
00446 fabs(params[2] - imageParams_[id][i].at(2)) < 0.05 &&
00447 fabs(params[3] - imageParams_[id][i].at(3)) < 0.1)
00448 {
00449 addSample = false;
00450 }
00451 }
00452 if(addSample)
00453 {
00454 boardAccepted[id] = true;
00455
00456 imagePoints_[id].push_back(pointBuf[id]);
00457 imageParams_[id].push_back(params);
00458
00459 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]);
00460 }
00461
00462
00463 std::vector<float> xRange(2, imageParams_[id][0].at(0));
00464 std::vector<float> yRange(2, imageParams_[id][0].at(1));
00465 std::vector<float> sizeRange(2, imageParams_[id][0].at(2));
00466 std::vector<float> skewRange(2, imageParams_[id][0].at(3));
00467 for(unsigned int i=1; i<imageParams_[id].size(); ++i)
00468 {
00469 xRange[0] = imageParams_[id][i].at(0) < xRange[0] ? imageParams_[id][i].at(0) : xRange[0];
00470 xRange[1] = imageParams_[id][i].at(0) > xRange[1] ? imageParams_[id][i].at(0) : xRange[1];
00471 yRange[0] = imageParams_[id][i].at(1) < yRange[0] ? imageParams_[id][i].at(1) : yRange[0];
00472 yRange[1] = imageParams_[id][i].at(1) > yRange[1] ? imageParams_[id][i].at(1) : yRange[1];
00473 sizeRange[0] = imageParams_[id][i].at(2) < sizeRange[0] ? imageParams_[id][i].at(2) : sizeRange[0];
00474 sizeRange[1] = imageParams_[id][i].at(2) > sizeRange[1] ? imageParams_[id][i].at(2) : sizeRange[1];
00475 skewRange[0] = imageParams_[id][i].at(3) < skewRange[0] ? imageParams_[id][i].at(3) : skewRange[0];
00476 skewRange[1] = imageParams_[id][i].at(3) > skewRange[1] ? imageParams_[id][i].at(3) : skewRange[1];
00477 }
00478
00479
00480
00481
00482
00483
00484
00485 float xGood = xRange[1] - xRange[0];
00486 float yGood = yRange[1] - yRange[0];
00487 float sizeGood = sizeRange[1] - sizeRange[0];
00488 float skewGood = skewRange[1] - skewRange[0];
00489
00490 if(id == 0)
00491 {
00492 ui_->progressBar_x->setValue(xGood*100);
00493 ui_->progressBar_y->setValue(yGood*100);
00494 ui_->progressBar_size->setValue(sizeGood*100);
00495 ui_->progressBar_skew->setValue(skewGood*100);
00496 if((int)imagePoints_[id].size() > ui_->progressBar_count->maximum())
00497 {
00498 ui_->progressBar_count->setMaximum((int)imagePoints_[id].size());
00499 }
00500 ui_->progressBar_count->setValue((int)imagePoints_[id].size());
00501 }
00502 else
00503 {
00504 ui_->progressBar_x_2->setValue(xGood*100);
00505 ui_->progressBar_y_2->setValue(yGood*100);
00506 ui_->progressBar_size_2->setValue(sizeGood*100);
00507 ui_->progressBar_skew_2->setValue(skewGood*100);
00508
00509 if((int)imagePoints_[id].size() > ui_->progressBar_count_2->maximum())
00510 {
00511 ui_->progressBar_count_2->setMaximum((int)imagePoints_[id].size());
00512 }
00513 ui_->progressBar_count_2->setValue((int)imagePoints_[id].size());
00514 }
00515
00516 if(imagePoints_[id].size() >= COUNT_MIN && xGood > 0.5 && yGood > 0.5 && sizeGood > 0.4 && skewGood > 0.5)
00517 {
00518 readyToCalibrate[id] = true;
00519 }
00520
00521
00522 if(inputRawImages[id].type() == CV_16UC1)
00523 {
00524
00525 minIrs_[id] = 0xFFFF;
00526 maxIrs_[id] = 0;
00527 for(size_t i = 0; i < pointBuf[id].size(); ++i)
00528 {
00529 const cv::Point2f &p = pointBuf[id][i];
00530 cv::Rect roi(std::max(0, (int)p.x - 3), std::max(0, (int)p.y - 3), 6, 6);
00531
00532 roi.width = std::min(roi.width, inputRawImages[id].cols - roi.x);
00533 roi.height = std::min(roi.height, inputRawImages[id].rows - roi.y);
00534
00535
00536 double min, max;
00537 cv::minMaxLoc(inputRawImages[id](roi), &min, &max);
00538 if(min < minIrs_[id])
00539 {
00540 minIrs_[id] = min;
00541 }
00542 if(max > maxIrs_[id])
00543 {
00544 maxIrs_[id] = max;
00545 }
00546 }
00547 }
00548 }
00549 }
00550 }
00551 ui_->label_baseline->setVisible(!depthDetected);
00552 ui_->label_baseline_name->setVisible(!depthDetected);
00553
00554 if(stereo_ && ((boardAccepted[0] && boardFound[1]) || (boardAccepted[1] && boardFound[0])))
00555 {
00556 stereoImagePoints_[0].push_back(pointBuf[0]);
00557 stereoImagePoints_[1].push_back(pointBuf[1]);
00558 UINFO("Add stereo image points (size=%d)", (int)stereoImagePoints_[0].size());
00559 }
00560
00561 if(!stereo_ && readyToCalibrate[0])
00562 {
00563 unlock();
00564 }
00565 else if(stereo_ && readyToCalibrate[0] && readyToCalibrate[1] && stereoImagePoints_[0].size())
00566 {
00567 unlock();
00568 }
00569
00570 if(ui_->radioButton_rectified->isChecked())
00571 {
00572 if(models_[0].isValidForRectification())
00573 {
00574 images[0] = models_[0].rectifyImage(images[0]);
00575 }
00576 if(models_[1].isValidForRectification())
00577 {
00578 images[1] = models_[1].rectifyImage(images[1]);
00579 }
00580 }
00581 else if(ui_->radioButton_stereoRectified->isChecked() &&
00582 (stereoModel_.left().isValidForRectification() &&
00583 stereoModel_.right().isValidForRectification()&&
00584 (!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0)))
00585 {
00586 images[0] = stereoModel_.left().rectifyImage(images[0]);
00587 images[1] = stereoModel_.right().rectifyImage(images[1]);
00588 }
00589
00590 if(ui_->checkBox_showHorizontalLines->isChecked())
00591 {
00592 for(int id=0; id<(stereo_?2:1); ++id)
00593 {
00594 int step = imageSize_[id].height/16;
00595 for(int i=step; i<imageSize_[id].height; i+=step)
00596 {
00597 cv::line(images[id], cv::Point(0, i), cv::Point(imageSize_[id].width, i), CV_RGB(0,255,0));
00598 }
00599 }
00600 }
00601
00602 ui_->label_left->setText(tr("%1x%2").arg(images[0].cols).arg(images[0].rows));
00603
00604
00605 ui_->image_view->setImage(uCvMat2QImage(images[0]).mirrored(ui_->checkBox_mirror->isChecked(), false));
00606 if(stereo_)
00607 {
00608 ui_->label_right->setText(tr("%1x%2").arg(images[1].cols).arg(images[1].rows));
00609 ui_->image_view_2->setImage(uCvMat2QImage(images[1]).mirrored(ui_->checkBox_mirror->isChecked(), false));
00610 }
00611 processingData_ = false;
00612 }
00613
00614 void CalibrationDialog::restart()
00615 {
00616
00617 savedCalibration_ = false;
00618 imagePoints_[0].clear();
00619 imagePoints_[1].clear();
00620 imageParams_[0].clear();
00621 imageParams_[1].clear();
00622 stereoImagePoints_[0].clear();
00623 stereoImagePoints_[1].clear();
00624 models_[0] = CameraModel();
00625 models_[1] = CameraModel();
00626 stereoModel_ = StereoCameraModel();
00627 minIrs_[0] = 0x0000;
00628 maxIrs_[0] = 0x7fff;
00629 minIrs_[1] = 0x0000;
00630 maxIrs_[1] = 0x7fff;
00631
00632 ui_->pushButton_calibrate->setEnabled(ui_->checkBox_unlock->isChecked());
00633 ui_->checkBox_fisheye->setEnabled(ui_->checkBox_unlock->isChecked());
00634 ui_->pushButton_save->setEnabled(false);
00635 ui_->radioButton_raw->setChecked(true);
00636 ui_->radioButton_rectified->setEnabled(false);
00637 ui_->radioButton_stereoRectified->setEnabled(false);
00638
00639 ui_->progressBar_count->reset();
00640 ui_->progressBar_count->setMaximum(COUNT_MIN);
00641 ui_->progressBar_x->reset();
00642 ui_->progressBar_y->reset();
00643 ui_->progressBar_size->reset();
00644 ui_->progressBar_skew->reset();
00645
00646 ui_->progressBar_count_2->reset();
00647 ui_->progressBar_count_2->setMaximum(COUNT_MIN);
00648 ui_->progressBar_x_2->reset();
00649 ui_->progressBar_y_2->reset();
00650 ui_->progressBar_size_2->reset();
00651 ui_->progressBar_skew_2->reset();
00652
00653 ui_->label_serial->clear();
00654 ui_->label_fx->setNum(0);
00655 ui_->label_fy->setNum(0);
00656 ui_->label_cx->setNum(0);
00657 ui_->label_cy->setNum(0);
00658 ui_->label_baseline->setNum(0);
00659 ui_->label_error->setNum(0);
00660 ui_->lineEdit_K->clear();
00661 ui_->lineEdit_D->clear();
00662 ui_->lineEdit_R->clear();
00663 ui_->lineEdit_P->clear();
00664 ui_->label_fx_2->setNum(0);
00665 ui_->label_fy_2->setNum(0);
00666 ui_->label_cx_2->setNum(0);
00667 ui_->label_cy_2->setNum(0);
00668 ui_->lineEdit_K_2->clear();
00669 ui_->lineEdit_D_2->clear();
00670 ui_->lineEdit_R_2->clear();
00671 ui_->lineEdit_P_2->clear();
00672 }
00673
00674 void CalibrationDialog::unlock()
00675 {
00676 ui_->pushButton_calibrate->setEnabled(true);
00677 ui_->checkBox_fisheye->setEnabled(true);
00678 }
00679
00680 void CalibrationDialog::calibrate()
00681 {
00682 processingData_ = true;
00683 savedCalibration_ = false;
00684
00685 QMessageBox mb(QMessageBox::Information,
00686 tr("Calibrating..."),
00687 tr("Operation in progress..."));
00688 mb.show();
00689 QApplication::processEvents();
00690 uSleep(100);
00691 QApplication::processEvents();
00692
00693 std::vector<std::vector<cv::Point3f> > objectPoints(1);
00694 cv::Size boardSize(ui_->spinBox_boardWidth->value(), ui_->spinBox_boardHeight->value());
00695 float squareSize = ui_->doubleSpinBox_squareSize->value();
00696
00697 for( int i = 0; i < boardSize.height; ++i )
00698 for( int j = 0; j < boardSize.width; ++j )
00699 objectPoints[0].push_back(cv::Point3f(float( j*squareSize ), float( i*squareSize ), 0));
00700
00701 for(int id=0; id<(stereo_?2:1); ++id)
00702 {
00703 UINFO("Calibrating camera %d (samples=%d)", id, (int)imagePoints_[id].size());
00704
00705 objectPoints.resize(imagePoints_[id].size(), objectPoints[0]);
00706
00707
00708 std::vector<cv::Mat> rvecs, tvecs;
00709 std::vector<float> reprojErrs;
00710 cv::Mat K, D;
00711 K = cv::Mat::eye(3,3,CV_64FC1);
00712 UINFO("calibrate!");
00713
00714 double rms = 0.0;
00715 #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)))
00716 bool fishEye = ui_->checkBox_fisheye->isChecked();
00717
00718 if(fishEye)
00719 {
00720 try
00721 {
00722 rms = cv::fisheye::calibrate(objectPoints,
00723 imagePoints_[id],
00724 imageSize_[id],
00725 K,
00726 D,
00727 rvecs,
00728 tvecs,
00729 cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC |
00730 cv::fisheye::CALIB_CHECK_COND |
00731 cv::fisheye::CALIB_FIX_SKEW);
00732 }
00733 catch(const cv::Exception & e)
00734 {
00735 UERROR("Error: %s (try restarting the calibration)", e.what());
00736 QMessageBox::warning(this, tr("Calibration failed!"), tr("Error: %1 (try restarting the calibration)").arg(e.what()));
00737 processingData_ = false;
00738 return;
00739 }
00740 }
00741 else
00742 #endif
00743 {
00744 rms = cv::calibrateCamera(objectPoints,
00745 imagePoints_[id],
00746 imageSize_[id],
00747 K,
00748 D,
00749 rvecs,
00750 tvecs);
00751 }
00752
00753 UINFO("Re-projection error reported by calibrateCamera: %f", rms);
00754
00755
00756 std::vector<cv::Point2f> imagePoints2;
00757 int i, totalPoints = 0;
00758 double totalErr = 0, err;
00759 reprojErrs.resize(objectPoints.size());
00760
00761 for( i = 0; i < (int)objectPoints.size(); ++i )
00762 {
00763 #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)))
00764 if(fishEye)
00765 {
00766 cv::fisheye::projectPoints( cv::Mat(objectPoints[i]), imagePoints2, rvecs[i], tvecs[i], K, D);
00767 }
00768 else
00769 #endif
00770 {
00771 cv::projectPoints( cv::Mat(objectPoints[i]), rvecs[i], tvecs[i], K, D, imagePoints2);
00772 }
00773 err = cv::norm(cv::Mat(imagePoints_[id][i]), cv::Mat(imagePoints2), CV_L2);
00774
00775 int n = (int)objectPoints[i].size();
00776 reprojErrs[i] = (float) std::sqrt(err*err/n);
00777 totalErr += err*err;
00778 totalPoints += n;
00779 }
00780
00781 double totalAvgErr = std::sqrt(totalErr/totalPoints);
00782
00783 UINFO("avg re projection error = %f", totalAvgErr);
00784
00785 cv::Mat P(3,4,CV_64FC1);
00786 P.at<double>(2,3) = 1;
00787 K.copyTo(P.colRange(0,3).rowRange(0,3));
00788
00789 #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)))
00790 if(fishEye)
00791 {
00792
00793 cv::Mat newD = cv::Mat::zeros(1,6,CV_64FC1);
00794 newD.at<double>(0,0) = D.at<double>(0,0);
00795 newD.at<double>(0,1) = D.at<double>(0,1);
00796 newD.at<double>(0,4) = D.at<double>(0,2);
00797 newD.at<double>(0,5) = D.at<double>(0,3);
00798 D = newD;
00799 }
00800 #endif
00801
00802 std::cout << "K = " << K << std::endl;
00803 std::cout << "D = " << D << std::endl;
00804 std::cout << "width = " << imageSize_[id].width << std::endl;
00805 std::cout << "height = " << imageSize_[id].height << std::endl;
00806
00807 models_[id] = CameraModel(cameraName_.toStdString(), imageSize_[id], K, D, cv::Mat::eye(3,3,CV_64FC1), P);
00808
00809 if(id == 0)
00810 {
00811 ui_->label_fx->setNum(models_[id].fx());
00812 ui_->label_fy->setNum(models_[id].fy());
00813 ui_->label_cx->setNum(models_[id].cx());
00814 ui_->label_cy->setNum(models_[id].cy());
00815 ui_->label_error->setNum(totalAvgErr);
00816
00817 std::stringstream strK, strD, strR, strP;
00818 strK << models_[id].K_raw();
00819 strD << models_[id].D_raw();
00820 strR << models_[id].R();
00821 strP << models_[id].P();
00822 ui_->lineEdit_K->setText(strK.str().c_str());
00823 ui_->lineEdit_D->setText(strD.str().c_str());
00824 ui_->lineEdit_R->setText(strR.str().c_str());
00825 ui_->lineEdit_P->setText(strP.str().c_str());
00826 }
00827 else
00828 {
00829 ui_->label_fx_2->setNum(models_[id].fx());
00830 ui_->label_fy_2->setNum(models_[id].fy());
00831 ui_->label_cx_2->setNum(models_[id].cx());
00832 ui_->label_cy_2->setNum(models_[id].cy());
00833 ui_->label_error_2->setNum(totalAvgErr);
00834
00835 std::stringstream strK, strD, strR, strP;
00836 strK << models_[id].K_raw();
00837 strD << models_[id].D_raw();
00838 strR << models_[id].R();
00839 strP << models_[id].P();
00840 ui_->lineEdit_K_2->setText(strK.str().c_str());
00841 ui_->lineEdit_D_2->setText(strD.str().c_str());
00842 ui_->lineEdit_R_2->setText(strR.str().c_str());
00843 ui_->lineEdit_P_2->setText(strP.str().c_str());
00844 }
00845 }
00846
00847 if(stereo_ && models_[0].isValidForRectification() && models_[1].isValidForRectification())
00848 {
00849 stereoModel_ = stereoCalibration(models_[0], models_[1], false);
00850
00851 std::stringstream strR1, strP1, strR2, strP2;
00852 strR1 << stereoModel_.left().R();
00853 strP1 << stereoModel_.left().P();
00854 strR2 << stereoModel_.right().R();
00855 strP2 << stereoModel_.right().P();
00856 ui_->lineEdit_R->setText(strR1.str().c_str());
00857 ui_->lineEdit_P->setText(strP1.str().c_str());
00858 ui_->lineEdit_R_2->setText(strR2.str().c_str());
00859 ui_->lineEdit_P_2->setText(strP2.str().c_str());
00860
00861 ui_->label_baseline->setNum(stereoModel_.baseline());
00862
00863 }
00864
00865 if(stereo_ &&
00866 stereoModel_.isValidForRectification())
00867 {
00868 stereoModel_.initRectificationMap();
00869 models_[0].initRectificationMap();
00870 models_[1].initRectificationMap();
00871 ui_->radioButton_rectified->setEnabled(true);
00872 ui_->radioButton_stereoRectified->setEnabled(true);
00873 ui_->radioButton_stereoRectified->setChecked(true);
00874 ui_->pushButton_save->setEnabled(true);
00875 }
00876 else if(models_[0].isValidForRectification())
00877 {
00878 models_[0].initRectificationMap();
00879 ui_->radioButton_rectified->setEnabled(true);
00880 ui_->radioButton_rectified->setChecked(true);
00881 ui_->pushButton_save->setEnabled(!stereo_);
00882 }
00883
00884 UINFO("End calibration");
00885 processingData_ = false;
00886 }
00887
00888 StereoCameraModel CalibrationDialog::stereoCalibration(const CameraModel & left, const CameraModel & right, bool ignoreStereoRectification) const
00889 {
00890 StereoCameraModel output;
00891 if (stereoImagePoints_[0].empty())
00892 {
00893 UERROR("No stereo correspondences!");
00894 return output;
00895 }
00896 UINFO("stereo calibration (samples=%d)...", (int)stereoImagePoints_[0].size());
00897
00898 if (left.K_raw().empty() || left.D_raw().empty())
00899 {
00900 UERROR("Empty intrinsic parameters (K, D) for the %s camera! Aborting stereo calibration...", leftSuffix_.toStdString().c_str());
00901 return output;
00902 }
00903 if (right.K_raw().empty() || right.D_raw().empty())
00904 {
00905 UERROR("Empty intrinsic parameters (K, D) for the %s camera! Aborting stereo calibration...", rightSuffix_.toStdString().c_str());
00906 return output;
00907 }
00908
00909 if (left.imageSize() != imageSize_[0])
00910 {
00911 UERROR("left model (%dx%d) has not the same size as the processed images (%dx%d)",
00912 left.imageSize().width, left.imageSize().height,
00913 imageSize_[0].width, imageSize_[0].height);
00914 return output;
00915 }
00916 if (right.imageSize() != imageSize_[1])
00917 {
00918 UERROR("right model (%dx%d) has not the same size as the processed images (%dx%d)",
00919 right.imageSize().width, right.imageSize().height,
00920 imageSize_[1].width, imageSize_[1].height);
00921 return output;
00922 }
00923
00924 cv::Size imageSize = imageSize_[0].width > imageSize_[1].width ? imageSize_[0] : imageSize_[1];
00925 cv::Mat R, T, E, F;
00926
00927 std::vector<std::vector<cv::Point3f> > objectPoints(1);
00928 cv::Size boardSize(ui_->spinBox_boardWidth->value(), ui_->spinBox_boardHeight->value());
00929 float squareSize = ui_->doubleSpinBox_squareSize->value();
00930
00931 for (int i = 0; i < boardSize.height; ++i)
00932 for (int j = 0; j < boardSize.width; ++j)
00933 objectPoints[0].push_back(cv::Point3f(float(j*squareSize), float(i*squareSize), 0));
00934 objectPoints.resize(stereoImagePoints_[0].size(), objectPoints[0]);
00935
00936 double rms = 0.0;
00937 #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)))
00938 bool fishEye = left.D_raw().cols == 6;
00939
00940 if(fishEye)
00941 {
00942 cv::Mat D_left(1,4,CV_64FC1);
00943 D_left.at<double>(0,0) = left.D_raw().at<double>(0,0);
00944 D_left.at<double>(0,1) = left.D_raw().at<double>(0,1);
00945 D_left.at<double>(0,2) = left.D_raw().at<double>(0,4);
00946 D_left.at<double>(0,3) = left.D_raw().at<double>(0,5);
00947 cv::Mat D_right(1,4,CV_64FC1);
00948 UASSERT(right.D_raw().cols == 6);
00949 D_right.at<double>(0,0) = right.D_raw().at<double>(0,0);
00950 D_right.at<double>(0,1) = right.D_raw().at<double>(0,1);
00951 D_right.at<double>(0,2) = right.D_raw().at<double>(0,4);
00952 D_right.at<double>(0,3) = right.D_raw().at<double>(0,5);
00953
00954 rms = cv::fisheye::stereoCalibrate(
00955 objectPoints,
00956 stereoImagePoints_[0],
00957 stereoImagePoints_[1],
00958 left.K_raw(), D_left,
00959 right.K_raw(), D_right,
00960 imageSize, R, T,
00961 cv::CALIB_FIX_INTRINSIC,
00962 cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
00963 }
00964 else
00965 #endif
00966 {
00967 #if CV_MAJOR_VERSION < 3
00968 rms = cv::stereoCalibrate(
00969 objectPoints,
00970 stereoImagePoints_[0],
00971 stereoImagePoints_[1],
00972 left.K_raw(), left.D_raw(),
00973 right.K_raw(), right.D_raw(),
00974 imageSize, R, T, E, F,
00975 cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5),
00976 cv::CALIB_FIX_INTRINSIC);
00977 #else
00978 rms = cv::stereoCalibrate(
00979 objectPoints,
00980 stereoImagePoints_[0],
00981 stereoImagePoints_[1],
00982 left.K_raw(), left.D_raw(),
00983 right.K_raw(), right.D_raw(),
00984 imageSize, R, T, E, F,
00985 cv::CALIB_FIX_INTRINSIC,
00986 cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
00987 #endif
00988 }
00989 UINFO("stereo calibration... done with RMS error=%f", rms);
00990
00991 std::cout << "R = " << R << std::endl;
00992 std::cout << "T = " << T << std::endl;
00993 std::cout << "E = " << E << std::endl;
00994 std::cout << "F = " << F << std::endl;
00995
00996 if(imageSize_[0] == imageSize_[1] && !ignoreStereoRectification)
00997 {
00998 UINFO("Compute stereo rectification");
00999
01000 cv::Mat R1, R2, P1, P2, Q;
01001 cv::stereoRectify(left.K_raw(), left.D_raw(),
01002 right.K_raw(), right.D_raw(),
01003 imageSize, R, T, R1, R2, P1, P2, Q,
01004 cv::CALIB_ZERO_DISPARITY, 0, imageSize);
01005
01006 double err = 0;
01007 int npoints = 0;
01008 std::vector<cv::Vec3f> lines[2];
01009 UINFO("Computing avg re-projection error...");
01010 for(unsigned int i = 0; i < stereoImagePoints_[0].size(); i++ )
01011 {
01012 int npt = (int)stereoImagePoints_[0][i].size();
01013
01014 cv::Mat imgpt0 = cv::Mat(stereoImagePoints_[0][i]);
01015 cv::Mat imgpt1 = cv::Mat(stereoImagePoints_[1][i]);
01016 cv::undistortPoints(imgpt0, imgpt0, left.K_raw(), left.D_raw(), R1, P1);
01017 cv::undistortPoints(imgpt1, imgpt1, right.K_raw(), right.D_raw(), R2, P2);
01018 computeCorrespondEpilines(imgpt0, 1, F, lines[0]);
01019 computeCorrespondEpilines(imgpt1, 2, F, lines[1]);
01020
01021 for(int j = 0; j < npt; j++ )
01022 {
01023 double errij = fabs(stereoImagePoints_[0][i][j].x*lines[1][j][0] +
01024 stereoImagePoints_[0][i][j].y*lines[1][j][1] + lines[1][j][2]) +
01025 fabs(stereoImagePoints_[1][i][j].x*lines[0][j][0] +
01026 stereoImagePoints_[1][i][j].y*lines[0][j][1] + lines[0][j][2]);
01027 err += errij;
01028 }
01029 npoints += npt;
01030 }
01031 double totalAvgErr = err/(double)npoints;
01032 UINFO("stereo avg re projection error = %f", totalAvgErr);
01033
01034 output = StereoCameraModel(
01035 cameraName_.toStdString(),
01036 imageSize_[0], left.K_raw(), left.D_raw(), R1, P1,
01037 imageSize_[1], right.K_raw(), right.D_raw(), R2, P2,
01038 R, T, E, F);
01039 }
01040 else
01041 {
01042 UDEBUG("%s", cameraName_.toStdString().c_str());
01043
01044 output = StereoCameraModel(
01045 cameraName_.toStdString(),
01046 imageSize_[0], left.K_raw(), left.D_raw(), left.R(), left.P(),
01047 imageSize_[1], right.K_raw(), right.D_raw(), right.R(), right.P(),
01048 R, T, E, F);
01049 }
01050 return output;
01051 }
01052
01053 bool CalibrationDialog::save()
01054 {
01055 bool saved = false;
01056 processingData_ = true;
01057 if(!stereo_)
01058 {
01059 UASSERT(models_[0].isValidForRectification());
01060 QString cameraName = models_[0].name().c_str();
01061 QString filePath = QFileDialog::getSaveFileName(this, tr("Export"), savingDirectory_+"/"+cameraName+".yaml", "*.yaml");
01062
01063 if(!filePath.isEmpty())
01064 {
01065 QString name = QFileInfo(filePath).baseName();
01066 QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
01067 models_[0].setName(name.toStdString());
01068 if(models_[0].save(dir.toStdString()))
01069 {
01070 QMessageBox::information(this, tr("Export"), tr("Calibration file saved to \"%1\".").arg(filePath));
01071 UINFO("Saved \"%s\"!", filePath.toStdString().c_str());
01072 savedCalibration_ = true;
01073 saved = true;
01074 }
01075 else
01076 {
01077 UERROR("Error saving \"%s\"", filePath.toStdString().c_str());
01078 }
01079 }
01080 }
01081 else
01082 {
01083 UASSERT(stereoModel_.left().isValidForRectification() &&
01084 stereoModel_.right().isValidForRectification());
01085 QString cameraName = stereoModel_.name().c_str();
01086 QString filePath = QFileDialog::getSaveFileName(this, tr("Export"), savingDirectory_ + "/" + cameraName, "*.yaml");
01087 QString name = QFileInfo(filePath).baseName();
01088 QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
01089 if(!name.isEmpty())
01090 {
01091 bool switched = ui_->checkBox_switchImages->isChecked();
01092 stereoModel_.setName(name.toStdString(), switched?rightSuffix_.toStdString():leftSuffix_.toStdString(), switched?leftSuffix_.toStdString():rightSuffix_.toStdString());
01093 std::string base = (dir+QDir::separator()+name).toStdString();
01094 std::string leftPath = base+"_"+stereoModel_.getLeftSuffix()+".yaml";
01095 std::string rightPath = base+"_"+stereoModel_.getRightSuffix()+".yaml";
01096 std::string posePath = base+"_pose.yaml";
01097 if(stereoModel_.save(dir.toStdString(), false))
01098 {
01099 QMessageBox::information(this, tr("Export"), tr("Calibration files saved:\n \"%1\"\n \"%2\"\n \"%3\".").
01100 arg(leftPath.c_str()).arg(rightPath.c_str()).arg(posePath.c_str()));
01101 UINFO("Saved \"%s\" and \"%s\"!", leftPath.c_str(), rightPath.c_str());
01102 savedCalibration_ = true;
01103 saved = true;
01104 }
01105 else
01106 {
01107 UERROR("Error saving \"%s\" and \"%s\"", leftPath.c_str(), rightPath.c_str());
01108 }
01109 }
01110 }
01111 processingData_ = false;
01112 return saved;
01113 }
01114
01115 float CalibrationDialog::getArea(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize)
01116 {
01117
01118
01119
01120
01121 cv::Point2f up_left = corners[0];
01122 cv::Point2f up_right = corners[boardSize.width-1];
01123 cv::Point2f down_right = corners[corners.size()-1];
01124 cv::Point2f down_left = corners[corners.size()-boardSize.width];
01125 cv::Point2f a = up_right - up_left;
01126 cv::Point2f b = down_right - up_right;
01127 cv::Point2f c = down_left - down_right;
01128 cv::Point2f p = b + c;
01129 cv::Point2f q = a + b;
01130 return std::fabs(p.x*q.y - p.y*q.x) / 2.0f;
01131 }
01132
01133 float CalibrationDialog::getSkew(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize)
01134 {
01135
01136
01137
01138
01139 cv::Point2f up_left = corners[0];
01140 cv::Point2f up_right = corners[boardSize.width-1];
01141 cv::Point2f down_right = corners[corners.size()-1];
01142
01143
01144
01145 cv::Point2f ab = up_left - up_right;
01146 cv::Point2f cb = down_right - up_right;
01147 float angle = std::acos(ab.dot(cb) / (cv::norm(ab) * cv::norm(cb)));
01148
01149 float r = 2.0f * std::fabs((CV_PI / 2.0f) - angle);
01150 return r > 1.0f?1.0f:r;
01151 }
01152
01153
01154
01155
01156
01157 void CalibrationDialog::getParams(const std::vector<cv::Point2f> & corners, const cv::Size & boardSize, const cv::Size & imageSize,
01158 float & x, float & y, float & size, float & skew)
01159 {
01160 float area = getArea(corners, boardSize);
01161 size = std::sqrt(area / (imageSize.width * imageSize.height));
01162 skew = getSkew(corners, boardSize);
01163 float meanX = 0.0f;
01164 float meanY = 0.0f;
01165 for(unsigned int i=0; i<corners.size(); ++i)
01166 {
01167 meanX += corners[i].x;
01168 meanY += corners[i].y;
01169 }
01170 meanX /= corners.size();
01171 meanY /= corners.size();
01172 x = meanX / imageSize.width;
01173 y = meanY / imageSize.height;
01174 }
01175
01176 }