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

fixing the bugs

Browse Source
This commit is contained in:
Ivan
2022-04-14 17:06:24 +03:00
parent 2a22c4ef7c
commit 1e079547ce
1 changed files with 67 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

131
src/vio.cpp
View File

@@ -853,8 +853,36 @@ int main(int argc, char** argv) {
if (drawTexture) { if (drawTexture) {
DrawImageTexture(imageTexture, img_cv_data[2]); 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); // 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.
@@ -1175,10 +1203,10 @@ void draw_scene(pangolin::View& view) {
drawLinesCube(xSkew, ySkew, zSkew, 1); drawLinesCube(xSkew, ySkew, zSkew, 1);
} }
// For the 6th sequence mono-kitti // For the 6th sequence mono-kitti
if (showCube1) {drawLinesCube(-5.5, 0.5, -35.0, cubeSize);} if (showCube1) {drawLinesCube(-5.5, 3.5, -35.0, cubeSize);}
if (showCube2) {drawLinesCube(5.5, 0.5, -40.0, cubeSize);} if (showCube2) {drawLinesCube(5.5, 3.5, -40.0, cubeSize);}
if (showCube3) {drawLinesCube(-5.5, 1.0, -95.0, cubeSize);} if (showCube3) {drawLinesCube(-5.5, 3.8, -95.0, cubeSize);}
if (showCube4) {drawLinesCube(5.5, 1.0, -150.0, cubeSize);} if (showCube4) {drawLinesCube(5.5, 3.8, -150.0, cubeSize);}
if (showCube5) {drawLinesCube(16.0, 4.8, -324.5, cubeSize);} if (showCube5) {drawLinesCube(16.0, 4.8, -324.5, cubeSize);}
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);}
@@ -1244,63 +1272,39 @@ 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);
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];
glMatrixMode(GL_PROJECTION);
P.Load();
glMatrixMode(GL_MODELVIEW);
//// std::cout << "Extracted T_w_i matrix: " << T_w_i.matrix() << std::endl; //// 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 // 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.). // 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::string desired_axes_sequence = axes_seq;
std::vector<double> axesTwi = {0.0, 0.0, 0.0}; // std::vector<double> axesTwi = {0.0, 0.0, 0.0};
std::vector<double> axesTic = {0.0, 0.0, 0.0}; // std::vector<double> axesTic = {0.0, 0.0, 0.0};
for (int i=0;i<3;i++){ // for (int i=0;i<3;i++){
if (desired_axes_sequence[i] == 'X'){ // if (desired_axes_sequence[i] == 'X'){
axesTwi[i] = T_w_i.angleX(); // axesTwi[i] = T_w_i.angleX();
axesTic[i] = T_i_c.angleX(); // axesTic[i] = T_i_c.angleX();
} // }
else if (desired_axes_sequence[i] == 'Y'){ // else if (desired_axes_sequence[i] == 'Y'){
axesTwi[i] = T_w_i.angleY(); // axesTwi[i] = T_w_i.angleY();
axesTic[i] = T_i_c.angleY(); // axesTic[i] = T_i_c.angleY();
} // }
else if (desired_axes_sequence[i] == 'Z'){ // else if (desired_axes_sequence[i] == 'Z'){
axesTwi[i] = T_w_i.angleZ(); // axesTwi[i] = T_w_i.angleZ();
axesTic[i] = T_i_c.angleZ(); // axesTic[i] = T_i_c.angleZ();
} // }
//
if (axes_signs[i] == '+'){ // if (axes_signs[i] == '+'){
axesTwi[i] = axesTwi[i]; // axesTwi[i] = axesTwi[i];
axesTic[i] = axesTic[i]; // axesTic[i] = axesTic[i];
} // }
else if (axes_signs[i] == '-'){ // else if (axes_signs[i] == '-'){
axesTwi[i] = -axesTwi[i]; // axesTwi[i] = -axesTwi[i];
axesTic[i] = -axesTic[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);}
// For the 1st sequence mono-kitti // For the 1st sequence mono-kitti
// drawLinesCube(0.0, 1.0, -95.0, cubeSize); // 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); // drawLinesCube(-64.0, 3.0, -85.0, cubeSize);
// std::cout << "Twi with rot before: " << T_w_i.matrix() << std::endl; // std::cout << "Twi with rot before: " << T_w_i.matrix() << std::endl;
Eigen::Matrix<double, 3, 3> Rot; // Eigen::Matrix<double, 3, 3> Rot;
euler2Rot(Rot, axesTwi[0], axesTwi[1], axesTwi[2]); // euler2Rot(Rot, axesTwi[0], axesTwi[1], axesTwi[2]);
T_w_i.setRotationMatrix(Rot); // T_w_i.setRotationMatrix(Rot);
// auto changed_translation = T_w_i.translation(); // auto changed_translation = T_w_i.translation();
// changed_translation[1] /= 5; // changed_translation[1] /= 5;
// T_w_i.translation() = changed_translation; // T_w_i.translation() = changed_translation;
euler2Rot(Rot, axesTic[0], axesTic[1], axesTic[2]); // euler2Rot(Rot, axesTic[0], axesTic[1], axesTic[2]);
T_i_c.setRotationMatrix(Rot); // T_i_c.setRotationMatrix(Rot);
// std::cout << "Twi with rot after: " << T_w_i.matrix() << std::endl; // std::cout << "Twi with rot after: " << T_w_i.matrix() << std::endl;
// std::cout << "Translation before: " << T_w_i.translation() << std::endl; // std::cout << "Translation before: " << T_w_i.translation() << std::endl;
// std::cout << std::endl << "Translation after: " << 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()); // 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); glClearColor(1.0f, 1.0f, 1.0f, 1.0f);