review with original version

GPU_version/vio/kernel/compute-residual.cl

@@ -34,33 +34,6 @@ float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature)
     return  (float3)(R00*ref_feature.x+R01*ref_feature.y+R02*ref_feature.z+error.x,
                      R10*ref_feature.x+R11*ref_feature.y+R12*ref_feature.z,
                      R20*ref_feature.x+R21*ref_feature.y+R22*ref_feature.z+error.y);
-/*
-    if(ref.z < 0.0)
-        ref.z = 3.141592653589793238462643383279502884197169399375 - ref.z;
-    else
-        ref.z = ref.z - 3.1415926535897932384626433832795028841971693993;
-    float cc_ss = cos(cur.z) * cos(ref.z) - sin(cur.z) * sin(ref.z);
-    float sc_cs = cos(cur.z) * sin(ref.z) + cos(ref.z) * sin(cur.z);
-
-    return  (float3)(cc_ss * ref_feature.x + sc_cs * ref_feature.z + cur.x - ref.x * cos(cur.z) - ref.z * sin(cur.z),
-                     ref_feature.y,
-                     -1.0 * sc_cs * ref_feature.x + cc_ss * ref_feature.z + cur.z + ref.x * sin(cur.z) - ref.z * cos(cur.z));
-
-    // alfa = 0,  betha = 15 degree - 0.261799 radian
-    //return  (float3)(((float)(cos(0.261799)*ref_feature.x)+(float)(sin(0.261799)*sin(cur.z)*ref_feature.y)+(float)(sin(0.261799)*cos(cur.z)*ref_feature.z)),
-    //                ((float)(cos(cur.z)*ref_feature.y)-(float)(sin(cur.z)*ref_feature.z)),
-    //                ((float)(-1*sin(0.261799)*ref_feature.x)+(float)(cos(0.261799)*sin(cur.z)*ref_feature.y)+(float)(cos(0.261799)*cos(cur.z)*ref_feature.z)));
-    //
-    // betha = 0,  alfa = 15 degree - 0.261799 radian
-    //return  (float3)(((float)(cos(0.261799)*ref_feature.x)+(float)(-1*sin(0.261799)*cos(cur.z)*ref_feature.y)+(float)(sin(0.261799)*sin(cur.z)*ref_feature.z)),
-    //                    ((float)(sin(0.261799)*ref_feature.x)+(float)(cos(0.261799)*cos(cur.z)*ref_feature.y)-(float)(cos(0.261799)*sin(cur.z)*ref_feature.z)),
-    //                    ((float)(sin(cur.z)*ref_feature.y)+(float)(cos(cur.z)*ref_feature.z)));
-    }
-
-
-    return  (float3)(cc_ss * ref_feature.x + sc_cs * ref_feature.z + cur.x - ref.x * cos(cur.z) - ref.z * sin(cur.z),
-                     ref_feature.y,
-                     -1.0 * sc_cs * ref_feature.x + cc_ss * ref_feature.z + cur.z + ref.x * sin(cur.z) - ref.z * cos(cur.z));*/
 }
 
 void jacobian_xyz2uv(float3 xyz_in_f, float* J)

GPU_version/vio/src/depth_filter.cpp

@@ -190,6 +190,8 @@ namespace vio {
                 }
             }
             updateSeeds(frame);
+            if(frame->isKeyframe())
+                initializeSeeds(frame);
         }
     }
 
@@ -215,7 +217,7 @@ namespace vio {
                 continue;
             }
             // check if point is visible in the current image
-            SE2 T(it->ftr->frame->T_f_w_.pitch()-frame->T_f_w_.pitch(),it->ftr->frame->T_f_w_.se2().translation()- frame->T_f_w_.se2().translation());
+            SE2 T(it->ftr->frame->T_f_w_.pitch()+frame->T_f_w_.pitch()+M_PI,it->ftr->frame->T_f_w_.se2().translation()- frame->T_f_w_.se2().translation());
             ///TODO add 15 degrees roll orientation
             Quaterniond q;
             q = AngleAxisd(-0.261799, Vector3d::UnitX())

GPU_version/vio/src/feature_alignment.cpp

