From b9b76b369da4dc4aed53c1f75febf883e256847d Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Tue, 1 Mar 2022 14:12:07 +0100
Subject: [PATCH] averaging solpe but only once orienation
---
src/subscriber_imu.cpp | 31 ++++++++++++++++++++-----------
1 file changed, 20 insertions(+), 11 deletions(-)
diff --git a/src/subscriber_imu.cpp b/src/subscriber_imu.cpp
index 41f6e72..22d5ff5 100644
--- a/src/subscriber_imu.cpp
+++ b/src/subscriber_imu.cpp
@@ -314,20 +314,32 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
// Publish gravity projection onto sloped 2D plane
if (pub_slope_.getNumSubscribers() != 0)
{
- Eigen::Vector2d g_local_xy;
+ double slope, orientation;
if (non_orthogonal_gravity_)
- g_local_xy = std::static_pointer_cast<SensorImu2d>(sensor_ptr_)->getStateVector("G");
+ {
+ Eigen::Vector2d g_local_xy = std::static_pointer_cast<SensorImu2d>(sensor_ptr_)->getStateVector("G");
+ slope = asin(g_local_xy.norm() / gravity().norm());
+ orientation = atan2(g_local_xy(1),g_local_xy(0));
+ }
else if (std::dynamic_pointer_cast<SensorImu>(sensor_ptr_) != nullptr)
{
- g_local_xy.setZero();
+ slope = 0;
+ auto n_frames = 0;
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>();
+ auto q_frame = Eigen::Quaterniond(Eigen::Vector4d(frm_it.second->getStateVector("O")));
+ auto g_local_xy = (q_frame.conjugate() * gravity()).head<2>();
+
+ slope += asin(g_local_xy.norm() / gravity().norm());
+
+ if (n_frames == 0)
+ orientation = atan2(g_local_xy(1),g_local_xy(0));
+
+ n_frames++;
}
- g_local_xy /= sensor_ptr_->getProblem()->getTrajectory()->getFrameMap().size();
+ slope /= n_frames;
}
else
return;
@@ -336,11 +348,8 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
geometry_msgs::Vector3Stamped g_msg;
g_msg.header.stamp = msg->header.stamp;
g_msg.header.frame_id = "/map";
- // slope
- g_msg.vector.x = asin(g_local_xy.norm() / gravity().norm());
- // orientation
- g_msg.vector.y = atan2(g_local_xy(1),g_local_xy(0));
- // empty
+ g_msg.vector.x = slope;
+ g_msg.vector.y = orientation;
g_msg.vector.z = 0;
// publish
--
GitLab