From 125c025ddaaa9826f118a93ead97f35d34a354cf Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Fri, 25 Feb 2022 13:18:06 +0100
Subject: [PATCH] averaging gravity in 3D
---
src/subscriber_imu.cpp | 9 +++++++--
1 file changed, 7 insertions(+), 2 deletions(-)
diff --git a/src/subscriber_imu.cpp b/src/subscriber_imu.cpp
index a399245..41f6e72 100644
--- a/src/subscriber_imu.cpp
+++ b/src/subscriber_imu.cpp
@@ -321,8 +321,13 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
else if (std::dynamic_pointer_cast<SensorImu>(sensor_ptr_) != nullptr)
{
- auto q_first = Eigen::Quaterniond(Eigen::Vector4d(sensor_ptr_->getProblem()->getTrajectory()->getFirstFrame()->getStateVector("O")));
- g_local_xy = (q_first.conjugate() * gravity()).head<2>();
+ g_local_xy.setZero();
+ for (auto frm_it : sensor_ptr_->getProblem()->getTrajectory()->getFrameMap())
+ {
+ auto q_first = Eigen::Quaterniond(Eigen::Vector4d(frm_it.second->getStateVector("O")));
+ g_local_xy += (q_first.conjugate() * gravity()).head<2>();
+ }
+ g_local_xy /= sensor_ptr_->getProblem()->getTrajectory()->getFrameMap().size();
}
else
return;
--
GitLab