@@ -36,9 +36,9 @@ bool align1D(
     Vector2d& cur_px_estimate,
     double& h_inv)
 {
-  const int halfpatch_size_ = 1;
+  const int halfpatch_size_ = 4;
-  const int patch_size = 2;
+  const int patch_size = 8;
-  const int patch_area = 4;
+  const int patch_area = 64;
   bool converged=false;
 
   // compute derivative of template and prepare inverse compositional
@@ -163,9 +163,9 @@ bool align2D(
     return align2D_NEON(cur_img, ref_patch_with_border, ref_patch, n_iter, cur_px_estimate);
 #endif
 
-  const int halfpatch_size_ = 1;
+  const int halfpatch_size_ = 4;
-  const int patch_size_ = 2;
+  const int patch_size_ = 8;
-  const int patch_area_ = 4;
+  const int patch_area_ = 64;
   bool converged=false;
 
   // compute derivative of template and prepare inverse compositional

GPU_version/vio/src/frame.cpp

@@ -138,7 +138,7 @@ void Frame::removeKeyPoint(Feature* ftr)
 
 bool Frame::isVisible(const Vector3d& xyz_w) const
 {
-  Vector3d xyz_f = Vector3d(this->getSE3Inv()*xyz_w);
+  Vector3d xyz_f = Vector3d(this->se3()*xyz_w);
   if(xyz_f.z() < 0.0)
     return false; // point is behind the camera
   Vector2d px = f2c(xyz_f);

GPU_version/vio/src/frame_handler_mono.cpp

@@ -171,7 +171,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
   if(img_align.run(last_frame_, new_frame_)==0)return  RESULT_FAILURE;
   //reprojector_.reprojectMap(new_frame_, overlap_kfs_);
-  //  reprojector_.reprojectMap1(new_frame_, last_frame_, overlap_kfs_);
+    //reprojector_.reprojectMap1(new_frame_, last_frame_, overlap_kfs_);
     reprojector_.reprojectMap2(new_frame_, overlap_kfs_);
   //assert (false);
 

GPU_version/vio/src/reprojector.cpp

@@ -70,55 +70,63 @@ void Reprojector::reprojectMap(
     FramePtr frame,
     std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs)
 {
-  resetGrid();
+    resetGrid();
-  list< pair<FramePtr,double> > close_kfs;
+
-  map_.getCloseKeyframes(frame, close_kfs);
+        list< pair<FramePtr,double> > close_kfs;
-  close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
+        map_.getCloseKeyframes(frame, close_kfs);
-                 boost::bind(&std::pair<FramePtr, double>::second, _2));
+
-  size_t n = 0;
+        close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
-  overlap_kfs.reserve(options_.max_n_kfs);
+                       boost::bind(&std::pair<FramePtr, double>::second, _2));
-  for(auto it_frame=close_kfs.begin(), ite_frame=close_kfs.end();
+
-      it_frame!=ite_frame && n<options_.max_n_kfs; ++it_frame, ++n)
+        size_t n = 0;
-  {
+        overlap_kfs.reserve(options_.max_n_kfs);
-    FramePtr ref_frame = it_frame->first;
+        for(auto it_frame=close_kfs.begin(), ite_frame=close_kfs.end();
-    overlap_kfs.push_back(pair<FramePtr,size_t>(ref_frame,0));
+            it_frame!=ite_frame && n<options_.max_n_kfs; ++it_frame, ++n)
-    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_projected_kf_id_ == frame->id_)
-        continue;
-      (*it_ftr)->point->last_projected_kf_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);
+            FramePtr ref_frame = it_frame->first;
-          it = map_.point_candidates_.candidates_.erase(it);
+            overlap_kfs.push_back(pair<FramePtr,size_t>(ref_frame,0));
-          continue;
+
+            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_projected_kf_id_ == frame->id_)
+                    continue;
+                (*it_ftr)->point->last_projected_kf_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;
         }
-      }
-      ++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 - First reimplementation
     void Reprojector::reprojectMap1(
@@ -214,11 +222,6 @@ void Reprojector::reprojectMap(
         extractor->compute(frame->img(), keypoints_cur, descriptors_cur);
         list< pair<FramePtr,double> > close_kfs;
         map_.getCloseKeyframes(frame, close_kfs);
-<<<<<<< HEAD
-
-        // Sort KFs with overlap according to their closeness
-=======
->>>>>>> internal/dev2
         close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
                        boost::bind(&std::pair<FramePtr, double>::second, _2));
         size_t n = 0;
@@ -228,20 +231,14 @@ void Reprojector::reprojectMap(
         {
             std::vector<cv::KeyPoint> keypoints_kfs;
             FramePtr ref_frame = it_frame->first;
-<<<<<<< HEAD
-            // Try to reproject each mappoint that the other KF observes
-=======
             overlap_kfs.push_back(pair<FramePtr,size_t>(ref_frame,0));
->>>>>>> internal/dev2
+
             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;
-<<<<<<< HEAD
+
-                // make sure we project a point only once
-=======
->>>>>>> internal/dev2
                 if((*it_ftr)->point->last_projected_kf_id_ == frame->id_)
                     continue;
                 (*it_ftr)->point->last_projected_kf_id_ = frame->id_;
@@ -250,26 +247,7 @@ void Reprojector::reprojectMap(
                 kp->pt.y = (*it_ftr)->px.y();
                 keypoints_kfs.push_back(*kp);
             }
-<<<<<<< HEAD
-            keypoints_ref.push_back(keypoints_kfs);
-        }
-        //SVO_STOP_TIMER("reproject_kfs");
-
-        cv::Mat descriptors_ref, descriptors_cur;
-        std::vector<cv::KeyPoint> keypoints_cur;
-        cv::FAST(frame->img(),keypoints_cur,50);
-        std::vector<cv::DMatch> matches;
-        cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 50.0f, 4);
-        cv::Ptr<cv::BFMatcher> matcher = cv::BFMatcher::create(cv::NORM_HAMMING, true);
-        int ii = 0;
-        extractor->compute(frame->img(), keypoints_cur, descriptors_cur);
-        for(auto it_frame = close_kfs.begin(), ite_frame = close_kfs.end();
-            it_frame != ite_frame; ++it_frame, ++ii)
-        {
-            extractor->compute(it_frame->first->img(), keypoints_ref[ii], descriptors_ref);
-=======
             extractor->compute(it_frame->first->img(), keypoints_kfs, descriptors_ref);
->>>>>>> internal/dev2
             matcher->match(descriptors_ref, descriptors_cur, matches);
             //std::cerr<<keypoints_cur.size()<<"\t"<<descriptors_cur.size<<"\t"<<keypoints_kfs.size()<<"\t"<<descriptors_ref.size<<"\t"<<matches.size()<<std::endl;
             for (int i; i < matches.size(); i++){
@@ -281,11 +259,6 @@ void Reprojector::reprojectMap(
                 if (reprojectPoint1(frame, (*fts2)->point, keypoints_cur[i].pt.x, keypoints_cur[i].pt.y)) {
                     overlap_kfs.back().second++;
                 }
-<<<<<<< HEAD
-                //keypoints_cur[i].class_id = 0;
-
-=======
->>>>>>> internal/dev2
             }
         }
         boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);