Stereo Calibration: doing separate calibrations before stereo calibration

guilib/include/rtabmap/gui/CalibrationDialog.h

@@ -50,8 +50,9 @@ public:
 	CalibrationDialog(bool stereo = false, const QString & savingDirectory = ".", QWidget * parent = 0);
 	virtual ~CalibrationDialog();
 
-	bool isCalibrated() const {return calibrated_;}
+	bool isCalibrated() const {return models_[0].isValid() && (stereo_?models_[1].isValid():true);}
-	const rtabmap::CameraModel & getCameraModel() const {return model_;}
+	const rtabmap::CameraModel & getLeftCameraModel() const {return models_[0];}
+	const rtabmap::CameraModel & getRightCameraModel() const {return models_[1];}
 	const rtabmap::StereoCameraModel & getStereoCameraModel() const {return stereoModel_;}
 
 	void saveSettings(QSettings & settings, const QString & group = "") const;
@@ -92,14 +93,14 @@ private:
 	QString savingDirectory_;
 
 	QString cameraName_;
-	bool calibrated_;
 	bool processingData_;
 	bool savedCalibration_;
 
 	std::vector<std::vector<std::vector<cv::Point2f> > > imagePoints_;
 	std::vector<std::vector<std::vector<float> > > imageParams_;
+	std::vector<std::vector<std::vector<cv::Point2f> > > stereoImagePoints_;
 	std::vector<cv::Size > imageSize_;
-	rtabmap::CameraModel model_;
+	std::vector<rtabmap::CameraModel> models_;
 	rtabmap::StereoCameraModel stereoModel_;
 
 	Ui_calibrationDialog * ui_;

guilib/src/CalibrationDialog.cpp

