From 5b4130f5d0f3ad04b2005f2c2b813aab537b6d1e Mon Sep 17 00:00:00 2001
From: VahdPourAmin <v.pouramin@gmail.com>
Date: Sun, 12 Sep 2021 21:53:30 +0200
Subject: [PATCH] Fixed some bugs and got better results

---
 GPU_version/vio/src/pose_optimizer.cpp | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/GPU_version/vio/src/pose_optimizer.cpp b/GPU_version/vio/src/pose_optimizer.cpp
index fa2fe60..a550133 100644
--- a/GPU_version/vio/src/pose_optimizer.cpp
+++ b/GPU_version/vio/src/pose_optimizer.cpp
@@ -63,9 +63,9 @@ void optimizeGaussNewton(
     return;
   vk::robust_cost::MADScaleEstimator scale_estimator;
   estimated_scale = 0.5*scale_estimator.compute(errors);
-#if VIO_DEBUG
+/*#if VIO_DEBUG
     fprintf(log,"[%s] Init estimate scale:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),estimated_scale);
-#endif
+#endif*/
 
   num_obs = errors.size();
   chi2_vec_init.reserve(num_obs);
@@ -111,10 +111,10 @@ void optimizeGaussNewton(
     // check if error increased
     if((iter > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dT[0]))
     {
-#if VIO_DEBUG
+/*#if VIO_DEBUG
           fprintf(log,"[%s] it:%d \t FAILURE \t new_chi2: %f\n",
                   vio::time_in_HH_MM_SS_MMM().c_str(),iter,new_chi2);
-#endif
+#endif*/
       frame->T_f_w_ = T_old; // roll-back
       break;
     }
@@ -122,13 +122,13 @@ void optimizeGaussNewton(
     dT=0.1*dT;
     // update the model
     T_old = frame->T_f_w_;
-    frame->T_f_w_=SE2_5(T_old.se2().translation().y()+dT.x(),T_old.se2().translation().y()+dT.y(),T_old.pitch()+dT.z());
+    frame->T_f_w_=SE2_5(T_old.se2().translation().x()+dT.x(),T_old.se2().translation().y()+dT.y(),T_old.pitch()+dT.z());
 
     chi2 = new_chi2;
-#if VIO_DEBUG
+/*#if VIO_DEBUG
       fprintf(log,"[%s] it:%d \t Success \t new_chi2: %f \t dT = %f, %f, %f\n",
               vio::time_in_HH_MM_SS_MMM().c_str(),iter,new_chi2,dT.x(),dT.y(),dT.z());
-#endif
+#endif*/
 
     // stop when converged
     if(vk::norm_max(dT) <= EPS)