From 19225a66be982ddfc9622d2e9089026620e73118 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Sun, 27 Mar 2016 10:09:30 -0400
Subject: [PATCH] Updated camera_info conversion for odometry nodes
 (https://github.com/introlab/rtabmap/issues/64)

---
 src/RGBDOdometryNode.cpp   | 18 ++----------------
 src/StereoOdometryNode.cpp | 19 +++++--------------
 2 files changed, 7 insertions(+), 30 deletions(-)

diff --git a/src/RGBDOdometryNode.cpp b/src/RGBDOdometryNode.cpp
index abd08b0c..c1c07958 100644
--- a/src/RGBDOdometryNode.cpp
+++ b/src/RGBDOdometryNode.cpp
@@ -189,14 +189,7 @@ public:
 
 			if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
 			{
-				image_geometry::PinholeCameraModel model;
-				model.fromCameraInfo(*cameraInfo);
-				rtabmap::CameraModel rtabmapModel(
-						model.fx(),
-						model.fy(),
-						model.cx(),
-						model.cy(),
-						localTransform);
+				rtabmap::CameraModel rtabmapModel = rtabmap_ros::cameraModelFromROS(*cameraInfo, localTransform);
 				cv_bridge::CvImagePtr ptrImage = cv_bridge::toCvCopy(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8");
 				cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth);
 
@@ -321,14 +314,7 @@ public:
 					return;
 				}
 
-				image_geometry::PinholeCameraModel model;
-				model.fromCameraInfo(*infoMsgs[i]);
-				cameraModels.push_back(rtabmap::CameraModel(
-						model.fx(),
-						model.fy(),
-						model.cx(),
-						model.cy(),
-						localTransform));
+				cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*infoMsgs[i], localTransform));
 			}
 
 			rtabmap::SensorData data(
diff --git a/src/StereoOdometryNode.cpp b/src/StereoOdometryNode.cpp
index 55d4a952..28f0dcac 100644
--- a/src/StereoOdometryNode.cpp
+++ b/src/StereoOdometryNode.cpp
@@ -145,24 +145,15 @@ public:
 			int quality = -1;
 			if(imageRectLeft->data.size() && imageRectRight->data.size())
 			{
-				image_geometry::StereoCameraModel model;
-				model.fromCameraInfo(*cameraInfoLeft, *cameraInfoRight);
-				if(model.baseline() <= 0)
+				rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*cameraInfoLeft, *cameraInfoRight, localTransform);
+				if(stereoModel.baseline() <= 0)
 				{
 					ROS_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
-							  "setup where the Tx (or P(0,3)) is negative in the right camera info msg.", model.baseline());
+							  "setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline());
 					return;
 				}
 
-				rtabmap::StereoCameraModel stereoModel(
-						model.left().fx(),
-						model.left().fy(),
-						model.left().cx(),
-						model.left().cy(),
-						model.baseline(),
-						localTransform);
-
-				if(model.baseline() > 10.0)
+				if(stereoModel.baseline() > 10.0)
 				{
 					static bool shown = false;
 					if(!shown)
@@ -170,7 +161,7 @@ public:
 						ROS_WARN("Detected baseline (%f m) is quite large! Is your "
 								 "right camera_info P(0,3) correctly set? Note that "
 								 "baseline=-P(0,3)/P(0,0). This warning is printed only once.",
-								 model.baseline());
+								 stereoModel.baseline());
 						shown = true;
 					}
 				}