Updated against latest changes from 0.11. Added localMap features to OdomInfo msg. Updated rgbdslam_datasets.launch.

CMakeLists.txt

@@ -52,6 +52,7 @@ add_message_files(
    Link.msg
    OdomInfo.msg
    Point2f.msg
+   Point3f.msg
    Goal.msg
 )
 

include/rtabmap_ros/MsgConversion.h

@@ -46,6 +46,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/Point3f.h>
 #include <rtabmap_ros/MapData.h>
 #include <rtabmap_ros/MapGraph.h>
 #include <rtabmap_ros/NodeData.h>
@@ -85,6 +86,12 @@ 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);
 
+cv::Point3f point3fFromROS(const rtabmap_ros::Point3f & msg);
+void point3fToROS(const cv::Point3f & kpt, rtabmap_ros::Point3f & msg);
+
+std::vector<cv::Point3f> points3fFromROS(const std::vector<rtabmap_ros::Point3f> & msg);
+void points3fToROS(const std::vector<cv::Point3f> & kpts, std::vector<rtabmap_ros::Point3f> & msg);
+
 rtabmap::CameraModel cameraModelFromROS(
 		const sensor_msgs::CameraInfo & camInfo,
 		const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity());

launch/tests/rgbdslam_datasets.launch

@@ -33,10 +33,10 @@
       <param name="Vis/CorType" type="string" value="0"/>        <!-- 0=features matching 1=Optical Flow -->
       <param name="Vis/EstimationType" type="string" value="0"/> <!-- 0=3D->3D, 1=3D->2D (PnP) -->
       <param name="Odom/FillInfoData" type="string" value="$(arg rtabmapviz)"/>
-      <param name="Vis/UseDepthAsMask" type="string" value="true"/>
       <param name="Vis/MaxDepth" type="string" value="4"/>
-      <param name="Vis/PnPRefineIterations" type="string" value="0"/>      
+      <param name="Vis/CorNNDR" type="string" value="0.6"/>
       <param name="Odom/ResetCountdown" type="string" value="15"/> 
+      <param name="Odom/KeyFrameThr" type="string" value="0.5"/>
 
       <param name="odom_frame_id" type="string" value="vis_odom"/>
       <param name="frame_id" type="string" value="kinect"/>
@@ -57,13 +57,11 @@
       <param name="subscribe_laserScan" type="bool" value="false"/>
       
       <param name="Rtabmap/StartNewMapOnLoopClosure" type="string" value="true"/>
-      <param name="Vis/EstimationType" type="string" value="1"/>
+      <param name="Vis/EstimationType" type="string" value="0"/>
       <param name="Vis/MaxDepth" type="string" value="4"/>
-      <param name="Vis/PnPRefineIterations" type="string" value="0"/>
       <param name="RGBD/LoopClosureReextractFeatures" type="string" value="false"/>
       <param name="Mem/RawDescriptorsKept" type="string" value="true"/>
-      <param name="Kp/DetectorStrategy" type="string" value="6"/>
+      <param name="Kp/DetectorStrategy" type="string" value="0"/>
-      <param name="Mem/UseDepthAsMask" type="string" value="true"/>
 
       <param name="frame_id" type="string" value="kinect"/>
       <param name="ground_truth_frame_id" type="string" value="world"/> 

msg/OdomInfo.msg

@@ -42,6 +42,8 @@ int32[] wordsKeys
 KeyPoint[] wordsValues
 int32[] wordMatches
 int32[] wordInliers
+int32[] localMapKeys
+Point3f[] localMapValues
 
 Point2f[] refCorners
 Point2f[] newCorners

msg/Point3f.msg

@@ -0,0 +1,10 @@
+#class cv::Point3f
+#{
+#    float x;
+#    float y;
+#    float z;
+#}
+
+float32 x
+float32 y
+float32 z

src/CoreWrapper.cpp

@@ -925,7 +925,7 @@ void CoreWrapper::commonDepthCallback(
 				if(sensorT.isNull())
 				{
 					ROS_WARN("Could not get odometry value for laser scan stamp (%fs). Latest odometry "
-							"stamp is %fs. The laser scan pose will not be synchronized with odometry.", scanMsg->header.stamp.toSec(), lastPoseStamp_.toSec());
+							"stamp is %fs. The laser scan pose will not be synchronized with odometry.", scan2dMsg->header.stamp.toSec(), lastPoseStamp_.toSec());
 				}
 				else
 				{

src/MsgConversion.cpp

@@ -295,6 +295,37 @@ void points2fToROS(const std::vector<cv::Point2f> & kpts, std::vector<rtabmap_ro
 	}
 }
 
