Merge branch 'devel' of https://github.com/introlab/rtabmap_ros

.travis.yml

@@ -33,6 +33,7 @@ script:
     - mv rtabmap_ros catkin_ws/src/.
     - git clone https://github.com/introlab/rtabmap.git
     - cd rtabmap
+    - if [ "$TRAVIS_BRANCH" = "devel" ]; then git checkout devel; fi
     - mkdir -p build && cd build
     - cmake -DCMAKE_INSTALL_PREFIX=~/build/introlab/catkin_ws/devel .. 
     - make

CMakeLists.txt

@@ -18,7 +18,7 @@ find_package(rviz)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-find_package(RTABMap 0.12.5 REQUIRED)
+find_package(RTABMap 0.13.0 REQUIRED)
 
 find_package(OpenCV REQUIRED)
 

include/rtabmap_ros/CoreWrapper.h

@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <std_msgs/Empty.h>
 #include <std_msgs/Int32.h>
 #include <nav_msgs/GetMap.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
 
 #include <rtabmap/core/Parameters.h>
 #include <rtabmap/core/Rtabmap.h>
@@ -116,6 +117,7 @@ private:
 	void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom
 
 	void userDataAsyncCallback(const rtabmap_ros::UserDataConstPtr & dataMsg);
+	void globalPoseAsyncCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & globalPoseMsg);
 
 	void goalCommonCallback(int id, const std::string & label, const rtabmap::Transform & pose, const ros::Time & stamp, double * planningTime = 0);
 	void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
@@ -127,8 +129,7 @@ private:
 			const rtabmap::SensorData & data,
 			const rtabmap::Transform & odom = rtabmap::Transform(),
 			const std::string & odomFrameId = "",
-			float odomRotationalVariance = 1.0,
-			float odomTransitionalVariance = 1.0);
+			const cv::Mat & odomCovariance = cv::Mat::eye(6,6,CV_64FC1));
 
 	bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
@@ -175,8 +176,7 @@ private:
 	rtabmap::Transform lastPose_;
 	ros::Time lastPoseStamp_;
 	bool lastPoseIntermediate_;
-	float rotVariance_;
-	float transVariance_;
+	cv::Mat covariance_;
 	rtabmap::Transform currentMetricGoal_;
 	rtabmap::Transform lastPublishedMetricGoal_;
 	bool latestNodeWasReached_;
@@ -258,6 +258,9 @@ private:
 	ros::Subscriber userDataAsyncSub_;
 	cv::Mat userData_;
 
+	ros::Subscriber globalPoseAsyncSub_;
+	geometry_msgs::PoseWithCovarianceStamped globalPose_;
+
 	bool stereoToDepth_;
 	bool odomSensorSync_;
 	float rate_;

msg/Link.msg

@@ -4,12 +4,11 @@
 #   int to;
 #   Type type;
 #   Transform transform;
-#   float variance;
+#   cv::Mat(6,6,CV_64FC1) information;
 #}
 
 int32 fromId
 int32 toId
 int32 type
 geometry_msgs/Transform transform
-float32 rotVariance
-float32 transVariance
+float64[36] information

msg/OdomInfo.msg

@@ -5,8 +5,7 @@ bool lost
 int32 matches
 int32 inliers
 float32 icpInliersRatio
-float32 varianceLin
-float32 varianceAng
+float64[36] covariance
 int32 features
 int32 localMapSize
 int32 localScanMapSize

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap_ros</name>
-  <version>0.12.2</version>
+  <version>0.13.0</version>
   <description>RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

src/CoreWrapper.cpp

@@ -83,8 +83,6 @@ CoreWrapper::CoreWrapper() :
 		paused_(false),
 		lastPose_(Transform::getIdentity()),
 		lastPoseIntermediate_(false),
-		rotVariance_(0),
-		transVariance_(0),
 		latestNodeWasReached_(false),
 		frameId_("base_link"),
 		odomFrameId_(""),
@@ -114,6 +112,7 @@ CoreWrapper::CoreWrapper() :
 		previousStamp_(0),
 		mbClient_("move_base", true)
 {
+	globalPose_.header.stamp = ros::Time(0);
 }
 
 void CoreWrapper::onInit()
