debug some issues

GPU_version/vio/param/vo_fast.yaml

@@ -1,6 +1,6 @@
 vio:
   grid_size: 8            #Feature grid size of a cell in [px].
-  max_n_kfs: 20            #Limit the number of keyframes in the map. This makes nslam essentially. a Visual Odometry. Set to 0 if unlimited number of keyframes are allowed.  Minimum number of keyframes is 3.
+  max_n_kfs: 200            #Limit the number of keyframes in the map. This makes nslam essentially. a Visual Odometry. Set to 0 if unlimited number of keyframes are allowed.  Minimum number of keyframes is 3.
   loba_num_iter: 10         #Number of iterations in the local bundle adjustment.
   quality_min_fts: 10      #If the number of tracked features drops below this threshold. Tracking quality is bad.
   quality_max_drop_fts: 90 #If within one frame, this amount of features are dropped. Tracking quality is bad.
@@ -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: 1.5              #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
+  CMD_ekf_orientation: 0.1  #control vector noise in EKF

GPU_version/vio/src/frame_handler_base.cpp

@@ -100,7 +100,11 @@ void FrameHandlerBase::optimizeStructure(
 {
   deque<std::shared_ptr<Point>> pts;
   for(auto&& it:frame->fts_){
-      if(it->point!=NULL)pts.push_back(it->point);
+      if(it->point!=NULL && !it->point->pos_.hasNaN() && it->point->pos_.norm() !=0.){
+          pts.push_back(it->point);
+      }else{
+          map_.safeDeletePoint(it->point);
+      }
   }
   //max_n_pts = min(max_n_pts, pts.size());
   //nth_element(pts.begin(), pts.begin() + max_n_pts, pts.end(), ptLastOptimComparator);

GPU_version/vio/src/frame_handler_mono.cpp

@@ -199,7 +199,11 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
 #endif
   // new keyframe selected
   for(auto&& it:new_frame_->fts_){
-      if(it->point != NULL)it->point->addFrameRef(it);
+      if(it->point != NULL && !it->point->pos_.hasNaN() && it->point->pos_.norm() !=0.){
+          it->point->addFrameRef(it);
+      }else{
+          map_.safeDeletePoint(it->point);
+      }
   }
 
   // if limited number of keyframes, remove the one furthest apart

GPU_version/vio/src/global_optimizer.cpp

@@ -87,9 +87,9 @@ namespace vio {
             g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
 
             optimizer.setAlgorithm(solver);
-
+            double* pam=map_.keyframes_.back()->cam_->params();
             // setup camera
-            g2o::CameraParameters* cam_params = new g2o::CameraParameters(1.0, Vector2d(0.,0.), 0.);
+            g2o::CameraParameters* cam_params = new g2o::CameraParameters(0.5*(pam[0]+pam[1]), Vector2d(pam[2],pam[3]), 0.);
             cam_params->setId(0);
             if (!optimizer.addParameter(cam_params)) {
                 assert(false);
@@ -113,6 +113,7 @@ namespace vio {
                     // for each keyframe add edges to all observed mapoints
                     std::shared_ptr<Point> mp = it_ftr->point;
                     if(mp->pos_.hasNaN())continue;
+                    if(mp->pos_.norm()==0.)continue;
                     g2o::VertexSBAPointXYZ* v_mp = mp->v_pt_;
                     if(v_mp == NULL)
                     {
@@ -124,12 +125,11 @@ namespace vio {
 
                     // Due to merging of mappoints it is possible that references in kfs suddenly
                     // have a very large reprojection error which may result in distorted results.
-                    Vector2d error = vk::project2d(it_ftr->f) - vk::project2d((*it_kf)->w2f(mp->pos_));
+                 /*   Vector2d error = vk::project2d(it_ftr->f) - vk::project2d((*it_kf)->w2f(mp->pos_));
                     if(error.squaredNorm() > pow(2,Config::poseOptimThresh()/(*it_kf)->cam_->errorMultiplier2()))
                         incorrect_edges.push_back(pair<FramePtr,std::shared_ptr<Feature>>(*it_kf, it_ftr));
                     else
-                    {
+                    {*/
-                        g2o::Edge_XYZ_VSC e1;
                         g2o::EdgeProjectXYZ2UV* e = createG2oEdgeSE3(v_kf, v_mp, vk::project2d(it_ftr->f),
                                                                          true,
                                                                          Config::poseOptimThresh()/(*it_kf)->cam_->errorMultiplier2()*Config::lobaRobustHuberWidth());
@@ -139,7 +139,7 @@ namespace vio {
                         edge.feature=it_ftr;
                         edges.push_back(edge);
                         optimizer.addEdge(e);
-                    }
+                    /*}*/
                 }
             }
             if(optimizer.vertices().size()<1)continue;
@@ -242,10 +242,11 @@ namespace vio {
        e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_point));
        e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_frame));
        e->setMeasurement(f_up);
-       e->information() = Eigen::Matrix2d::Identity();
+       e->information() = weight*Eigen::Matrix2d::Identity();
        g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
        rk->setDelta(huber_width);
        e->setRobustKernel(rk);
+
        e->setParameterId(0, 0); //old: e->setId(v_point->id());
        return e;
    }

GPU_version/vio/src/reprojector.cpp

@@ -140,7 +140,7 @@ namespace vio {
                         // pose with respect to reference frame
                         Vector3d pos=vk::triangulateFeatureNonLin(T_ref_cur.rotation_matrix(),T_ref_cur.translation(),
                                                                   (*it_ref)->f,frame->c2f(px));
-                        if(pos.norm()==0.0)continue;
+                        if(pos.norm()==0.)continue;
                         // point in world frame
                         std::shared_ptr<Point> new_point=std::make_shared<Point>(it_frame.item.first->se3()*pos,(*it_ref));
 
@@ -170,7 +170,7 @@ namespace vio {
                 }
                 ++it_ref;
             }
-            if(points_count>300)return;
+            //if(points_count>300)return;
         }
 /*        std::cerr<<"here\n";
         exit(0);*/

GPU_version/vio/src/sparse_img_align_gpu.cpp

@@ -43,6 +43,8 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame, FILE* log)
   feature_counter_ = 0; // is used to compute the index of the cached jacobian
   for(auto it=ref_frame->fts_.begin();it!=ref_frame->fts_.end();++it){
         if((*it)->point == nullptr)continue;
+        if((*it)->point->pos_.hasNaN())continue;
+        if((*it)->point->pos_.norm()==0.)continue;
         Vector3d xyz_ref=(*it)->f*(ref_frame->se3().inverse()*(*it)->point->pos_).norm();
         features[feature_counter_].x=xyz_ref(0);
         features[feature_counter_].y=xyz_ref(1);