From 50192e3b0473907cce48634c868d19e25faf4bcc Mon Sep 17 00:00:00 2001
From: gfuse <majidgeravand68@gmail.com>
Date: Thu, 26 Aug 2021 00:01:09 +0200
Subject: [PATCH] edit some parametrs, a try to improve the accureccy

---
 GPU_version/vio/include/vio/global.h       |  6 +-----
 GPU_version/vio/param/vo_fast.yaml         |  2 +-
 GPU_version/vio/src/depth_filter.cpp       |  2 +-
 GPU_version/vio/src/feature_alignment.cpp  |  2 +-
 GPU_version/vio/src/frame_handler_mono.cpp | 13 ++++++-------
 GPU_version/vio/src/matcher.cpp            |  2 +-
 GPU_version/vio/src/sparse_img_align.cpp   |  2 +-
 7 files changed, 12 insertions(+), 17 deletions(-)

diff --git a/GPU_version/vio/include/vio/global.h b/GPU_version/vio/include/vio/global.h
index d3adc3c..86cc64a 100644
--- a/GPU_version/vio/include/vio/global.h
+++ b/GPU_version/vio/include/vio/global.h
@@ -91,11 +91,7 @@ namespace vio
         }
         SE2 inverse() const{
             double yaw=atan2(T2_.so2().unit_complex().imag(),T2_.so2().unit_complex().real());
-            if(yaw<0.0)
-                yaw=M_PI-yaw;
-            else
-                yaw=yaw-M_PI;
-            return SE2(yaw,-1.0*T2_.translation());
+            return SE2(yaw+M_PI,-1.0*T2_.translation());
         }
         double pitch()const{
             return atan2(T2_.rotation_matrix()(0,1),T2_.rotation_matrix()(0,0));
diff --git a/GPU_version/vio/param/vo_fast.yaml b/GPU_version/vio/param/vo_fast.yaml
index 04d1559..69954f4 100644
--- a/GPU_version/vio/param/vo_fast.yaml
+++ b/GPU_version/vio/param/vo_fast.yaml
@@ -12,5 +12,5 @@ svo:
   init_min_inliers: 30
   ACC_noise: 0.5          #acc white noise in continuous in EKF
   GYO_noise: 0.001          #gyro white noise in continuous in EKF
-  svo_ekf: 1.0                #Svo noise that will be devided by number of Reprojected points after opmization in EKF
+  svo_ekf: 1.5                #Svo noise that will be devided by number of Reprojected points after opmization in EKF
   Cmd_Cov: 1.0  #control vector noise in EKF
diff --git a/GPU_version/vio/src/depth_filter.cpp b/GPU_version/vio/src/depth_filter.cpp
index a02b47a..0a1f1e2 100644
--- a/GPU_version/vio/src/depth_filter.cpp
+++ b/GPU_version/vio/src/depth_filter.cpp
@@ -234,7 +234,7 @@ void DepthFilter::updateSeeds(FramePtr frame)
         continue;
     }
     // check if point is visible in the current image
-    SE2 T(it->ftr->frame->T_f_w_.se2() * frame->T_f_w_.inverse());
+    SE2 T(it->ftr->frame->T_f_w_.pitch()-frame->T_f_w_.pitch(),it->ftr->frame->T_f_w_.se2().translation()- frame->T_f_w_.se2().translation());
     Eigen::Matrix<double,3,3> R;
     R<<T.rotation_matrix()(0,0),0.0,T.rotation_matrix()(0,1),
     0.0,1.0,0.0,