@@ -493,6 +492,7 @@ void CoreWrapper::onInit()
 	}
 
 	userDataAsyncSub_ = nh.subscribe("user_data_async", 1, &CoreWrapper::userDataAsyncCallback, this);
+	globalPoseAsyncSub_ = nh.subscribe("global_pose", 1, &CoreWrapper::globalPoseAsyncCallback, this);
 }
 
 CoreWrapper::~CoreWrapper()
@@ -641,8 +641,7 @@ bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
 		{
 			UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", MAX(odomMsg->pose.covariance[0], odomMsg->twist.covariance[0]));
 			rtabmap_.triggerNewMap();
-			rotVariance_ = 0;
-			transVariance_ = 0;
+			covariance_ = cv::Mat();
 		}
 
 		lastPoseIntermediate_ = false;
@@ -652,28 +651,29 @@ bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
 		// Only update variance if odom is not null
 		if(!odom.isNull())
 		{
-			// using MIN in case of 3DoF mapping (maybe no parameters are set, except x and yaw for the twist)
-			float transVariance = uMax3(odomMsg->twist.covariance[0], MIN(odomMsg->twist.covariance[7], BAD_COVARIANCE), MIN(odomMsg->twist.covariance[14], BAD_COVARIANCE));
-			float rotVariance = uMax3(MIN(odomMsg->twist.covariance[21],BAD_COVARIANCE), MIN(odomMsg->twist.covariance[28], BAD_COVARIANCE), odomMsg->twist.covariance[35]);
-
-			if(transVariance == BAD_COVARIANCE)
+			cv::Mat covariance;
+			float variance = odomMsg->twist.covariance[0];
+			if(variance == BAD_COVARIANCE)
 			{
 				//use the one of the pose
-				transVariance = uMax3(odomMsg->pose.covariance[0]/2.0, MIN(odomMsg->pose.covariance[7]/2.0, BAD_COVARIANCE), MIN(odomMsg->pose.covariance[14]/2.0, BAD_COVARIANCE));
+				covariance = cv::Mat(6,6,CV_64FC1, (void*)odomMsg->pose.covariance.data()).clone();
+				covariance /= 2.0;
 			}
-			if(rotVariance == BAD_COVARIANCE)
+			else
 			{
-				//use the one of the pose
-				rotVariance = uMax3(MIN(odomMsg->pose.covariance[21]/2.0,BAD_COVARIANCE), MIN(odomMsg->pose.covariance[28]/2.0, BAD_COVARIANCE), odomMsg->pose.covariance[35]/2.0);
+				covariance = cv::Mat(6,6,CV_64FC1, (void*)odomMsg->twist.covariance.data()).clone();
 			}
 
-			if(uIsFinite(rotVariance) && rotVariance != 1.0f)
+			if(uIsFinite(covariance.at<double>(0,0)) && covariance.at<double>(0,0) != 1.0 && covariance.at<double>(0,0)>0.0)
 			{
-				rotVariance_ += rotVariance;
-			}
-			if(uIsFinite(transVariance) && transVariance != 1.0f)
-			{
-				transVariance_ += transVariance;
+				if(covariance_.empty())
+				{
+					covariance_ = covariance;
+				}
+				else
+				{
+					covariance_ += covariance;
+				}
 			}
 		}
 
@@ -724,8 +724,7 @@ bool CoreWrapper::odomTFUpdate(const ros::Time & stamp)
 		{
 			UWARN("Odometry is reset (identity pose detected). Increment map id!");
 			rtabmap_.triggerNewMap();
-			rotVariance_ = 0;
-			transVariance_ = 0;
+			covariance_ = cv::Mat();
 		}
 
 		lastPoseIntermediate_ = false;
@@ -931,7 +930,7 @@ void CoreWrapper::commonDepthCallbackImpl(
 		userData = rtabmap_ros::userDataFromROS(*userDataMsg);
 		if(!userData_.empty())
 		{
-			ROS_WARN("Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!");
+			NODELET_WARN("Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!");
 			userData_ = cv::Mat();
 		}
 	}
