Added imu input topic to rtabmap node. Updated for rtabmap 0.19.3.

src/CoreWrapper.cpp

@@ -653,6 +653,7 @@ void CoreWrapper::onInit()
 #ifdef WITH_APRILTAGS2_ROS
 	tagDetectionsSub_ = nh.subscribe("tag_detections", 1, &CoreWrapper::tagDetectionsAsyncCallback, this);
 #endif
+	imuSub_ = nh.subscribe("imu", 100, &CoreWrapper::imuAsyncCallback, this);
 }
 
 CoreWrapper::~CoreWrapper()
@@ -1126,12 +1127,6 @@ void CoreWrapper::commonDepthCallbackImpl(
 		}
 	}
 
-	Transform groundTruthPose;
-	if(!groundTruthFrameId_.empty())
-	{
-		groundTruthPose = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, lastPoseStamp_, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
-	}
-
 	cv::Mat userData;
 	if(userDataMsg.get())
 	{
@@ -1158,68 +1153,6 @@ void CoreWrapper::commonDepthCallbackImpl(
 			lastPoseIntermediate_?-1:imageMsgs[0]->header.seq,
 			rtabmap_ros::timestampFromROS(lastPoseStamp_),
 			userData);
-	data.setGroundTruth(groundTruthPose);
-
-	//global pose
-	if(!globalPose_.header.stamp.isZero())
-	{
-		// assume sensor is fixed
-		Transform sensorToBase = rtabmap_ros::getTransform(
-				globalPose_.header.frame_id,
-				frameId_,
-				lastPoseStamp_,
-				tfListener_,
-				waitForTransform_?waitForTransformDuration_:0.0);
-		if(!sensorToBase.isNull())
-		{
-			Transform globalPose = rtabmap_ros::transformFromPoseMsg(globalPose_.pose.pose);
-			globalPose *= sensorToBase; // transform global pose from sensor frame to robot base frame
-
-			// Correction of the global pose accounting the odometry movement since we received it
-			Transform correction = rtabmap_ros::getTransform(
-					frameId_,
-					odomFrameId,
-					globalPose_.header.stamp,
-					lastPoseStamp_,
-					tfListener_,
-					waitForTransform_?waitForTransformDuration_:0.0);
-			if(!correction.isNull())
-			{
-				globalPose *= correction;
-			}
-			else
-			{
-				NODELET_WARN("Could not adjust global pose accordingly to latest odometry pose. "
-						"If odometry is small since it received the global pose and "
-						"covariance is large, this should not be a problem.");
-			}
-			cv::Mat globalPoseCovariance = cv::Mat(6,6, CV_64FC1, (void*)globalPose_.pose.covariance.data()).clone();
-			data.setGlobalPose(globalPose, globalPoseCovariance);
-		}
-	}
-	globalPose_.header.stamp = ros::Time(0);
-
-	if(gps_.stamp() > 0.0)
-	{
-		data.setGPS(gps_);
-	}
-	gps_ = rtabmap::GPS();
-
-	//tag detections
-	Landmarks landmarks = rtabmap_ros::landmarksFromROS(
-			tags_,
-			frameId_,
-			odomFrameId,
-			lastPoseStamp_,
-			tfListener_,
-			waitForTransform_?waitForTransformDuration_:0,
-			landmarkDefaultLinVariance_,
-			landmarkDefaultAngVariance_);
-	tags_.clear();
-	if(!landmarks.empty())
-	{
-		data.setLandmarks(landmarks);
-	}
 
 	OdometryInfo odomInfo;
 	if(odomInfoMsg.get())
@@ -1392,12 +1325,6 @@ void CoreWrapper::commonStereoCallback(
 		}
 	}
 
