fixed build for latest rtabmap library

CMakeLists.txt

@@ -46,6 +46,7 @@ add_message_files(
    Link.msg
    OdomInfo.msg
    UserData.msg
+   Point2f.msg
 )
 
 ## Generate services in the 'srv' folder

include/rtabmap_ros/MsgConversion.h

@@ -44,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap_ros/Link.h>
 #include <rtabmap_ros/KeyPoint.h>
+#include <rtabmap_ros/Point2f.h>
 #include <rtabmap_ros/MapData.h>
 #include <rtabmap_ros/Graph.h>
 #include <rtabmap_ros/NodeData.h>
@@ -77,6 +78,12 @@ void keypointToROS(const cv::KeyPoint & kpt, rtabmap_ros::KeyPoint & msg);
 std::vector<cv::KeyPoint> keypointsFromROS(const std::vector<rtabmap_ros::KeyPoint> & msg);
 void keypointsToROS(const std::vector<cv::KeyPoint> & kpts, std::vector<rtabmap_ros::KeyPoint> & msg);
 
+cv::Point2f point2fFromROS(const rtabmap_ros::Point2f & msg);
+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);
+
 void mapGraphFromROS(
 		const rtabmap_ros::Graph & msg,
 		std::map<int, rtabmap::Transform> & poses,

msg/KeyPoint.msg

@@ -8,8 +8,7 @@
 #    int class_id;
 #}
 
-float32 ptx
+Point2f pt
-float32 pty
 float32 size
 float32 angle
 float32 response

msg/OdomInfo.msg

@@ -19,8 +19,8 @@ Header header
 #    std::vector<int> wordInliers;
 #
 #    // Optical Flow odometry
-#    std::vector<cv::KeyPoint> refCorners;
+#    std::vector<cv::Point2f> refCorners;
-#    std::vector<cv::KeyPoint> newCorners;
+#    std::vector<cv::Point2f> newCorners;
 #    std::vector<int> cornerInliers;
 #}
 
@@ -39,7 +39,7 @@ KeyPoint[] wordsValues
 int32[] wordMatches
 int32[] wordInliers
 
-KeyPoint[] refCorners
+Point2f[] refCorners
-KeyPoint[] newCorners
+Point2f[] newCorners
 int32[] cornerInliers
 

msg/Point2f.msg

@@ -0,0 +1,8 @@
+#class cv::Point2f
+#{
+#    float x;
+#    float y;
+#}
+
+float32 x
+float32 y

src/CoreWrapper.cpp

@@ -263,6 +263,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	oldParameterNames.push_back("RGBD/ScanMatchingSize");
 	oldParameterNames.push_back("RGBD/LocalLoopDetectionRadius");
 	oldParameterNames.push_back("RGBD/ToroIterations");
+	oldParameterNames.push_back("Mem/RehearsedNodesKept");
 	for(std::list<std::string>::iterator iter=oldParameterNames.begin(); iter!=oldParameterNames.end(); ++iter)
 	{
 		std::string vStr;
@@ -298,6 +299,12 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 						Parameters::kRGBDOptimizeIterations().c_str());
 				parameters_.at(Parameters::kRGBDOptimizeIterations())= vStr;
 			}
+			else if(iter->compare("Mem/RehearsedNodesKept") == 0)
+			{
+				ROS_WARN("Parameter name changed: Mem/RehearsedNodesKept -> %s. Please update your launch file accordingly.",
+						Parameters::kMemNotLinkedNodesKept().c_str());
+				parameters_.at(Parameters::kMemNotLinkedNodesKept())= vStr;
+			}
 		}
 	}
 
@@ -2040,10 +2047,10 @@ std::map<int, rtabmap::Transform> CoreWrapper::updateMapCaches(
 							// Stereo detected, we should uncompress left image too
 							data.uncompressDataConst(&image, 0, 0);
 						}
-						float fx = data.getDepthFx();
+						float fx = data.getFx();
-						float fy = data.getDepthFy();
+						float fy = data.getFy();
-						float cx = data.getDepthCx();
+						float cx = data.getCx();
-						float cy = data.getDepthCy();
+						float cy = data.getCy();
 
 						pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB;
 						pcl::PointCloud<pcl::PointXYZ>::Ptr cloudXYZ;