+cv::Point3f point3fFromROS(const rtabmap_ros::Point3f & msg)
+{
+	return cv::Point3f(msg.x, msg.y, msg.z);
+}
+
+void point3fToROS(const cv::Point3f & kpt, rtabmap_ros::Point3f & msg)
+{
+	msg.x = kpt.x;
+	msg.y = kpt.y;
+	msg.z = kpt.z;
+}
+
+std::vector<cv::Point3f> points3fFromROS(const std::vector<rtabmap_ros::Point3f> & msg)
+{
+	std::vector<cv::Point3f> v(msg.size());
+	for(unsigned int i=0; i<msg.size(); ++i)
+	{
+		v[i] = point3fFromROS(msg[i]);
+	}
+	return v;
+}
+
+void points3fToROS(const std::vector<cv::Point3f> & kpts, std::vector<rtabmap_ros::Point3f> & msg)
+{
+	msg.resize(kpts.size());
+	for(unsigned int i=0; i<msg.size(); ++i)
+	{
+		point3fToROS(kpts[i], msg[i]);
+	}
+}
+
 rtabmap::CameraModel cameraModelFromROS(
 		const sensor_msgs::CameraInfo & camInfo,
 		const rtabmap::Transform & localTransform)
@@ -306,7 +337,9 @@ rtabmap::CameraModel cameraModelFromROS(
 			model.fy(),
 			model.cx(),
 			model.cy(),
-			localTransform);
+			localTransform,
+			0.0,
+			cv::Size(model.fullResolution().width, model.fullResolution().height));
 }
 rtabmap::StereoCameraModel stereoCameraModelFromROS(
 		const sensor_msgs::CameraInfo & leftCamInfo,
@@ -321,7 +354,8 @@ rtabmap::StereoCameraModel stereoCameraModelFromROS(
 			model.left().cx(),
 			model.left().cy(),
 			model.baseline(),
-			localTransform);
+			localTransform,
+			cv::Size(model.left().fullResolution().width, model.left().fullResolution().height));
 }
 
 void mapDataFromROS(
@@ -647,6 +681,12 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
 	info.transform = transformFromGeometryMsg(msg.transform);
 	info.transformFiltered = transformFromGeometryMsg(msg.transformFiltered);
 
+	UASSERT(msg.localMapKeys.size() == msg.localMapValues.size());
+	for(unsigned int i=0; i<msg.localMapKeys.size(); ++i)
+	{
+		info.localMap.insert(std::make_pair(msg.localMapKeys[i], point3fFromROS(msg.localMapValues[i])));
+	}
+
 	return info;
 }
 
@@ -664,7 +704,6 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
 	msg.interval = info.interval;
 	msg.distanceTravelled = info.distanceTravelled;
 
-
 	msg.type = info.type;
 
 	msg.wordsKeys = uKeys(info.words);
@@ -680,6 +719,9 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
 	transformToGeometryMsg(info.transform, msg.transform);
 	transformToGeometryMsg(info.transformFiltered, msg.transformFiltered);
 
+	msg.localMapKeys = uKeys(info.localMap);
+	points3fToROS(uValues(info.localMap), msg.localMapValues);
+
 }
 
 }

src/OdometryROS.cpp

@@ -369,15 +369,14 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 			//set velocity
 			if(previousStamp_.isValid())
 			{
-				float dt = 1.0f/(stamp - previousStamp_).toSec();
 				float x,y,z,roll,pitch,yaw;
-				odometry_->previousTransform().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+				odometry_->previousVelocityTransform().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
-				odom.twist.twist.linear.x = x*dt;
+				odom.twist.twist.linear.x = x;
-				odom.twist.twist.linear.y = y*dt;
+				odom.twist.twist.linear.y = y;
-				odom.twist.twist.linear.z = z*dt;
+				odom.twist.twist.linear.z = z;
-				odom.twist.twist.angular.x = roll*dt;
+				odom.twist.twist.angular.x = roll;
-				odom.twist.twist.angular.y = pitch*dt;
+				odom.twist.twist.angular.y = pitch;
-				odom.twist.twist.angular.z = yaw*dt;
+				odom.twist.twist.angular.z = yaw;
 			}
 			previousStamp_ = stamp;