Changed CameraModel::isValid() to CameraModel::isValidForProjection() for clarity (in contrast to CameraModel::isValidForRectification())

guilib/src/CalibrationDialog.cpp

@@ -200,10 +200,9 @@ void CalibrationDialog::setSquareSize(double size)
 
 void CalibrationDialog::closeEvent(QCloseEvent* event)
 {
-	if(!savedCalibration_ && models_[0].isValid() &&
+	if(!savedCalibration_ && models_[0].isValidForRectification() &&
 			(!stereo_ ||
-					(stereoModel_.left().isValid() &&
-					stereoModel_.right().isValid()&&
+					(stereoModel_.isValidForRectification() &&
 					(!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0))))
 	{
 		QMessageBox::StandardButton b = QMessageBox::question(this, tr("Save calibration?"),
@@ -737,7 +736,7 @@ void CalibrationDialog::calibrate()
 		}
 	}
 
-	if(stereo_ && models_[0].isValid() && models_[1].isValid())
+	if(stereo_ && models_[0].isValidForRectification() && models_[1].isValidForRectification())
 	{
 		UINFO("stereo calibration (samples=%d)...", (int)stereoImagePoints_[0].size());
 		cv::Size imageSize = imageSize_[0].width > imageSize_[1].width?imageSize_[0]:imageSize_[1];
@@ -758,8 +757,8 @@ void CalibrationDialog::calibrate()
 				objectPoints,
 				stereoImagePoints_[0],
 				stereoImagePoints_[1],
-				models_[0].K(), models_[0].D(),
-				models_[1].K(), models_[1].D(),
+				models_[0].K_raw(), models_[0].D_raw(),
+				models_[1].K_raw(), models_[1].D_raw(),
 				imageSize, R, T, E, F,
 				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5),
 				cv::CALIB_FIX_INTRINSIC);
@@ -768,72 +767,66 @@ void CalibrationDialog::calibrate()
 				objectPoints,
 				stereoImagePoints_[0],
 				stereoImagePoints_[1],
-				models_[0].K(), models_[0].D(),
-				models_[1].K(), models_[1].D(),
+				models_[0].K_raw(), models_[0].D_raw(),
+				models_[1].K_raw(), models_[1].D_raw(),
 				imageSize, R, T, E, F,
 				cv::CALIB_FIX_INTRINSIC,
 				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
 #endif
 		UINFO("stereo calibration... done with RMS error=%f", rms);
 
-		double err = 0;
-		int npoints = 0;
-		std::vector<cv::Vec3f> lines[2];
-		UINFO("Computing avg re-projection error...");
-		for(unsigned int i = 0; i < stereoImagePoints_[0].size(); i++ )
+		if(imageSize_[0] == imageSize_[1])
 		{
-			int npt = (int)stereoImagePoints_[0][i].size();
-			cv::Mat imgpt[2];
-			for(int k = 0; k < 2; k++ )
+			//Stereo, compute stereo rectification
+
+			cv::Mat R1, R2, P1, P2, Q;
+			cv::stereoRectify(models_[0].K_raw(), models_[0].D_raw(),
+							models_[1].K_raw(), models_[1].D_raw(),
+							imageSize, R, T, R1, R2, P1, P2, Q,
+							cv::CALIB_ZERO_DISPARITY, 0, imageSize);
+
+			double err = 0;
+			int npoints = 0;
+			std::vector<cv::Vec3f> lines[2];
+			UINFO("Computing avg re-projection error...");
+			for(unsigned int i = 0; i < stereoImagePoints_[0].size(); i++ )
 			{
-				imgpt[k] = cv::Mat(stereoImagePoints_[k][i]);
-				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]);
+				int npt = (int)stereoImagePoints_[0][i].size();
+
+					cv::Mat imgpt0 = cv::Mat(stereoImagePoints_[0][i]);
+					cv::Mat imgpt1 = cv::Mat(stereoImagePoints_[1][i]);
+					cv::undistortPoints(imgpt0, imgpt0, models_[0].K_raw(), models_[0].D_raw(), R1, P1);
+					cv::undistortPoints(imgpt1, imgpt1, models_[1].K_raw(), models_[1].D_raw(), R2, P2);
+					computeCorrespondEpilines(imgpt0, 1, F, lines[0]);
+					computeCorrespondEpilines(imgpt1, 2, F, lines[1]);
+
+				for(int j = 0; j < npt; j++ )
+				{
+					double errij = fabs(stereoImagePoints_[0][i][j].x*lines[1][j][0] +
+										stereoImagePoints_[0][i][j].y*lines[1][j][1] + lines[1][j][2]) +
+								   fabs(stereoImagePoints_[1][i][j].x*lines[0][j][0] +
+										stereoImagePoints_[1][i][j].y*lines[0][j][1] + lines[0][j][2]);
+					err += errij;
+				}
+				npoints += npt;
 			}
-			for(int j = 0; j < npt; j++ )
-			{
-				double errij = fabs(stereoImagePoints_[0][i][j].x*lines[1][j][0] +
-									stereoImagePoints_[0][i][j].y*lines[1][j][1] + lines[1][j][2]) +
-							   fabs(stereoImagePoints_[1][i][j].x*lines[0][j][0] +
-									stereoImagePoints_[1][i][j].y*lines[0][j][1] + lines[0][j][2]);
-				err += errij;
-			}
-			npoints += npt;
-		}
-		double totalAvgErr = err/(double)npoints;
+			double totalAvgErr = err/(double)npoints;
+			UINFO("stereo avg re projection error = %f", totalAvgErr);
 
