From b17e0dc26d74e99884278656883da242160d5cad Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Fri, 30 Sep 2016 14:38:45 -0400
Subject: [PATCH] Calibration 3-steps: fixed crash on step 3 if intrinsic
 parameters are not correctly loaded (empty distorsion model)

---
 guilib/src/CalibrationDialog.cpp | 39 +++++++++++++++++++++-----------
 guilib/src/PreferencesDialog.cpp |  6 ++++-
 2 files changed, 31 insertions(+), 14 deletions(-)

diff --git a/guilib/src/CalibrationDialog.cpp b/guilib/src/CalibrationDialog.cpp
index b4ffd236..d59ccc31 100644
--- a/guilib/src/CalibrationDialog.cpp
+++ b/guilib/src/CalibrationDialog.cpp
@@ -825,36 +825,49 @@ void CalibrationDialog::calibrate()
 StereoCameraModel CalibrationDialog::stereoCalibration(const CameraModel & left, const CameraModel & right, bool ignoreStereoRectification) const
 {
 	StereoCameraModel output;
-	if(stereoImagePoints_[0].empty())
+	if (stereoImagePoints_[0].empty())
 	{
 		UERROR("No stereo correspondences!");
 		return output;
 	}
 	UINFO("stereo calibration (samples=%d)...", (int)stereoImagePoints_[0].size());
 
-	if(left.imageSize()!=imageSize_[0])
+	if (left.K_raw().empty() || left.D_raw().empty())
 	{
-		UERROR("left model (%dx%d) has not the same size as the processed images (%dx%d)",
-				left.imageSize().width, left.imageSize().height,
-				imageSize_[0].width, imageSize_[0].height);
+		UERROR("Empty intrinsic parameters (K, D) for the %s camera! Aborting stereo calibration...", leftSuffix_.toStdString().c_str());
+		return output;
 	}
-	if(right.imageSize()!=imageSize_[1])
+	if (right.K_raw().empty() || right.D_raw().empty())
 	{
-		UERROR("right model (%dx%d) has not the same size as the processed images (%dx%d)",
-				right.imageSize().width, right.imageSize().height,
-				imageSize_[1].width, imageSize_[1].height);
+		UERROR("Empty intrinsic parameters (K, D) for the %s camera! Aborting stereo calibration...", rightSuffix_.toStdString().c_str());
+		return output;
 	}
 
-	cv::Size imageSize = imageSize_[0].width > imageSize_[1].width?imageSize_[0]:imageSize_[1];
+	if (left.imageSize() != imageSize_[0])
+	{
+		UERROR("left model (%dx%d) has not the same size as the processed images (%dx%d)",
+			left.imageSize().width, left.imageSize().height,
+			imageSize_[0].width, imageSize_[0].height);
+		return output;
+	}
+	if (right.imageSize() != imageSize_[1])
+	{
+		UERROR("right model (%dx%d) has not the same size as the processed images (%dx%d)",
+			right.imageSize().width, right.imageSize().height,
+			imageSize_[1].width, imageSize_[1].height);
+		return output;
+	}
+
+	cv::Size imageSize = imageSize_[0].width > imageSize_[1].width ? imageSize_[0] : imageSize_[1];
 	cv::Mat R, T, E, F;
 
 	std::vector<std::vector<cv::Point3f> > objectPoints(1);
 	cv::Size boardSize(ui_->spinBox_boardWidth->value(), ui_->spinBox_boardHeight->value());
 	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));
+	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
diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp
index 7a9e629d..b9fc8b0a 100644
--- a/guilib/src/PreferencesDialog.cpp
+++ b/guilib/src/PreferencesDialog.cpp
@@ -4881,7 +4881,11 @@ void PreferencesDialog::calibrate()
 						if(stereoModel.stereoTransform().isNull())
 						{
 							QMessageBox::warning(this, tr("Calibration"),
-									tr("Extrinsic calibration has failed!"), QMessageBox::Ok);
+									tr("Extrinsic calibration has failed! Look on the terminal "
+									   "for possible error messages. Make sure the calibration "
+									   "name is set and that corresponding calibration files exist "
+									   "in \"Documents/RTAB-Map/camera_info\" folder. If not, re-do "
+									   "step 1 and 2 and make sure to export the calibration files."), QMessageBox::Ok);
 						}
 						else if(stereoModel.saveStereoTransform(this->getCameraInfoDir().toStdString()))
 						{