Added input stamps double-verification in some nodelets (data_throttle, stereo_throttle, rgbd_sync, stereo_sync, pointcloud_to_depthimage)

rtabmapviz: added "max_odom_update_rate" parameters
Odom: moved IMU callback from stereo_odometry nodelet to OdometryROS, added "wait_imu_to_init" parameter to initialize odom with IMU first
Fixed fake camera local transform on scan-only callbacks (rtabmap, rtabmapviz)

src/CoreWrapper.cpp

@@ -1749,7 +1749,7 @@ void CoreWrapper::commonLaserScanCallback(
 			1,
 			0.5,
 			1,
-			Transform::getIdentity(),
+			scanLocalTransform*Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
 			0,
 			cv::Size(1,2));
 

src/GuiWrapper.cpp

@@ -70,6 +70,7 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 		waitForTransform_(true),
 		waitForTransformDuration_(0.2), // 200 ms
 		odomSensorSync_(false),
+		maxOdomUpdateRate_(10),
 		cameraNodeName_(""),
 		lastOdomInfoUpdateTime_(0)
 {
@@ -110,6 +111,7 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 	pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
 	pnh.param("wait_for_transform_duration",  waitForTransformDuration_, waitForTransformDuration_);
 	pnh.param("odom_sensor_sync", odomSensorSync_, odomSensorSync_);
+	pnh.param("max_odom_update_rate", maxOdomUpdateRate_, maxOdomUpdateRate_);
 	pnh.param("camera_node_name", cameraNodeName_, cameraNodeName_); // used to pause the rtabmap_ros/camera when pausing the process
 	pnh.param("init_cache_path", initCachePath, initCachePath);
 	if(initCachePath.size())
@@ -496,9 +498,11 @@ void GuiWrapper::commonDepthCallback(
 	rtabmap::OdometryInfo info;
 	bool ignoreData = false;
 
-	if(UTimer::now() - lastOdomInfoUpdateTime_ > 0.1 &&
+	// limit update rate
+	if(maxOdomUpdateRate_<=0.0 ||
+	   (UTimer::now() - lastOdomInfoUpdateTime_ > 1.0/maxOdomUpdateRate_ &&
 	   !mainWindow_->isProcessingOdometry() &&
-	   !mainWindow_->isProcessingStatistics())
+	   !mainWindow_->isProcessingStatistics()))
 	{
 		lastOdomInfoUpdateTime_ = UTimer::now();
 
@@ -652,10 +656,11 @@ void GuiWrapper::commonStereoCallback(
 	rtabmap::OdometryInfo info;
 	bool ignoreData = false;
 
-	// limit 10 Hz max
-	if(UTimer::now() - lastOdomInfoUpdateTime_ > 0.1 &&
+	// limit update rate
+	if(maxOdomUpdateRate_<=0.0 ||
+	   (UTimer::now() - lastOdomInfoUpdateTime_ > 1.0/maxOdomUpdateRate_ &&
 	   !mainWindow_->isProcessingOdometry() &&
-	   !mainWindow_->isProcessingStatistics())
+	   !mainWindow_->isProcessingStatistics()))
 	{
 		lastOdomInfoUpdateTime_ = UTimer::now();
 
@@ -802,10 +807,11 @@ void GuiWrapper::commonLaserScanCallback(
 	rtabmap::OdometryInfo info;
 	bool ignoreData = false;
 
-	// limit 10 Hz max
-	if(UTimer::now() - lastOdomInfoUpdateTime_ > 0.1 &&
+	// limit update rate
+	if(maxOdomUpdateRate_<=0.0 ||
+	   (UTimer::now() - lastOdomInfoUpdateTime_ > 1.0/maxOdomUpdateRate_ &&
 	   !mainWindow_->isProcessingOdometry() &&
-	   !mainWindow_->isProcessingStatistics())
+	   !mainWindow_->isProcessingStatistics()))
 	{
 		lastOdomInfoUpdateTime_ = UTimer::now();
 
@@ -850,6 +856,16 @@ void GuiWrapper::commonLaserScanCallback(
 	}
 	else if(odomInfoMsg.get())
 	{
+		//just get scan local transform to adjust camera frame
+		if(scan2dMsg.get() != 0)
+		{
+			scanLocalTransform = getTransform(frameId_, scan2dMsg->header.frame_id, scan2dMsg->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
+		}
+		else if(scan3dMsg.get() != 0)
+		{
+			scanLocalTransform = getTransform(frameId_, scan3dMsg->header.frame_id, scan3dMsg->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0);
+		}
+
 		info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg).copyWithoutData();
 		ignoreData = true;
 	}
@@ -866,17 +882,22 @@ void GuiWrapper::commonLaserScanCallback(
 			1,
 			0.5,
 			1,
-			Transform::getIdentity(),
+			scanLocalTransform*Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
 			0,
 			cv::Size(1,2));
 
 	info.reg.covariance = covariance;
 	rtabmap::OdometryEvent odomEvent(
 		rtabmap::SensorData(
-				LaserScan::backwardCompatibility(scan,
-						scan2dMsg.get()?(int)scan2dMsg->ranges.size():0,
-						scan2dMsg.get()?(int)scan2dMsg->range_max:0,
-						scanLocalTransform),
+				scan2dMsg.get() != 0?
+					LaserScan::backwardCompatibility(scan,
+							scan2dMsg->range_min,
+							scan2dMsg->range_max,
+							scan2dMsg->angle_min,
+							scan2dMsg->angle_max,
+							scan2dMsg->angle_increment,
+							scanLocalTransform):
+					LaserScan::backwardCompatibility(scan,0,0,scanLocalTransform),
 				rgb,
 				depth,
 				model,

src/OdometryROS.cpp

@@ -78,7 +78,9 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
 	visParams_(visParams),
 	icpParams_(icpParams),
 	previousStamp_(0.0),
-	expectedUpdateRate_(0.0)
+	expectedUpdateRate_(0.0),
+	odomStrategy_(Parameters::defaultOdomStrategy()),
+	waitIMUToinit_(false)
 {
 
 }
@@ -148,6 +150,8 @@ void OdometryROS::onInit()
 
 	pnh.param("expected_update_rate", expectedUpdateRate_, expectedUpdateRate_);
 
+	pnh.param("wait_imu_to_init", waitIMUToinit_, waitIMUToinit_);
+
 	if(publishTf_ && !guessFrameId_.empty() && guessFrameId_.compare(odomFrameId_) == 0)
 	{
 		NODELET_WARN( "\"publish_tf\" and \"guess_frame_id\" cannot be used "
@@ -169,6 +173,7 @@ void OdometryROS::onInit()
 	NODELET_INFO("Odometry: guess_min_translation  = %f", guessMinTranslation_);
 	NODELET_INFO("Odometry: guess_min_rotation     = %f", guessMinRotation_);
 	NODELET_INFO("Odometry: expected_update_rate   = %f Hz", expectedUpdateRate_);
+	NODELET_INFO("Odometry: wait_imu_to_init       = %s", waitIMUToinit_?"true":"false");
 
 	configPath = uReplaceChar(configPath, '~', UDirectory::homeDir());
 	if(configPath.size() && configPath.at(0) != '/')
@@ -333,6 +338,16 @@ void OdometryROS::onInit()
 	setLogWarnSrv_ = pnh.advertiseService("log_warning", &OdometryROS::setLogWarn, this);
 	setLogErrorSrv_ = pnh.advertiseService("log_error", &OdometryROS::setLogError, this);
 
+	odomStrategy_ = 0;
+	Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy_);
+	if(waitIMUToinit_ || odomStrategy_ == Odometry::kTypeF2F || odomStrategy_ == Odometry::kTypeF2F)
+	{
+		int queueSize = 10;
+		pnh.param("queue_size", queueSize, queueSize);
+		imuSub_ = nh.subscribe("imu", queueSize*5, &OdometryROS::callbackIMU, this);
+		NODELET_INFO("odometry: Subscribing to IMU topic %s", imuSub_.getTopic().c_str());
+	}
+
 	onOdomInit();
 }
 
@@ -390,8 +405,86 @@ Transform OdometryROS::getTransform(const std::string & fromFrameId, const std::
 	return transform;
 }
 
+void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
+{
+	if(!this->isPaused())
+	{
+		if(odomStrategy_ != Odometry::kTypeOkvis &&
+		   odomStrategy_ != Odometry::kTypeMSCKF &&
+		   !odometry_->getPose().isIdentity())
+		{
+			// For non-inertial odometry approaches, IMU is only used to initialize the initial orientation below
+			return;
+		}
+
+		double stamp = msg->header.stamp.toSec();
+		rtabmap::Transform localTransform = rtabmap::Transform::getIdentity();
+		if(this->frameId().compare(msg->header.frame_id) != 0)
+		{
+			localTransform = getTransform(this->frameId(), msg->header.frame_id, msg->header.stamp);
+		}
+		if(localTransform.isNull())
+		{
+			ROS_ERROR("Could not transform IMU msg from frame \"%s\" to frame \"%s\", TF not available at time %f",
+					msg->header.frame_id.c_str(), this->frameId().c_str(), stamp);
+			return;
+		}
+
+		IMU imu(
+				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)
+		{
+			if(!odometry_->getPose().isIdentity())
+			{
+				// For these approaches, IMU is only used to initialize the initial orientation
+				return;
+			}
+
+			if(	imu.linearAcceleration()[0]!=0.0 &&
+				imu.linearAcceleration()[1]!=0.0 &&
+				imu.linearAcceleration()[2]!=0.0 &&
+				!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);
+			}
+		}
+		else
+		{
+			SensorData data(imu, 0, stamp);
+			this->processData(data, msg->header.stamp);
+		}
+	}
+}
+
 void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 {
+	if(waitIMUToinit_ && odometry_->framesProcessed() == 0 && odometry_->getPose().isIdentity() && data.imu().empty())
+	{
+		NODELET_WARN("odometry: waiting imu to initialize orientation (wait_imu_to_init=true)");
+		return;
+	}
+
 	if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
 	{
 		if(previousStamp_>0.0 && previousStamp_ >= stamp.toSec())
@@ -753,12 +846,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
 {
 	NODELET_INFO( "visual_odometry: reset odom!");
-	odometry_->reset();
-	guess_.setNull();
-	guessPreviousPose_.setNull();
-	previousStamp_ = 0.0;
-	resetCurrentCount_ = resetCountdown_;
-	this->flushCallbacks();
+	reset();
 	return true;
 }
 
@@ -766,13 +854,18 @@ bool OdometryROS::resetToPose(rtabmap_ros::ResetPose::Request& req, rtabmap_ros:
 {
 	Transform pose(req.x, req.y, req.z, req.roll, req.pitch, req.yaw);
 	NODELET_INFO( "visual_odometry: reset odom to pose %s!", pose.prettyPrint().c_str());
+	reset(pose);
+	return true;
+}
+
+void OdometryROS::reset(const Transform & pose)
+{
 	odometry_->reset(pose);
 	guess_.setNull();
 	guessPreviousPose_.setNull();
 	previousStamp_ = 0.0;
 	resetCurrentCount_ = resetCountdown_;
 	this->flushCallbacks();
-	return true;
 }
 
 bool OdometryROS::pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&)

src/nodelets/data_throttle.cpp

@@ -140,38 +140,10 @@ private:
 
 		last_update_ = ros::Time::now();
 
-		if(imagePub_.getNumSubscribers())
-		{
-			if(decimation_ > 1)
-			{
-				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
-				cv_bridge::CvImage out;
-				out.header = imagePtr->header;
-				out.encoding = imagePtr->encoding;
-				out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
-				imagePub_.publish(out.toImageMsg());
-			}
-			else
-			{
-				imagePub_.publish(image);
-			}
-		}
-		if(imageDepthPub_.getNumSubscribers())
-		{
-			if(decimation_ > 1)
-			{
-				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageDepth);
-				cv_bridge::CvImage out;
-				out.header = imagePtr->header;
-				out.encoding = imagePtr->encoding;
-				out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
-				imageDepthPub_.publish(out.toImageMsg());
-			}
-			else
-			{
-				imageDepthPub_.publish(imageDepth);
-			}
-		}
+		double rgbStamp = image->header.stamp.toSec();
+		double depthStamp = imageDepth->header.stamp.toSec();
+		double infoStamp = camInfo->header.stamp.toSec();
+
 		if(infoPub_.getNumSubscribers())
 		{
 			if(decimation_ > 1)
@@ -196,6 +168,52 @@ private:
 				infoPub_.publish(camInfo);
 			}
 		}
+		if(imagePub_.getNumSubscribers())
+		{
+			if(decimation_ > 1)
+			{
+				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
+				cv_bridge::CvImage out;
+				out.header = imagePtr->header;
+				out.encoding = imagePtr->encoding;
+				out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
+				imagePub_.publish(out.toImageMsg());
+			}
+			else
+			{
+				imagePub_.publish(image);
+			}
+		}
+
+		if(imageDepthPub_.getNumSubscribers())
+		{
+			if(decimation_ > 1)
+			{
+				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageDepth);
+				cv_bridge::CvImage out;
+				out.header = imagePtr->header;
+				out.encoding = imagePtr->encoding;
+				out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
+				imageDepthPub_.publish(out.toImageMsg());
+			}
+			else
+			{
+				imageDepthPub_.publish(imageDepth);
+			}
+		}
+
+		if( rgbStamp != image->header.stamp.toSec() ||
+			depthStamp != imageDepth->header.stamp.toSec() ||
+			infoStamp != camInfo->header.stamp.toSec())
+		{
+			NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
+					"sure the node publishing the topics doesn't override the same data after publishing them. A "
+					"solution is to use this node within another nodelet manager. Stamps: "
+					"rgb=%f->%f depth=%f->%f info=%f->%f",
+					rgbStamp, image->header.stamp.toSec(),
+					depthStamp, imageDepth->header.stamp.toSec(),
+					infoStamp, camInfo->header.stamp.toSec());
+		}
 	}
 
 	image_transport::Publisher imagePub_;

src/nodelets/pointcloud_to_depthimage.cpp

@@ -144,6 +144,9 @@ private:
 	{
 		if(depthImage32Pub_.getNumSubscribers() > 0 || depthImage16Pub_.getNumSubscribers() > 0)
 		{
+			double cloudStamp = pointCloud2Msg->header.stamp.toSec();
+			double infoStamp = cameraInfoMsg->header.stamp.toSec();
+
 			rtabmap::Transform cloudDisplacement = rtabmap::Transform::getIdentity();
 			if(!fixedFrameId_.empty())
 			{
@@ -221,6 +224,17 @@ private:
 				depthImage.image = rtabmap::util2d::cvtDepthFromFloat(depthImage.image);
 				depthImage16Pub_.publish(depthImage.toImageMsg());
 			}
+
+			if( cloudStamp != pointCloud2Msg->header.stamp.toSec() ||
+				infoStamp != cameraInfoMsg->header.stamp.toSec())
+			{
+				NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
+						"sure the node publishing the topics doesn't override the same data after publishing them. A "
+						"solution is to use this node within another nodelet manager. Stamps: "
+						"cloud=%f->%f info=%f->%f",
+						cloudStamp, pointCloud2Msg->header.stamp.toSec(),
+						infoStamp, cameraInfoMsg->header.stamp.toSec());
+			}
 		}
 	}
 

src/nodelets/rgbd_sync.cpp

@@ -161,6 +161,10 @@ private:
 		callbackCalled_ = true;
 		if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
 		{
+			double rgbStamp = image->header.stamp.toSec();
+			double depthStamp = depth->header.stamp.toSec();
+			double infoStamp = cameraInfo->header.stamp.toSec();
+
 			rtabmap_ros::RGBDImage msg;
 			msg.header.frame_id = cameraInfo->header.frame_id;
 			msg.header.stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp;
@@ -204,6 +208,19 @@ private:
 				}
 				rgbdImagePub_.publish(msg);
 			}
+
+			if( rgbStamp != image->header.stamp.toSec() ||
+				depthStamp != depth->header.stamp.toSec() ||
+				infoStamp != cameraInfo->header.stamp.toSec())
+			{
+				NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
+						"sure the node publishing the topics doesn't override the same data after publishing them. A "
+						"solution is to use this node within another nodelet manager. Stamps: "
+						"rgb=%f->%f depth=%f->%f info=%f->%f",
+						rgbStamp, image->header.stamp.toSec(),
+						depthStamp, depth->header.stamp.toSec(),
+						infoStamp, cameraInfo->header.stamp.toSec());
+			}
 		}
 	}
 

