From b88547671d668444a18d3d5f06f55e1fd85a061e Mon Sep 17 00:00:00 2001 From: gfuse Date: Fri, 8 Oct 2021 11:41:16 +0200 Subject: [PATCH] corrent situation --- GPU_version/vio/include/vio/frame.h | 2 +- GPU_version/vio/include/vio/global_optimizer.h | 4 ++-- GPU_version/vio/param/vo_fast.yaml | 2 +- GPU_version/vio/src/frame_handler_mono.cpp | 2 +- GPU_version/vio/src/global_optimizer.cpp | 16 ++++++++-------- 5 files changed, 13 insertions(+), 13 deletions(-) diff --git a/GPU_version/vio/include/vio/frame.h b/GPU_version/vio/include/vio/frame.h index bd98e98..7b456d0 100644 --- a/GPU_version/vio/include/vio/frame.h +++ b/GPU_version/vio/include/vio/frame.h @@ -50,7 +50,7 @@ namespace vio { Features fts_; //!< List of features in the image. vector> key_pts_; //!< Five features and associated 3D points which are used to detect if two frames have overlapping field of view. bool is_keyframe_; //!< Was this frames selected as keyframe? - g2o::VertexCam* v_kf_=nullptr; //!< Temporary pointer to the g2o node object of the keyframe. + g2o::VertexSE3Expmap* v_kf_=nullptr; //!< Temporary pointer to the g2o node object of the keyframe. int last_published_ts_; //!< Timestamp of last publishing. Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp); diff --git a/GPU_version/vio/include/vio/global_optimizer.h b/GPU_version/vio/include/vio/global_optimizer.h index ae371aa..14ac164 100644 --- a/GPU_version/vio/include/vio/global_optimizer.h +++ b/GPU_version/vio/include/vio/global_optimizer.h @@ -84,7 +84,7 @@ protected: }; /// Create a g2o vertice from a keyframe object. - g2o::VertexCam* createG2oFrameSE3( + g2o::VertexSE3Expmap* createG2oFrameSE3( FramePtr kf, size_t id, bool fixed); @@ -95,7 +95,7 @@ protected: bool fixed); /// Creates a g2o edge between a g2o keyframe and mappoint vertice with the provided measurement. g2o::EdgeProjectXYZ2UV* createG2oEdgeSE3( - g2o::VertexCam* v_kf, + g2o::VertexSE3Expmap* v_kf, g2o::VertexSBAPointXYZ* v_mp, const Vector2d& f_up, bool robust_kernel, diff --git a/GPU_version/vio/param/vo_fast.yaml b/GPU_version/vio/param/vo_fast.yaml index bba7df1..cb79071 100644 --- a/GPU_version/vio/param/vo_fast.yaml +++ b/GPU_version/vio/param/vo_fast.yaml @@ -13,6 +13,6 @@ vio: ACC_ekf: 0.2 #acc white noise in continuous in EKF GYO_ekf: 0.5 #gyro white noise in continuous in EKF VO_ekf_translation: 1.0 #VO noise in EKF - VO_ekf_orientation: 0.05 #VO noise in EKF + VO_ekf_orientation: 1.5 #VO noise in EKF CMD_ekf_translation: 0.1 #control vector noise in EKF CMD_ekf_orientation: 0.01 #control vector noise in EKF diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index d5f0c51..85c6b01 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -132,7 +132,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame() fprintf(log_,"[%s] Init: Selected Second frame. \t The number of features: %d depth mean:%f min:%f\n", vio::time_in_HH_MM_SS_MMM().c_str(),new_frame_->fts_.size(),depth_mean,depth_min); #endif - //ba_glob_->new_key_frame(); + ba_glob_->new_key_frame(); ROS_INFO("VIO initialized :)"); ROS_INFO("Running ..."); return RESULT_IS_KEYFRAME; diff --git a/GPU_version/vio/src/global_optimizer.cpp b/GPU_version/vio/src/global_optimizer.cpp index 8cc1454..c4bbc76 100644 --- a/GPU_version/vio/src/global_optimizer.cpp +++ b/GPU_version/vio/src/global_optimizer.cpp @@ -104,7 +104,7 @@ namespace vio { it_kf != map_.keyframes_.end(); ++it_kf) { // New Keyframe Vertex - g2o::VertexCam* v_kf = createG2oFrameSE3(*it_kf, v_id++, false); + g2o::VertexSE3Expmap* v_kf = createG2oFrameSE3(*it_kf, v_id++, false); (*it_kf)->v_kf_ = v_kf; optimizer.addVertex(v_kf); for(auto&& it_ftr:(*it_kf)->fts_) @@ -200,17 +200,17 @@ namespace vio { } } - g2o::VertexCam* + g2o::VertexSE3Expmap* BA_Glob::createG2oFrameSE3(FramePtr frame, size_t id, bool fixed) { - if(frame->id_==0 || frame->id_==1){ - g2o::VertexCam* v= new g2o::VertexCam(); +/* if(frame->id_==0 || frame->id_==1){ + g2o::VertexSE3Expmap* v= new g2o::VertexSE3Expmap(); v->setId(id); v->setToOrigin(); v->setFixed(true); v->setEstimate(g2o::SE3Quat(frame->se3().unit_quaternion(), frame->se3().translation())); - } - g2o::VertexCam* v= new g2o::VertexCam(); + }*/ + g2o::VertexSE3Expmap* v= new g2o::VertexSE3Expmap(); v->setId(id); v->setFixed(false); v->setEstimate(g2o::SE3Quat(frame->se3().unit_quaternion(), frame->se3().translation())); @@ -224,14 +224,14 @@ namespace vio { { g2o::VertexSBAPointXYZ* v =new g2o::VertexSBAPointXYZ(); v->setId(id); - v->setFixed(fixed); + //v->setFixed(fixed); v->setMarginalized(true); v->setEstimate(pos); return v; } - g2o::EdgeProjectXYZ2UV* BA_Glob::createG2oEdgeSE3( g2o::VertexCam* v_frame, + g2o::EdgeProjectXYZ2UV* BA_Glob::createG2oEdgeSE3( g2o::VertexSE3Expmap* v_frame, g2o::VertexSBAPointXYZ* v_point, const Vector2d& f_up, bool robust_kernel,