dlr-rm / granite

Granite

IEEE Xplore | arxiv | Video

This project contains the code for the IEEE IROS 2021 paper Towards Robust Monocular Visual Odometry for Flying Robots on Planetary Missions, M. Wudenka, M. G. Müller, N. Demmel, A. Wedler, R. Triebel, D. Cremers, W. Stürzl.

It is mainly build on basalt.

Main differences are:

• support of monocular setups for visual odometry (not mapping)
• new keyframe selection strategy based on negative entropy for non-inertial setups
• integration of landmarks "at infinity" to the optimization
• detection of features to track on more image pyramid levels
• computation of a scalar indicator for scale variance (stereo) or drift variance (monocular)

Installation

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 installed.

git clone --recursive https://github.com/DLR-RM/granite.git
cd granite
./scripts/install_deps.sh

Go to include/granite/utils/common_types.h and decide if the PixelType should be uint8_t or uint16_t. The PixelType determines how many bytes granite uses internally for image representation.

mkdir build
cd build
cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo
make -j8

NOTE: It is possible to compile the code on Ubuntu 16.04, but you need to install cmake-3.10 or higher and gcc-7. See corresponding Dockerfile as an example.

Usage

• Camera, IMU and Mocap calibration. (TUM-VI, Euroc, UZH-FPV and Kalibr datasets)
• Visual odometry (no IMU). (MADMAX and KITTI dataset)
• Visual-inertial odometry and mapping. (TUM-VI and Euroc datasets)
• Simulation tools to test different components of the system.

Development

• Development environment setup.

Licence

The source code is provided under a MIT license. See the LICENSE file for details. Note also the different licenses of thirdparty submodules.

The original source code from Basalt was distributed under a BSD 3-clause license.

granite's Issues

JSON Parsing failed - provided NVP (cam_update_rate) not found

When I test the Kitti dataset it shows this.Is there a problem with the JSON file or with my settings?

error，when cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo

CMake Error at thirdparty/granite-headers/test/CMakeLists.txt:4 (add_subdirectory):
The source directory

/home/sharine/Mine/MyResearch/granite/thirdparty/granite-headers/test/googletest

does not contain a CMakeLists.txt file.

CMake Error at thirdparty/granite-headers/test/CMakeLists.txt:36 (add_subdirectory):
The source directory

/home/sharine/Mine/MyResearch/granite/thirdparty/granite-headers/test/benchmark

does not contain a CMakeLists.txt file.

Camera drifts downwards despite being still

I'm testing granite with a VIO dataset collected with realsense d435i. I've tested this dataset with basalt and I could track it without issues. But when I run the same dataset with granite (and using only the left camera) the camera seems to drift downwards from the beginning of the sequence despite it being still. The sequence starts with camera still and then a forward motion, which I presume is good enough for granite's monocular initialisation. If I start the sequence at the point the forward motion is happening the granite output pose follows the forward motion while drifting downwards as before. In both cases Granite eventually crashes with and error SO3::exp failed! omega: nan nan nan, real: nan, img: nan.

I'm doing these tests with a ros wrapper I made myself for both basalt and granite projects. This way I can feed the dataset from a rosbag and display the camera pose in rviz. The calibration and configuration files are essentially the same for basalt and granite, just adapting for a few different parameters in each project. I think I've setup the granite pipeline correctly, so I'll assume, for the moment, I could rule out my ros wrapper for granite as the cause of this downwards drift. In this case, would you have any advice for what could be the reason for this downwards drift? any configuration parameters that could be the cause? something related with gravity?

how can i use grantie vo (without odometry) by ROS?

wrong issue, sorry

make -j8 error while building from source

Moved #3 (comment) to new issues

OS: Ubuntu 18.04.6 LTS
Compiler: gcc (Ubuntu 7.5.0-3ubuntu1 18.04) 7.5.0

make -j8 error while building from source

[100%] Built target test_nfr
/home/alwyn/granite/src/vio_sim.cpp: In function ‘void load_spline(std::__cxx11::string)’:
/home/alwyn/granite/src/vio_sim.cpp:775:8: error: ‘std::filesystem’ has not been declared
   std::filesystem::path path = spline_path;
        ^~~~~~~~~~
