From 607dc67135f54fa377575b83f8851ad500269de1 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 10 Apr 2020 19:24:45 -0400 Subject: [PATCH] IMU: removing yaw from local imu transform when creating gravity links --- corelib/src/Memory.cpp | 3 ++- corelib/src/odometry/OdometryF2M.cpp | 2 +- guilib/src/DatabaseViewer.cpp | 1 + 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index edcc137c..7de7b07e 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -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()); diff --git a/corelib/src/odometry/OdometryF2M.cpp b/corelib/src/odometry/OdometryF2M.cpp index 8854a584..dce19024 100644 --- a/corelib/src/odometry/OdometryF2M.cpp +++ b/corelib/src/odometry/OdometryF2M.cpp @@ -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) { diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 016f4936..8681fb57 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -4195,6 +4195,7 @@ void DatabaseViewer::update(int value, gravityLink.begin()->second.transform().getEulerAngles(roll, pitch, yaw); Eigen::Vector3d v = Transform(0,0,0,roll,pitch,0).toEigen3d() * -Eigen::Vector3d::UnitZ(); labelGravity->setText(QString("x=%1 y=%2 z=%3").arg(v[0]).arg(v[1]).arg(v[2])); + labelGravity->setToolTip(QString("roll=%1 pitch=%2 yaw=%3").arg(roll).arg(pitch).arg(yaw)); } if(gps.stamp()>0.0)