@@ -940,6 +939,9 @@ void CoreWrapper::commonDepthCallbackImpl(
 		userData = userData_;
 		userData_ = cv::Mat();
 	}
+
+
+
 	SensorData data(scan,
 			LaserScanInfo(
 					scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():(genScan_?genMaxScanPts:scan3dMsg.get() != 0?scanCloudMaxPoints_:0),
@@ -953,14 +955,51 @@ void CoreWrapper::commonDepthCallbackImpl(
 			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);
+
 	process(lastPoseStamp_,
 			data,
 			lastPose_,
 			odomFrameId,
-			rotVariance_,
-			transVariance_);
-	rotVariance_ = 0;
-	transVariance_ = 0;
+			covariance_);
+	covariance_ = cv::Mat();
 }
 
 void CoreWrapper::commonStereoCallback(
@@ -1147,11 +1186,9 @@ void CoreWrapper::commonStereoCallback(
 			data,
 			lastPose_,
 			odomFrameId,
-			rotVariance_,
-			transVariance_);
+			covariance_);
 
-	rotVariance_ = 0;
-	transVariance_ = 0;
+	covariance_ = cv::Mat();
 }
 
 void CoreWrapper::process(
@@ -1159,8 +1196,7 @@ void CoreWrapper::process(
 		const SensorData & data,
 		const Transform & odom,
 		const std::string & odomFrameId,
-		float odomRotationalVariance,
-		float odomTransitionalVariance)
+		const cv::Mat & odomCovariance)
 {
 	UTimer timer;
 	if(rtabmap_.isIDsGenerated() || data.id() > 0)
@@ -1169,16 +1205,25 @@ void CoreWrapper::process(
 		double timeUpdateMaps = 0.0;
 		double timePublishMaps = 0.0;
 
-		if(!uIsFinite(odomRotationalVariance) || odomRotationalVariance<=0.0f)
+		cv::Mat covariance = odomCovariance;
+		if(covariance.empty() || !uIsFinite(covariance.at<double>(0,0)) || covariance.at<double>(0,0)<=0.0f)
 		{
-			odomRotationalVariance = odomDefaultAngVariance_;
-		}
-		if(!uIsFinite(odomTransitionalVariance) || odomTransitionalVariance<=0.0f)
-		{
-			odomTransitionalVariance = odomDefaultLinVariance_;
+			covariance = cv::Mat::eye(6,6,CV_64FC1);
+			if(odomDefaultLinVariance_ > 0.0f)
+			{
+				covariance.at<double>(0,0) = odomDefaultLinVariance_;
+				covariance.at<double>(1,1) = odomDefaultLinVariance_;
+				covariance.at<double>(2,2) = odomDefaultLinVariance_;
+			}
+			if(odomDefaultAngVariance_ > 0.0f)
+			{
+				covariance.at<double>(3,3) = odomDefaultAngVariance_;
+				covariance.at<double>(4,4) = odomDefaultAngVariance_;
+				covariance.at<double>(5,5) = odomDefaultAngVariance_;
+			}
 		}
 
-		if(rtabmap_.process(data, odom, OdometryEvent::generateCovarianceMatrix(odomRotationalVariance, odomTransitionalVariance)))
+		if(rtabmap_.process(data, odom, covariance))
 		{
 			timeRtabmap = timer.ticks();
 			mapToOdomMutex_.lock();
@@ -1339,6 +1384,14 @@ void CoreWrapper::userDataAsyncCallback(const rtabmap_ros::UserDataConstPtr & da
 	}
 }
 
+void CoreWrapper::globalPoseAsyncCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & globalPoseMsg)
+{
+	if(!paused_)
+	{
+		globalPose_ = *globalPoseMsg;
+	}
+}
+
 void CoreWrapper::goalCommonCallback(
 		int id,
 		const std::string & label,
@@ -1546,8 +1599,7 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
 {
 	NODELET_INFO("rtabmap: Reset");
 	rtabmap_.resetMemory();
-	rotVariance_ = 0;
-	transVariance_ = 0;
+	covariance_ = cv::Mat();
 	lastPose_.setIdentity();
 	lastPoseIntermediate_ = false;
 	currentMetricGoal_.setNull();
@@ -1556,6 +1608,7 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
 	mapsManager_.clear();
 	previousStamp_ = ros::Time(0);
 	userData_ = cv::Mat();
+	globalPose_.header.stamp = ros::Time(0);
 	return true;
 }
 
@@ -1604,13 +1657,13 @@ bool CoreWrapper::backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Em
 	rtabmap_.close();
 	NODELET_INFO("Backup: Saving memory... done!");
 
-	rotVariance_ = 0;
-	transVariance_ = 0;
+	covariance_ = cv::Mat();
 	lastPose_.setIdentity();
 	currentMetricGoal_.setNull();
 	lastPublishedMetricGoal_.setNull();
 	latestNodeWasReached_ = false;
 	userData_ = cv::Mat();
+	globalPose_.header.stamp = ros::Time(0);
 
 	NODELET_INFO("Backup: Saving \"%s\" to \"%s\"...", databasePath_.c_str(), (databasePath_+".back").c_str());
 	UFile::copy(databasePath_, databasePath_+".back");

src/DbPlayerNode.cpp

@@ -163,9 +163,11 @@ int main(int argc, char** argv)
 	tf2_ros::TransformBroadcaster tfBroadcaster;
 
 	UTimer timer;
-	rtabmap::CameraInfo info;
-	rtabmap::SensorData data = reader.takeImage(&info);
-	rtabmap::OdometryEvent odom(data, info.odomPose, info.odomCovariance);
+	rtabmap::CameraInfo cameraInfo;
+	rtabmap::SensorData data = reader.takeImage(&cameraInfo);
+	rtabmap::OdometryInfo odomInfo;
+	odomInfo.covariance = cameraInfo.odomCovariance;
+	rtabmap::OdometryEvent odom(data, cameraInfo.odomPose, odomInfo);
 	double acquisitionTime = timer.ticks();
 	while(ros::ok() && odom.data().id())
 	{
@@ -490,9 +492,10 @@ int main(int argc, char** argv)
 		}
 
 		timer.restart();
-		info = rtabmap::CameraInfo();
-		data = reader.takeImage(&info);
-		odom = rtabmap::OdometryEvent(data, info.odomPose, info.odomCovariance);
+		cameraInfo = rtabmap::CameraInfo();
+		data = reader.takeImage(&cameraInfo);
+		odomInfo.covariance = cameraInfo.odomCovariance;
+		odom = rtabmap::OdometryEvent(data, cameraInfo.odomPose, odomInfo);
 		acquisitionTime = timer.ticks();
 	}
 

src/GuiWrapper.cpp

@@ -564,6 +564,7 @@ void GuiWrapper::commonDepthCallback(
 		return;
 	}
 
+	info.covariance = covariance;
 	rtabmap::OdometryEvent odomEvent(
 		rtabmap::SensorData(
 				scan,
@@ -577,7 +578,6 @@ void GuiWrapper::commonDepthCallback(
 				odomHeader.seq,
 				rtabmap_ros::timestampFromROS(odomHeader.stamp)),
 		odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
-		covariance,
 		info);
 
 	QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData));