@@ -50,13 +50,14 @@ CalibrationDialog::CalibrationDialog(bool stereo, const QString & savingDirector
 	QDialog(parent),
 	stereo_(stereo),
 	savingDirectory_(savingDirectory),
-	calibrated_(false),
 	processingData_(false),
 	savedCalibration_(false)
 {
 	imagePoints_.resize(2);
 	imageParams_.resize(2);
 	imageSize_.resize(2);
+	stereoImagePoints_.resize(2);
+	models_.resize(2);
 
 	qRegisterMetaType<cv::Mat>("cv::Mat");
 
@@ -77,6 +78,8 @@ CalibrationDialog::CalibrationDialog(bool stereo, const QString & savingDirector
 
 	ui_->progressBar_count->setMaximum(COUNT_MIN);
 	ui_->progressBar_count->setFormat("%v");
+	ui_->progressBar_count_2->setMaximum(COUNT_MIN);
+	ui_->progressBar_count_2->setFormat("%v");
 
 	this->setStereoMode(stereo_);
 }
@@ -132,11 +135,13 @@ void CalibrationDialog::setStereoMode(bool stereo)
 	ui_->progressBar_y_2->setVisible(stereo_);
 	ui_->progressBar_size_2->setVisible(stereo_);
 	ui_->progressBar_skew_2->setVisible(stereo_);
+	ui_->progressBar_count_2->setVisible(stereo_);
 	ui_->image_view_2->setVisible(stereo_);
 	ui_->label_fx_2->setVisible(stereo_);
 	ui_->label_fy_2->setVisible(stereo_);
 	ui_->label_cx_2->setVisible(stereo_);
 	ui_->label_cy_2->setVisible(stereo_);
+	ui_->label_error_2->setVisible(stereo_);
 	ui_->label_baseline->setVisible(stereo_);
 	ui_->label_baseline_name->setVisible(stereo_);
 	ui_->lineEdit_K_2->setVisible(stereo_);
@@ -174,7 +179,7 @@ void CalibrationDialog::setSquareSize(double size)
 
 void CalibrationDialog::closeEvent(QCloseEvent* event)
 {
-	if(!savedCalibration_ && calibrated_)
+	if(!savedCalibration_ && models_[0].isValid() && (!stereo_ || stereoModel_.isValid()))
 	{
 		QMessageBox::StandardButton b = QMessageBox::question(this, tr("Save calibration?"),
 				tr("The camera is calibrated but you didn't "
@@ -240,14 +245,17 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
 
 	}
 	std::vector<cv::Mat> images(2);
-	images[0] = imageLeft;
+	cv::resize(imageLeft, images[0], cv::Size(), 2.0, 2.0, CV_INTER_CUBIC);
+	//images[0] = imageLeft;
 	images[1] = imageRight;
-	imageSize_[0] = imageLeft.size();
+	imageSize_[0] = images[0].size();
-	imageSize_[1] = imageRight.size();
+	imageSize_[1] = images[1].size();
+
+	bool boardFound[2] = {false};
+	bool boardAccepted[2] = {false};
+	bool readyToCalibrate[2] = {false};
 
 	std::vector<std::vector<cv::Point2f> > pointBuf(2);
-	std::vector<std::vector<float> > params(2, std::vector<float>(4, 0));
-	bool boardAccepted[2] = {false};
 
 	for(int id=0; id<(stereo_?2:1); ++id)
 	{
@@ -269,86 +277,58 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
 			UERROR("Image %d is empty!! Should not!", id);
 		}
 
-		bool found = false;
+		//Dot it only if not yet calibrated
-		cv::Size boardSize(ui_->spinBox_boardWidth->value(), ui_->spinBox_boardHeight->value());
+		if(!ui_->pushButton_save->isEnabled())
-		if(!viewGray.empty())
 		{
-			int maxScale = 1;
+			cv::Size boardSize(ui_->spinBox_boardWidth->value(), ui_->spinBox_boardHeight->value());
-			for( int scale = 1; scale <= maxScale; scale++ )
+			if(!viewGray.empty())
 			{
-				cv::Mat timg;
+				int flags = CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE;
-				if( scale == 1 )
+				boardFound[id] = cv::findChessboardCorners(viewGray, boardSize, pointBuf[id], flags);
-					timg = viewGray;
+			}
-				else
+
-					cv::resize(viewGray, timg, cv::Size(), scale, scale);
+			if(boardFound[id]) // If done with success,
-				found = cv::findChessboardCorners(timg, boardSize, pointBuf[id],
+			{
-						CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE);
+				// improve the found corners' coordinate accuracy for chessboard
-				if(found)
+				float minSquareDistance = -1.0f;
+				for(unsigned int i=0; i<pointBuf[id].size()-1; ++i)
 				{
-					if( scale > 1 )
+					float d = cv::norm(pointBuf[id][i] - pointBuf[id][i+1]);
+					if(minSquareDistance == -1.0f || minSquareDistance > d)
 					{
-						cv::Mat cornersMat(pointBuf[id]);
+						minSquareDistance = d;
-						cornersMat *= 1./scale;
 					}
-					break;
 				}
-			}
+				float radius = minSquareDistance/2.0f +0.5f;
-		}
+				cv::cornerSubPix( viewGray, pointBuf[id], cv::Size(radius, radius), cv::Size(-1,-1),
+						cv::TermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1 ));
 
-		if(found) // If done with success,
+				// Draw the corners.
-		{
+				cv::drawChessboardCorners(images[id], boardSize, cv::Mat(pointBuf[id]), boardFound[id]);
-			// improve the found corners' coordinate accuracy for chessboard
+
-			float minSquareDistance = -1.0f;
+				std::vector<float> params(4,0);
-			for(unsigned int i=0; i<pointBuf[id].size()-1; ++i)
+				getParams(pointBuf[id], boardSize, imageSize_[id], params[0], params[1], params[2], params[3]);
-			{
+
-				float d = cv::norm(pointBuf[id][i] - pointBuf[id][i+1]);
+				bool addSample = true;
-				if(minSquareDistance == -1.0f || minSquareDistance > d)
+				for(unsigned int i=0; i<imageParams_[id].size(); ++i)
 				{
-					minSquareDistance = d;
+					if(fabs(params[0] - imageParams_[id][i].at(0)) < 0.1 && // x
+						fabs(params[1] - imageParams_[id][i].at(1)) < 0.1 && // y
+						fabs(params[2] - imageParams_[id][i].at(2)) < 0.05 && // size
+						fabs(params[3] - imageParams_[id][i].at(3)) < 0.1) // skew
+					{
+						addSample = false;
+					}
 				}
-			}
+				if(addSample)
-			float radius = minSquareDistance/2.0f +0.5f;
-			cv::cornerSubPix( viewGray, pointBuf[id], cv::Size(radius, radius), cv::Size(-1,-1),
-					cv::TermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1 ));
-
-			boardAccepted[id] = true;
-			getParams(pointBuf[id], boardSize, imageSize_[id], params[id][0], params[id][1], params[id][2], params[id][3]);
-
-			// Draw the corners.
-			cv::drawChessboardCorners(images[id], boardSize, cv::Mat(pointBuf[id]), found);
-		}
-	}
-
-	bool readyToCalibrate[2] = {false};
-	if(boardAccepted[0] && (!stereo_ || boardAccepted[1]))
-	{
-		// verify if view is different from any previous samples
-		bool addSample = true;
-		for(int id=0; id<(stereo_?2:1); ++id)
-		{
-			for(unsigned int i=0; i<imageParams_[id].size(); ++i)
-			{
-				if(fabs(params[id][0] - imageParams_[id][i].at(0)) < 0.1 && // x
-					fabs(params[id][1] - imageParams_[id][i].at(1)) < 0.1 && // y
-					fabs(params[id][2] - imageParams_[id][i].at(2)) < 0.05 && // size
-					fabs(params[id][3] - imageParams_[id][i].at(3)) < 0.1) // skew
 				{
-					addSample = false;
+					boardAccepted[id] = true;
-				}
-			}
-			if(addSample)
-			{
-				break;
-			}
-		}
 
-		if(addSample)
+					imagePoints_[id].push_back(pointBuf[id]);
-		{
+					imageParams_[id].push_back(params);
-			for(int id=0; id<(stereo_?2:1); ++id)
+
-			{
+					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]);
-				UINFO("Added board. (x=%f, y=%f, size=%f, skew=%f)", params[id][0], params[id][1], params[id][2], params[id][3]);
+				}
-				imagePoints_[id].push_back(pointBuf[id]);
-				imageParams_[id].push_back(params[id]);
 
 				// update statistics
 				std::vector<float> xRange(2, imageParams_[id][0].at(0));
@@ -366,12 +346,12 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
 					skewRange[0] = imageParams_[id][i].at(3) < skewRange[0] ? imageParams_[id][i].at(3) : skewRange[0];
 					skewRange[1] = imageParams_[id][i].at(3) > skewRange[1] ? imageParams_[id][i].at(3) : skewRange[1];
 				}
-				UINFO("Stats:");
+				//UINFO("Stats [%d]:", id);
-				UINFO("  Count = %d", (int)imagePoints_[id].size());
+				//UINFO("  Count = %d", (int)imagePoints_[id].size());
-				UINFO("  x =    [%f -> %f]", xRange[0], xRange[1]);
+				//UINFO("  x =    [%f -> %f]", xRange[0], xRange[1]);
-				UINFO("  y =    [%f -> %f]", yRange[0], yRange[1]);
+				//UINFO("  y =    [%f -> %f]", yRange[0], yRange[1]);
-				UINFO("  size = [%f -> %f]", sizeRange[0], sizeRange[1]);
+				//UINFO("  size = [%f -> %f]", sizeRange[0], sizeRange[1]);
-				UINFO("  skew = [%f -> %f]", skewRange[0], skewRange[1]);
+				//UINFO("  skew = [%f -> %f]", skewRange[0], skewRange[1]);
 
 				float xGood = xRange[1] - xRange[0];
 				float yGood = yRange[1] - yRange[0];
@@ -384,6 +364,11 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
 					ui_->progressBar_y->setValue(yGood*100);
 					ui_->progressBar_size->setValue(sizeGood*100);
 					ui_->progressBar_skew->setValue(skewGood*100);
+					if((int)imagePoints_[id].size() > ui_->progressBar_count->maximum())
+					{
+						ui_->progressBar_count->setMaximum((int)imagePoints_[id].size());
+					}
+					ui_->progressBar_count->setValue((int)imagePoints_[id].size());
 				}
 				else
 				{
@@ -391,6 +376,12 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
 					ui_->progressBar_y_2->setValue(yGood*100);
 					ui_->progressBar_size_2->setValue(sizeGood*100);
 					ui_->progressBar_skew_2->setValue(skewGood*100);
+
+					if((int)imagePoints_[id].size() > ui_->progressBar_count_2->maximum())
+					{
+						ui_->progressBar_count_2->setMaximum((int)imagePoints_[id].size());
+					}
+					ui_->progressBar_count_2->setValue((int)imagePoints_[id].size());
 				}
 
 				if(imagePoints_[id].size() >= COUNT_MIN && xGood > 0.5 && yGood > 0.5 && sizeGood > 0.4 && skewGood > 0.5)
