From 1e079547cee351a47e1870204f5338470d467f5a Mon Sep 17 00:00:00 2001 From: Ivan Date: Thu, 14 Apr 2022 17:06:24 +0300 Subject: [PATCH] fixing the bugs --- src/vio.cpp | 131 +++++++++++++++++++++++++++------------------------- 1 file changed, 67 insertions(+), 64 deletions(-) diff --git a/src/vio.cpp b/src/vio.cpp index 69c211b..bfbf1c2 100644 --- a/src/vio.cpp +++ b/src/vio.cpp @@ -853,8 +853,36 @@ int main(int argc, char** argv) { if (drawTexture) { DrawImageTexture(imageTexture, img_cv_data[2]); } - glClear(GL_DEPTH_BUFFER_BIT); -// 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); + P.Load(); + + // drawLinesCube pushes the matrices, therefore, I need to define in which + // stack to push that matrices. + glMatrixMode(GL_MODELVIEW); + if (zeroOut){ + xSkew = 0; + ySkew = 0; + zSkew = 0; + } + if (drawCubeBool){ + DrawCube(cubeSize, xSkew, ySkew, zSkew); + } + if (drawLineCubeBool){ + drawLinesCube(xSkew, ySkew, zSkew, cubeSize); + } + // For the 6th sequence mono-kitti + if (showCube1) {drawLinesCube(-5.5, 0.5, -35.0, cubeSize);} + if (showCube2) {drawLinesCube(5.5, 0.5, -40.0, cubeSize);} + if (showCube3) {drawLinesCube(-5.5, 1.0, -95.0, cubeSize);} + if (showCube4) {drawLinesCube(5.5, 1.0, -150.0, cubeSize);} + if (showCube5) {drawLinesCube(16.0, 4.8, -324.5, cubeSize);} + 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); // Ok, so here by the moment we are watching from the camera position and orientation. @@ -1175,10 +1203,10 @@ void draw_scene(pangolin::View& view) { drawLinesCube(xSkew, ySkew, zSkew, 1); } // For the 6th sequence mono-kitti - if (showCube1) {drawLinesCube(-5.5, 0.5, -35.0, cubeSize);} - if (showCube2) {drawLinesCube(5.5, 0.5, -40.0, cubeSize);} - if (showCube3) {drawLinesCube(-5.5, 1.0, -95.0, cubeSize);} - if (showCube4) {drawLinesCube(5.5, 1.0, -150.0, cubeSize);} + if (showCube1) {drawLinesCube(-5.5, 3.5, -35.0, cubeSize);} + if (showCube2) {drawLinesCube(5.5, 3.5, -40.0, cubeSize);} + if (showCube3) {drawLinesCube(-5.5, 3.8, -95.0, cubeSize);} + if (showCube4) {drawLinesCube(5.5, 3.8, -150.0, cubeSize);} if (showCube5) {drawLinesCube(16.0, 4.8, -324.5, cubeSize);} if (showCube6) {drawLinesCube(32.8, 4.8, -324.5, cubeSize);} if (showCube7) {drawLinesCube(32.5, 4.8, -345.7, cubeSize);} @@ -1244,63 +1272,39 @@ void draw_scene(pangolin::View& view) { void draw_scene_no_camera(pangolin::View& view) { UNUSED(view); view.Activate(ar_3d_camera); - auto T_w_i = vio_T_w_i[show_frame]; - auto T_i_c = calib.T_i_c[0]; +// auto T_w_i = vio_T_w_i[show_frame]; +// auto T_i_c = calib.T_i_c[0]; - glMatrixMode(GL_PROJECTION); - P.Load(); - - glMatrixMode(GL_MODELVIEW); //// std::cout << "Extracted T_w_i matrix: " << T_w_i.matrix() << std::endl; // If you want to translate OR to Load the constructed matrix (doesn't matter) for // the camera, you need to pass it with the NEGATIVE sign (but probably only the translation part.). - std::string desired_axes_sequence = axes_seq; - std::vector axesTwi = {0.0, 0.0, 0.0}; - std::vector axesTic = {0.0, 0.0, 0.0}; - for (int i=0;i<3;i++){ - if (desired_axes_sequence[i] == 'X'){ - axesTwi[i] = T_w_i.angleX(); - axesTic[i] = T_i_c.angleX(); - } - else if (desired_axes_sequence[i] == 'Y'){ - axesTwi[i] = T_w_i.angleY(); - axesTic[i] = T_i_c.angleY(); - } - else if (desired_axes_sequence[i] == 'Z'){ - axesTwi[i] = T_w_i.angleZ(); - axesTic[i] = T_i_c.angleZ(); - } - - if (axes_signs[i] == '+'){ - axesTwi[i] = axesTwi[i]; - axesTic[i] = axesTic[i]; - } - else if (axes_signs[i] == '-'){ - axesTwi[i] = -axesTwi[i]; - axesTic[i] = -axesTic[i]; - } - } - - if (zeroOut){ - xSkew = 0; - ySkew = 0; - zSkew = 0; - } - if (drawCubeBool){ - DrawCube(cubeSize, xSkew, ySkew, zSkew); - } - if (drawLineCubeBool){ - drawLinesCube(xSkew, ySkew, zSkew, cubeSize); - } - // For the 6th sequence mono-kitti - if (showCube1) {drawLinesCube(-5.5, 0.5, -35.0, cubeSize);} - if (showCube2) {drawLinesCube(5.5, 0.5, -40.0, cubeSize);} - if (showCube3) {drawLinesCube(-5.5, 1.0, -95.0, cubeSize);} - if (showCube4) {drawLinesCube(5.5, 1.0, -150.0, cubeSize);} - if (showCube5) {drawLinesCube(16.0, 4.8, -324.5, cubeSize);} - if (showCube6) {drawLinesCube(32.8, 4.8, -324.5, cubeSize);} - if (showCube7) {drawLinesCube(32.5, 4.8, -345.7, cubeSize);} +// std::string desired_axes_sequence = axes_seq; +// std::vector axesTwi = {0.0, 0.0, 0.0}; +// std::vector axesTic = {0.0, 0.0, 0.0}; +// for (int i=0;i<3;i++){ +// if (desired_axes_sequence[i] == 'X'){ +// axesTwi[i] = T_w_i.angleX(); +// axesTic[i] = T_i_c.angleX(); +// } +// else if (desired_axes_sequence[i] == 'Y'){ +// axesTwi[i] = T_w_i.angleY(); +// axesTic[i] = T_i_c.angleY(); +// } +// else if (desired_axes_sequence[i] == 'Z'){ +// axesTwi[i] = T_w_i.angleZ(); +// axesTic[i] = T_i_c.angleZ(); +// } +// +// if (axes_signs[i] == '+'){ +// axesTwi[i] = axesTwi[i]; +// axesTic[i] = axesTic[i]; +// } +// else if (axes_signs[i] == '-'){ +// axesTwi[i] = -axesTwi[i]; +// axesTic[i] = -axesTic[i]; +// } +// } // For the 1st sequence mono-kitti // drawLinesCube(0.0, 1.0, -95.0, cubeSize); @@ -1312,20 +1316,19 @@ void draw_scene_no_camera(pangolin::View& view) { // drawLinesCube(-64.0, 3.0, -85.0, cubeSize); // std::cout << "Twi with rot before: " << T_w_i.matrix() << std::endl; - Eigen::Matrix Rot; - euler2Rot(Rot, axesTwi[0], axesTwi[1], axesTwi[2]); - T_w_i.setRotationMatrix(Rot); +// Eigen::Matrix Rot; +// euler2Rot(Rot, axesTwi[0], axesTwi[1], axesTwi[2]); +// T_w_i.setRotationMatrix(Rot); // auto changed_translation = T_w_i.translation(); // changed_translation[1] /= 5; // T_w_i.translation() = changed_translation; - euler2Rot(Rot, axesTic[0], axesTic[1], axesTic[2]); - T_i_c.setRotationMatrix(Rot); +// euler2Rot(Rot, axesTic[0], axesTic[1], axesTic[2]); +// T_i_c.setRotationMatrix(Rot); // std::cout << "Twi with rot after: " << T_w_i.matrix() << std::endl; // std::cout << "Translation before: " << T_w_i.translation() << std::endl; // std::cout << std::endl << "Translation after: " << T_w_i.translation() << std::endl; // LoadCameraPose((T_w_i * T_i_c).matrix()); - ar_3d_camera.Follow((T_w_i * T_i_c).matrix()); glClearColor(1.0f, 1.0f, 1.0f, 1.0f);