From 3d53f9c9c4c84bc91fa0a43a42803a8423d77c2d Mon Sep 17 00:00:00 2001
From: Ahmed Baza <Ahmed.Baza@skoltech.ru>
Date: Thu, 18 Apr 2024 17:56:20 +0300
Subject: [PATCH] Data structures for handling imu lidar msgs

---
 src/inertial/filter_lidar_inertial.cpp      |  3 +-
 src/inertial/mrob/filter_lidar_inertial.hpp | 57 ++++++++++++++++++++-
 2 files changed, 57 insertions(+), 3 deletions(-)

diff --git a/src/inertial/filter_lidar_inertial.cpp b/src/inertial/filter_lidar_inertial.cpp
index 26de387..f5d10f8 100644
--- a/src/inertial/filter_lidar_inertial.cpp
+++ b/src/inertial/filter_lidar_inertial.cpp
@@ -38,7 +38,8 @@ FilterLidarInertial::~FilterLidarInerial()
 
 }
 
-void FilterLidarInertial::read_observations()
+void FilterLidarInertial::read_observations(InertialData LaodingData)
 {
+  this-> Data = LaodingData;
   // fill in the function and change the heather.
 }
\ No newline at end of file
diff --git a/src/inertial/mrob/filter_lidar_inertial.hpp b/src/inertial/mrob/filter_lidar_inertial.hpp
index 264a2da..82e0688 100644
--- a/src/inertial/mrob/filter_lidar_inertial.hpp
+++ b/src/inertial/mrob/filter_lidar_inertial.hpp
@@ -32,18 +32,71 @@
 
 namespace mrob
 {
+struct orientation {
+    double x;
+    double y;
+    double z;
+    double w;
+}; 
+struct angular_vel {
+    double x;
+    double y;
+    double z;
+};
+
+struct linear_acc {
+    double x;
+    double y;
+    double z;
+};
+struct Imu_msg {
+    double stamp;
+    orientation orientation;
+    angular_vel angular_velocity;
+    linear_acc linear_acceleration;
+
+    Mat19 orientation_covariance;
+    Mat19 angular_velocity_covariance;
+    Mat19 linear_acceleration_covariance;
+};
+
+struct LidarPoint
+{
+  LidarPoint() // constructor 
+  {
+    point(0,0) = 0.0;
+    point(1,0) = 0.0;
+    point(2,0) = 0.0; 
+    stamp = 0.0; 
+    intensity = 0.0 ;
+  };
+  Mat31 point; 
+  float stamp;  // time in nanosec 
+  float intensity;
+};
 
+struct InertialData
+{
+  InertialData()
+  {
+    
+  };
+  std::vector<Imu_msg> imu_buffer; 
+  std::vector<LidarPoint> pointcloud;
+};
 class FilterLidarInertial
 {
+  private: 
+    InertialData Data;
   public:
     // Here constructor shou,d have all covariances values and hyperparams.
     FilterLidarInertial();
     ~FilterLidarInertial();
-
-    void read_observations();
+    void read_observations(InertialData LaodingData);
     void solve();
 
 };
+
 }//namespace