syn imu

GPU_version/vio/include/vio/ukf.h

@@ -122,15 +122,24 @@ 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(!start_)return;
         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
+/*        ++imu_syn_count_;
+        if(imu_syn_[imu_syn_count_ % 7]){
+            imu_syn_[imu_syn_count_ % 7]=false;
+        }else{
+            return;
+        }*/
+        if(imu_syn_count_>50)imu_syn_count_=0;
         boost::unique_lock<boost::mutex> lock(ekf_mut_);
         filter_->correct(x,y,theta,1e-9*time.toNSec());
     };
     //IMU frame y front, x right, z up -> left hands (theta counts from y)
     void UpdateCmd(double x/*in imu frame*/,double y/*in imu frame*/,double theta/*in imu frame*/,const ros::Time& time) {
-        if(!start_)return;
+        cam_syn_[cmd_syn_count_ % 7]=true;
+        imu_syn_[cmd_syn_count_ % 7]=true;
+        ++cmd_syn_count_;
+        if(cmd_syn_count_>50)cmd_syn_count_=0;
         boost::unique_lock<boost::mutex> lock(ekf_mut_);
         filter_->predict(x,y,theta,1e-9*time.toNSec());
     };
@@ -153,7 +162,10 @@ public:
                 filter_->cov_(2,0),filter_->cov_(2,1),filter_->cov_(2,2);
         return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(cov,vio::SE2_5(filter_->state_(0),filter_->state_(1),-1.0*(filter_->state_(2))));
     }
-    bool start_=false;
+    size_t cmd_syn_count_=0;
+    size_t imu_syn_count_=2;
+    bool cam_syn_[7]={false,false,false,false,false,false,false};
+    bool imu_syn_[7]={false,false,false,false,false,false,false};
 private:
      Base* filter_= nullptr;
     boost::mutex ekf_mut_;

GPU_version/vio/param/vo_fast.yaml

@@ -13,4 +13,4 @@ svo:
   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
-  Cmd_Cov: 0.5  #control vector noise in EKF
+  Cmd_Cov: 0.1  #control vector noise in EKF

GPU_version/vio/src/frame_handler_mono.cpp

@@ -99,6 +99,7 @@ FrameHandlerMono::~FrameHandlerMono()
 
 void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const ros::Time& time)
 {
+
   if(!startFrameProcessingCommon(timestamp)){
       return;
   }
@@ -130,7 +131,6 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const
 
 FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
 {
-    ukfPtr_.start_=true;
   if(klt_homography_init_->addFirstFrame(new_frame_) == initialization::FAILURE){
       return RESULT_NO_KEYFRAME;
   }

GPU_version/vio/src/vo_node.cpp

@@ -48,7 +48,6 @@ public:
                 vo_->depthFilter()->startThread();
                 vo_->reset();
             }
-
             imu_the_=new boost::thread(&VioNode::imu_th,this);
             ++trace_id_;
             res.ret=0;
@@ -79,6 +78,7 @@ public:
     };
 private:
     double* imu_;
+    size_t cam_syn_=2;
     ros::Time imu_time_;
     int width;
     int height;
@@ -116,6 +116,13 @@ VioNode::~VioNode()
 
 void VioNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
 {
+/*    ++cam_syn_;
+    if(vo_->ukfPtr_.cam_syn_[cam_syn_ % 3]){
+        vo_->ukfPtr_.cam_syn_[cam_syn_ % 3]=false;
+    }else{
+        return;
+    }
+    if(cam_syn_>50)cam_syn_=0;*/
     if(!start_)return;
       try {
           cv::Mat img=cv_bridge::toCvShare(msg, "mono8")->image;