From aa9143b875a957ffabf3bd1d825da6bd213973e8 Mon Sep 17 00:00:00 2001 From: gfuse Date: Tue, 12 Oct 2021 23:28:30 +0200 Subject: [PATCH] get an unstable currect result --- GPU_version/vio/include/vio/config.h | 36 ---------------------- GPU_version/vio/param/vo_fast.yaml | 2 +- GPU_version/vio/src/frame_handler_mono.cpp | 4 +-- GPU_version/vio/src/global_optimizer.cpp | 14 ++++++--- GPU_version/vio/src/point.cpp | 5 +-- GPU_version/vio/src/pose_optimizer.cpp | 14 ++++----- GPU_version/vio/src/vo_node.cpp | 3 -- 7 files changed, 22 insertions(+), 56 deletions(-) diff --git a/GPU_version/vio/include/vio/config.h b/GPU_version/vio/include/vio/config.h index b7ee3d5..99ee152 100644 --- a/GPU_version/vio/include/vio/config.h +++ b/GPU_version/vio/include/vio/config.h @@ -33,23 +33,10 @@ class Config public: static Config& getInstance(); - /// Base-name of the tracefiles. - static string& traceName() { return getInstance().trace_name; } - - /// Directory where the tracefiles are saved. - static string& traceDir() { return getInstance().trace_dir; } /// Number of pyramid levels used for features. static size_t& nPyrLevels() { return getInstance().n_pyr_levels; } - /// Use the IMU to get relative rotations. - static bool& useImu() { return getInstance().use_imu; } - - /// Number of keyframes in the core. The core-kfs are optimized through bundle adjustment. - static size_t& coreNKfs() { return getInstance().core_n_kfs; } - - /// Initial scale of the map. Depends on the distance the camera is moved for the initialization. - static double& mapScale() { return getInstance().map_scale; } /// Feature grid size of a cell in [px]. static size_t& gridSize() { return getInstance().grid_size; } @@ -57,8 +44,6 @@ public: /// Initialization: Minimum required disparity between the first two frames. static double& initMinDisparity() { return getInstance().init_min_disparity; } - /// Initialization: Minimum number of tracked features. - static size_t& initMinTracked() { return getInstance().init_min_tracked; } /// Initialization: Minimum number of inliers after RANSAC. static size_t& initMinInliers() { return getInstance().init_min_inliers; } @@ -69,14 +54,10 @@ public: /// Minimum level of the Lucas Kanade tracker. static size_t& kltMinLevel() { return getInstance().klt_min_level; } - /// Reprojection threshold [px]. - static double& reprojThresh() { return getInstance().reproj_thresh; } /// Reprojection threshold after pose optimization. static double& poseOptimThresh() { return getInstance().poseoptim_thresh; } - /// Number of iterations in local bundle adjustment. - static size_t& poseOptimNumIter() { return getInstance().poseoptim_num_iter; } /// Maximum number of points to optimize at every iteration. static size_t& structureOptimMaxPts() { return getInstance().structureoptim_max_pts; } @@ -85,7 +66,6 @@ public: static size_t& structureOptimNumIter() { return getInstance().structureoptim_num_iter; } /// Reprojection threshold after bundle adjustment. - static double& lobaThresh() { return getInstance().loba_thresh; } /// Threshold for the robust Huber kernel of the local bundle adjustment. static double& lobaRobustHuberWidth() { return getInstance().loba_robust_huber_width; } @@ -93,31 +73,15 @@ public: /// Number of iterations in the local bundle adjustment. static size_t& lobaNumIter() { return getInstance().loba_num_iter; } - /// Minimum distance between two keyframes. Relative to the average height in the map. - static double& kfSelectMinDist() { return getInstance().kfselect_mindist; } - /// Select only features with a minimum Harris corner score for triangulation. static double& triangMinCornerScore() { return getInstance().triang_min_corner_score; } - /// Subpixel refinement of reprojection and triangulation. Set to 0 if no subpix refinement required! - static size_t& subpixNIter() { return getInstance().subpix_n_iter; } /// Limit the number of keyframes in the map. This makes nslam essentially. /// a Visual Odometry. Set to 0 if unlimited number of keyframes are allowed. /// Minimum number of keyframes is 3. static size_t& maxNKfs() { return getInstance().max_n_kfs; } - /// How much (in milliseconds) is the camera delayed with respect to the imu. - static double& imgImuDelay() { return getInstance().img_imu_delay; } - - /// Maximum number of features that should be tracked. - static size_t& maxFts() { return getInstance().max_fts; } - - /// If the number of tracked features drops below this threshold. Tracking quality is bad. - static size_t& qualityMinFts() { return getInstance().quality_min_fts; } - - /// If within one frame, this amount of features are dropped. Tracking quality is bad. - static int& qualityMaxFtsDrop() { return getInstance().quality_max_drop_fts; } /// acc white noise in continuous. static double& ACC_Noise() { return getInstance().ACC_noise; } diff --git a/GPU_version/vio/param/vo_fast.yaml b/GPU_version/vio/param/vo_fast.yaml index da4c0af..0bad1f9 100644 --- a/GPU_version/vio/param/vo_fast.yaml +++ b/GPU_version/vio/param/vo_fast.yaml @@ -8,7 +8,7 @@ vio: init_min_disparity: 10 structureoptim_max_pts: 50 kfselect_mindist: 0.01 - poseoptim_thresh: 8.0 + poseoptim_thresh: 1.75 init_min_inliers: 40 ACC_ekf: 0.2 #acc white noise in continuous in EKF GYO_ekf: 0.5 #gyro white noise in continuous in EKF diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index 6064c89..4073327 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; @@ -158,7 +158,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() size_t sfba_n_edges_final=0; double sfba_thresh, sfba_error_init, sfba_error_final; pose_optimizer::optimizeGaussNewton( - Config::poseOptimThresh(), + 10, new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final,map_,log_); #if VIO_DEBUG fprintf(log_,"[%s] After pose optimization, distance between ekf and vo x:%f ,z=%f,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(), diff --git a/GPU_version/vio/src/global_optimizer.cpp b/GPU_version/vio/src/global_optimizer.cpp index 70d0e64..44e0afd 100644 --- a/GPU_version/vio/src/global_optimizer.cpp +++ b/GPU_version/vio/src/global_optimizer.cpp @@ -126,7 +126,7 @@ namespace vio { // Due to merging of mappoints it is possible that references in kfs suddenly // have a very large reprojection error which may result in distorted results. - /* Vector2d error = vk::project2d(it_ftr->f) - vk::project2d((*it_kf)->w2f(mp->pos_)); +/* Vector2d error = vk::project2d(it_ftr->f) - vk::project2d((*it_kf)->w2f(mp->pos_)); if(error.squaredNorm() > pow(2,Config::poseOptimThresh()/(*it_kf)->cam_->errorMultiplier2())) incorrect_edges.push_back(pair>(*it_kf, it_ftr)); else @@ -140,10 +140,13 @@ namespace vio { edge.feature=it_ftr; edges.push_back(edge); optimizer.addEdge(e); - /*}*/ + // } } } - if(optimizer.vertices().size()<1)continue; + if(optimizer.vertices().empty()){ + ba_mux_.unlock(); + continue; + } // Optimization optimizer.initializeOptimization(); optimizer.computeActiveErrors(); @@ -161,7 +164,10 @@ namespace vio { fprintf(log_,"[%s] init error: %f \n", vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2()); #endif - if(optimizer.optimize(vio::Config::lobaNumIter())<1)continue; + if(optimizer.optimize(vio::Config::lobaNumIter())<1){ + ba_mux_.unlock(); + continue; + } #if VIO_DEBUG fprintf(log_,"[%s] end error: %f \n", vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2()); diff --git a/GPU_version/vio/src/point.cpp b/GPU_version/vio/src/point.cpp index cfe9c97..e4c14ad 100644 --- a/GPU_version/vio/src/point.cpp +++ b/GPU_version/vio/src/point.cpp @@ -19,6 +19,7 @@ #include #include #include +#include namespace vio { @@ -166,10 +167,10 @@ void Point::optimize(const size_t n_iter) for(auto it=obs_.begin(); it!=obs_.end(); ++it) { Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d((*it)->frame->se3() * pos_)); e /= (1<<(*it)->level); - if (e.norm() > 4.0/ (*it)->frame->cam_->errorMultiplier2()) + if (e.norm() > vio::Config::poseOptimThresh()/ (*it)->frame->cam_->errorMultiplier2()) n_failed_reproj_++; } - if(n_failed_reproj_<0.75*obs_.size()) + if(n_failed_reproj_<0.70*obs_.size()) type_=TYPE_GOOD; } diff --git a/GPU_version/vio/src/pose_optimizer.cpp b/GPU_version/vio/src/pose_optimizer.cpp index 01a0cfa..2c392d3 100644 --- a/GPU_version/vio/src/pose_optimizer.cpp +++ b/GPU_version/vio/src/pose_optimizer.cpp @@ -22,6 +22,7 @@ #include #include #include +#include namespace vio { namespace pose_optimizer { @@ -81,7 +82,6 @@ void optimizeGaussNewton( vk::robust_cost::MADScaleEstimator scale_estimator; estimated_scale = scale_estimator.compute(errors); - num_obs = errors.size(); double scale = estimated_scale; for(size_t iter=0; iterfts_.begin(); it!=frame->fts_.end(); /*++it*/) { if((*it)->point == NULL) { map.safeDeletePoint((*it)->point); it = frame->fts_.erase(it); - ++n_deleted_refs; continue; } if((*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0.){ @@ -149,19 +148,18 @@ void optimizeGaussNewton( Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_)); e /= (1<<(*it)->level); chi2_vec_final.push_back(e.norm()); - if(e.norm() > 1.0 / frame->cam_->errorMultiplier2()) + if(e.norm() > 0.5*vio::Config::poseOptimThresh() / frame->cam_->errorMultiplier2()) { map.safeDeletePoint((*it)->point); it = frame->fts_.erase(it); - ++n_deleted_refs; }else{ (*it)->point->type_=vio::Point::TYPE_CANDIDATE; + ++num_obs; it++; } } //point_mut_.unlock(); - num_obs -= n_deleted_refs; #if VIO_DEBUG error_init=0.0; error_final=0.0; @@ -169,8 +167,8 @@ void optimizeGaussNewton( error_init /=chi2_vec_init.size(); if(!chi2_vec_final.empty())for(auto&& i:chi2_vec_final)error_final+=i; error_final /= chi2_vec_final.size(); - fprintf(log,"[%s] n deleted obs = %d \t n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =%d \t error init =%f \t error end=%f\n", - vio::time_in_HH_MM_SS_MMM().c_str(),n_deleted_refs,num_obs,error_init,error_final); + fprintf(log,"[%s] n obs with reprojection error less than 1.0 / frame->cam_->errorMultiplier2() =%d \t error init =%f \t error end=%f\n", + vio::time_in_HH_MM_SS_MMM().c_str(),num_obs,error_init,error_final); #endif } diff --git a/GPU_version/vio/src/vo_node.cpp b/GPU_version/vio/src/vo_node.cpp index 6b52a4c..9b27e4d 100644 --- a/GPU_version/vio/src/vo_node.cpp +++ b/GPU_version/vio/src/vo_node.cpp @@ -147,9 +147,6 @@ void VioNode::imuCb(const sensor_msgs::ImuPtr &imu) { imu_in[0] = 0.2*imu_[0]+0.8*imu->linear_acceleration.x; imu_in[1] = 0.2*imu_[1]+0.8*imu->linear_acceleration.y; imu_in[2] = 0.2*imu_[2]+0.8*imu->angular_velocity.z; - if(fabs(imu_in[2])>0.4)return; - if(fabs(imu_in[1])>1.0)return; - if(fabs(imu_in[0])>1.0)return; memcpy(imu_, imu_in, static_cast(3*sizeof(double))); vo_->UpdateIMU(imu_in,imu->header.stamp); imu_time_=imu->header.stamp;