pipline is working but we have a problem number of observation is too high

GPU_version/vio/depth_filter_log.txt

@@ -1,6 +1,18 @@
-[17:12:03.647] New key frame 
+[19:47:24.922] New key frame 
-[17:12:03.647] BA loop 
+[19:47:24.922] BA loop 
-[17:12:44.004] New key frame 
+[19:47:25.028] New key frame 
-[17:12:44.004] BA loop 
+[19:47:25.028] BA loop 
-[17:12:44.192] New key frame 
+[19:47:25.138] New key frame 
-[17:12:44.192] BA loop 
+[19:47:25.138] BA loop 
+[19:47:25.234] New key frame 
+[19:47:25.234] BA loop 
+[19:47:30.451] New key frame 
+[19:47:30.451] BA loop 
+[19:47:32.031] New key frame 
+[19:47:32.031] BA loop 
+[19:47:44.267] New key frame 
+[19:47:44.268] BA loop 
+[19:48:08.881] New key frame 
+[19:48:08.881] BA loop 
+[19:49:01.049] New key frame 
+[19:49:01.049] BA loop 

GPU_version/vio/include/vio/ukf.h

@@ -23,14 +23,14 @@ public:
     virtual ~Base(){
     };
     void predict(double dx,double dy,double dpitch,double time){
-        double dt=time-t_cmd;
+        double dt=0.1;
-        if(t_cmd==0.0){
+        /*if(t_cmd==0.0){
             t_cmd=time;
             return;
-        }
+        }*/
         t_cmd=time;
         state_h_(2)=state_(2)+dt*dpitch;
-        state_h_(1)=state_(1)+dt*2.0*(dy*cos(state_(2))-dx*sin(state_(2)));
+        state_h_(1)=state_(1)+dt*/*2.0**/(dy*cos(state_(2))-dx*sin(state_(2)));
         state_h_(0)=state_(0)+dt*(dx*cos(state_(2))+dy*sin(state_(2)));
         state_h_(3)=dpitch;
         Eigen::Matrix<double,4,4> R;
@@ -40,28 +40,28 @@ public:
            0,0,0,vio::Config::Cmd_Cov();
         Eigen::Matrix<double,4,4> G;
         G<<1.0,0,dt*(-dx*sin(state_(2))+dy*cos(state_(2))),0.0,
-           0,1.0,dt*2.0*(-dy*sin(state_(2))-dx*cos(state_(2))),0.0,
+           0,1.0,dt*/*2.0**/(-dy*sin(state_(2))-dx*cos(state_(2))),0.0,
            0,0,1.0,dt,
            0,0,0,1.0;
         cov_h_=G*cov_*G.transpose()+R;
     }
     void correct(double ddx, double ddy, double dpitch, double time){
-        double dt=time-t_imu;
+        double dt=0.005;
-        if(t_imu==0.0){
+        /*if(t_imu==0.0){
             t_imu=time;
             return;
-        }
+        }*/
         t_imu=time;
         Eigen::Matrix<double,4,4> H;
         Eigen::Matrix<double,4,4> Q;
         Eigen::Matrix<double,4,1> E;
         Eigen::Matrix<double,4,4> k;
         E(0)=state_(0)+pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2)))-state_h_(0);
-        E(1)=state_(1)+2.0*pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2)))-state_h_(1);
+        E(1)=state_(1)+/*2.0**/pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2)))-state_h_(1);
         E(2)=state_(2)+dt*state_(3)-state_h_(2);
         E(3)=dpitch-state_h_(3);
         H<<1.0,0,pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2))),0.0,
-           0,1.0,-2.0*pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2))),0.0,
+           0,1.0,-/*2.0**/pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2))),0.0,
            0,0,1.0,dt,
            0,0,0,1.0;
         Q<<vio::Config::ACC_Noise(),0,0,0,
@@ -72,7 +72,7 @@ public:
         state_=state_h_+k*E;
         cov_=(Eigen::MatrixXd::Identity(4,4)-k*H)*cov_h_;
         state_h_(2)=state_(2)+dt*state_(3);
-        state_h_(1)=state_(1)+2.0*pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2)));
+        state_h_(1)=state_(1)+/*2.0**/pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2)));
         state_h_(0)=state_(0)+pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2)));
         state_h_(3)=state_(3);
         Eigen::Matrix<double,4,4> R;
@@ -82,7 +82,7 @@ public:
                 0,0,0,vio::Config::GYO_Noise();
         Eigen::Matrix<double,4,4> G;
         G<<1.0,0,pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2))),0,
-           0,1.0,2.0*pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2))),0,
+           0,1.0,/*2.0**/pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2))),0,
            0,0,1.0,dt,
            0,0,0,1.0;
         cov_h_=G*cov_*G.transpose()+R;
