RegVis: if calibration is not found, local features matching using guess is not done, global matching is done instead. CameraModel/StereoCameraModel: added imageSize argument to constructors

corelib/include/rtabmap/core/CameraModel.h

@@ -59,7 +59,8 @@ public:
 			double cx,
 			double cy,
 			const Transform & localTransform = Transform::getIdentity(),
-			double Tx = 0.0f);
+			double Tx = 0.0f,
+			const cv::Size & imageSize = cv::Size(0,0));
 	// minimal to be saved
 	CameraModel(
 			const std::string & name,
@@ -68,7 +69,8 @@ public:
 			double cx,
 			double cy,
 			const Transform & localTransform = Transform::getIdentity(),
-			double Tx = 0.0f);
+			double Tx = 0.0f,
+			const cv::Size & imageSize = cv::Size(0,0));
 
 	virtual ~CameraModel() {}
 

corelib/include/rtabmap/core/StereoCameraModel.h

@@ -68,7 +68,8 @@ public:
 			double cx,
 			double cy,
 			double baseline,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = Transform::getIdentity(),
+			const cv::Size & imageSize = cv::Size(0,0));
 	//minimal to be saved
 	StereoCameraModel(
 			const std::string & name,
@@ -77,7 +78,8 @@ public:
 			double cx,
 			double cy,
 			double baseline,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = Transform::getIdentity(),
+			const cv::Size & imageSize = cv::Size(0,0));
 	virtual ~StereoCameraModel() {}
 
 	bool isValidForProjection() const {return left_.isValidForProjection() && right_.isValidForProjection() && baseline() > 0.0;}

corelib/src/CameraModel.cpp

@@ -67,7 +67,9 @@ CameraModel::CameraModel(
 		double cx,
 		double cy,
 		const Transform & localTransform,
-		double Tx) :
+		double Tx,
+		const cv::Size & imageSize) :
+		imageSize_(imageSize),
 		K_(cv::Mat::eye(3, 3, CV_64FC1)),
 		localTransform_(localTransform)
 {
@@ -99,8 +101,10 @@ CameraModel::CameraModel(
 		double cx,
 		double cy,
 		const Transform & localTransform,
-		double Tx) :
+		double Tx,
+		const cv::Size & imageSize) :
 		name_(name),
+		imageSize_(imageSize),
 		K_(cv::Mat::eye(3, 3, CV_64FC1)),
 		localTransform_(localTransform)
 {

corelib/src/RegistrationVis.cpp

@@ -540,9 +540,18 @@ Transform RegistrationVis::computeTransformationImpl(
 			// We have all data we need here, so match!
 			if(descriptorsFrom.rows > 0 && descriptorsTo.rows > 0)
 			{
+				cv::Size imageSize = toSignature.sensorData().imageRaw().size();
+				bool isCalibrated = false;
+				if(imageSize.height == 0 || imageSize.width == 0)
+				{
+					imageSize = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].imageSize():fromSignature.sensorData().stereoCameraModel().left().imageSize();
+				}
+				isCalibrated = imageSize.height != 0 && imageSize.width != 0 && fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].isValidForProjection():fromSignature.sensorData().stereoCameraModel().isValidForProjection();
+
 				// If guess is set, limit the search of matches using optical flow window size
 				bool guessSet = !guess.isIdentity() && !guess.isNull();
-				if(guessSet && _guessWinSize > 0 && kptsFrom3D.size())
+				if(guessSet && _guessWinSize > 0 && kptsFrom3D.size() &&
+						isCalibrated) // needed for projection
 				{
 					UDEBUG("");
 					UASSERT((int)kptsTo.size() == descriptorsTo.rows);
@@ -573,8 +582,8 @@ Transform RegistrationVis::computeTransformationImpl(
 					int oi=0;
 					for(unsigned int i=0; i<projected.size(); ++i)
 					{
-						if(uIsInBounds(projected[i].x, 0.0f, float(toSignature.sensorData().imageRaw().cols-1)) &&
+						if(uIsInBounds(projected[i].x, 0.0f, float(imageSize.width-1)) &&
-						   uIsInBounds(projected[i].y, 0.0f, float(toSignature.sensorData().imageRaw().rows-1)))
+						   uIsInBounds(projected[i].y, 0.0f, float(imageSize.height-1)))
 						{
 							projectedIndexToDescIndex[oi] = i;
 							cornersProjected[oi++] = projected[i];
@@ -755,6 +764,13 @@ Transform RegistrationVis::computeTransformationImpl(
 				}
 				else
 				{
+					if(guessSet && _guessWinSize > 0 && kptsFrom3D.size() && !isCalibrated)
+					{
+						UWARN("Calibration not found! Finding correspondences "
+							   "with the guess cannot be done, global matching is "
+							   "done instead.");
+					}
+
 					UDEBUG("");
 					// match between all descriptors
 					VWDictionary dictionary(_featureParameters);

corelib/src/StereoCameraModel.cpp

@@ -130,9 +130,10 @@ StereoCameraModel::StereoCameraModel(
 		double cx,
 		double cy,
 		double baseline,
-		const Transform & localTransform) :
+		const Transform & localTransform,
-	left_(fx, fy, cx, cy, localTransform),
+		const cv::Size & imageSize) :
-	right_(fx, fy, cx, cy, localTransform, baseline*-fx)
+	left_(fx, fy, cx, cy, localTransform, 0, imageSize),
+	right_(fx, fy, cx, cy, localTransform, baseline*-fx, imageSize)
 {
 }
 
@@ -144,9 +145,10 @@ StereoCameraModel::StereoCameraModel(
 		double cx,
 		double cy,
 		double baseline,
-		const Transform & localTransform) :
+		const Transform & localTransform,
-	left_(name+"_left", fx, fy, cx, cy, localTransform),
+		const cv::Size & imageSize) :
-	right_(name+"_right", fx, fy, cx, cy, localTransform, baseline*-fx),
+	left_(name+"_left", fx, fy, cx, cy, localTransform, 0, imageSize),
+	right_(name+"_right", fx, fy, cx, cy, localTransform, baseline*-fx, imageSize),
 	name_(name)
 {
 }