CameraRealSense2: updated how imu are published in inter mode and fixed some local transforms. GUI-VINS: features are shown in Odometry view.

2026-09-02 09:30:25 +08:00 · 2019-10-13 14:48:11 -04:00
parent 9cb1e4bbc5
commit 1e298dcfa2
9 changed files with 124 additions and 83 deletions
--- a/corelib/include/rtabmap/core/Parameters.h
+++ b/corelib/include/rtabmap/core/Parameters.h
@@ -413,7 +413,7 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(GTSAM, Optimizer,       int, 1,          "0=Levenberg 1=GaussNewton 2=Dogleg");

    // Odometry
-    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO");
+    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion");
    RTABMAP_PARAM(Odom, ResetCountdown,         int, 0,       "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
    RTABMAP_PARAM(Odom, Holonomic,              bool, true,   "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
    RTABMAP_PARAM(Odom, FillInfoData,           bool, true,   "Fill info with data (inliers/outliers features).");
--- a/corelib/include/rtabmap/core/camera/CameraRealSense2.h
+++ b/corelib/include/rtabmap/core/camera/CameraRealSense2.h
@@ -90,7 +90,8 @@ public:
 			const double & stamp,
 			Transform & pose,
 			unsigned int & poseConfidence,
-			IMU & imu) const;
+			IMU & imu,
+			int maxWaitTimeMs = 35) const;
 #endif

 protected:
--- a/corelib/include/rtabmap/core/odometry/OdometryMSCKF.h
+++ b/corelib/include/rtabmap/core/odometry/OdometryMSCKF.h
@@ -55,7 +55,7 @@ private:
 	MsckfVioNoROS * msckf_;
 	IMU lastImu_;
 	ParametersMap parameters_;
-	Transform flipXY_;
+	Transform fixPoseRotation_;
 	Transform previousPose_;
 	bool initGravity_;
 #endif
--- a/corelib/src/camera/CameraRealSense2.cpp
+++ b/corelib/src/camera/CameraRealSense2.cpp
@@ -216,45 +216,45 @@ void CameraRealSense2::imu_callback(rs2::frame frame)
 	UScopeMutex sm(imuMutex_);
 	if(stream == RS2_STREAM_GYRO)
 	{
-		gyroBuffer_.insert(gyroBuffer_.end(), std::make_pair(hostStartStamp_ == 0?UTimer::now():frame.get_timestamp(), crnt_reading));
-		if(gyroBuffer_.size() > 100)
+		gyroBuffer_.insert(gyroBuffer_.end(), std::make_pair(frame.get_timestamp(), crnt_reading));
+		if(gyroBuffer_.size() > 1000)
 		{
 			gyroBuffer_.erase(gyroBuffer_.begin());
 		}
 	}
 	else
 	{
-		accBuffer_.insert(accBuffer_.end(), std::make_pair(hostStartStamp_ == 0?UTimer::now():frame.get_timestamp(), crnt_reading));
-		if(accBuffer_.size() > 100)
+		accBuffer_.insert(accBuffer_.end(), std::make_pair(frame.get_timestamp(), crnt_reading));
+		if(accBuffer_.size() > 1000)
 		{
 			accBuffer_.erase(accBuffer_.begin());
 		}
 	}
 }

+// See https://github.com/IntelRealSense/realsense-ros/blob/2a45f09003c98a5bdf39ee89df032bdb9c9bcd2d/realsense2_camera/src/base_realsense_node.cpp#L1397-L1404
 Transform CameraRealSense2::realsense2PoseRotation_ = Transform(
 		0, 0,-1,0,
 		-1, 0, 0,0,
 		 0, 1, 0,0);
