edit svo

svo/CMakeLists.txt

@@ -14,6 +14,8 @@ CMAKE_MINIMUM_REQUIRED (VERSION 2.8.3)
 SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo
 SET(CMAKE_VERBOSE_MAKEFILE OFF)
 SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/CMakeModules/")
+set(CMAKE_C_STANDARD 14)
+set(CMAKE_CXX_STANDARD 14)
 
 # Set definitions
 IF(TRACE)
@@ -33,7 +35,7 @@ ENDIF()
 # Set build flags. Set IS_ARM on odroid board as environment variable
 SET(CMAKE_CXX_FLAGS "-Wall -D_LINUX -D_REENTRANT -march=native -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas")
 IF(DEFINED ENV{ARM_ARCHITECTURE})
-  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -march=armv7-a")
+  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ")
 ELSE()
   SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3")
 ENDIF()
@@ -50,11 +52,13 @@ FIND_PACKAGE(Eigen REQUIRED)
 FIND_PACKAGE(Sophus REQUIRED)
 FIND_PACKAGE(fast REQUIRED)
 FIND_PACKAGE(Boost REQUIRED COMPONENTS thread system)
+FIND_PACKAGE(GTSAM REQUIRED)
 
+set(GTSAM_INCLUDE_DIRS "${GTSAM_INCLUDE_DIR}")
 IF(USE_ROS)
   FIND_PACKAGE(catkin REQUIRED COMPONENTS roscpp roslib cmake_modules vikit_common vikit_ros)
   catkin_package(
-      DEPENDS Eigen OpenCV Sophus Boost fast
+      DEPENDS Eigen OpenCV Sophus Boost fast GTSAM
       CATKIN_DEPENDS roscpp roslib vikit_common vikit_ros
       INCLUDE_DIRS include
       LIBRARIES svo
@@ -72,7 +76,7 @@ INCLUDE_DIRECTORIES(
   ${OpenCV_INCLUDE_DIRS}
   ${Sophus_INCLUDE_DIRS}
   ${Boost_INCLUDE_DIRS}
-  ${fast_INCLUDE_DIRS}
+  ${fast_INCLUDE_DIRS} ${GTSAM_INCLUDE_DIR}
 )
 
 # Set link libraries
@@ -82,7 +86,6 @@ LIST(APPEND LINK_LIBS
   ${Boost_LIBRARIES}
   ${fast_LIBRARIES}
 )
-
 IF(USE_ROS)
   INCLUDE_DIRECTORIES(${catkin_INCLUDE_DIRS})
   LIST(APPEND LINK_LIBS ${catkin_LIBRARIES})
@@ -106,7 +109,7 @@ LIST(APPEND SOURCEFILES
   src/feature_detection.cpp
   src/depth_filter.cpp
   src/config.cpp
-  src/sparse_img_align.cpp)
+  src/sparse_img_align.cpp src/imu_integration.cpp)
 
 # Add g2o if available
 IF(HAVE_G2O)
@@ -124,7 +127,9 @@ ENDIF(HAVE_G2O)
 
 # Create svo library
 ADD_LIBRARY(svo SHARED ${SOURCEFILES})
-TARGET_LINK_LIBRARIES(svo ${LINK_LIBS})
+TARGET_LINK_LIBRARIES(svo ${LINK_LIBS} gtsam)
+set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 14)
+set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD_REQUIRED ON)
 
 ################################################################################
 # TESTS

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

svo/src/frame_handler_mono.cpp

@@ -30,13 +30,14 @@
 
 namespace svo {
 
-FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam) :
+FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Sophus::SE3& SE_init) :
   FrameHandlerBase(),
   cam_(cam),
   reprojector_(cam_, map_),
   depth_filter_(NULL)
 {
   initialize();
+  imu_integPtr_=std::make_unique<Imu_Integration>(SE_init);
 }
 
 void FrameHandlerMono::initialize()
@@ -159,6 +160,9 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   // pose optimization subject to change for adding IMU
   SVO_START_TIMER("pose_optimizer");
   size_t sfba_n_edges_final;