/home/alwyn/granite/src/vio_sim.cpp:777:13: error: ‘std::filesystem’ has not been declared
   if (!std::filesystem::exists(path)) {
             ^~~~~~~~~~
/home/alwyn/granite/src/vio_sim.cpp:777:32: error: ‘path’ was not declared in this scope
   if (!std::filesystem::exists(path)) {
                                ^~~~
/home/alwyn/granite/src/vio_sim.cpp:777:32: note: suggested alternative: ‘powh’
   if (!std::filesystem::exists(path)) {
                                ^~~~
                                powh
/home/alwyn/granite/src/vio_sim.cpp:782:42: error: ‘path’ was not declared in this scope
   std::cout << "Loading spline from " << path << std::endl;
                                          ^~~~
/home/alwyn/granite/src/vio_sim.cpp:782:42: note: suggested alternative: ‘powh’
   std::cout << "Loading spline from " << path << std::endl;
                                          ^~~~
                                          powh
/home/alwyn/granite/src/vio_sim.cpp:774:30: warning: unused parameter ‘spline_path’ [-Wunused-parameter]
 void load_spline(std::string spline_path) {
                              ^~~~~~~~~~~
In file included from /home/alwyn/granite/src/vio_sim.cpp:68:0:
/home/alwyn/granite/thirdparty/granite-headers/include/granite/spline/se3_spline.h: At global scope:
/home/alwyn/granite/thirdparty/granite-headers/include/granite/spline/se3_spline.h:114:23: error: inline function ‘void granite::Se3SplineBase<Scalar>::knots_push_back(const SE3&) [with Scalar = double; granite::Se3SplineBase<Scalar>::SE3 = Sophus::SE3<double>]’ used but never defined [-Werror]
   virtual inline void knots_push_back(const SE3 &knot) = 0;
                       ^~~~~~~~~~~~~~~
/home/alwyn/granite/thirdparty/granite-headers/include/granite/spline/se3_spline.h:90:36: error: inline function ‘int granite::Se3SplineBase<Scalar>::getDegree() [with Scalar = double]’ used but never defined [-Werror]
   [[nodiscard]] virtual inline int getDegree() = 0;
                                    ^~~~~~~~~
/home/alwyn/granite/thirdparty/granite-headers/include/granite/spline/se3_spline.h:186:23: error: inline function ‘granite::Se3SplineBase<Scalar>::Vec3 granite::Se3SplineBase<Scalar>::transAccelWorld(int64_t) const [with Scalar = double; granite::Se3SplineBase<Scalar>::Vec3 = Eigen::Matrix<double, 3, 1>; int64_t = long int]’ used but never defined [-Werror]
   virtual inline Vec3 transAccelWorld(int64_t time_ns) const = 0;
                       ^~~~~~~~~~~~~~~
/home/alwyn/granite/thirdparty/granite-headers/include/granite/spline/se3_spline.h:196:23: error: inline function ‘granite::Se3SplineBase<Scalar>::Vec3 granite::Se3SplineBase<Scalar>::rotVelBody(int64_t) const [with Scalar = double; granite::Se3SplineBase<Scalar>::Vec3 = Eigen::Matrix<double, 3, 1>; int64_t = long int]’ used but never defined [-Werror]
   virtual inline Vec3 rotVelBody(int64_t time_ns) const = 0;
                       ^~~~~~~~~~
/home/alwyn/granite/thirdparty/granite-headers/include/granite/spline/se3_spline.h:191:23: error: inline function ‘granite::Se3SplineBase<Scalar>::Vec3 granite::Se3SplineBase<Scalar>::transVelWorld(int64_t) const [with Scalar = double; granite::Se3SplineBase<Scalar>::Vec3 = Eigen::Matrix<double, 3, 1>; int64_t = long int]’ used but never defined [-Werror]
   virtual inline Vec3 transVelWorld(int64_t time_ns) const = 0;
                       ^~~~~~~~~~~~~
/home/alwyn/granite/thirdparty/granite-headers/include/granite/spline/se3_spline.h:151:29: error: inline function ‘const SO3& granite::Se3SplineBase<Scalar>::getKnotSO3(size_t) const [with Scalar = double; granite::Se3SplineBase<Scalar>::SO3 = Sophus::SO3<double>; size_t = long unsigned int]’ used but never defined [-Werror]
   virtual inline const SO3 &getKnotSO3(size_t i) const = 0;
                             ^~~~~~~~~~
/home/alwyn/granite/thirdparty/granite-headers/include/granite/spline/se3_spline.h:163:30: error: inline function ‘const Vec3& granite::Se3SplineBase<Scalar>::getKnotPos(size_t) const [with Scalar = double; granite::Se3SplineBase<Scalar>::Vec3 = Eigen::Matrix<double, 3, 1>; size_t = long unsigned int]’ used but never defined [-Werror]
   virtual inline const Vec3 &getKnotPos(size_t i) const = 0;
                              ^~~~~~~~~~
cc1plus: all warnings being treated as errors
CMakeFiles/granite_vio_sim.dir/build.make:62: recipe for target 'CMakeFiles/granite_vio_sim.dir/src/vio_sim.cpp.o' failed
make[2]: *** [CMakeFiles/granite_vio_sim.dir/src/vio_sim.cpp.o] Error 1
CMakeFiles/Makefile2:107: recipe for target 'CMakeFiles/granite_vio_sim.dir/all' failed
make[1]: *** [CMakeFiles/granite_vio_sim.dir/all] Error 2
Makefile:140: recipe for target 'all' failed
make: *** [all] Error 2

Calibration on monocular camera

I ran monocular camera calibration with 700+ images with April tags on basalt and granite but got (bit) worse reprojection error. I expected an improved calibration with granite when compared to basalt. Camera model ds, reprojection error in basalt is 0.254697 and granite is 0.254957.

As our target application requires accurate measurement at around 20cm from the camera, we used a custom target which is a scaled-down A0 kalibr april-tag target to A3.

Original A0 kalibr april-tag config:

"tagCols": 6,
"tagRows": 6,
"tagSize": 0.088,
"tagSpacing": 0.3

Our custom A3 kalibr april-tag config:

"tagCols": 6,
"tagRows": 6,
"tagSize": 0.03,
"tagSpacing": 0.3

compilation error

granite/src/mapper_sim.cpp:586:41: error: too few arguments to function 'std::pair<double, double> granite::alignSVD(const std::vector&, Eigen::aligned_vector<Eigen::Matrix<double, 3, 1> >&, const std::vector&, Eigen::aligned_vector<Eigen::Matrix<double, 3, 1> >&, Sophus::SE3d&, Sophus::Sim3d&, bool)'

Build error

When building this project I get the following errors:

/home/mtosas/develop/thirdparty/granite/src/rs_t265_vio.cpp:194:33: error: cannot convert ‘tbb::concurrent_bounded_queue<std::shared_ptr<granite::ImuData> >**’ to ‘tbb::concurrent_bounded_queue<std::shared_ptr<granite::ImuData> >*’ in assignment
  194 |   t265_device->imu_data_queue = &vio->imu_data_queue;
      |                                 ^~~~~~~~~~~~~~~~~~~~
      |                                 |
      |                                 tbb::concurrent_bounded_queue<std::shared_ptr<granite::ImuData> >**
/home/mtosas/develop/thirdparty/granite/src/rs_t265_vio.cpp:196:38: error: no match for ‘operator=’ (operand types are ‘tbb::concurrent_bounded_queue<std::shared_ptr<granite::OpticalFlowResult> >’ and ‘tbb::concurrent_bounded_queue<std::shared_ptr<granite::OpticalFlowResult> >**’)
  196 |   opt_flow_ptr->output_queue = &vio->vision_data_queue;
      |                                      ^~~~~~~~~~~~~~~~~
In file included from /home/mtosas/develop/thirdparty/granite/src/rs_t265_vio.cpp:61:
/usr/include/tbb/concurrent_queue.h:210:7: note: candidate: ‘tbb::concurrent_bounded_queue<std::shared_ptr<granite::OpticalFlowResult> >& tbb::concurrent_bounded_queue<std::shared_ptr<granite::OpticalFlowResult> >::operator=(const tbb::concurrent_bounded_queue<std::shared_ptr<granite::OpticalFlowResult> >&)’ <deleted>
  210 | class concurrent_bounded_queue: public internal::concurrent_queue_base_v8 {
      |       ^~~~~~~~~~~~~~~~~~~~~~~~
/usr/include/tbb/concurrent_queue.h:210:7: note:   no known conversion for argument 1 from ‘tbb::concurrent_bounded_queue<std::shared_ptr<granite::OpticalFlowResult> >**’ to ‘const tbb::concurrent_bounded_queue<std::shared_ptr<granite::OpticalFlowResult> >&’
/home/mtosas/develop/thirdparty/granite/src/rs_t265_vio.cpp:198:26: error: cannot convert ‘tbb::concurrent_bounded_queue<std::shared_ptr<granite::PoseVelBiasState> >*’ to ‘tbb::concurrent_bounded_queue<std::shared_ptr<granite::VioStateData> >*’ in assignment
  198 |   vio->out_state_queue = &out_state_queue;
      |                          ^~~~~~~~~~~~~~~~
      |                          |
      |                          tbb::concurrent_bounded_queue<std::shared_ptr<granite::PoseVelBiasState> >*
/home/mtosas/develop/thirdparty/granite/src/rs_t265_vio.cpp: In lambda function:
/home/mtosas/develop/thirdparty/granite/src/rs_t265_vio.cpp:267:48: error: base operand of ‘->’ has non-pointer type ‘tbb::concurrent_bounded_queue<std::shared_ptr<granite::OpticalFlowResult> >’
  267 |                   << opt_flow_ptr->output_queue->size() << " out_state_queue "
      |                                                ^~

How can I resolved these errors?