diff --git a/backend/CMakeLists.txt b/backend/CMakeLists.txt
index 09cffcf..1e2fabd 100644
--- a/backend/CMakeLists.txt
+++ b/backend/CMakeLists.txt
@@ -1,3 +1,7 @@
+add_library(backend
+    sensor_fusion/imu/imu_types.cc
+)
+
 add_executable(gauss_newton_method example/gauss_newton_method.cpp)
 target_link_libraries(gauss_newton_method tiny_ceres)
 
diff --git a/backend/sensor_fusion/imu/imu_types.cc b/backend/sensor_fusion/imu/imu_types.cc
new file mode 100644
index 0000000..3dd4cb8
--- /dev/null
+++ b/backend/sensor_fusion/imu/imu_types.cc
@@ -0,0 +1,393 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel
+ * and Juan D. Tardós, University of Zaragoza. Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M.
+ * Montiel and Juan D. Tardós, University of Zaragoza.
+ *
+ * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU
+ * General Public License as published by the Free Software Foundation, either version 3 of the
+ * License, or (at your option) any later version.
+ *
+ * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without
+ * even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along with ORB-SLAM3.
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "backend/sensor_fusion/imu/imu_types.h"
+
+#include <iostream>
+
+namespace backend {
+
+namespace IMU {
+
+const float eps = 1e-4;
+
+Eigen::Matrix3f NormalizeRotation( const Eigen::Matrix3f &R ) {
+  Eigen::JacobiSVD<Eigen::Matrix3f> svd( R, Eigen::ComputeFullU | Eigen::ComputeFullV );
+  return svd.matrixU() * svd.matrixV().transpose();
+}
+
+Eigen::Matrix3f RightJacobianSO3( const float &x, const float &y, const float &z ) {
+  Eigen::Matrix3f I;
+  I.setIdentity();
+  const float d2 = x * x + y * y + z * z;
+  const float d = sqrt( d2 );
+  Eigen::Vector3f v;
+  v << x, y, z;
+  Eigen::Matrix3f W = Sophus::SO3f::hat( v );
+  if ( d < eps ) {
+    return I;
+  } else {
+    return I - W * ( 1.0f - cos( d ) ) / d2 + W * W * ( d - sin( d ) ) / ( d2 * d );
+  }
+}
+
+Eigen::Matrix3f RightJacobianSO3( const Eigen::Vector3f &v ) {
+  return RightJacobianSO3( v( 0 ), v( 1 ), v( 2 ) );
+}
+
+Eigen::Matrix3f InverseRightJacobianSO3( const float &x, const float &y, const float &z ) {
+  Eigen::Matrix3f I;
+  I.setIdentity();
+  const float d2 = x * x + y * y + z * z;
+  const float d = sqrt( d2 );
+  Eigen::Vector3f v;
+  v << x, y, z;
+  Eigen::Matrix3f W = Sophus::SO3f::hat( v );
+
+  if ( d < eps ) {
+    return I;
+  } else {
+    return I + W / 2 + W * W * ( 1.0f / d2 - ( 1.0f + cos( d ) ) / ( 2.0f * d * sin( d ) ) );
+  }
+}
+
+Eigen::Matrix3f InverseRightJacobianSO3( const Eigen::Vector3f &v ) {
+  return InverseRightJacobianSO3( v( 0 ), v( 1 ), v( 2 ) );
+}
+
+IntegratedRotation::IntegratedRotation( const Eigen::Vector3f &angVel, const Bias &imuBias,
+                                        const float &time ) {
+  const float x = ( angVel( 0 ) - imuBias.bwx ) * time;
+  const float y = ( angVel( 1 ) - imuBias.bwy ) * time;
+  const float z = ( angVel( 2 ) - imuBias.bwz ) * time;
+
+  const float d2 = x * x + y * y + z * z;
+  const float d = sqrt( d2 );
+
+  Eigen::Vector3f v;
+  v << x, y, z;
+  Eigen::Matrix3f W = Sophus::SO3f::hat( v );
+  if ( d < eps ) {
+    deltaR = Eigen::Matrix3f::Identity() + W;
+    rightJ = Eigen::Matrix3f::Identity();
+  } else {
+    deltaR = Eigen::Matrix3f::Identity() + W * sin( d ) / d + W * W * ( 1.0f - cos( d ) ) / d2;
+    rightJ = Eigen::Matrix3f::Identity() - W * ( 1.0f - cos( d ) ) / d2 +
+             W * W * ( d - sin( d ) ) / ( d2 * d );
+  }
+}
+
+Preintegrated::Preintegrated( const Bias &b_, const Calib &calib ) {
+  Nga = calib.Cov;
+  NgaWalk = calib.CovWalk;
+  Initialize( b_ );
+}
+
+// Copy constructor
+Preintegrated::Preintegrated( Preintegrated *pImuPre )
+    : dT( pImuPre->dT ),
+      C( pImuPre->C ),
+      Info( pImuPre->Info ),
+      Nga( pImuPre->Nga ),
+      NgaWalk( pImuPre->NgaWalk ),
+      b( pImuPre->b ),
+      dR( pImuPre->dR ),
+      dV( pImuPre->dV ),
+      dP( pImuPre->dP ),
+      JRg( pImuPre->JRg ),
+      JVg( pImuPre->JVg ),
+      JVa( pImuPre->JVa ),
+      JPg( pImuPre->JPg ),
+      JPa( pImuPre->JPa ),
+      avgA( pImuPre->avgA ),
+      avgW( pImuPre->avgW ),
+      bu( pImuPre->bu ),
+      db( pImuPre->db ),
+      mvMeasurements( pImuPre->mvMeasurements ) {}
+
+void Preintegrated::CopyFrom( Preintegrated *pImuPre ) {
+  dT = pImuPre->dT;
+  C = pImuPre->C;
+  Info = pImuPre->Info;
+  Nga = pImuPre->Nga;
+  NgaWalk = pImuPre->NgaWalk;
+  b.CopyFrom( pImuPre->b );
+  dR = pImuPre->dR;
+  dV = pImuPre->dV;
+  dP = pImuPre->dP;
+  JRg = pImuPre->JRg;
+  JVg = pImuPre->JVg;
+  JVa = pImuPre->JVa;
+  JPg = pImuPre->JPg;
+  JPa = pImuPre->JPa;
+  avgA = pImuPre->avgA;
+  avgW = pImuPre->avgW;
+  bu.CopyFrom( pImuPre->bu );
+  db = pImuPre->db;
+  mvMeasurements = pImuPre->mvMeasurements;
+}
+
+void Preintegrated::Initialize( const Bias &b_ ) {
+  dR.setIdentity();
+  dV.setZero();
+  dP.setZero();
+  JRg.setZero();
+  JVg.setZero();
+  JVa.setZero();
+  JPg.setZero();
+  JPa.setZero();
+  C.setZero();
+  Info.setZero();
+  db.setZero();
+  b = b_;
+  bu = b_;
+  avgA.setZero();
+  avgW.setZero();
+  dT = 0.0f;
+  mvMeasurements.clear();
+}
+
+void Preintegrated::Reintegrate() {
+  std::unique_lock<std::mutex> lock( mMutex );
+  const std::vector<integrable> aux = mvMeasurements;
+  Initialize( bu );
+  for ( size_t i = 0; i < aux.size(); i++ ) IntegrateNewMeasurement( aux[i].a, aux[i].w, aux[i].t );
+}
+
+void Preintegrated::IntegrateNewMeasurement( const Eigen::Vector3f &acceleration,
+                                             const Eigen::Vector3f &angVel, const float &dt ) {
+  mvMeasurements.push_back( integrable( acceleration, angVel, dt ) );
+
+  // Position is updated firstly, as it depends on previously computed velocity and rotation.
+  // Velocity is updated secondly, as it depends on previously computed rotation.
+  // Rotation is the last to be updated.
+
+  // Matrices to compute covariance
+  Eigen::Matrix<float, 9, 9> A;
+  A.setIdentity();
+  Eigen::Matrix<float, 9, 6> B;
+  B.setZero();
+
+  Eigen::Vector3f acc, accW;
+  acc << acceleration( 0 ) - b.bax, acceleration( 1 ) - b.bay, acceleration( 2 ) - b.baz;
+  accW << angVel( 0 ) - b.bwx, angVel( 1 ) - b.bwy, angVel( 2 ) - b.bwz;
+
+  avgA = ( dT * avgA + dR * acc * dt ) / ( dT + dt );
+  avgW = ( dT * avgW + accW * dt ) / ( dT + dt );
+
+  // Update delta position dP and velocity dV (rely on no-updated delta rotation)
+  dP = dP + dV * dt + 0.5f * dR * acc * dt * dt;
+  dV = dV + dR * acc * dt;
+
+  // Compute velocity and position parts of matrices A and B (rely on non-updated delta rotation)
+  Eigen::Matrix<float, 3, 3> Wacc = Sophus::SO3f::hat( acc );
+
+  A.block<3, 3>( 3, 0 ) = -dR * dt * Wacc;
+  A.block<3, 3>( 6, 0 ) = -0.5f * dR * dt * dt * Wacc;
+  A.block<3, 3>( 6, 3 ) = Eigen::DiagonalMatrix<float, 3>( dt, dt, dt );
+  B.block<3, 3>( 3, 3 ) = dR * dt;
+  B.block<3, 3>( 6, 3 ) = 0.5f * dR * dt * dt;
+
+  // Update position and velocity jacobians wrt bias correction
+  JPa = JPa + JVa * dt - 0.5f * dR * dt * dt;
+  JPg = JPg + JVg * dt - 0.5f * dR * dt * dt * Wacc * JRg;
+  JVa = JVa - dR * dt;
+  JVg = JVg - dR * dt * Wacc * JRg;
+
+  // Update delta rotation
+  IntegratedRotation dRi( angVel, b, dt );
+  dR = NormalizeRotation( dR * dRi.deltaR );
+
+  // Compute rotation parts of matrices A and B
+  A.block<3, 3>( 0, 0 ) = dRi.deltaR.transpose();
+  B.block<3, 3>( 0, 0 ) = dRi.rightJ * dt;
+
+  // Update covariance
+  C.block<9, 9>( 0, 0 ) = A * C.block<9, 9>( 0, 0 ) * A.transpose() + B * Nga * B.transpose();
+  C.block<6, 6>( 9, 9 ) += NgaWalk;
+
+  // Update rotation jacobian wrt bias correction
+  JRg = dRi.deltaR.transpose() * JRg - dRi.rightJ * dt;
+
+  // Total integrated time
+  dT += dt;
+}
+
+void Preintegrated::MergePrevious( Preintegrated *pPrev ) {
+  if ( pPrev == this ) return;
+
+  std::unique_lock<std::mutex> lock1( mMutex );
+  std::unique_lock<std::mutex> lock2( pPrev->mMutex );
+  Bias bav;
+  bav.bwx = bu.bwx;
+  bav.bwy = bu.bwy;
+  bav.bwz = bu.bwz;
+  bav.bax = bu.bax;
+  bav.bay = bu.bay;
+  bav.baz = bu.baz;
+
+  const std::vector<integrable> aux1 = pPrev->mvMeasurements;
+  const std::vector<integrable> aux2 = mvMeasurements;
+
+  Initialize( bav );
+  for ( size_t i = 0; i < aux1.size(); i++ )
+    IntegrateNewMeasurement( aux1[i].a, aux1[i].w, aux1[i].t );
+  for ( size_t i = 0; i < aux2.size(); i++ )
+    IntegrateNewMeasurement( aux2[i].a, aux2[i].w, aux2[i].t );
+}
+
+void Preintegrated::SetNewBias( const Bias &bu_ ) {
+  std::unique_lock<std::mutex> lock( mMutex );
+  bu = bu_;
+
+  db( 0 ) = bu_.bwx - b.bwx;
+  db( 1 ) = bu_.bwy - b.bwy;
+  db( 2 ) = bu_.bwz - b.bwz;
+  db( 3 ) = bu_.bax - b.bax;
+  db( 4 ) = bu_.bay - b.bay;
+  db( 5 ) = bu_.baz - b.baz;
+}
+
+IMU::Bias Preintegrated::GetDeltaBias( const Bias &b_ ) {
+  std::unique_lock<std::mutex> lock( mMutex );
+  return IMU::Bias( b_.bax - b.bax, b_.bay - b.bay, b_.baz - b.baz, b_.bwx - b.bwx, b_.bwy - b.bwy,
+                    b_.bwz - b.bwz );
+}
+
+Eigen::Matrix3f Preintegrated::GetDeltaRotation( const Bias &b_ ) {
+  std::unique_lock<std::mutex> lock( mMutex );
+  Eigen::Vector3f dbg;
+  dbg << b_.bwx - b.bwx, b_.bwy - b.bwy, b_.bwz - b.bwz;
+  return NormalizeRotation( dR * Sophus::SO3f::exp( JRg * dbg ).matrix() );
+}
+
+Eigen::Vector3f Preintegrated::GetDeltaVelocity( const Bias &b_ ) {
+  std::unique_lock<std::mutex> lock( mMutex );
+  Eigen::Vector3f dbg, dba;
+  dbg << b_.bwx - b.bwx, b_.bwy - b.bwy, b_.bwz - b.bwz;
+  dba << b_.bax - b.bax, b_.bay - b.bay, b_.baz - b.baz;
+  return dV + JVg * dbg + JVa * dba;
+}
+
+Eigen::Vector3f Preintegrated::GetDeltaPosition( const Bias &b_ ) {
+  std::unique_lock<std::mutex> lock( mMutex );
+  Eigen::Vector3f dbg, dba;
+  dbg << b_.bwx - b.bwx, b_.bwy - b.bwy, b_.bwz - b.bwz;
+  dba << b_.bax - b.bax, b_.bay - b.bay, b_.baz - b.baz;
+  return dP + JPg * dbg + JPa * dba;
+}
+
+Eigen::Matrix3f Preintegrated::GetUpdatedDeltaRotation() {
+  std::unique_lock<std::mutex> lock( mMutex );
+  return NormalizeRotation( dR * Sophus::SO3f::exp( JRg * db.head( 3 ) ).matrix() );
+}
+
+Eigen::Vector3f Preintegrated::GetUpdatedDeltaVelocity() {
+  std::unique_lock<std::mutex> lock( mMutex );
+  return dV + JVg * db.head( 3 ) + JVa * db.tail( 3 );
+}
+
+Eigen::Vector3f Preintegrated::GetUpdatedDeltaPosition() {
+  std::unique_lock<std::mutex> lock( mMutex );
+  return dP + JPg * db.head( 3 ) + JPa * db.tail( 3 );
+}
+
+Eigen::Matrix3f Preintegrated::GetOriginalDeltaRotation() {
+  std::unique_lock<std::mutex> lock( mMutex );
+  return dR;
+}
+
+Eigen::Vector3f Preintegrated::GetOriginalDeltaVelocity() {
+  std::unique_lock<std::mutex> lock( mMutex );
+  return dV;
+}
+
+Eigen::Vector3f Preintegrated::GetOriginalDeltaPosition() {
+  std::unique_lock<std::mutex> lock( mMutex );
+  return dP;
+}
+
+Bias Preintegrated::GetOriginalBias() {
+  std::unique_lock<std::mutex> lock( mMutex );
+  return b;
+}
+
+Bias Preintegrated::GetUpdatedBias() {
+  std::unique_lock<std::mutex> lock( mMutex );
+  return bu;
+}
+
+Eigen::Matrix<float, 6, 1> Preintegrated::GetDeltaBias() {
+  std::unique_lock<std::mutex> lock( mMutex );
+  return db;
+}
+
+void Bias::CopyFrom( Bias &b ) {
+  bax = b.bax;
+  bay = b.bay;
+  baz = b.baz;
+  bwx = b.bwx;
+  bwy = b.bwy;
+  bwz = b.bwz;
+}
+
+std::ostream &operator<<( std::ostream &out, const Bias &b ) {
+  if ( b.bwx > 0 ) out << " ";
+  out << b.bwx << ",";
+  if ( b.bwy > 0 ) out << " ";
+  out << b.bwy << ",";
+  if ( b.bwz > 0 ) out << " ";
+  out << b.bwz << ",";
+  if ( b.bax > 0 ) out << " ";
+  out << b.bax << ",";
+  if ( b.bay > 0 ) out << " ";
+  out << b.bay << ",";
+  if ( b.baz > 0 ) out << " ";
+  out << b.baz;
+
+  return out;
+}
+
+void Calib::Set( const Sophus::SE3<float> &sophTbc, const float &ng, const float &na,
+                 const float &ngw, const float &naw ) {
+  mbIsSet = true;
+  const float ng2 = ng * ng;
+  const float na2 = na * na;
+  const float ngw2 = ngw * ngw;
+  const float naw2 = naw * naw;
+
+  // Sophus/Eigen
+  mTbc = sophTbc;
+  mTcb = mTbc.inverse();
+  Cov.diagonal() << ng2, ng2, ng2, na2, na2, na2;
+  CovWalk.diagonal() << ngw2, ngw2, ngw2, naw2, naw2, naw2;
+}
+
+Calib::Calib( const Calib &calib ) {
+  mbIsSet = calib.mbIsSet;
+  // Sophus/Eigen parameters
+  mTbc = calib.mTbc;
+  mTcb = calib.mTcb;
+  Cov = calib.Cov;
+  CovWalk = calib.CovWalk;
+}
+
+}  // namespace IMU
+
+}  // namespace backend
diff --git a/backend/sensor_fusion/imu/imu_types.h b/backend/sensor_fusion/imu/imu_types.h
new file mode 100644
index 0000000..5d08e61
--- /dev/null
+++ b/backend/sensor_fusion/imu/imu_types.h
@@ -0,0 +1,268 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel
+ * and Juan D. Tardós, University of Zaragoza. Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M.
+ * Montiel and Juan D. Tardós, University of Zaragoza.
+ *
+ * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU
+ * General Public License as published by the Free Software Foundation, either version 3 of the
+ * License, or (at your option) any later version.
+ *
+ * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without
+ * even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along with ORB-SLAM3.
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef IMUTYPES_H
+#define IMUTYPES_H
+
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+#include <boost/serialization/serialization.hpp>
+#include <boost/serialization/vector.hpp>
+#include <mutex>
+#include <opencv2/core/core.hpp>
+#include <sophus/se3.hpp>
+#include <utility>
+#include <vector>
+
+#include "backend/sensor_fusion/imu/serialization_utils.h"
+
+namespace backend {
+
+namespace IMU {
+
+// TODO
+const float GRAVITY_VALUE = 9.81;
+
+// IMU measurement (gyro, accelerometer and timestamp)
+class Point
+{
+public:
+  Point( const float &acc_x, const float &acc_y, const float &acc_z, const float &ang_vel_x,
+         const float &ang_vel_y, const float &ang_vel_z, const double &timestamp )
+      : a( acc_x, acc_y, acc_z ), w( ang_vel_x, ang_vel_y, ang_vel_z ), t( timestamp ) {}
+  Point( const cv::Point3f Acc, const cv::Point3f Gyro, const double &timestamp )
+      : a( Acc.x, Acc.y, Acc.z ), w( Gyro.x, Gyro.y, Gyro.z ), t( timestamp ) {}
+
+public:
+  Eigen::Vector3f a;
+  Eigen::Vector3f w;
+  double t;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+// IMU biases (gyro and accelerometer)
+class Bias
+{
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize( Archive &ar, const unsigned int version ) {
+    ar & bax;
+    ar & bay;
+    ar & baz;
+
+    ar & bwx;
+    ar & bwy;
+    ar & bwz;
+  }
+
+public:
+  Bias() : bax( 0 ), bay( 0 ), baz( 0 ), bwx( 0 ), bwy( 0 ), bwz( 0 ) {}
+  Bias( const float &b_acc_x, const float &b_acc_y, const float &b_acc_z, const float &b_ang_vel_x,
+        const float &b_ang_vel_y, const float &b_ang_vel_z )
+      : bax( b_acc_x ),
+        bay( b_acc_y ),
+        baz( b_acc_z ),
+        bwx( b_ang_vel_x ),
+        bwy( b_ang_vel_y ),
+        bwz( b_ang_vel_z ) {}
+  void CopyFrom( Bias &b );
+  friend std::ostream &operator<<( std::ostream &out, const Bias &b );
+
+public:
+  float bax, bay, baz;
+  float bwx, bwy, bwz;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+// IMU calibration (Tbc, Tcb, noise)
+class Calib
+{
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize( Archive &ar, const unsigned int version ) {
+    serializeSophusSE3( ar, mTcb, version );
+    serializeSophusSE3( ar, mTbc, version );
+
+    ar &boost::serialization::make_array( Cov.diagonal().data(), Cov.diagonal().size() );
+    ar &boost::serialization::make_array( CovWalk.diagonal().data(), CovWalk.diagonal().size() );
+
+    ar & mbIsSet;
+  }
+
+public:
+  Calib( const Sophus::SE3<float> &Tbc, const float &ng, const float &na, const float &ngw,
+         const float &naw ) {
+    Set( Tbc, ng, na, ngw, naw );
+  }
+
+  Calib( const Calib &calib );
+  Calib() { mbIsSet = false; }
+
+  // void Set(const cv::Mat &cvTbc, const float &ng, const float &na, const float &ngw, const float
+  // &naw);
+  void Set( const Sophus::SE3<float> &sophTbc, const float &ng, const float &na, const float &ngw,
+            const float &naw );
+
+public:
+  // Sophus/Eigen implementation
+  Sophus::SE3<float> mTcb;
+  Sophus::SE3<float> mTbc;
+  Eigen::DiagonalMatrix<float, 6> Cov, CovWalk;
+  bool mbIsSet;
+};
+
+// Integration of 1 gyro measurement
+class IntegratedRotation
+{
+public:
+  IntegratedRotation() {}
+  IntegratedRotation( const Eigen::Vector3f &angVel, const Bias &imuBias, const float &time );
+
+public:
+  float deltaT;  // integration time
+  Eigen::Matrix3f deltaR;
+  Eigen::Matrix3f rightJ;  // right jacobian
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+// Preintegration of Imu Measurements
+class Preintegrated
+{
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize( Archive &ar, const unsigned int version ) {
+    ar & dT;
+    ar &boost::serialization::make_array( C.data(), C.size() );
+    ar &boost::serialization::make_array( Info.data(), Info.size() );
+    ar &boost::serialization::make_array( Nga.diagonal().data(), Nga.diagonal().size() );
+    ar &boost::serialization::make_array( NgaWalk.diagonal().data(), NgaWalk.diagonal().size() );
+    ar & b;
+    ar &boost::serialization::make_array( dR.data(), dR.size() );
+    ar &boost::serialization::make_array( dV.data(), dV.size() );
+    ar &boost::serialization::make_array( dP.data(), dP.size() );
+    ar &boost::serialization::make_array( JRg.data(), JRg.size() );
+    ar &boost::serialization::make_array( JVg.data(), JVg.size() );
+    ar &boost::serialization::make_array( JVa.data(), JVa.size() );
+    ar &boost::serialization::make_array( JPg.data(), JPg.size() );
+    ar &boost::serialization::make_array( JPa.data(), JPa.size() );
+    ar &boost::serialization::make_array( avgA.data(), avgA.size() );
+    ar &boost::serialization::make_array( avgW.data(), avgW.size() );
+
+    ar & bu;
+    ar &boost::serialization::make_array( db.data(), db.size() );
+    ar & mvMeasurements;
+  }
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  Preintegrated( const Bias &b_, const Calib &calib );
+  Preintegrated( Preintegrated *pImuPre );
+  Preintegrated() {}
+  ~Preintegrated() {}
+  void CopyFrom( Preintegrated *pImuPre );
+  void Initialize( const Bias &b_ );
+  void IntegrateNewMeasurement( const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel,
+                                const float &dt );
+  void Reintegrate();
+  void MergePrevious( Preintegrated *pPrev );
+  void SetNewBias( const Bias &bu_ );
+  IMU::Bias GetDeltaBias( const Bias &b_ );
+
+  Eigen::Matrix3f GetDeltaRotation( const Bias &b_ );
+  Eigen::Vector3f GetDeltaVelocity( const Bias &b_ );
+  Eigen::Vector3f GetDeltaPosition( const Bias &b_ );
+
+  Eigen::Matrix3f GetUpdatedDeltaRotation();
+  Eigen::Vector3f GetUpdatedDeltaVelocity();
+  Eigen::Vector3f GetUpdatedDeltaPosition();
+
+  Eigen::Matrix3f GetOriginalDeltaRotation();
+  Eigen::Vector3f GetOriginalDeltaVelocity();
+  Eigen::Vector3f GetOriginalDeltaPosition();
+
+  Eigen::Matrix<float, 6, 1> GetDeltaBias();
+
+  Bias GetOriginalBias();
+  Bias GetUpdatedBias();
+
+  void printMeasurements() const {
+    std::cout << "pint meas:\n";
+    for ( int i = 0; i < mvMeasurements.size(); i++ ) {
+      std::cout << "meas " << mvMeasurements[i].t << std::endl;
+    }
+    std::cout << "end pint meas:\n";
+  }
+
+public:
+  float dT;
+  Eigen::Matrix<float, 15, 15> C;
+  Eigen::Matrix<float, 15, 15> Info;
+  Eigen::DiagonalMatrix<float, 6> Nga, NgaWalk;
+
+  // Values for the original bias (when integration was computed)
+  Bias b;
+  Eigen::Matrix3f dR;
+  Eigen::Vector3f dV, dP;
+  Eigen::Matrix3f JRg, JVg, JVa, JPg, JPa;
+  Eigen::Vector3f avgA, avgW;
+
+private:
+  // Updated bias
+  Bias bu;
+  // Dif between original and updated bias
+  // This is used to compute the updated values of the preintegration
+  Eigen::Matrix<float, 6, 1> db;
+
+  struct integrable
+  {
+    template <class Archive>
+    void serialize( Archive &ar, const unsigned int version ) {
+      ar &boost::serialization::make_array( a.data(), a.size() );
+      ar &boost::serialization::make_array( w.data(), w.size() );
+      ar & t;
+    }
+
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    integrable() {}
+    integrable( const Eigen::Vector3f &a_, const Eigen::Vector3f &w_, const float &t_ )
+        : a( a_ ), w( w_ ), t( t_ ) {}
+    Eigen::Vector3f a, w;
+    float t;
+  };
+
+  std::vector<integrable> mvMeasurements;
+
+  std::mutex mMutex;
+};
+
+// Lie Algebra Functions
+Eigen::Matrix3f RightJacobianSO3( const float &x, const float &y, const float &z );
+Eigen::Matrix3f RightJacobianSO3( const Eigen::Vector3f &v );
+
+Eigen::Matrix3f InverseRightJacobianSO3( const float &x, const float &y, const float &z );
+Eigen::Matrix3f InverseRightJacobianSO3( const Eigen::Vector3f &v );
+
+Eigen::Matrix3f NormalizeRotation( const Eigen::Matrix3f &R );
+
+}  // namespace IMU
+
+}  // namespace backend
+
+#endif  // IMUTYPES_H
diff --git a/backend/sensor_fusion/imu/serialization_utils.h b/backend/sensor_fusion/imu/serialization_utils.h
new file mode 100644
index 0000000..ac30983
--- /dev/null
+++ b/backend/sensor_fusion/imu/serialization_utils.h
@@ -0,0 +1,134 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel
+ * and Juan D. Tardós, University of Zaragoza. Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M.
+ * Montiel and Juan D. Tardós, University of Zaragoza.
+ *
+ * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU
+ * General Public License as published by the Free Software Foundation, either version 3 of the
+ * License, or (at your option) any later version.
+ *
+ * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without
+ * even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along with ORB-SLAM3.
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef SERIALIZATION_UTILS_H
+#define SERIALIZATION_UTILS_H
+
+#include <Eigen/Core>
+#include <boost/serialization/serialization.hpp>
+#include <boost/serialization/vector.hpp>
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <sophus/se3.hpp>
+#include <vector>
+
+namespace ORB_SLAM3 {
+
+template <class Archive>
+void serializeSophusSE3( Archive& ar, Sophus::SE3f& T, const unsigned int version ) {
+  Eigen::Vector4f quat;
+  Eigen::Vector3f transl;
+
+  if ( Archive::is_saving::value ) {
+    Eigen::Quaternionf q = T.unit_quaternion();
+    quat << q.w(), q.x(), q.y(), q.z();
+    transl = T.translation();
+  }
+
+  ar& boost::serialization::make_array( quat.data(), quat.size() );
+  ar& boost::serialization::make_array( transl.data(), transl.size() );
+
+  if ( Archive::is_loading::value ) {
+    Eigen::Quaternionf q( quat[0], quat[1], quat[2], quat[3] );
+    T = Sophus::SE3f( q, transl );
+  }
+}
+
+template <class Archive>
+void serializeMatrix( Archive& ar, cv::Mat& mat, const unsigned int version ) {
+  int cols, rows, type;
+  bool continuous;
+
+  if ( Archive::is_saving::value ) {
+    cols = mat.cols;
+    rows = mat.rows;
+    type = mat.type();
+    continuous = mat.isContinuous();
+  }
+
+  ar & cols & rows & type & continuous;
+
+  if ( Archive::is_loading::value ) mat.create( rows, cols, type );
+
+  if ( continuous ) {
+    const unsigned int data_size = rows * cols * mat.elemSize();
+    ar& boost::serialization::make_array( mat.ptr(), data_size );
+  } else {
+    const unsigned int row_size = cols * mat.elemSize();
+    for ( int i = 0; i < rows; i++ ) {
+      ar& boost::serialization::make_array( mat.ptr( i ), row_size );
+    }
+  }
+}
+
+template <class Archive>
+void serializeMatrix( Archive& ar, const cv::Mat& mat, const unsigned int version ) {
+  cv::Mat matAux = mat;
+
+  serializeMatrix( ar, matAux, version );
+
+  if ( Archive::is_loading::value ) {
+    cv::Mat* ptr;
+    ptr = (cv::Mat*)( &mat );
+    *ptr = matAux;
+  }
+}
+
+template <class Archive>
+void serializeVectorKeyPoints( Archive& ar, const std::vector<cv::KeyPoint>& vKP,
+                               const unsigned int version ) {
+  int NumEl;
+
+  if ( Archive::is_saving::value ) {
+    NumEl = vKP.size();
+  }
+
+  ar & NumEl;
+
+  std::vector<cv::KeyPoint> vKPaux = vKP;
+  if ( Archive::is_loading::value ) vKPaux.reserve( NumEl );
+
+  for ( int i = 0; i < NumEl; ++i ) {
+    cv::KeyPoint KPi;
+
+    if ( Archive::is_loading::value ) KPi = cv::KeyPoint();
+
+    if ( Archive::is_saving::value ) KPi = vKPaux[i];
+
+    ar & KPi.angle;
+    ar & KPi.response;
+    ar & KPi.size;
+    ar & KPi.pt.x;
+    ar & KPi.pt.y;
+    ar & KPi.class_id;
+    ar & KPi.octave;
+
+    if ( Archive::is_loading::value ) vKPaux.push_back( KPi );
+  }
+
+  if ( Archive::is_loading::value ) {
+    std::vector<cv::KeyPoint>* ptr;
+    ptr = (std::vector<cv::KeyPoint>*)( &vKP );
+    *ptr = vKPaux;
+  }
+}
+
+}  // namespace ORB_SLAM3
+
+#endif  // SERIALIZATION_UTILS_H