@@ -720,6 +720,7 @@ void GuiWrapper::commonStereoCallback(
 		return;
 	}
 
+	info.covariance = covariance;
 	rtabmap::OdometryEvent odomEvent(
 		rtabmap::SensorData(
 				scan,
@@ -733,7 +734,6 @@ void GuiWrapper::commonStereoCallback(
 				odomHeader.seq,
 				rtabmap_ros::timestampFromROS(odomHeader.stamp)),
 		odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
-		covariance,
 		info);
 
 	QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData));

src/MsgConversion.cpp

@@ -301,7 +301,8 @@ void infoToROS(const rtabmap::Statistics & stats, rtabmap_ros::Info & info)
 
 rtabmap::Link linkFromROS(const rtabmap_ros::Link & msg)
 {
-	return rtabmap::Link(msg.fromId, msg.toId, (rtabmap::Link::Type)msg.type, transformFromGeometryMsg(msg.transform), msg.rotVariance, msg.transVariance);
+	cv::Mat information = cv::Mat(6,6,CV_64FC1, (void*)msg.information.data()).clone();
+	return rtabmap::Link(msg.fromId, msg.toId, (rtabmap::Link::Type)msg.type, transformFromGeometryMsg(msg.transform), information);
 }
 
 void linkToROS(const rtabmap::Link & link, rtabmap_ros::Link & msg)
