From bfecc6bda7bbb45d7379b5de7931147146f1686a Mon Sep 17 00:00:00 2001
From: gfuse <majidgeravand68@gmail.com>
Date: Tue, 12 Oct 2021 18:29:07 +0200
Subject: [PATCH] debug some issues

---
 GPU_version/vio/launch/Px30.launch         |  2 +-
 GPU_version/vio/param/vo_fast.yaml         |  4 ++--
 GPU_version/vio/src/frame_handler_mono.cpp |  2 +-
 GPU_version/vio/src/pose_optimizer.cpp     | 16 +++++++++++-----
 4 files changed, 15 insertions(+), 9 deletions(-)

diff --git a/GPU_version/vio/launch/Px30.launch b/GPU_version/vio/launch/Px30.launch
index d12f08d..7c4cbdf 100644
--- a/GPU_version/vio/launch/Px30.launch
+++ b/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" />
diff --git a/GPU_version/vio/param/vo_fast.yaml b/GPU_version/vio/param/vo_fast.yaml
index 0a9d279..d28e8fe 100644
--- a/GPU_version/vio/param/vo_fast.yaml
+++ b/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.5              #VO noise  in EKF
+  VO_ekf_orientation: 0.1              #VO noise  in EKF
   CMD_ekf_translation: 0.1  #control vector noise in EKF
-  CMD_ekf_orientation: 0.1  #control vector noise in EKF
+  CMD_ekf_orientation: 0.01  #control vector noise in EKF
diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp
index 365c550..7143cd9 100644
--- a/GPU_version/vio/src/frame_handler_mono.cpp
+++ b/GPU_version/vio/src/frame_handler_mono.cpp
@@ -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;
 }
 
diff --git a/GPU_version/vio/src/pose_optimizer.cpp b/GPU_version/vio/src/pose_optimizer.cpp
index fde83f4..8ea3fa5 100644
--- a/GPU_version/vio/src/pose_optimizer.cpp
+++ b/GPU_version/vio/src/pose_optimizer.cpp
@@ -48,14 +48,18 @@ void optimizeGaussNewton(
   std::vector<float> errors;
   for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/)
   {
-    if((*it)->point == NULL)
-      continue;
+      if((*it)->point == NULL || (*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0.) {
+          map.safeDeletePoint((*it)->point);
+          it = frame->fts_.erase(it);
+          continue;
+      }
     //Reprojection error
     Vector2d e = vk::project2d((*it)->f)
                - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
     if(std::isnan(e.norm())){
         map.safeDeletePoint((*it)->point);
         it = frame->fts_.erase(it);
+        continue;
     }
     e *= 1.0 / (1<<(*it)->level);
     chi2_vec_init.push_back(e.norm()); // just for debug
@@ -82,7 +86,7 @@ void optimizeGaussNewton(
     // compute residual
     for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
     {
-      if((*it)->point == NULL)
+      if((*it)->point == NULL || (*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0.)
         continue;
       Matrix23d J;
       Vector3d xyz_f(Vector3d(frame->se3()*(*it)->point->pos_));
@@ -121,8 +125,10 @@ void optimizeGaussNewton(
   size_t n_deleted_refs = 0;
   for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/)
   {
-    if((*it)->point == NULL) {
-        it++;
+    if((*it)->point == NULL || (*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0.) {
+        map.safeDeletePoint((*it)->point);
+        it = frame->fts_.erase(it);
+        ++n_deleted_refs;
         continue;
     }
     Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));