edit frame and change atan to atan2

GPU_version/vio/include/vio/global.h

@@ -90,7 +90,7 @@ namespace vio
             return T2_;
         }
         SE2 inverse() const{
-            double yaw=atan(T2_.so2().unit_complex().imag()/T2_.so2().unit_complex().real());
+            double yaw=atan2(T2_.so2().unit_complex().imag(),T2_.so2().unit_complex().real());
             if(yaw<0.0)
                 yaw=M_PI-yaw;
             else
@@ -98,7 +98,7 @@ namespace vio
             return SE2(yaw,-1.0*T2_.translation());
         }
         double pitch()const{
-            return atan(T2_.rotation_matrix()(0,1)/T2_.rotation_matrix()(0,0));
+            return atan2(T2_.rotation_matrix()(0,1),T2_.rotation_matrix()(0,0));
         }
         SE3 se3() const{
             Eigen::Matrix<double,3,3> R;

GPU_version/vio/include/vio/ukf.h

@@ -127,25 +127,16 @@ public:
     std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> UpdateSvo(double x,double z,double pitch,size_t match,ros::Time& time) {
         while(lock)usleep(5);
         lock=true;
-        if(pitch<0.0)
+        pitch=pitch+M_PI;
-            pitch=M_PI-pitch;
-        else
-            pitch=pitch-M_PI;
         if(filter_->predict_up)filter_->correct(-x,-z,pitch,match,1e-9*time.toNSec());
         lock=false;
         double pitch_f;
-        if(filter_->state_(0)<0.0)
+        pitch_f=filter_->state_(2)+M_PI;
-            pitch_f=M_PI-filter_->state_(2);
-        else
-            pitch_f=filter_->state_(2)-M_PI;
         return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(0),-filter_->state_(1),pitch_f));
     };
     std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> get_location(){
         double pitch_f;
-        if(filter_->state_(0)<0.0)
+        pitch_f=filter_->state_(2)+M_PI;
-            pitch_f=M_PI-filter_->state_(2);
-        else
-            pitch_f=filter_->state_(2)-M_PI;
         return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(0),-filter_->state_(1),pitch_f));
     }
 private:

GPU_version/vio/param/vo_fast.yaml

@@ -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: 40
+  init_min_disparity: 60
   structureoptim_max_pts: 50
   kfselect_mindist: 0.01
   poseoptim_thresh: 4.0
   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.5                #Svo noise that will be devided by number of Reprojected points after opmization in EKF
+  svo_ekf: 1.0                #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

@@ -217,7 +217,7 @@ void DepthFilter::updateSeeds(FramePtr frame)
 
   const double focal_length = frame->cam_->errorMultiplier2();
   double px_noise = 1.0;
-  double px_error_angle = atan(px_noise/(2.0*focal_length))*2.0; // law of chord (sehnensatz)
+  double px_error_angle = atan2(px_noise,(2.0*focal_length))*2.0; // law of chord (sehnensatz)
   while( it!=seeds_.end())
   {
     // set this value true when seeds updating should be interrupted

GPU_version/vio/src/feature_alignment.cpp

@@ -202,7 +202,7 @@ bool align2D(
   bool sign=true;// +
 
   // termination condition
-  const float min_update_squared = 0.05;//0.001
+  const float min_update_squared = 0.5;//0.001
   const int cur_step = cur_img.step.p[0];
 //  float chi2 = 0;
   Vector3f update; update.setZero();

GPU_version/vio/src/frame_handler_base.cpp

@@ -77,7 +77,7 @@ int FrameHandlerBase::finishFrameProcessingCommon(
   if(dropout == RESULT_FAILURE &&
       (stage_ == STAGE_DEFAULT_FRAME || stage_ == STAGE_RELOCALIZING ))
   {
-    stage_ = STAGE_RELOCALIZING;
+    //stage_ = STAGE_RELOCALIZING;
     tracking_quality_ = TRACKING_INSUFFICIENT;
   }
   if(set_reset_)

GPU_version/vio/src/frame_handler_mono.cpp

@@ -170,34 +170,31 @@ 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;
-  //assert (false);
+  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;
   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()>2.0 ||
+
-     fabs(last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>M_PI ||
-     sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()){
-      new_frame_=last_frame_;
-      return RESULT_NO_KEYFRAME;
-  }
   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;
   new_frame_->Cov_ = result.first;
-  /*
+  if(sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()){
-  std::cout<<"Reprojection Map nPoint: "<<overlap_kfs_.back().second
+      return RESULT_FAILURE;
-  <<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_
+  }
-  <<"\tReprojected points after opmization: "<<sfba_n_edges_final<<"\t distance between two frames: "<<
+
-  (last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n';
-*/
   optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
   // select keyframe
   core_kfs_.insert(new_frame_);
@@ -209,6 +206,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   double depth_mean, depth_min;
 
   frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min);
+
   if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD)//edited
   {
     depth_filter_->addFrame(new_frame_);

GPU_version/vio/src/sparse_img_align_gpu.cpp

@@ -64,8 +64,6 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame)
   {
       cv::Mat cur_img = cur_frame->img_pyr_.at(level_);
       cv::Mat ref_img = ref_frame->img_pyr_.at(level_);
-     // cv::imwrite(std::string(getenv("HOME"))+"/Project/cur"+to_string(level_)+".png",cur_img);
-     // cv::imwrite(std::string(getenv("HOME"))+"/Project/fer"+to_string(level_)+".png",ref_img);
       residual_->reload_buf(1,0,cur_img);
       residual_->reload_buf(1,1,ref_img);
       residual_->write_buf(1,6,level_);
@@ -74,6 +72,7 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame)
   }
   cl_float3 pos[1]={0};
   residual_->read(1,2,1,pos);