-	Transform groundTruthPose;
-	if(!groundTruthFrameId_.empty())
-	{
-		groundTruthPose = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, lastPoseStamp_, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
-	}
-
 	cv::Mat userData;
 	if(userDataMsg.get())
 	{
@@ -1424,68 +1351,6 @@ void CoreWrapper::commonStereoCallback(
 			lastPoseIntermediate_?-1:leftImageMsg->header.seq,
 			rtabmap_ros::timestampFromROS(lastPoseStamp_),
 			userData);
-	data.setGroundTruth(groundTruthPose);
-
-	//global pose
-	if(!globalPose_.header.stamp.isZero())
-	{
-		// assume sensor is fixed
-		Transform sensorToBase = rtabmap_ros::getTransform(
-				globalPose_.header.frame_id,
-				frameId_,
-				lastPoseStamp_,
-				tfListener_,
-				waitForTransform_?waitForTransformDuration_:0.0);
-		if(!sensorToBase.isNull())
-		{
-			Transform globalPose = rtabmap_ros::transformFromPoseMsg(globalPose_.pose.pose);
-			globalPose *= sensorToBase; // transform global pose from sensor frame to robot base frame
-
-			// Correction of the global pose accounting the odometry movement since we received it
-			Transform correction = rtabmap_ros::getTransform(
-					frameId_,
-					odomFrameId,
-					globalPose_.header.stamp,
-					lastPoseStamp_,
-					tfListener_,
-					waitForTransform_?waitForTransformDuration_:0.0);
-			if(!correction.isNull())
-			{
-				globalPose *= correction;
-			}
-			else
-			{
-				NODELET_WARN("Could not adjust global pose accordingly to latest odometry pose. "
-						"If odometry is small since it received the global pose and "
-						"covariance is large, this should not be a problem.");
-			}
-			cv::Mat globalPoseCovariance = cv::Mat(6,6, CV_64FC1, (void*)globalPose_.pose.covariance.data()).clone();
-			data.setGlobalPose(globalPose, globalPoseCovariance);
-		}
-	}
-	globalPose_.header.stamp = ros::Time(0);
-
-	if(gps_.stamp() > 0.0)
-	{
-		data.setGPS(gps_);
-	}
-	gps_ = rtabmap::GPS();
-
-	//tag detections
-	Landmarks landmarks = rtabmap_ros::landmarksFromROS(
-			tags_,
-			frameId_,
-			odomFrameId,
-			lastPoseStamp_,
-			tfListener_,
-			waitForTransform_?waitForTransformDuration_:0,
-			landmarkDefaultLinVariance_,
-			landmarkDefaultAngVariance_);
-	tags_.clear();
-	if(!landmarks.empty())
-	{
-		data.setLandmarks(landmarks);
-	}
 
 	OdometryInfo odomInfo;
 	if(odomInfoMsg.get())
@@ -1588,12 +1453,6 @@ void CoreWrapper::commonLaserScanCallback(
 		}
 	}
 
