From 9d73ebfc7217dbc5023a7c27d4cd72e8e5e8cb93 Mon Sep 17 00:00:00 2001 From: Ivan Date: Thu, 14 Apr 2022 13:09:25 +0300 Subject: [PATCH] v2 --- src/vio.cpp | 257 ++++++++++++++++++++++++++++++---------------------- 1 file changed, 147 insertions(+), 110 deletions(-) diff --git a/src/vio.cpp b/src/vio.cpp index 0acaf2a..c322811 100644 --- a/src/vio.cpp +++ b/src/vio.cpp @@ -99,15 +99,26 @@ pangolin::Var show_frame("ui.show_frame", 0, 0, 1500); // Added by Ivan Podmogilnyi 17.02.2022 pangolin::Var changeAngleSign("ui.ChangeAngleSign", false, true); pangolin::Var drawCubeBool("ui.DrawCube", false, true); -pangolin::Var drawLineCubeBool("ui.DrawCubeLine", true, true); +pangolin::Var drawLineCubeBool("ui.DrawCubeLine", false, true); pangolin::Var zeroOut("ui.ZeroOut", false, true); pangolin::Var drawTexture("ui.drawTexture", true, false); pangolin::Var IfDrawOpenCVCube("ui.DrawOpenCVCube", false, true); +pangolin::Var saveRenderScale("ui.SaveRenderScale", 1.653, 1.50, 1.8); +pangolin::Var saveRenderBool("ui.SaveRender", false, true); + +pangolin::Var showCube1("ui.Cube1", true, false); +pangolin::Var showCube2("ui.Cube2", true, false); +pangolin::Var showCube3("ui.Cube3", true, false); +pangolin::Var showCube4("ui.Cube4", false, true); +pangolin::Var showCube5("ui.Cube5", false, true); +pangolin::Var showCube6("ui.Cube6", false, true); +pangolin::Var showCube7("ui.Cube7", false, true); + pangolin::Var xSkew("ui.x", 0, -500, 500); pangolin::Var ySkew("ui.y", 0, -50, 50); pangolin::Var zSkew("ui.z", 0, -500, 500); -pangolin::Var cubeSize("ui.cubeSize", 0.05, 0.01, 2); +pangolin::Var cubeSize("ui.cubeSize", 2, 1, 5); //End of append by Ivan pangolin::Var show_flow("ui.show_flow", false, false, true); @@ -125,7 +136,7 @@ Button next_step_btn("ui.next_step", &next_step); Button prev_step_btn("ui.prev_step", &prev_step); pangolin::Var continue_btn("ui.continue", false, false, true); -pangolin::Var continue_fast("ui.continue_fast", true, false, true); +pangolin::Var continue_fast("ui.continue_fast", false, false, true); Button align_se3_btn("ui.align_se3", &alignButton); @@ -259,7 +270,7 @@ Eigen::Vector3d rot2rvec(Eigen::Matrix& Mat, bool changeAngleSign_ return rvec; } -void drawLinesCube(float x, float y, float z){ +void drawLinesCube(float x, float y, float z, int scale){ // float size = 1.0; pangolin::OpenGlMatrix M = pangolin::OpenGlMatrix::Translate(-x,-y,-z); x = 0; y = 0; z = 0; @@ -273,53 +284,53 @@ void drawLinesCube(float x, float y, float z){ // Bottom // glColor3ui(133, 247, 208); glVertex3f(x, y, z); - glVertex3f(x, y+1, z); + glVertex3f(x, y+scale, z); // glColor3ui(253, 59, 86); glVertex3f(x, y, z); - glVertex3f(x+1, y, z); + glVertex3f(x+scale, y, z); // glColor3ui(147, 175, 215); - glVertex3f(x, y+1, z); - glVertex3f(x+1, y+1, z); + glVertex3f(x, y+scale, z); + glVertex3f(x+scale, y+scale, z); // glColor3ui(80, 209, 168); - glVertex3f(x+1, y, z); - glVertex3f(x+1, y+1, z); + glVertex3f(x+scale, y, z); + glVertex3f(x+scale, y+scale, z); // Top // glColor3ui(154, 13, 88); - glVertex3f(x, y, z+1); - glVertex3f(x, y+1, z+1); + glVertex3f(x, y, z+scale); + glVertex3f(x, y+scale, z+scale); // glColor3ui(253, 59, 86); - glVertex3f(x, y, z+1); - glVertex3f(x+1, y, z+1); + glVertex3f(x, y, z+scale); + glVertex3f(x+scale, y, z+scale); // glColor3ui(5, 26, 72); - glVertex3f(x, y+1, z+1); - glVertex3f(x+1, y+1, z+1); + glVertex3f(x, y+scale, z+scale); + glVertex3f(x+scale, y+scale, z+scale); // glColor3ui(72, 182, 8); - glVertex3f(x+1, y, z+1); - glVertex3f(x+1, y+1, z+1); + glVertex3f(x+scale, y, z+scale); + glVertex3f(x+scale, y+scale, z+scale); // Sides // glColor3ui(28, 122, 71); glVertex3f(x, y, z); - glVertex3f(x, y, z+1); + glVertex3f(x, y, z+scale); // glColor3ui(244, 207, 185); - glVertex3f(x, y+1, z); - glVertex3f(x, y+1, z+1); + glVertex3f(x, y+scale, z); + glVertex3f(x, y+scale, z+scale); // glColor3ui(88, 153, 225); - glVertex3f(x+1, y, z); - glVertex3f(x+1, y, z+1); + glVertex3f(x+scale, y, z); + glVertex3f(x+scale, y, z+scale); // glColor3ui(184, 151, 253); - glVertex3f(x+1, y+1, z); - glVertex3f(x+1, y+1, z+1); + glVertex3f(x+scale, y+scale, z); + glVertex3f(x+scale, y+scale, z+scale); glEnd(); glPopMatrix(); @@ -663,11 +674,13 @@ int main(int argc, char** argv) { 0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0); pangolin::View& img_view_display = pangolin::CreateDisplay() - .SetBounds(0.4, 1.0, 0.0, 0.4) + .SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0) .SetLayout(pangolin::LayoutEqual); +// pangolin::View& plot_display = pangolin::CreateDisplay().SetBounds( +// 0.0, 0.4, pangolin::Attach::Pix(UI_WIDTH), 1.0); pangolin::View& plot_display = pangolin::CreateDisplay().SetBounds( - 0.0, 0.4, pangolin::Attach::Pix(UI_WIDTH), 1.0); + 0.0, 0.0, 0.0, 0.0); plotter = new pangolin::Plotter(&imu_data_log, 0.0, 100, -10.0, 10.0, 0.01f, 0.01f); @@ -711,28 +724,28 @@ int main(int argc, char** argv) { auto cx = intrinsics_vector[2]; auto cy = intrinsics_vector[3]; - // std::cout << "Hessian calib" << std::endl; - K.at(0,0) = 718.856; - // std::cout << "fx: " << K.at(0, 0); - K.at(1,1) = 718.856; - // std::cout << "fy: " << K.at(1, 1); - K.at(0,2) = 607.1928; - // std::cout << "cx: " << K.at(0, 2); - K.at(1,2) = 185.2157; - // std::cout << "cy: " << K.at (1, 2); - - distCoeffs[0] = 0.003482389402; - distCoeffs[1] = 0.000715034845; - distCoeffs[2] = -0.002053236141; - distCoeffs[3] = 0.000202936736; - distCoeffs[4] = 0; +// // std::cout << "Hessian calib" << std::endl; +// K.at(0,0) = 718.856; +// // std::cout << "fx: " << K.at(0, 0); +// K.at(1,1) = 718.856; +// // std::cout << "fy: " << K.at(1, 1); +// K.at(0,2) = 607.1928; +// // std::cout << "cx: " << K.at(0, 2); +// K.at(1,2) = 185.2157; +// // std::cout << "cy: " << K.at (1, 2); +// +// distCoeffs[0] = 0.003482389402; +// distCoeffs[1] = 0.000715034845; +// distCoeffs[2] = -0.002053236141; +// distCoeffs[3] = 0.000202936736; +// distCoeffs[4] = 0; ar_3d_camera = pangolin::OpenGlRenderState( pangolin::ProjectionMatrix(1241.0, 376.0, fx, fy, cx, cy, 0.001, 10000), pangolin::ModelViewLookAt(0.0, 0.0, 0.0, 0, 0, 1, pangolin::AxisNegY)); - P = pangolin::ProjectionMatrix(1241, 376, fx, fy, cx, cy, 0.001, 10000); + P = pangolin::ProjectionMatrix(1241.0, 376.0, fx, fy, cx, cy, 0.001, 10000); // pangolin::GlTexture imageTexture(1241, // 376, GL_LUMINANCE, false, 0, GL_LUMINANCE, GL_UNSIGNED_BYTE); // Appended @@ -902,6 +915,11 @@ int main(int argc, char** argv) { draw_plots(); } + if (saveRenderBool){ + ar_view.SaveRenderNow(std::to_string(show_frame) + "_demo", saveRenderScale); + } +// ar_view.SaveOnRender(std::to_string(show_frame) + "_demo.png"); + if (show_est_vel.GuiChanged() || show_est_pos.GuiChanged() || show_est_ba.GuiChanged() || show_est_bg.GuiChanged()) { draw_plots(); @@ -1142,12 +1160,12 @@ void draw_scene(pangolin::View& view) { UNUSED(view); view.Activate(camera); - if (drawCubeBool){ - DrawCube(cubeSize, xSkew, ySkew, zSkew); - } - else if(drawLineCubeBool){ - drawLinesCube(xSkew, ySkew, zSkew); - } +// if (drawCubeBool){ +// DrawCube(cubeSize, xSkew, ySkew, zSkew); +// } +// else if(drawLineCubeBool){ +// drawLinesCube(xSkew, ySkew, zSkew, 1); +// } // try{ // view.Activate(T); // throw (0); @@ -1167,44 +1185,44 @@ void draw_scene(pangolin::View& view) { 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 sub_gt(vio_t_w_i.begin(), - vio_t_w_i.begin() + end); - pangolin::glDrawLineStrip(sub_gt); - } +// if (!vio_t_w_i.empty()) { +// size_t end = std::min(vio_t_w_i.size(), size_t(show_frame + 1)); +// Eigen::aligned_vector 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); +// 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]; - auto it = vis_map.find(t_ns); +// size_t frame_id = show_frame; +// int64_t t_ns = vio_dataset->get_image_timestamps()[frame_id]; +// auto it = vis_map.find(t_ns); +// +// if (it != vis_map.end()) { +// // T_i_c - transformation from camera coordinate point to the imu coordinate point. +// for (size_t i = 0; i < calib.T_i_c.size(); i++) +// if (!it->second->states.empty()) { +// render_camera((it->second->states.back() * calib.T_i_c[i]).matrix(), +// 2.0f, cam_color, 0.1f); +// } else if (!it->second->frames.empty()) { +// render_camera((it->second->frames.back() * calib.T_i_c[i]).matrix(), +// 2.0f, cam_color, 0.1f); +// } +// +// for (const auto& p : it->second->states) +// for (size_t i = 0; i < calib.T_i_c.size(); i++) +// render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, state_color, 0.1f); +// +// for (const auto& p : it->second->frames) +// for (size_t i = 0; i < calib.T_i_c.size(); i++) +// render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, pose_color, 0.1f); +// +// glColor3ubv(pose_color); +// pangolin::glDrawPoints(it->second->points); +// } - if (it != vis_map.end()) { - // T_i_c - transformation from camera coordinate point to the imu coordinate point. - for (size_t i = 0; i < calib.T_i_c.size(); i++) - if (!it->second->states.empty()) { - render_camera((it->second->states.back() * calib.T_i_c[i]).matrix(), - 2.0f, cam_color, 0.1f); - } else if (!it->second->frames.empty()) { - render_camera((it->second->frames.back() * calib.T_i_c[i]).matrix(), - 2.0f, cam_color, 0.1f); - } - - for (const auto& p : it->second->states) - for (size_t i = 0; i < calib.T_i_c.size(); i++) - render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, state_color, 0.1f); - - for (const auto& p : it->second->frames) - for (size_t i = 0; i < calib.T_i_c.size(); i++) - render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, pose_color, 0.1f); - - glColor3ubv(pose_color); - pangolin::glDrawPoints(it->second->points); - } - - pangolin::glDrawAxis(Sophus::SE3d().matrix(), 1.0); +// pangolin::glDrawAxis(Sophus::SE3d().matrix(), 1.0); } void draw_scene_no_camera(pangolin::View& view) { @@ -1257,13 +1275,33 @@ void draw_scene_no_camera(pangolin::View& view) { DrawCube(cubeSize, xSkew, ySkew, zSkew); } else if(drawLineCubeBool){ - drawLinesCube(xSkew, ySkew, zSkew); + 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);} + + // For the 1st sequence mono-kitti +// drawLinesCube(0.0, 1.0, -95.0, cubeSize); +// drawLinesCube(0.0, 0.5, -35.0, cubeSize); +// drawLinesCube(5.0, 0.5, -40.0, cubeSize); +// drawLinesCube(-32.0, 1.0, -85.0, cubeSize); +// drawLinesCube(-32.0, 1.5, -95.0, cubeSize); +// drawLinesCube(-64.0, 3.0, -95.0, cubeSize); +// 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); +// 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); // std::cout << "Twi with rot after: " << T_w_i.matrix() << std::endl; @@ -1292,33 +1330,32 @@ void draw_scene_no_camera(pangolin::View& view) { 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]; - auto it = vis_map.find(t_ns); - - if (it != vis_map.end()) { - // T_i_c - transformation from camera coordinate point to the imu coordinate point. - for (size_t i = 0; i < calib.T_i_c.size(); i++) - if (!it->second->states.empty()) { - render_camera((it->second->states.back() * calib.T_i_c[i]).matrix(), - 2.0f, cam_color, 0.1f); - } else if (!it->second->frames.empty()) { - render_camera((it->second->frames.back() * calib.T_i_c[i]).matrix(), - 2.0f, cam_color, 0.1f); - } - - for (const auto& p : it->second->states) - for (size_t i = 0; i < calib.T_i_c.size(); i++) - render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, state_color, 0.1f); - - for (const auto& p : it->second->frames) - for (size_t i = 0; i < calib.T_i_c.size(); i++) - render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, pose_color, 0.1f); - - glColor3ubv(pose_color); - pangolin::glDrawPoints(it->second->points); - } +// size_t frame_id = show_frame; +// int64_t t_ns = vio_dataset->get_image_timestamps()[frame_id]; +// auto it = vis_map.find(t_ns); +// if (it != vis_map.end()) { +// // T_i_c - transformation from camera coordinate point to the imu coordinate point. +// for (size_t i = 0; i < calib.T_i_c.size(); i++) +// if (!it->second->states.empty()) { +// render_camera((it->second->states.back() * calib.T_i_c[i]).matrix(), +// 2.0f, cam_color, 0.1f); +// } else if (!it->second->frames.empty()) { +// render_camera((it->second->frames.back() * calib.T_i_c[i]).matrix(), +// 2.0f, cam_color, 0.1f); +// } +// +// for (const auto& p : it->second->states) +// for (size_t i = 0; i < calib.T_i_c.size(); i++) +// render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, state_color, 0.1f); +// +// for (const auto& p : it->second->frames) +// for (size_t i = 0; i < calib.T_i_c.size(); i++) +// render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, pose_color, 0.1f); +// +// glColor3ubv(pose_color); +// pangolin::glDrawPoints(it->second->points); +// } pangolin::glDrawAxis(Sophus::SE3d().matrix(), 1.0); }