diff --git a/msg/OdomInfo.msg b/msg/OdomInfo.msg
index a7f07257..76e65c7b 100644
--- a/msg/OdomInfo.msg
+++ b/msg/OdomInfo.msg
@@ -5,6 +5,9 @@ bool lost
 int32 matches
 int32 inliers
 float32 icpInliersRatio
+float32 icpRotation
+float32 icpTranslation
+float32 icpStructuralComplexity
 float64[36] covariance
 int32 features
 int32 localMapSize
diff --git a/src/DbPlayerNode.cpp b/src/DbPlayerNode.cpp
index 94bd55df..d7d60c15 100644
--- a/src/DbPlayerNode.cpp
+++ b/src/DbPlayerNode.cpp
@@ -166,7 +166,7 @@ int main(int argc, char** argv)
 	rtabmap::CameraInfo cameraInfo;
 	rtabmap::SensorData data = reader.takeImage(&cameraInfo);
 	rtabmap::OdometryInfo odomInfo;
-	odomInfo.covariance = cameraInfo.odomCovariance;
+	odomInfo.reg.covariance = cameraInfo.odomCovariance;
 	rtabmap::OdometryEvent odom(data, cameraInfo.odomPose, odomInfo);
 	double acquisitionTime = timer.ticks();
 	while(ros::ok() && odom.data().id())
@@ -494,7 +494,7 @@ int main(int argc, char** argv)
 		timer.restart();
 		cameraInfo = rtabmap::CameraInfo();
 		data = reader.takeImage(&cameraInfo);
-		odomInfo.covariance = cameraInfo.odomCovariance;
+		odomInfo.reg.covariance = cameraInfo.odomCovariance;
 		odom = rtabmap::OdometryEvent(data, cameraInfo.odomPose, odomInfo);
 		acquisitionTime = timer.ticks();
 	}
diff --git a/src/GuiWrapper.cpp b/src/GuiWrapper.cpp
index d63ebdaf..b7190d93 100644
--- a/src/GuiWrapper.cpp
+++ b/src/GuiWrapper.cpp
@@ -569,7 +569,7 @@ void GuiWrapper::commonDepthCallback(
 		return;
 	}
 
