add landmark type

GPU_version/vio/launch/Px30.launch

@@ -1,6 +1,6 @@
 <launch>
 
-  <!-- node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" / -->
+  <node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" />
 
     <!-- Camera topic to subscribe to -->
     <param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />

GPU_version/vio/param/vo_fast.yaml

@@ -13,6 +13,6 @@ vio:
   ACC_ekf: 0.2          #acc white noise in continuous in EKF
   GYO_ekf: 0.5         #gyro white noise in continuous in EKF
   VO_ekf_translation: 1.0              #VO noise  in EKF
-  VO_ekf_orientation: 0.1              #VO noise  in EKF
+  VO_ekf_orientation: 0.5              #VO noise  in EKF
   CMD_ekf_translation: 0.1  #control vector noise in EKF
   CMD_ekf_orientation: 0.01  #control vector noise in EKF

GPU_version/vio/src/frame_handler_mono.cpp

@@ -181,7 +181,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
 #endif
   double depth_mean=0.0, depth_min=0.0;
   frame_utils::getSceneDepth(new_frame_, depth_mean, depth_min);
-  if(depth_mean>10.0)return RESULT_NO_KEYFRAME;
+  if(depth_mean>20.0)return RESULT_NO_KEYFRAME;
 
   optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
   // select keyframe
@@ -215,7 +215,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   // add keyframe to map
   map_.addKeyframe(new_frame_);
   map_.checkKeyFrames();
-  //ba_glob_->new_key_frame();
+  ba_glob_->new_key_frame();
   return RESULT_IS_KEYFRAME;
 }
 

GPU_version/vio/src/global_optimizer.cpp

@@ -110,6 +110,7 @@ namespace vio {
                 for(auto&& it_ftr:(*it_kf)->fts_)
                 {
                     if(it_ftr->point==NULL)continue;
+                    if(it_ftr->point->type_ != vio::Point::TYPE_GOOD)continue;
                     // for each keyframe add edges to all observed mapoints
                     std::shared_ptr<Point> mp = it_ftr->point;
                     if(mp->pos_.hasNaN())continue;

GPU_version/vio/src/initialization.cpp

@@ -102,6 +102,7 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
                                                                          frame_cur->c2f(px_cur_.at(f.index).x,px_cur_.at(f.index).y), f.item->score,f.item->level,f.item->descriptor);
               frame_cur->addFeature(ftr_cur);
               new_point->addFrameRef(ftr_cur);
+              new_point->type_=vio::Point::TYPE_GOOD;
 
               std::shared_ptr<Feature> ftr_ref=std::make_shared<Feature>(frame_ref_, new_point, f.item->px, f.item->f, f.item->score,f.item->level,f.item->descriptor);
               frame_ref_->addFeature(ftr_ref);

GPU_version/vio/src/point.cpp

@@ -166,11 +166,11 @@ void Point::optimize(const size_t n_iter)
   for(auto it=obs_.begin(); it!=obs_.end(); ++it) {
             Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d((*it)->frame->se3() * pos_));
             e /= (1<<(*it)->level);
-            if (e.norm() > 3.0/ (*it)->frame->cam_->errorMultiplier2())
+            if (e.norm() > 4.0/ (*it)->frame->cam_->errorMultiplier2())
                 n_failed_reproj_++;
   }
-
+  if(n_failed_reproj_<0.75*obs_.size())
-
+      type_=TYPE_GOOD;
 }
 
 } // namespace vio

GPU_version/vio/src/pose_optimizer.cpp

@@ -48,11 +48,20 @@ void optimizeGaussNewton(
   std::vector<float> errors;
   for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/)
   {
-      if((*it)->point == NULL || (*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0.) {
+      if((*it)->point == NULL) {
           map.safeDeletePoint((*it)->point);
           it = frame->fts_.erase(it);
           continue;
       }
+      if((*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0.){
+          map.safeDeletePoint((*it)->point);
+          it = frame->fts_.erase(it);
+          continue;
+      }
+      if((*it)->point->type_==vio::Point::TYPE_UNKNOWN){
+          it++;
+          continue;
+      }
     //Reprojection error
     Vector2d e = vk::project2d((*it)->f)
                - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
@@ -86,7 +95,8 @@ void optimizeGaussNewton(
     // compute residual
     for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
     {
-      if((*it)->point == NULL || (*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0.)
+      if((*it)->point == NULL)continue;
+      if((*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0. || (*it)->point->type_==vio::Point::TYPE_UNKNOWN)
         continue;
       Matrix23d J;
       Vector3d xyz_f(Vector3d(frame->se3()*(*it)->point->pos_));
@@ -125,12 +135,17 @@ void optimizeGaussNewton(
   size_t n_deleted_refs = 0;
   for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/)
   {
-    if((*it)->point == NULL || (*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0.) {
+    if((*it)->point == NULL) {
         map.safeDeletePoint((*it)->point);
         it = frame->fts_.erase(it);
         ++n_deleted_refs;
         continue;
     }
+    if((*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0.){
+          map.safeDeletePoint((*it)->point);
+          it = frame->fts_.erase(it);
+          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.norm());
@@ -139,9 +154,11 @@ void optimizeGaussNewton(
       map.safeDeletePoint((*it)->point);
       it = frame->fts_.erase(it);
       ++n_deleted_refs;
-    }
+    }else{
-    else
+        (*it)->point->type_=vio::Point::TYPE_CANDIDATE;
         it++;
+    }
+
   }
   //point_mut_.unlock();
   num_obs -= n_deleted_refs;

GPU_version/vio/src/reprojector.cpp

@@ -152,16 +152,17 @@ namespace vio {
                         new_point->addFrameRef(frame->fts_.back());
                         added_keypoints.push_back(match.trainIdx);
                         frame->fts_.back()->point->last_frame_overlap_id_=it_frame.item.first->id_;
+                        frame->fts_.back()->point->type_=vio::Point::TYPE_UNKNOWN;
                         grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px));
                         overlap_kfs.back().second++;
                         ++points_count;
                     }else{
                         if(!matcher_.findMatchDirect(*(*it_ref)->point, *frame, px))continue;
-
                         (*it_ref)->point->last_frame_overlap_id_ = frame->id_;
                         frame->addFeature(std::make_shared<Feature>(frame,
                                                                     (*it_ref)->point,
                                                                     px,(*it_cur)->level,(*it_cur)->score,(*it_cur)->descriptor));
+                        (*it_ref)->point->type_=vio::Point::TYPE_CANDIDATE;
                         added_keypoints.push_back(match.trainIdx);
                         grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px));
                         overlap_kfs.back().second++;