src/nodelets/stereo_odometry.cpp

@@ -38,7 +38,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/image_encodings.h>
-#include <sensor_msgs/Imu.h>
 
 #include <image_geometry/stereo_camera_model.h>
 
@@ -143,14 +142,6 @@ private:
 					cameraInfoRight_.getTopic().c_str());
 		}
 
-		int odomStrategy = 0;
-		Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy);
-		if(odomStrategy == Odometry::kTypeOkvis || odomStrategy == Odometry::kTypeMSCKF)
-		{
-			imuSub_ = nh.subscribe("imu", queueSize_*5, &StereoOdometry::callbackIMU, this);
-			NODELET_INFO("VIO approach selected, subscribing to IMU topic %s", imuSub_.getTopic().c_str());
-		}
-
 		this->startWarningThread(subscribedTopicsMsg, approxSync);
 	}
 
@@ -329,36 +320,6 @@ private:
 		}
 	}
 
-	void callbackIMU(
-			const sensor_msgs::ImuConstPtr& msg)
-	{
-		if(!this->isPaused())
-		{
-			double stamp = msg->header.stamp.toSec();
-			rtabmap::Transform localTransform = rtabmap::Transform::getIdentity();
-			if(this->frameId().compare(msg->header.frame_id) != 0)
-			{
-				localTransform = getTransform(this->frameId(), msg->header.frame_id, msg->header.stamp);
-			}
-			if(localTransform.isNull())
-			{
-				ROS_ERROR("Could not transform IMU msg from frame \"%s\" to frame \"%s\", TF not available at time %f",
-						msg->header.frame_id.c_str(), this->frameId().c_str(), stamp);
-				return;
-			}
-
-			IMU imu(
-					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);
-
-			SensorData data(imu, 0, stamp);
-			this->processData(data, msg->header.stamp);
-		}
-	}
-
 protected:
 	virtual void flushCallbacks()
 	{

src/nodelets/stereo_sync.cpp

@@ -161,6 +161,11 @@ private:
 		callbackCalled_ = true;
 		if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
 		{
+			double leftStamp = imageLeft->header.stamp.toSec();
+			double rightStamp = imageRight->header.stamp.toSec();
+			double leftInfoStamp = cameraInfoLeft->header.stamp.toSec();
+			double rightInfoStamp = cameraInfoRight->header.stamp.toSec();
+
 			rtabmap_ros::RGBDImage msg;
 			msg.header.frame_id = cameraInfoLeft->header.frame_id;
 			msg.header.stamp = imageLeft->header.stamp>imageRight->header.stamp?imageLeft->header.stamp:imageRight->header.stamp;
@@ -186,6 +191,21 @@ private:
 				msg.depth = *imageRight;
 				rgbdImagePub_.publish(msg);
 			}
+
+			if( leftStamp != imageLeft->header.stamp.toSec() ||
+				rightStamp != imageRight->header.stamp.toSec() ||
+				leftInfoStamp != cameraInfoLeft->header.stamp.toSec() ||
+				rightInfoStamp != cameraInfoRight->header.stamp.toSec())
+			{
+				NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
+						"sure the node publishing the topics doesn't override the same data after publishing them. A "
+						"solution is to use this node within another nodelet manager. Stamps: "
+						"left%f->%f right=%f->%f info_left=%f->%f info_right=%f->%f",
+						leftStamp, imageLeft->header.stamp.toSec(),
+						rightStamp, imageRight->header.stamp.toSec(),
+						leftInfoStamp, cameraInfoLeft->header.stamp.toSec(),
+						rightInfoStamp, cameraInfoRight->header.stamp.toSec());
+			}
 		}
 	}
 