src/MsgConversion.cpp

@@ -215,7 +215,7 @@ void linkToROS(const rtabmap::Link & link, rtabmap_ros::Link & msg)
 
 cv::KeyPoint keypointFromROS(const rtabmap_ros::KeyPoint & msg)
 {
-	return cv::KeyPoint(msg.ptx, msg.pty, msg.size, msg.angle, msg.response, msg.octave, msg.class_id);
+	return cv::KeyPoint(msg.pt.x, msg.pt.y, msg.size, msg.angle, msg.response, msg.octave, msg.class_id);
 }
 
 void keypointToROS(const cv::KeyPoint & kpt, rtabmap_ros::KeyPoint & msg)
@@ -223,8 +223,8 @@ void keypointToROS(const cv::KeyPoint & kpt, rtabmap_ros::KeyPoint & msg)
 	msg.angle = kpt.angle;
 	msg.class_id = kpt.class_id;
 	msg.octave = kpt.octave;
-	msg.ptx = kpt.pt.x;
+	msg.pt.x = kpt.pt.x;
-	msg.pty = kpt.pt.y;
+	msg.pt.y = kpt.pt.y;
 	msg.response = kpt.response;
 	msg.size = kpt.size;
 }
@@ -248,6 +248,36 @@ void keypointsToROS(const std::vector<cv::KeyPoint> & kpts, std::vector<rtabmap_
 	}
 }
 
+cv::Point2f point2fFromROS(const rtabmap_ros::Point2f & msg)
+{
+	return cv::Point2f(msg.x, msg.y);
+}
+
+void point2fToROS(const cv::Point2f & kpt, rtabmap_ros::Point2f & msg)
+{
+	msg.x = kpt.x;
+	msg.y = kpt.y;
+}
+
+std::vector<cv::Point2f> points2fFromROS(const std::vector<rtabmap_ros::Point2f> & msg)
+{
+	std::vector<cv::Point2f> v(msg.size());
+	for(unsigned int i=0; i<msg.size(); ++i)
+	{
+		v[i] = point2fFromROS(msg[i]);
+	}
+	return v;
+}
+
+void points2fToROS(const std::vector<cv::Point2f> & kpts, std::vector<rtabmap_ros::Point2f> & msg)
+{
+	msg.resize(kpts.size());
+	for(unsigned int i=0; i<msg.size(); ++i)
+	{
+		point2fToROS(kpts[i], msg[i]);
+	}
+}
+
 void mapGraphFromROS(
 		const rtabmap_ros::Graph & msg,
 		std::map<int, rtabmap::Transform> & poses,
@@ -394,10 +424,10 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 	compressedMatToBytes(signature.getImageCompressed(), msg.image);
 	compressedMatToBytes(signature.getDepthCompressed(), msg.depth);
 	compressedMatToBytes(signature.getLaserScanCompressed(), msg.laserScan);
-	msg.fx = signature.getDepthFx();
+	msg.fx = signature.getFx();
-	msg.fy = signature.getDepthFy();
+	msg.fy = signature.getFy();
-	msg.cx = signature.getDepthCx();
+	msg.cx = signature.getCx();
-	msg.cy = signature.getDepthCy();
+	msg.cy = signature.getCy();
 	transformToGeometryMsg(signature.getLocalTransform(), msg.localTransform);
 
 	//Features stuff...
@@ -454,8 +484,8 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
 	info.wordMatches = msg.wordMatches;
 	info.wordInliers = msg.wordInliers;
 
-	info.refCorners = keypointsFromROS(msg.refCorners);
+	info.refCorners = points2fFromROS(msg.refCorners);
-	info.newCorners = keypointsFromROS(msg.newCorners);
+	info.newCorners = points2fFromROS(msg.newCorners);
 	info.cornerInliers = msg.cornerInliers;
 
 	return info;
@@ -479,8 +509,8 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
 	msg.wordMatches = info.wordMatches;
 	msg.wordInliers = info.wordInliers;
 
-	keypointsToROS(info.refCorners, msg.refCorners);
+	points2fToROS(info.refCorners, msg.refCorners);
-	keypointsToROS(info.newCorners, msg.newCorners);
+	points2fToROS(info.newCorners, msg.newCorners);
 	msg.cornerInliers = info.cornerInliers;
 
 }