increase init disparity

GPU_version/vio/param/vo_fast.yaml

@@ -5,7 +5,7 @@ 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: 60
   structureoptim_max_pts: 50
   kfselect_mindist: 0.01
   poseoptim_thresh: 4.0

GPU_version/vio/src/frame_handler_mono.cpp

@@ -164,12 +164,12 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   if(img_align.run(last_frame_, new_frame_)==0)return  RESULT_FAILURE;
   //assert (false);
   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';*/
-
+             (last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n';
+*/
   size_t sfba_n_edges_final=0;
   double sfba_thresh, sfba_error_init, sfba_error_final;
   pose_optimizer::optimizeGaussNewton(

GPU_version/vio/src/visualizer.cpp

@@ -52,7 +52,7 @@ void Visualizer::publishMinimal(
   {
     // publish cam in world frame (Estimated odometry in the worls frame)
     auto odom=ukf.get_location();
-    Quaterniond q(AngleAxisd(odom.second.pitch()+M_PI_2, Vector3d::UnitZ()));
+    Quaterniond q(AngleAxisd(odom.second.pitch(), Vector3d::UnitZ()));
     geometry_msgs::PoseWithCovarianceStampedPtr msg_pose_with_cov(new geometry_msgs::PoseWithCovarianceStamped);
       msg_pose_with_cov->header = header_msg;
       msg_pose_with_cov->pose.pose.position.x = -odom.second.se2().translation()(0);