change feature score in fast algorithm and add brightness

GPU_version/vio/include/vio/feature_detection.h

@@ -104,7 +104,7 @@ public:
       std::shared_ptr<Frame> frame,
       const ImgPyr& img_pyr,
       const double detection_threshold,
-      Features& fts,
+      list<shared_ptr<Feature>>& fts,
       cv::Mat* descriptors= nullptr);
   opencl* gpu_fast_;
 };

GPU_version/vio/include/vio/ukf.h

@@ -132,12 +132,12 @@ public:
     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);
         lock=true;
-        filter_->correct(x,-1.0*z,pitch);
+        filter_->correct(10.0*x,-1.0*z,-1.0*pitch);
         lock=false;
-        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(0),-1.0*filter_->state_(1),(filter_->state_(2))));
+        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(0.1*filter_->state_(0),-1.0*filter_->state_(1),-1.0*(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(filter_->state_(0),-1.0*filter_->state_(1),(filter_->state_(2))));
+        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(0.1*filter_->state_(0),-1.0*filter_->state_(1),-1.0*(filter_->state_(2))));
     }
     bool start_=false;
 private:

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

@@ -10,7 +10,8 @@ svo:
   kfselect_mindist: 0.01
   poseoptim_thresh: 8.0
   init_min_inliers: 30
-  ACC_noise: 100          #acc white noise in continuous in EKF
+  map_scale: 0.5
-  GYO_noise: 100          #gyro white noise in continuous in EKF
+  ACC_noise: 2000          #acc 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
+  GYO_noise: 2000         #gyro white noise in continuous in EKF
-  Cmd_Cov: 100  #control vector noise in EKF
+  svo_ekf: 0.0001              #Svo noise that will be devided by number of Reprojected points after opmization in EKF
+  Cmd_Cov: 2000  #control vector noise in EKF

GPU_version/vio/src/depth_filter.cpp

