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

IMU: removing yaw from local imu transform when creating gravity links

Browse Source
This commit is contained in:
matlabbe
2020-04-10 19:24:45 -04:00
parent d6269bb5d6
commit 607dc67135
3 changed files with 4 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
corelib/src/Memory.cpp
View File

@@ -5167,7 +5167,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
{
Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]);
orientation*=data.imu().localTransform().rotation().inverse();
// orientation includes roll and pitch but not yaw in local transform
orientation= Transform(0,0,data.imu().localTransform().theta()) * orientation * data.imu().localTransform().rotation().inverse();
s->addLink(Link(s->id(), s->id(), Link::kGravity, orientation));
UDEBUG("Added gravity constraint: %s", orientation.prettyPrint().c_str());

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

@@ -215,7 +215,7 @@ Transform OdometryF2M::computeTransform(
else
{
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());
// orientation includes roll and pitch but not yaw in local transform
imus_.insert(std::make_pair(data.stamp(), Transform(0,0,data.imu().localTransform().theta()) * orientation*data.imu().localTransform().inverse()));
if(imus_.size() > 1000)
{