@@ -398,29 +389,31 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
 					readyToCalibrate[id] = true;
 				}
 			}
-
-			if((int)imagePoints_[0].size() > ui_->progressBar_count->maximum())
-			{
-				ui_->progressBar_count->setMaximum((int)imagePoints_[0].size());
-			}
-			ui_->progressBar_count->setValue((int)imagePoints_[0].size());
 		}
 	}
 
+
+	if(stereo_ && ((boardAccepted[0] && boardFound[1]) || (boardAccepted[1] && boardFound[0])))
+	{
+		stereoImagePoints_[0].push_back(pointBuf[0]);
+		stereoImagePoints_[1].push_back(pointBuf[1]);
+		UINFO("Add stereo image points (size=%d)", (int)stereoImagePoints_[0].size());
+	}
+
 	if(!stereo_ && readyToCalibrate[0])
 	{
 		ui_->pushButton_calibrate->setEnabled(true);
 	}
-	else if(stereo_ && readyToCalibrate[0] && readyToCalibrate[1])
+	else if(stereo_ && readyToCalibrate[0] && readyToCalibrate[1] && stereoImagePoints_[0].size())
 	{
 		ui_->pushButton_calibrate->setEnabled(true);
 	}
 
-	if(calibrated_ && ui_->checkBox_rectified->isChecked())
+	if(models_[0].isValid() && ui_->checkBox_rectified->isChecked())
 	{
 		if(!stereo_)
 		{
-			images[0] = model_.rectifyImage(images[0]);
+			images[0] = models_[0].rectifyImage(images[0]);
 		}
 		else
 		{
@@ -441,10 +434,13 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
 		}
 	}
 