@@ -269,7 +269,7 @@ namespace vio {
 #if VIO_DEBUG
             fprintf(log_,"[%s]  If point is visible? %f, %f, %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),xyz_f.x(),xyz_f.y(),xyz_f.z());
 #endif
-            if(xyz_f.z() > 0.0)  {
+            if(xyz_f.z() < 0.0)  {
                 ++it; // behind the camera
                 continue;
             }

GPU_version/vio/src/feature_detection.cpp

@@ -69,51 +69,51 @@ void FastDetector::detect(
     std::shared_ptr<Frame> frame,
     const ImgPyr& img_pyr,
     const double detection_threshold,
-    Features& fts,
+    list<shared_ptr<Feature>>& fts,
     cv::Mat* descriptors){
-  Corners corners(grid_n_cols_*grid_n_rows_, Corner(0,0,detection_threshold,0,0.0f));
+  Corners corners(grid_n_cols_*grid_n_rows_, Corner(0,0,0.0,0,0.0f));
   std::vector<cv::KeyPoint> keypoints;
+  fts.clear();
   for(int L=0; L<n_pyr_levels_; ++L)
   {
     const int scale = (1<<L);
     cv::Mat img=img_pyr.at(L);
     if(!gpu_fast_->reload_buf(0,0,img)){
-        std::cerr<<"Failed to write into GPU goodbye :)"<<'\n';
+        ROS_ERROR("Failed to write into GPU goodbye :)");
         exit(0);
     };
     cl_int icorner[1]={0};
     if(!gpu_fast_->reload_buf(0,2,icorner)){
-          std::cerr<<"Failed to write into GPU goodbye :)"<<'\n';
+          ROS_ERROR("Failed to write into GPU goodbye :)");
           exit(0);
       };
     gpu_fast_->run(0,img.cols,img.rows);
-    int count[1]={0};
+    cl_int count[1]={0};
     gpu_fast_->read(0,2,1,count);
     cl_int2 fast_corners[350000]={0};
     gpu_fast_->read(0,1,count[0],fast_corners);
+    if(count[0]<1 ){
+          ROS_ERROR("Can not communicate with GPU");
+          exit(0);
+    }
     for(uint i=0;i<count[0];++i)
     {
-      if(fast_corners[i].x<0 || fast_corners[i].x>img_pyr[L].cols || fast_corners[i].y>img_pyr[L].rows || fast_corners[i].y<0)
+      if(fast_corners[i].x<0 || fast_corners[i].x>img_pyr[L].cols || fast_corners[i].y>img_pyr[L].rows || fast_corners[i].y<0){
           continue;
-
+      }
       const int k = static_cast<int>((fast_corners[i].y*scale)/cell_size_)*grid_n_cols_
                   + static_cast<int>((fast_corners[i].x*scale)/cell_size_);
 
-      if(grid_occupancy_[k] || k > corners.size())
+      if(grid_occupancy_[k] || k > corners.size()){
-        continue;
+          continue;
-
+      }
       const float score = vk::shiTomasiScore(img_pyr[L], fast_corners[i].x, fast_corners[i].y);
-
       if(score > corners.at(k).score){
-          corners.at(k) = Corner(fast_corners[i].x, fast_corners[i].y, score, L, 0.0f);
+          corners.at(k) = Corner(fast_corners[i].x*scale, fast_corners[i].y*scale, score, L, 0.0f);
-          // Create feature for every corner that has high enough corner score
+          fts.push_back(make_shared<Feature>(frame, Vector2d(corners.at(k).x, corners.at(k).y)*scale, corners.at(k).level));
-          if(corners.at(k).score > detection_threshold){
+          if(descriptors)keypoints.push_back(cv::KeyPoint(fast_corners[i].x*scale, fast_corners[i].y*scale, 1));
-              fts.push_back(make_shared<Feature>(frame, Vector2d(corners.at(k).x, corners.at(k).y)*scale, corners.at(k).level));
-              if(descriptors)keypoints.push_back(cv::KeyPoint(fast_corners[i].x*scale, fast_corners[i].y*scale, 1));
-          }
       }
     }
-
   }
   if(descriptors){
       cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4);

GPU_version/vio/src/frame_handler_mono.cpp

@@ -131,8 +131,9 @@ 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)
+  if(klt_homography_init_->addFirstFrame(new_frame_) == initialization::FAILURE){
-    return RESULT_NO_KEYFRAME;
+      return RESULT_NO_KEYFRAME;
+  }
   new_frame_->setKeyframe();
   map_.reset();
   map_.addKeyframe(new_frame_);
@@ -224,7 +225,8 @@ 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()/*10*/){
         auto init_f= ukfPtr_.get_location();
         new_frame_->T_f_w_=init_f.second;
         new_frame_->Cov_ = init_f.first;
@@ -242,7 +244,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
 #endif
   optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
   depth_filter_->addFrame(new_frame_);
-  if(sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()/*10*/){
+  if(sfba_n_edges_final < Config::qualityMinFts()){
       return RESULT_FAILURE;
   }
 

GPU_version/vio/src/initialization.cpp

@@ -28,9 +28,12 @@ namespace initialization {
 InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref)
 {
   reset();
+  features_ref_.clear();
+  px_ref_.clear();
   detectFeatures(frame_ref, px_ref_, features_ref_, gpu_fast_);
   if(px_ref_.size() < 100)
   {
+    ROS_WARN("Process first frame. Detected observations are px=%d, features=%d less than 100",px_ref_.size(),features_ref_.size());
     return FAILURE;
   }
 #if VIO_DEBUG
@@ -80,6 +83,15 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
       return NO_KEYFRAME;
   }
     frame_cur->T_f_w_ = T_cur_from_ref_;
+    // Rescale the map such that the mean scene depth is equal to the specified scale
+    vector<double> depth_vec;
+    for(size_t i=0; i<xyz_in_cur_.size(); ++i)
+        depth_vec.push_back((xyz_in_cur_[i]).z());
+    double scene_depth_median = vk::getMedian(depth_vec);
+    double scale = Config::mapScale()/scene_depth_median;
+    frame_cur->T_f_w_ = SE2_5(T_cur_from_ref_.se2() * frame_ref_->T_f_w_.se2());
+    frame_cur->T_f_w_ = SE2_5(SE2(frame_cur->T_f_w_.se2().rotation_matrix(),
+                                  -frame_cur->T_f_w_.se2().rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos()))));
   // For each inlier create 3D point and add feature in both frames
   for(auto&& id:inliers_){
       Features::iterator first=features_ref_.begin();

GPU_version/vio/src/pose_optimizer.cpp

@@ -102,10 +102,7 @@ void optimizeGaussNewton(
     // check if error increased
     if((iter > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dT[0]))
     {
-//#if VIO_DEBUG
+
-//          fprintf(log,"[%s] it:%d \t FAILURE \t new_chi2: %f\n",
-//                  vio::time_in_HH_MM_SS_MMM().c_str(),iter,new_chi2);
-//#endif
       frame->T_f_w_ = T_old; // roll-back
       break;
     }
@@ -113,12 +110,7 @@ void optimizeGaussNewton(
     // update the model
     T_old = frame->T_f_w_;
     frame->T_f_w_=SE2_5(T_old.se2().translation().x()+dT.x(),T_old.se2().translation().y()+dT.y(),T_old.pitch()+dT.z());
-
     chi2 = new_chi2;
-//#if VIO_DEBUG
-//      fprintf(log,"[%s] it:%d \t Success \t new_chi2: %f \t dT = %f, %f, %f\n",
-//              vio::time_in_HH_MM_SS_MMM().c_str(),iter,new_chi2,dT.x(),dT.y(),dT.z());
-//#endif
 
     // stop when converged
     if(vk::norm_max(dT) <= EPS)
@@ -148,7 +140,7 @@ void optimizeGaussNewton(
         error_init /=chi2_vec_init.size();
     if(!chi2_vec_final.empty())for(auto&& i:chi2_vec_final)error_final+=i;
         error_final /= chi2_vec_final.size();
-    fprintf(log,"[%s] n deleted obs = %d \t n obs felt =%d \t error init =%f \t error end=%f\n",
+    fprintf(log,"[%s] n deleted obs = %d \t n obs with reprojection error less than 0.5 px =%d \t error init =%f \t error end=%f\n",
             vio::time_in_HH_MM_SS_MMM().c_str(),n_deleted_refs,num_obs,error_init,error_final);
 #endif
 }

GPU_version/vio/src/reprojector.cpp

@@ -112,6 +112,7 @@ namespace vio {
                         const int k = static_cast<int>((*point)->px.y() / grid_.cell_size) *
                                       grid_.grid_n_cols
                                       + static_cast<int>((*point)->px.x() / grid_.cell_size);
+                        if(k>grid_.cells.size()-1)continue;
                         assert(grid_.cells.at(k) != nullptr);
                         Vector2d px((int) (*point)->px.x(),
                                     (int) (*point)->px.y());
@@ -129,6 +130,7 @@ namespace vio {
                         const int k = static_cast<int>((*point)->px.y()/ grid_.cell_size) *
                                       grid_.grid_n_cols
                                       + static_cast<int>((*point)->px.x() / grid_.cell_size);
+                        if(k>grid_.cells.size()-1)continue;
                         assert(grid_.cells.at(k) != nullptr);
                         Vector2d px((int) (*point)->px.x(),
                                     (int) (*point)->px.y());

GPU_version/vio/src/vo_node.cpp

@@ -119,12 +119,22 @@ void VioNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
       try {
           cv::Mat img=cv_bridge::toCvShare(msg, "mono8")->image;
           if(img.empty())return;
-          cv::Mat imgbul;//tem;
+          cv::Mat imgbul,float_img,frame,show;//tem;
-          cv::Laplacian(img,imgbul,CV_64F);
+          img.convertTo(float_img,CV_64F,1.f/255);
+          float_img*=2.0;
+          float_img+=0.2;
+          float_img.convertTo(frame,CV_8UC1,255);
+          cv::Laplacian(frame,imgbul,CV_64F);
           cv::Scalar mean, stddev;
           meanStdDev(imgbul, mean, stddev, cv::Mat());
-          if(stddev.val[0] * stddev.val[0]< 50.0)return;
+          cv::cvtColor(frame,show,CV_GRAY2RGB);
-          vo_->addImage(img, msg->header.stamp.toSec(),msg->header.stamp);
+          imshow("New Image", show);
+          cv::waitKey(10);
+          if(stddev.val[0] * stddev.val[0]< 30.0){
+              ROS_WARN("Frame is blur or too dark");
+              return;
+          }
+          vo_->addImage(frame, msg->header.stamp.toSec(),msg->header.stamp);
       } catch (cv_bridge::Exception& e) {
         ROS_ERROR("vo exception: %s", e.what());
       }
@@ -152,7 +162,7 @@ void VioNode::imuCb(const sensor_msgs::ImuPtr &imu) {
     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());
+            odom.second.pitch());
     visualizer_.publishMinimal(vo_->ukfPtr_, imu->header.stamp.toSec());
 #endif
 }
@@ -170,7 +180,7 @@ void VioNode::imu_th(){
     while(start_ && !boost::this_thread::interruption_requested())
     {
         Q.callOne(ros::WallDuration(10,0.0));
-        usleep(20000);
+        usleep(10000);
     }
 
 }