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

Fixed some bugs and got better results

Browse Source
This commit is contained in:
VahdPourAmin
2021-09-12 21:53:30 +02:00
parent 604ae26297
commit 5b4130f5d0
1 changed files with 7 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/src/pose_optimizer.cpp
+7 -7
View File
@@ -63,9 +63,9 @@ void optimizeGaussNewton(
return; return;
vk::robust_cost::MADScaleEstimator scale_estimator; vk::robust_cost::MADScaleEstimator scale_estimator;
estimated_scale = 0.5*scale_estimator.compute(errors); estimated_scale = 0.5*scale_estimator.compute(errors);
#if VIO_DEBUG /*#if VIO_DEBUG
fprintf(log,"[%s] Init estimate scale:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),estimated_scale); 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(); num_obs = errors.size();
chi2_vec_init.reserve(num_obs); chi2_vec_init.reserve(num_obs);
@@ -111,10 +111,10 @@ void optimizeGaussNewton(
// check if error increased // check if error increased
if((iter > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dT[0])) 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", fprintf(log,"[%s] it:%d \t FAILURE \t new_chi2: %f\n",
vio::time_in_HH_MM_SS_MMM().c_str(),iter,new_chi2); vio::time_in_HH_MM_SS_MMM().c_str(),iter,new_chi2);
#endif #endif*/
frame->T_f_w_ = T_old; // roll-back frame->T_f_w_ = T_old; // roll-back
break; break;
} }
@@ -122,13 +122,13 @@ void optimizeGaussNewton(
dT=0.1*dT; dT=0.1*dT;
// update the model // update the model
T_old = frame->T_f_w_; T_old = frame->T_f_w_;
frame->T_f_w_=SE2_5(T_old.se2().translation().y()+dT.x(),T_old.se2().translation().y()+dT.y(),T_old.pitch()+dT.z()); 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; 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", 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()); vio::time_in_HH_MM_SS_MMM().c_str(),iter,new_chi2,dT.x(),dT.y(),dT.z());
#endif #endif*/
// stop when converged // stop when converged
if(vk::norm_max(dT) <= EPS) if(vk::norm_max(dT) <= EPS)