+  if(!imu_integPtr_->predict(new_frame_,sfba_n_edges_final))
+        return RESULT_FAILURE;
+  /*
   double sfba_thresh, sfba_error_init, sfba_error_final;
   pose_optimizer::optimizeGaussNewton(
       Config::poseOptimThresh(), Config::poseOptimNumIter(), false,
@@ -167,6 +171,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   SVO_LOG4(sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final);
   SVO_DEBUG_STREAM("PoseOptimizer:\t ErrInit = "<<sfba_error_init<<"px\t thresh = "<<sfba_thresh);
   SVO_DEBUG_STREAM("PoseOptimizer:\t ErrFin. = "<<sfba_error_final<<"px\t nObsFin. = "<<sfba_n_edges_final);
+   */
   if(sfba_n_edges_final < 20)
     return RESULT_FAILURE;
 

svo/src/imu_integration.cpp

@@ -0,0 +1,58 @@
+//
+// Created by gfuse on 4/9/21.
+//
+//#define NDEBUG
+#include "svo/imu_integration.h"
+#include <memory>
+#include <utility>
+
+Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
+    statePtr=std::make_shared<gtsam::NavState>(gtsam::Pose3(gtsam::Rot3(SE_init.rotation_matrix()),SE_init.translation()),
+                                               gtsam::Vector3(0.0,0.0,0.0));
+    imu_biasPtr=std::make_shared<gtsam::imuBias::ConstantBias>();
+    valuesPtr=std::make_unique<gtsam::Values>();
+    valuesPtr->insert(P(0),statePtr->pose());
+    valuesPtr->insert(V(0),statePtr->v());
+    valuesPtr->insert(B(0),*imu_biasPtr.get());
+    graphPtr=std::make_unique<gtsam::NonlinearFactorGraph>();
+    graphPtr->addPrior(P(0),statePtr->pose(),gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5).finished()));
+    graphPtr->addPrior(V(0),statePtr->v(),gtsam::noiseModel::Isotropic::Sigma(3, 0.1));
+    graphPtr->addPrior(B(0),*imu_biasPtr.get(),gtsam::noiseModel::Isotropic::Sigma(6, 1e-3));
+    parameterPtr = gtsam::PreintegratedCombinedMeasurements::Params::MakeSharedD();
+    parameterPtr->accelerometerCovariance=gtsam::I_3x3 * pow(0.0003924, 2); // acc white noise in continuous
+    parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * pow(0.00020568902491,2); // gyro white noise in continuous
+    parameterPtr->integrationCovariance=gtsam::I_3x3 * 1e-8; // integration uncertainty continuous
+    // PreintegrationCombinedMeasurements params:
+    parameterPtr->biasAccCovariance = gtsam::I_3x3 * pow(0.004905, 2);;   // acc bias in continuous
+    parameterPtr->biasOmegaCovariance = gtsam::I_3x3 * pow(0.000001454441043, 2);  // gyro bias in continuous
+    parameterPtr->biasAccOmegaInt = gtsam::I_6x6 * 1e-5; // error in the bias used for preintegration
+    preintegratedPtr = std::make_shared<gtsam::PreintegrationType>(parameterPtr,*imu_biasPtr.get());
+    assert(preintegratedPtr);
+}
+Imu_Integration::~Imu_Integration(){
+
+}
+bool Imu_Integration::update(float* imu){
+    if(t_1<1e-4){
+        preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0],imu[1],imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]),0.005);
+        t_1=imu[6];
+    }else{
+        preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0],imu[1],imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]),imu[6]-t_1);
+        t_1=imu[6];
+    }
+    return true;
+}
+bool Imu_Integration::reset(){
+    preintegratedPtr->resetIntegrationAndSetBias(*imu_biasPtr.get());
+    return true;
+}
+bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,std::size_t& num_obs){
+    // Adding IMU factor and GPS factor and optimizing.
+    auto preint_imu_combined =dynamic_cast<const gtsam::PreintegratedCombinedMeasurements&>(*preintegratedPtr);
+    ++imu_factor_id;
+    gtsam::CombinedImuFactor imu_factor(P(imu_factor_id - 1), V(imu_factor_id - 1), P(imu_factor_id), V(imu_factor_id), B(imu_factor_id - 1), B(imu_factor_id),
+                                 preint_imu_combined);
+    graphPtr->add(imu_factor);
+    if(!reset())return false;
+    return true;
+}