+  if(isnan(pos[0].x) || isnan(pos[0].y) || isnan(pos[0].z))return 0;
   cur_frame->T_f_w_ = SE2_5(pos[0].x,pos[0].y,pos[0].z);
   return 1;
 }
@@ -90,7 +89,6 @@ double SparseImgAlignGpu::computeResiduals(
     residual_->reload_buf(1,10,chi2_);
     residual_->reload_buf(1,8,H);
     residual_->reload_buf(1,9,J);
-    /// TODO GPU compute based on the feature
     residual_->run(1,feature_counter_);
     float error[300]={0.0};
     float chi[300]={0.0};
@@ -129,19 +127,18 @@ int SparseImgAlignGpu::solve()
                   (double)H[6],(double)H[7],(double)H[8]};
     double Jd[3]={(double)J[0].x,(double)J[0].y,(double)J[0].z};
     x_ = Eigen::Matrix<double,3,3>(Hd).ldlt().solve(Eigen::Vector3d(Jd[0],Jd[1],Jd[2]));
-    if((bool) std::isnan((double) x_[0]))
+    if((bool) std::isnan((double) x_[0]) || (bool) std::isnan((double) x_[1]) ||(bool) std::isnan((double) x_[2]))
         return 0;
     return 1;
 }
 void SparseImgAlignGpu::update()
 {
-/// TODO the update situation may have a smarter solution
     cl_float3 pos[1]={0.0,0.0,0.0};
     residual_->read(1,2,1,pos);
     Sophus::SE2 update =  Sophus::SE2(pos[0].z,Eigen::Vector2d(pos[0].x,pos[0].y)) * Sophus::SE2::exp(-1.0*x_);
     pos[0].x=(float)update.translation()(0);
     pos[0].y=(float)update.translation()(1);
-    pos[0].z=(float)atan(update.so2().unit_complex().imag()/update.so2().unit_complex().real());
+    pos[0].z=(float)atan2(update.so2().unit_complex().imag(),update.so2().unit_complex().real());
     residual_->reload_buf(1,2,pos);
 }
 

GPU_version/vio/src/visualizer.cpp

@@ -55,8 +55,8 @@ void Visualizer::publishMinimal(
     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);
+      msg_pose_with_cov->pose.pose.position.x = odom.second.se2().translation()(1);
-      msg_pose_with_cov->pose.pose.position.y = -odom.second.se2().translation()(1);
+      msg_pose_with_cov->pose.pose.position.y = odom.second.se2().translation()(0);
       msg_pose_with_cov->pose.pose.position.z = 0.0;
       msg_pose_with_cov->pose.pose.orientation.x = q.x();
       msg_pose_with_cov->pose.pose.orientation.y = q.y();
@@ -71,7 +71,7 @@ void Visualizer::publishMinimal(
     pub_pose_with_cov_.publish(msg_pose_with_cov);
     if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0){
           publishPointMarker(
-                  pub_points_, Eigen::Vector3d(-odom.second.se2().translation()(0),-odom.second.se2().translation()(1),0.0), "trajectory",
+                  pub_points_, Eigen::Vector3d(odom.second.se2().translation()(1),odom.second.se2().translation()(0),0.0), "trajectory",
                   ros::Time::now(), trace_id_, 0, 0.01, Vector3d(0.,0.,0.5));
     }
   }

GPU_version/vio/src/vo_node.cpp

@@ -13,7 +13,7 @@
 //
 // You should have received a copy of the GNU General Public License
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
-#define VIO_DEBUG false
+#define VIO_DEBUG true
 #include <ros/ros.h>
 #include <string>
 #include <chrono>
@@ -107,7 +107,7 @@ VioNode::VioNode() :
     if(!vk::camera_loader::loadFromRosNs("vio", cam_))
         throw std::runtime_error("Camera model not correctly specified.");
     Eigen::Matrix<double,3,1> init;
-    init<<1e-19,1e-19,1e-19;
+    init<<1e-19,1e-19,M_PI;
     vo_ = new vio::FrameHandlerMono(cam_,init);
     usleep(500);
 }
@@ -184,8 +184,8 @@ bool VioNode::getOdom(vio::getOdom::Request &req, vio::getOdom::Response &res) {
         res.header.frame_id = "/world";
         res.header.seq = trace_id_;
         auto odom=vo_->ukfPtr_.get_location();
-        res.x=-odom.second.se2().translation()(0);
+        res.x=odom.second.se2().translation()(1);
-        res.y=-odom.second.se2().translation()(1);
+        res.y=odom.second.se2().translation()(0);
         res.yaw=odom.second.pitch();
         res.cov={odom.first(0,0),odom.first(0,1),odom.first(0,2),
                  odom.first(1,0),odom.first(1,1),odom.first(1,2),