ml/ar_basalt
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Compare commits

base: ml:master
Branches Tags
ml:master ml:drawing_cubes_in_function
..
compare: ml:drawing_cubes_in_function
Branches Tags
ml:drawing_cubes_in_function ml:master

2 Commits
master ... drawing_cu

Author SHA1 Message Date
Ivan
06bd532caa v3 2022-04-17 14:01:07 +03:00
Ivan
617e16f8b4 v1 2022-04-15 15:50:35 +03:00
1 changed files with 88 additions and 80 deletions
Expand all files Collapse all files

168
src/vio.cpp
View File

@@ -104,7 +104,10 @@ 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);
// For the sequence 6
//pangolin::Var<float> saveRenderScale("ui.SaveRenderScale", 1.653, 1.50, 1.8);
// For the sequence 0, 1.68 is the right.
pangolin::Var<float> saveRenderScale("ui.SaveRenderScale", 1.68, 1.5, 1.8);
pangolin::Var<bool> saveRenderBool("ui.SaveRender", false, true);
pangolin::Var<bool> showCube1("ui.Cube1", true, false);
@@ -678,9 +681,13 @@ int main(int argc, char** argv) {
pangolin::View& main_display = pangolin::CreateDisplay().SetBounds(
0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0);
// For the saving of the files. Use 1.68 scale save of 00 sequence.
pangolin::View& img_view_display = pangolin::CreateDisplay()
.SetBounds(0.4, 1.0, 0, 0.4)
.SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0)
.SetLayout(pangolin::LayoutEqual);
// pangolin::View& img_view_display = pangolin::CreateDisplay()
// .SetBounds(0.4, 1.0, 0.0, 0.4)
// .SetLayout(pangolin::LayoutEqual);
// pangolin::View& plot_display = pangolin::CreateDisplay().SetBounds(
// 0.0, 0.4, pangolin::Attach::Pix(UI_WIDTH), 1.0);
@@ -764,6 +771,7 @@ int main(int argc, char** argv) {
display3D.extern_draw_function = draw_scene;
// Appended.
// Appended.
ar_view.extern_draw_function = draw_scene_no_camera;
@@ -832,7 +840,6 @@ 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();
glColor3f(1.0, 1.0, 1.0);
// Get the last image pose. (probably, it's T_w_i
@@ -846,7 +853,6 @@ 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);
@@ -855,44 +861,8 @@ int main(int argc, char** argv) {
if (drawTexture) {
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);
P.Load();
// 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;
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);}
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.
// Although! We want to draw the cube from the world coordinate system, therefore
@@ -994,6 +964,8 @@ 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));
@@ -1012,7 +984,6 @@ int main(int argc, char** argv) {
continue_fast = false;
}
}
pangolin::FinishFrame();
}
// If GUI closed but VIO not yet finished --> abort input queues, which in
@@ -1210,13 +1181,22 @@ void draw_scene(pangolin::View& view) {
drawLinesCube(xSkew, ySkew, zSkew, 1);
}
// For the 6th sequence mono-kitti
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);}
// 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);}
// For the 1st sequence mono-kitti
if (showCube1) {drawLinesCube(0.0, 1.0, -95.0, cubeSize);}
if (showCube2) {drawLinesCube(0.0, 1.5, -35.0, cubeSize);}
if (showCube3) {drawLinesCube(5.0, 1.5, -40.0, cubeSize);}
if (showCube4) {drawLinesCube(-32.0, 1.5, -85.0, cubeSize);}
if (showCube5) {drawLinesCube(-32.0, 1.5, -95.0, cubeSize);}
if (showCube6) {drawLinesCube(-64.0, 3.0, -95.0, cubeSize);}
if (showCube7) {drawLinesCube(-64.0, 3.0, -85.0, cubeSize);}
// try{
// view.Activate(T);
// throw (0);
@@ -1279,9 +1259,8 @@ void draw_scene(pangolin::View& view) {
void draw_scene_no_camera(pangolin::View& view) {
UNUSED(view);
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];
auto T_w_i = vio_T_w_i[show_frame];
auto T_i_c = calib.T_i_c[0];
//// 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
@@ -1314,16 +1293,6 @@ void draw_scene_no_camera(pangolin::View& view) {
// }
// }
// 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]);
@@ -1339,24 +1308,63 @@ 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);
// 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);}
// For the 1st sequence mono-kitti
if (showCube1) {drawLinesCube(0.0, 1.0, -95.0, cubeSize);}
if (showCube2) {drawLinesCube(0.0, 1.5, -35.0, cubeSize);}
if (showCube3) {drawLinesCube(5.0, 1.5, -40.0, cubeSize);}
if (showCube4) {drawLinesCube(-32.0, 1.5, -85.0, cubeSize);}
if (showCube5) {drawLinesCube(-32.0, 1.5, -95.0, cubeSize);}
if (showCube6) {drawLinesCube(-64.0, 3.0, -95.0, cubeSize);}
if (showCube7) {drawLinesCube(-64.0, 3.0, -85.0, cubeSize);}
ar_3d_camera.Follow((T_w_i * T_i_c).matrix());
// size_t frame_id = show_frame;
// int64_t t_ns = vio_dataset->get_image_timestamps()[frame_id];
@@ -1384,7 +1392,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) {