剥离imuprocess.hpp

faster-lio/include/common_lib.h

@@ -1,9 +1,9 @@
 #ifndef COMMON_LIB_H
 #define COMMON_LIB_H
 
-#include <eigen_conversions/eigen_msg.h>
-#include <nav_msgs/Odometry.h>
-#include <sensor_msgs/Imu.h>
+//#include <eigen_conversions/eigen_msg.h>
+//#include <nav_msgs/Odometry.h>
+//#include <sensor_msgs/Imu.h>
 
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -13,7 +13,7 @@
 #include <boost/array.hpp>
 #include <unsupported/Eigen/ArpackSupport>
 
-#include "faster_lio/Pose6D.h"
+//#include "faster_lio/Pose6D.h"
 #include "options.h"
 #include "so3_math.h"
 
@@ -60,7 +60,29 @@ inline std::string DEBUG_FILE_DIR(const std::string &name)
     return std::string(ROOT_DIR) + "Log/" + name;
 }
 
-using Pose6D = faster_lio::Pose6D;
+//using Pose6D = faster_lio::Pose6D;
+
+typedef struct
+{
+    typedef double _offset_time_type;
+    _offset_time_type offset_time;
+
+    typedef boost::array<double, 3>  _acc_type;
+    _acc_type acc;
+
+    typedef boost::array<double, 3>  _gyr_type;
+    _gyr_type gyr;
+
+    typedef boost::array<double, 3>  _vel_type;
+    _vel_type vel;
+
+    typedef boost::array<double, 3>  _pos_type;
+    _pos_type pos;
+
+    typedef boost::array<double, 9>  _rot_type;
+    _rot_type rot;
+}Pose6D;
+
 using V3D = Eigen::Vector3d;
 using V4D = Eigen::Vector4d;
 using V5D = Eigen::Matrix<double, 5, 1>;

faster-lio/include/imu_processing.h

@@ -0,0 +1,75 @@
+#ifndef FASTER_LIO_IMU_PROCESSING_H
+#define FASTER_LIO_IMU_PROCESSING_H
+
+#include <glog/logging.h>
+#include <sensor_msgs/Imu.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <cmath>
+#include <deque>
+#include <fstream>
+
+#include "common_lib.h"
+#include "so3_math.h"
+#include "use-ikfom.h"
+#include "utils.h"
+
+namespace faster_lio {
+
+
+constexpr int MAX_INI_COUNT = 20;
+
+bool time_list(const PointType &x, const PointType &y) ;
+
+/// IMU Process and undistortion
+class ImuProcess {
+ public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    ImuProcess();
+    ~ImuProcess();
+
+    void Reset();
+    void SetExtrinsic(const common::V3D &transl, const common::M3D &rot);
+    void SetGyrCov(const common::V3D &scaler);
+    void SetAccCov(const common::V3D &scaler);
+    void SetGyrBiasCov(const common::V3D &b_g);
+    void SetAccBiasCov(const common::V3D &b_a);
+    void Process(const common::MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state,
+                 PointCloudType::Ptr pcl_un_);
+
+    std::ofstream fout_imu_;
+    Eigen::Matrix<double, 12, 12> Q_;
+    common::V3D cov_acc_;
+    common::V3D cov_gyr_;
+    common::V3D cov_acc_scale_;
+    common::V3D cov_gyr_scale_;
+    common::V3D cov_bias_gyr_;
+    common::V3D cov_bias_acc_;
+
+ private:
+    void IMUInit(const std::deque<common::ImuData> &imus, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, int &N);
+
+
+    void UndistortPcl(const common::MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state,
+                      PointCloudType &pcl_out);
+
+    PointCloudType::Ptr cur_pcl_un_;
+    common::ImuData last_imu_;
+    std::deque<sensor_msgs::ImuConstPtr> v_imu_;
+    std::vector<common::Pose6D> IMUpose_;
+    std::vector<common::M3D> v_rot_pcl_;
+    common::M3D Lidar_R_wrt_IMU_;
+    common::V3D Lidar_T_wrt_IMU_;
+    common::V3D mean_acc_;
+    common::V3D mean_gyr_;
+    common::V3D angvel_last_;
+    common::V3D acc_s_last_;
+    double last_lidar_end_time_ = 0;
+    int init_iter_num_ = 1;
+    bool b_first_frame_ = true;
+    bool imu_need_init_ = true;
+};
+
+}  // namespace faster_lio
+
+#endif

faster-lio/include/laser_mapping.h