diff --git a/GPU_version/vio/src/feature_alignment.cpp b/GPU_version/vio/src/feature_alignment.cpp
index f46d185..0d8b455 100644
--- a/GPU_version/vio/src/feature_alignment.cpp
+++ b/GPU_version/vio/src/feature_alignment.cpp
@@ -202,7 +202,7 @@ bool align2D(
   bool sign=true;// +
 
   // termination condition
-  const float min_update_squared = 0.5;//0.001
+  const float min_update_squared = 0.05;//0.001
   const int cur_step = cur_img.step.p[0];
 //  float chi2 = 0;
   Vector3f update; update.setZero();
diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp
index 02ad775..576b57d 100644
--- a/GPU_version/vio/src/frame_handler_mono.cpp
+++ b/GPU_version/vio/src/frame_handler_mono.cpp
@@ -170,23 +170,22 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   //SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlign::LevenbergMarquardt, false, false);
   SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
   if(img_align.run(last_frame_, new_frame_)==0)return  RESULT_FAILURE;
-  if((last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.5 ||
-       fabs(last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>M_PI){
-        return RESULT_FAILURE;
-  }
   reprojector_.reprojectMap(new_frame_, overlap_kfs_);
-  /*
+/*
   std::cout<<"Reprojection Map nPoint: "<<overlap_kfs_.back().second
              <<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_
              <<"\t distance between two frames: "<<
              (last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n';*/
-  if( reprojector_.n_matches_ < 5)return RESULT_FAILURE;
+  if( reprojector_.n_trials_ < 10)return RESULT_FAILURE;
   size_t sfba_n_edges_final=0;
   double sfba_thresh, sfba_error_init, sfba_error_final;
   pose_optimizer::optimizeGaussNewton(
             Config::poseOptimThresh(), Config::poseOptimNumIter(), false,
             new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final);
-
+  if((last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.5 ||
+       fabs(last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>M_PI){
+        return RESULT_FAILURE;
+    }
   auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.se2().translation()(0),
                                 new_frame_->T_f_w_.se2().translation()(1),new_frame_->T_f_w_.pitch(),sfba_n_edges_final,time_);
   new_frame_->T_f_w_ =result.second;
diff --git a/GPU_version/vio/src/matcher.cpp b/GPU_version/vio/src/matcher.cpp
index ec16cd3..0b9b9f7 100644
--- a/GPU_version/vio/src/matcher.cpp
+++ b/GPU_version/vio/src/matcher.cpp
@@ -189,7 +189,7 @@ bool Matcher::findEpipolarMatchDirect(
     double& depth)
 {
   if(isnan(d_min) || isnan(d_max))return false;
-  SE2_5 T_cur_ref(cur_frame.T_f_w_.se2() * ref_frame.T_f_w_.inverse());
+  SE2_5 T_cur_ref(SE2(cur_frame.T_f_w_.pitch() - ref_frame.T_f_w_.pitch(),cur_frame.T_f_w_.se2().translation() - ref_frame.T_f_w_.se2().translation()));
   int zmssd_best = PatchScore::threshold();
   Vector2d uv_best;
 
diff --git a/GPU_version/vio/src/sparse_img_align.cpp b/GPU_version/vio/src/sparse_img_align.cpp
index 128be16..ee58082 100644
--- a/GPU_version/vio/src/sparse_img_align.cpp
+++ b/GPU_version/vio/src/sparse_img_align.cpp
@@ -53,7 +53,7 @@ size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
   cur_frame_ = cur_frame;
   ref_patch_cache_ = cv::Mat(ref_frame_->fts_.size(), patch_area_, CV_32F);
   jacobian_cache_.resize(Eigen::NoChange, ref_patch_cache_.rows*patch_area_);
-  SE2 T_cur_from_ref(cur_frame_->T_f_w_.se2() * ref_frame_->T_f_w_.inverse());
+  SE2 T_cur_from_ref(SE2(cur_frame->T_f_w_.pitch() - ref_frame->T_f_w_.pitch(),cur_frame->T_f_w_.se2().translation() - ref_frame->T_f_w_.se2().translation()));
   for(level_=max_level_; level_>=min_level_; --level_)
   {
     visible_fts_.resize(ref_patch_cache_.rows, false); // TODO: should it be reset at each level?