svo/src/pose_optimizer.cpp

@@ -49,6 +49,7 @@ void optimizeGaussNewton(
   {
     if((*it)->point == NULL)
       continue;
+    //Reprojection error
     Vector2d e = vk::project2d((*it)->f)
                - vk::project2d(frame->T_f_w_ * (*it)->point->pos_);
     e *= 1.0 / (1<<(*it)->level);

vio_svo/launch/test.launch

@@ -4,6 +4,7 @@
 
     <!-- Camera topic to subscribe to -->
     <param name="cam_topic" value="/camera/image_raw" type="str" />
+    <param name="imu_topic" value="/SensorNode/imu" type="str" />
 
     <!-- Camera calibration file -->
     <rosparam file="$(find vio_svo)/param/camera_atan.yaml" />
@@ -18,4 +19,4 @@
 
   </node>
 
-</launch>
+</launch>

vio_svo/src/vo_node.cpp

@@ -30,6 +30,7 @@
 #include <vikit/abstract_camera.h>
 #include <vikit/camera_loader.h>
 #include <vikit/user_input_thread.h>
+#include <svo/frame.h>
 
 
 namespace svo {
@@ -44,12 +45,12 @@ public:
       vk::AbstractCamera* cam_;
       FILE* logtime;
       bool quit_;
-      boost::thread* vio_th;
+      boost::thread* imu_th;
       VoNode();
       ~VoNode();
       void imgCb(const sensor_msgs::ImageConstPtr& msg);
       void imuCb(const sensor_msgs::ImuPtr& imu);
-      void vio();
+      void imu();
 };
 
 VoNode::VoNode() :
@@ -70,10 +71,10 @@ VoNode::VoNode() :
                           vk::getParam<double>("vio/init_ty", 0.0),
                           vk::getParam<double>("vio/init_tz", 0.0)));
       // Init VO and start
-      vo_ = new svo::FrameHandlerMono(cam_);
+      vo_ = new svo::FrameHandlerMono(cam_,visualizer_.T_world_from_vision_);
       vo_->start();
-      logtime = fopen((std::string(getenv("HOME"))+"/Project/time.txt").c_str(), "w+");
-    vio_th=new boost::thread(&VoNode::vio,this);
+    imu_th=new boost::thread(&VoNode::imu,this);
+    logtime = fopen((std::string(getenv("HOME"))+"/Project/time.txt").c_str(), "w+");
 
 }
 
@@ -81,8 +82,8 @@ VoNode::~VoNode()
 {
     quit_=true;
     fclose(logtime);
-    vio_th->join();
-    delete vio_th;
+    imu_th->join();
+    delete imu_th;
     delete logtime;
     delete vo_;
     delete cam_;
@@ -91,7 +92,6 @@ VoNode::~VoNode()
 void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
 {
       try {
-          ROS_INFO("image back");
           auto start=std::chrono::steady_clock::now();
           cv::Mat img=cv_bridge::toCvShare(msg, "mono8")->image;
           vo_->addImage(img, msg->header.stamp.toSec());
@@ -107,9 +107,17 @@ void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
       }
 }
 void VoNode::imuCb(const sensor_msgs::ImuPtr &imu) {
-    ROS_INFO("imu back");
+    float imu_in[7];
+    imu_in[0] = imu->angular_velocity.x;
+    imu_in[1] = imu->angular_velocity.y;
+    imu_in[2] = imu->angular_velocity.z;
+    imu_in[3] = imu->linear_acceleration.x;
+    imu_in[4] = imu->linear_acceleration.y;
+    imu_in[5] = imu->linear_acceleration.z;
+    imu_in[6] = imu->header.stamp.sec;
+    vo_->imu_integPtr_->update(imu_in);
 }
-void VoNode::vio(){
+void VoNode::imu(){
     ros::NodeHandle nh;
     std::cout << "create vo_node" << std::endl;
     // subscribe to cam msgs
@@ -119,7 +127,6 @@ void VoNode::vio(){
     while(ros::ok() && !quit_)
     {
         ros::spinOnce();
-        // TODO check when last image was processed. when too long ago. publish warning that no msgs are received!
     }
 
 }