updated package to rtabmap 0.8.4. Fixed not saved database on exit (bug introduced with the last commit).

CMakeLists.txt

@@ -13,7 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-find_package(RTABMap 0.8 REQUIRED)
+find_package(RTABMap 0.8.4 REQUIRED)
 find_package(octomap REQUIRED)
 
 #Qt stuff

msg/Link.msg

@@ -11,4 +11,5 @@ int32 fromId
 int32 toId
 int32 type
 geometry_msgs/Transform transform
-int32 variance
+float32 rotVariance
+float32 transVariance

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap_ros</name>
-  <version>0.8.3</version>
+  <version>0.8.4</version>
   <description>RTAB-Map's ros-pkg. RTAB-Map is an RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

src/CoreWrapper.cpp

@@ -70,10 +70,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 using namespace rtabmap;
 
+float max3( const float& a, const float& b, const float& c)
+{
+	float m=a>b?a:b;
+	return m>c?m:c;
+}
+
 CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 		paused_(false),
 		lastPose_(Transform::getIdentity()),
-		variance_(0),
+		rotVariance_(0),
+		transVariance_(0),
 		frameId_("base_link"),
 		mapFrameId_("map"),
 		odomFrameId_(""),
@@ -99,6 +106,11 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 		stereoScanSync_(0),
 		stereoApproxSync_(0),
 		stereoExactSync_(0),
+		depthTFSync_(0),
+		depthScanTFSync_(0),
+		stereoScanTFSync_(0),
+		stereoApproxTFSync_(0),
+		stereoExactTFSync_(0),
 		transformThread_(0),
 		rate_(Parameters::defaultRtabmapDetectionRate()),
 		time_(ros::Time::now()),
@@ -537,13 +549,20 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
 		{
 			UWARN("Odometry is reset (identity pose detected). Increment map id!");
 			rtabmap_.triggerNewMap();
-			variance_ = 0;
+			rotVariance_ = 0;
+			transVariance_ = 0;
 		}
 
 		lastPose_ = odom;
-		if(odomMsg->pose.covariance[0] > variance_)
+		float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
+		float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
+		if(rotVariance > rotVariance_)
 		{
-			variance_ = odomMsg->pose.covariance[0];
+			rotVariance_ = rotVariance;
+		}
+		if(transVariance > transVariance_)
+		{
+			transVariance_ = transVariance;
 		}
 
 		// Throttle
@@ -591,7 +610,8 @@ bool CoreWrapper::commonOdomTFUpdate(const ros::Time & stamp)
 		{
 			UWARN("Odometry is reset (identity pose detected). Increment map id!");
 			rtabmap_.triggerNewMap();
-			variance_ = 0;
+			rotVariance_ = 0;
+			transVariance_ = 0;
 		}
 
 		lastPose_ = odom;