@@ -9,7 +9,12 @@
 #include <condition_variable>
 #include <thread>
 
-#include "imu_processing.hpp"
+#include <eigen_conversions/eigen_msg.h>
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/Imu.h>
+#include <sensor_msgs/PointCloud2.h>
+
+#include "imu_processing.h"
 #include "ivox3d/ivox3d.h"
 #include "options.h"
 #include "pointcloud_preprocess.h"
@@ -62,7 +67,7 @@ class TTMapping
     bool Init(int ivox_nearby_type,double gyr_cov, double acc_cov,
               double b_gyr_cov, double b_acc_cov,double filter_size_surf_min);
     void AddPointCloud(PointCloudType::Ptr cloud,double timebase);
-    void AddImu(const sensor_msgs::Imu::ConstPtr &msg_in,double timebase);
+    void AddImu(const common::ImuData &msg_in,double timebase);
     bool Run();
     bool SyncPackages();
     void MapIncremental();

faster-lio/include/use-ikfom.h

@@ -0,0 +1,33 @@
+#ifndef FASTER_LIO_USE_IKFOM_H
+#define FASTER_LIO_USE_IKFOM_H
+
+#include "IKFoM_toolkit/esekfom/esekfom.hpp"
+
+namespace faster_lio {
+
+typedef MTK::vect<3, double> vect3;
+typedef MTK::SO3<double> SO3;
+typedef MTK::S2<double, 98090, 10000, 1> S2;
+typedef MTK::vect<1, double> vect1;
+typedef MTK::vect<2, double> vect2;
+
+MTK_BUILD_MANIFOLD(state_ikfom, ((vect3, pos))((SO3, rot))((SO3, offset_R_L_I))((vect3, offset_T_L_I))((vect3, vel))(
+                                    (vect3, bg))((vect3, ba))((S2, grav)));
+
+MTK_BUILD_MANIFOLD(input_ikfom, ((vect3, acc))((vect3, gyro))); //输入数据，imu三轴加速度、角速度
+
+MTK_BUILD_MANIFOLD(process_noise_ikfom, ((vect3, ng))((vect3, na))((vect3, nbg))((vect3, nba)));
+
+MTK::get_cov<process_noise_ikfom>::type process_noise_cov() ;
+
+Eigen::Matrix<double, 24, 1> get_f(state_ikfom &s, const input_ikfom &in);
+
+Eigen::Matrix<double, 24, 23> df_dx(state_ikfom &s, const input_ikfom &in) ;
+
+Eigen::Matrix<double, 24, 12> df_dw(state_ikfom &s, const input_ikfom &in) ;
+
+vect3 SO3ToEuler(const SO3 &orient) ;
+
+}  // namespace faster_lio
+
+#endif

faster-lio/src/CMakeLists.txt

@@ -1,11 +1,13 @@
 add_library(${PROJECT_NAME}
         laser_mapping.cc
+        use-ikfom.cc
         pointcloud_preprocess.cc
+        imu_processing.cc
         options.cc
         utils.cc
         )
 