-Transform CameraRealSense2::realsense2PoseRotationInv_ = realsense2PoseRotation_.inverse();

 void CameraRealSense2::pose_callback(rs2::frame frame)
 {
 	rs2_pose pose = frame.as<rs2::pose_frame>().get_pose_data();
+	// See https://github.com/IntelRealSense/realsense-ros/blob/2a45f09003c98a5bdf39ee89df032bdb9c9bcd2d/realsense2_camera/src/base_realsense_node.cpp#L1397-L1404
 	Transform poseT = Transform(
-				pose.translation.x,
+				-pose.translation.z,
+				-pose.translation.x,
 				pose.translation.y,
-				pose.translation.z,
-				pose.rotation.x,
+				-pose.rotation.z,
+				-pose.rotation.x,
 				pose.rotation.y,
-				pose.rotation.z,
 				pose.rotation.w);
-	poseT = realsense2PoseRotation_ * poseT * realsense2PoseRotationInv_;

 	UDEBUG("POSE callback! %f %s (confidence=%d)", frame.get_timestamp(), poseT.prettyPrint().c_str(), (int)pose.tracker_confidence);

 	UScopeMutex sm(poseMutex_);
-	poseBuffer_.insert(poseBuffer_.end(), std::make_pair(hostStartStamp_ == 0?UTimer::now():frame.get_timestamp(), std::make_pair(poseT, pose.tracker_confidence)));
+	poseBuffer_.insert(poseBuffer_.end(), std::make_pair(frame.get_timestamp(), std::make_pair(poseT, pose.tracker_confidence)));
 	if(poseBuffer_.size() > 100)
 	{
 		poseBuffer_.erase(poseBuffer_.begin());
@@ -268,7 +268,7 @@ void CameraRealSense2::frame_callback(rs2::frame frame)
 }
 void CameraRealSense2::multiple_message_callback(rs2::frame frame)
 {
-	if(dev_[1]==0 && frame.get_timestamp() < UTimer::now()+1000000000)
+	if(frame.get_timestamp() < UTimer::now()+1000000000)
 	{
 		// 1) In dual setup, use host time
 		// 2) ISSUE: my D435i reports timestamps for images 50 years in the future,
@@ -307,7 +307,8 @@ void CameraRealSense2::getPoseAndIMU(
 		const double & stamp,
 		Transform & pose,
 		unsigned int & poseConfidence,
-		IMU & imu) const
+		IMU & imu,
+		int maxWaitTimeMs) const
 {
 	pose.setNull();
 	imu = IMU();
@@ -317,14 +318,12 @@ void CameraRealSense2::getPoseAndIMU(
 		return;
 	}

-	int maxWaitTime = 35;
-
 	// Interpolate pose
 	if(!poseBuffer_.empty())
 	{
 		poseMutex_.lock();
 		int waitTry = 0;
-		while(poseBuffer_.rbegin()->first < stamp && waitTry < maxWaitTime)
+		while(maxWaitTimeMs>0 && poseBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
 		{
 			poseMutex_.unlock();
 			++waitTry;
@@ -333,7 +332,10 @@ void CameraRealSense2::getPoseAndIMU(
 		}
 		if(poseBuffer_.rbegin()->first < stamp)
 		{
-			UWARN("Could not find poses to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTime, poseBuffer_.rbegin()->first);
+			if(maxWaitTimeMs > 0)
+			{
+				UWARN("Could not find poses to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, poseBuffer_.rbegin()->first);
+			}
 		}
 		else
 		{
@@ -370,7 +372,7 @@ void CameraRealSense2::getPoseAndIMU(
 	{
 		imuMutex_.lock();
 		int waitTry = 0;
-		while(accBuffer_.rbegin()->first < stamp && waitTry < maxWaitTime)
+		while(maxWaitTimeMs > 0 && accBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
 		{
 			imuMutex_.unlock();
 			++waitTry;
@@ -379,7 +381,10 @@ void CameraRealSense2::getPoseAndIMU(
 		}
 		if(accBuffer_.rbegin()->first < stamp)
 		{
-			UWARN("Could not find acc data to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTime, accBuffer_.rbegin()->first);
+			if(maxWaitTimeMs>0)
+			{
+				UWARN("Could not find acc data to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, accBuffer_.rbegin()->first);
+			}
 			imuMutex_.unlock();
 			return;
 		}
@@ -423,7 +428,7 @@ void CameraRealSense2::getPoseAndIMU(
 	{
 		imuMutex_.lock();
 		int waitTry = 0;
-		while(gyroBuffer_.rbegin()->first < stamp && waitTry < maxWaitTime)
+		while(maxWaitTimeMs>0 && gyroBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
 		{
 			imuMutex_.unlock();
 			++waitTry;
@@ -432,7 +437,10 @@ void CameraRealSense2::getPoseAndIMU(
 		}
 		if(gyroBuffer_.rbegin()->first < stamp)
 		{
-			UWARN("Could not find gyro data to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTime, gyroBuffer_.rbegin()->first);
+			if(maxWaitTimeMs>0)
+			{
+				UWARN("Could not find gyro data to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, gyroBuffer_.rbegin()->first);
+			}
 			imuMutex_.unlock();
 			return;
 		}
@@ -740,19 +748,19 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 					profilesPerSensor[i].push_back(profile);
 					auto intrinsic = video_profile.get_intrinsics();
 					if(pi==0)
-					{
-						// RIGHT FISHEYE
-						depthBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0));
-						depthStreamProfile = profile;
-						*depthIntrinsics_ = intrinsic;
-					}
-					else
 					{
 						// LEFT FISHEYE
 						rgbBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0));
 						rgbStreamProfile = profile;
 						*rgbIntrinsics_ = intrinsic;
 					}
+					else
+					{
+						// RIGHT FISHEYE
+						depthBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0));
+						depthStreamProfile = profile;
+						*depthIntrinsics_ = intrinsic;
+					}
 					added = true;
 				}
 				else if(video_profile.format() == RS2_FORMAT_MOTION_XYZ32F || video_profile.format() == RS2_FORMAT_6DOF)
@@ -797,13 +805,11 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		 if(dualMode_)
 		 {
 			 Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
+			 UINFO("Set base to pose");
 			 this->setLocalTransform(this->getLocalTransform()*opticalTransform.inverse());
 			 UINFO("poseToLeftIR = %s", dualExtrinsics_.prettyPrint().c_str());
-			 if(ir_)
-			 {
-				 this->setLocalTransform(this->getLocalTransform()*dualExtrinsics_*opticalTransform);
-			 }
-			 else
+			 Transform baseToCam = this->getLocalTransform()*dualExtrinsics_*opticalTransform;
+			 if(!ir_)
 			 {
 				 Transform leftIRToRGB(
 						 depthToRGBExtrinsics_->rotation[0], depthToRGBExtrinsics_->rotation[1], depthToRGBExtrinsics_->rotation[2], depthToRGBExtrinsics_->translation[0],
@@ -811,11 +817,11 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 						 depthToRGBExtrinsics_->rotation[6], depthToRGBExtrinsics_->rotation[7], depthToRGBExtrinsics_->rotation[8], depthToRGBExtrinsics_->translation[2]);
 				 leftIRToRGB = leftIRToRGB.inverse();
 				 UINFO("leftIRToRGB = %s", leftIRToRGB.prettyPrint().c_str());
-				 this->setLocalTransform(this->getLocalTransform()*dualExtrinsics_*opticalTransform*leftIRToRGB);
+				 baseToCam *= leftIRToRGB;
 			 }
 			 UASSERT(profilesPerSensor.size()>=2);
 			 UASSERT(profilesPerSensor.back().size() == 3);
-			 rs2_extrinsics poseToIMU = profilesPerSensor.back()[2].get_extrinsics_to(profilesPerSensor.back()[0]);
+			 rs2_extrinsics poseToIMU = profilesPerSensor.back()[0].get_extrinsics_to(profilesPerSensor.back()[2]);

 			 Transform poseToIMUT(
 					 poseToIMU.rotation[0], poseToIMU.rotation[1], poseToIMU.rotation[2], poseToIMU.translation[0],
@@ -824,9 +830,9 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			 poseToIMUT = realsense2PoseRotation_ * poseToIMUT;
 			 UINFO("poseToIMU = %s", poseToIMUT.prettyPrint().c_str());

-			 UINFO("PoseToCam = %s", this->getLocalTransform().prettyPrint().c_str());
-			 model_.setLocalTransform(this->getLocalTransform());
-			 imuLocalTransform_ = poseToIMUT;
+			 UINFO("BaseToCam = %s", baseToCam.prettyPrint().c_str());
+			 model_.setLocalTransform(baseToCam);
+			 imuLocalTransform_ = this->getLocalTransform() * poseToIMUT;
 		 }

 		 if(ir_ && !irDepth_ && profilesPerSensor.size() >= 2 && profilesPerSensor[1].size() >= 2)
@@ -900,19 +906,16 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		}

 		// Get extrinsics with pose as the base frame:
-		// 0=Right fisheye
-		// 1=Left fisheye
+		// 0=Left fisheye
+		// 1=Right fisheye
 		// 2=GYRO
 		// 3=ACC
 		// 4=POSE
 		UASSERT(profilesPerSensor[0].size() == 5);
 		if(odometryProvided_)
 		{
-			rs2_extrinsics poseToLeft = profilesPerSensor[0][4].get_extrinsics_to(profilesPerSensor[0][1]);
-			rs2_extrinsics poseToIMU = profilesPerSensor[0][4].get_extrinsics_to(profilesPerSensor[0][2]);
-			Transform realsense2_pose_rotation(0, 0,-1,0,
-								-1, 0, 0,0,
-								 0, 1, 0,0);
+			rs2_extrinsics poseToLeft = profilesPerSensor[0][0].get_extrinsics_to(profilesPerSensor[0][4]);
+			rs2_extrinsics poseToIMU = profilesPerSensor[0][2].get_extrinsics_to(profilesPerSensor[0][4]);
 			Transform poseToLeftT(
 					poseToLeft.rotation[0], poseToLeft.rotation[1], poseToLeft.rotation[2], poseToLeft.translation[0],
 					poseToLeft.rotation[3], poseToLeft.rotation[4], poseToLeft.rotation[5], poseToLeft.translation[1],
@@ -927,17 +930,16 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			poseToIMUT = realsense2PoseRotation_ * poseToIMUT;
 			UINFO("poseToIMU = %s", poseToIMUT.prettyPrint().c_str());

-			UINFO("Removing optical rotation to match realsense2 poses.");
-			Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
-			this->setLocalTransform(this->getLocalTransform()*opticalTransform.inverse());
+			UINFO("Set base to pose");
+			this->setLocalTransform(this->getLocalTransform()*poseToLeftT.inverse());

 			stereoModel_.setLocalTransform(this->getLocalTransform()*poseToLeftT);
-			imuLocalTransform_ = poseToIMUT;
+			imuLocalTransform_ = this->getLocalTransform()* poseToIMUT;
 		}
 		else
 		{
 			// Set imu transform based on the left camera instead of pose
-			 rs2_extrinsics leftToIMU = profilesPerSensor[0][1].get_extrinsics_to(profilesPerSensor[0][2]);
+			 rs2_extrinsics leftToIMU = profilesPerSensor[0][2].get_extrinsics_to(profilesPerSensor[0][0]);
 			 Transform leftToIMUT(
 					 leftToIMU.rotation[0], leftToIMU.rotation[1], leftToIMU.rotation[2], leftToIMU.translation[0],
 					 leftToIMU.rotation[3], leftToIMU.rotation[4], leftToIMU.rotation[5], leftToIMU.translation[1],
@@ -1091,7 +1093,7 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 		{
 			double stamp;
 			// See ISSUE in multiple_message_callback()
-			if(frameset.get_timestamp() > UTimer::now()+1000000000 || hostStartStamp_ == 0)
+			if(frameset.get_timestamp() >= UTimer::now()+1000000000)
 			{
 				stamp = UTimer::now();
 			}
@@ -1218,20 +1220,25 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)

 			IMU imu;
 			unsigned int confidence = 0;
-			double imuStamp = hostStartStamp_==0?stamp:frameset.get_timestamp()> UTimer::now()+1000000000?stamp*1000.0:frameset.get_timestamp();
+			double imuStamp = frameset.get_timestamp()> UTimer::now()+1000000000?stamp*1000.0:frameset.get_timestamp();
 			getPoseAndIMU(imuStamp, info->odomPose, confidence, imu);

 			if(odometryProvided_ && !info->odomPose.isNull())
 			{
+				// Transform in base frame (local transform should contain base to pose transform)
+				info->odomPose = this->getLocalTransform() * info->odomPose * this->getLocalTransform().inverse();
+
 				info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1) * 0.0001;
 				info->odomCovariance.rowRange(0,3) *= pow(10, 3-(int)confidence);
 				info->odomCovariance.rowRange(3,6) *= pow(10, 1-(int)confidence);
 			}
-			if(!imu.empty())
+			if(!imu.empty() && !publishInterIMU_)
 			{
 				data.setIMU(imu);
-
-				if(publishInterIMU_ && lastImuStamp_ > 0.0)
+			}
+			else if(publishInterIMU_ && !gyroBuffer_.empty())
+			{
+				if(lastImuStamp_ > 0.0)
 				{
 					UASSERT(imuStamp > lastImuStamp_);
 					imuMutex_.lock();
@@ -1241,8 +1248,13 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 					{
 						++iterA;
 					}
+					if(iterB != gyroBuffer_.end())
+					{
+						++iterB;
+					}
 					if(iterA != iterB)
 					{
+						int pub = 0;
 						for(;iterA != iterB;++iterA)
 						{
 							Transform tmp;
@@ -1251,8 +1263,14 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 							if(!imuTmp.empty())
 							{
 								UEventsManager::post(new IMUEvent(imuTmp, iterA->first/1000.0));
+								pub++;
+							}
+							else
+							{
+								break;
 							}
 						}
+						UDEBUG("inter imu published=%d, %f -> %f", pub, lastImuStamp_, imuStamp);
 					}
 					imuMutex_.unlock();
 				}
--- a/corelib/src/odometry/OdometryMSCKF.cpp
+++ b/corelib/src/odometry/OdometryMSCKF.cpp
@@ -111,7 +111,10 @@ public:
 		cam1_intrinsics[2] = rectified?model.right().cx():model.right().K_raw().at<double>(0,2);
 		cam1_intrinsics[3] = rectified?model.right().cy():model.right().K_raw().at<double>(1,2);

+		UINFO("Local transform=%s", model.localTransform().prettyPrint().c_str());
+		UINFO("imuLocalTransform=%s", imuLocalTransform.prettyPrint().c_str());
 		Transform imuCam = model.localTransform().inverse() * imuLocalTransform;
+		UINFO("imuCam=%s", imuCam.prettyPrint().c_str());
 		cv::Mat     T_imu_cam0 = imuCam.dataMatrix();
 		cv::Matx33d R_imu_cam0(T_imu_cam0(cv::Rect(0,0,3,3)));
 		cv::Vec3d   t_imu_cam0 = T_imu_cam0(cv::Rect(3,0,1,3));
@@ -130,7 +133,7 @@ public:
 		{
 			cam0cam1 = model.stereoTransform();
 		}
-
+		UINFO("cam0cam1=%s", cam0cam1.prettyPrint().c_str());
 		UASSERT(!cam0cam1.isNull());
 		Transform imuCam1 = cam0cam1 * imuCam;
 		cv::Mat T_imu_cam1 = imuCam1.dataMatrix();
@@ -423,7 +426,10 @@ public:
 			state_server.state_cov(i, i) = extrinsic_translation_cov;

 		// Transformation offsets between the frames involved.
+		UINFO("Local transform=%s", model.localTransform().prettyPrint().c_str());
+		UINFO("imuLocalTransform=%s", imuLocalTransform.prettyPrint().c_str());
 		Transform imuCam = model.localTransform().inverse() * imuLocalTransform;
+		UINFO("imuCam=%s", imuCam.prettyPrint().c_str());
 		Eigen::Isometry3d T_imu_cam0(imuCam.toEigen4d());
 		Eigen::Isometry3d T_cam0_imu = T_imu_cam0.inverse();

@@ -441,8 +447,9 @@ public:
 		{
 			cam0cam1 = model.stereoTransform();
 		}
+		UINFO("cam0cam1=%s", cam0cam1.prettyPrint().c_str());
 		msckf_vio::CAMState::T_cam0_cam1 = cam0cam1.toEigen3d().matrix();
-		msckf_vio::IMUState::T_imu_body = Transform::getIdentity().toEigen3d().matrix();
+		msckf_vio::IMUState::T_imu_body = imuLocalTransform.toEigen3d().matrix();

 		// Maximum number of camera states to be stored
 		Parameters::parse(parameters_, Parameters::kOdomMSCKFMaxCamStateSize(), max_cam_state_size); //30
@@ -748,7 +755,7 @@ OdometryMSCKF::OdometryMSCKF(const ParametersMap & parameters) :
 imageProcessor_(0),
 msckf_(0),
 parameters_(parameters),
-flipXY_(-1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1, 0),
+fixPoseRotation_(0, 0, -1, 0, 0, 1, 0, 0, 1, 0, 0, 0),
 previousPose_(Transform::getIdentity()),
 initGravity_(false)
 #endif
@@ -889,19 +896,21 @@ Transform OdometryMSCKF::computeTransform(
 			pcl::PointCloud<pcl::PointXYZ>::Ptr localMap;
 			nav_msgs::Odometry odom = msckf_->featureCallback2(measurements, localMap);

-			Transform p = Transform(
-					odom.pose.pose.position.x,
-					odom.pose.pose.position.y,
-					odom.pose.pose.position.z,
-					odom.pose.pose.orientation.x,
-					odom.pose.pose.orientation.y,
-					odom.pose.pose.orientation.z,
-					odom.pose.pose.orientation.w);
-
-			if(!p.isNull())
+			if( odom.pose.pose.orientation.x != 0.0f ||
+				odom.pose.pose.orientation.y != 0.0f ||
+				odom.pose.pose.orientation.z != 0.0f ||
+				odom.pose.pose.orientation.w != 0.0f)
 			{
-				// pose in rtabmap/ros coordinates
-				p = flipXY_*p*lastImu_.localTransform();
+				Transform p = Transform(
+						odom.pose.pose.position.x,
+						odom.pose.pose.position.y,
+						odom.pose.pose.position.z,
+						odom.pose.pose.orientation.x,
+						odom.pose.pose.orientation.y,
+						odom.pose.pose.orientation.z,
+						odom.pose.pose.orientation.w);
+
+				p = fixPoseRotation_*p;

 				if(this->getPose().rotation().isIdentity())
 				{
@@ -956,10 +965,10 @@ Transform OdometryMSCKF::computeTransform(
 					{
 						if(localMap.get() && localMap->size())
 						{
-							Eigen::Affine3f flip = (this->getPose()*previousPoseInv*flipXY_).toEigen3f();
+							Eigen::Affine3f fixRot = fixPoseRotation_.toEigen3f();
 							for(unsigned int i=0; i<localMap->size(); ++i)
 							{
-								pcl::PointXYZ pt = pcl::transformPoint(localMap->at(i), flip);
+								pcl::PointXYZ pt = pcl::transformPoint(localMap->at(i), fixRot);
 								info->localMap.insert(std::make_pair(i, cv::Point3f(pt.x, pt.y, pt.z)));
 							}
 						}
@@ -980,9 +989,8 @@ Transform OdometryMSCKF::computeTransform(
 						}
 					}
 				}
+				UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str());
 			}
-
-			UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str());
 		}
 	}

--- a/corelib/src/odometry/OdometryVINS.cpp
+++ b/corelib/src/odometry/OdometryVINS.cpp
@@ -355,6 +355,8 @@ Transform OdometryVINS::computeTransform(
 		Vector3d acc(dx, dy, dz);
 		Vector3d gyr(rx, ry, rz);

+		UDEBUG("IMU update stamp=%f", data.stamp());
+
 		if(vinsEstimator_ != 0)
 		{
 			vinsEstimator_->inputIMU(t, acc, gyr);
@@ -450,6 +452,7 @@ Transform OdometryVINS::computeTransform(

 				if(info)
 				{
+					info->type = this->getType();
 					info->reg.covariance = cv::Mat::eye(6,6, CV_64FC1);
 					info->reg.covariance *= this->framesProcessed() == 0?9999:0.0001;

@@ -473,7 +476,16 @@ Transform OdometryVINS::computeTransform(
 						p.z = w_pts_i(2);
 						p = util3d::transformPoint(p, fixT);
 						info->localMap.insert(std::make_pair(it_per_id.feature_id, p));
+
+						if(this->imagesAlreadyRectified())
+						{
+							cv::Point2f pt;
+							data.stereoCameraModel().left().reproject(pts_i(0), pts_i(1), pts_i(2), pt.x, pt.y);
+							info->reg.inliersIDs.push_back(info->newCorners.size());
+							info->newCorners.push_back(pt);
+						}
 					}
+					info->features = info->newCorners.size();
 					info->localMapSize = info->localMap.size();
 				}
 				UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str());