track features in all direction but acuraccy is not good

GPU_version/vio/include/vio/math_utils.h

@@ -129,6 +129,20 @@ T getMedian(vector<T>& data_vec)
   nth_element(data_vec.begin(), it, data_vec.end());
   return *it;
 }
+template<class T>
+T getMean(vector<T>& data_vec,T& min=0,T& max=0)
+{
+    assert(!data_vec.empty());
+    T sum=0;
+    min=std::numeric_limits<T>::max();
+    max=std::numeric_limits<T>::min();
+    for(auto&& i:data_vec){
+        sum+=i;
+        if(i<min)min=i;
+        if(i>max)max=i;
+    }
+    return sum/data_vec.size();
+}
 
 inline double pyrFromZero_d(double x_0, int level)
 {

GPU_version/vio/include/vio/ukf.h

@@ -6,8 +6,10 @@
 
 #include <iostream>
 #include <fstream>
-#include "vio/config.h"
+#include <vio/config.h>
 #include <Eigen/Dense>
+#include <boost/thread.hpp>
+#include <boost/function.hpp>
 class Base{
 public:
     Base(const Eigen::Matrix<double, 3, 1> &init) {
@@ -112,36 +114,31 @@ public:
     };
     void UpdateIMU(double x/*in imu frame*/,double y/*in imu frame*/,double theta/*in imu frame*/,const ros::Time& time) {
         if(!start_)return;
-        while(lock)usleep(5);
+        /*x=pow(x,3);//picked up from your code
-        lock=true;
+        y=pow(y,3);//picked up from your code*/
-        x=pow(x,3);//picked up from your code
+        boost::unique_lock<boost::mutex> lock(ekf_mut_);
-        y=pow(y,3);//picked up from your code
         filter_->correct(x,y,theta,1e-9*time.toNSec());
-        lock=false;
     };
     //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;
-        while(lock)usleep(5);
+        boost::unique_lock<boost::mutex> lock(ekf_mut_);
-        lock=true;
         filter_->predict(x,y,theta,1e-9*time.toNSec());
-        lock=false;
     };
-    //Camera frame z front, x right, y down -> right hands (pitch counts from x)correct
+    //Camera frame z back, x right, y down -> right hands (pitch counts from x)correct
     //Notice T_F_W is the position of the first frame with respect to the new frame while they are looking at one feature
     std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> UpdateSvo(double x/*in camera frame*/,double z/*in camera frame*/,double pitch/*in camera frame*/) {
-        while(lock)usleep(5);
+        boost::unique_lock<boost::mutex> lock(ekf_mut_);
-        lock=true;
+        filter_->correct(10.0*x,-10.0*z,-1.0*pitch);
-        filter_->correct(10.0*x,-10.0*z,pitch);
+        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(0.1*filter_->state_(0),-0.1*filter_->state_(1),-1.0*(filter_->state_(2))));
-        lock=false;
-        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(0.1*filter_->state_(0),-0.1*filter_->state_(1),(filter_->state_(2))));
     };
     std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> get_location(){
-        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(0.1*filter_->state_(0),-0.1*filter_->state_(1),(filter_->state_(2))));
+        boost::unique_lock<boost::mutex> lock(ekf_mut_);
+        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(0.1*filter_->state_(0),-0.1*filter_->state_(1),-1.0*(filter_->state_(2))));
     }
     bool start_=false;
 private:
      Base* filter_= nullptr;
-     bool lock=false;
+    boost::mutex ekf_mut_;
 };
 #endif //VIO_UKF_H

GPU_version/vio/param/vo_fast.yaml

@@ -10,7 +10,7 @@ svo:
   kfselect_mindist: 0.01
   poseoptim_thresh: 8.0
   init_min_inliers: 30
-  ACC_noise: 200000 #0.5          #acc white noise in continuous in EKF
+  ACC_noise: 0.5          #acc white noise in continuous in EKF
-  GYO_noise: 200000 #0.5         #gyro 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: 2000000 #0.5  #control vector noise in EKF
+  Cmd_Cov: 0.5  #control vector noise in EKF

GPU_version/vio/src/depth_filter.cpp

