averaged again

src/subscriber_imu.cpp

@@ -325,12 +325,12 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
 
         else if (std::dynamic_pointer_cast<SensorImu>(sensor_ptr_) != nullptr)
         {
-            auto q_frame = Eigen::Quaterniond(Eigen::Vector4d(sensor_ptr_->getProblem()->getTrajectory()->getFirstFrame()->getStateVector("O")));
-            auto g_local_xy = (q_frame.conjugate() * gravity()).head<2>();
-            slope = asin(g_local_xy.norm() / gravity().norm());
-            orientation = atan2(g_local_xy(1),g_local_xy(0));
+            // auto q_frame = Eigen::Quaterniond(Eigen::Vector4d(sensor_ptr_->getProblem()->getTrajectory()->getFirstFrame()->getStateVector("O")));
+            // auto g_local_xy = (q_frame.conjugate() * gravity()).head<2>();
+            // slope = asin(g_local_xy.norm() / gravity().norm());
+            // orientation = atan2(g_local_xy(1),g_local_xy(0));
 
-            /*slope = 0;
+            slope = 0;
             auto n_frames = 0;
             for (auto frm_it : sensor_ptr_->getProblem()->getTrajectory()->getFrameMap())
             {
@@ -344,7 +344,7 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
                 
                 n_frames++;
             }
-            slope /= n_frames;*/
+            slope /= n_frames;
         }
         else
             return;