add freak kernel init

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));

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

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,

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();

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;
 

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?