@@ -700,7 +720,8 @@ void CoreWrapper::commonDepthCallback(
 			ptrImage->image,
 			lastPose_,
 			odomFrameId,
-			variance_>0?variance_:1.0f,
+			rotVariance_>0?rotVariance_:1.0f,
+			transVariance_>0?transVariance_:1.0f,
 			ptrDepth->image,
 			fx,
 			fy,
@@ -708,7 +729,8 @@ void CoreWrapper::commonDepthCallback(
 			cy,
 			localTransform,
 			scan);
-	variance_ = 0;
+	rotVariance_ = 0;
+	transVariance_ = 0;
 }
 
 void CoreWrapper::commonStereoCallback(
@@ -779,7 +801,8 @@ void CoreWrapper::commonStereoCallback(
 			ptrLeftImage->image,
 			lastPose_,
 			odomFrameId,
-			variance_>0?variance_:1.0f,
+			rotVariance_>0?rotVariance_:1.0f,
+			transVariance_>0?transVariance_:1.0f,
 			ptrRightImage->image,
 			fx,
 			baseline,
@@ -787,7 +810,8 @@ void CoreWrapper::commonStereoCallback(
 			cy,
 			localTransform,
 			scan);
-	variance_ = 0;
+	rotVariance_ = 0;
+	transVariance_ = 0;
 }
 
 void CoreWrapper::depthCallback(
@@ -905,7 +929,8 @@ void CoreWrapper::process(
 		const cv::Mat & image,
 		const Transform & odom,
 		const std::string & odomFrameId,
-		float odomVariance,
+		float odomRotationalVariance,
+		float odomTransitionalVariance,
 		const cv::Mat & depthOrRightImage,
 		float fx,
 		float fyOrBaseline,
@@ -963,7 +988,8 @@ void CoreWrapper::process(
 				cy,
 				localTransform,
 				odom,
-				odomVariance,
+				odomRotationalVariance,
+				odomTransitionalVariance,
 				id);
 
 		if(rtabmap_.process(data))
@@ -1227,7 +1253,8 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
 {
 	ROS_INFO("rtabmap: Reset");
 	rtabmap_.resetMemory();
-	variance_ = 0;
+	rotVariance_ = 0;
+	transVariance_ = 0;
 	lastPose_.setIdentity();
 	currentMetricGoal_.setNull();
 	return true;

src/CoreWrapper.h

@@ -166,7 +166,8 @@ private:
 			const cv::Mat & image,
 			const rtabmap::Transform & odom = rtabmap::Transform(),
 			const std::string & odomFrameId = "",
-			float odomVariance = 1.0f,
+			float odomRotationalVariance = 1.0f,
+			float odomTransitionalVariance = 1.0f,
 			const cv::Mat & depthOrRightImage = cv::Mat(),
 			float fx = 0.0f,
 			float fyOrBaseline = 0.0f,
@@ -215,7 +216,8 @@ private:
 	rtabmap::Rtabmap rtabmap_;
 	bool paused_;
 	rtabmap::Transform lastPose_;
-	float variance_;
+	float rotVariance_;
+	float transVariance_;
 	rtabmap::Transform currentMetricGoal_;
 
 	std::string frameId_;

src/DataRecorderNode.cpp

@@ -63,6 +63,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 using namespace rtabmap;
 
+float max3( const float& a, const float& b, const float& c)
+{
+	float m=a>b?a:b;
+	return m>c?m:c;
+}
 
 class DataRecorderWrapper
 {
@@ -283,7 +288,9 @@ private:
 			cy,
 			localTransform,
 			Transform(),
-			1.0f);
+			1.0f,
+			1.0f,
+			0);
 		recorder_.addData(data);
 	}
 
@@ -352,6 +359,9 @@ private:
 			depth16 = ptrDepth->image.clone();
 		}
 
+		float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
+		float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
+
 		rtabmap::SensorData data(
 			ptrImage->image.clone(),
 			depth16,
@@ -361,7 +371,9 @@ private:
 			cy,
 			localTransform,
 			rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
-			odomMsg->pose.covariance[0]>0?odomMsg->pose.covariance[0]:1.0f);
+			rotVariance>0?rotVariance:1.0f,
+			transVariance>0?transVariance:1.0f,
+			0);
 		recorder_.addData(data);
 	}
 
@@ -445,6 +457,9 @@ private:
 			depth16 = ptrDepth->image.clone();
 		}
 
+		float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
+		float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
+
 		rtabmap::SensorData data(
 			scan,
 			ptrImage->image.clone(),
@@ -455,7 +470,9 @@ private:
 			cy,
 			localTransform,
 			rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
-			odomMsg->pose.covariance[0]>0?odomMsg->pose.covariance[0]:1.0f);
+			rotVariance>0?rotVariance:1.0f,
+			transVariance>0?transVariance:1.0f,
+			0);
 		recorder_.addData(data);
 	}
 
@@ -499,6 +516,8 @@ private:
 		float cx = model.right().cx();
 		float cy = model.right().cy();
 
