From 941577314c617dd645295bbd91dc31dfc6d25f83 Mon Sep 17 00:00:00 2001 From: gfuse Date: Thu, 12 Aug 2021 16:59:46 +0200 Subject: [PATCH] increase init disparity --- GPU_version/vio/param/vo_fast.yaml | 2 +- GPU_version/vio/src/frame_handler_mono.cpp | 6 +++--- GPU_version/vio/src/visualizer.cpp | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/GPU_version/vio/param/vo_fast.yaml b/GPU_version/vio/param/vo_fast.yaml index b4e3359..fe018f0 100644 --- a/GPU_version/vio/param/vo_fast.yaml +++ b/GPU_version/vio/param/vo_fast.yaml @@ -5,7 +5,7 @@ svo: quality_min_fts: 10 #If the number of tracked features drops below this threshold. Tracking quality is bad. quality_max_drop_fts: 90 #If within one frame, this amount of features are dropped. Tracking quality is bad. init_min_tracked: 100 - init_min_disparity: 30 + init_min_disparity: 60 structureoptim_max_pts: 50 kfselect_mindist: 0.01 poseoptim_thresh: 4.0 diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index e8a91cf..236400a 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -164,12 +164,12 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() if(img_align.run(last_frame_, new_frame_)==0)return RESULT_FAILURE; //assert (false); reprojector_.reprojectMap(new_frame_, overlap_kfs_); - /* +/* std::cout<<"Reprojection Map nPoint: "<T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n';*/ - + (last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n'; +*/ size_t sfba_n_edges_final=0; double sfba_thresh, sfba_error_init, sfba_error_final; pose_optimizer::optimizeGaussNewton( diff --git a/GPU_version/vio/src/visualizer.cpp b/GPU_version/vio/src/visualizer.cpp index 279d73c..6cf2e32 100644 --- a/GPU_version/vio/src/visualizer.cpp +++ b/GPU_version/vio/src/visualizer.cpp @@ -52,7 +52,7 @@ void Visualizer::publishMinimal( { // publish cam in world frame (Estimated odometry in the worls frame) auto odom=ukf.get_location(); - Quaterniond q(AngleAxisd(odom.second.pitch()+M_PI_2, Vector3d::UnitZ())); + Quaterniond q(AngleAxisd(odom.second.pitch(), Vector3d::UnitZ())); geometry_msgs::PoseWithCovarianceStampedPtr msg_pose_with_cov(new geometry_msgs::PoseWithCovarianceStamped); msg_pose_with_cov->header = header_msg; msg_pose_with_cov->pose.pose.position.x = -odom.second.se2().translation()(0);