-	Transform groundTruthPose;
-	if(!groundTruthFrameId_.empty())
-	{
-		groundTruthPose = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, lastPoseStamp_, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
-	}
-
 	cv::Mat userData;
 	if(userDataMsg.get())
 	{
@@ -1631,52 +1490,6 @@ void CoreWrapper::commonLaserScanCallback(
 			lastPoseIntermediate_?-1:scan2dMsg.get() != 0?scan2dMsg->header.seq:scan3dMsg->header.seq,
 			rtabmap_ros::timestampFromROS(lastPoseStamp_),
 			userData);
-	data.setGroundTruth(groundTruthPose);
-
-	//global pose
-	if(!globalPose_.header.stamp.isZero())
-	{
-		// assume sensor is fixed
-		Transform sensorToBase = rtabmap_ros::getTransform(
-				globalPose_.header.frame_id,
-				frameId_,
-				lastPoseStamp_,
-				tfListener_,
-				waitForTransform_?waitForTransformDuration_:0.0);
-		if(!sensorToBase.isNull())
-		{
-			Transform globalPose = rtabmap_ros::transformFromPoseMsg(globalPose_.pose.pose);
-			globalPose *= sensorToBase; // transform global pose from sensor frame to robot base frame
-
-			// Correction of the global pose accounting the odometry movement since we received it
-			Transform correction = rtabmap_ros::getTransform(
-					frameId_,
-					odomFrameId,
-					globalPose_.header.stamp,
-					lastPoseStamp_,
-					tfListener_,
-					waitForTransform_?waitForTransformDuration_:0.0);
-			if(!correction.isNull())
-			{
-				globalPose *= correction;
-			}
-			else
-			{
-				NODELET_WARN("Could not adjust global pose accordingly to latest odometry pose. "
-						"If odometry is small since it received the global pose and "
-						"covariance is large, this should not be a problem.");
-			}
-			cv::Mat globalPoseCovariance = cv::Mat(6,6, CV_64FC1, (void*)globalPose_.pose.covariance.data()).clone();
-			data.setGlobalPose(globalPose, globalPoseCovariance);
-		}
-	}
-	globalPose_.header.stamp = ros::Time(0);
-
-	if(gps_.stamp() > 0.0)
-	{
-		data.setGPS(gps_);
-	}
-	gps_ = rtabmap::GPS();
 
 	OdometryInfo odomInfo;
 	if(odomInfoMsg.get())
@@ -1684,22 +1497,6 @@ void CoreWrapper::commonLaserScanCallback(
 		odomInfo = odomInfoFromROS(*odomInfoMsg);
 	}
 
-	//tag detections
-	Landmarks landmarks = rtabmap_ros::landmarksFromROS(
-			tags_,
-			frameId_,
-			odomFrameId,
-			lastPoseStamp_,
-			tfListener_,
-			waitForTransform_?waitForTransformDuration_:0,
-			landmarkDefaultLinVariance_,
-			landmarkDefaultAngVariance_);
-	tags_.clear();
-	if(!landmarks.empty())
-	{
-		data.setLandmarks(landmarks);
-	}
-
 	process(lastPoseStamp_,
 			data,
 			lastPose_,
@@ -1724,12 +1521,6 @@ void CoreWrapper::commonOdomCallback(
 		return;
 	}
 
-	Transform groundTruthPose;
-	if(!groundTruthFrameId_.empty())
-	{
-		groundTruthPose = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, lastPoseStamp_, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
-	}
-
 	cv::Mat userData;
 	if(userDataMsg.get())
 	{
@@ -1766,52 +1557,6 @@ void CoreWrapper::commonOdomCallback(
 			lastPoseIntermediate_?-1:odomMsg->header.seq,
 			rtabmap_ros::timestampFromROS(lastPoseStamp_),
 			userData);
-	data.setGroundTruth(groundTruthPose);
-
-	//global pose
-	if(!globalPose_.header.stamp.isZero())
-	{
-		// assume sensor is fixed
-		Transform sensorToBase = rtabmap_ros::getTransform(
-				globalPose_.header.frame_id,
-				frameId_,
-				lastPoseStamp_,
-				tfListener_,
-				waitForTransform_?waitForTransformDuration_:0.0);
-		if(!sensorToBase.isNull())
-		{
-			Transform globalPose = rtabmap_ros::transformFromPoseMsg(globalPose_.pose.pose);
-			globalPose *= sensorToBase; // transform global pose from sensor frame to robot base frame
-
-			// Correction of the global pose accounting the odometry movement since we received it
-			Transform correction = rtabmap_ros::getTransform(
-					frameId_,
-					odomFrameId,
-					globalPose_.header.stamp,
-					lastPoseStamp_,
-					tfListener_,
-					waitForTransform_?waitForTransformDuration_:0.0);
-			if(!correction.isNull())
-			{
-				globalPose *= correction;
-			}
-			else
-			{
-				NODELET_WARN("Could not adjust global pose accordingly to latest odometry pose. "
-						"If odometry is small since it received the global pose and "
-						"covariance is large, this should not be a problem.");
-			}
-			cv::Mat globalPoseCovariance = cv::Mat(6,6, CV_64FC1, (void*)globalPose_.pose.covariance.data()).clone();
-			data.setGlobalPose(globalPose, globalPoseCovariance);
-		}
-	}
-	globalPose_.header.stamp = ros::Time(0);
-
-	if(gps_.stamp() > 0.0)
-	{
-		data.setGPS(gps_);
-	}
-	gps_ = rtabmap::GPS();
 
 	OdometryInfo odomInfo;
 	if(odomInfoMsg.get())
@@ -1819,22 +1564,6 @@ void CoreWrapper::commonOdomCallback(
 		odomInfo = odomInfoFromROS(*odomInfoMsg);
 	}
 
-	//tag detections
-	Landmarks landmarks = rtabmap_ros::landmarksFromROS(
-			tags_,
-			frameId_,
-			odomFrameId,
-			lastPoseStamp_,
-			tfListener_,
-			waitForTransform_?waitForTransformDuration_:0,
-			landmarkDefaultLinVariance_,
-			landmarkDefaultAngVariance_);
-	tags_.clear();
-	if(!landmarks.empty())
-	{
-		data.setLandmarks(landmarks);
-	}
-
 	process(lastPoseStamp_,
 			data,
 			lastPose_,
@@ -1847,7 +1576,7 @@ void CoreWrapper::commonOdomCallback(
 
 void CoreWrapper::process(
 		const ros::Time & stamp,
-		const SensorData & data,
+		SensorData & data,
 		const Transform & odom,
 		const std::string & odomFrameId,
 		const cv::Mat & odomCovariance,
@@ -1856,6 +1585,96 @@ void CoreWrapper::process(
 	UTimer timer;
 	if(rtabmap_.isIDsGenerated() || data.id() > 0)
 	{
+		//Add async stuff
+		Transform groundTruthPose;
+		if(!groundTruthFrameId_.empty())
+		{
+			groundTruthPose = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, lastPoseStamp_, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
+		}
+		data.setGroundTruth(groundTruthPose);
+
+		//global pose
+		if(!globalPose_.header.stamp.isZero())
+		{
+			// assume sensor is fixed
+			Transform sensorToBase = rtabmap_ros::getTransform(
+					globalPose_.header.frame_id,
+					frameId_,
+					lastPoseStamp_,
+					tfListener_,
+					waitForTransform_?waitForTransformDuration_:0.0);
+			if(!sensorToBase.isNull())
+			{
+				Transform globalPose = rtabmap_ros::transformFromPoseMsg(globalPose_.pose.pose);
+				globalPose *= sensorToBase; // transform global pose from sensor frame to robot base frame
+
+				// Correction of the global pose accounting the odometry movement since we received it
+				Transform correction = rtabmap_ros::getTransform(
+						frameId_,
+						odomFrameId,
+						globalPose_.header.stamp,
+						lastPoseStamp_,
+						tfListener_,
+						waitForTransform_?waitForTransformDuration_:0.0);
+				if(!correction.isNull())
+				{
+					globalPose *= correction;
+				}
+				else
+				{
+					NODELET_WARN("Could not adjust global pose accordingly to latest odometry pose. "
+							"If odometry is small since it received the global pose and "
+							"covariance is large, this should not be a problem.");
+				}
+				cv::Mat globalPoseCovariance = cv::Mat(6,6, CV_64FC1, (void*)globalPose_.pose.covariance.data()).clone();
+				data.setGlobalPose(globalPose, globalPoseCovariance);
+			}
+		}
+		globalPose_.header.stamp = ros::Time(0);
+
+		if(gps_.stamp() > 0.0)
+		{
+			data.setGPS(gps_);
+		}
+		gps_ = rtabmap::GPS();
+
+		//tag detections
+		Landmarks landmarks = rtabmap_ros::landmarksFromROS(
+				tags_,
+				frameId_,
+				odomFrameId,
+				lastPoseStamp_,
+				tfListener_,
+				waitForTransform_?waitForTransformDuration_:0,
+				landmarkDefaultLinVariance_,
+				landmarkDefaultAngVariance_);
+		tags_.clear();
+		if(!landmarks.empty())
+		{
+			data.setLandmarks(landmarks);
+		}
+
+		// IMU
+		if(!imus_.empty())
+		{
+			double stampDiff = 0.0;
+			Transform t = Transform::getClosestTransform(imus_, data.stamp(), &stampDiff);
+			if(!t.isNull() && stampDiff == 0.0)
+			{
+				Eigen::Quaterniond q = t.getQuaterniond();
+				data.setIMU(IMU(cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat::eye(3,3,CV_64FC1),
+						cv::Vec3d(), cv::Mat(),
+						cv::Vec3d(), cv::Mat(),
+						Transform::getIdentity()));
+			}
+			else
+			{
+				ROS_WARN("We are receiving imu data (buffer=%d), but cannot interpolate "
+						"imu transform at time %f (closest is at %f). IMU won't be added to graph.",
+						(int)imus_.size(), data.stamp(), stampDiff);
+			}
+		}
+
 		double timeRtabmap = 0.0;
 		double timeUpdateMaps = 0.0;
 		double timePublishMaps = 0.0;
@@ -2198,6 +2017,37 @@ void CoreWrapper::tagDetectionsAsyncCallback(const apriltags2_ros::AprilTagDetec
 }
 #endif
 
+void CoreWrapper::imuAsyncCallback(const sensor_msgs::ImuConstPtr & msg)
+{
+	if(!paused_)
+	{
+		if(msg->orientation.x == 0 && msg->orientation.y == 0 && msg->orientation.z == 0 && msg->orientation.w == 0)
+		{
+			UERROR("IMU received doesn't have orientation set, it is ignored.");
+		}
+		else
+		{
+			double stamp = msg->header.stamp.toSec();
+			rtabmap::Transform localTransform = rtabmap::Transform::getIdentity();
+			if(frameId_.compare(msg->header.frame_id) != 0)
+			{
+				localTransform = getTransform(frameId_, msg->header.frame_id, msg->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
+				if(localTransform.isNull())
+				{
+					return;
+				}
+			}
+
+			Transform orientation(0,0,0, msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w);
+			imus_.insert(std::make_pair(msg->header.stamp.toSec(), orientation*localTransform.inverse()));
+			if(imus_.size() > 1000)
+			{
+				imus_.erase(imus_.begin());
+			}
+		}
+	}
+}
+
 
 void CoreWrapper::initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg)
 {
@@ -2471,6 +2321,7 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
 	userDataMutex_.lock();
 	userData_ = cv::Mat();
 	userDataMutex_.unlock();
+	imus_.clear();
 	return true;
 }
 

src/GuiNode.cpp

@@ -71,6 +71,7 @@ int main(int argc, char** argv)
 	// Now wait for application to finish
 	int r = app->exec();// MUST be called by the Main Thread
 
+	ROS_INFO("rtabmapviz stopping spinner...");
 	spinner->stop();
 	delete spinner;
 

src/OdometryROS.cpp

@@ -343,10 +343,7 @@ void OdometryROS::onInit()
 
 	odomStrategy_ = 0;
 	Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy_);
-	if(waitIMUToinit_ ||
-		odomStrategy_ == Odometry::kTypeMSCKF ||
-		odomStrategy_ == Odometry::kTypeOkvis ||
-		odomStrategy_ == Odometry::kTypeVINS)
+	if(waitIMUToinit_ || odometry_->canProcessIMU())
 	{
 		int queueSize = 10;
 		pnh.param("queue_size", queueSize, queueSize);
@@ -415,9 +412,7 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
 {
 	if(!this->isPaused())
 	{
-		if(odomStrategy_ != Odometry::kTypeOkvis &&
-		   odomStrategy_ != Odometry::kTypeMSCKF &&
-		   odomStrategy_ != Odometry::kTypeVINS &&
+		if(!odometry_->canProcessIMU() &&
 		   !odometry_->getPose().isIdentity())
 		{
 			// For non-inertial odometry approaches, IMU is only used to initialize the initial orientation below
@@ -437,16 +432,15 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
 			return;
 		}
 
-		IMU imu(
+		IMU imu(cv::Vec4d(msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w),
+				cv::Mat(3,3,CV_64FC1,(void*)msg->orientation_covariance.data()).clone(),
 				cv::Vec3d(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z),
 				cv::Mat(3,3,CV_64FC1,(void*)msg->angular_velocity_covariance.data()).clone(),
 				cv::Vec3d(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z),
 				cv::Mat(3,3,CV_64FC1,(void*)msg->linear_acceleration_covariance.data()).clone(),
 				localTransform);
 
-		if(odomStrategy_ != Odometry::kTypeOkvis &&
-		   odomStrategy_ != Odometry::kTypeMSCKF &&
-		   odomStrategy_ != Odometry::kTypeVINS)
+		if(!odometry_->canProcessIMU())
 		{
 			if(!odometry_->getPose().isIdentity())
 			{
@@ -454,27 +448,39 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
 				return;
 			}
 
-			if(	imu.linearAcceleration()[0]!=0.0 &&
-				imu.linearAcceleration()[1]!=0.0 &&
-				imu.linearAcceleration()[2]!=0.0 &&
-				!imu.localTransform().isNull())
+			// align with gravity
+			if(!imu.localTransform().isNull())
 			{
-				// align with gravity
-				Eigen::Vector3f n(imu.linearAcceleration()[0], imu.linearAcceleration()[1], imu.linearAcceleration()[2]);
-				n = imu.localTransform().rotation().toEigen3f() * n;
-				n.normalize();
-				Eigen::Vector3f z(0,0,1);
-				//get rotation from z to n;
-				Eigen::Matrix3f R;
-				R = Eigen::Quaternionf().setFromTwoVectors(n,z);
-				Transform rotation(
-						R(0,0), R(0,1), R(0,2), 0,
-						R(1,0), R(1,1), R(1,2), 0,
-						R(2,0), R(2,1), R(2,2), 0);
-				this->reset(rotation);
-				float r,p,y;
-				rotation.getEulerAngles(r,p,y);
-				NODELET_WARN("odometry: Initialized odometry orientation with IMU (rpy = %f %f %f).", r,p,y);
+				if(imu.orientation()[0] != 0 || imu.orientation()[1] != 0 || imu.orientation()[2] != 0 || imu.orientation()[3] != 0)
+				{
+					Transform rotation(0,0,0, imu.orientation()[0], imu.orientation()[1], imu.orientation()[2], imu.orientation()[3]);
+					rotation = rotation * imu.localTransform().rotation().inverse();
+					this->reset(rotation);
+					float r,p,y;
+					rotation.getEulerAngles(r,p,y);
+					NODELET_WARN("odometry: Initialized odometry with IMU's orientation (rpy = %f %f %f).", r,p,y);
+				}
+				else if(imu.linearAcceleration()[0]!=0.0 &&
+					imu.linearAcceleration()[1]!=0.0 &&
+					imu.linearAcceleration()[2]!=0.0 &&
+					!imu.localTransform().isNull())
+				{
+					Eigen::Vector3f n(imu.linearAcceleration()[0], imu.linearAcceleration()[1], imu.linearAcceleration()[2]);
+					n = imu.localTransform().rotation().toEigen3f() * n;
+					n.normalize();
+					Eigen::Vector3f z(0,0,1);
+					//get rotation from z to n;
+					Eigen::Matrix3f R;
+					R = Eigen::Quaternionf().setFromTwoVectors(n,z);
+					Transform rotation(
+							R(0,0), R(0,1), R(0,2), 0,
+							R(1,0), R(1,1), R(1,2), 0,
+							R(2,0), R(2,1), R(2,2), 0);
+					this->reset(rotation);
+					float r,p,y;
+					rotation.getEulerAngles(r,p,y);
+					NODELET_WARN("odometry: Initialized odometry with IMU's accelerometer (rpy = %f %f %f).", r,p,y);
+				}
 			}
 		}
 		else
@@ -493,6 +499,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 		return;
 	}
 
+	Transform groundTruth;
 	if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
 	{
 		if(previousStamp_>0.0 && previousStamp_ >= stamp.toSec())
@@ -509,36 +516,36 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 					1.0/(stamp.toSec()-previousStamp_), expectedUpdateRate_, previousStamp_, stamp.toSec());
 			return;
 		}
-	}
 
-	Transform groundTruth;
-	if(!groundTruthFrameId_.empty())
-	{
-		groundTruth = getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, stamp);
-
-		if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
+		if(!groundTruthFrameId_.empty())
 		{
-			if(odometry_->getPose().isIdentity())
+			groundTruth = getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, stamp);
+
+			if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
 			{
-				// sync with the first value of the ground truth
-				if(groundTruth.isNull())
+				if(odometry_->getPose().isIdentity())
 				{
-					NODELET_WARN("Ground truth frames \"%s\" -> \"%s\" are set but failed to "
-							"get them, odometry won't be initialized with ground truth.",
-							groundTruthFrameId_.c_str(), groundTruthBaseFrameId_.c_str());
-				}
-				else
-				{
-					NODELET_INFO( "Initializing odometry pose to %s (from \"%s\" -> \"%s\")",
-							groundTruth.prettyPrint().c_str(),
-							groundTruthFrameId_.c_str(),
-							groundTruthBaseFrameId_.c_str());
-					odometry_->reset(groundTruth);
+					// sync with the first value of the ground truth
+					if(groundTruth.isNull())
+					{
+						NODELET_WARN("Ground truth frames \"%s\" -> \"%s\" are set but failed to "
+								"get them, odometry won't be initialized with ground truth.",
+								groundTruthFrameId_.c_str(), groundTruthBaseFrameId_.c_str());
+					}
+					else
+					{
+						NODELET_INFO( "Initializing odometry pose to %s (from \"%s\" -> \"%s\")",
+								groundTruth.prettyPrint().c_str(),
+								groundTruthFrameId_.c_str(),
+								groundTruthBaseFrameId_.c_str());
+						odometry_->reset(groundTruth);
+					}
 				}
 			}
 		}
 	}
 
+
 	Transform guessCurrentPose;
 	if(!guessFrameId_.empty())
 	{