+	ui_->label_left->setText(tr("%1x%2").arg(images[0].cols).arg(images[0].rows));
+
 	//show frame
 	ui_->image_view->setImage(uCvMat2QImage(images[0]).mirrored(ui_->checkBox_mirror->isChecked(), false));
 	if(stereo_)
 	{
+		ui_->label_right->setText(tr("%1x%2").arg(images[1].cols).arg(images[1].rows));
 		ui_->image_view_2->setImage(uCvMat2QImage(images[1]).mirrored(ui_->checkBox_mirror->isChecked(), false));
 	}
 	processingData_ = false;
@@ -453,13 +449,15 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
 void CalibrationDialog::restart()
 {
 	// restart
-	calibrated_ = false;
 	savedCalibration_ = false;
 	imagePoints_[0].clear();
 	imagePoints_[1].clear();
 	imageParams_[0].clear();
 	imageParams_[1].clear();
-	model_ = CameraModel();
+	stereoImagePoints_[0].clear();
+	stereoImagePoints_[1].clear();
+	models_[0] = CameraModel();
+	models_[1] = CameraModel();
 	stereoModel_ = StereoCameraModel();
 	cameraName_.clear();
 
@@ -474,6 +472,8 @@ void CalibrationDialog::restart()
 	ui_->progressBar_size->reset();
 	ui_->progressBar_skew->reset();
 
+	ui_->progressBar_count_2->reset();
+	ui_->progressBar_count_2->setMaximum(COUNT_MIN);
 	ui_->progressBar_x_2->reset();
 	ui_->progressBar_y_2->reset();
 	ui_->progressBar_size_2->reset();
@@ -502,13 +502,12 @@ void CalibrationDialog::restart()
 
 void CalibrationDialog::calibrate()
 {
-	UINFO("Calibration...");
 	processingData_ = true;
 	savedCalibration_ = false;
 
 	QMessageBox mb(QMessageBox::Information,
 			tr("Calibrating..."),
-			tr("Operation in progress. You have time to take a coffee!"));
+			tr("Operation in progress..."));
 	mb.show();
 	QApplication::processEvents();
 	uSleep(100); // hack make sure the text in the QMessageBox is shown...
@@ -522,33 +521,29 @@ void CalibrationDialog::calibrate()
 		for( int j = 0; j < boardSize.width; ++j )
 			objectPoints[0].push_back(cv::Point3f(float( j*squareSize ), float( i*squareSize ), 0));
 
-	objectPoints.resize(imagePoints_[0].size(),objectPoints[0]);
+	for(int id=0; id<(stereo_?2:1); ++id)
-
-	if(!stereo_)
 	{
+		UINFO("Calibrating camera %d (samples=%d)", id, (int)imagePoints_[id].size());
+
+		objectPoints.resize(imagePoints_[id].size(), objectPoints[0]);
+
 		//calibrate
 		std::vector<cv::Mat> rvecs, tvecs;
 		std::vector<float> reprojErrs;
 		cv::Mat K, D;
 		K = cv::Mat::eye(3,3,CV_64FC1);
-
+		UINFO("calibrate!");
 		//Find intrinsic and extrinsic camera parameters
 		double rms = cv::calibrateCamera(objectPoints,
-				imagePoints_[0],
+				imagePoints_[id],
-				imageSize_[0],
+				imageSize_[id],
 				K,
 				D,
 				rvecs,
-				tvecs,
+				tvecs);
-				CV_CALIB_FIX_K4|CV_CALIB_FIX_K5);
-
-		std::cout << "cameraMatrix = " << K << std::endl;
-		std::cout << "distCoeffs = " << D << std::endl;
 
 		UINFO("Re-projection error reported by calibrateCamera: %f", rms);
 
-		calibrated_ = checkRange(K) && checkRange(D);
-
 		// compute reprojection errors
 		std::vector<cv::Point2f> imagePoints2;
 		int i, totalPoints = 0;
@@ -558,7 +553,7 @@ void CalibrationDialog::calibrate()
 		for( i = 0; i < (int)objectPoints.size(); ++i )
 		{
 			cv::projectPoints( cv::Mat(objectPoints[i]), rvecs[i], tvecs[i], K, D, imagePoints2);
-			err = cv::norm(cv::Mat(imagePoints_[0][i]), cv::Mat(imagePoints2), CV_L2);
+			err = cv::norm(cv::Mat(imagePoints_[id][i]), cv::Mat(imagePoints2), CV_L2);
 
 			int n = (int)objectPoints[i].size();
 			reprojErrs[i] = (float) std::sqrt(err*err/n);
@@ -568,144 +563,149 @@ void CalibrationDialog::calibrate()
 
 		double totalAvgErr =  std::sqrt(totalErr/totalPoints);
 
-		UINFO("%s. avg re projection error = %f", calibrated_ ? "Calibration succeeded" : "Calibration failed", totalAvgErr);
+		UINFO("avg re projection error = %f", totalAvgErr);
 
-		if(calibrated_)
+		cv::Mat P(3,4,CV_64FC1);
+		P.at<double>(2,3) = 1;
+		K.copyTo(P.colRange(0,3).rowRange(0,3));
+
+		std::cout << "cameraMatrix = " << K << std::endl;
+		std::cout << "distCoeffs = " << D << std::endl;
+		std::cout << "width = " << imageSize_[id].width << std::endl;
+		std::cout << "height = " << imageSize_[id].height << std::endl;
+
+		models_[id] = CameraModel(cameraName_.toStdString(), imageSize_[id], K, D, cv::Mat::eye(3,3,CV_64FC1), P);
+
+		if(id == 0)
 		{
-			cv::Mat P(3,4,CV_64FC1);
+			ui_->label_fx->setNum(models_[id].fx());
-			P.at<double>(2,3) = 1;
+			ui_->label_fy->setNum(models_[id].fy());
-			K.copyTo(P.colRange(0,3).rowRange(0,3));
+			ui_->label_cx->setNum(models_[id].cx());
-
+			ui_->label_cy->setNum(models_[id].cy());
-			model_ = CameraModel(cameraName_.toStdString(), imageSize_[0], K, D, cv::Mat::eye(3,3,CV_64FC1), P);
-
-			ui_->label_fx->setNum(model_.fx());
-			ui_->label_fy->setNum(model_.fy());
-			ui_->label_cx->setNum(model_.cx());
-			ui_->label_cy->setNum(model_.cy());
 			ui_->label_error->setNum(totalAvgErr);
 
 			std::stringstream strK, strD, strR, strP;
-			strK << model_.K();
+			strK << models_[id].K();
-			strD << model_.D();
+			strD << models_[id].D();
-			strR << model_.R();
+			strR << models_[id].R();
-			strP << model_.P();
+			strP << models_[id].P();
 			ui_->lineEdit_K->setText(strK.str().c_str());
 			ui_->lineEdit_D->setText(strD.str().c_str());
 			ui_->lineEdit_R->setText(strR.str().c_str());
 			ui_->lineEdit_P->setText(strP.str().c_str());
+		}
+		else
+		{
+			ui_->label_fx_2->setNum(models_[id].fx());
+			ui_->label_fy_2->setNum(models_[id].fy());
+			ui_->label_cx_2->setNum(models_[id].cx());
+			ui_->label_cy_2->setNum(models_[id].cy());
+			ui_->label_error_2->setNum(totalAvgErr);
 
-			ui_->checkBox_rectified->setEnabled(true);
+			std::stringstream strK, strD, strR, strP;
-			ui_->checkBox_rectified->setChecked(true);
+			strK << models_[id].K();
-
+			strD << models_[id].D();
-			ui_->pushButton_save->setEnabled(true);
+			strR << models_[id].R();
+			strP << models_[id].P();
+			ui_->lineEdit_K_2->setText(strK.str().c_str());
+			ui_->lineEdit_D_2->setText(strD.str().c_str());
+			ui_->lineEdit_R_2->setText(strR.str().c_str());
+			ui_->lineEdit_P_2->setText(strP.str().c_str());
 		}
 	}
-	else
+
+	if(stereo_ && models_[0].isValid() && models_[1].isValid())
 	{
-		UINFO("stereo calibration...");
+		UINFO("stereo calibration (samples=%d)...", (int)stereoImagePoints_[0].size());
 		cv::Size imageSize = imageSize_[0];
-		std::vector<cv::Mat> K(2), D(2);
-		K[0] = cv::Mat::eye(3, 3, CV_64F);
-		K[1] = cv::Mat::eye(3, 3, CV_64F);
 		cv::Mat R, T, E, F;
 
-		double rms = cv::stereoCalibrate(objectPoints, imagePoints_[0], imagePoints_[1],
+		std::vector<std::vector<cv::Point3f> > objectPoints(1);
-						K[0], D[0],
+		cv::Size boardSize(ui_->spinBox_boardWidth->value(), ui_->spinBox_boardHeight->value());
-						K[1], D[1],
+		float squareSize = ui_->doubleSpinBox_squareSize->value();
+		// compute board corner positions
+		for( int i = 0; i < boardSize.height; ++i )
+			for( int j = 0; j < boardSize.width; ++j )
+				objectPoints[0].push_back(cv::Point3f(float( j*squareSize ), float( i*squareSize ), 0));
+		objectPoints.resize(stereoImagePoints_[0].size(), objectPoints[0]);
+
+		// calibrate extrinsic
+		double rms = cv::stereoCalibrate(objectPoints, stereoImagePoints_[0], stereoImagePoints_[1],
+						models_[0].K(), models_[0].D(),
+						models_[1].K(), models_[1].D(),
 						imageSize, R, T, E, F,
 						cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5),
-						cv::CALIB_FIX_ASPECT_RATIO +
+						cv::CALIB_FIX_INTRINSIC);
-						cv::CALIB_ZERO_TANGENT_DIST +
-						cv::CALIB_SAME_FOCAL_LENGTH +
-						cv::CALIB_RATIONAL_MODEL +
-						cv::CALIB_FIX_K3 + cv::CALIB_FIX_K4 + cv::CALIB_FIX_K5);
 		UINFO("stereo calibration... done with RMS error=%f", rms);
 
-		calibrated_ = checkRange(K[0]) && checkRange(D[0]) && checkRange(K[1]) && checkRange(D[1]);
-
 		double err = 0;
 		int npoints = 0;
 		std::vector<cv::Vec3f> lines[2];
 		UINFO("Computing avg re-projection error...");
-		for(unsigned int i = 0; i < imagePoints_[0].size(); i++ )
+		for(unsigned int i = 0; i < stereoImagePoints_[0].size(); i++ )
 		{
-			int npt = (int)imagePoints_[0][i].size();
+			int npt = (int)stereoImagePoints_[0][i].size();
 			cv::Mat imgpt[2];
 			for(int k = 0; k < 2; k++ )
 			{
-				imgpt[k] = cv::Mat(imagePoints_[k][i]);
+				imgpt[k] = cv::Mat(stereoImagePoints_[k][i]);
-				cv::undistortPoints(imgpt[k], imgpt[k], K[k], D[k], cv::Mat(), K[k]);
+				cv::undistortPoints(imgpt[k], imgpt[k], models_[k].K(), models_[k].D(), cv::Mat(), models_[k].K());
 				computeCorrespondEpilines(imgpt[k], k+1, F, lines[k]);
 			}
 			for(int j = 0; j < npt; j++ )
 			{
-				double errij = fabs(imagePoints_[0][i][j].x*lines[1][j][0] +
+				double errij = fabs(stereoImagePoints_[0][i][j].x*lines[1][j][0] +
-									 imagePoints_[0][i][j].y*lines[1][j][1] + lines[1][j][2]) +
+									stereoImagePoints_[0][i][j].y*lines[1][j][1] + lines[1][j][2]) +
-							   fabs(imagePoints_[1][i][j].x*lines[0][j][0] +
+							   fabs(stereoImagePoints_[1][i][j].x*lines[0][j][0] +
-								    imagePoints_[1][i][j].y*lines[0][j][1] + lines[0][j][2]);
+									stereoImagePoints_[1][i][j].y*lines[0][j][1] + lines[0][j][2]);
 				err += errij;
 			}
 			npoints += npt;
 		}
 		double totalAvgErr = err/(double)npoints;
 
-		UINFO("%s. avg re projection error = %f", calibrated_ ? "Calibration succeeded" : "Calibration failed", totalAvgErr);
+		UINFO("stereo avg re projection error = %f", totalAvgErr);
 
-		if(calibrated_)
+		cv::Mat R1, R2, P1, P2, Q;
-		{
+		cv::Rect validRoi[2];
 
-			cv::Mat R1, R2, P1, P2, Q;
+		cv::stereoRectify(models_[0].K(), models_[0].D(),
-			cv::Rect validRoi[2];
+						models_[1].K(), models_[1].D(),
+						imageSize, R, T, R1, R2, P1, P2, Q,
+						cv::CALIB_ZERO_DISPARITY, 0, imageSize, &validRoi[0], &validRoi[1]);
 
-			cv::stereoRectify(K[0], D[0],
+		UINFO("Valid ROI1 = %d,%d,%d,%d  ROI2 = %d,%d,%d,%d newImageSize=%d/%d",
-							K[1], D[1],
+				validRoi[0].x, validRoi[0].y, validRoi[0].width, validRoi[0].height,
-							imageSize, R, T, R1, R2, P1, P2, Q,
+				validRoi[1].x, validRoi[1].y, validRoi[1].width, validRoi[1].height,
-							cv::CALIB_ZERO_DISPARITY, 0, imageSize, &validRoi[0], &validRoi[1]);
+				imageSize.width, imageSize.height);
 
-			UINFO("Valid ROI1 = %d,%d,%d,%d  ROI2 = %d,%d,%d,%d",
+		stereoModel_ = StereoCameraModel(
-					validRoi[0].x, validRoi[0].y, validRoi[0].width, validRoi[0].height,
+				cameraName_.toStdString(),
-					validRoi[1].x, validRoi[1].y, validRoi[1].width, validRoi[1].height);
+				imageSize,
+				models_[0].K(), models_[0].D(), R1, P1,
+				models_[1].K(), models_[1].D(), R2, P2);
 
-			stereoModel_ = StereoCameraModel(cameraName_.toStdString(), imageSize, K[0], D[0], R1, P1, K[1], D[1], R2, P2);
+		std::stringstream strR1, strP1, strR2, strP2;
+		strR1 << stereoModel_.left().R();
+		strP1 << stereoModel_.left().P();
+		strR2 << stereoModel_.right().R();
+		strP2 << stereoModel_.right().P();
+		ui_->lineEdit_R->setText(strR1.str().c_str());
+		ui_->lineEdit_P->setText(strP1.str().c_str());
+		ui_->lineEdit_R_2->setText(strR2.str().c_str());
+		ui_->lineEdit_P_2->setText(strP2.str().c_str());
 
-			ui_->label_fx->setNum(stereoModel_.left().fx());
+		ui_->label_baseline->setNum(stereoModel_.baseline());
-			ui_->label_fy->setNum(stereoModel_.left().fy());
+		//ui_->label_error_stereo->setNum(totalAvgErr);
-			ui_->label_cx->setNum(stereoModel_.left().cx());
+	}
-			ui_->label_cy->setNum(stereoModel_.left().cy());
-			ui_->label_fx_2->setNum(stereoModel_.right().fx());
-			ui_->label_fy_2->setNum(stereoModel_.right().fy());
-			ui_->label_cx_2->setNum(stereoModel_.right().cx());
-			ui_->label_cy_2->setNum(stereoModel_.right().cy());
-			ui_->label_baseline->setVisible(stereoModel_.baseline());
-			ui_->label_error->setNum(totalAvgErr);
 
-			std::stringstream strK, strD, strR, strP;
+	if((!stereo_ && models_[0].isValid()) ||
-			strK << stereoModel_.left().K();
+		(stereo_ && stereoModel_.isValid()))
-			strD << stereoModel_.left().D();
+	{
-			strR << stereoModel_.left().R();
+		ui_->checkBox_rectified->setEnabled(true);
-			strP << stereoModel_.left().P();
+		ui_->checkBox_rectified->setChecked(true);
-			ui_->lineEdit_K->setText(strK.str().c_str());
-			ui_->lineEdit_D->setText(strD.str().c_str());
-			ui_->lineEdit_R->setText(strR.str().c_str());
-			ui_->lineEdit_P->setText(strP.str().c_str());
-			strK.clear();
-			strD.clear();
-			strR.clear();
-			strP.clear();
-			strK << stereoModel_.right().K();
-			strD << stereoModel_.right().D();
-			strR << stereoModel_.right().R();
-			strP << stereoModel_.right().P();
-			ui_->lineEdit_K_2->setText(strK.str().c_str());
-			ui_->lineEdit_D_2->setText(strD.str().c_str());
-			ui_->lineEdit_R_2->setText(strR.str().c_str());
-			ui_->lineEdit_P_2->setText(strP.str().c_str());
 
-			ui_->checkBox_rectified->setEnabled(true);
+		ui_->pushButton_save->setEnabled(true);
-			ui_->checkBox_rectified->setChecked(true);
-
-			ui_->pushButton_save->setEnabled(true);
-		}
 	}
 
 	UINFO("End calibration");
