fixing the bugs
This commit is contained in:
Ivan
131
src/vio.cpp
131
src/vio.cpp
@@ -853,8 +853,36 @@ int main(int argc, char** argv) {
|
||||
if (drawTexture) {
|
||||
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);
|
||||
|
||||
// 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);
|
||||
}
|
||||
// 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 (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);}
|
||||
@@ -1244,63 +1272,39 @@ void draw_scene(pangolin::View& view) {
|
||||
void draw_scene_no_camera(pangolin::View& view) {
|
||||
UNUSED(view);
|
||||
view.Activate(ar_3d_camera);
|
||||
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];
|
||||
|
||||
glMatrixMode(GL_PROJECTION);
|
||||
P.Load();
|
||||
|
||||
glMatrixMode(GL_MODELVIEW);
|
||||
//// 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
|
||||
// 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::vector<double> axesTwi = {0.0, 0.0, 0.0};
|
||||
std::vector<double> axesTic = {0.0, 0.0, 0.0};
|
||||
for (int i=0;i<3;i++){
|
||||
if (desired_axes_sequence[i] == 'X'){
|
||||
axesTwi[i] = T_w_i.angleX();
|
||||
axesTic[i] = T_i_c.angleX();
|
||||
}
|
||||
else if (desired_axes_sequence[i] == 'Y'){
|
||||
axesTwi[i] = T_w_i.angleY();
|
||||
axesTic[i] = T_i_c.angleY();
|
||||
}
|
||||
else if (desired_axes_sequence[i] == 'Z'){
|
||||
axesTwi[i] = T_w_i.angleZ();
|
||||
axesTic[i] = T_i_c.angleZ();
|
||||
}
|
||||
|
||||
if (axes_signs[i] == '+'){
|
||||
axesTwi[i] = axesTwi[i];
|
||||
axesTic[i] = axesTic[i];
|
||||
}
|
||||
else if (axes_signs[i] == '-'){
|
||||
axesTwi[i] = -axesTwi[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);}
|
||||
// std::string desired_axes_sequence = axes_seq;
|
||||
// std::vector<double> axesTwi = {0.0, 0.0, 0.0};
|
||||
// std::vector<double> axesTic = {0.0, 0.0, 0.0};
|
||||
// for (int i=0;i<3;i++){
|
||||
// if (desired_axes_sequence[i] == 'X'){
|
||||
// axesTwi[i] = T_w_i.angleX();
|
||||
// axesTic[i] = T_i_c.angleX();
|
||||
// }
|
||||
// else if (desired_axes_sequence[i] == 'Y'){
|
||||
// axesTwi[i] = T_w_i.angleY();
|
||||
// axesTic[i] = T_i_c.angleY();
|
||||
// }
|
||||
// else if (desired_axes_sequence[i] == 'Z'){
|
||||
// axesTwi[i] = T_w_i.angleZ();
|
||||
// axesTic[i] = T_i_c.angleZ();
|
||||
// }
|
||||
//
|
||||
// if (axes_signs[i] == '+'){
|
||||
// axesTwi[i] = axesTwi[i];
|
||||
// axesTic[i] = axesTic[i];
|
||||
// }
|
||||
// else if (axes_signs[i] == '-'){
|
||||
// axesTwi[i] = -axesTwi[i];
|
||||
// axesTic[i] = -axesTic[i];
|
||||
// }
|
||||
// }
|
||||
|
||||
// For the 1st sequence mono-kitti
|
||||
// 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);
|
||||
|
||||
// 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);
|
||||
// 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);
|
||||
// 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;
|
||||
// std::cout << "Translation before: " << 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());
|
||||
ar_3d_camera.Follow((T_w_i * T_i_c).matrix());
|
||||
|
||||
glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user