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

Merge remote-tracking branch 'origin/dev2' into dev2

Browse Source 
# Conflicts:
#	GPU_version/vio/src/frame_handler_mono.cpp
This commit is contained in:
VahdPourAmin
2021-09-14 08:52:29 +02:00
parent e95c2ab7cd ef3de7af29
commit eefd5079c5
9 changed files with 29951 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
Executable
+24461
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/frame_handler_log.txt
Executable
+5455
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/launch/Px30.launch
+1 -1
View File
@@ -1,6 +1,6 @@
<launch>
<!-- node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" /-->
<node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" />
<!-- Camera topic to subscribe to -->
<param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />
GPU_version/vio/param/vo_fast.yaml
+3 -3
View File
@@ -5,12 +5,12 @@ 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: 40
structureoptim_max_pts: 50
kfselect_mindist: 0.01
poseoptim_thresh: 8.0
init_min_inliers: 20
ACC_noise: 0.5 #acc white noise in continuous in EKF
GYO_noise: 0.1 #gyro white noise in continuous in EKF
svo_ekf: 0.1 #Svo noise that will be devided by number of Reprojected points after opmization in EKF
GYO_noise: 0.5 #gyro white noise in continuous in EKF
svo_ekf: 0.05 #Svo noise that will be devided by number of Reprojected points after opmization in EKF
Cmd_Cov: 0.1 #control vector noise in EKF
GPU_version/vio/src/depth_filter.cpp
+1 -1
View File
@@ -269,7 +269,7 @@ namespace vio {
#if VIO_DEBUG
fprintf(log_,"[%s] If point is visible? %f, %f, %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),xyz_f.x(),xyz_f.y(),xyz_f.z());
#endif
if(xyz_f.z() < 0.0) {
if(xyz_f.z() > 0.0) {
++it; // behind the camera
continue;
}
GPU_version/vio/src/feature_alignment.cpp
+2 -2
View File
@@ -336,7 +336,7 @@ bool align2D_SSE2(
float v = cur_px_estimate.y();
// termination condition
const float min_update_squared = 0.1*0.1;
const float min_update_squared = 0.01*0.01;
const int cur_step = cur_img.step.p[0];
const float Dinv = 1.0f/(A11*A22 - A12*A12); // we are missing an extra factor 16
float chi2 = 0;
@@ -501,7 +501,7 @@ bool align2D_NEON (
float v = cur_px_estimate.y();
// termination condition
const float min_update_squared = 0.1*0.1;
const float min_update_squared = 0.01*0.01;
const int cur_step = cur_img.step.p[0];
Vector3f update;
Vector3f Jres;
GPU_version/vio/src/frame_handler_mono.cpp
+1 -1
View File
@@ -221,7 +221,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
}
fprintf(log_,"null : %d, null point : %d\n", nullcount, nullpointcount);
#endif*/
reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_, gpu_fast_);
reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_);
int n_point=0;
for(auto i:overlap_kfs_)n_point+=i.second;
#if VIO_DEBUG
GPU_version/vio/src/pose_optimizer.cpp
+26 -44
View File
@@ -40,7 +40,7 @@ void optimizeGaussNewton(
// init
double chi2(0.0);
vector<double> chi2_vec_init, chi2_vec_final;
vk::robust_cost::TukeyWeightFunction weight_function;
vk::robust_cost::HuberWeightFunction weight_function;
SE2_5 T_old(frame->T_f_w_);
Matrix3d A;
Vector3d b;
@@ -54,32 +54,32 @@ void optimizeGaussNewton(
//Reprojection error
Vector2d e = vk::project2d((*it)->f)
- vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
e *= 1.0 / (1<<(*it)->level);
chi2_vec_init.push_back(e.squaredNorm()); // just for debug
errors.push_back(e.norm());
}
if(errors.empty())
return;
vk::robust_cost::MADScaleEstimator scale_estimator;
estimated_scale = 0.5*scale_estimator.compute(errors);
/*#if VIO_DEBUG
estimated_scale = scale_estimator.compute(errors);
#if VIO_DEBUG
fprintf(log,"[%s] Init estimate scale:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),estimated_scale);
#endif*/
#endif
num_obs = errors.size();
chi2_vec_init.reserve(num_obs);
chi2_vec_final.reserve(num_obs);
double scale = estimated_scale;
bool direction=true;
size_t n_direction=0;
for(size_t iter=0; iter<n_iter; iter++)
{
b.setZero();
A.setZero();
double new_chi2(0.0);
// overwrite scale
if(iter == 5)
scale = 0.85/frame->cam_->errorMultiplier2();
// compute residual
for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
{
@@ -89,20 +89,13 @@ void optimizeGaussNewton(
Vector3d xyz_f(Vector3d(frame->se3()*(*it)->point->pos_));
frame->jacobian_xyz2uv_((*it)->f,(*it)->point->pos_,J);
Vector2d e = vk::project2d((*it)->f) - vk::project2d(xyz_f);
/*#if VIO_DEBUG
fprintf(log,"[%s] xyz_f:%f, %f, %f Reprojection Error: %f, %f Feature: %f, %f, %f\n",
vio::time_in_HH_MM_SS_MMM().c_str(),xyz_f.x(),xyz_f.y(),xyz_f.z(),e.x(),e.y(),
(*it)->point->pos_.x(),(*it)->point->pos_.y(),(*it)->point->pos_.z());
#endif*/
double sqrt_inv_cov = 1.0 / (1<<(*it)->level);
e *= sqrt_inv_cov;
if(iter == 0)
chi2_vec_init.push_back(e.squaredNorm()); // just for debug
J *= sqrt_inv_cov;
double weight = weight_function.value(e.norm()/scale);
A.noalias() += J.transpose()*J*weight;
b.noalias() -= J.transpose()*e*weight;
new_chi2 += e.squaredNorm()*weight;
new_chi2 += e.squaredNorm();
}
// solve linear system
@@ -111,65 +104,54 @@ void optimizeGaussNewton(
// check if error increased
if((iter > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dT[0]))
{
/*#if VIO_DEBUG
#if VIO_DEBUG
fprintf(log,"[%s] it:%d \t FAILURE \t new_chi2: %f\n",
vio::time_in_HH_MM_SS_MMM().c_str(),iter,new_chi2);
#endif*/
#endif
frame->T_f_w_ = T_old; // roll-back
break;
}
scale *=0.5;
dT=0.1*dT;
dT *=new_chi2;
// update the model
T_old = frame->T_f_w_;
frame->T_f_w_=SE2_5(T_old.se2().translation().x()+dT.x(),T_old.se2().translation().y()+dT.y(),T_old.pitch()+dT.z());
chi2 = new_chi2;
/*#if VIO_DEBUG
#if VIO_DEBUG
fprintf(log,"[%s] it:%d \t Success \t new_chi2: %f \t dT = %f, %f, %f\n",
vio::time_in_HH_MM_SS_MMM().c_str(),iter,new_chi2,dT.x(),dT.y(),dT.z());
#endif*/
#endif
// stop when converged
if(vk::norm_max(dT) <= EPS)
break;
}
// Set covariance as inverse information matrix. Optimistic estimator!
//const double pixel_variance=1.0;
//frame->Cov_ = pixel_variance*(A*std::pow(frame->cam_->errorMultiplier2(),2)).inverse();
// Remove Measurements with too large reprojection error
double reproj_thresh_scaled = reproj_thresh / frame->cam_->errorMultiplier2();
size_t n_deleted_refs = 0;
for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
{
if((*it)->point == NULL)
continue;
Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
double sqrt_inv_cov = 1.0 / (1<<(*it)->level);
e *= sqrt_inv_cov;
e /= (1<<(*it)->level);
chi2_vec_final.push_back(e.squaredNorm());
if(e.norm() > reproj_thresh_scaled)
if(e.norm() > 0.01)
{
// we don't need to delete a reference in the point since it was not created yet
(*it)->point = NULL;
++n_deleted_refs;
}
}
error_init=0.0;
error_final=0.0;
if(!chi2_vec_init.empty())
error_init = sqrt(vk::getMedian(chi2_vec_init))*frame->cam_->errorMultiplier2();
if(!chi2_vec_final.empty())
error_final = sqrt(vk::getMedian(chi2_vec_final))*frame->cam_->errorMultiplier2();
estimated_scale *= frame->cam_->errorMultiplier2();
if(verbose)
std::cout << "n deleted obs = " << n_deleted_refs
<< "\t scale = " << estimated_scale
<< "\t error init = " << error_init
<< "\t error end = " << error_final << std::endl;
#if VIO_DEBUG
error_init=0.0;
error_final=0.0;
if(!chi2_vec_init.empty())for(auto&& i:chi2_vec_init)error_init+=i;
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 scale =%f \t error init =%f \t error end=%f\n",
vio::time_in_HH_MM_SS_MMM().c_str(),n_deleted_refs,estimated_scale,error_init,error_final);
#endif
num_obs -= n_deleted_refs;
}
GPU_version/vio/src/sparse_img_align_gpu.cpp
+1 -1
View File
@@ -65,7 +65,7 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame, FILE* log)
featue_px[feature_counter_].y=(*it)->px(1);
++feature_counter_;
}
if(!feature_counter_) // more than 10
if(feature_counter_<10) // more than 10
{
return 0;
}