Fixed build with latest rtabmap 0.11

include/rtabmap_ros/MsgConversion.h

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <tf/tf.h>
 #include <geometry_msgs/Transform.h>
 #include <geometry_msgs/Pose.h>
+#include <sensor_msgs/CameraInfo.h>
 
 #include <opencv2/opencv.hpp>
 #include <opencv2/features2d/features2d.hpp>
@@ -40,6 +41,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Signature.h>
 #include <rtabmap/core/OdometryInfo.h>
 #include <rtabmap/core/Statistics.h>
+#include <rtabmap/core/StereoCameraModel.h>
 
 #include <rtabmap_ros/Link.h>
 #include <rtabmap_ros/KeyPoint.h>
@@ -83,6 +85,14 @@ void point2fToROS(const cv::Point2f & kpt, rtabmap_ros::Point2f & msg);
 std::vector<cv::Point2f> points2fFromROS(const std::vector<rtabmap_ros::Point2f> & msg);
 void points2fToROS(const std::vector<cv::Point2f> & kpts, std::vector<rtabmap_ros::Point2f> & msg);
 
+rtabmap::CameraModel cameraModelFromROS(
+		const sensor_msgs::CameraInfo & camInfo,
+		const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity());
+rtabmap::StereoCameraModel stereoCameraModelFromROS(
+		const sensor_msgs::CameraInfo & leftCamInfo,
+		const sensor_msgs::CameraInfo & rightCamInfo,
+		const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity());
+
 void mapDataFromROS(
 		const rtabmap_ros::MapData & msg,
 		std::map<int, rtabmap::Transform> & poses,

src/CameraNode.cpp

@@ -193,7 +193,7 @@ public:
 			if(!path.empty() && UDirectory::exists(path))
 			{
 				//images
-				camera_ = new rtabmap::CameraImages(path, 1, false, false, false, frameRate);
+				camera_ = new rtabmap::CameraImages(path, frameRate);
 			}
 			else if(!path.empty() && UFile::exists(path))
 			{

src/CoreWrapper.cpp

@@ -54,7 +54,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl_conversions/pcl_conversions.h>
 
 #include <laser_geometry/laser_geometry.h>
-#include <image_geometry/stereo_camera_model.h>
 
 #ifdef WITH_OCTOMAP
 #include <octomap_msgs/conversions.h>
@@ -817,23 +816,16 @@ void CoreWrapper::commonDepthCallback(
 			return;
 		}
 
-		image_geometry::PinholeCameraModel model;
+		cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*cameraInfoMsgs[i], localTransform));
-		model.fromCameraInfo(*cameraInfoMsgs[i]);
-		cameraModels.push_back(rtabmap::CameraModel(
-				model.fx(),
-				model.fy(),
-				model.cx(),
-				model.cy(),
-				localTransform));
 
 		if(scan2dMsg.get() == 0 && genScan_)
 		{
 			scanCloud2d += util3d::laserScanFromDepthImage(
 					subDepth,
-					model.fx(),
+					cameraModels.back().fx(),
-					model.fy(),
+					cameraModels.back().fy(),
-					model.cx(),
+					cameraModels.back().cx(),
-					model.cy(),
+					cameraModels.back().cy(),
 					genScanMaxDepth_,
 					localTransform);
 			genMaxScanPts += subDepth.cols;
@@ -1009,17 +1001,9 @@ void CoreWrapper::commonStereoCallback(
 	}
 	ptrRightImage = cv_bridge::toCvShare(rightImageMsg, "mono8");
 
-	image_geometry::StereoCameraModel model;
+	rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform);
-	model.fromCameraInfo(*leftCamInfoMsg, *rightCamInfoMsg);
-	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)
@@ -1027,7 +1011,7 @@ void CoreWrapper::commonStereoCallback(
 			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;
 		}
 	}

src/GuiWrapper.cpp

@@ -40,9 +40,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <opencv2/highgui/highgui.hpp>
 
-#include <image_geometry/pinhole_camera_model.h>
-#include <image_geometry/stereo_camera_model.h>
-
 #include <rtabmap/gui/MainWindow.h>
 #include <rtabmap/core/RtabmapEvent.h>
 #include <rtabmap/core/Parameters.h>
@@ -692,14 +689,7 @@ void GuiWrapper::commonDepthCallback(
 					return;
 				}
 
-				image_geometry::PinholeCameraModel model;
+				cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*cameraInfoMsgs[i], localTransform));
-				model.fromCameraInfo(*cameraInfoMsgs[i]);
-				cameraModels.push_back(rtabmap::CameraModel(
-						model.fx(),
-						model.fy(),
-						model.cx(),
-						model.cy(),
-						localTransform));
 			}
 		}
 
@@ -862,17 +852,9 @@ void GuiWrapper::commonStereoCallback(
 			}
 		}
 