@@ -309,8 +310,10 @@ void linkToROS(const rtabmap::Link & link, rtabmap_ros::Link & msg)
 	msg.fromId = link.from();
 	msg.toId = link.to();
 	msg.type = link.type();
-	msg.rotVariance = link.rotVariance();
-	msg.transVariance = link.transVariance();
+	if(link.infMatrix().type() == CV_64FC1 && link.infMatrix().cols == 6 && link.infMatrix().rows == 6)
+	{
+		memcpy(msg.information.data(), link.infMatrix().data, 36*sizeof(double));
+	}
 	transformToGeometryMsg(link.transform(), msg.transform);
 }
 
@@ -883,8 +886,7 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
 	info.matches = msg.matches;
 	info.inliers = msg.inliers;
 	info.icpInliersRatio = msg.icpInliersRatio;
-	info.varianceLin = msg.varianceLin;
-	info.varianceAng = msg.varianceAng;
+	info.covariance = cv::Mat(6,6,CV_64FC1, (void*)msg.covariance.data()).clone();
 	info.features = msg.features;
 	info.localMapSize = msg.localMapSize;
 	info.localScanMapSize = msg.localScanMapSize;
@@ -934,8 +936,10 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
 	msg.matches = info.matches;
 	msg.inliers = info.inliers;
 	msg.icpInliersRatio = info.icpInliersRatio;
-	msg.varianceLin = info.varianceLin;
-	msg.varianceAng = info.varianceAng;
+	if(info.covariance.type() == CV_64FC1 && info.covariance.cols == 6 && info.covariance.rows == 6)
+	{
+		memcpy(msg.covariance.data(), info.covariance.data, 36*sizeof(double));
+	}
 	msg.features = info.features;
 	msg.localMapSize = info.localMapSize;
 	msg.localScanMapSize = info.localScanMapSize;
@@ -1046,7 +1050,7 @@ rtabmap::Transform getTransform(
 	}
 	catch(tf::TransformException & ex)
 	{
-		ROS_WARN("%s",ex.what());
+		ROS_WARN("(getting transform %s -> %s) %s", fromFrameId.c_str(), toFrameId.c_str(), ex.what());
 	}
 	return transform;
 }
@@ -1083,7 +1087,7 @@ rtabmap::Transform getTransform(
 	}
 	catch(tf::TransformException & ex)
 	{
-		ROS_WARN("%s",ex.what());
+		ROS_WARN("(getting transform movement of %s according to fixed %s) %s", sourceTargetFrame.c_str(), fixedFrame.c_str(), ex.what());
 	}
 	return transform;
 }
@@ -1124,7 +1128,8 @@ bool convertRGBDMsgs(
 			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
 			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
 			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
-			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
+			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
+			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
 			!(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
 			 depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
 			 depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))

src/OdometryROS.cpp

@@ -431,12 +431,12 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 
 			//set covariance
 			// libviso2 uses approximately vel variance * 2
-			odom.pose.covariance.at(0) = info.varianceLin*2;  // xx
-			odom.pose.covariance.at(7) = info.varianceLin*2;  // yy
-			odom.pose.covariance.at(14) = info.varianceLin*2; // zz
-			odom.pose.covariance.at(21) = info.varianceAng*2; // rr
-			odom.pose.covariance.at(28) = info.varianceAng*2; // pp
-			odom.pose.covariance.at(35) = info.varianceAng*2; // yawyaw
+			odom.pose.covariance.at(0) = info.covariance.at<double>(0,0)*2;  // xx
+			odom.pose.covariance.at(7) = info.covariance.at<double>(1,1)*2;  // yy
+			odom.pose.covariance.at(14) = info.covariance.at<double>(2,2)*2; // zz
+			odom.pose.covariance.at(21) = info.covariance.at<double>(3,3)*2; // rr
+			odom.pose.covariance.at(28) = info.covariance.at<double>(4,4)*2; // pp
+			odom.pose.covariance.at(35) = info.covariance.at<double>(5,5)*2; // yawyaw
 
 			//set velocity
 			bool setTwist = !odometry_->previousVelocityTransform().isNull();