src/nodelets/stereo_throttle.cpp

@@ -138,38 +138,11 @@ private:
 
 		last_update_ = ros::Time::now();
 
-		if(imageLeftPub_.getNumSubscribers())
-		{
-			if(decimation_ > 1)
-			{
-				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageLeft);
-				cv_bridge::CvImage out;
-				out.header = imagePtr->header;
-				out.encoding = imagePtr->encoding;
-				out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
-				imageLeftPub_.publish(out.toImageMsg());
-			}
-			else
-			{
-				imageLeftPub_.publish(imageLeft);
-			}
-		}
-		if(imageRightPub_.getNumSubscribers())
-		{
-			if(decimation_ > 1)
-			{
-				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageRight);
-				cv_bridge::CvImage out;
-				out.header = imagePtr->header;
-				out.encoding = imagePtr->encoding;
-				out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
-				imageRightPub_.publish(out.toImageMsg());
-			}
-			else
-			{
-				imageRightPub_.publish(imageRight);
-			}
-		}
+		double leftStamp = imageLeft->header.stamp.toSec();
+		double rightStamp = imageRight->header.stamp.toSec();
+		double leftInfoStamp = camInfoLeft->header.stamp.toSec();
+		double rightInfoStamp = camInfoRight->header.stamp.toSec();
+
 		if(infoLeftPub_.getNumSubscribers())
 		{
 			if(decimation_ > 1)
@@ -220,6 +193,55 @@ private:
 				infoRightPub_.publish(camInfoRight);
 			}
 		}
