public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

get an unstable currect result

Browse Source
This commit is contained in:
gfuse
2021-10-12 23:28:30 +02:00
parent 094c9f4282
commit aa9143b875
7 changed files with 22 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/include/vio/config.h
-36
View File
@@ -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; }
GPU_version/vio/param/vo_fast.yaml
+1 -1
View File
@@ -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
GPU_version/vio/src/frame_handler_mono.cpp
+2 -2
View File
@@ -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(),
GPU_version/vio/src/global_optimizer.cpp
+10 -4
View File
@@ -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<FramePtr,std::shared_ptr<Feature>>(*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());
GPU_version/vio/src/point.cpp
+3 -2
View File
@@ -19,6 +19,7 @@
#include <vio/point.h>
#include <vio/frame.h>
#include <vio/feature.h>
#include <vio/config.h>
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;
}
GPU_version/vio/src/pose_optimizer.cpp
+6 -8
View File
@@ -22,6 +22,7 @@
#include <vio/robust_cost.h>
#include <vio/math_utils.h>
#include <sophus/se2.h>
#include <vio/config.h>
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; iter<n_iter; iter++)
{
@@ -132,13 +132,12 @@ void optimizeGaussNewton(
break;
}
//point_mut_.lock();
size_t n_deleted_refs = 0;
num_obs=0;
for(auto it=frame->fts_.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
}
GPU_version/vio/src/vo_node.cpp
-3
View File
@@ -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<std::size_t>(3*sizeof(double)));
vo_->UpdateIMU(imu_in,imu->header.stamp);
imu_time_=imu->header.stamp;