From 3f6811b156a1db80d7273a6f6ba06dac95b9303b Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Tue, 19 Apr 2016 18:53:03 -0400
Subject: [PATCH] Odom: Added twist covariance (pose covariance/2)

---
 src/CoreWrapper.cpp |  3 ++-
 src/OdometryROS.cpp | 27 +++++++++++++++++++++++----
 src/OdometryROS.h   |  1 -
 3 files changed, 25 insertions(+), 6 deletions(-)

diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp
index b9d6303e..bc095262 100644
--- a/src/CoreWrapper.cpp
+++ b/src/CoreWrapper.cpp
@@ -59,6 +59,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <octomap_msgs/conversions.h>
 #endif
 
+#define BAD_COVARIANCE 9999
 
 //msgs
 #include "rtabmap_ros/Info.h"
@@ -609,7 +610,7 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
 	if(!paused_)
 	{
 		Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose);
-		if(!lastPose_.isIdentity() && (odom.isIdentity() || odomMsg->pose.covariance[0] >= 9999))
+		if(!lastPose_.isIdentity() && (odom.isIdentity() || odomMsg->pose.covariance[0] >= BAD_COVARIANCE))
 		{
 			UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", odomMsg->pose.covariance[0]);
 			rtabmap_.triggerNewMap();
diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp
index 0f7afa0e..02b36fce 100644
--- a/src/OdometryROS.cpp
+++ b/src/OdometryROS.cpp
@@ -49,6 +49,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UStl.h"
 #include "rtabmap/utilite/UFile.h"
 
+#define BAD_COVARIANCE 9999
+
 using namespace rtabmap;
 
 namespace rtabmap_ros {
@@ -385,7 +387,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 			odom.pose.covariance.at(35) = info.variance; // yawyaw
 
 			//set velocity
-			if(previousStamp_.isValid())
+			bool setTwist = !odometry_->previousVelocityTransform().isNull();
+			if(setTwist)
 			{
 				float x,y,z,roll,pitch,yaw;
 				odometry_->previousVelocityTransform().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
@@ -396,7 +399,13 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 				odom.twist.twist.angular.y = pitch;
 				odom.twist.twist.angular.z = yaw;
 			}
-			previousStamp_ = stamp;
+			// libviso2 uses approximately pose variance/2
+			odom.twist.covariance.at(0) = setTwist?odom.pose.covariance.at(0)/2.0:BAD_COVARIANCE;  // xx
+			odom.twist.covariance.at(7) = setTwist?odom.pose.covariance.at(7)/2.0:BAD_COVARIANCE;  // yy
+			odom.twist.covariance.at(14) = setTwist?odom.pose.covariance.at(14)/2.0:BAD_COVARIANCE; // zz
+			odom.twist.covariance.at(21) = setTwist?odom.pose.covariance.at(21)/2.0:BAD_COVARIANCE; // rr
+			odom.twist.covariance.at(28) = setTwist?odom.pose.covariance.at(28)/2.0:BAD_COVARIANCE; // pp
+			odom.twist.covariance.at(35) = setTwist?odom.pose.covariance.at(35)/2.0:BAD_COVARIANCE; // yawyaw
 
 			//publish the message
 			odomPub_.publish(odom);
@@ -471,6 +480,18 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 		odom.header.stamp = stamp; // use corresponding time stamp to image
 		odom.header.frame_id = odomFrameId_;
 		odom.child_frame_id = frameId_;
+		odom.pose.covariance.at(0) = BAD_COVARIANCE;  // xx
+		odom.pose.covariance.at(7) = BAD_COVARIANCE;  // yy
+		odom.pose.covariance.at(14) = BAD_COVARIANCE; // zz
+		odom.pose.covariance.at(21) = BAD_COVARIANCE; // rr
+		odom.pose.covariance.at(28) = BAD_COVARIANCE; // pp
+		odom.pose.covariance.at(35) = BAD_COVARIANCE; // yawyaw
+		odom.twist.covariance.at(0) = BAD_COVARIANCE;  // xx
+		odom.twist.covariance.at(7) = BAD_COVARIANCE;  // yy
+		odom.twist.covariance.at(14) = BAD_COVARIANCE; // zz
+		odom.twist.covariance.at(21) = BAD_COVARIANCE; // rr
+		odom.twist.covariance.at(28) = BAD_COVARIANCE; // pp
+		odom.twist.covariance.at(35) = BAD_COVARIANCE; // yawyaw
 
 		//publish the message
 		odomPub_.publish(odom);
@@ -521,7 +542,6 @@ bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
 {
 	ROS_INFO("visual_odometry: reset odom!");
 	odometry_->reset();
-	previousStamp_ = ros::Time();
 	return true;
 }
 
@@ -530,7 +550,6 @@ bool OdometryROS::resetToPose(rtabmap_ros::ResetPose::Request& req, rtabmap_ros:
 	Transform pose(req.x, req.y, req.z, req.roll, req.pitch, req.yaw);
 	ROS_INFO("visual_odometry: reset odom to pose %s!", pose.prettyPrint().c_str());
 	odometry_->reset(pose);
-	previousStamp_ = ros::Time();
 	return true;
 }
 
diff --git a/src/OdometryROS.h b/src/OdometryROS.h
index bbeb2dcb..ec705d41 100644
--- a/src/OdometryROS.h
+++ b/src/OdometryROS.h
@@ -104,7 +104,6 @@ private:
 	bool paused_;
 	int resetCountdown_;
 	int resetCurrentCount_;
-	ros::Time previousStamp_;
 };
 
 }