i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

OdometryVINS: fixed output pose orientation (D435i and euroc compatible)

Browse Source
This commit is contained in:
matlabbe
2019-08-15 16:46:56 -04:00
parent 4b118ae660
commit 2a1b737470
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/src/odometry/OdometryVINS.cpp
View File

@@ -417,7 +417,7 @@ Transform OdometryVINS::computeTransform(
{ {
if(!lastImu_.localTransform().isNull()) 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()) if(this->getPose().rotation().isIdentity())
@@ -442,7 +442,7 @@ Transform OdometryVINS::computeTransform(
info->reg.covariance *= this->framesProcessed() == 0?9999:0.0001; info->reg.covariance *= this->framesProcessed() == 0?9999:0.0001;
// feature map // 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) for (auto &it_per_id : vinsEstimator_->f_manager.feature)
{ {
int used_num; int used_num;