@@ -281,12 +281,12 @@ namespace vio {
             float z_min = (*it)->mu + sqrt((*it)->sigma2);
             float z_max = max((*it)->mu - sqrt((*it)->sigma2), 0.00000001f);
 #if VIO_DEBUG
-            fprintf(log_,"[%s]  Z min: %f, Z max: %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),z_min,z_max);
+            fprintf(log_,"[%s]  Z inverse min: %f, Z inverse max: %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),z_min,z_max);
 #endif
-            if(z_min < 0){
+/*            if(z_min < 0){
                 ++it;
                 continue;
-            }
+            }*/
 
             double z;
             if(!matcher_.findEpipolarMatchDirect(

GPU_version/vio/src/frame.cpp

@@ -180,17 +180,21 @@ bool getSceneDepth(const Frame& frame, double& depth_mean, double& depth_min)
   {
     if((*it)->point != NULL)
     {
-      const double z = frame.w2f((*it)->point->pos_).z();
+      double z = frame.w2f((*it)->point->pos_).z();
-      depth_vec.push_back(z);
+      if(z>0 && z<20.0){
-      depth_min = fmin(z, depth_min);
+          depth_vec.push_back(z);
+          depth_min = fmin(z, depth_min);
+      }else{
+          (*it)->point.reset();
+      }
     }
   }
   if(depth_vec.empty())
   {
-    //SVO_WARN_STREAM("Cannot set scene depth. Frame has no point-observations!");
     return false;
   }
-  depth_mean = vk::getMedian(depth_vec);
+  double min,max;
+  depth_mean = vk::getMean(depth_vec,min,max);
   return true;
 }
 

GPU_version/vio/src/frame_handler_mono.cpp

@@ -225,7 +225,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
             fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch()));
 #endif
     if((last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.5 ||
-       fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch())>M_PI_2){
+       fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch())>M_PI_2 || sfba_n_edges_final < Config::qualityMinFts()){
         auto init_f= ukfPtr_.get_location();
         new_frame_->T_f_w_=init_f.second;
         new_frame_->Cov_ = init_f.first;
@@ -243,9 +243,9 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
 #endif
   optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
   depth_filter_->addFrame(new_frame_);
-  if(sfba_n_edges_final < Config::qualityMinFts()){
+/*  if(sfba_n_edges_final < Config::qualityMinFts()){
       return RESULT_FAILURE;
-  }
+  }*/
 
   // select keyframe
   //core_kfs_.insert(new_frame_);

GPU_version/vio/src/initialization.cpp

@@ -54,7 +54,8 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
       ukf_->UpdateSvo(0.0,0.0,0.0);
       return NO_KEYFRAME;
   }
-  double disparity = vk::getMedian(disparities_);
+  double min,max;
+  double disparity = vk::getMean(disparities_,min,max);
     if(disparity < Config::initMinDisparity()){
         auto result=ukf_->get_location();
         if(fabs(result.second.se2().translation().x())>0.2 || fabs(result.second.pitch())>0.0698132)ukf_->UpdateSvo(0.0,result.second.se2().translation().y(),0.0);

GPU_version/vio/src/robust_cost.cpp

@@ -70,7 +70,8 @@ float MADScaleEstimator::
 compute(std::vector<float>& errors) const
 {
   // error must be in absolute values!
-  return NORMALIZER * vk::getMedian(errors);
+  float min,max;
+  return NORMALIZER * vk::getMean(errors,min,max);
 }
 
 float NormalDistributionScaleEstimator::

GPU_version/vio/src/vo_node.cpp

@@ -142,27 +142,13 @@ void VioNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
 void VioNode::imuCb(const sensor_msgs::ImuPtr &imu) {
     if(!start_)return;
     double imu_in[3];
-    if(abs(imu->linear_acceleration.x)>5.0){
+    imu_in[0] = imu->linear_acceleration.x;
-        return;
+    imu_in[1] = imu->linear_acceleration.y;
-    }else{
-        imu_in[0]=imu->linear_acceleration.x;
-    };
-    if(abs(imu->linear_acceleration.y)>5.0){
-        return;
-    }else{
-        imu_in[1]=imu->linear_acceleration.y;
-    };
-    imu_in[0] = 0.5*imu_[0]+0.5*imu->linear_acceleration.x;
-    imu_in[1] = 0.5*imu_[1]+0.5*imu->linear_acceleration.y;
     imu_in[2] = imu->angular_velocity.z;
     memcpy(imu_, imu_in, static_cast<std::size_t>(3*sizeof(double)));
     vo_->UpdateIMU(imu_in,imu->header.stamp);
     imu_time_=imu->header.stamp;
 #if VIO_DEBUG
-    auto odom=vo_->ukfPtr_.get_location();
-    fprintf(vo_->log_,"[%s] Odometry x=%f, y=%f, theta=%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
-            odom.second.se2().translation()(0),odom.second.se2().translation()(1),
-            odom.second.pitch());
     visualizer_.publishMinimal(vo_->ukfPtr_, imu->header.stamp.toSec());
 #endif
 }
@@ -170,17 +156,23 @@ void VioNode::cmdCb(const geometry_msgs::TwistPtr &cmd) {
     if(!start_)return;
     double _cmd[3]={cmd->linear.x,cmd->linear.y,cmd->angular.z};
     vo_->UpdateCmd(_cmd,imu_time_);
+#if VIO_DEBUG
+    auto odom=vo_->ukfPtr_.get_location();
+    fprintf(vo_->log_,"[%s] Odometry x=%f, y=%f, theta=%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
+            10.0*odom.second.se2().translation()(0),10.0*odom.second.se2().translation()(1),
+            odom.second.pitch());
+#endif
 }
 void VioNode::imu_th(){
     ros::NodeHandle nh;
     ros::CallbackQueue Q;
     nh.setCallbackQueue(&Q);
-    ros::Subscriber imu_sub=nh.subscribe(vk::getParam<std::string>("vio/imu_topic", "imu/raw"),10,&VioNode::imuCb, this,ros::TransportHints().tcpNoDelay());
+    ros::Subscriber imu_sub=nh.subscribe(vk::getParam<std::string>("vio/imu_topic", "imu/raw"),1,&VioNode::imuCb, this,ros::TransportHints().tcpNoDelay());
-    ros::Subscriber cmd_sub=nh.subscribe(vk::getParam<std::string>("vio/cmd_topic", "cmd/raw"),10,&VioNode::cmdCb, this,ros::TransportHints().tcpNoDelay());
+    ros::Subscriber cmd_sub=nh.subscribe(vk::getParam<std::string>("vio/cmd_topic", "cmd/raw"),1,&VioNode::cmdCb, this,ros::TransportHints().tcpNoDelay());
     while(start_ && !boost::this_thread::interruption_requested())
     {
         Q.callOne(ros::WallDuration(10,0.0));
-        usleep(10000);
+        usleep(5000);
     }
 
 }