rtabmap: use largest odom covariance instead of summation

src/CoreWrapper.cpp

@@ -763,16 +763,15 @@ bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
 				covariance = cv::Mat(6,6,CV_64FC1, (void*)odomMsg->twist.covariance.data()).clone();
 			}
 
-			if(uIsFinite(covariance.at<double>(0,0)) && covariance.at<double>(0,0) != 1.0 && covariance.at<double>(0,0)>0.0)
+			if(uIsFinite(covariance.at<double>(0,0)) &&
+				covariance.at<double>(0,0) != 1.0 &&
+				covariance.at<double>(0,0)>0.0)
 			{
-				if(covariance_.empty())
+				// Use largest covariance error (to be independent of the odometry frame rate)
+				if(covariance_.empty() || covariance.at<double>(0,0) > covariance_.at<double>(0,0))
 				{
 					covariance_ = covariance;
 				}
-				else
-				{
-					covariance_ += covariance;
-				}
 			}
 		}
 

src/GuiWrapper.cpp

@@ -302,7 +302,10 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
 			if(!cameraNodeName_.empty())
 			{
 				std::string str = uFormat("rosrun dynamic_reconfigure dynparam set %s pause true", cameraNodeName_.c_str());
-				system(str.c_str());
+				if(system(str.c_str()) !=0)
+				{
+					ROS_ERROR("Command \"%s\" returned non zero value.", str.c_str());
+				}
 			}
 
 			// Pause visual_odometry
@@ -329,7 +332,10 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
 			if(!cameraNodeName_.empty())
 			{
 				std::string str = uFormat("rosrun dynamic_reconfigure dynparam set %s pause false", cameraNodeName_.c_str());
-				system(str.c_str());
+				if(system(str.c_str()) !=0)
+				{
+					ROS_ERROR("Command \"%s\" returned non zero value.", str.c_str());
+				}
 			}
 		}
 		else if(cmd == rtabmap::RtabmapEventCmd::kCmdTriggerNewMap)

src/OdometryROS.cpp

@@ -356,14 +356,18 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 		Transform initialPose = getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, stamp);
 		if(initialPose.isNull())
 		{
-			return;
+			NODELET_WARN("Ground truth frames \"%s\" -> \"%s\" are set but failed to "
+					"get them, odometry won't be synchronized with ground truth.",
+					groundTruthFrameId_.c_str(), groundTruthBaseFrameId_.c_str());
+		}
+		else
+		{
+			NODELET_INFO( "Initializing odometry pose to %s (from \"%s\" -> \"%s\")",
+					initialPose.prettyPrint().c_str(),
+					groundTruthFrameId_.c_str(),
+					groundTruthBaseFrameId_.c_str());
+			odometry_->reset(initialPose);
 		}
-
-		NODELET_INFO( "Initializing odometry pose to %s (from \"%s\" -> \"%s\")",
-				initialPose.prettyPrint().c_str(),
-				groundTruthFrameId_.c_str(),
-				groundTruthBaseFrameId_.c_str());
-		odometry_->reset(initialPose);
 	}
 
 	Transform guess;