most updated propagate

src/CMakeLists.txt

@@ -20,7 +20,8 @@ ADD_SUBDIRECTORY(geometry)
 
 INCLUDE_DIRECTORIES(FGraph)
 ADD_SUBDIRECTORY(FGraph)
-
+INCLUDE_DIRECTORIES(inertial)
+ADD_SUBDIRECTORY(inertial)
 INCLUDE_DIRECTORIES(PCRegistration)
 ADD_SUBDIRECTORY(PCRegistration)
 
@@ -30,6 +31,6 @@ ADD_SUBDIRECTORY(plane-surfaces)
 INCLUDE_DIRECTORIES(visual)
 ADD_SUBDIRECTORY(visual)
 
-IF(NOT ANDROID)
-    ADD_SUBDIRECTORY(pybind)
-ENDIF(NOT ANDROID)
+# IF(NOT ANDROID)
+#     ADD_SUBDIRECTORY(pybind)
+# ENDIF(NOT ANDROID)

src/geometry/SE3vel.cpp

@@ -49,7 +49,9 @@ Mat31 SE3vel::t() const
 {
     return T_.block<3,1>(0,3);
 }
-
+void SE3vel::update(const SE3vel& T){
+    this->T_ = T.T_;
+}
 Mat31 SE3vel::v() const
 {
     return T_.topRightCorner<3,1>();

src/geometry/mrob/SE3vel.hpp

@@ -62,7 +62,7 @@ class SE3vel{
 
 
         Mat9 adj() const;
-
+        void update(const SE3vel& T);
         void Exp(const Mat91 &xi);
         Mat91 Ln(void) const;
 

src/inertial/factors/nodeInertial3d.cpp

@@ -37,7 +37,7 @@ NodeInertial3d::NodeInertial3d(const Mat<3,7> state,
 
 void NodeInertial3d::update(VectRefConst &dx)
 {
-    Vect<15> dxf = dx;
+    //Vect<15> dxf = dx;
 }
 
 

src/inertial/filter_lidar_inertial.cpp

@@ -25,30 +25,73 @@
 
 
 #include "mrob/filter_lidar_inertial.hpp"
-
+#include <fstream> 
 
 using namespace mrob;
 FilterLidarInertial::FilterLidarInertial()
+: Data(nullptr), prev_state_stamp(0.0)
 {
 
 }
 
-FilterLidarInertial::~FilterLidarInerial()
+FilterLidarInertial::~FilterLidarInertial()
 {
 
 }
 
-void FilterLidarInertial::read_observations(const InertialData *LaodingData)
+void FilterLidarInertial::read_observations(const InertialData * LaodingData)
 {
-  this-> Data = LaodingData;
+  Data = LaodingData;
   // fill in the function and change the heather.
 }
-void print_data(){
 
-}; 
-void propagate(){
+void  FilterLidarInertial::print_data(){
+  std::ofstream outfile("propagate_data.txt", std::ios::app); 
+
+    if (!outfile.is_open()) {
+        return;
+    }
+    outfile << "Timestamp: " << prev_state_stamp << std::endl;
+    outfile << "Velocity: " << curr_state.v().transpose() << std::endl;
+    outfile << "Translation: " << curr_state.t().transpose() << std::endl;
+    outfile << "Rotation:" << std::endl << curr_state.R() << std::endl;
+    outfile << std::endl;
+    outfile.close();
+
 
 }
-void solve(){
+void  FilterLidarInertial::propagate(){
+   if (init) {
+        if (Data && !Data->imu_buffer.empty()) {
+            prev_state_stamp = Data->imu_buffer.front().stamp;
+            init = false;
+        } else {
+            return;
+        }
+    }
+  for (const auto &imu_msg : Data->imu_buffer) {
+        double dt = imu_msg.stamp - prev_state_stamp; 
+        if (dt <= 0) continue;
+        Mat31 gravity(0, 0, -9.81);
+        Mat31 a = imu_msg.linear_acceleration+gravity;//check reference, seems it needs rotation
+        Mat31 v = prev_state.v() + a * dt;
+        Mat31 t = prev_state.t()+ v * dt;
+        if (imu_msg.angular_velocity.size() != 3) continue;
+        Mat3 omega;
+        // chek: hat3()
+        omega << 0, -imu_msg.angular_velocity(2), imu_msg.angular_velocity(1),
+        imu_msg.angular_velocity(2), 0, -imu_msg.angular_velocity(0),
+        -imu_msg.angular_velocity(1), imu_msg.angular_velocity(0), 0 ;
 
+        SO3 dR(prev_state.R());
+        dR.update_rhs(omega*dt);
+        //Mat3 dR = Mat3::Identity() + omega * dt + 0.5 * omega * omega * dt * dt;
+        //SO3 orientation = prev_state.R() * dR.R();
+        prev_state.update(mrob::SE3vel(dR, t, v)); 
+        prev_state_stamp = imu_msg.stamp; 
+  }
+  curr_state.update(prev_state);
+  print_data();
+}
+void  FilterLidarInertial::solve(){
 }

src/inertial/mrob/factors/nodeInertial3d.hpp

@@ -84,7 +84,7 @@ class NodeInertial3d : public Node
  *  - obs from acc and gyro (TBD)
 */
 
-void propagate_interial3d(NodeInertial3d node, const Matx &obs);
+// void propagate_interial3d(NodeInertial3d node, const Mat91 &obs);
 
 }
 

src/inertial/mrob/filter_lidar_inertial.hpp

@@ -27,8 +27,10 @@
 #define FILTERLIDARINERTIAL_HPP_
 
 
-#include "/home/mars_ugv/mrob/src/common/mrob/matrix_base.hpp"
-
+// #include "mrob/matrix_base.hpp"
+#include <mrob/matrix_base.hpp>
+#include <mrob/SE3velCov.hpp>
+#include <mrob/SO3.hpp>
 
 namespace mrob
 {
@@ -51,24 +53,23 @@ struct Linear_acc {
 };
 struct Imu_msg {
     double stamp;
-    Orientation orientation;
-    Angular_vel angular_velocity;
-    Linear_acc linear_acceleration;
-
+    Mat41 orientation;
+    Mat31 angular_velocity;
+    Mat31 linear_acceleration;
     Mat3 orientation_covariance;
     Mat3 angular_velocity_covariance;
     Mat3 linear_acceleration_covariance;
 };
 
 struct LidarPoint
 {
-  LidarPoint() // constructor 
+  LidarPoint() : point(Mat31::Zero()), stamp(0.0), intensity(0.0)// constructor 
   {
-    point(0,0) = 0.0;
+    /*point(0,0) = 0.0;
     point(1,0) = 0.0;
     point(2,0) = 0.0; 
     stamp = 0.0; 
-    intensity = 0.0 ;
+    intensity = 0.0 ;*/
   };
   Mat31 point; 
   float stamp;  // time in nanosec 
@@ -86,8 +87,6 @@ struct InertialData
 };
 class FilterLidarInertial
 {
-  private: 
-    InertialData Data;
   public:
     // Here constructor shou,d have all covariances values and hyperparams.
     FilterLidarInertial();
@@ -96,8 +95,14 @@ class FilterLidarInertial
     void print_data();
     void propagate();
     void solve();
-    void init_state();
+    // void init_state();
 
+  private: 
+    const InertialData *Data;
+    mrob::SE3vel prev_state;
+    mrob::SE3vel curr_state;
+    bool init = true; 
+    double prev_state_stamp = 0; 
 };
 
 }//namespace