averaging solpe but only once orienation

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