v3

CMakeLists.txt

@@ -324,8 +324,6 @@ add_executable(basalt_calibrate src/calibrate.cpp src/calibration/cam_calib.cpp)
 target_link_libraries(basalt_calibrate basalt pangolin basalt::cli11)
 
 add_executable(euler2RotTest test/ivan-tests/euler2RotTest.cpp)
-target_link_directories(euler2RotTest PUBLIC ${EIGEN3_INCLUDE_DIR})
-target_link_libraries(euler2RotTest basalt pangolin basalt::cli11)
 
 add_executable(basalt_calibrate_imu src/calibrate_imu.cpp src/calibration/cam_imu_calib.cpp)
 target_link_libraries(basalt_calibrate_imu basalt pangolin basalt::cli11)

README.md

@@ -1,16 +1,75 @@
-# AR Basalt
+[![pipeline status](https://gitlab.com/VladyslavUsenko/basalt/badges/master/pipeline.svg)](https://gitlab.com/VladyslavUsenko/basalt/commits/master)
-Link to the original repository: https://gitlab.com/VladyslavUsenko/basalt
+
+## Basalt
+For more information see https://vision.in.tum.de/research/vslam/basalt
+
+![teaser](doc/img/teaser.png)
+
+This project contains tools for:
+* Camera, IMU and motion capture calibration.
+* Visual-inertial odometry and mapping.
+* Simulated environment to test different components of the system.
+
+Some reusable components of the system are available as a separate [header-only library](https://gitlab.com/VladyslavUsenko/basalt-headers) ([Documentation](https://vladyslavusenko.gitlab.io/basalt-headers/)).
+
+There is also a [Github mirror](https://github.com/VladyslavUsenko/basalt-mirror) of this project to enable easy forking.
+
+## Related Publications
+Visual-Inertial Odometry and Mapping:
+* **Visual-Inertial Mapping with Non-Linear Factor Recovery**, V. Usenko, N. Demmel, D. Schubert, J. Stückler, D. Cremers, In IEEE Robotics and Automation Letters (RA-L) [[DOI:10.1109/LRA.2019.2961227]](https://doi.org/10.1109/LRA.2019.2961227) [[arXiv:1904.06504]](https://arxiv.org/abs/1904.06504).
+
+Calibration (explains implemented camera models):
+* **The Double Sphere Camera Model**, V. Usenko and N. Demmel and D. Cremers, In 2018 International Conference on 3D Vision (3DV), [[DOI:10.1109/3DV.2018.00069]](https://doi.org/10.1109/3DV.2018.00069), [[arXiv:1807.08957]](https://arxiv.org/abs/1807.08957).
+
+Calibration (demonstrates how these tools can be used for dataset calibration):
+* **The TUM VI Benchmark for Evaluating Visual-Inertial Odometry**, D. Schubert, T. Goll,  N. Demmel, V. Usenko, J. Stückler, D. Cremers, In 2018 International Conference on Intelligent Robots and Systems (IROS), [[DOI:10.1109/IROS.2018.8593419]](https://doi.org/10.1109/IROS.2018.8593419), [[arXiv:1804.06120]](https://arxiv.org/abs/1804.06120).
+
+Calibration (describes B-spline trajectory representation used in camera-IMU calibration):
+* **Efficient Derivative Computation for Cumulative B-Splines on Lie Groups**, C. Sommer, V. Usenko, D. Schubert, N. Demmel, D. Cremers, In 2020 Conference on Computer Vision and Pattern Recognition (CVPR), [[DOI:10.1109/CVPR42600.2020.01116]](https://doi.org/10.1109/CVPR42600.2020.01116), [[arXiv:1911.08860]](https://arxiv.org/abs/1911.08860).
+
+Optimization (describes square-root optimization and marginalization used in VIO/VO):
+* **Square Root Marginalization for Sliding-Window Bundle Adjustment**, N. Demmel, D. Schubert, C. Sommer, D. Cremers, V. Usenko, In 2021 International Conference on Computer Vision (ICCV), [[arXiv:2109.02182]](https://arxiv.org/abs/2109.02182)
 
-## Main features
-The main feature is that it shows the AR cubes in the Pangolin visualization window. 
 
 ## Installation
-Please refer to the original repository for the installation and running instructions: https://gitlab.com/VladyslavUsenko/basalt
+### APT installation for Ubuntu 20.04 and 18.04 (Fast)
-
+Set up keys, add the repository to the sources list, update the Ubuntu package index and install Basalt:
-### Building
 ```
+sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-keys 0AD9A3000D97B6C9
+sudo sh -c 'echo "deb [arch=amd64] http://packages.usenko.eu/ubuntu $(lsb_release -sc) $(lsb_release -sc)/main" > /etc/apt/sources.list.d/basalt.list'
+sudo apt-get update
+sudo apt-get dist-upgrade
+sudo apt-get install basalt
+```
+
+### Source installation for Ubuntu >= 18.04 and MacOS >= 10.14 Mojave
+Clone the source code for the project and build it. For MacOS you should have [Homebrew](https://brew.sh/) installed.
+```
+git clone --recursive https://gitlab.com/VladyslavUsenko/basalt.git
+cd basalt
+./scripts/install_deps.sh
 mkdir build
 cd build
-cmake ..
+cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo
-make
+make -j8
 ```
+
+## Usage
+* [Camera, IMU and Mocap calibration. (TUM-VI, Euroc, UZH-FPV and Kalibr datasets)](doc/Calibration.md)
+* [Visual-inertial odometry and mapping. (TUM-VI and Euroc datasets)](doc/VioMapping.md)
+* [Visual odometry (no IMU). (KITTI dataset)](doc/Vo.md)
+* [Simulation tools to test different components of the system.](doc/Simulation.md)
+* [Batch evaluation tutorial (ICCV'21 experiments)](doc/BatchEvaluation.md)
+
+## Device support
+* [Tutorial on Camera-IMU and Motion capture calibration with Realsense T265.](doc/Realsense.md)
+
+## Development
+* [Development environment setup.](doc/DevSetup.md)
+
+## Licence
+The code is provided under a BSD 3-clause license. See the LICENSE file for details.
+Note also the different licenses of thirdparty submodules.
+
+Some improvements are ported back from the fork
+[granite](https://github.com/DLR-RM/granite) (MIT license).

src/vio.cpp

@@ -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 (showCube1) {drawLinesCube(-5.5, 3.5, -35.0, cubeSize);}
-  if (showCube2) {drawLinesCube(5.5, 3.5, -40.0, cubeSize);}
+//  if (showCube2) {drawLinesCube(5.5, 3.5, -40.0, cubeSize);}
-  if (showCube3) {drawLinesCube(-5.5, 3.8, -95.0, cubeSize);}
+//  if (showCube3) {drawLinesCube(-5.5, 3.8, -95.0, cubeSize);}
-  if (showCube4) {drawLinesCube(5.5, 3.8, -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);}
+
+  // 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_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];
 
 ////  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);
+  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);
+
+  // 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) {

thirdparty/basalt-headers/thirdparty/eigen/scripts/CMakeLists.txt

@@ -1,5 +1,5 @@
 get_property(EIGEN_TESTS_LIST GLOBAL PROPERTY EIGEN_TESTS_LIST)
-#configure_file(buildtests.in ${CMAKE_BINARY_DIR}/buildtests.sh @ONLY)
+configure_file(buildtests.in ${CMAKE_BINARY_DIR}/buildtests.sh @ONLY)
 
 configure_file(check.in ${CMAKE_BINARY_DIR}/check.sh COPYONLY)
 configure_file(debug.in ${CMAKE_BINARY_DIR}/debug.sh COPYONLY)