@@ -718,13 +718,13 @@ bool CalibrationDialog::save()
 	processingData_ = true;
 	if(!stereo_)
 	{
-		UASSERT(model_.isValid());
+		UASSERT(models_[0].isValid());
-		QString cameraName = model_.name().c_str();
+		QString cameraName = models_[0].name().c_str();
 		QString filePath = QFileDialog::getSaveFileName(this, tr("Export"), savingDirectory_+"/"+cameraName+".yaml", "*.yaml");
 
 		if(!filePath.isEmpty())
 		{
-			if(model_.save(filePath.toStdString()))
+			if(models_[0].save(filePath.toStdString()))
 			{
 				QMessageBox::information(this, tr("Export"), tr("Calibration file saved to \"%1\".").arg(filePath));
 				UINFO("Saved \"%s\"!", filePath.toStdString().c_str());

guilib/src/ui/calibrationDialog.ui

@@ -7,36 +7,79 @@
     <x>0</x>
     <y>0</y>
     <width>1314</width>
-    <height>783</height>
+    <height>795</height>
    </rect>
   </property>
   <property name="windowTitle">
    <string>Dialog</string>
   </property>
-  <layout class="QVBoxLayout" name="verticalLayout_3">
+  <layout class="QVBoxLayout" name="verticalLayout_5">
    <property name="spacing">
-    <number>6</number>
+    <number>0</number>
    </property>
-   <property name="margin">
+   <property name="leftMargin">
+    <number>0</number>
+   </property>
+   <property name="topMargin">
+    <number>0</number>
+   </property>
+   <property name="rightMargin">
+    <number>0</number>
+   </property>
+   <property name="bottomMargin">
     <number>0</number>
    </property>
    <item>
     <layout class="QHBoxLayout" name="horizontalLayout_4" stretch="1,0">
-     <property name="spacing">
-      <number>0</number>
-     </property>
      <item>
-      <layout class="QVBoxLayout" name="verticalLayout_2" stretch="1,0">
+      <layout class="QVBoxLayout" name="verticalLayout_4">
+       <property name="spacing">
+        <number>0</number>
+       </property>
        <item>
         <layout class="QHBoxLayout" name="horizontalLayout_3">
          <property name="spacing">
           <number>0</number>
          </property>
          <item>
-          <widget class="UImageView" name="image_view" native="true"/>
+          <layout class="QVBoxLayout" name="verticalLayout_2" stretch="0,1">
+           <property name="spacing">
+            <number>0</number>
+           </property>
+           <item>
+            <widget class="QLabel" name="label_left">
+             <property name="text">
+              <string/>
+             </property>
+             <property name="alignment">
+              <set>Qt::AlignCenter</set>
+             </property>
+            </widget>
+           </item>
+           <item>
+            <widget class="UImageView" name="image_view" native="true"/>
+           </item>
+          </layout>
          </item>
          <item>
-          <widget class="UImageView" name="image_view_2" native="true"/>
+          <layout class="QVBoxLayout" name="verticalLayout_3" stretch="0,1">
+           <property name="spacing">
+            <number>0</number>
+           </property>
+           <item>
+            <widget class="QLabel" name="label_right">
+             <property name="text">
+              <string/>
+             </property>
+             <property name="alignment">
+              <set>Qt::AlignCenter</set>
+             </property>
+            </widget>
+           </item>
+           <item>
+            <widget class="UImageView" name="image_view_2" native="true"/>
+           </item>
+          </layout>
          </item>
         </layout>
        </item>
@@ -154,6 +197,16 @@
             </property>
            </widget>
           </item>
+          <item row="0" column="0">
+           <widget class="QLabel" name="label_21">
+            <property name="toolTip">
+             <string>Number of inner squares on the board</string>
+            </property>
+            <property name="text">
+             <string>Download</string>
+            </property>
+           </widget>
+          </item>
           <item row="0" column="1">
            <widget class="QLabel" name="label_20">
             <property name="toolTip">
@@ -167,16 +220,6 @@
             </property>
            </widget>
           </item>
-          <item row="0" column="0">
-           <widget class="QLabel" name="label_21">
-            <property name="toolTip">
-             <string>Number of inner squares on the board</string>
-            </property>
-            <property name="text">
-             <string>Download</string>
-            </property>
-           </widget>
-          </item>
          </layout>
         </widget>
        </item>
@@ -325,6 +368,19 @@
             </property>
            </widget>
           </item>
+          <item row="1" column="2">
+           <widget class="QProgressBar" name="progressBar_count_2">
+            <property name="value">
+             <number>24</number>
+            </property>
+            <property name="invertedAppearance">
+             <bool>true</bool>
+            </property>
+            <property name="format">
+             <string>%p</string>
+            </property>
+           </widget>
+          </item>
          </layout>
         </widget>
        </item>
@@ -579,6 +635,13 @@
             </property>
            </widget>
           </item>
+          <item row="9" column="2">
+           <widget class="QLabel" name="label_error_2">
+            <property name="text">
+             <string>0</string>
+            </property>
+           </widget>
+          </item>
          </layout>
         </widget>
        </item>