-add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencpp livox_ros_driver_gencpp)
+#add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencpp livox_ros_driver_gencpp)
 
 target_link_libraries(${PROJECT_NAME}
         ${catkin_LIBRARIES}

faster-lio/include/imu_processing.hpp

@@ -1,76 +1,14 @@
-#ifndef FASTER_LIO_IMU_PROCESSING_H
-#define FASTER_LIO_IMU_PROCESSING_H
-
-#include <glog/logging.h>
-#include <sensor_msgs/Imu.h>
-#include <sensor_msgs/PointCloud2.h>
-#include <cmath>
-#include <deque>
-#include <fstream>
-
-#include "common_lib.h"
-#include "so3_math.h"
-#include "use-ikfom.hpp"
-#include "utils.h"
-
-namespace faster_lio {
-
-
-constexpr int MAX_INI_COUNT = 20;
+//
+// Created by zx on 22-10-31.
+//
+#include "imu_processing.h"
+namespace faster_lio
+{
 
 bool time_list(const PointType &x, const PointType &y) { return (x.curvature < y.curvature); };
 
-/// IMU Process and undistortion
-class ImuProcess {
-   public:
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-    ImuProcess();
-    ~ImuProcess();
-
-    void Reset();
-    void SetExtrinsic(const common::V3D &transl, const common::M3D &rot);
-    void SetGyrCov(const common::V3D &scaler);
-    void SetAccCov(const common::V3D &scaler);
-    void SetGyrBiasCov(const common::V3D &b_g);
-    void SetAccBiasCov(const common::V3D &b_a);
-    void Process(const common::MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state,
-                 PointCloudType::Ptr pcl_un_);
-
-    std::ofstream fout_imu_;
-    Eigen::Matrix<double, 12, 12> Q_;
-    common::V3D cov_acc_;
-    common::V3D cov_gyr_;
-    common::V3D cov_acc_scale_;
-    common::V3D cov_gyr_scale_;
-    common::V3D cov_bias_gyr_;
-    common::V3D cov_bias_acc_;
-
-   private:
-    void IMUInit(const std::deque<common::ImuData> &imus, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, int &N);
-
-
-    void UndistortPcl(const common::MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state,
-                      PointCloudType &pcl_out);
-
-    PointCloudType::Ptr cur_pcl_un_;
-    common::ImuData last_imu_;
-    std::deque<sensor_msgs::ImuConstPtr> v_imu_;
-    std::vector<common::Pose6D> IMUpose_;
-    std::vector<common::M3D> v_rot_pcl_;
-    common::M3D Lidar_R_wrt_IMU_;
-    common::V3D Lidar_T_wrt_IMU_;
-    common::V3D mean_acc_;
-    common::V3D mean_gyr_;
-    common::V3D angvel_last_;
-    common::V3D acc_s_last_;
-    double last_lidar_end_time_ = 0;
-    int init_iter_num_ = 1;
-    bool b_first_frame_ = true;
-    bool imu_need_init_ = true;
-};
-
-ImuProcess::ImuProcess() : b_first_frame_(true), imu_need_init_(true) {
+ImuProcess::ImuProcess() : b_first_frame_(true), imu_need_init_(true)
+{
     init_iter_num_ = 1;
     Q_ = process_noise_cov();
     cov_acc_ = common::V3D(0.1, 0.1, 0.1);
@@ -85,41 +23,60 @@ ImuProcess::ImuProcess() : b_first_frame_(true), imu_need_init_(true) {
     //last_imu_.reset(new sensor_msgs::Imu());
 }
 
-ImuProcess::~ImuProcess() {}
+ImuProcess::~ImuProcess()
+{
+}
 
-void ImuProcess::Reset() {
+void ImuProcess::Reset()
+{
     mean_acc_ = common::V3D(0, 0, -1.0);
     mean_gyr_ = common::V3D(0, 0, 0);
     angvel_last_ = common::Zero3d;
     imu_need_init_ = true;
     init_iter_num_ = 1;
-    v_imu_.clear();
+    //v_imu_.clear();
     IMUpose_.clear();
     //last_imu_.reset(new sensor_msgs::Imu());
     cur_pcl_un_.reset(new PointCloudType());
 }
 
-void ImuProcess::SetExtrinsic(const common::V3D &transl, const common::M3D &rot) {
+void ImuProcess::SetExtrinsic(const common::V3D &transl, const common::M3D &rot)
+{
     Lidar_T_wrt_IMU_ = transl;
     Lidar_R_wrt_IMU_ = rot;
 }
 
-void ImuProcess::SetGyrCov(const common::V3D &scaler) { cov_gyr_scale_ = scaler; }
+void ImuProcess::SetGyrCov(const common::V3D &scaler)
+{
+    cov_gyr_scale_ = scaler;
+}
 
-void ImuProcess::SetAccCov(const common::V3D &scaler) { cov_acc_scale_ = scaler; }
+void ImuProcess::SetAccCov(const common::V3D &scaler)
+{
+    cov_acc_scale_ = scaler;
+}
 
-void ImuProcess::SetGyrBiasCov(const common::V3D &b_g) { cov_bias_gyr_ = b_g; }
+void ImuProcess::SetGyrBiasCov(const common::V3D &b_g)
+{
+    cov_bias_gyr_ = b_g;
+}
 
-void ImuProcess::SetAccBiasCov(const common::V3D &b_a) { cov_bias_acc_ = b_a; }
+void ImuProcess::SetAccBiasCov(const common::V3D &b_a)
+{
+    cov_bias_acc_ = b_a;
+}
 
-void ImuProcess::IMUInit(const std::deque<common::ImuData>& imus, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state,
-                         int &N) {
+void ImuProcess::IMUInit(const std::deque<common::ImuData> &imus,
+                         esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state,
+                         int &N)
+{
     /** 1. initializing the gravity_, gyro bias, acc and gyro covariance
      ** 2. normalize the acceleration measurenments to unit gravity_ **/
 
     common::V3D cur_acc, cur_gyr;
 
-    if (b_first_frame_) {
+    if (b_first_frame_)
+    {
         Reset();
         N = 1;
         b_first_frame_ = false;
@@ -129,7 +86,8 @@ void ImuProcess::IMUInit(const std::deque<common::ImuData>& imus, esekfom::esekf
         mean_gyr_ << gyr_acc[0], gyr_acc[1], gyr_acc[2];
     }
 
-    for (const auto &imu : imus) {
+    for (const auto &imu : imus)
+    {
         const auto &imu_acc = imu.linear_acceleration;
         const auto &gyr_acc = imu.angular_velocity;
         cur_acc << imu_acc[0], imu_acc[1], imu_acc[2];
@@ -166,7 +124,8 @@ void ImuProcess::IMUInit(const std::deque<common::ImuData>& imus, esekfom::esekf
 }
 
 void ImuProcess::UndistortPcl(const common::MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state,
-                              PointCloudType &pcl_out) {
+                              PointCloudType &pcl_out)
+{
     /*** add the imu_ of the last frame-tail to the of current frame-head ***/
     auto v_imu = meas.imu_;
     v_imu.push_front(last_imu_);
@@ -192,26 +151,31 @@ void ImuProcess::UndistortPcl(const common::MeasureGroup &meas, esekfom::esekf<s
     double dt = 0;
 
     input_ikfom in;
-    for (auto it_imu = v_imu.begin(); it_imu < (v_imu.end() - 1); it_imu++) {
+    for (auto it_imu = v_imu.begin(); it_imu < (v_imu.end() - 1); it_imu++)
+    {
         auto &&head = *(it_imu);
         auto &&tail = *(it_imu + 1);
 
-        if (tail.timebase < last_lidar_end_time_) {
+        if (tail.timebase < last_lidar_end_time_)
+        {
             continue;
         }
 
         angvel_avr << 0.5 * (head.angular_velocity[0] + tail.angular_velocity[0]),
-            0.5 * (head.angular_velocity[1] + tail.angular_velocity[1]),
-            0.5 * (head.angular_velocity[2] + tail.angular_velocity[2]);
+                0.5 * (head.angular_velocity[1] + tail.angular_velocity[1]),
+                0.5 * (head.angular_velocity[2] + tail.angular_velocity[2]);
         acc_avr << 0.5 * (head.linear_acceleration[0] + tail.linear_acceleration[0]),
-            0.5 * (head.linear_acceleration[1] + tail.linear_acceleration[1]),
-            0.5 * (head.linear_acceleration[2] + tail.linear_acceleration[2]);
+                0.5 * (head.linear_acceleration[1] + tail.linear_acceleration[1]),
+                0.5 * (head.linear_acceleration[2] + tail.linear_acceleration[2]);
 
         acc_avr = acc_avr * common::G_m_s2 / mean_acc_.norm();  // - state_inout.ba;
 
-        if (head.timebase < last_lidar_end_time_) {
+        if (head.timebase < last_lidar_end_time_)
+        {
             dt = tail.timebase - last_lidar_end_time_;
-        } else {
+        }
+        else
+        {
             dt = tail.timebase - head.timebase;
         }
 
@@ -227,7 +191,8 @@ void ImuProcess::UndistortPcl(const common::MeasureGroup &meas, esekfom::esekf<s
         imu_state = kf_state.get_x();
         angvel_last_ = angvel_avr - imu_state.bg;
         acc_s_last_ = imu_state.rot * (acc_avr - imu_state.ba);
-        for (int i = 0; i < 3; i++) {
+        for (int i = 0; i < 3; i++)
+        {
             acc_s_last_[i] += imu_state.grav[i];
         }
 
@@ -246,11 +211,13 @@ void ImuProcess::UndistortPcl(const common::MeasureGroup &meas, esekfom::esekf<s
     last_lidar_end_time_ = pcl_end_time;
 
     /*** undistort each lidar point (backward propagation) ***/
-    if (pcl_out.points.empty()) {
+    if (pcl_out.points.empty())
+    {
         return;
     }
     auto it_pcl = pcl_out.points.end() - 1;
-    for (auto it_kp = IMUpose_.end() - 1; it_kp != IMUpose_.begin(); it_kp--) {
+    for (auto it_kp = IMUpose_.end() - 1; it_kp != IMUpose_.begin(); it_kp--)
+    {
         auto head = it_kp - 1;
         auto tail = it_kp;
         R_imu = common::MatFromArray(head->rot);
@@ -259,7 +226,8 @@ void ImuProcess::UndistortPcl(const common::MeasureGroup &meas, esekfom::esekf<s
         acc_imu = common::VecFromArray(tail->acc);
         angvel_avr = common::VecFromArray(tail->gyr);
 
-        for (; it_pcl->curvature / double(1000) > head->offset_time; it_pcl--) {
+        for (; it_pcl->curvature / double(1000) > head->offset_time; it_pcl--)
+        {
             dt = it_pcl->curvature / double(1000) - head->offset_time;
 
             /* Transform to the 'end' frame, using only the rotation
@@ -271,16 +239,17 @@ void ImuProcess::UndistortPcl(const common::MeasureGroup &meas, esekfom::esekf<s
             common::V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z);
             common::V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos);
             common::V3D p_compensate =
-                imu_state.offset_R_L_I.conjugate() *
-                (imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) -
-                 imu_state.offset_T_L_I);  // not accurate!
+                    imu_state.offset_R_L_I.conjugate() *
+                            (imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) -
+                                    imu_state.offset_T_L_I);  // not accurate!
 
             // save Undistorted points and their rotation
             it_pcl->x = p_compensate(0);
             it_pcl->y = p_compensate(1);
             it_pcl->z = p_compensate(2);
 
-            if (it_pcl == pcl_out.points.begin()) {
+            if (it_pcl == pcl_out.points.begin())
+            {
                 break;
             }
         }
@@ -289,14 +258,18 @@ void ImuProcess::UndistortPcl(const common::MeasureGroup &meas, esekfom::esekf<s
 
 
 void ImuProcess::Process(const common::MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state,
-                         PointCloudType::Ptr cur_pcl_un_) {
-    if (meas.imu_.empty()) {
+                         PointCloudType::Ptr cur_pcl_un_)
+{
+    if (meas.imu_.empty())
+    {
         return;
     }
 
-    ROS_ASSERT(meas.lidar_ != nullptr);
+    if (meas.lidar_ == nullptr)
+        printf("imuprocess ERROR : meas.lidar_ == nullptr\n");
 
-    if (imu_need_init_) {
+    if (imu_need_init_)
+    {
         /// The very first lidar frame
         IMUInit(meas.imu_, kf_state, init_iter_num_);
 
@@ -319,9 +292,9 @@ void ImuProcess::Process(const common::MeasureGroup &meas, esekfom::esekf<state_
 
         return;
     }
-    Timer::Evaluate([&, this]() { UndistortPcl(meas, kf_state, *cur_pcl_un_); }, "Undistort Pcl");
+    Timer::Evaluate([&, this]() {
+      UndistortPcl(meas, kf_state, *cur_pcl_un_);
+    }, "Undistort Pcl");
 
 }
-}  // namespace faster_lio
-
-#endif
+}

faster-lio/src/laser_mapping.cc

@@ -336,15 +336,18 @@ void LaserMapping::IMUCallBack(const sensor_msgs::Imu::ConstPtr &msg_in) {
 
     double timestamp = msg->header.stamp.toSec();
 
-    mapper_.mtx_buffer_.lock();
-    /*if (timestamp < last_timestamp_imu_) {
-        LOG(WARNING) << "imu loop back, clear buffer";
-        imu_buffer_.clear();
-    }
 
-    last_timestamp_imu_ = timestamp;
-    imu_buffer_.emplace_back(msg);*/
-    mapper_.AddImu(msg,timestamp);
+    common::ImuData imu;
+    imu.timebase=msg_in->header.stamp.toSec();
+    imu.orientation=Eigen::Quaterniond(msg_in->orientation.w,msg_in->orientation.x,
+                                       msg_in->orientation.y,msg_in->orientation.z);
+    imu.orientation_covariance=Eigen::Matrix3d(msg_in->orientation_covariance.data());
+    imu.angular_velocity=Eigen::Vector3d(msg_in->angular_velocity.x,msg_in->angular_velocity.y,msg_in->angular_velocity.z);
+    imu.angular_velocity_covariance=Eigen::Matrix3d(msg_in->angular_velocity_covariance.data());
+    imu.linear_acceleration=Eigen::Vector3d(msg_in->linear_acceleration.x,msg_in->linear_acceleration.y,msg_in->linear_acceleration.z);
+    imu.linear_acceleration_covariance=Eigen::Matrix3d(msg_in->linear_acceleration_covariance.data());
+    mapper_.mtx_buffer_.lock();
+    mapper_.AddImu(imu,timestamp);
     mapper_.mtx_buffer_.unlock();
 }
 
@@ -906,7 +909,7 @@ void TTMapping::AddPointCloud(PointCloudType::Ptr cloud,double timebase)
     lidar_buffer_.emplace_back(cloud);
     time_buffer_.emplace_back(timebase);
 }
-void TTMapping::AddImu(const sensor_msgs::Imu::ConstPtr &msg_in,double timebase)
+void TTMapping::AddImu(const common::ImuData &imu,double timebase)
 {
     publish_count_++;
     if (timebase < last_timestamp_imu_) {
@@ -915,16 +918,6 @@ void TTMapping::AddImu(const sensor_msgs::Imu::ConstPtr &msg_in,double timebase)
     }
     last_timestamp_imu_ = timebase;
 
-    common::ImuData imu;
-    imu.timebase=msg_in->header.stamp.toSec();
-    imu.orientation=Eigen::Quaterniond(msg_in->orientation.w,msg_in->orientation.x,
-                                       msg_in->orientation.y,msg_in->orientation.z);
-    imu.orientation_covariance=Eigen::Matrix3d(msg_in->orientation_covariance.data());
-    imu.angular_velocity=Eigen::Vector3d(msg_in->angular_velocity.x,msg_in->angular_velocity.y,msg_in->angular_velocity.z);
-    imu.angular_velocity_covariance=Eigen::Matrix3d(msg_in->angular_velocity_covariance.data());
-    imu.linear_acceleration=Eigen::Vector3d(msg_in->linear_acceleration.x,msg_in->linear_acceleration.y,msg_in->linear_acceleration.z);
-    imu.linear_acceleration_covariance=Eigen::Matrix3d(msg_in->linear_acceleration_covariance.data());
-
     imu_buffer_.emplace_back(imu);
 }
 

faster-lio/include/use-ikfom.hpp

@@ -1,23 +1,9 @@
-#ifndef FASTER_LIO_USE_IKFOM_H
-#define FASTER_LIO_USE_IKFOM_H
-
-#include "IKFoM_toolkit/esekfom/esekfom.hpp"
-
+//
+// Created by zx on 22-10-31.
+//
+#include "use-ikfom.h"
 namespace faster_lio {
 
-typedef MTK::vect<3, double> vect3;
-typedef MTK::SO3<double> SO3;
-typedef MTK::S2<double, 98090, 10000, 1> S2;
-typedef MTK::vect<1, double> vect1;
-typedef MTK::vect<2, double> vect2;
-
-MTK_BUILD_MANIFOLD(state_ikfom, ((vect3, pos))((SO3, rot))((SO3, offset_R_L_I))((vect3, offset_T_L_I))((vect3, vel))(
-                                    (vect3, bg))((vect3, ba))((S2, grav)));
-
-MTK_BUILD_MANIFOLD(input_ikfom, ((vect3, acc))((vect3, gyro))); //输入数据，imu三轴加速度、角速度
-
-MTK_BUILD_MANIFOLD(process_noise_ikfom, ((vect3, ng))((vect3, na))((vect3, nbg))((vect3, nba)));
-
 MTK::get_cov<process_noise_ikfom>::type process_noise_cov() {
     MTK::get_cov<process_noise_ikfom>::type cov = MTK::get_cov<process_noise_ikfom>::type::Zero();
     MTK::setDiagonal<process_noise_ikfom, vect3, 0>(cov, &process_noise_ikfom::ng, 0.0001);  // 0.03
@@ -32,7 +18,8 @@ MTK::get_cov<process_noise_ikfom>::type process_noise_cov() {
 
 // double L_offset_to_I[3] = {0.04165, 0.02326, -0.0284}; // Avia
 // vect3 Lidar_offset_to_IMU(L_offset_to_I, 3);
-Eigen::Matrix<double, 24, 1> get_f(state_ikfom &s, const input_ikfom &in) {
+Eigen::Matrix<double, 24, 1> get_f(state_ikfom &s, const input_ikfom &in)
+{
     Eigen::Matrix<double, 24, 1> res = Eigen::Matrix<double, 24, 1>::Zero();
     vect3 omega;
     in.gyro.boxminus(omega, s.bg);
@@ -105,6 +92,4 @@ vect3 SO3ToEuler(const SO3 &orient) {
     return euler_ang;
 }
 
-}  // namespace faster_lio
-
-#endif
+}  // namespace faster_lio