-		image_geometry::StereoCameraModel model;
+		rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform);
-		model.fromCameraInfo(*leftCamInfoMsg, *rightCamInfoMsg);
-		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)
@@ -880,7 +862,7 @@ void GuiWrapper::commonStereoCallback(
 				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;
 			}
 		}

src/MsgConversion.cpp

@@ -36,6 +36,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl_conversions/pcl_conversions.h>
 #include <eigen_conversions/eigen_msg.h>
 #include <tf_conversions/tf_eigen.h>
+#include <image_geometry/pinhole_camera_model.h>
+#include <image_geometry/stereo_camera_model.h>
 
 namespace rtabmap_ros {
 
@@ -293,6 +295,35 @@ void points2fToROS(const std::vector<cv::Point2f> & kpts, std::vector<rtabmap_ro
 	}
 }
 
+rtabmap::CameraModel cameraModelFromROS(
+		const sensor_msgs::CameraInfo & camInfo,
+		const rtabmap::Transform & localTransform)
+{
+	image_geometry::PinholeCameraModel model;
+	model.fromCameraInfo(camInfo);
+	return rtabmap::CameraModel(
+			model.fx(),
+			model.fy(),
+			model.cx(),
+			model.cy(),
+			localTransform);
+}
+rtabmap::StereoCameraModel stereoCameraModelFromROS(
+		const sensor_msgs::CameraInfo & leftCamInfo,
+		const sensor_msgs::CameraInfo & rightCamInfo,
+		const rtabmap::Transform & localTransform)
+{
+	image_geometry::StereoCameraModel model;
+	model.fromCameraInfo(leftCamInfo, rightCamInfo);
+	return rtabmap::StereoCameraModel(
+			model.left().fx(),
+			model.left().fy(),
+			model.left().cx(),
+			model.left().cy(),
+			model.baseline(),
+			localTransform);
+}
+
 void mapDataFromROS(
 		const rtabmap_ros::MapData & msg,
 		std::map<int, rtabmap::Transform> & poses,
@@ -384,7 +415,7 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 {
 	//Features stuff...
 	std::multimap<int, cv::KeyPoint> words;
-	std::multimap<int, pcl::PointXYZ> words3D;
+	std::multimap<int, cv::Point3f> words3D;
 	pcl::PointCloud<pcl::PointXYZ> cloud;
 	if(msg.wordPts.data.size() &&
 	   msg.wordPts.data.size() == msg.wordIds.size())
@@ -398,7 +429,7 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 		words.insert(std::make_pair(wordId, pt));
 		if(i< cloud.size())
 		{
-			words3D.insert(std::make_pair(wordId, cloud[i]));
+			words3D.insert(std::make_pair(wordId, cv::Point3f(cloud[i].x, cloud[i].y, cloud[i].z)));
 		}
 	}
 
@@ -539,11 +570,13 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 		pcl::PointCloud<pcl::PointXYZ> cloud;
 		cloud.resize(signature.getWords3().size());
 		index = 0;
-		for(std::multimap<int, pcl::PointXYZ>::const_iterator jter=signature.getWords3().begin();
+		for(std::multimap<int, cv::Point3f>::const_iterator jter=signature.getWords3().begin();
 			jter!=signature.getWords3().end();
 			++jter)
 		{
-			cloud[index++] = jter->second;
+			cloud[index].x = jter->second.x;
+			cloud[index].y = jter->second.y;
+			cloud[index++].z = jter->second.z;
 		}
 		pcl::toROSMsg(cloud, msg.wordPts);
 	}

src/OdometryROS.cpp

@@ -216,8 +216,8 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) :
 	Parameters::parse(parameters_, Parameters::kOdomStrategy(), odomStrategy);
 	if(odomStrategy == 1)
 	{
-		ROS_INFO("Using OdometryOpticalFlow");
+		ROS_INFO("Using OdometryF2F");
-		odometry_ = new rtabmap::OdometryOpticalFlow(parameters_);
+		odometry_ = new rtabmap::OdometryF2F(parameters_);
 	}
 	else
 	{
@@ -369,13 +369,14 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 			odomPub_.publish(odom);
 		}
 
