replace kernel fast with opencv

GPU_version/vio/src/point.cpp

@@ -104,6 +104,7 @@ bool Point::getCloseViewObs(const Vector2d& framepos, Feature*& ftr,int id) cons
   Vector3d obs_dir(Vector3d(framepos(0),1e-9,framepos(1)) + pos_); obs_dir.normalize();
   double max_cos_angle = 1.0;
   for(auto&& ob:obs_){
+      if(!ob->frame)continue;
       if(ob->frame->id_==id){
           ftr = ob;
           return true;

GPU_version/vio/src/pose_optimizer.cpp

@@ -55,7 +55,7 @@ void optimizeGaussNewton(
     Vector2d e = vk::project2d((*it)->f)
                - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
     e *= 1.0 / (1<<(*it)->level);
-    chi2_vec_init.push_back(e.squaredNorm()); // just for debug
+    chi2_vec_init.push_back(e.norm()); // just for debug
     errors.push_back(e.norm());
   }
 
@@ -68,8 +68,6 @@ void optimizeGaussNewton(
 #endif
 
   num_obs = errors.size();
-  chi2_vec_init.reserve(num_obs);
-  chi2_vec_final.reserve(num_obs);
   double scale = estimated_scale;
   for(size_t iter=0; iter<n_iter; iter++)
   {
@@ -95,7 +93,7 @@ void optimizeGaussNewton(
       double weight = weight_function.value(e.norm()/scale);
       A.noalias() += J.transpose()*J*weight;
       b.noalias() -= J.transpose()*e*weight;
-      new_chi2 += e.squaredNorm();
+      new_chi2 += e.norm();
     }
 
     // solve linear system
@@ -134,14 +132,15 @@ void optimizeGaussNewton(
       continue;
     Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
     e /= (1<<(*it)->level);
-    chi2_vec_final.push_back(e.squaredNorm());
+    chi2_vec_final.push_back(e.norm());
-    if(e.norm() > 0.01)
+    if(e.norm() > 0.5)
     {
       // we don't need to delete a reference in the point since it was not created yet
       (*it)->point = NULL;
       ++n_deleted_refs;
     }
   }
+    num_obs -= n_deleted_refs;
 #if VIO_DEBUG
     error_init=0.0;
     error_final=0.0;
@@ -149,10 +148,9 @@ 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 scale =%f \t error init =%f \t error end=%f\n",
+    fprintf(log,"[%s] n deleted obs = %d \t n obs felt =%d \t error init =%f \t error end=%f\n",
-            vio::time_in_HH_MM_SS_MMM().c_str(),n_deleted_refs,estimated_scale,error_init,error_final);
+            vio::time_in_HH_MM_SS_MMM().c_str(),n_deleted_refs,num_obs,error_init,error_final);
 #endif
-  num_obs -= n_deleted_refs;
 }
 
 } // namespace pose_optimizer

GPU_version/vio/src/reprojector.cpp

@@ -64,61 +64,6 @@ namespace vio {
     }
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-// TODO - Original implementation
-    void Reprojector::reprojectMap(
-            FramePtr frame,
-            std::vector<std::pair<FramePtr, std::size_t> > &overlap_kfs) {
-        resetGrid();
-
-        list<pair<FramePtr, double> > close_kfs;
-        map_.getCloseKeyframes(frame, close_kfs);
-
-        close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
-                       boost::bind(&std::pair<FramePtr, double>::second, _2));
-
-        size_t n = 0;
-        overlap_kfs.reserve(options_.max_n_kfs);
-        for (auto it_frame = close_kfs.begin(), ite_frame = close_kfs.end();
-             it_frame != ite_frame && n < options_.max_n_kfs; ++it_frame, ++n) {
-            FramePtr ref_frame = it_frame->first;
-            overlap_kfs.push_back(pair<FramePtr, size_t>(ref_frame, 0));
-
-            for (auto it_ftr = ref_frame->fts_.begin(), ite_ftr = ref_frame->fts_.end();
-                 it_ftr != ite_ftr; ++it_ftr) {
-                if ((*it_ftr)->point == NULL)
-                    continue;
-                if ((*it_ftr)->point->last_frame_overlap_id_ == frame->id_)
-                    continue;
-                (*it_ftr)->point->last_frame_overlap_id_ = frame->id_;
-                if (reprojectPoint(frame, (*it_ftr)->point)) {
-                    overlap_kfs.back().second++;
-                }
-
-            }
-        }
-        {
-            boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
-            auto it = map_.point_candidates_.candidates_.begin();
-            while (it != map_.point_candidates_.candidates_.end()) {
-                if (!reprojectPoint(frame, it->first)) {
-                    it->first->n_failed_reproj_ += 3;
-                    if (it->first->n_failed_reproj_ > 30) {
-                        map_.point_candidates_.deleteCandidate(*it);
-                        it = map_.point_candidates_.candidates_.erase(it);
-                        continue;
-                    }
-                }
-                ++it;
-            }
-        }
-        for (size_t i = 0; i < grid_.cells.size(); ++i) {
-            if (reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame))
-                ++n_matches_;
-            if (n_matches_ > (size_t) Config::maxFts())
-                break;
-        }
-    }
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 // TODO - second reimplementation
     void Reprojector::reprojectMap2(
             FramePtr frame,
@@ -128,11 +73,12 @@ namespace vio {
         if(frame->id_<1)return;
         resetGrid();
         cv::Mat descriptors_cur;
-        std::vector<cv::KeyPoint> keypoints_cur;
+        Features keypoints_cur;
         cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4);
         cv::Ptr<cv::BFMatcher> matcher = cv::BFMatcher::create(cv::NORM_HAMMING, true);
-        fast_detector(frame->img(), gpu_fast_, keypoints_cur);
+        feature_detection::FastDetector detector(
-        extractor->compute(frame->img(), keypoints_cur, descriptors_cur);
+                frame->img().cols, frame->img().rows, Config::gridSize(), gpu_fast_,Config::nPyrLevels());
+        detector.detect(frame.get(), frame->img_pyr_, Config::triangMinCornerScore(), keypoints_cur,&descriptors_cur);
         list<pair<FramePtr, double> > close_kfs;
         map_.getCloseKeyframes(frame, close_kfs);
         if (!last_frame->fts_.empty())
@@ -157,13 +103,15 @@ namespace vio {
                 for (auto &&match:matches) {
                     if (match.queryIdx != f.index)continue;
                     if(keypoints_cur.size() < match.trainIdx)continue;
+                    list<Feature*>::iterator point=keypoints_cur.begin();
+                    std::advance(point,match.trainIdx);
                     if (f.item->point == NULL){
-                        const int k = static_cast<int>(keypoints_cur.at(match.trainIdx).pt.y / grid_.cell_size) *
+                        const int k = static_cast<int>((*point)->px.y() / grid_.cell_size) *
                                       grid_.grid_n_cols
-                                      + static_cast<int>(keypoints_cur.at(match.trainIdx).pt.x / grid_.cell_size);
+                                      + static_cast<int>((*point)->px.x() / grid_.cell_size);
                         assert(grid_.cells.at(k) != nullptr);
-                        Vector2d px((int) keypoints_cur.at(match.trainIdx).pt.x,
+                        Vector2d px((int) (*point)->px.x(),
-                                    (int) keypoints_cur.at(match.trainIdx).pt.y);
+                                    (int) (*point)->px.y());
                         frame->fts_.push_back(new Feature(it_frame.item.first.get(),
                                                           new Point(it_frame.item.first->cam_->cam2world(f.item->px.x(), f.item->px.y()),f.item),
                                 px,it_frame.item.first->cam_->cam2world(f.item->px),0));
@@ -174,12 +122,12 @@ namespace vio {
                     }else{
                         if (f.item->point->last_frame_overlap_id_ == frame->id_)continue;
                         f.item->point->last_frame_overlap_id_ = frame->id_;
-                        const int k = static_cast<int>(keypoints_cur.at(match.trainIdx).pt.y / grid_.cell_size) *
+                        const int k = static_cast<int>((*point)->px.y()/ grid_.cell_size) *
                                       grid_.grid_n_cols
-                                      + static_cast<int>(keypoints_cur.at(match.trainIdx).pt.x / grid_.cell_size);
+                                      + static_cast<int>((*point)->px.x() / grid_.cell_size);
                         assert(grid_.cells.at(k) != nullptr);
-                        Vector2d px((int) keypoints_cur.at(match.trainIdx).pt.x,
+                        Vector2d px((int) (*point)->px.x(),
-                                    (int) keypoints_cur.at(match.trainIdx).pt.y);
+                                    (int) (*point)->px.y());
                         //f.item->point->addFrameRef(f.item);
                         grid_.cells.at(k)->push_back(Candidate(f.item->point, px));
                         overlap_kfs.back().second++;