@@ -452,12 +452,12 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 				odom.twist.twist.angular.z = yaw;
 			}
 
-			odom.twist.covariance.at(0) = setTwist?info.varianceLin:BAD_COVARIANCE;  // xx
-			odom.twist.covariance.at(7) = setTwist?info.varianceLin:BAD_COVARIANCE;  // yy
-			odom.twist.covariance.at(14) = setTwist?info.varianceLin:BAD_COVARIANCE; // zz
-			odom.twist.covariance.at(21) = setTwist?info.varianceAng:BAD_COVARIANCE; // rr
-			odom.twist.covariance.at(28) = setTwist?info.varianceAng:BAD_COVARIANCE; // pp
-			odom.twist.covariance.at(35) = setTwist?info.varianceAng:BAD_COVARIANCE; // yawyaw
+			odom.twist.covariance.at(0) = setTwist?info.covariance.at<double>(0,0):BAD_COVARIANCE;  // xx
+			odom.twist.covariance.at(7) = setTwist?info.covariance.at<double>(1,1):BAD_COVARIANCE;  // yy
+			odom.twist.covariance.at(14) = setTwist?info.covariance.at<double>(2,2):BAD_COVARIANCE; // zz
+			odom.twist.covariance.at(21) = setTwist?info.covariance.at<double>(3,3):BAD_COVARIANCE; // rr
+			odom.twist.covariance.at(28) = setTwist?info.covariance.at<double>(4,4):BAD_COVARIANCE; // pp
+			odom.twist.covariance.at(35) = setTwist?info.covariance.at<double>(5,5):BAD_COVARIANCE; // yawyaw
 
 			//publish the message
 			odomPub_.publish(odom);
@@ -608,16 +608,16 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 	{
 		if(icpParams_)
 		{
-			NODELET_INFO( "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs", info.inliers, info.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.varianceLin), pose.isNull()?0.0f:std::sqrt(info.varianceAng), (ros::WallTime::now()-time).toSec());
+			NODELET_INFO( "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs", info.inliers, info.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
 		}
 		else
 		{
-			NODELET_INFO( "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.inliers, pose.isNull()?0.0f:std::sqrt(info.varianceLin), pose.isNull()?0.0f:std::sqrt(info.varianceAng), (ros::WallTime::now()-time).toSec());
+			NODELET_INFO( "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.inliers, pose.isNull()?0.0f:std::sqrt(info.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
 		}
 	}
 	else
 	{
-		NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.varianceLin), pose.isNull()?0.0f:std::sqrt(info.varianceAng), (ros::WallTime::now()-time).toSec());
+		NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
 	}
 
 }

src/nodelets/point_cloud_xyzrgb.cpp

@@ -200,9 +200,12 @@ private:
 	{
 		if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
 			image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
-			image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
+			image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
 			image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
-			image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) &&
+			image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
+			image->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
+			image->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
+			image->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
 		   !(imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 ||
 			 imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 ||
 			 imageDepth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0))

src/nodelets/rgbd_odometry.cpp

@@ -234,7 +234,8 @@ private:
 				 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
 				 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
 				 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
-				 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
+				 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
+				 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
 				!(depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
 				 depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
 				 depthImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))

src/nodelets/rgbdicp_odometry.cpp

@@ -228,7 +228,10 @@ private:
 				 image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
 				 image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
 				 image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
-				 image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
+				 image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
+				 image->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
+				 image->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
+				 image->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
 			   !(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 ||
 				 depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 ||
 				 depth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0))