@@ -122,8 +122,8 @@ public:
         delete filter_;
     };
     void UpdateIMU(double x/*in imu frame*/,double y/*in imu frame*/,double theta/*in imu frame*/,const ros::Time& time) {
-        if(abs(x)<1.0)x=pow(x,3);//picked up from your code
+       /* if(abs(x)<1.0)x=pow(x,3);//picked up from your code
-        if(abs(y)<1.0)y=pow(y,3);//picked up from your code
+        if(abs(y)<1.0)y=pow(y,3);//picked up from your code*/
         ++imu_syn_count_;
         if(imu_syn_[imu_syn_count_ % 7]){
             imu_syn_[imu_syn_count_ % 7]=false;

GPU_version/vio/launch/Px30.launch

@@ -1,6 +1,6 @@
 <launch>
 
-  <!-- node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" / -->
+  <node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" />
 
     <!-- Camera topic to subscribe to -->
     <param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />

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.5         #gyro white noise in continuous in EKF
-  svo_ekf: 0.05              #Svo noise that will be devided by number of Reprojected points after opmization in EKF
+  svo_ekf: 0.1              #Svo noise that will be devided by number of Reprojected points after opmization in EKF
-  Cmd_Cov: 1.0  #control vector noise in EKF
+  Cmd_Cov: 0.1  #control vector noise in EKF

GPU_version/vio/src/frame_handler_mono.cpp

@@ -276,12 +276,17 @@ bool FrameHandlerMono::needNewKf()
     SE2_5 closest_kfs(0,0,0);
   for(auto&& it:overlap_kfs_)
   {
+      if(it.first->id_==last_frame_->id_)continue;
       if(it.second>com_obs){
           com_obs=it.second;
           closest_kfs=SE2_5(it.first->T_f_w_.se2());
       }
   }
-  if(fabs(closest_kfs.pitch()-new_frame_->T_f_w_.pitch()) > 0.43 || fabs((closest_kfs.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm())>0.15)return true;
+#if VIO_DEBUG
+    fprintf(log_,"[%s] need key frame pitch dis: %f translation dif:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
+            fabs(closest_kfs.pitch()-new_frame_->T_f_w_.pitch()),(closest_kfs.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm());
+#endif
+  if(fabs(closest_kfs.pitch()-new_frame_->T_f_w_.pitch()) > 0.2 || fabs((closest_kfs.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm())>0.1)return true;
   return false;
 }
 void FrameHandlerMono::UpdateIMU(double* value,const ros::Time& time){

GPU_version/vio/src/point.cpp

@@ -184,16 +184,11 @@ void Point::optimize(const size_t n_iter)
       break;
   }
 
-/*    for(auto it=obs_.begin(); it!=obs_.end(); ++it) {
+    for(auto it=obs_.begin(); it!=obs_.end(); ++it) {
             Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d((*it)->frame->se3() * (*it)->point->pos_));
-            //e /= (1 << (*it)->level);
+            if (e.norm() > 1.0)
-//            std::cerr<<"error - point.h : "<<e.norm()<<std::endl;
-            if (e.norm() > 1.0) //{
-                // we don't need to delete a reference in the point since it was not created yet
                 (*it)->point->n_failed_reproj_++;
-//            } else
+    }
-//                it++;
-    }*/
 
 }
 

GPU_version/vio/src/reprojector.cpp

@@ -81,18 +81,10 @@ namespace vio {
         std::vector<cv::KeyPoint> keypoints_cur;
         list<std::shared_ptr<Feature>>::iterator it_cur=keypoints.begin();
         cv::Mat cur_des=cv::Mat(keypoints.size(),64,CV_8UC1);
-//        std::cerr<<"84"<<std::endl;
+
-        //cv::Mat maskup = cv::Mat(keypoints.size(),64,CV_8UC1);
-        //cv::Mat maskdown = cv::Mat(keypoints.size(),64,CV_8UC1);
         cv::Mat maskup = cv::Mat_<uchar>(1, keypoints.size());
         cv::Mat maskdown = cv::Mat_<uchar>(1, keypoints.size());
-//        std::cerr<<"88"<<std::endl;
+
-////        uint8_t one[64] = {255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
-////                           255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
-////                           255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
-////                           255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255};
-////        uint8_t zero[64] = {0};
-//        std::cerr<<"94"<<std::endl;
 
         for (int i=0;i<keypoints.size() && it_cur !=keypoints.end();++i) {
             keypoints_cur.push_back(cv::KeyPoint((*it_cur)->px.x(), (*it_cur)->px.y(), 7.f, (*it_cur)->score));
@@ -100,18 +92,13 @@ namespace vio {
             if ((*it_cur)->px.y() < frame->img().rows/2) {
                 maskup.at<uchar>(0, i) = 1;
                 maskdown.at<uchar>(0, i) = 0;
-                //memcpy(maskup.data + (i * 64), one, sizeof(uint8_t) * 64);
-                //memcpy(maskdown.data + (i * 64), zero, sizeof(uint8_t) * 64);
             }
             else{
                 maskup.at<uchar>(0, i) = 0;
                 maskdown.at<uchar>(0, i) = 1;
-                //memcpy(maskup.data + (i * 64), zero, sizeof(uint8_t) * 64);
-                //memcpy(maskdown.data + (i * 64), one, sizeof(uint8_t) * 64);
             }
             ++it_cur;
         }
-//        std::cerr<<"114"<<std::endl;
         list<pair<FramePtr, double> > close_kfs;
         map_.getCloseKeyframes(frame, close_kfs);
         if (!last_frame->fts_.empty())