From 2a1b73747078a16b5ba0d6831f7c215ff88bc212 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Thu, 15 Aug 2019 16:46:56 -0400
Subject: [PATCH] OdometryVINS: fixed output pose orientation (D435i and euroc
 compatible)

---
 corelib/src/odometry/OdometryVINS.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/corelib/src/odometry/OdometryVINS.cpp b/corelib/src/odometry/OdometryVINS.cpp
index 2129a33f..1645b745 100644
--- a/corelib/src/odometry/OdometryVINS.cpp
+++ b/corelib/src/odometry/OdometryVINS.cpp
@@ -417,7 +417,7 @@ Transform OdometryVINS::computeTransform(
 			{
 				if(!lastImu_.localTransform().isNull())
 				{
-					p = p * lastImu_.localTransform();
+					p = Transform(0,1,0,0,-1,0,0,0, 0,0,1,0) * p * lastImu_.localTransform().inverse();
 				}
 
 				if(this->getPose().rotation().isIdentity())
@@ -442,7 +442,7 @@ Transform OdometryVINS::computeTransform(
 					info->reg.covariance *= this->framesProcessed() == 0?9999:0.0001;
 
 					// feature map
-					Transform fixT = this->getPose()*previousPoseInv;
+					Transform fixT = this->getPose()*previousPoseInv*Transform(0,1,0,0,-1,0,0,0, 0,0,1,0);
 					for (auto &it_per_id : vinsEstimator_->f_manager.feature)
 					{
 						int used_num;