edit svo

svo/include/svo/frame_handler_mono.h

@@ -22,6 +22,7 @@
 #include <svo/frame_handler_base.h>
 #include <svo/reprojector.h>
 #include <svo/initialization.h>
+#include <svo/imu_integration.h>
 
 namespace svo {
 
@@ -31,7 +32,7 @@ class FrameHandlerMono : public FrameHandlerBase
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   
-  FrameHandlerMono(vk::AbstractCamera* cam);
+  FrameHandlerMono(vk::AbstractCamera* cam,Sophus::SE3& SE_init);
   virtual ~FrameHandlerMono();
 
   /// Provide an image.
@@ -59,6 +60,7 @@ public:
       const SE3& T_kf_f,
       const cv::Mat& img,
       const double timestamp);
+  std::unique_ptr<Imu_Integration> imu_integPtr_;  //Integration of IMU
 
 protected:
   vk::AbstractCamera* cam_;                     //!< Camera model, can be ATAN, Pinhole or Ocam (see vikit).
@@ -70,6 +72,7 @@ protected:
   initialization::KltHomographyInit klt_homography_init_; //!< Used to estimate pose of the first two keyframes by estimating a homography.
   DepthFilter* depth_filter_;                   //!< Depth estimation algorithm runs in a parallel thread and is used to initialize new 3D points.
 
+
   /// Initialize the visual odometry algorithm.
   virtual void initialize();
 

svo/include/svo/imu_integration.h

@@ -0,0 +1,39 @@
+//
+// Created by gufe on 4/9/21.
+//
+
+#ifndef SVO_IMU_INTEGRATION_H
+#define SVO_IMU_INTEGRATION_H
+#include <gtsam/navigation/CombinedImuFactor.h>
+#include <gtsam/navigation/ImuFactor.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/nonlinear/ISAM2.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/navigation/ImuBias.h>
+#include <gtsam/inference/Symbol.h>
+#include <sophus/se3.h>
+#include <svo/frame.h>
+
+using gtsam::symbol_shorthand::B;  // Bias  (ax,ay,az,gx,gy,gz)
+using gtsam::symbol_shorthand::V;  // Vel   (xdot,ydot,zdot)
+using gtsam::symbol_shorthand::P;  // Pose3 (x,y,z,r,p,y)
+
+class Imu_Integration{
+public:
+    Imu_Integration(Sophus::SE3& SE_init);
+    ~Imu_Integration();
+    bool reset();
+    bool update(float* imu= nullptr);
+    bool predict(boost::shared_ptr<svo::Frame>&,std::size_t&);
+private:
+    std::shared_ptr<gtsam::NavState> statePtr;
+    std::shared_ptr<gtsam::imuBias::ConstantBias> imu_biasPtr;
+    std::unique_ptr<gtsam::Values> valuesPtr;
+    std::unique_ptr<gtsam::NonlinearFactorGraph> graphPtr;
+    boost::shared_ptr<gtsam::PreintegratedCombinedMeasurements::Params> parameterPtr;
+    std::shared_ptr<gtsam::PreintegrationType> preintegratedPtr;
+    std::uint32_t t_1=1e-18;
+    std::uint32_t imu_factor_id=0;
+};
+#endif //SVO_IMU_INTEGRATION_H