local m: working version 3

GPU_version/vio/include/vio/frame.h

@@ -128,7 +128,7 @@ namespace vio {
             //TODO add 15 degree roll orientation
             SE2 inv=T_f_w_.inverse();
             Quaterniond q;
-            q = AngleAxisd(0.261799, Vector3d::UnitX())
+            q = AngleAxisd(-0.261799, Vector3d::UnitX())
                 * AngleAxisd(atan2(inv.so2().unit_complex().imag(),inv.so2().unit_complex().real()), Vector3d::UnitY())
                 * AngleAxisd(0.0, Vector3d::UnitZ());
             SE3 out(q.toRotationMatrix(),Vector3d(inv.translation()(0), 0.0,inv.translation()(1)));

GPU_version/vio/include/vio/global.h

@@ -92,7 +92,7 @@ namespace vio
         SE3 se3() const{
             //Todo add 15 roll orientation
             Quaterniond q;
-            q = AngleAxisd(0.261799, Vector3d::UnitX())
+            q = AngleAxisd(-0.261799, Vector3d::UnitX())
                 * AngleAxisd(pitch(), Vector3d::UnitY())
                 * AngleAxisd(0.0, Vector3d::UnitZ());
             return SE3(q.toRotationMatrix(),Vector3d(T2_.translation()(0), 0.0,T2_.translation()(1)));

GPU_version/vio/kernel/compute-residual.cl

@@ -21,7 +21,7 @@ float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature)
     float3 error=ref+cur;
     float yaw=0.0;
     float pitch=error.z;
-    float roll=0.26;
+    float roll=-0.26;
     float R00=cos(yaw)*cos(pitch);//cos(pitch)
     float R01=cos(yaw)*sin(pitch)*sin(roll)-sin(yaw)*cos(roll);//0
     float R02=cos(yaw)*sin(pitch)*cos(roll)+sin(yaw)*sin(roll);//sin(pitch)

GPU_version/vio/src/depth_filter.cpp

@@ -218,7 +218,7 @@ namespace vio {
             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());
             ///TODO add 15 degrees roll orientation
             Quaterniond q;
-            q = AngleAxisd(0.261799, Vector3d::UnitX())
+            q = AngleAxisd(-0.261799, Vector3d::UnitX())
                 * AngleAxisd(atan2(T.so2().unit_complex().imag(),T.so2().unit_complex().real()), Vector3d::UnitY())
                 * AngleAxisd(0.0, Vector3d::UnitZ());
             SE3 T_ref_cur(q.toRotationMatrix(),Vector3d(T.translation()(0), 0.0,T.translation()(1)));

GPU_version/vio/src/frame_handler_mono.cpp

@@ -94,7 +94,7 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const
   overlap_kfs_.clear();
 
   // create new frame
-  new_frame_.reset(new Frame(cam_, img.clone(), timestamp));
+  new_frame_=boost::make_shared<Frame>(cam_, img.clone(), timestamp);
   time_=time;
   // process frame
   UpdateResult res = RESULT_FAILURE;
@@ -107,7 +107,6 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const
 
   // set last frame
   if(!new_frame_->fts_.empty() || stage_ != STAGE_DEFAULT_FRAME)last_frame_ = new_frame_;
-  new_frame_.reset();
   // finish processing
   finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs());
   if(stage_ == STAGE_RELOCALIZING){
@@ -168,11 +167,12 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   new_frame_->Cov_ = init_f.first;
   // sparse image align
   //SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlign::LevenbergMarquardt, false, false);
+  //assert(new_frame_.get()== nullptr);
   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_.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_);
+    reprojector_.reprojectMap2(new_frame_, overlap_kfs_);
   //assert (false);
 
   std::cout<<"Reprojection Map nPoint: "<<overlap_kfs_.back().second

GPU_version/vio/src/map.cpp

@@ -112,6 +112,7 @@ void Map::getCloseKeyframes(
   for(auto kf : keyframes_)
   {
     // check if kf has overlaping field of view with frame, use therefore KeyPoints
+    std::cerr<<kf->key_pts_.size()<<'\t';
     for(auto keypoint : kf->key_pts_)
     {
       if(keypoint == nullptr)

GPU_version/vio/src/reprojector.cpp

@@ -71,18 +71,11 @@ void Reprojector::reprojectMap(
     std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs)
 {
   resetGrid();
-
-  // Identify those Keyframes which share a common field of view.
-  //  SVO_START_TIMER("reproject_kfs");
   list< pair<FramePtr,double> > close_kfs;
+  //assert(frame== nullptr);
   map_.getCloseKeyframes(frame, close_kfs);
-
-  // Sort KFs with overlap according to their closeness
   close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
                  boost::bind(&std::pair<FramePtr, double>::second, _2));
-
-  // Reproject all mappoints of the closest N kfs with overlap. We only store
-  // in which grid cell the points fall.
   size_t n = 0;
   overlap_kfs.reserve(options_.max_n_kfs);
   for(auto it_frame=close_kfs.begin(), ite_frame=close_kfs.end();
@@ -90,16 +83,11 @@ void Reprojector::reprojectMap(
   {
     FramePtr ref_frame = it_frame->first;
     overlap_kfs.push_back(pair<FramePtr,size_t>(ref_frame,0));
-
-    // Try to reproject each mappoint that the other KF observes
     for(auto it_ftr=ref_frame->fts_.begin(), ite_ftr=ref_frame->fts_.end();
         it_ftr!=ite_ftr; ++it_ftr)
     {
-      // check if the feature has a mappoint assigned
       if((*it_ftr)->point == NULL)
         continue;
-
-      // make sure we project a point only once
       if((*it_ftr)->point->last_projected_kf_id_ == frame->id_)
         continue;
       (*it_ftr)->point->last_projected_kf_id_ = frame->id_;
@@ -108,11 +96,6 @@ void Reprojector::reprojectMap(
       }
     }
   }
-  //SVO_STOP_TIMER("reproject_kfs");
-
-  // Now project all point candidates
- // SVO_START_TIMER("reproject_candidates");
-  {
     boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
     auto it=map_.point_candidates_.candidates_.begin();
     while(it!=map_.point_candidates_.candidates_.end())
@@ -129,21 +112,14 @@ void Reprojector::reprojectMap(
       }
       ++it;
     }
-  } // unlock the mutex when out of scope
-  //SVO_STOP_TIMER("reproject_candidates");
-  // Now we go through each grid cell and select one point to match.
-  // At the end, we should have at maximum one reprojected point per cell.
-  //SVO_START_TIMER("feature_align");
   for(size_t i=0; i<grid_.cells.size(); ++i)
   {
-    // we prefer good quality points over unkown quality (more likely to match)
+
-    // and unknown quality over candidates (position not optimized)
     if(reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame))
       ++n_matches_;
     if(n_matches_ > (size_t) Config::maxFts())
       break;
   }
-// SVO_STOP_TIMER("feature_align");
 }
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 // TODO - First reimplementation
@@ -373,7 +349,7 @@ bool Reprojector::reprojectCell(Cell& cell, FramePtr frame)
       it = cell.erase(it);
       continue;
     }
-    /*bool found_match = true;
+    bool found_match = true;
     if(options_.find_match_direct)
       found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px);
     if(!found_match)
@@ -385,7 +361,7 @@ bool Reprojector::reprojectCell(Cell& cell, FramePtr frame)
         map_.point_candidates_.deleteCandidatePoint(it->pt);
       it = cell.erase(it);
       continue;
-    }*/
+    }
     it->pt->n_succeeded_reproj_++;
     if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10)
       it->pt->type_ = Point::TYPE_GOOD;