-		UINFO("stereo avg re projection error = %f", totalAvgErr);
-
-		cv::Mat R1, R2, P1, P2, Q;
-		cv::Rect validRoi[2];
-
-		cv::stereoRectify(models_[0].K(), models_[0].D(),
-						models_[1].K(), models_[1].D(),
-						imageSize, R, T, R1, R2, P1, P2, Q,
-						cv::CALIB_ZERO_DISPARITY, 0, imageSize, &validRoi[0], &validRoi[1]);
-
-		UINFO("Valid ROI1 = %d,%d,%d,%d  ROI2 = %d,%d,%d,%d newImageSize=%d/%d",
-				validRoi[0].x, validRoi[0].y, validRoi[0].width, validRoi[0].height,
-				validRoi[1].x, validRoi[1].y, validRoi[1].width, validRoi[1].height,
-				imageSize.width, imageSize.height);
-
-		if(imageSize_[0].width == imageSize_[1].width)
-		{
-			//Stereo, keep new extrinsic projection matrix
 			stereoModel_ = StereoCameraModel(
-					cameraName_.toStdString(),
-					imageSize_[0], models_[0].K(), models_[0].D(), R1, P1,
-					imageSize_[1], models_[1].K(), models_[1].D(), R2, P2,
-					R, T, E, F);
+							cameraName_.toStdString(),
+							imageSize_[0], models_[0].K_raw(), models_[0].D_raw(), R1, P1,
+							imageSize_[1], models_[1].K_raw(), models_[1].D_raw(), R2, P2,
+							R, T, E, F);
 		}
 		else
 		{
-			//Kinect
+			//Kinect, ignore the stereo rectification
 			stereoModel_ = StereoCameraModel(
-					cameraName_.toStdString(),
-					imageSize_[0], models_[0].K(), models_[0].D(), cv::Mat::eye(3,3,CV_64FC1), models_[0].P(),
-					imageSize_[1], models_[1].K(), models_[1].D(), cv::Mat::eye(3,3,CV_64FC1), models_[1].P(),
-					R, T, E, F);
+							cameraName_.toStdString(),
+							imageSize_[0], models_[0].K_raw(), models_[0].D_raw(), models_[0].R(), models_[0].P(),
+							imageSize_[1], models_[1].K_raw(), models_[1].D_raw(), models_[1].R(), models_[1].P(),
+							R, T, E, F);
 		}
 
 		std::stringstream strR1, strP1, strR2, strP2;
