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

v1

Browse Source
This commit is contained in:
Ivan
2022-04-15 15:09:37 +03:00
parent 1e079547ce
commit cb1d2b4e81
1 changed files with 34 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

59
src/vio.cpp
View File

@@ -831,7 +831,8 @@ int main(int argc, char** argv) {
// Appended. // Appended.
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 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); glColor3f(1.0, 1.0, 1.0);
// Get the last image pose. (probably, it's T_w_i // 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(); T_w_i = it_ar->second->frames.back();
} }
auto T_i_c = calib.T_i_c[0]; auto T_i_c = calib.T_i_c[0];
// Draw OpenCV cube // Draw OpenCV cube
if (IfDrawOpenCVCube){ if (IfDrawOpenCVCube){
DrawOpenCVCube(img_cv_data[2], (T_w_i * T_i_c), K, distCoeffs); 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]); 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 // TODO: Can I define it outside of the loop? There's no need I guess to
// redefine the the projection matrix every time // redefine the the projection matrix every time
glMatrixMode(GL_PROJECTION); glMatrixMode(GL_PROJECTION);
@@ -862,6 +869,9 @@ int main(int argc, char** argv) {
// drawLinesCube pushes the matrices, therefore, I need to define in which // drawLinesCube pushes the matrices, therefore, I need to define in which
// stack to push that matrices. // stack to push that matrices.
glMatrixMode(GL_MODELVIEW); glMatrixMode(GL_MODELVIEW);
// ar_3d_camera.Follow((T_w_i * T_i_c).matrix());
glPushMatrix();
if (zeroOut){ if (zeroOut){
xSkew = 0; xSkew = 0;
ySkew = 0; ySkew = 0;
@@ -882,8 +892,7 @@ int main(int argc, char** argv) {
if (showCube6) {drawLinesCube(32.8, 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 (showCube7) {drawLinesCube(32.5, 4.8, -345.7, cubeSize);}
ar_3d_camera.Follow((T_w_i * T_i_c).matrix()); glPopMatrix();
// glClear(GL_DEPTH_BUFFER_BIT);
// Ok, so here by the moment we are watching from the camera position and orientation. // 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. // 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; // record = false;
// } // }
pangolin::FinishFrame();
if (continue_btn) { if (continue_btn) {
if (!next_step()) if (!next_step())
std::this_thread::sleep_for(std::chrono::milliseconds(50)); std::this_thread::sleep_for(std::chrono::milliseconds(50));
@@ -1006,6 +1013,7 @@ int main(int argc, char** argv) {
continue_fast = false; continue_fast = false;
} }
} }
pangolin::FinishFrame();
} }
// If GUI closed but VIO not yet finished --> abort input queues, which in // 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) { void draw_scene_no_camera(pangolin::View& view) {
UNUSED(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_w_i = vio_T_w_i[show_frame];
// auto T_i_c = calib.T_i_c[0]; // 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()); // LoadCameraPose((T_w_i * T_i_c).matrix());
glClearColor(1.0f, 1.0f, 1.0f, 1.0f); // glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
//
glPointSize(3); // glPointSize(3);
glColor3f(1.0, 0.0, 0.0); // glColor3f(1.0, 0.0, 0.0);
glEnable(GL_BLEND); // glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); // glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
//
glColor3ubv(cam_color); // glColor3ubv(cam_color);
//
if (!vio_t_w_i.empty()) { // if (!vio_t_w_i.empty()) {
size_t end = std::min(vio_t_w_i.size(), size_t(show_frame + 1)); // 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(), // Eigen::aligned_vector<Eigen::Vector3d> sub_gt(vio_t_w_i.begin(),
vio_t_w_i.begin() + end); // vio_t_w_i.begin() + end);
pangolin::glDrawLineStrip(sub_gt); // pangolin::glDrawLineStrip(sub_gt);
} // }
//
glColor3ubv(gt_color); // 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; // size_t frame_id = show_frame;
// int64_t t_ns = vio_dataset->get_image_timestamps()[frame_id]; // 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); // glColor3ubv(pose_color);
// pangolin::glDrawPoints(it->second->points); // 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) { void load_data(const std::string& calib_path) {