pi/ar_basalt
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

v2

Browse Source
This commit is contained in:
Ivan
2022-04-14 13:09:25 +03:00
parent 6dc0eb0fcf
commit 9d73ebfc72
1 changed files with 147 additions and 110 deletions
Download Patch File Download Diff File Expand all files Collapse all files

257
src/vio.cpp
View File

@@ -99,15 +99,26 @@ pangolin::Var<int> show_frame("ui.show_frame", 0, 0, 1500);
// Added by Ivan Podmogilnyi 17.02.2022
pangolin::Var<bool> changeAngleSign("ui.ChangeAngleSign", false, true);
pangolin::Var<bool> drawCubeBool("ui.DrawCube", false, true);
pangolin::Var<bool> drawLineCubeBool("ui.DrawCubeLine", true, true);
pangolin::Var<bool> drawLineCubeBool("ui.DrawCubeLine", false, true);
pangolin::Var<bool> zeroOut("ui.ZeroOut", false, true);
pangolin::Var<bool> drawTexture("ui.drawTexture", true, false);
pangolin::Var<bool> IfDrawOpenCVCube("ui.DrawOpenCVCube", false, true);
pangolin::Var<float> saveRenderScale("ui.SaveRenderScale", 1.653, 1.50, 1.8);
pangolin::Var<bool> saveRenderBool("ui.SaveRender", false, true);
pangolin::Var<bool> showCube1("ui.Cube1", true, false);
pangolin::Var<bool> showCube2("ui.Cube2", true, false);
pangolin::Var<bool> showCube3("ui.Cube3", true, false);
pangolin::Var<bool> showCube4("ui.Cube4", false, true);
pangolin::Var<bool> showCube5("ui.Cube5", false, true);
pangolin::Var<bool> showCube6("ui.Cube6", false, true);
pangolin::Var<bool> showCube7("ui.Cube7", false, true);
pangolin::Var<double> xSkew("ui.x", 0, -500, 500);
pangolin::Var<double> ySkew("ui.y", 0, -50, 50);
pangolin::Var<double> zSkew("ui.z", 0, -500, 500);
pangolin::Var<double> cubeSize("ui.cubeSize", 0.05, 0.01, 2);
pangolin::Var<int> cubeSize("ui.cubeSize", 2, 1, 5);
//End of append by Ivan
pangolin::Var<bool> 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<bool> continue_btn("ui.continue", false, false, true);
pangolin::Var<bool> continue_fast("ui.continue_fast", true, false, true);
pangolin::Var<bool> 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<double, 3, 3>& 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<double>(0,0) = 718.856;
// std::cout << "fx: " << K.at<double>(0, 0);
K.at<double>(1,1) = 718.856;
// std::cout << "fy: " << K.at<double>(1, 1);
K.at<double>(0,2) = 607.1928;
// std::cout << "cx: " << K.at<double>(0, 2);
K.at<double>(1,2) = 185.2157;
// std::cout << "cy: " << K.at<double> (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<double>(0,0) = 718.856;
// // std::cout << "fx: " << K.at<double>(0, 0);
// K.at<double>(1,1) = 718.856;
// // std::cout << "fy: " << K.at<double>(1, 1);
// K.at<double>(0,2) = 607.1928;
// // std::cout << "cx: " << K.at<double>(0, 2);
// K.at<double>(1,2) = 185.2157;
// // std::cout << "cy: " << K.at<double> (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<Eigen::Vector3d> 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<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);
// 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<double, 3, 3> 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);
}