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

OdomF2M: fixed roll/pitch ignored in imu local transform

Browse Source
This commit is contained in:
matlabbe
2020-04-09 19:22:57 -04:00
parent 1b4b1c2832
commit 43df2a05da
1 changed files with 1 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/odometry/OdometryF2M.cpp
View File

@@ -216,7 +216,7 @@ Transform OdometryF2M::computeTransform(
{ {
Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]); Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]);
//UWARN("%fs %s", data.stamp(), orientation.prettyPrint().c_str()); //UWARN("%fs %s", data.stamp(), orientation.prettyPrint().c_str());
imus_.insert(std::make_pair(data.stamp(), orientation*data.imu().localTransform().inverse())); imus_.insert(std::make_pair(data.stamp(), Transform(0,0,data.imu().localTransform().theta()) * orientation*data.imu().localTransform().inverse()));
if(imus_.size() > 1000) if(imus_.size() > 1000)
{ {
imus_.erase(imus_.begin()); imus_.erase(imus_.begin());