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;