+		float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
+		float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
 		rtabmap::SensorData data(
 				ptrLeftImage->image.clone(),
 				ptrRightImage->image.clone(),
@@ -508,7 +527,9 @@ private:
 				cy,
 				localTransform,
 				rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
-				odomMsg->pose.covariance[0]>0?odomMsg->pose.covariance[0]:1.0f);
+				rotVariance>0?rotVariance:1.0f,
+				transVariance>0?transVariance:1.0f,
+				0);
 		recorder_.addData(data);
 	}
 
@@ -560,7 +581,9 @@ private:
 				cy,
 				localTransform,
 				Transform(),
-				1.0f);
+				1.0f,
+				1.0f,
+				0);
 		recorder_.addData(data);
 	}
 

src/DbPlayerNode.cpp

@@ -252,12 +252,12 @@ int main(int argc, char** argv)
 				odom.header.frame_id = odomFrameId;
 				odom.header.stamp = time;
 				rtabmap_ros::transformToPoseMsg(data.pose(), odom.pose.pose);
-				odom.pose.covariance[0] = data.poseVariance();
+				odom.pose.covariance[0] = data.poseTransVariance();
-				odom.pose.covariance[7] = data.poseVariance();
+				odom.pose.covariance[7] = data.poseTransVariance();
-				odom.pose.covariance[14] = data.poseVariance();
+				odom.pose.covariance[14] = data.poseTransVariance();
-				odom.pose.covariance[21] = data.poseVariance();
+				odom.pose.covariance[21] = data.poseRotVariance();
-				odom.pose.covariance[28] = data.poseVariance();
+				odom.pose.covariance[28] = data.poseRotVariance();
-				odom.pose.covariance[35] = data.poseVariance();
+				odom.pose.covariance[35] = data.poseRotVariance();
 				odometryPub.publish(odom);
 			}
 		}

src/GuiWrapper.cpp

@@ -58,6 +58,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl_conversions/pcl_conversions.h>
 #include <laser_geometry/laser_geometry.h>
 
+float max3( const float& a, const float& b, const float& c)
+{
+	float m=a>b?a:b;
+	return m>c?m:c;
+}
+
 GuiWrapper::GuiWrapper(int & argc, char** argv) :
 		app_(0),
 		mainWindow_(0),
@@ -364,7 +370,9 @@ void GuiWrapper::defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg)
 {
 	Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose);
 	rtabmap::SensorData data(cv::Mat(), odomMsg->header.seq);
-	data.setPose(odom, odomMsg->pose.covariance[0]);
+	float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
+	float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
+	data.setPose(odom, rotVariance, transVariance);
 	this->post(new OdometryEvent(data));
 }
 