-	info.covariance = covariance;
+	info.reg.covariance = covariance;
 	rtabmap::OdometryEvent odomEvent(
 		rtabmap::SensorData(
 				scan,
@@ -724,7 +724,7 @@ void GuiWrapper::commonStereoCallback(
 		return;
 	}
 
-	info.covariance = covariance;
+	info.reg.covariance = covariance;
 	rtabmap::OdometryEvent odomEvent(
 		rtabmap::SensorData(
 				scan,
diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp
index f9bc69c0..7a4c75a2 100644
--- a/src/MsgConversion.cpp
+++ b/src/MsgConversion.cpp
@@ -884,10 +884,13 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
 {
 	rtabmap::OdometryInfo info;
 	info.lost = msg.lost;
-	info.matches = msg.matches;
-	info.inliers = msg.inliers;
-	info.icpInliersRatio = msg.icpInliersRatio;
-	info.covariance = cv::Mat(6,6,CV_64FC1, (void*)msg.covariance.data()).clone();
+	info.reg.matches = msg.matches;
+	info.reg.inliers = msg.inliers;
+	info.reg.icpInliersRatio = msg.icpInliersRatio;
+	info.reg.icpRotation = msg.icpRotation;
+	info.reg.icpTranslation = msg.icpTranslation;
+	info.reg.icpStructuralComplexity = msg.icpStructuralComplexity;
+	info.reg.covariance = cv::Mat(6,6,CV_64FC1, (void*)msg.covariance.data()).clone();
 	info.features = msg.features;
 	info.localMapSize = msg.localMapSize;
 	info.localScanMapSize = msg.localScanMapSize;
@@ -910,8 +913,8 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
 		info.words.insert(std::make_pair(msg.wordsKeys[i], keypointFromROS(msg.wordsValues[i])));
 	}
 
-	info.wordMatches = msg.wordMatches;
-	info.wordInliers = msg.wordInliers;
+	info.reg.matchesIDs = msg.wordMatches;
+	info.reg.inliersIDs = msg.wordInliers;
 
 	info.refCorners = points2fFromROS(msg.refCorners);
 	info.newCorners = points2fFromROS(msg.newCorners);
@@ -934,12 +937,15 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
 void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg)
 {
 	msg.lost = info.lost;
-	msg.matches = info.matches;
-	msg.inliers = info.inliers;
-	msg.icpInliersRatio = info.icpInliersRatio;
-	if(info.covariance.type() == CV_64FC1 && info.covariance.cols == 6 && info.covariance.rows == 6)
+	msg.matches = info.reg.matches;
+	msg.inliers = info.reg.inliers;
+	msg.icpInliersRatio = info.reg.icpInliersRatio;
+	msg.icpRotation = info.reg.icpRotation;
+	msg.icpTranslation = info.reg.icpTranslation;
+	msg.icpStructuralComplexity = info.reg.icpStructuralComplexity;
+	if(info.reg.covariance.type() == CV_64FC1 && info.reg.covariance.cols == 6 && info.reg.covariance.rows == 6)
 	{
-		memcpy(msg.covariance.data(), info.covariance.data, 36*sizeof(double));
+		memcpy(msg.covariance.data(), info.reg.covariance.data, 36*sizeof(double));
 	}
 	msg.features = info.features;
 	msg.localMapSize = info.localMapSize;
@@ -960,8 +966,8 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
 	msg.wordsKeys = uKeys(info.words);
 	keypointsToROS(uValues(info.words), msg.wordsValues);
 
-	msg.wordMatches = info.wordMatches;
-	msg.wordInliers = info.wordInliers;
+	msg.wordMatches = info.reg.matchesIDs;
+	msg.wordInliers = info.reg.inliersIDs;
 
 	points2fToROS(info.refCorners, msg.refCorners);
 	points2fToROS(info.newCorners, msg.newCorners);
diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp
index 2e51922d..12c2c9b3 100644
--- a/src/OdometryROS.cpp
+++ b/src/OdometryROS.cpp
@@ -449,12 +449,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.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
+			odom.pose.covariance.at(0) = info.reg.covariance.at<double>(0,0)*2;  // xx
+			odom.pose.covariance.at(7) = info.reg.covariance.at<double>(1,1)*2;  // yy
+			odom.pose.covariance.at(14) = info.reg.covariance.at<double>(2,2)*2; // zz
+			odom.pose.covariance.at(21) = info.reg.covariance.at<double>(3,3)*2; // rr
+			odom.pose.covariance.at(28) = info.reg.covariance.at<double>(4,4)*2; // pp
+			odom.pose.covariance.at(35) = info.reg.covariance.at<double>(5,5)*2; // yawyaw
 
 			//set velocity
 			bool setTwist = !odometry_->previousVelocityTransform().isNull();
@@ -470,12 +470,12 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 				odom.twist.twist.angular.z = yaw;
 			}
 
-			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
+			odom.twist.covariance.at(0) = setTwist?info.reg.covariance.at<double>(0,0):BAD_COVARIANCE;  // xx
+			odom.twist.covariance.at(7) = setTwist?info.reg.covariance.at<double>(1,1):BAD_COVARIANCE;  // yy
+			odom.twist.covariance.at(14) = setTwist?info.reg.covariance.at<double>(2,2):BAD_COVARIANCE; // zz
+			odom.twist.covariance.at(21) = setTwist?info.reg.covariance.at<double>(3,3):BAD_COVARIANCE; // rr
+			odom.twist.covariance.at(28) = setTwist?info.reg.covariance.at<double>(4,4):BAD_COVARIANCE; // pp
+			odom.twist.covariance.at(35) = setTwist?info.reg.covariance.at<double>(5,5):BAD_COVARIANCE; // yawyaw
 
 			//publish the message
 			odomPub_.publish(odom);
@@ -539,8 +539,6 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 					cloudMsg.header.frame_id = odomFrameId_;
 					odomLastFrame_.publish(cloudMsg);
 				}
-			}else{
-				NODELET_ERROR("ERROR, Wrong Type of Odometry, Shouldn't happen");
 			}
 		}
 
@@ -626,16 +624,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.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
+			NODELET_INFO( "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.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.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
+			NODELET_INFO( "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.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.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
+		NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
 	}
 
 }