@@ -854,6 +847,8 @@ void CalibrationDialog::calibrate()
 		stereoModel_.isValidForRectification())
 	{
 		stereoModel_.initRectificationMap();
+		models_[0].initRectificationMap();
+		models_[1].initRectificationMap();
 		ui_->radioButton_rectified->setEnabled(true);
 		ui_->radioButton_stereoRectified->setEnabled(true);
 		ui_->radioButton_stereoRectified->setChecked(true);
@@ -877,7 +872,7 @@ bool CalibrationDialog::save()
 	processingData_ = true;
 	if(!stereo_)
 	{
-		UASSERT(models_[0].isValid());
+		UASSERT(models_[0].isValidForRectification());
 		QString cameraName = models_[0].name().c_str();
 		QString filePath = QFileDialog::getSaveFileName(this, tr("Export"), savingDirectory_+"/"+cameraName+".yaml", "*.yaml");
 
@@ -901,8 +896,8 @@ bool CalibrationDialog::save()
 	}
 	else
 	{
-		UASSERT(stereoModel_.left().isValid() &&
-				stereoModel_.right().isValid()&&
+		UASSERT(stereoModel_.left().isValidForRectification() &&
+				stereoModel_.right().isValidForRectification() &&
 				(!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0));
 		QString cameraName = stereoModel_.name().c_str();
 		QString filePath = QFileDialog::getSaveFileName(this, tr("Export"), savingDirectory_ + "/" + cameraName, "*.yaml");

guilib/src/CameraViewer.cpp

@@ -139,7 +139,7 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
 	}
 
 	if(!data.depthOrRightRaw().empty() &&
-	   (data.stereoCameraModel().isValid() || (data.cameraModels().size() && data.cameraModels().at(0).isValid())))
+	   (data.stereoCameraModel().isValidForProjection() || (data.cameraModels().size() && data.cameraModels().at(0).isValidForProjection())))
 	{
 		if(showCloudCheckbox_->isChecked())
 		{

guilib/src/CreateSimpleCalibrationDialog.cpp

@@ -171,7 +171,7 @@ void CreateSimpleCalibrationDialog::saveCalibration()
 					ui_->doubleSpinBox_fy->value(),
 					ui_->doubleSpinBox_cx->value(),
 					ui_->doubleSpinBox_cy->value());
-			UASSERT(modelLeft.isValid());
+			UASSERT(modelLeft.isValidForProjection());
 		}
 		else
 		{
@@ -216,8 +216,9 @@ void CreateSimpleCalibrationDialog::saveCalibration()
 						ui_->doubleSpinBox_cy->value(),
 						Transform::getIdentity(),
 						ui_->doubleSpinBox_baseline->value()*-ui_->doubleSpinBox_fx->value());
-				UASSERT(modelRight.isValid());
+				UASSERT(modelRight.isValidForProjection());
 				stereoModel = StereoCameraModel(name.toStdString(), modelLeft, modelRight, Transform());
+				UASSERT(stereoModel.isValidForProjection());
 			}
 			else if(ui_->comboBox_advanced->currentIndex() == 1)
 			{
@@ -250,6 +251,7 @@ void CreateSimpleCalibrationDialog::saveCalibration()
 
 				UASSERT(Transform::canParseString(ui_->lineEdit_RT->text().trimmed().toStdString()));
 				stereoModel = StereoCameraModel(name.toStdString(), modelLeft, modelRight, Transform::fromString(ui_->lineEdit_RT->text().toStdString()));
+				UASSERT(stereoModel.isValidForRectification());
 			}
 
 			std::string base = (dir+QDir::separator()+name).toStdString();

guilib/src/DatabaseViewer.cpp

@@ -866,7 +866,7 @@ void DatabaseViewer::extractImages()
 				{
 					UERROR("Cannot save calibration file, database name is empty!");
 				}
