use level in features

GPU_version/vio/include/vio/initialization.h

@@ -46,6 +46,7 @@ public:
 
 protected:
   vector<cv::Point2f> px_ref_;      //!< keypoints to be tracked in reference frame.
+  vector<int> px_level;
   vector<cv::Point2f> px_cur_;      //!< tracked keypoints in current frame.
   vector<Vector3d> f_ref_;          //!< bearing vectors corresponding to the keypoints in the reference image.
   vector<Vector3d> f_cur_;          //!< bearing vectors corresponding to the keypoints in the current image.
@@ -107,6 +108,7 @@ protected:
 void detectFeatures(
     FramePtr frame,
     vector<cv::Point2f>& px_vec,
+    vector<int>& px_lvl,
     vector<Vector3d>& f_vec,
     opencl* gpu_fast);
 void trackKlt(

GPU_version/vio/src/feature_detection.cpp

@@ -99,7 +99,7 @@ void FastDetector::detect(
       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*scale, fast_corners[i].y*scale, score, L, 0.0f);
+          corners.at(k) = Corner(fast_corners[i].x, fast_corners[i].y, score, L, 0.0f);
           // Create feature for every corner that has high enough corner score
           if(corners.at(k).score > detection_threshold){
               fts.push_back(make_shared<Feature>(frame, Vector2d(corners.at(k).x, corners.at(k).y), corners.at(k).level));

GPU_version/vio/src/initialization.cpp

@@ -28,7 +28,7 @@ namespace initialization {
 InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref)
 {
   reset();
-  detectFeatures(frame_ref, px_ref_, f_ref_,gpu_fast_);
+  detectFeatures(frame_ref, px_ref_, px_level, f_ref_, gpu_fast_);
   if(px_ref_.size() < 100)
   {
     SVO_WARN_STREAM_THROTTLE(2.0, "First image has less than 100 features. Retry in more textured environment.");
@@ -91,11 +91,11 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
             Vector3d pos = frame_cur->getSE3Inv() * (xyz_in_cur_[*it]/* *scale */);
             std::shared_ptr<Point> new_point = std::make_shared<Point>(pos);
 
-            std::shared_ptr<Feature> ftr_cur=std::make_shared<Feature>(frame_cur, new_point, px_cur, f_cur_[*it], 0);
+            std::shared_ptr<Feature> ftr_cur=std::make_shared<Feature>(frame_cur, new_point, px_cur, f_cur_[*it], px_level[*it]);
             frame_cur->addFeature(ftr_cur);
             new_point->addFrameRef(ftr_cur);
 
-            std::shared_ptr<Feature> ftr_ref=std::make_shared<Feature>(frame_ref_, new_point, px_ref, f_ref_[*it], 0);
+            std::shared_ptr<Feature> ftr_ref=std::make_shared<Feature>(frame_ref_, new_point, px_ref, f_ref_[*it], px_level[*it]);
             frame_ref_->addFeature(ftr_ref);
             new_point->addFrameRef(ftr_ref);
         }
@@ -121,6 +121,7 @@ void KltHomographyInit::reset()
 void detectFeatures(
     FramePtr frame,
     vector<cv::Point2f>& px_vec,
+    vector<int>& px_lvl,
     vector<Vector3d>& f_vec,
     opencl* gpu_fast)
 {
@@ -133,6 +134,7 @@ void detectFeatures(
   f_vec.clear();
   for(auto&& ftr:new_features){
       px_vec.push_back(cv::Point2f(ftr->px[0], ftr->px[1]));
+      px_lvl.push_back(ftr->level);
       f_vec.push_back(ftr->f);
       ftr.reset();
   }

GPU_version/vio/src/pose_optimizer.cpp

@@ -63,9 +63,9 @@ void optimizeGaussNewton(
     return;
   vk::robust_cost::MADScaleEstimator scale_estimator;
   estimated_scale = scale_estimator.compute(errors);
-#if VIO_DEBUG
-    fprintf(log,"[%s] Init estimate scale:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),estimated_scale);
-#endif
+//#if VIO_DEBUG
+//    fprintf(log,"[%s] Init estimate scale:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),estimated_scale);
+//#endif
 
   num_obs = errors.size();
   double scale = estimated_scale;
@@ -102,10 +102,10 @@ 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
+//#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;
     }
@@ -115,10 +115,10 @@ void optimizeGaussNewton(
     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
+//#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)