From d5144f292f2baa7478599baf961662c9df54ce0b Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Wed, 8 Feb 2017 15:16:29 -0500
Subject: [PATCH] Added "odom_tf_angular_variance" and
 "odom_tf_linear_variance" parameters for rtabmap node when TF is used for
 odometry

---
 include/rtabmap_ros/CoreWrapper.h |  2 ++
 launch/rtabmap.launch             | 13 +++++++++----
 src/CoreWrapper.cpp               | 22 ++++++++++++++++++----
 3 files changed, 29 insertions(+), 8 deletions(-)

diff --git a/include/rtabmap_ros/CoreWrapper.h b/include/rtabmap_ros/CoreWrapper.h
index 6855aaf7..b1f0be5d 100644
--- a/include/rtabmap_ros/CoreWrapper.h
+++ b/include/rtabmap_ros/CoreWrapper.h
@@ -186,6 +186,8 @@ private:
 	std::string groundTruthBaseFrameId_;
 	std::string configPath_;
 	std::string databasePath_;
+	float odomDefaultAngVariance_;
+	float odomDefaultLinVariance_;
 	bool waitForTransform_;
 	double waitForTransformDuration_;
 	bool useActionForGoal_;
diff --git a/launch/rtabmap.launch b/launch/rtabmap.launch
index b421cd23..0e231dba 100644
--- a/launch/rtabmap.launch
+++ b/launch/rtabmap.launch
@@ -63,10 +63,13 @@
   <arg name="subscribe_scan_cloud"    default="false"/>
   <arg name="scan_cloud_topic"        default="/scan_cloud"/>
    
-  <arg name="visual_odometry"         default="true"/>          <!-- Launch rtabmap visual odometry node -->
-  <arg name="odom_topic"              default="/odom"/>         <!-- Odometry topic used if visual_odometry is false -->
-  <arg name="odom_frame_id"           default=""/>              <!-- If set, TF is used to get odometry instead of the topic -->
-  <arg name="odom_args"               default="$(arg rtabmap_args)"/>
+  <arg name="visual_odometry"          default="true"/>          <!-- Launch rtabmap visual odometry node -->
+  <arg name="odom_topic"               default="/odom"/>         <!-- Odometry topic used if visual_odometry is false -->
+  <arg name="odom_frame_id"            default=""/>              <!-- If set, TF is used to get odometry instead of the topic -->
+  <arg name="odom_tf_angular_variance" default="1"/>             <!-- If TF is used to get odometry, this is the default angular variance -->
+  <arg name="odom_tf_linear_variance"  default="1"/>             <!-- If TF is used to get odometry, this is the default linear variance -->
+  <arg name="odom_args"                default="$(arg rtabmap_args)"/>
+  
   
   <!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
   <arg if="$(arg compressed)"     name="rgb_topic_relay"           default="$(arg rgb_topic)_relay"/>
@@ -128,6 +131,8 @@
       <param name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
       <param name="frame_id"             type="string" value="$(arg frame_id)"/>
       <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>
+      <param name="odom_tf_angular_variance" type="double" value="$(arg odom_tf_angular_variance)"/>
+      <param name="odom_tf_linear_variance"  type="double" value="$(arg odom_tf_linear_variance)"/>
       <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
       <param name="database_path"        type="string" value="$(arg database_path)"/>
       <param name="approx_sync"          type="bool"   value="$(arg approx_sync)"/>
diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp
index 98583494..4d614e66 100644
--- a/src/CoreWrapper.cpp
+++ b/src/CoreWrapper.cpp
@@ -93,6 +93,8 @@ CoreWrapper::CoreWrapper() :
 		groundTruthBaseFrameId_(""), // e.g., "base_link_gt"
 		configPath_(""),
 		databasePath_(UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName()),
+		odomDefaultAngVariance_(1.0),
+		odomDefaultLinVariance_(1.0),
 		waitForTransform_(true),
 		waitForTransformDuration_(0.2), // 200 ms
 		useActionForGoal_(false),
@@ -145,6 +147,8 @@ void CoreWrapper::onInit()
 	pnh.param("tf_delay",            tfDelay, tfDelay);
 	pnh.param("tf_prefix",           tfPrefix, tfPrefix);
 	pnh.param("tf_tolerance",        tfTolerance, tfTolerance);
+	pnh.param("odom_tf_angular_variance", odomDefaultAngVariance_, odomDefaultAngVariance_);
+	pnh.param("odom_tf_linear_variance", odomDefaultLinVariance_, odomDefaultLinVariance_);
 	pnh.param("wait_for_transform",  waitForTransform_, waitForTransform_);
 	pnh.param("wait_for_transform_duration",  waitForTransformDuration_, waitForTransformDuration_);
 	pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_);
@@ -951,8 +955,8 @@ void CoreWrapper::commonDepthCallbackImpl(
 			data,
 			lastPose_,
 			odomFrameId,
-			uIsFinite(rotVariance_) && rotVariance_>0?rotVariance_:1.0,
-			uIsFinite(transVariance_) && transVariance_>0?transVariance_:1.0);
+			rotVariance_,
+			transVariance_);
 	rotVariance_ = 0;
 	transVariance_ = 0;
 }
@@ -1137,8 +1141,8 @@ void CoreWrapper::commonStereoCallback(
 			data,
 			lastPose_,
 			odomFrameId,
-			uIsFinite(rotVariance_) && rotVariance_>0?rotVariance_:1.0,
-			uIsFinite(transVariance_) && transVariance_>0?transVariance_:1.0);
+			rotVariance_,
+			transVariance_);
 
 	rotVariance_ = 0;
 	transVariance_ = 0;
@@ -1158,6 +1162,16 @@ void CoreWrapper::process(
 		double timeRtabmap = 0.0;
 		double timeUpdateMaps = 0.0;
 		double timePublishMaps = 0.0;
+
+		if(!uIsFinite(odomRotationalVariance) || odomRotationalVariance<=0.0f)
+		{
+			odomRotationalVariance = odomDefaultAngVariance_;
+		}
+		if(!uIsFinite(odomTransitionalVariance) || odomTransitionalVariance<=0.0f)
+		{
+			odomTransitionalVariance = odomDefaultLinVariance_;
+		}
+
 		if(rtabmap_.process(data, odom, OdometryEvent::generateCovarianceMatrix(odomRotationalVariance, odomTransitionalVariance)))
 		{
 			timeRtabmap = timer.ticks();