-				else if(data.stereoCameraModel().isValid())
+				else if(data.stereoCameraModel().isValidForProjection())
 				{
 					std::string cameraName = uSplit(databaseFileName_, '.').front();
 					StereoCameraModel model(
@@ -913,7 +913,7 @@ void DatabaseViewer::extractImages()
 				{
 					UERROR("Only one camera calibration can be saved at this time (%d detected)", (int)data.cameraModels().size());
 				}
-				else if(data.cameraModels().size() == 1 && data.cameraModels().front().isValid())
+				else if(data.cameraModels().size() == 1 && data.cameraModels().front().isValidForProjection())
 				{
 					std::string cameraName = uSplit(databaseFileName_, '.').front();
 					CameraModel model(cameraName,
@@ -2349,7 +2349,7 @@ void DatabaseViewer::updateStereo(const SensorData * data)
 		!data->imageRaw().empty() &&
 		!data->depthOrRightRaw().empty() &&
 		data->depthOrRightRaw().type() == CV_8UC1 &&
-		data->stereoCameraModel().isValid())
+		data->stereoCameraModel().isValidForProjection())
 	{
 		cv::Mat leftMono;
 		if(data->imageRaw().channels() == 3)

guilib/src/MainWindow.cpp

@@ -807,7 +807,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
 				if(odom.data().depthOrRightRaw().cols == odom.data().imageRaw().cols &&
 				   odom.data().depthOrRightRaw().rows == odom.data().imageRaw().rows &&
 				   !odom.data().depthOrRightRaw().empty() &&
-				   (odom.data().cameraModels().size() || odom.data().stereoCameraModel().isValid()) &&
+				   (odom.data().cameraModels().size() || odom.data().stereoCameraModel().isValidForProjection()) &&
 				   _preferencesDialog->isCloudsShown(1))
 				{
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
@@ -5097,11 +5097,11 @@ bool MainWindow::getExportedClouds(
 					{
 						const Signature & s = _cachedSignatures.value(jter->first);
 						CameraModel model;
-						if(s.sensorData().stereoCameraModel().isValid())
+						if(s.sensorData().stereoCameraModel().isValidForProjection())
 						{
 							model = s.sensorData().stereoCameraModel().left();
 						}
-						else if(s.sensorData().cameraModels().size() == 1 && s.sensorData().cameraModels()[0].isValid())
+						else if(s.sensorData().cameraModels().size() == 1 && s.sensorData().cameraModels()[0].isValidForProjection())
 						{
 							model = s.sensorData().cameraModels()[0];
 						}
@@ -5110,7 +5110,7 @@ bool MainWindow::getExportedClouds(
 						{
 							s.sensorData().uncompressDataConst(&image, 0, 0, 0);
 						}
-						if(!jter->second.isNull() && model.isValid() && !image.empty())
+						if(!jter->second.isNull() && model.isValidForProjection() && !image.empty())
 						{
 							cameraPoses.insert(std::make_pair(jter->first, jter->second));
 							cameraModels.insert(std::make_pair(jter->first, model));
@@ -5171,7 +5171,7 @@ void MainWindow::exportImages()
 			QDir dir;
 			dir.mkdir(QString("%1/left").arg(path));
 			dir.mkdir(QString("%1/right").arg(path));
-			if(data.stereoCameraModel().isValid())
+			if(data.stereoCameraModel().isValidForProjection())
 			{
 				std::string cameraName = "calibration";
 				StereoCameraModel model(
@@ -5214,7 +5214,7 @@ void MainWindow::exportImages()
 			{
 				UERROR("Only one camera calibration can be saved at this time (%d detected)", (int)data.cameraModels().size());
 			}
-			else if(data.cameraModels().size() == 1 && data.cameraModels().front().isValid())
+			else if(data.cameraModels().size() == 1 && data.cameraModels().front().isValidForProjection())
 			{
 				std::string cameraName = "calibration";
 				CameraModel model(cameraName,
@@ -5313,8 +5313,8 @@ void MainWindow::exportBundlerFormat()
 			{
 				UWARN("Missing image in cache for node %d", iter->first);
 			}
-			else if((_cachedSignatures[iter->first].sensorData().cameraModels().size() == 1 && _cachedSignatures[iter->first].sensorData().cameraModels().at(0).isValid()) ||
-			         _cachedSignatures[iter->first].sensorData().stereoCameraModel().isValid())
+			else if((_cachedSignatures[iter->first].sensorData().cameraModels().size() == 1 && _cachedSignatures[iter->first].sensorData().cameraModels().at(0).isValidForProjection()) ||
+			         _cachedSignatures[iter->first].sensorData().stereoCameraModel().isValidForProjection())
 			{
 				poses.insert(*iter);
 			}

guilib/src/OdometryViewer.cpp

@@ -192,7 +192,7 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
 
 	if(!odom.data().imageRaw().empty() &&
 		!odom.data().depthOrRightRaw().empty() &&
-		(odom.data().stereoCameraModel().isValid() || odom.data().cameraModels().size()))
+		(odom.data().stereoCameraModel().isValidForProjection() || odom.data().cameraModels().size()))
 	{
 		UDEBUG("New pose = %s, quality=%d", odom.pose().prettyPrint().c_str(), quality);