@@ -407,6 +415,9 @@ void GuiWrapper::depthCallback(
 	float cx = model.cx();
 	float cy = model.cy();
 
+	float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
+	float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
+
 	rtabmap::SensorData image(
 			ptrImage->image.clone(),
 			ptrDepth->image.clone(),
@@ -416,7 +427,8 @@ void GuiWrapper::depthCallback(
 			cy,
 			localTransform,
 			rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
-			odomMsg->pose.covariance[0],
+			rotVariance,
+			transVariance,
 			odomMsg->header.seq);
 	this->post(new OdometryEvent(image));
 }
@@ -461,6 +473,9 @@ void GuiWrapper::depthOdomInfoCallback(
 	float cx = model.cx();
 	float cy = model.cy();
 
+	float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
+	float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
+
 	rtabmap::SensorData image(
 			ptrImage->image.clone(),
 			ptrDepth->image.clone(),
@@ -470,7 +485,8 @@ void GuiWrapper::depthOdomInfoCallback(
 			cy,
 			localTransform,
 			rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
-			odomMsg->pose.covariance[0],
+			rotVariance,
+			transVariance,
 			odomMsg->header.seq);
 	OdometryInfo info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg);
 	this->post(new OdometryEvent(image, info));
@@ -525,6 +541,9 @@ void GuiWrapper::depthScanCallback(
 	float cx = model.cx();
 	float cy = model.cy();
 
+	float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
+	float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
+
 	rtabmap::SensorData image(
 			scan,
 			ptrImage->image.clone(),
@@ -535,7 +554,8 @@ void GuiWrapper::depthScanCallback(
 			cy,
 			localTransform,
 			rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
-			odomMsg->pose.covariance[0],
+			rotVariance,
+			transVariance,
 			odomMsg->header.seq);
 	this->post(new OdometryEvent(image));
 }
@@ -613,6 +633,9 @@ void GuiWrapper::stereoScanCallback(
 	float cy = model.left().cy();
 	float baseline = model.baseline();
 
+	float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
+	float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
+
 	rtabmap::SensorData image(
 			scan,
 			ptrLeftImage->image.clone(),
@@ -623,7 +646,8 @@ void GuiWrapper::stereoScanCallback(
 			cy,
 			localTransform,
 			rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
-			odomMsg->pose.covariance[0],
+			rotVariance,
+			transVariance,
 			odomMsg->header.seq);
 	this->post(new OdometryEvent(image));
 }
@@ -692,6 +716,9 @@ void GuiWrapper::stereoOdomInfoCallback(
 	float cy = model.left().cy();
 	float baseline = model.baseline();
 
+	float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
+	float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
+
 	rtabmap::SensorData image(
 			ptrLeftImage->image.clone(),
 			ptrRightImage->image.clone(),
@@ -701,7 +728,8 @@ void GuiWrapper::stereoOdomInfoCallback(
 			cy,
 			localTransform,
 			rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
-			odomMsg->pose.covariance[0],
+			rotVariance,
+			transVariance,
 			odomMsg->header.seq);
 	OdometryInfo info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg);
 	this->post(new OdometryEvent(image, info));
@@ -770,6 +798,9 @@ void GuiWrapper::stereoCallback(
 	float cy = model.left().cy();
 	float baseline = model.baseline();
 
+	float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
+	float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
+
 	rtabmap::SensorData image(
 			ptrLeftImage->image.clone(),
 			ptrRightImage->image.clone(),
@@ -779,7 +810,8 @@ void GuiWrapper::stereoCallback(
 			cy,
 			localTransform,
 			rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
-			odomMsg->pose.covariance[0],
+			rotVariance,
+			transVariance,
 			odomMsg->header.seq);
 	this->post(new OdometryEvent(image));
 }

src/MsgConversion.cpp

@@ -200,7 +200,7 @@ void infoToROS(const rtabmap::Statistics & stats, rtabmap_ros::Info & info)
 
 rtabmap::Link linkFromROS(const rtabmap_ros::Link & msg)
 {
-	return rtabmap::Link(msg.fromId, msg.toId, (rtabmap::Link::Type)msg.type, transformFromGeometryMsg(msg.transform), msg.variance);
+	return rtabmap::Link(msg.fromId, msg.toId, (rtabmap::Link::Type)msg.type, transformFromGeometryMsg(msg.transform), msg.rotVariance, msg.transVariance);
 }
 
 void linkToROS(const rtabmap::Link & link, rtabmap_ros::Link & msg)
@@ -208,7 +208,8 @@ void linkToROS(const rtabmap::Link & link, rtabmap_ros::Link & msg)
 	msg.fromId = link.from();
 	msg.toId = link.to();
 	msg.type = link.type();
-	msg.variance = link.variance();
+	msg.rotVariance = link.rotVariance();
+	msg.transVariance = link.transVariance();
 	transformToGeometryMsg(link.transform(), msg.transform);
 }
 

src/RGBDOdometryNode.cpp

@@ -147,7 +147,9 @@ public:
 						cy,
 						rtabmap_ros::transformFromTF(localTransform),
 						rtabmap::Transform(),
-						1.0f);
+						1.0f,
+						1.0f,
+						0);
 
 				this->processData(data, image->header);
 			}

src/StereoOdometryNode.cpp

@@ -171,7 +171,9 @@ public:
 						cy,
 						rtabmap_ros::transformFromTF(localTransform),
 						rtabmap::Transform(),
-						1.0f);
+						1.0f,
+						1.0f,
+						0);
 
 				this->processData(data, imageRectLeft->header);
 			}