latest working version

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/include/vio/point.h

@@ -54,7 +54,7 @@ public:
   size_t                      n_obs_;                   //!< Number of obervations: Keyframes AND successful reprojections in intermediate frames.
   g2oPoint*                   v_pt_;                    //!< Temporary pointer to the point-vertex in g2o during bundle adjustment.
   int                         last_published_ts_;       //!< Timestamp of last publishing.
-  int                         last_projected_kf_id_;    //!< Flag for the reprojection: don't reproject a pt twice.
+  int                         last_frame_overlap_id_;    //!< Flag for the reprojection: don't reproject a pt twice.
   PointType                   type_;                    //!< Quality of the point.
   int                         n_failed_reproj_;         //!< Number of failed reprojections. Used to assess the quality of the point.
   int                         n_succeeded_reproj_;      //!< Number of succeeded reprojections. Used to assess the quality of the point.

GPU_version/vio/include/vio/ukf.h

@@ -125,20 +125,15 @@ public:
         filter_->predict(x,y,pitch,1e-9*time.toNSec());
         lock=false;
     };
-    std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> UpdateSvo(double y/*in camera frame*/,double z/*in camera frame*/,double roll/*in camera frame*/,size_t match,ros::Time& time) {
+    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*/,size_t match,ros::Time& time) {
         while(lock)usleep(5);
         lock=true;
-        roll=roll+M_PI;
+        //if(filter_->predict_up)filter_->correct(z,x,pitch,match,1e-9*time.toNSec());
-        if(filter_->predict_up)filter_->correct(z,y,roll,match,1e-9*time.toNSec());
         lock=false;
-        double roll_f;
+        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(1),filter_->state_(0),filter_->state_(2)));
-        roll_f=filter_->state_(2)+M_PI;
-        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(1),-filter_->state_(0),roll_f));
     };
     std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> get_location(){
-        double pitch_f;
+        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(1),filter_->state_(0),filter_->state_(2)));
-        pitch_f=filter_->state_(2)+M_PI;
-        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(1),-filter_->state_(0),pitch_f));
     }
 private:
      Base* filter_= nullptr;

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

@@ -220,7 +220,7 @@ namespace vio {
             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())
+            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

@@ -168,8 +168,8 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   // 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_);
+  //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;
+  //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_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_);

GPU_version/vio/src/point.cpp

@@ -31,7 +31,7 @@ Point::Point(const Vector3d& pos) :
   n_obs_(0),
   v_pt_(NULL),
   last_published_ts_(0),
-  last_projected_kf_id_(-1),
+  last_frame_overlap_id_(-10),
   type_(TYPE_UNKNOWN),
   n_failed_reproj_(0),
   n_succeeded_reproj_(0),
@@ -45,7 +45,7 @@ Point::Point(const Vector3d& pos, Feature* ftr) :
   n_obs_(1),
   v_pt_(NULL),
   last_published_ts_(0),
-  last_projected_kf_id_(-1),
+  last_frame_overlap_id_(-10),
   type_(TYPE_UNKNOWN),
   n_failed_reproj_(0),
   n_succeeded_reproj_(0),

GPU_version/vio/src/reprojector.cpp

@@ -92,9 +92,9 @@ void Reprojector::reprojectMap(
             {
                 if((*it_ftr)->point == NULL)
                     continue;
-                if((*it_ftr)->point->last_projected_kf_id_ == frame->id_)
+                if((*it_ftr)->point->last_frame_overlap_id_ == frame->id_)
                     continue;
-                (*it_ftr)->point->last_projected_kf_id_ = frame->id_;
+                (*it_ftr)->point->last_frame_overlap_id_= frame->id_;
                 if(reprojectPoint(frame, (*it_ftr)->point)){
                     overlap_kfs.back().second++;
                 }
@@ -240,15 +240,15 @@ void Reprojector::reprojectMap(
             extractor->compute(it_frame.item.first->img(), keypoints_kfs, descriptors_ref);
             matcher->match(descriptors_ref, descriptors_cur, matches);
             for(auto&& f: _for(it_frame.item.first->fts_)){
-               // if(f.item->point->last_projected_kf_id_ == it_frame.item.first->id_ || f.index > matches.size()-1)continue;
+                if(f.item->point==NULL)f.item->point=new Point(it_frame.item.first->cam_->cam2world(f.item->px.x(),f.item->px.y()));
-               // f.item->point->last_projected_kf_id_ = it_frame.item.first->id_;
+                if(f.item->point->last_frame_overlap_id_ == frame->id_)continue;
+                f.item->point->last_frame_overlap_id_ = frame->id_;
                 for(auto&& match:matches){
                     if(match.queryIdx!=f.index)continue;
                     assert(keypoints_cur.size()>match.trainIdx);
                     const int k = static_cast<int>(keypoints_cur.at(match.trainIdx).pt.y / grid_.cell_size) * grid_.grid_n_cols
                                   + static_cast<int>(keypoints_cur.at(match.trainIdx).pt.x / grid_.cell_size);
                     assert(grid_.cells.at(k)!= nullptr);
-                    if(f.item->point==NULL)continue;
                     Vector2d px((int)keypoints_cur.at(match.trainIdx).pt.x,(int)keypoints_cur.at(match.trainIdx).pt.y);
                     grid_.cells.at(k)->push_back(Candidate(f.item->point,px));
                     overlap_kfs.back().second++;
@@ -256,22 +256,6 @@ void Reprojector::reprojectMap(
                 }
             }
         }
-        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))