From 56c7f230882a300829a512b089ffa67536de954a Mon Sep 17 00:00:00 2001 From: gfuse Date: Mon, 12 Apr 2021 12:43:04 +0200 Subject: [PATCH] edit svo --- svo/CMakeLists.txt | 17 +++++--- svo/include/svo/frame_handler_mono.h | 5 ++- svo/include/svo/imu_integration.h | 39 +++++++++++++++++++ svo/src/frame_handler_mono.cpp | 7 +++- svo/src/imu_integration.cpp | 58 ++++++++++++++++++++++++++++ svo/src/pose_optimizer.cpp | 1 + vio_svo/launch/test.launch | 3 +- vio_svo/src/vo_node.cpp | 29 ++++++++------ 8 files changed, 139 insertions(+), 20 deletions(-) create mode 100644 svo/include/svo/imu_integration.h create mode 100644 svo/src/imu_integration.cpp diff --git a/svo/CMakeLists.txt b/svo/CMakeLists.txt index 15257be..19beebd 100644 --- a/svo/CMakeLists.txt +++ b/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 diff --git a/svo/include/svo/frame_handler_mono.h b/svo/include/svo/frame_handler_mono.h index aed73be..58789c8 100644 --- a/svo/include/svo/frame_handler_mono.h +++ b/svo/include/svo/frame_handler_mono.h @@ -22,6 +22,7 @@ #include #include #include +#include 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_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(); diff --git a/svo/include/svo/imu_integration.h b/svo/include/svo/imu_integration.h new file mode 100644 index 0000000..7f05e20 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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&,std::size_t&); +private: + std::shared_ptr statePtr; + std::shared_ptr imu_biasPtr; + std::unique_ptr valuesPtr; + std::unique_ptr graphPtr; + boost::shared_ptr parameterPtr; + std::shared_ptr preintegratedPtr; + std::uint32_t t_1=1e-18; + std::uint32_t imu_factor_id=0; +}; +#endif //SVO_IMU_INTEGRATION_H diff --git a/svo/src/frame_handler_mono.cpp b/svo/src/frame_handler_mono.cpp index b8bba92..e4a3140 100644 --- a/svo/src/frame_handler_mono.cpp +++ b/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(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 = "< +#include + +Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){ + statePtr=std::make_shared(gtsam::Pose3(gtsam::Rot3(SE_init.rotation_matrix()),SE_init.translation()), + gtsam::Vector3(0.0,0.0,0.0)); + imu_biasPtr=std::make_shared(); + valuesPtr=std::make_unique(); + valuesPtr->insert(P(0),statePtr->pose()); + valuesPtr->insert(V(0),statePtr->v()); + valuesPtr->insert(B(0),*imu_biasPtr.get()); + graphPtr=std::make_unique(); + 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(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& new_frame,std::size_t& num_obs){ + // Adding IMU factor and GPS factor and optimizing. + auto preint_imu_combined =dynamic_cast(*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; +} \ No newline at end of file diff --git a/svo/src/pose_optimizer.cpp b/svo/src/pose_optimizer.cpp index 00ca321..1c491b0 100644 --- a/svo/src/pose_optimizer.cpp +++ b/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); diff --git a/vio_svo/launch/test.launch b/vio_svo/launch/test.launch index 388b536..4bcd965 100644 --- a/vio_svo/launch/test.launch +++ b/vio_svo/launch/test.launch @@ -4,6 +4,7 @@ + @@ -18,4 +19,4 @@ - \ No newline at end of file + diff --git a/vio_svo/src/vo_node.cpp b/vio_svo/src/vo_node.cpp index 8fd503f..908abb8 100644 --- a/vio_svo/src/vo_node.cpp +++ b/vio_svo/src/vo_node.cpp @@ -30,6 +30,7 @@ #include #include #include +#include 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("vio/init_ty", 0.0), vk::getParam("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! } }