averaging gravity in 3D

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;