+		// local map / reference frame
 		if(odomLocalMap_.getNumSubscribers() && dynamic_cast<OdometryBOW*>(odometry_))
 		{
-			const std::map<int, pcl::PointXYZ> & map = ((OdometryBOW*)odometry_)->getLocalMap();
 			pcl::PointCloud<pcl::PointXYZ> cloud;
-			for(std::map<int, pcl::PointXYZ>::const_iterator iter=map.begin(); iter!=map.end(); ++iter)
+			const std::map<int, cv::Point3f> & map = ((OdometryBOW*)odometry_)->getLocalMap();
+			for(std::map<int, cv::Point3f>::const_iterator iter=map.begin(); iter!=map.end(); ++iter)
 			{
-				cloud.push_back(iter->second);
+				cloud.push_back(pcl::PointXYZ(iter->second.y, iter->second.y, iter->second.z));
 			}
 			sensor_msgs::PointCloud2 cloudMsg;
 			pcl::toROSMsg(cloud, cloudMsg);
@@ -391,13 +392,13 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 				const rtabmap::Signature * s  = ((OdometryBOW*)odometry_)->getMemory()->getLastWorkingSignature();
 				if(s)
 				{
-					const std::multimap<int, pcl::PointXYZ> & words3 = s->getWords3();
+					const std::multimap<int, cv::Point3f> & words3 = s->getWords3();
 					pcl::PointCloud<pcl::PointXYZ> cloud;
-					for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter)
+					for(std::multimap<int, cv::Point3f>::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter)
 					{
 						// transform to odom frame
-						pcl::PointXYZ pt = util3d::transformPoint(iter->second, pose);
+						cv::Point3f pt = util3d::transformPoint(iter->second, pose);
-						cloud.push_back(pt);
+						cloud.push_back(pcl::PointXYZ(pt.x, pt.y, pt.z));
 					}
 
 					sensor_msgs::PointCloud2 cloudMsg;
@@ -409,14 +410,19 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 			}
 			else
 			{
-				//Optical flow
+				//Frame to Frame
-				const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud = ((OdometryOpticalFlow*)odometry_)->getLastCorners3D();
+				const Signature & refFrame = ((OdometryF2F*)odometry_)->getRefFrame();
-				if(cloud->size())
+				if(refFrame.getWords3().size())
 				{
-					pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTransformed;
+					pcl::PointCloud<pcl::PointXYZ> cloud;
-					cloudTransformed = util3d::transformPointCloud(cloud, pose);
+					for(std::multimap<int, cv::Point3f>::const_iterator iter=refFrame.getWords3().begin(); iter!=refFrame.getWords3().end(); ++iter)
+					{
+						// transform to odom frame
+						cv::Point3f pt = util3d::transformPoint(iter->second, pose);
+						cloud.push_back(pcl::PointXYZ(pt.x, pt.y, pt.z));
+					}
 					sensor_msgs::PointCloud2 cloudMsg;
-					pcl::toROSMsg(*cloudTransformed, cloudMsg);
+					pcl::toROSMsg(cloud, cloudMsg);
 					cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
 					cloudMsg.header.frame_id = odomFrameId_;
 					odomLastFrame_.publish(cloudMsg);

src/nodelets/point_cloud_xyz.cpp

@@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pluginlib/class_list_macros.h>
 #include <nodelet/nodelet.h>
 
+#include <rtabmap_ros/MsgConversion.h>
+
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl_conversions/pcl_conversions.h>
@@ -238,18 +240,12 @@ private:
 
 		if(cloudPub_.getNumSubscribers())
 		{
-			image_geometry::PinholeCameraModel model;
-			model.fromCameraInfo(*cameraInfo);
-			float cx = model.cx();
-			float cy = model.cy();
-
 			pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
+			rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo);
+			rtabmap::StereoCameraModel stereoModel(disparityMsg->f, disparityMsg->f, leftModel.cx(), leftModel.cy(), disparityMsg->T);
 			pclCloud = rtabmap::util3d::cloudFromDisparity(
 					disparity,
-					cx,
+					stereoModel,
-					cy,
-					disparityMsg->f,
-					disparityMsg->T,
 					decimation_);
 
 			processAndPublish(pclCloud, disparityMsg->header);

src/nodelets/point_cloud_xyzrgb.cpp

@@ -33,6 +33,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/point_types.h>
 #include <pcl_conversions/pcl_conversions.h>
 
+#include <rtabmap_ros/MsgConversion.h>
+
 #include <sensor_msgs/PointCloud2.h>
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/image_encodings.h>
@@ -228,22 +230,11 @@ private:
 			}
 			ptrRightImage = cv_bridge::toCvShare(imageRight, "mono8");
 
-			image_geometry::StereoCameraModel model;
-			model.fromCameraInfo(*camInfoLeft, *camInfoRight);
-
-			float fx = model.left().fx();
-			float cx = model.left().cx();
-			float cy = model.left().cy();
-			float baseline = model.baseline();
-
 			pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
 			pclCloud = rtabmap::util3d::cloudFromStereoImages(
 					ptrLeftImage->image,
 					ptrRightImage->image,
-					cx,
+					rtabmap_ros::stereoCameraModelFromROS(*camInfoLeft, *camInfoRight),
-					cy,
-					fx,
-					baseline,
 					decimation_);
 
 			processAndPublish(pclCloud, imageLeft->header);