+
+
+		if(imageLeftPub_.getNumSubscribers())
+		{
+			if(decimation_ > 1)
+			{
+				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageLeft);
+				cv_bridge::CvImage out;
+				out.header = imagePtr->header;
+				out.encoding = imagePtr->encoding;
+				out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
+				imageLeftPub_.publish(out.toImageMsg());
+			}
+			else
+			{
+				imageLeftPub_.publish(imageLeft);
+			}
+		}
+		if(imageRightPub_.getNumSubscribers())
+		{
+			if(decimation_ > 1)
+			{
+				cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageRight);
+				cv_bridge::CvImage out;
+				out.header = imagePtr->header;
+				out.encoding = imagePtr->encoding;
+				out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
+				imageRightPub_.publish(out.toImageMsg());
+			}
+			else
+			{
+				imageRightPub_.publish(imageRight);
+			}
+		}
+
+		if( leftStamp != imageLeft->header.stamp.toSec() ||
+			rightStamp != imageRight->header.stamp.toSec() ||
+			leftInfoStamp != camInfoLeft->header.stamp.toSec() ||
+			rightInfoStamp != camInfoRight->header.stamp.toSec())
+		{
+			NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
+					"sure the node publishing the topics doesn't override the same data after publishing them. A "
+					"solution is to use this node within another nodelet manager. Stamps: "
+					"left%f->%f right=%f->%f info_left=%f->%f info_right=%f->%f",
+					leftStamp, imageLeft->header.stamp.toSec(),
+					rightStamp, imageRight->header.stamp.toSec(),
+					leftInfoStamp, camInfoLeft->header.stamp.toSec(),
+					rightInfoStamp, camInfoRight->header.stamp.toSec());
+		}
 	}
 
 	image_transport::Publisher imageLeftPub_;