From cb1d2b4e817959dfca5f5208a24c438e4b7dd3d3 Mon Sep 17 00:00:00 2001
From: Ivan <mascomen4@gmail.com>
Date: Fri, 15 Apr 2022 15:09:37 +0300
Subject: [PATCH] v1

---
 src/vio.cpp | 59 ++++++++++++++++++++++++++++++-----------------------
 1 file changed, 34 insertions(+), 25 deletions(-)

diff --git a/src/vio.cpp b/src/vio.cpp
index bfbf1c2..bfaca5a 100644
--- a/src/vio.cpp
+++ b/src/vio.cpp
@@ -831,7 +831,8 @@ int main(int argc, char** argv) {
 
       // Appended.
       glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
-      ar_view.Activate(ar_3d_camera);
+//      ar_view.Activate(ar_3d_camera);
+      ar_view.Activate();
 
       glColor3f(1.0, 1.0, 1.0);
       // Get the last image pose. (probably, it's T_w_i
@@ -845,6 +846,7 @@ int main(int argc, char** argv) {
         T_w_i = it_ar->second->frames.back();
       }
       auto T_i_c = calib.T_i_c[0];
+
       // Draw OpenCV cube
       if (IfDrawOpenCVCube){
         DrawOpenCVCube(img_cv_data[2], (T_w_i * T_i_c), K, distCoeffs);
@@ -854,6 +856,11 @@ int main(int argc, char** argv) {
         DrawImageTexture(imageTexture, img_cv_data[2]);
       }
 
+      // Clear the buffer before drawing. Probably, the cause is in the DrawImageTexture.
+      // It kind of overrides the view, and doesn't let to draw the cubes. Probably,
+      // DrawImageTexture takes also the depth buffer when uploading, so we need clean it.
+      glClear(GL_DEPTH_BUFFER_BIT);
+
       // TODO: Can I define it outside of the loop? There's no need I guess to
       //  redefine the the projection matrix every time
       glMatrixMode(GL_PROJECTION);
@@ -862,6 +869,9 @@ int main(int argc, char** argv) {
       // drawLinesCube pushes the matrices, therefore, I need to define in which
       // stack to push that matrices.
       glMatrixMode(GL_MODELVIEW);
+//      ar_3d_camera.Follow((T_w_i * T_i_c).matrix());
+
+      glPushMatrix();
       if (zeroOut){
         xSkew = 0;
         ySkew = 0;
@@ -882,8 +892,7 @@ int main(int argc, char** argv) {
       if (showCube6) {drawLinesCube(32.8, 4.8, -324.5, cubeSize);}
       if (showCube7) {drawLinesCube(32.5, 4.8, -345.7, cubeSize);}
 
-      ar_3d_camera.Follow((T_w_i * T_i_c).matrix());
-//      glClear(GL_DEPTH_BUFFER_BIT);
+      glPopMatrix();
 
       // Ok, so here by the moment we are watching from the camera position and orientation.
       // Drawing the cube from here might result in drawing the cube in the camera coordinate system.
@@ -986,8 +995,6 @@ int main(int argc, char** argv) {
       //        record = false;
       //      }
 
-      pangolin::FinishFrame();
-
       if (continue_btn) {
         if (!next_step())
           std::this_thread::sleep_for(std::chrono::milliseconds(50));
@@ -1006,6 +1013,7 @@ int main(int argc, char** argv) {
           continue_fast = false;
         }
       }
+      pangolin::FinishFrame();
     }
 
     // If GUI closed but VIO not yet finished --> abort input queues, which in
@@ -1271,7 +1279,8 @@ void draw_scene(pangolin::View& view) {
 
 void draw_scene_no_camera(pangolin::View& view) {
   UNUSED(view);
-  view.Activate(ar_3d_camera);
+//  view.Activate(ar_3d_camera);
+//  view.Activate();
 //  auto T_w_i = vio_T_w_i[show_frame];
 //  auto T_i_c = calib.T_i_c[0];
 
@@ -1330,24 +1339,24 @@ void draw_scene_no_camera(pangolin::View& view) {
 
 //  LoadCameraPose((T_w_i * T_i_c).matrix());
 
-  glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
-
-  glPointSize(3);
-  glColor3f(1.0, 0.0, 0.0);
-  glEnable(GL_BLEND);
-  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
-
-  glColor3ubv(cam_color);
-
-  if (!vio_t_w_i.empty()) {
-    size_t end = std::min(vio_t_w_i.size(), size_t(show_frame + 1));
-    Eigen::aligned_vector<Eigen::Vector3d> sub_gt(vio_t_w_i.begin(),
-                                                  vio_t_w_i.begin() + end);
-    pangolin::glDrawLineStrip(sub_gt);
-  }
-
-  glColor3ubv(gt_color);
-  if (show_gt) pangolin::glDrawLineStrip(gt_t_w_i);
+//  glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
+//
+//  glPointSize(3);
+//  glColor3f(1.0, 0.0, 0.0);
+//  glEnable(GL_BLEND);
+//  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+//
+//  glColor3ubv(cam_color);
+//
+//  if (!vio_t_w_i.empty()) {
+//    size_t end = std::min(vio_t_w_i.size(), size_t(show_frame + 1));
+//    Eigen::aligned_vector<Eigen::Vector3d> sub_gt(vio_t_w_i.begin(),
+//                                                  vio_t_w_i.begin() + end);
+//    pangolin::glDrawLineStrip(sub_gt);
+//  }
+//
+//  glColor3ubv(gt_color);
+//  if (show_gt) pangolin::glDrawLineStrip(gt_t_w_i);
 
 //  size_t frame_id = show_frame;
 //  int64_t t_ns = vio_dataset->get_image_timestamps()[frame_id];
@@ -1375,7 +1384,7 @@ void draw_scene_no_camera(pangolin::View& view) {
 //    glColor3ubv(pose_color);
 //    pangolin::glDrawPoints(it->second->points);
 //  }
-  pangolin::glDrawAxis(Sophus::SE3d().matrix(), 1.0);
+//  pangolin::glDrawAxis(Sophus::SE3d().matrix(), 1.0);
 }
 
 void load_data(const std::string& calib_path) {