Giter Club home page Giter Club logo

rpg_svo_pro_open's Introduction

rpg_svo_pro

This repo includes SVO Pro which is the newest version of Semi-direct Visual Odometry (SVO) developed over the past few years at the Robotics and Perception Group (RPG). SVO was born as a fast and versatile visual front-end as described in the SVO paper (TRO-17). Since then, different extensions have been integrated through various research and industrial projects. SVO Pro features the support of different camera models, active exposure control, a sliding window based backend, and global bundle adjustment with loop closure.

In summary, this repository offers the following functionalities:

  • Visual-odometry: The most recent version of SVO that supports perspective and fisheye/catadioptric cameras in monocular or stereo setup. It also includes active exposure control.
  • Visual-inertial odometry: SVO fronted + visual-inertial sliding window optimization backend (modified from OKVIS)
  • Visual-inertial SLAM: SVO frontend + visual-inertial sliding window optimization backend + globally bundle adjusted map (using iSAM2). The global map is updated in real-time, thanks to iSAM2, and used for localization at frame-rate.
  • Visual-inertial SLAM with loop closure: Loop closures, via DBoW2, are integrated in the global bundle adjustment. Pose graph optimization is also included as a lightweight replacement of the global bundle adjustment.

An example of the visual-inertial SLAM pipeline on EuRoC dataset is below (green points - sliding window; blue points - iSAM2 map):

SVO Pro and its extensions have been used to support various projects at RPG, such as our recent work on multiple camera SLAM, voxel map for visual SLAM and the tight-coupling of global positional measurements into VIO. We hope that the efforts we made can facilitate the research and applications of SLAM and spatial perception.

License

The code is licensed under GPLv3. For commercial use, please contact sdavide [at] ifi [dot] uzh [dot] ch.

The visual-inertial backend is modified from OKVIS, and the license is retained at the beginning of the related files.

Credits

If you use the code in the academic context, please cite:

  • Christian Forster, Matia Pizzoli, Davide Scaramuzza. SVO: Fast Semi-Direct Monocular Visual Odometry. ICRA, 2014. bibtex
  • Christian Forster, Zichao Zhang, Michael Gassner, Manuel Werlberger, Davide Scaramuzza. SVO: Semi-Direct Visual Odometry for Monocular and Multi-Camera Systems. TRO, 2017. bibtex

Additionally, please cite the following papers for the specific extensions you make use of:

  • Fisheye/catadioptric camera extension: Zichao Zhang, Henri Rebecq, Christian Forster, Davide Scaramuzza. Benefit of Large Field-of-View Cameras for Visual Odometry. ICRA, 2016. bibtex
  • Brightness/exposure compensation: Zichao Zhang, Christian Forster, Davide Scaramuzza. Active Exposure Control for Robust Visual Odometry in HDR Environments. ICRA, 2017. bibtex
  • Ceres-based optimization backend: Stefan Leutenegger, Simon Lynen, Michael Bosse, Roland Siegwart, Paul Timothy Furgale. Keyframe-based visual–inertial odometry using nonlinear optimization. IJRR, 2015. bibtex
  • Global map powered by iSAM2: Michael Kaess, Hordur Johannsson, Richard Roberts, Viorela Ila, John Leonard, Frank Dellaert. iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree. IJRR, 2012. bibtex
  • Loop closure: Dorian Gálvez-López and Juan D. Tardós. Bags of Binary Words for Fast Place Recognition in Image Sequences. TRO, 2012. bibtex

Our recent publications that use SVO Pro are:

  • Multiple camera SLAM: Juichung Kuo, Manasi Muglikar, Zichao Zhang, Davide Scaramuzza. Redesigning SLAM for Arbitrary Multi-Camera Systems. ICRA, 2020. bibtex
  • Voxel map for visual SLAM: Manasi Muglikar, Zichao Zhang, Davide Scaramuzza. Voxel Map for Visual SLAM. ICRA, 2020. bibtex
  • Tight-coupling of global positional measurements into VIO: Giovanni Cioffi, Davide Scaramuzza. Tightly-coupled Fusion of Global Positional Measurements in Optimization-based Visual-Inertial Odometry. IROS, 2020. bibtex

Install

The code has been tested on

  • Ubuntu 18.04 with ROS Melodic
  • Ubuntu 20.04 with ROS Noetic

Install dependences

Install catkin tools and vcstools if you haven't done so before. Depending on your operating system, run

# For Ubuntu 18.04 + Melodic
sudo apt-get install python-catkin-tools python-vcstool

or

# For Ubuntu 20.04 + Noetic
sudo apt-get install python3-catkin-tools python3-vcstool python3-osrf-pycommon

Install system dependencies and dependencies for Ceres Solver

# system dep.
sudo apt-get install libglew-dev libopencv-dev libyaml-cpp-dev 
# Ceres dep.
sudo apt-get install libblas-dev liblapack-dev libsuitesparse-dev

Clone and compile

Create a workspace and clone the code (ROS-DISTRO=melodic/noetic):

mkdir svo_ws && cd svo_ws
# see below for the reason for specifying the eigen path
catkin config --init --mkdirs --extend /opt/ros/<ROS-DISTRO> --cmake-args -DCMAKE_BUILD_TYPE=Release -DEIGEN3_INCLUDE_DIR=/usr/include/eigen3
cd src
git clone [email protected]:uzh-rpg/rpg_svo_pro_open.git
vcs-import < ./rpg_svo_pro_open/dependencies.yaml
touch minkindr/minkindr_python/CATKIN_IGNORE
# vocabulary for place recognition
cd rpg_svo_pro_open/svo_online_loopclosing/vocabularies && ./download_voc.sh
cd ../../..

There are two types of builds that you can proceed from here

  1. Build without the global map (front-end + sliding window back-end + loop closure/pose graph)

    catkin build
  2. Build with the global map using iSAM2 (all functionalities)

    First, enable the global map feature

    rm rpg_svo_pro_open/svo_global_map/CATKIN_IGNORE

    and in svo_cmake/cmake/Modules/SvoSetup.cmake

    SET(USE_GLOBAL_MAP TRUE)

    Second, clone GTSAM

    git clone --branch 4.0.3 [email protected]:borglab/gtsam.git

    and modify GTSAM compilation flags a bit:

    # 1. gtsam/CMakelists.txt: use system Eigen
    -option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF)
    +option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" ON)
    # 2. gtsam/cmake/GtsamBuildTypes: disable avx instruction set
    # below the line `list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC "-march=native")`
    list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC "-mno-avx")

    Using the same version of Eigen helps avoid memory issues. Disabling avx instruction set also helps with some segment faults in our experience (this can be however OS and hardware dependent).

    And finally build the whole workspace

    # building GTSAM may take a while
    catkin build

Instructions

Troubleshooting

  1. Weird building issues after some tinkering. It is recommend to

    • clean your workspace (catkin clean --all at the workspace root) and rebuild your workspace (catkin build)
    • or catkin build --force-cmake

    after your have made changes to CMake files (CMakeLists.txt or *.cmake) to make sure the changes take effect.

    Longer explanation Catkin tools can detect changes in CMake files and re-build affected files only. But since we are working with a multi-package project, some changes may not be detected as desired. For example, changing the building flags in `svo_cmake/cmake/Modules/SvoSetup.cmake` will affect all the packages but the re-compiling may not be done automatically (since the files in each package are not changed). Also, we need to keep the linking (e.g., library version) and compiling flags consistent across different packages. Therefore, unless you are familiar with how the compilation works out, it is the safest to re-build the whole workspace. `catkin build --force-cmake` should also work in most cases.
  2. Compiling/linking error related to OpenCV: find find_package(OpenCV REQUIRED) in the CMakeLists.txt files in each package (in rpg_common, svo_ros, svo_direct, vikit/vikit_common and svo_online_loopclosing) and replace it with

    # Ubuntu 18.04 + Melodic
    find_package(OpenCV 3 REQUIRED)
    # Ubuntu 20.04 + Noetic
    find_package(OpenCV 4 REQUIRED)
    Longer explanation First, ROS is built against OpenCV 3 on Ubuntu 18.04 and OpenCV 4 on Ubuntu 20.04. It is desired to keep the OpenCV version linked in SVO consistent with the ROS one, since in `svo_ros` we need to link everything with ROS. Second, The original `CMakeLists.txt` files will work fine if you only have the default OpenCV installed. But if you have some customized version of OpenCV installed (e.g., from source), it is recommended to explicitly specify the version of OpenCV that should be used (=the version ROS uses) as mentione above.
  3. Visualization issues with the PointCloud2: Using Points to visualize PointCloud2 in RVIZ seems to be problematic in Ubuntu 20.04. We use other visualization types instead of Points per default. However, it is good to be aware of this if you want to customize the visualization.

  4. Pipeline crashes with loop closure enabled: If the pipeline crashes calling svo::loadVoc(), did you forgot to download the vocabulary files as mentioned above?

    cd rpg_svo_pro_open/svo_online_loopclosing/vocabularies && ./download_voc.sh
  5. Inconsistent Eigen versions during compilation: The same Eigen should be used across the whole project (which should be system Eigen, since we are also using ROS). Check whether eigen_catkin and gtsam find the same version of Eigen:

    # for eigen_catkin
    catkin build eigen_catkin --force-cmake --verbose
    # for gtsam
    catkin build gtsam --force-cmake --verbose
    Longer explanation One common pitfall of using Eigen in your projects is have different libraries compiled against different Eigen versions. For SVO, eigen_catkin (https://github.com/ethz-asl/eigen_catkin) is used to keep the Eigen version same, which should be the system one (under /usr/include) on 18.04 and 20.04. For GTSAM, system Eigen is found via a cumstomized cmake file (https://github.com/borglab/gtsam/blob/develop/cmake/FindEigen3.cmake#L66). It searches for `/usr/local/include` first, which may contain Eigen versions that are manually installed. Therefore, we explicitly specifies `EIGEN_INCLUDE_PATH` when configuring the workspace to force GTSAM to find system Eigen. If you still encounter inconsistent Eigen versions, the first thing to check is whether different versions of Eigen are still used.

Acknowledgement

Thanks to Simon Klenk, Manasi Muglikar, Giovanni Cioffi and Javier Hidalgo-Carrió for their valuable help and comments for the open source code.

The work is made possible thanks to the efforts of many contributors from RPG. Apart from the authors listed in the above papers, Titus Cieslewski and Henri Rebecq made significant contributions to the visual front-end. Jeffrey Delmerico made great efforts to apply SVO on different real robots, which in turn helped improve the pipeline. Many PhD and master students and lab engineers have also contributed to the code.

The Ceres-based optimization back-end is based on code developed at Zurich-eye, a spin-off from RPG. Jonathan Huber is the main contributor that integrated the back-end with SVO. Kunal Shrivastava (now CEO of SUIND) developed the loop closure module during his semester project and internship at RPG. The integration of the iSAM2-based global map was developed by Zichao Zhang.

We would like to thank our collaborators at Prophesee for pointing out several bugs in the visual front-end. Part of the code was developed during a funded project with Huawei.

rpg_svo_pro_open's People

Contributors

davsca avatar gcioffi avatar liulimingcode avatar sudo-robot-destroy avatar

Stargazers

 avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar

Watchers

 avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar

rpg_svo_pro_open's Issues

catkin build error

svo_ws/src/rpg_svo_pro_open/svo_ceres_backend/src/ceres_backend_interface.cpp:16:14: error: expected constructor, destructor, or type conversion before ‘(’ token
16 | DEFINE_double(FLAGS_extrinsics_sigma_rel_translation, 0.0,

svo_ws/src/rpg_svo_pro_open/svo_ceres_backend/src/ceres_backend_interface.cpp:50:40: error: ‘FLAGS_extrinsics_sigma_rel_translation’ was not declared in this scope

SVO crashed when running stereo imu frontend vio

Today I'm trying to test stereo frontend vio of SVO on ARM platform(RK3399 ubuntu20.04 ROS noetic).
I followed instructions of doc/frontend/visual_frontend.md
And SVO crashed in few seconds.
Below is the stack trace info:
[ INFO] [1637825128.329917831]: Img Align: Maximum Number of Features = 540
[ INFO] [1637825128.329973246]: ref_frame: 88
[ INFO] [1637825128.330030411]: fts.szie = 59
[ INFO] [1637825128.330105950]: Img Align: Maximum Number of Features = 88
[ INFO] [1637825128.330148240]: Img Align: Tracking 117 features.
*** Aborted at 1637825128 (unix time) try "date -d @1637825128" if you are using GNU date ***
PC: @ 0x0 (unknown)
*** SIGSEGV (@0xfffffc23288f) received by PID 26633 (TID 0xffff87ff3680) from PID 18446744073644746895; stack trace: ***
@ 0xffffa5314bb8 google::(anonymous namespace)::FailureSignalHandler()
@ 0xffffa6248588 ([vdso]+0x587)
@ 0xffffa559415c svo::warp::warpAffine()
@ 0xffffa55b28a4 svo::Matcher::findEpipolarMatchDirect()
@ 0xffffa55b6ee4 svo::depth_filter_utils::updateSeed()
@ 0xffffa55b7e18 svo::DepthFilter::updateSeeds()
@ 0xffffa564cc70 svo::FrameHandlerStereo::makeKeyframe()
@ 0xffffa564b50c svo::FrameHandlerStereo::processFrame()
@ 0xffffa56663c4 svo::FrameHandlerBase::addFrameBundle()
@ 0xffffa5667ab0 svo::FrameHandlerBase::addImageBundle()
@ 0xffffa5f8cefc svo::SvoInterface::processImageBundle()
@ 0xffffa5f8f5b0 svo::SvoInterface::stereoCallback()
@ 0xffffa5fa14c4 boost::detail::function::void_function_obj_invoker9<>::invoke()
@ 0xffffa5fab7a8 message_filters::CallbackHelper9T<>::call()
@ 0xffffa5fa5794 message_filters::sync_policies::ExactTime<>::checkTuple()
@ 0xffffa5fa719c message_filters::Synchronizer<>::cb<>()
@ 0xffffa5fa221c image_transport::SubscriberFilter::cb()
@ 0xffff94229c88 image_transport::RawSubscriber::internalCallback()
@ 0xffffa595c2bc boost::detail::function::void_function_obj_invoker1<>::invoke()
@ 0xffff9422db30 ros::SubscriptionCallbackHelperT<>::call()
@ 0xffffa61b4264 ros::SubscriptionQueue::call()
@ 0xffffa6160e64 ros::CallbackQueue::callOneCB()
@ 0xffffa616258c ros::CallbackQueue::callAvailable()
@ 0xffffa5f91068 svo::SvoInterface::stereoLoop()
@ 0xffffa5e3b76c (unknown)
@ 0xffffa60084fc start_thread
@ 0xffffa5cacf2c (unknown)
[svo-1] process has died [pid 26633, exit code -11, cmd /home/khadas/svo_ws/devel/lib/svo_ros/svo_node __name:=svo __log:=/home/khadas/.ros/log/3e96beba-4db7-11ec-9700-c863147043e2/svo-1.log].
log file: /home/khadas/.ros/log/3e96beba-4db7-11ec-9700-c863147043e2/svo-1*.log

And This is the code of crashed function:
bool warpAffine(
const AffineTransformation2& A_cur_ref,
const cv::Mat& img_ref,
const Eigen::Ref& px_ref,
const int level_ref,
const int search_level,
const int halfpatch_size,
uint8_t* patch)
{
Eigen::Matrix2f A_ref_cur = A_cur_ref.inverse().cast()*(1<<search_level);
if(std::isnan(A_ref_cur(0,0)))
{
LOG(WARNING) << "Affine warp is NaN, probably camera has no translation";
return false;
}

// Perform the warp on a larger patch.
uint8_t* patch_ptr = patch;
const Eigen::Vector2f px_ref_pyr = px_ref.cast() / (1<<level_ref);
const int stride = img_ref.step.p[0];
for (int y=-halfpatch_size; y<halfpatch_size; ++y)
{
for (int x=-halfpatch_size; x<halfpatch_size; ++x, ++patch_ptr)
{
const Eigen::Vector2f px_patch(x, y);
const Eigen::Vector2f px(A_ref_curpx_patch + px_ref_pyr);
const int xi = std::floor(px[0]);
const int yi = std::floor(px[1]);
if (xi<0 || yi<0 || xi+1>=img_ref.cols || yi+1>=img_ref.rows)
return false;
else
{
const float subpix_x = px[0]-xi;
const float subpix_y = px[1]-yi;
const float w00 = (1.0f-subpix_x)
(1.0f-subpix_y);
const float w01 = (1.0f-subpix_x)subpix_y;
const float w10 = subpix_x
(1.0f-subpix_y);
const float w11 = 1.0f - w00 - w01 - w10;
const uint8_t* const ptr = img_ref.data + yistride + xi;
patch_ptr = static_cast<uint8_t>(w00ptr[0] + w01
ptr[stride] + w10ptr[1] + w11ptr[stride+1]);
}
}
}
return true;
}

it seems that the pointer is out of the patch border.

Does anybody know why????? Thx.

libsvo.so libsvo_ceres_backend.so

Hello everyone, I encountered the following problems when executing the command

building GTSAM may take a while

catkin build
and I checked the relevant information. I found out that it is not a problem of undefined virtual functions , and there is no problem with opengv compilation. Could you tell me what caused this problem? thank you very much

/home/li/svo_ws2.0/devel/.private/svo/lib/libsvo.so: undefined reference to vtable for opengv::sac_problems::relative_pose::TranslationOnlySacProblem' /home/li/svo_ws2.0/devel/.private/svo/lib/libsvo.so: undefined reference to opengv::relative_pose::CentralRelativeAdapter::CentralRelativeAdapter(std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>,
Eigen::aligned_allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > > const&, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>,
Eigen::aligned_allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > > const&)'
/home/li/svo_ws2.0/devel/.private/svo/lib/libsvo.so: undefined reference to
opengv::relative_pose::CentralRelativeAdapter::CentralRelativeAdapter(std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > > const&, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > > const&, Eigen::Matrix<double, 3, 3, 0, 3, 3> const&)' /home/li/svo_ws2.0/devel/.private/svo/lib/libsvo.so: undefined reference to opengv::relative_pose::CentralRelativeAdapter::getNumberCorrespondences() const'
/home/li/svo_ws2.0/devel/.private/svo/lib/libsvo.so: undefined reference to
opengv::sac_problems::relative_pose::TranslationOnlySacProblem::computeModelCoefficients(std::vector<int, std::allocator<int> > const&, Eigen::Matrix<double, 3, 4, 0, 3, 4>&) const' /home/li/svo_ws2.0/devel/.private/svo/lib/libsvo.so: undefined reference to opengv::triangulation::triangulate2(opengv::relative_pose::RelativeAdapterBase const&, unsigned long)'
/home/li/svo_ws2.0/devel/.private/svo/lib/libsvo.so: undefined reference to
opengv::sac_problems::relative_pose::TranslationOnlySacProblem::optimizeModelCoefficients(std::vector<int, std::allocator<int> > const&, Eigen::Matrix<double, 3, 4, 0, 3, 4> const&, Eigen::Matrix<double, 3, 4, 0, 3, 4>&)' /home/li/svo_ws2.0/devel/.private/svo/lib/libsvo.so: undefined reference to opengv::relative_pose::CentralRelativeAdapter::~CentralRelativeAdapter()'
/home/li/svo_ws2.0/devel/.private/svo_ceres_backend/lib/libsvo_ceres_backend.so: undefined reference to vtable for opengv::sac_problems::point_cloud::PointCloudSacProblem' /home/li/svo_ws2.0/devel/.private/svo_ceres_backend/lib/libsvo_ceres_backend.so: undefined reference to opengv::point_cloud::PointCloudAdapter::getNumberCorrespondences() const'
/home/li/svo_ws2.0/devel/.private/svo/lib/libsvo.so: undefined reference to
opengv::sac_problems::relative_pose::TranslationOnlySacProblem::getSampleSize() const' /home/li/svo_ws2.0/devel/.private/svo_ceres_backend/lib/libsvo_ceres_backend.so: undefined reference to opengv::point_cloud::PointCloudAdapter::~PointCloudAdapter()'
/home/li/svo_ws2.0/devel/.private/svo/lib/libsvo.so: undefined reference to vtable for opengv::sac_problems::relative_pose::CentralRelativePoseSacProblem' /home/li/svo_ws2.0/devel/.private/svo/lib/libsvo.so: undefined reference to typeinfo for
opengv::sac_problems::relative_pose::TranslationOnlySacProblem'
/home/li/svo_ws2.0/devel/.private/svo_ceres_backend/lib/libsvo_ceres_backend.so: undefined reference to
`opengv::point_cloud::PointCloudAdapter::PointCloudAdapter(std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>,
Eigen::aligned_allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > > const&, std::vector<Eigen::Matrix<double, 3, 1, 0, 3, 1>,
Eigen::aligned_allocator<Eigen::Matrix<double, 3, 1, 0, 3, 1> > > const&)'
collect2: error: ld returned 1 exit status
make[2]: *** [/home/li/svo_ws2.0/devel/.private/svo_ros/lib/svo_ros/svo_node] Error 1
make[1]: *** [CMakeFiles/svo_node.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....

yaml-serialization.h:42 Key "label" does not exist

is this correct?

Start experiments for MH_01_easy...
Will do 1 MC runs.
Trace will be saved in /home/lhm/workspace/slam/svo_ws/src/rpg_svo_pro_open/svo_benchmarking/results/20211104_155414_test/MH_01_easy.
Copy data/groundtruth.txt to the trace folder.
Will write evaluation config:{'align_type': 'posyaw', 'align_num_frames': -1}
Trial 0...
Flags are {'v': 0, 'logtostderr': 0, 'trial_idx': -1}
Will log CPU Usage
Start ROS node
[ INFO] [1636012454.884895131]: Found parameter: dataset_directory, value: /home/lhm/workspace/slam/svo_ws/src/rpg_svo_pro_open/svo_benchmarking/data/MH_01_easy
[ WARN] [1636012454.886252130]: Cannot find value for parameter: dataset_is_kitti, assigning default: 0
[ INFO] [1636012454.887359362]: Found parameter: dataset_is_blender, value: 0
[ INFO] [1636012454.888616096]: Found parameter: dataset_is_stereo, value: 0
[ WARN] [1636012454.889080700]: Cannot find value for parameter: set_initial_attitude_from_gravity, assigning default: 1
[ INFO] [1636012454.890593570]: Found parameter: automatic_reinitialization, value: 1
[ INFO] [1636012454.891696981]: Found parameter: calib_file, value: /home/lhm/workspace/slam/svo_ws/src/rpg_svo_pro_open/svo_benchmarking/data/MH_01_easy/calib.yaml
E1104 15:54:14.892223 520922 yaml-serialization.h:42] Key "label" does not exist
E1104 15:54:14.892793 520922 ncamera_yaml_serialization.cpp:23] Unable to get the label for the ncamera.
*** Aborted at 1636012454 (unix time) try "date -d @1636012454" if you are using GNU date ***
PC: @ 0x7fedb3e27ac0 vk::cameras::NCamera::loadFromYaml()
*** SIGSEGV (@0x20) received by PID 520922 (TID 0x7feda5e37a00) from PID 32; stack trace: ***
@ 0x7fedb3dc6781 google::(anonymous namespace)::FailureSignalHandler()
@ 0x7fedb3281210 (unknown)
@ 0x7fedb3e27ac0 vk::cameras::NCamera::loadFromYaml()
@ 0x7fedb369691b svo::factory::loadCameraFromYaml()
@ 0x7fedb3698a7c svo::factory::makeMono()
@ 0x7fedb3670898 svo::SvoInterface::SvoInterface()
@ 0x55e434beaac0 svo::BenchmarkNode::BenchmarkNode()
@ 0x55e434be27d3 main
@ 0x7fedb32620b3 __libc_start_main
@ 0x55e434be2d2e _start
Segmentation fault (core dumped)
ROS node finished processing.

Installation error caused by yaml-cpp

Was not able to build everything, since the yaml-cpp files weren't compatible. Example error message:

undefined reference to YAML::detail::

The solution was to downgrade the yaml installation. The working version was "yaml-cpp-0.6.0"
With that version, everything compiled fine

Error in svo/loop_closures

We are attempting to use the vio with a VN100 imu and a Realsense D435 camera. We ran the calibration using the kalibr package and after close inspection it seems that the resulting calibration is good. In the svo_ros/launch/euroroc_vio_mono.launch file we have replaced the calibr_file param to point to our own (see attached realsense_test.txt - converted from yaml for attaching to this post) and of course we have updated the cam0_topic and imu_topic params.

The file launches and runs properly and starts the vio process. However, when we start moving the camera + imu it quickly diverges. The only clear error we receive is in the rviz window:
loop_query/0 Uninitialized quaternion, assuming identity. Points should not be empty for specified marker type
loop_detection/0 Uninitialized quaternion, assuming identity. Points should not be empty for specified marker type
loop_correction/0 Uninitialized quaternion, assuming identity. Points should not be empty for specified marker type

The process also dies after some time once it diverges. The error here is F1104 10:41:38.296813 11173 estimator.cpp:167] Check failed: num_used_imu_measurements > 1 (1 vs. 1) No imu measurements is used for reinitialization. Something wrong with the IMU bookkeeping. However we are certain the IMU is constantly publishing
realsense_test.txt

Can I run SVO simply on stereo without IMU?

Currently I can run SVO on monocular sensing successfully? And I would further run it on stereo setup for comparison on my own dataset as done in your paper. However, it seems that all launching scripts related with stereo sensing are all performed with IMU (loosely coupled in frontend and tightly coupled). So, I would like to ask if this repo supports any implementation of runnning SVO simply based on stereo camera without IMU measurement?
Thanks in advance.

trajectory evaluation

Hello, I would like to ask, if the trajectory is evaluated, is there any relevant file generated?

Multicamera support

It seems that support for a multicamera (non-stereo) system was available:

  • initialization classes ArrayInitGeometric and ArrayInitOptimization
  • enum PipelineType::kArray
    but, by looking at the source code of ArrayInitGeometric, ArrayInitOptimization I can see commented code and TODOs.

Is multicamera support currently deprecated/not maintained?
If the problems in the initialization procedure will be fixed, would the multicamera configuration work? Or further work is required?

How to transform pose from camera frame to world frame?

topic /pose_cam/0 outputs camera pose obviously under camera frame. This note says that "The camera position in world coordinates must be obtained by inversion". However in visualizer.cpp where topic /pose_cam/0 is published, I tried to adapt inversion, but nothing changed.
This is how I code:
`void Visualizer::publishCameraPoses(const FrameBundlePtr& frame_bundle,
const int64_t timestamp_nanoseconds)
{
vk::output_helper::publishTfTransform(
frame_bundle->at(0)->T_cam_world(),
ros::Time().fromNSec(timestamp_nanoseconds), "cam_pos", kWorldFrame, br_);

for (size_t i = 0; i < frame_bundle->size(); ++i)
{
// if (pub_cam_poses_.at(i).getNumSubscribers() == 0)
// return;
VLOG(100) << "Publish camera pose " << i;

const FramePtr& frame = frame_bundle->at(i);
Transformation T_world_from_cam(frame->T_f_w_.inverse());
Eigen::Quaterniond q =
    T_world_from_cam.getRotation().toImplementation();
Eigen::Vector3d p = T_world_from_cam.getPosition();
geometry_msgs::PoseStampedPtr msg_pose(new geometry_msgs::PoseStamped);
msg_pose->header.seq = trace_id_;
msg_pose->header.stamp = ros::Time().fromNSec(timestamp_nanoseconds);
msg_pose->header.frame_id = "cam" + std::to_string(i);
msg_pose->pose.position.x = p[0];
msg_pose->pose.position.y = p[1];
msg_pose->pose.position.z = p[2];
msg_pose->pose.orientation.x = q.x();
msg_pose->pose.orientation.y = q.y();
msg_pose->pose.orientation.z = q.z();
msg_pose->pose.orientation.w = q.w();
pub_cam_poses_.at(i).publish(msg_pose);

}
}`

And, if I get it right, its original code that uses function T_world_cam() performed transformation already. So why the outputs were not so? And it seems like publisher of /pose_cam/0 only appeared in visualizer.cpp.

How to transform the camera pose to world frame? Which file should I edit? Or which topic should I look up to?

point cloud intensity and seed variance

what is the meaning of intensity values of points in the point cloud?
Is it measurement accuracy? Is there any relation between intensity and seed variances ?

Question about the `/svo/pose_imu` without including covariance matrix

I'd like to use the pose estimate by SVO Pro foir further use, which requires the covariance matrix along with the pose estimate. However, even I find you used geometry_msgs::posewithcovariancestamped as the message type of imu pose, the covariance part is all zeros. Is that what you planned in your algorithm or should I do something in the configuartion file (I actually haven't found any)?
Currently, I am using your vio setup by virtue of the configuration file (https://github.com/uzh-rpg/rpg_svo_pro_open/blob/master/svo_ros/param/vio_mono.yaml)
Thanks in advance for your help!

Installation Error

I followed the installation guide in the documentation these are the following error i found:

  • The git clone link tells there is public key error, so i had to clone it and it solved
  • The dependencies .yaml gives me the same public key error, though I have to search for induvial repos, is there a way it can be done automatically?

catkin build failed under Ubuntu 16.04

`zyj@zyj-ubuntu16:~/svo_ws/src$ catkin build

Profile: default
Extending: [explicit] /opt/ros/kinetic
Workspace: /home/zyj/svo_ws

Build Space: [exists] /home/zyj/svo_ws/build
Devel Space: [exists] /home/zyj/svo_ws/devel
Install Space: [unused] /home/zyj/svo_ws/install
Log Space: [missing] /home/zyj/svo_ws/logs
Source Space: [exists] /home/zyj/svo_ws/src
DESTDIR: [unused] None

Devel Space Layout: linked
Install Space Layout: None

Additional CMake Args: -DCMAKE_BUILD_TYPE=Release -DEIGEN3_INCLUDE_DIR=/usr/include/eigen3
Additional Make Args: None
Additional catkin Make Args: None
Internal Make Job Server: True
Cache Job Environments: False

Whitelisted Packages: None
Blacklisted Packages: None

Workspace configuration appears valid.

NOTE: Forcing CMake to run for each package.

[build] Found '25' packages in 0.0 seconds.
[build] Updating package table.
Starting >>> catkin_tools_prebuild


Warnings << catkin_tools_prebuild:cmake /home/zyj/svo_ws/logs/catkin_tools_prebuild/build.cmake.000.log
CMake Deprecation Warning at CMakeLists.txt:1 (cmake_minimum_required):
Compatibility with CMake < 2.8.12 will be removed from a future version of
CMake.

Update the VERSION argument value or use a ... suffix to tell
CMake that the project does not need compatibility with older versions.

CMake Deprecation Warning at /usr/src/gmock/CMakeLists.txt:41 (cmake_minimum_required):
Compatibility with CMake < 2.8.12 will be removed from a future version of
CMake.

Update the VERSION argument value or use a ... suffix to tell
CMake that the project does not need compatibility with older versions.

CMake Deprecation Warning at /usr/src/gtest/CMakeLists.txt:43 (cmake_minimum_required):
Compatibility with CMake < 2.8.12 will be removed from a future version of
CMake.

Update the VERSION argument value or use a ... suffix to tell
CMake that the project does not need compatibility with older versions.

cd /home/zyj/svo_ws/build/catkin_tools_prebuild; catkin build --get-env catkin_tools_prebuild | catkin env -si /usr/bin/cmake /home/zyj/svo_ws/build/catkin_tools_prebuild --no-warn-unused-cli -DCATKIN_DEVEL_PREFIX=/home/zyj/svo_ws/devel/.private/catkin_tools_prebuild -DCMAKE_INSTALL_PREFIX=/home/zyj/svo_ws/install -DCMAKE_BUILD_TYPE=Release -DEIGEN3_INCLUDE_DIR=/usr/include/eigen3; cd -
......................................................................................................................................................................................................................................
Finished <<< catkin_tools_prebuild [ 1.0 seconds ]
Starting >>> cmake_external_project_catkin
Starting >>> eigen_checks
Starting >>> rpg_trajectory_evaluation
Starting >>> rqt_svo
Starting >>> svo_cmake
Starting >>> svo_msgs
Starting >>> vikit_py
Starting >>> vikit_solver


Errors << vikit_solver:cmake /home/zyj/svo_ws/logs/vikit_solver/build.cmake.000.log
CMake Deprecation Warning at CMakeLists.txt:1 (cmake_minimum_required):
Compatibility with CMake < 2.8.12 will be removed from a future version of
CMake.

Update the VERSION argument value or use a ... suffix to tell
CMake that the project does not need compatibility with older versions.

CMake Error at /home/zyj/svo_ws/src/rpg_svo_pro_open/vikit/vikit_solver/CMakeLists.txt:3 (find_package):
By not providing "Findcatkin_simple.cmake" in CMAKE_MODULE_PATH this
project has asked CMake to find a package configuration file provided by
"catkin_simple", but CMake did not find one.

Could not find a package configuration file provided by "catkin_simple"
with any of the following names:

catkin_simpleConfig.cmake
catkin_simple-config.cmake

Add the installation prefix of "catkin_simple" to CMAKE_PREFIX_PATH or set
"catkin_simple_DIR" to a directory containing one of the above files. If
"catkin_simple" provides a separate development package or SDK, be sure it
has been installed.

cd /home/zyj/svo_ws/build/vikit_solver; catkin build --get-env vikit_solver | catkin env -si /usr/bin/cmake /home/zyj/svo_ws/src/rpg_svo_pro_open/vikit/vikit_solver --no-warn-unused-cli -DCATKIN_DEVEL_PREFIX=/home/zyj/svo_ws/devel/.private/vikit_solver -DCMAKE_INSTALL_PREFIX=/home/zyj/svo_ws/install -DCMAKE_BUILD_TYPE=Release -DEIGEN3_INCLUDE_DIR=/usr/include/eigen3; cd -
......................................................................................................................................................................................................................................
Failed << vikit_solver:cmake [ Exited with code 1 ]
Failed <<< vikit_solver [ 0.4 seconds ]
Abandoned <<< minkindr [ Unrelated job failed ]
Abandoned <<< rpg_common [ Unrelated job failed ]
Abandoned <<< vikit_common [ Unrelated job failed ]
Abandoned <<< vikit_cameras [ Unrelated job failed ]
Abandoned <<< svo_common [ Unrelated job failed ]
Abandoned <<< svo_pgo [ Unrelated job failed ]
Abandoned <<< svo_online_loopclosing [ Unrelated job failed ]
Abandoned <<< svo_vio_common [ Unrelated job failed ]
Abandoned <<< vikit_ros [ Unrelated job failed ]
Abandoned <<< svo_test_utils [ Unrelated job failed ]
Abandoned <<< svo_direct [ Unrelated job failed ]
Abandoned <<< svo_img_align [ Unrelated job failed ]
Abandoned <<< svo_tracker [ Unrelated job failed ]
Abandoned <<< svo [ Unrelated job failed ]
Abandoned <<< svo_ceres_backend [ Unrelated job failed ]
Abandoned <<< svo_ros [ Unrelated job failed ]
Abandoned <<< svo_benchmarking [ Unrelated job failed ]


Errors << eigen_checks:cmake /home/zyj/svo_ws/logs/eigen_checks/build.cmake.000.log
CMake Deprecation Warning at CMakeLists.txt:25 (cmake_minimum_required):
Compatibility with CMake < 2.8.12 will be removed from a future version of
CMake.

Update the VERSION argument value or use a ... suffix to tell
CMake that the project does not need compatibility with older versions.

CMake Error at /home/zyj/svo_ws/src/eigen_checks/CMakeLists.txt:28 (find_package):
By not providing "Findcatkin_simple.cmake" in CMAKE_MODULE_PATH this
project has asked CMake to find a package configuration file provided by
"catkin_simple", but CMake did not find one.

Could not find a package configuration file provided by "catkin_simple"
with any of the following names:

catkin_simpleConfig.cmake
catkin_simple-config.cmake

Add the installation prefix of "catkin_simple" to CMAKE_PREFIX_PATH or set
"catkin_simple_DIR" to a directory containing one of the above files. If
"catkin_simple" provides a separate development package or SDK, be sure it
has been installed.

cd /home/zyj/svo_ws/build/eigen_checks; catkin build --get-env eigen_checks | catkin env -si /usr/bin/cmake /home/zyj/svo_ws/src/eigen_checks --no-warn-unused-cli -DCATKIN_DEVEL_PREFIX=/home/zyj/svo_ws/devel/.private/eigen_checks -DCMAKE_INSTALL_PREFIX=/home/zyj/svo_ws/install -DCMAKE_BUILD_TYPE=Release -DEIGEN3_INCLUDE_DIR=/usr/include/eigen3; cd -
......................................................................................................................................................................................................................................
Failed << eigen_checks:cmake [ Exited with code 1 ]
Failed <<< eigen_checks [ 0.4 seconds ]


Errors << rpg_trajectory_evaluation:cmake /home/zyj/svo_ws/logs/rpg_trajectory_evaluation/build.cmake.000.log
CMake Deprecation Warning at CMakeLists.txt:1 (cmake_minimum_required):
Compatibility with CMake < 2.8.12 will be removed from a future version of
CMake.

Update the VERSION argument value or use a ... suffix to tell
CMake that the project does not need compatibility with older versions.

CMake Error at /home/zyj/svo_ws/src/rpg_trajectory_evaluation/CMakeLists.txt:4 (find_package):
By not providing "Findcatkin_simple.cmake" in CMAKE_MODULE_PATH this
project has asked CMake to find a package configuration file provided by
"catkin_simple", but CMake did not find one.

Could not find a package configuration file provided by "catkin_simple"
with any of the following names:

catkin_simpleConfig.cmake
catkin_simple-config.cmake

Add the installation prefix of "catkin_simple" to CMAKE_PREFIX_PATH or set
"catkin_simple_DIR" to a directory containing one of the above files. If
"catkin_simple" provides a separate development package or SDK, be sure it
has been installed.

cd /home/zyj/svo_ws/build/rpg_trajectory_evaluation; catkin build --get-env rpg_trajectory_evaluation | catkin env -si /usr/bin/cmake /home/zyj/svo_ws/src/rpg_trajectory_evaluation --no-warn-unused-cli -DCATKIN_DEVEL_PREFIX=/home/zyj/svo_ws/devel/.private/rpg_trajectory_evaluation -DCMAKE_INSTALL_PREFIX=/home/zyj/svo_ws/install -DCMAKE_BUILD_TYPE=Release -DEIGEN3_INCLUDE_DIR=/usr/include/eigen3; cd -
......................................................................................................................................................................................................................................
Failed << rpg_trajectory_evaluation:cmake [ Exited with code 1 ]
Failed <<< rpg_trajectory_evaluation [ 0.4 seconds ]


Errors << svo_cmake:cmake /home/zyj/svo_ws/logs/svo_cmake/build.cmake.000.log
CMake Deprecation Warning at CMakeLists.txt:1 (cmake_minimum_required):
Compatibility with CMake < 2.8.12 will be removed from a future version of
CMake.

Update the VERSION argument value or use a ... suffix to tell
CMake that the project does not need compatibility with older versions.

CMake Error at /home/zyj/svo_ws/src/rpg_svo_pro_open/svo_cmake/CMakeLists.txt:4 (find_package):
By not providing "Findcatkin_simple.cmake" in CMAKE_MODULE_PATH this
project has asked CMake to find a package configuration file provided by
"catkin_simple", but CMake did not find one.

Could not find a package configuration file provided by "catkin_simple"
with any of the following names:

catkin_simpleConfig.cmake
catkin_simple-config.cmake

Add the installation prefix of "catkin_simple" to CMAKE_PREFIX_PATH or set
"catkin_simple_DIR" to a directory containing one of the above files. If
"catkin_simple" provides a separate development package or SDK, be sure it
has been installed.

cd /home/zyj/svo_ws/build/svo_cmake; catkin build --get-env svo_cmake | catkin env -si /usr/bin/cmake /home/zyj/svo_ws/src/rpg_svo_pro_open/svo_cmake --no-warn-unused-cli -DCATKIN_DEVEL_PREFIX=/home/zyj/svo_ws/devel/.private/svo_cmake -DCMAKE_INSTALL_PREFIX=/home/zyj/svo_ws/install -DCMAKE_BUILD_TYPE=Release -DEIGEN3_INCLUDE_DIR=/usr/include/eigen3; cd -
......................................................................................................................................................................................................................................
Failed << svo_cmake:cmake [ Exited with code 1 ]
Failed <<< svo_cmake [ 0.4 seconds ]


Warnings << rqt_svo:cmake /home/zyj/svo_ws/logs/rqt_svo/build.cmake.000.log
CMake Deprecation Warning at CMakeLists.txt:1 (cmake_minimum_required):
Compatibility with CMake < 2.8.12 will be removed from a future version of
CMake.

Update the VERSION argument value or use a ... suffix to tell
CMake that the project does not need compatibility with older versions.

CMake Deprecation Warning at /usr/src/gmock/CMakeLists.txt:41 (cmake_minimum_required):
Compatibility with CMake < 2.8.12 will be removed from a future version of
CMake.

Update the VERSION argument value or use a ... suffix to tell
CMake that the project does not need compatibility with older versions.

CMake Deprecation Warning at /usr/src/gtest/CMakeLists.txt:43 (cmake_minimum_required):
Compatibility with CMake < 2.8.12 will be removed from a future version of
CMake.

Update the VERSION argument value or use a ... suffix to tell
CMake that the project does not need compatibility with older versions.

cd /home/zyj/svo_ws/build/rqt_svo; catkin build --get-env rqt_svo | catkin env -si /usr/bin/cmake /home/zyj/svo_ws/src/rpg_svo_pro_open/rqt_svo --no-warn-unused-cli -DCATKIN_DEVEL_PREFIX=/home/zyj/svo_ws/devel/.private/rqt_svo -DCMAKE_INSTALL_PREFIX=/home/zyj/svo_ws/install -DCMAKE_BUILD_TYPE=Release -DEIGEN3_INCLUDE_DIR=/usr/include/eigen3; cd -
......................................................................................................................................................................................................................................
Finished <<< rqt_svo [ 1.5 seconds ]


Errors << cmake_external_project_catkin:cmake /home/zyj/svo_ws/logs/cmake_external_project_catkin/build.cmake.000.log
CMake Deprecation Warning at CMakeLists.txt:1 (cmake_minimum_required):
Compatibility with CMake < 2.8.12 will be removed from a future version of
CMake.

Update the VERSION argument value or use a ... suffix to tell
CMake that the project does not need compatibility with older versions.

CMake Deprecation Warning at /usr/src/gmock/CMakeLists.txt:41 (cmake_minimum_required):
Compatibility with CMake < 2.8.12 will be removed from a future version of
CMake.

Update the VERSION argument value or use a ... suffix to tell
CMake that the project does not need compatibility with older versions.

CMake Deprecation Warning at /usr/src/gtest/CMakeLists.txt:43 (cmake_minimum_required):
Compatibility with CMake < 2.8.12 will be removed from a future version of
CMake.

Update the VERSION argument value or use a ... suffix to tell
CMake that the project does not need compatibility with older versions.

CMake Error at /home/zyj/svo_ws/src/cmake_external_project_catkin/CMakeLists.txt:11 (find_package):
By not providing "Findcatkin_simple.cmake" in CMAKE_MODULE_PATH this
project has asked CMake to find a package configuration file provided by
"catkin_simple", but CMake did not find one.

Could not find a package configuration file provided by "catkin_simple"
with any of the following names:

catkin_simpleConfig.cmake
catkin_simple-config.cmake

Add the installation prefix of "catkin_simple" to CMAKE_PREFIX_PATH or set
"catkin_simple_DIR" to a directory containing one of the above files. If
"catkin_simple" provides a separate development package or SDK, be sure it
has been installed.

cd /home/zyj/svo_ws/build/cmake_external_project_catkin; catkin build --get-env cmake_external_project_catkin | catkin env -si /usr/bin/cmake /home/zyj/svo_ws/src/cmake_external_project_catkin --no-warn-unused-cli -DCATKIN_DEVEL_PREFIX=/home/zyj/svo_ws/devel/.private/cmake_external_project_catkin -DCMAKE_INSTALL_PREFIX=/home/zyj/svo_ws/install -DCMAKE_BUILD_TYPE=Release -DEIGEN3_INCLUDE_DIR=/usr/include/eigen3; cd -
......................................................................................................................................................................................................................................
Failed << cmake_external_project_catkin:cmake [ Exited with code 1 ]
Failed <<< cmake_external_project_catkin [ 1.1 seconds ]


Warnings << svo_msgs:cmake /home/zyj/svo_ws/logs/svo_msgs/build.cmake.000.log
CMake Deprecation Warning at CMakeLists.txt:1 (cmake_minimum_required):
Compatibility with CMake < 2.8.12 will be removed from a future version of
CMake.

Update the VERSION argument value or use a ... suffix to tell
CMake that the project does not need compatibility with older versions.

CMake Deprecation Warning at /usr/src/gmock/CMakeLists.txt:41 (cmake_minimum_required):
Compatibility with CMake < 2.8.12 will be removed from a future version of
CMake.

Update the VERSION argument value or use a ... suffix to tell
CMake that the project does not need compatibility with older versions.

CMake Deprecation Warning at /usr/src/gtest/CMakeLists.txt:43 (cmake_minimum_required):
Compatibility with CMake < 2.8.12 will be removed from a future version of
CMake.

Update the VERSION argument value or use a ... suffix to tell
CMake that the project does not need compatibility with older versions.

cd /home/zyj/svo_ws/build/svo_msgs; catkin build --get-env svo_msgs | catkin env -si /usr/bin/cmake /home/zyj/svo_ws/src/rpg_svo_pro_open/svo_msgs --no-warn-unused-cli -DCATKIN_DEVEL_PREFIX=/home/zyj/svo_ws/devel/.private/svo_msgs -DCMAKE_INSTALL_PREFIX=/home/zyj/svo_ws/install -DCMAKE_BUILD_TYPE=Release -DEIGEN3_INCLUDE_DIR=/usr/include/eigen3; cd -
......................................................................................................................................................................................................................................


Warnings << vikit_py:cmake /home/zyj/svo_ws/logs/vikit_py/build.cmake.000.log
CMake Deprecation Warning at CMakeLists.txt:1 (cmake_minimum_required):
Compatibility with CMake < 2.8.12 will be removed from a future version of
CMake.

Update the VERSION argument value or use a ... suffix to tell
CMake that the project does not need compatibility with older versions.

CMake Deprecation Warning at /usr/src/gmock/CMakeLists.txt:41 (cmake_minimum_required):
Compatibility with CMake < 2.8.12 will be removed from a future version of
CMake.

Update the VERSION argument value or use a ... suffix to tell
CMake that the project does not need compatibility with older versions.

CMake Deprecation Warning at /usr/src/gtest/CMakeLists.txt:43 (cmake_minimum_required):
Compatibility with CMake < 2.8.12 will be removed from a future version of
CMake.

Update the VERSION argument value or use a ... suffix to tell
CMake that the project does not need compatibility with older versions.

cd /home/zyj/svo_ws/build/vikit_py; catkin build --get-env vikit_py | catkin env -si /usr/bin/cmake /home/zyj/svo_ws/src/rpg_svo_pro_open/vikit/vikit_py --no-warn-unused-cli -DCATKIN_DEVEL_PREFIX=/home/zyj/svo_ws/devel/.private/vikit_py -DCMAKE_INSTALL_PREFIX=/home/zyj/svo_ws/install -DCMAKE_BUILD_TYPE=Release -DEIGEN3_INCLUDE_DIR=/usr/include/eigen3; cd -
......................................................................................................................................................................................................................................
Finished <<< vikit_py [ 1.6 seconds ]
Finished <<< svo_msgs [ 2.4 seconds ]
[build] Summary: 4 of 26 packages succeeded.
[build] Ignored: None.
[build] Warnings: 4 packages succeeded with warnings.
[build] Abandoned: 17 packages were abandoned.
[build] Failed: 5 packages failed.
[build] Runtime: 3.9 seconds total.
[build] Note: Workspace packages have changed, please re-source setup files to use them.
`

catkin build error

I came across an error while running "catkin build". Any assistance would be highly appreciated.

____________________________________________________________________________________________________
Errors     << ceres_catkin:make /home/aref/svo_ws/logs/ceres_catkin/build.make.000.log              
Cloning into 'ceres_src'...
Note: checking out '1.14.0'.

You are in 'detached HEAD' state. You can look around, make experimental
changes and commit them, and you can discard any commits you make in this
state without impacting any branches by performing another checkout.

If you want to create a new branch to retain commits you create, you may
do so (now or later) by using -b with the checkout command again. Example:

  git checkout -b <new-branch-name>

HEAD is now at facb199f Add the missing (2,4,6) specialization to Android.mk
-- Found Eigen version 3.3.4: /usr/include/eigen3
-- Enabling use of Eigen as a sparse linear algebra library.
-- Found LAPACK library: /usr/lib/x86_64-linux-gnu/liblapack.so;/usr/lib/x86_64-linux-gnu/libblas.so
-- Found SuiteSparse 5.1.2, building with SuiteSparse.
-- Found CXSparse version: 3.1.9, building with CXSparse.
-- Found Google Flags header in: /home/aref/svo_ws/devel/include, in namespace: google
-- Found Google Log (glog). Assuming glog was built with gflags support as gflags was found. This will make gflags a public dependency of Ceres.
-- Building with OpenMP.
-- Found unordered_map/set in std namespace.
-- Found shared_ptr in std namespace using <memory> header.
-- Building Ceres as a shared library.
-- Do not build any example.
make[3]: warning: -jN forced in submake: disabling jobserver mode.
c++: internal compiler error: Killed (program cc1plus)
Please submit a full bug report,
with preprocessed source if appropriate.
See <file:///usr/share/doc/gcc-7/README.Bugs> for instructions.
make[5]: *** [internal/ceres/CMakeFiles/ceres.dir/generated/schur_eliminator_2_2_4.cc.o] Error 4
make[5]: *** Waiting for unfinished jobs....
make[4]: *** [internal/ceres/CMakeFiles/ceres.dir/all] Error 2
make[3]: *** [all] Error 2
make[2]: *** [ceres_src-prefix/src/ceres_src-stamp/ceres_src-build] Error 2
make[1]: *** [CMakeFiles/ceres_src.dir/all] Error 2
make: *** [all] Error 2
cd /home/aref/svo_ws/build/ceres_catkin; catkin build --get-env ceres_catkin | catkin env -si  /usr/bin/make --jobserver-fds=6,7 -j; cd -
....................................................................................................
Failed     << ceres_catkin:make                            [ Exited with code 2 ]                   
Failed    <<< ceres_catkin                                 [ 1 hour 1 minute and 2.8 seconds ]      
Abandoned <<< svo_pgo                                      [ Unrelated job failed ]                 
Abandoned <<< svo_online_loopclosing                       [ Unrelated job failed ]                 
Abandoned <<< svo                                          [ Unrelated job failed ]                 
Abandoned <<< svo_ceres_backend                            [ Unrelated job failed ]                 
Abandoned <<< svo_ros                                      [ Unrelated job failed ]                 
Abandoned <<< svo_benchmarking                             [ Unrelated job failed ]                 
[build] Summary: 28 of 35 packages succeeded.                                                       
[build]   Ignored:   None.                                                                          
[build]   Warnings:  4 packages succeeded with warnings.                                            
[build]   Abandoned: 6 packages were abandoned.                                                     
[build]   Failed:    1 packages failed.                                                             
[build] Runtime: 1 hour 6 minutes and 32.4 seconds total.                                           
[build] Note: Workspace packages have changed, please re-source setup files to use them.

USE_GTSAM is off in svo_ros

By default, USE_GTSAM is set to be off in svo_ros. Any reason of not using it in the backend? Could you help explain the pro and cons?

Minimum computation requirement

Dear sir,
Thank you for opensourcing great piece of work i know it may be too much to ask but will it be possible to run svo_pro in jetson nano single board computer especially the vio ceres backend.
Thank you

Error about vocabularies format

I had run ./download_voc.sh to download all vocabulary files like voc_GEN_8X4.yml.gz, but when I run the VIO + global map demo by using:

roslaunch svo_ros euroc_global_map_mono.launch

I got error message like this:

*** Aborted at 1626761251 (unix time) try "date -d @1626761251" if you are using GNU date ***
PC: @     0x7f2ddd78f507 (unknown)
*** SIGSEGV (@0x0) received by PID 10602 (TID 0x7f2dde5296c0) from PID 0; stack trace: ***
    @     0x7f2ddd640040 (unknown)
    @     0x7f2ddd78f507 (unknown)
    @     0x7f2ddd65bb45 (unknown)
    @     0x7f2ddd65ddba _IO_vfprintf
    @     0x7f2ddd732ff9 __vsnprintf_chk
    @     0x7f2dd8eefc4e cv::format()
    @     0x7f2dd8eefe2c cv::Exception::formatMessage()
    @     0x7f2dd75dacfa cv::Exception::Exception()
    @     0x7f2dd75daf1a cv::error()
    @     0x7f2dd7594a05 cv::FileStorage::Impl::parseError()
    @     0x7f2dd75a0516 cv::FileStorage::Impl::addNode()
    @     0x7f2dd75b9661 cv::YAMLParser::parse()
    @     0x7f2dd759e968 cv::FileStorage::Impl::open()
    @     0x7f2dd759fcce cv::FileStorage::FileStorage()
    @     0x7f2dda86e415 svo::loadVoc()
    @     0x7f2dda890425 svo::LoopClosing::LoopClosing()
    @     0x7f2dddddcd20 svo::factory::getLoopClosingModule()
    @     0x7f2ddddbfc81 svo::SvoInterface::SvoInterface()
    @     0x7f2dddde5524 svo_ros::SvoNodeBase::SvoNodeBase()
    @     0x55fc5eb6424c (unknown)
    @     0x7f2ddd622bf7 __libc_start_main
    @     0x55fc5eb642ea (unknown)
[svo-2] process has died [pid 10602, exit code -11, cmd /home/***/WorkSpace/svo_ws/devel/lib/svo_ros/svo_node --v=0 __name:=svo __log:=/home/***/.ros/log/c473ad64-e920-11eb-a930-2016b9af7260/svo-2.log].
log file: /home/***/.ros/log/c473ad64-e920-11eb-a930-2016b9af7260/svo-2*.log

It seems like the function svo::loadVoc() could not read vocabulary file correctly?(I guess).
And I had tried other vocabulary files like voc_MH01_8X4.yml.gz, I got the same error.
Are there any way to fix this issue? Any help would be greateful, thanks in Advance! >_<

catkin build error

Errors << svo_ros:make /home/xyc/svo2_ws/logs/svo_ros/build.make.000.log
In file included from /home/xyc/svo2_ws/src/rpg_svo_pro_open/svo_ros/include/svo_ros/visualizer.h:23,
from /home/xyc/svo2_ws/src/rpg_svo_pro_open/svo_ros/src/visualizer.cpp:6:
/opt/ros/melodic/include/pcl_ros/point_cloud.h:303:27: error: variable templates only available with -std=c++14 or -std=gnu++14 [-Werror]
constexpr static bool pcl_uses_boost = true;
^~~~~~~~~~~~~~
In file included from /home/xyc/svo2_ws/src/rpg_svo_pro_open/svo_ros/include/svo_ros/visualizer.h:23,
from /home/xyc/svo2_ws/src/rpg_svo_pro_open/svo_ros/src/svo_interface.cpp:6:
/opt/ros/melodic/include/pcl_ros/point_cloud.h:303:27: error: variable templates only available with -std=c++14 or -std=gnu++14 [-Werror]
constexpr static bool pcl_uses_boost = true;
^~~~~~~~~~~~~~
In file included from /home/xyc/svo2_ws/src/rpg_svo_pro_open/svo_ceres_backend/include/svo/ceres_backend_publisher.hpp:10,
from /home/xyc/svo2_ws/src/rpg_svo_pro_open/svo_ceres_backend/include/svo/ceres_backend_interface.hpp:11,
from /home/xyc/svo2_ws/src/rpg_svo_pro_open/svo_ros/include/svo_ros/ceres_backend_factory.h:5,
from /home/xyc/svo2_ws/src/rpg_svo_pro_open/svo_ros/src/ceres_backend_factory.cpp:1:
/opt/ros/melodic/include/pcl_ros/point_cloud.h:303:27: error: variable templates only available with -std=c++14 or -std=gnu++14 [-Werror]
constexpr static bool pcl_uses_boost = true;
^~~~~~~~~~~~~~
cc1plus: all warnings being treated as errors
make[2]: *** [CMakeFiles/svo_ros.dir/src/ceres_backend_factory.cpp.o] Error 1
make[2]: *** 正在等待未完成的任务....
cc1plus: all warnings being treated as errors
make[2]: *** [CMakeFiles/svo_ros.dir/src/visualizer.cpp.o] Error 1
cc1plus: all warnings being treated as errors
make[2]: *** [CMakeFiles/svo_ros.dir/src/svo_interface.cpp.o] Error 1
make[1]: *** [CMakeFiles/svo_ros.dir/all] Error 2
make: *** [all] Error 2
cd /home/xyc/svo2_ws/build/svo_ros; catkin build --get-env svo_ros | catkin env -si /usr/bin/make --jobserver-fds=6,7 -j; cd -
...............................................................................
Failed << svo_ros:make [ Exited with code 2 ]
Failed <<< svo_ros [ 22.5 seconds ]
Abandoned <<< svo_benchmarking [ Unrelated job failed ]
[build] Summary: 39 of 41 packages succeeded.
[build] Ignored: None.
[build] Warnings: 9 packages succeeded with warnings.
[build] Abandoned: 1 packages were abandoned.
[build] Failed: 1 packages failed.
[build] Runtime: 6 minutes and 52.5 seconds total.
[build] Note: Workspace packages have changed, please re-source setup files to use them.

svo_ros compilatoin failed: variable templates only available with -std=c++14 or -std=gnu++14

Hi.

I am trying to compile svo_ros inside a docker container on a Jetson Xavier NX, but I am getting the following error

Errors << svo_ros:make /root/catkin_ws/logs/svo_ros/build.make.000.log         
In file included from /root/catkin_ws/src/rpg_svo_pro_open/svo_ros/include/svo_ros/visualizer.h:23:0,
                 from /root/catkin_ws/src/rpg_svo_pro_open/svo_ros/src/visualizer.cpp:6:
/opt/ros/melodic/include/pcl_ros/point_cloud.h:303:27: error: variable templates only available with -std=c++14 or -std=gnu++14 [-Werror]
     constexpr static bool pcl_uses_boost = true;
                           ^~~~~~~~~~~~~~
In file included from /root/catkin_ws/src/rpg_svo_pro_open/svo_ros/include/svo_ros/visualizer.h:23:0,
                 from /root/catkin_ws/src/rpg_svo_pro_open/svo_ros/src/svo_interface.cpp:6:
/opt/ros/melodic/include/pcl_ros/point_cloud.h:303:27: error: variable templates only available with -std=c++14 or -std=gnu++14 [-Werror]
     constexpr static bool pcl_uses_boost = true;
                           ^~~~~~~~~~~~~~
In file included from /root/catkin_ws/src/rpg_svo_pro_open/svo_ceres_backend/include/svo/ceres_backend_publisher.hpp:10:0,
                 from /root/catkin_ws/src/rpg_svo_pro_open/svo_ceres_backend/include/svo/ceres_backend_interface.hpp:11,
                 from /root/catkin_ws/src/rpg_svo_pro_open/svo_ros/include/svo_ros/ceres_backend_factory.h:5,
                 from /root/catkin_ws/src/rpg_svo_pro_open/svo_ros/src/ceres_backend_factory.cpp:1:
/opt/ros/melodic/include/pcl_ros/point_cloud.h:303:27: error: variable templates only available with -std=c++14 or -std=gnu++14 [-Werror]
     constexpr static bool pcl_uses_boost = true;
                           ^~~~~~~~~~~~~~
cc1plus: all warnings being treated as errors
make[2]: *** [CMakeFiles/svo_ros.dir/src/ceres_backend_factory.cpp.o] Error 1
make[2]: *** Waiting for unfinished jobs....
cc1plus: all warnings being treated as errors
make[2]: *** [CMakeFiles/svo_ros.dir/src/visualizer.cpp.o] Error 1
cc1plus: all warnings being treated as errors
make[2]: *** [CMakeFiles/svo_ros.dir/src/svo_interface.cpp.o] Error 1

make[1]: *** [CMakeFiles/svo_ros.dir/all] Error 2
make: *** [all] Error 2
cd /root/catkin_ws/build/svo_ros; catkin build --get-env svo_ros | catkin env -si  /usr/bin/make --jobserver-fds=6,7 -j; cd -

Any idea on what could be the problem ?

Using Ubuntu 18 + ROS Melodic,cmake version 3.10.2, g++ (Ubuntu/Linaro 7.5.0-3ubuntu1~18.04) 7.5.0
Thanks in advance.

svo_ceres_backend library Runtime error

when I run ceres with openmp was aborted by solve() with error of double free; The stack trace as fellow:
so please tell me how should i to fix it? thanks alot!
Is this related with Macro of eigen,because i set the Macro of -DEIGEN_MALLOC_ALREADY_ALIGNED=0.if i do not set the DEIGEN_MALLOC_ALREADY_ALIGNED ,my program will have error which is related with eigen alignment error.
double free or corruption (out)
*** SIGSEGV (@0x0) received by PID 9900 (TID 0x7efc7bfff700) from PID 0; stack trace: ***
@ 0x7efcb8540f20 (unknown)
@ 0x55cd9567d875 Eigen::internal::gemm_pack_lhs<>::operator()()
@ 0x55cd9569c758 Eigen::internal::general_matrix_matrix_product<>::run()
@ 0x7efcb671cc62 Eigen::internal::gemm_functor<>::operator()()
@ 0x7efcb671babf Eigen::internal::parallelize_gemm<>()
@ 0x7efcb671a859 Eigen::internal::generic_product_impl<>::scaleAndAddTo<>()
@ 0x7efcb6719010 Eigen::internal::generic_product_impl<>::evalTo<>()
@ 0x7efcb6717a26 Eigen::internal::Assignment<>::run()
@ 0x7efcb6715ede Eigen::internal::call_assignment_no_alias<>()
@ 0x7efcb6714352 Eigen::PlainObjectBase<>::_set_noalias<>()
@ 0x7efcb671216f Eigen::PlainObjectBase<>::_init1<>()
@ 0x7efcb671128e Eigen::Matrix<>::Matrix<>()
@ 0x7efcb6710912 Eigen::internal::call_assignment<>()
@ 0x7efcb670fc40 Eigen::internal::call_assignment<>()
@ 0x7efcb670f273 Eigen::MatrixBase<>::operator=<>()
@ 0x7efcb670e794 svo::ceres_backend::SpeedAndBiasError::EvaluateWithMinimalJacobians()
@ 0x7efcb670e5e8 svo::ceres_backend::SpeedAndBiasError::Evaluate()
@ 0x7efcae8e3735 ceres::internal::ResidualBlock::Evaluate()
@ 0x7efcae9058e1 _ZN5ceres8internal16ProgramEvaluatorINS0_21BlockEvaluatePreparerENS0_19BlockJacobianWriterENS0_21NullJacobianFinalizerEE8EvaluateERKNS0_9Evaluator15EvaluateOptionsEPKdPdSC_SC_PNS0_12SparseMatrixE._omp_fn.2
@ 0x7efcae1a0edf GOMP_parallel
@ 0x7efcae90737a ceres::internal::ProgramEvaluator<>::Evaluate()
@ 0x7efcae91a10d ceres::internal::TrustRegionMinimizer::EvaluateGradientAndJacobian()
@ 0x7efcae91aba2 ceres::internal::TrustRegionMinimizer::IterationZero()
@ 0x7efcae91f504 ceres::internal::TrustRegionMinimizer::Minimize()
@ 0x7efcae8f121d ceres::Solver::Solve()
@ 0x7efcae8f20a9 ceres::Solve()
@ 0x7efcb6563149 svo::ceres_backend::Map::solve()
@ 0x7efcb655e2ed svo::Estimator::optimize()
@ 0x7efcb64ecd06 svo::CeresBackendInterface::optimizationLoop()
@ 0x7efcb6504766 std::__invoke_impl<>()
@ 0x7efcb64fc9c3 std::__invoke<>()
@ 0x7efcb6556b55 _ZNSt6thread8_InvokerISt5tupleIJMN3svo21CeresBackendInterfaceEFvvEPS3_EEE9_M_invokeIJLm0ELm1EEEEDTcl8__invokespcl10_S_declvalIXT_EEEEESt12_Index_tupleIJXspT_EEE

Compilation in arm64 fails because Vikit

Errors << vikit_common:make /home/imother/svo_ws/logs/vikit_common/build.make.000.log c++: error: unrecognized command line option ‘-mmmx’ c++: error: unrecognized command line option ‘-msse’; did you mean ‘-fdse’? c++: error: unrecognized command line option ‘-msse2’ c++: error: unrecognized command line option ‘-mmmx’ c++: error: unrecognized command line option ‘-msse’; did you mean ‘-fdse’? c++: error: unrecognized command line option ‘-msse3’ c++: error: unrecognized command line option ‘-msse2’ c++: error: unrecognized command line option ‘-msse3’ c++: error: unrecognized command line option ‘-mssse3’ c++: error: unrecognized command line option ‘-mmmx’ c++: error: unrecognized command line option ‘-msse’; did you mean ‘-fdse’? c++: error: unrecognized command line option ‘-msse2’ c++: error: unrecognized command line option ‘-mno-avx’; did you mean ‘-Wno-abi’? c++: error: unrecognized command line option ‘-mssse3’ c++: error: unrecognized command line option ‘-mmmx’ c++: error: unrecognized command line option ‘-msse’; did you mean ‘-fdse’? c++: error: unrecognized command line option ‘-mno-avx’; did you mean ‘-Wno-abi’? c++: error: unrecognized command line option ‘-msse3’ c++: error: unrecognized command line option ‘-msse2’ c++: error: unrecognized command line option ‘-mmmx’ c++: error: unrecognized command line option ‘-msse3’ c++: error: unrecognized command line option ‘-msse’; did you mean ‘-fdse’? c++: error: unrecognized command line option ‘-mssse3’ c++: error: unrecognized command line option ‘-msse2’ c++: error: unrecognized command line option ‘-mssse3’ c++: error: unrecognized command line option ‘-msse3’ c++: error: unrecognized command line option ‘-mno-avx’; did you mean ‘-Wno-abi’? c++: error: unrecognized command line option ‘-mssse3’ make[2]: *** [CMakeFiles/vikit_common.dir/build.make:104: CMakeFiles/vikit_common.dir/src/performance_monitor.cpp.o] Error 1 make[2]: *** Waiting for unfinished jobs.... c++: error: unrecognized command line option ‘-mno-avx’; did you mean ‘-Wno-abi’? make[2]: *** [CMakeFiles/vikit_common.dir/build.make:90: CMakeFiles/vikit_common.dir/src/math_utils.cpp.o] Error 1 c++: error: unrecognized command line option ‘-mno-avx’; did you mean ‘-Wno-abi’? c++: error: unrecognized command line option ‘-mmmx’ c++: error: unrecognized command line option ‘-msse’; did you mean ‘-fdse’? c++: error: unrecognized command line option ‘-msse2’ c++: error: unrecognized command line option ‘-msse3’ c++: error: unrecognized command line option ‘-mssse3’ c++: error: unrecognized command line option ‘-mno-avx’; did you mean ‘-Wno-abi’? make[2]: *** [CMakeFiles/vikit_common.dir/build.make:76: CMakeFiles/vikit_common.dir/src/homography.cpp.o] Error 1

dvs_tracking has died

Hi,
I am using ros melodic on ubuntu 18.04, opencv 3.2.0, i succeded in running the evo_flyingroom.bag without crashes,
My next step was to use DAVIS-346 and run the algorithm in live mode.
i have calibrated the camera by using this ros node (rosrun camera_calibration cameracalibrator.py --size 8x5 --square 0.108 image:=/dvs/image camera:=/dvs --no-service-check)

When i run the live.launch and also i run the davis_ros_driver, the algorithm works correctly for sometimes 20-50 seconds and the it crashes with this error :

REQUIRED process [dvs_tracking-6] has died!
process has died [pid 67877, exit code -8, cmd /home/ghani/catkin_ws/devel/lib/dvs_tracking/dvs_tracking_ros events:=/dvs/events remote_key:=/evo/remote_key pointcloud:=dvs_mapping/pointcloud __name:=dvs_tracking __log:=/home/ghani/.ros/log/c655846c-b9e5-11ed-b072-000c29d348d7/dvs_tracking-6.log].
log file: /home/ghani/.ros/log/c655846c-b9e5-11ed-b072-000c29d348d7/dvs_tracking-6*.log
Initiating shutdown!

Any suggestions!

Cannot complie the fast_neon on Nvidia Xavier

Dear Sir,

Thanks for open sourcing this amazing SVO2.0!

We try to compile and deploy SVO2.0 on the Nvidia Xavier platform, however, some errors happened when the fast_neon package is being compiled.

The error message shows like: "unrecognized command line option ‘-mfpu=neon’", "unknown value ‘armv7-a’ for -march".

If we delete the "-mfpu=neon" part, the following the whole fast_neon package failed. Message shows like: " #error "This file requires NEON support. Check your compiler flags." This is obvious because we do not add the "-fpu=neon" in the flags.

Could you please give me some advice about how to fix this problem? Thanks you in advance for your kindly help!

Regards,
Hilbert Xu

ceres problem

Hi,
First of all, thank you for sharing your great work.

we followed your instruction and test EuRoC dataset with svo_ros package.

and the below error messages occur, when we play the rosbag file.

''
[ INFO] [1634872641.317277074]: DepthFilter: RESET.
F1022 12:17:21.317512 7869 problem_impl.cc:635] Parameter block not found: 0x7fe8941a4da0. You must add the parameter block to the problem before it can be set constant.
*** Check failure stack trace: ***
@ 0x7fe8dd80bb0d google::LogMessage::Fail()
@ 0x7fe8dd80d9b1 google::LogMessage::SendToLog()
@ 0x7fe8dd80b63d google::LogMessage::Flush()
@ 0x7fe8dd80e389 google::LogMessageFatal::~LogMessageFatal()
@ 0x7fe8da334d25 ceres::internal::ProblemImpl::SetParameterBlockConstant()
@ 0x7fe8df194746 svo::ceres_backend::Map::setParameterBlockConstant()
@ 0x7fe8df156916 svo::Estimator::addStates()
@ 0x7fe8df13ba78 svo::CeresBackendInterface::addStatesAndInertialMeasurementsToBackend()
@ 0x7fe8df13e992 svo::CeresBackendInterface::loadMapFromBundleAdjustment()
@ 0x7fe8dee5c5cd svo::FrameHandlerBase::addFrameBundle()
@ 0x7fe8dee5ede4 svo::FrameHandlerBase::addImageBundle()
@ 0x7fe8e19b9d1c svo::SvoInterface::processImageBundle()
@ 0x7fe8e19bc90c svo::SvoInterface::stereoCallback()
@ 0x7fe8e19c75f8 boost::detail::function::void_function_obj_invoker9<>::invoke()
@ 0x7fe8e19d2b6d message_filters::CallbackHelper9T<>::call()
@ 0x7fe8e19ce384 message_filters::Signal9<>::call()
@ 0x7fe8e19ce9a3 message_filters::sync_policies::ExactTime<>::checkTuple()
@ 0x7fe8e19d07c7 message_filters::Synchronizer<>::cb<>()
@ 0x7fe8e19cde5e image_transport::SubscriberFilter::cb()
@ 0x7fe8b1cb95db image_transport::RawSubscriber::internalCallback()
@ 0x7fe8df52371e boost::detail::function::void_function_obj_invoker1<>::invoke()
@ 0x7fe8b1cbd791 ros::SubscriptionCallbackHelperT<>::call()
@ 0x7fe8e1d62152 ros::SubscriptionQueue::call()
@ 0x7fe8e1d0c829 ros::CallbackQueue::callOneCB()
@ 0x7fe8e1d0e5cb ros::CallbackQueue::callAvailable()
@ 0x7fe8e19be77c svo::SvoInterface::stereoLoop()
@ 0x7fe8e16aa6df (unknown)
@ 0x7fe8dfd1a6db start_thread
@ 0x7fe8e110571f clone
[svo-2] process has died [pid 7772, exit code -6, cmd /home/daehyun/ros_ws/svo_ws/devel/lib/svo_ros/svo_node --v=0 __name:=svo __log:=/home/daehyun/.ros/log/8f812112-32e6-11ec-852a-2cf05d939206/svo-2.log].
log file: /home/daehyun/.ros/log/8f812112-32e6-11ec-852a-2cf05d939206/svo-2*.log

''
how to solve this error?

thank you.

Catkin build issues on ubuntu 20.04 due to multiple invalid command errors

My system is ROS noetic on Ubuntu 20.04. I made some modifications, such as manually cloning the github repositories from the dependencies.yaml file using HTTPS instead of SSH.

I'm trying to build with the global map using iSAM2 (option 2). After a couple of errors when first building, I installed tf_convrsions, and fixed gtsam so it can appropriately find Eigen. I also modified a file in the DBoW2 to git clone using HTTPS instead of SSH.

After fixing the above errors, the result of catkin build looks like the following:

-----------------------------------------------------------------------------------
Profile:                     default
Extending:        [explicit] /opt/ros/noetic
Workspace:                   /home/glenn/svo_ws
-----------------------------------------------------------------------------------
Build Space:        [exists] /home/glenn/svo_ws/build
Devel Space:        [exists] /home/glenn/svo_ws/devel
Install Space:      [unused] /home/glenn/svo_ws/install
Log Space:          [exists] /home/glenn/svo_ws/logs
Source Space:       [exists] /home/glenn/svo_ws/src
DESTDIR:            [unused] None
-----------------------------------------------------------------------------------
Devel Space Layout:          linked
Install Space Layout:        None
-----------------------------------------------------------------------------------
Additional CMake Args:       -DCMAKE_BUILD_TYPE=Release -DGTSAM_USE_SYSTEM_EIGEN=ON
Additional Make Args:        None
Additional catkin Make Args: None
Internal Make Job Server:    True
Cache Job Environments:      False
-----------------------------------------------------------------------------------
Buildlisted Packages:        None
Skiplisted Packages:         None
-----------------------------------------------------------------------------------
Workspace configuration appears valid.
-----------------------------------------------------------------------------------
[build] Found 36 packages in 0.0 seconds.                                                                                                                                                                 
[build] Package table is up to date.                                                                                                                                                                      
Starting  >>> catkin_simple                                                                                                                                                                               
Starting  >>> gtsam                                                                                                                                                                                       
Starting  >>> rqt_svo                                                                                                                                                                                     
Starting  >>> svo_msgs                                                                                                                                                                                    
Starting  >>> vikit_py                                                                                                                                                                                    
Finished  <<< catkin_simple                                [ 0.1 seconds ]                                                                                                                                
Starting  >>> cmake_external_project_catkin                                                                                                                                                               
Starting  >>> eigen_catkin                                                                                                                                                                                
Starting  >>> fast                                                                                                                                                                                        
Starting  >>> gflags_catkin                                                                                                                                                                               
Starting  >>> rpg_trajectory_evaluation                                                                                                                                                                   
Starting  >>> svo_cmake                                                                                                                                                                                   
Finished  <<< eigen_catkin                                 [ 0.1 seconds ]                                                                                                                                
Starting  >>> opengv                                                                                                                                                                                      
Finished  <<< svo_msgs                                     [ 0.2 seconds ]                                                                                                                                
Finished  <<< gflags_catkin                                [ 0.1 seconds ]                                                                                                                                
Starting  >>> glog_catkin                                                                                                                                                                                 
Finished  <<< svo_cmake                                    [ 0.1 seconds ]                                                                                                                                
Finished  <<< fast                                         [ 0.2 seconds ]                                                                                                                                
Finished  <<< cmake_external_project_catkin                [ 0.1 seconds ]                                                                                                                                
Starting  >>> dbow2_catkin                                                                                                                                                                                
__________________________________________________________________________________________________________________________________________________________________________________________________________
Errors     << rqt_svo:cmake /home/glenn/svo_ws/logs/rqt_svo/build.cmake.009.log                                                                                                                           
CMake Deprecation Warning at CMakeLists.txt:1 (cmake_minimum_required):
  Compatibility with CMake < 2.8.12 will be removed from a future version of
  CMake.

  Update the VERSION argument <min> value or use a ...<max> suffix to tell
  CMake that the project does not need compatibility with older versions.


CMake Warning (dev) at CMakeLists.txt:2 (project):
  Policy CMP0048 is not set: project() command manages VERSION variables.
  Run "cmake --help-policy CMP0048" for policy details.  Use the cmake_policy
  command to set the policy and suppress this warning.

  The following variable(s) would be set to empty:

    CMAKE_PROJECT_VERSION
    CMAKE_PROJECT_VERSION_MAJOR
    CMAKE_PROJECT_VERSION_MINOR
    CMAKE_PROJECT_VERSION_PATCH
This warning is for project developers.  Use -Wno-dev to suppress it.

CMake Deprecation Warning at /usr/src/googletest/CMakeLists.txt:4 (cmake_minimum_required):
  Compatibility with CMake < 2.8.12 will be removed from a future version of
  CMake.

  Update the VERSION argument <min> value or use a ...<max> suffix to tell
  CMake that the project does not need compatibility with older versions.


CMake Deprecation Warning at /usr/src/googletest/googlemock/CMakeLists.txt:45 (cmake_minimum_required):
  Compatibility with CMake < 2.8.12 will be removed from a future version of
  CMake.

  Update the VERSION argument <min> value or use a ...<max> suffix to tell
  CMake that the project does not need compatibility with older versions.


CMake Deprecation Warning at /usr/src/googletest/googletest/CMakeLists.txt:56 (cmake_minimum_required):
  Compatibility with CMake < 2.8.12 will be removed from a future version of
  CMake.

  Update the VERSION argument <min> value or use a ...<max> suffix to tell
  CMake that the project does not need compatibility with older versions.


/opt/ros/noetic/lib/python3/dist-packages/_distutils_hack/__init__.py:18: UserWarning: Distutils was imported before Setuptools, but importing Setuptools also replaces the `distutils` module in `sys.modules`. This may lead to undesirable behaviors or errors. To avoid these issues, avoid using distutils directly, ensure that setuptools is installed in the traditional way (e.g. not an editable install), and/or make sure that setuptools is always imported before distutils.
  warnings.warn(
/opt/ros/noetic/lib/python3/dist-packages/_distutils_hack/__init__.py:33: UserWarning: Setuptools is replacing distutils.
  warnings.warn("Setuptools is replacing distutils.")
usage: setup.py [global_opts] cmd1 [cmd1_opts] [cmd2 [cmd2_opts] ...]
   or: setup.py --help [cmd1 cmd2 ...]
   or: setup.py --help-commands
   or: setup.py cmd --help

ERROR: invalid command 'rqt_svo'
CMake Error at /opt/ros/noetic/share/catkin/cmake/safe_execute_process.cmake:11 (message):

  execute_process(/home/glenn/svo_ws/build/rqt_svo/catkin_generated/env_cached.sh
  "/usr/bin/python3"
  "/opt/ros/noetic/share/catkin/cmake/interrogate_setup_dot_py.py" "rqt_svo"
  "/home/glenn/svo_ws/src/rpg_svo_pro_open/rqt_svo/setup.py"
  "/home/glenn/svo_ws/build/rqt_svo/catkin_generated/setup_py_interrogation.cmake")
  returned error code 1
Call Stack (most recent call first):
  /opt/ros/noetic/share/catkin/cmake/catkin_python_setup.cmake:46 (safe_execute_process)
  CMakeLists.txt:11 (catkin_python_setup)


cd /home/glenn/svo_ws/build/rqt_svo; catkin build --get-env rqt_svo | catkin env -si  /home/linuxbrew/.linuxbrew/bin/cmake /home/glenn/svo_ws/src/rpg_svo_pro_open/rqt_svo --no-warn-unused-cli -DCATKIN_DEVEL_PREFIX=/home/glenn/svo_ws/devel/.private/rqt_svo -DCMAKE_INSTALL_PREFIX=/home/glenn/svo_ws/install -DCMAKE_BUILD_TYPE=Release -DGTSAM_USE_SYSTEM_EIGEN=ON; cd -

..........................................................................................................................................................................................................
Failed     << rqt_svo:cmake                                [ Exited with code 1 ]                                                                                                                         
Failed    <<< rqt_svo                                      [ 1.0 seconds ]                                                                                                                                
Abandoned <<< ceres_catkin                                 [ Unrelated job failed ]                                                                                                                       
Abandoned <<< eigen_checks                                 [ Unrelated job failed ]                                                                                                                       
Abandoned <<< minkindr                                     [ Unrelated job failed ]                                                                                                                       
Abandoned <<< minkindr_conversions                         [ Unrelated job failed ]                                                                                                                       
Abandoned <<< rpg_common                                   [ Unrelated job failed ]                                                                                                                       
Abandoned <<< vikit_common                                 [ Unrelated job failed ]                                                                                                                       
Abandoned <<< vikit_cameras                                [ Unrelated job failed ]                                                                                                                       
Abandoned <<< svo_common                                   [ Unrelated job failed ]                                                                                                                       
Abandoned <<< svo_pgo                                      [ Unrelated job failed ]                                                                                                                       
Abandoned <<< svo_online_loopclosing                       [ Unrelated job failed ]                                                                                                                       
Abandoned <<< svo_vio_common                               [ Unrelated job failed ]                                                                                                                       
Abandoned <<< svo_global_map                               [ Unrelated job failed ]                                                                                                                       
Abandoned <<< vikit_ros                                    [ Unrelated job failed ]                                                                                                                       
Abandoned <<< svo_test_utils                               [ Unrelated job failed ]                                                                                                                       
Abandoned <<< vikit_solver                                 [ Unrelated job failed ]                                                                                                                       
Abandoned <<< svo_direct                                   [ Unrelated job failed ]                                                                                                                       
Abandoned <<< svo_img_align                                [ Unrelated job failed ]                                                                                                                       
Abandoned <<< svo_tracker                                  [ Unrelated job failed ]                                                                                                                       
Abandoned <<< svo                                          [ Unrelated job failed ]                                                                                                                       
Abandoned <<< svo_ceres_backend                            [ Unrelated job failed ]                                                                                                                       
Abandoned <<< svo_ros                                      [ Unrelated job failed ]                                                                                                                       
Abandoned <<< svo_benchmarking                             [ Unrelated job failed ]                                                                                                                       
Finished  <<< glog_catkin                                  [ 0.1 seconds ]                                                                                                                                
__________________________________________________________________________________________________________________________________________________________________________________________________________
Errors     << vikit_py:cmake /home/glenn/svo_ws/logs/vikit_py/build.cmake.009.log                                                                                                                         
CMake Deprecation Warning at CMakeLists.txt:1 (cmake_minimum_required):
  Compatibility with CMake < 2.8.12 will be removed from a future version of
  CMake.

  Update the VERSION argument <min> value or use a ...<max> suffix to tell
  CMake that the project does not need compatibility with older versions.


CMake Warning (dev) at CMakeLists.txt:2 (project):
  Policy CMP0048 is not set: project() command manages VERSION variables.
  Run "cmake --help-policy CMP0048" for policy details.  Use the cmake_policy
  command to set the policy and suppress this warning.

  The following variable(s) would be set to empty:

    CMAKE_PROJECT_VERSION
    CMAKE_PROJECT_VERSION_MAJOR
    CMAKE_PROJECT_VERSION_MINOR
    CMAKE_PROJECT_VERSION_PATCH
This warning is for project developers.  Use -Wno-dev to suppress it.

CMake Deprecation Warning at /usr/src/googletest/CMakeLists.txt:4 (cmake_minimum_required):
  Compatibility with CMake < 2.8.12 will be removed from a future version of
  CMake.

  Update the VERSION argument <min> value or use a ...<max> suffix to tell
  CMake that the project does not need compatibility with older versions.


CMake Deprecation Warning at /usr/src/googletest/googlemock/CMakeLists.txt:45 (cmake_minimum_required):
  Compatibility with CMake < 2.8.12 will be removed from a future version of
  CMake.

  Update the VERSION argument <min> value or use a ...<max> suffix to tell
  CMake that the project does not need compatibility with older versions.


CMake Deprecation Warning at /usr/src/googletest/googletest/CMakeLists.txt:56 (cmake_minimum_required):
  Compatibility with CMake < 2.8.12 will be removed from a future version of
  CMake.

  Update the VERSION argument <min> value or use a ...<max> suffix to tell
  CMake that the project does not need compatibility with older versions.


/opt/ros/noetic/lib/python3/dist-packages/_distutils_hack/__init__.py:18: UserWarning: Distutils was imported before Setuptools, but importing Setuptools also replaces the `distutils` module in `sys.modules`. This may lead to undesirable behaviors or errors. To avoid these issues, avoid using distutils directly, ensure that setuptools is installed in the traditional way (e.g. not an editable install), and/or make sure that setuptools is always imported before distutils.
  warnings.warn(
/opt/ros/noetic/lib/python3/dist-packages/_distutils_hack/__init__.py:33: UserWarning: Setuptools is replacing distutils.
  warnings.warn("Setuptools is replacing distutils.")
usage: setup.py [global_opts] cmd1 [cmd1_opts] [cmd2 [cmd2_opts] ...]
   or: setup.py --help [cmd1 cmd2 ...]
   or: setup.py --help-commands
   or: setup.py cmd --help

ERROR: invalid command 'vikit_py'
CMake Error at /opt/ros/noetic/share/catkin/cmake/safe_execute_process.cmake:11 (message):

  execute_process(/home/glenn/svo_ws/build/vikit_py/catkin_generated/env_cached.sh
  "/usr/bin/python3"
  "/opt/ros/noetic/share/catkin/cmake/interrogate_setup_dot_py.py" "vikit_py"
  "/home/glenn/svo_ws/src/rpg_svo_pro_open/vikit/vikit_py/setup.py"
  "/home/glenn/svo_ws/build/vikit_py/catkin_generated/setup_py_interrogation.cmake")
  returned error code 1
Call Stack (most recent call first):
  /opt/ros/noetic/share/catkin/cmake/catkin_python_setup.cmake:46 (safe_execute_process)
  CMakeLists.txt:14 (catkin_python_setup)


cd /home/glenn/svo_ws/build/vikit_py; catkin build --get-env vikit_py | catkin env -si  /home/linuxbrew/.linuxbrew/bin/cmake /home/glenn/svo_ws/src/rpg_svo_pro_open/vikit/vikit_py --no-warn-unused-cli -DCATKIN_DEVEL_PREFIX=/home/glenn/svo_ws/devel/.private/vikit_py -DCMAKE_INSTALL_PREFIX=/home/glenn/svo_ws/install -DCMAKE_BUILD_TYPE=Release -DGTSAM_USE_SYSTEM_EIGEN=ON; cd -

..........................................................................................................................................................................................................
Failed     << vikit_py:cmake                               [ Exited with code 1 ]                                                                                                                         
Failed    <<< vikit_py                                     [ 0.8 seconds ]                                                                                                                                
Finished  <<< opengv                                       [ 0.3 seconds ]                                                                                                                                
__________________________________________________________________________________________________________________________________________________________________________________________________________
Errors     << rpg_trajectory_evaluation:cmake /home/glenn/svo_ws/logs/rpg_trajectory_evaluation/build.cmake.006.log                                                                                       
CMake Deprecation Warning at CMakeLists.txt:1 (cmake_minimum_required):
  Compatibility with CMake < 2.8.12 will be removed from a future version of
  CMake.

  Update the VERSION argument <min> value or use a ...<max> suffix to tell
  CMake that the project does not need compatibility with older versions.


CMake Warning (dev) at CMakeLists.txt:2 (project):
  Policy CMP0048 is not set: project() command manages VERSION variables.
  Run "cmake --help-policy CMP0048" for policy details.  Use the cmake_policy
  command to set the policy and suppress this warning.

  The following variable(s) would be set to empty:

    CMAKE_PROJECT_VERSION
    CMAKE_PROJECT_VERSION_MAJOR
    CMAKE_PROJECT_VERSION_MINOR
    CMAKE_PROJECT_VERSION_PATCH
This warning is for project developers.  Use -Wno-dev to suppress it.

CMake Deprecation Warning at /usr/src/googletest/CMakeLists.txt:4 (cmake_minimum_required):
  Compatibility with CMake < 2.8.12 will be removed from a future version of
  CMake.

  Update the VERSION argument <min> value or use a ...<max> suffix to tell
  CMake that the project does not need compatibility with older versions.


CMake Deprecation Warning at /usr/src/googletest/googlemock/CMakeLists.txt:45 (cmake_minimum_required):
  Compatibility with CMake < 2.8.12 will be removed from a future version of
  CMake.

  Update the VERSION argument <min> value or use a ...<max> suffix to tell
  CMake that the project does not need compatibility with older versions.


CMake Deprecation Warning at /usr/src/googletest/googletest/CMakeLists.txt:56 (cmake_minimum_required):
  Compatibility with CMake < 2.8.12 will be removed from a future version of
  CMake.

  Update the VERSION argument <min> value or use a ...<max> suffix to tell
  CMake that the project does not need compatibility with older versions.


/opt/ros/noetic/lib/python3/dist-packages/_distutils_hack/__init__.py:18: UserWarning: Distutils was imported before Setuptools, but importing Setuptools also replaces the `distutils` module in `sys.modules`. This may lead to undesirable behaviors or errors. To avoid these issues, avoid using distutils directly, ensure that setuptools is installed in the traditional way (e.g. not an editable install), and/or make sure that setuptools is always imported before distutils.
  warnings.warn(
/opt/ros/noetic/lib/python3/dist-packages/_distutils_hack/__init__.py:33: UserWarning: Setuptools is replacing distutils.
  warnings.warn("Setuptools is replacing distutils.")
usage: setup.py [global_opts] cmd1 [cmd1_opts] [cmd2 [cmd2_opts] ...]
   or: setup.py --help [cmd1 cmd2 ...]
   or: setup.py --help-commands
   or: setup.py cmd --help

ERROR: invalid command 'rpg_trajectory_evaluation'
CMake Error at /opt/ros/noetic/share/catkin/cmake/safe_execute_process.cmake:11 (message):

  execute_process(/home/glenn/svo_ws/build/rpg_trajectory_evaluation/catkin_generated/env_cached.sh
  "/usr/bin/python3"
  "/opt/ros/noetic/share/catkin/cmake/interrogate_setup_dot_py.py"
  "rpg_trajectory_evaluation"
  "/home/glenn/svo_ws/src/rpg_trajectory_evaluation/setup.py"
  "/home/glenn/svo_ws/build/rpg_trajectory_evaluation/catkin_generated/setup_py_interrogation.cmake")
  returned error code 1
Call Stack (most recent call first):
  /opt/ros/noetic/share/catkin/cmake/catkin_python_setup.cmake:46 (safe_execute_process)
  CMakeLists.txt:7 (catkin_python_setup)


cd /home/glenn/svo_ws/build/rpg_trajectory_evaluation; catkin build --get-env rpg_trajectory_evaluation | catkin env -si  /home/linuxbrew/.linuxbrew/bin/cmake /home/glenn/svo_ws/src/rpg_trajectory_evaluation --no-warn-unused-cli -DCATKIN_DEVEL_PREFIX=/home/glenn/svo_ws/devel/.private/rpg_trajectory_evaluation -DCMAKE_INSTALL_PREFIX=/home/glenn/svo_ws/install -DCMAKE_BUILD_TYPE=Release -DGTSAM_USE_SYSTEM_EIGEN=ON; cd -

..........................................................................................................................................................................................................
Failed     << rpg_trajectory_evaluation:cmake              [ Exited with code 1 ]                                                                                                                         
Failed    <<< rpg_trajectory_evaluation                    [ 0.8 seconds ]                                                                                                                                
__________________________________________________________________________________________________________________________________________________________________________________________________________
Warnings   << dbow2_catkin:make /home/glenn/svo_ws/logs/dbow2_catkin/build.make.007.log                                                                                                                   
make[3]: warning: jobserver unavailable: using -j1.  Add '+' to parent make rule.
make[3]: warning: jobserver unavailable: using -j1.  Add '+' to parent make rule.
cd /home/glenn/svo_ws/build/dbow2_catkin; catkin build --get-env dbow2_catkin | catkin env -si  /usr/bin/make --jobserver-auth=3,4; cd -

..........................................................................................................................................................................................................
Finished  <<< dbow2_catkin                                 [ 0.5 seconds ]                                                                                                                                
Finished  <<< gtsam                                        [ 3.5 seconds ]                                                                                                                                
[build] Summary: 11 of 36 packages succeeded.                                                                                                                                                             
[build]   Ignored:   None.                                                                                                                                                                                
[build]   Warnings:  1 packages succeeded with warnings.                                                                                                                                                  
[build]   Abandoned: 22 packages were abandoned.                                                                                                                                                          
[build]   Failed:    3 packages failed.                                                                                                                                                                   
[build] Runtime: 4.0 seconds total.                                                         

I tried looking around in the folder and there were folders for "vikit_py", "rqt_svo" and "rpg_trajectory_evaluation" but unsure why they are seen as invalidcommands. Any pointers would be appreciate. Thank you.

Fixing the scale of a scene and comparing against ground truth

Hi,
I have two questions which are related.

Q1. How can I constrain the scale of a scene, for example I know the average depth of a scene I am operating in and I want to tell the depth filter to constrain the depth between a bound provided by me, how and where can I do that?

Q2. How do you compare the results of monocular visual odometry against ground truth? Because, the ground truth is in a given scale but monocular visual odometry is not, do you re-scale the monocular visual odometry's map and trajectory in order to compare it against the map?

How can I get the trajectory?

I want to evaluate ATE,but I am confused about the path of the results. Could you please tell me the path of the saved trajectory in which I can get the results of running dataset.

raspberry pi's installation

Dear manager,

I want to install the project in Raspberry pi (miniPC).

According to the developer's reminder , the code is only tested in:
Ubuntu 18.04 with ROS Melodic
Ubuntu 20.04 with ROS Noetic

But the system of raspberry pi is Raspbian.
if it's possible to successfully install the project into Raspberry pi?
if yes, May you give me some tips?

thank you very much! very appreciate :)

Keyframe selection approach

Hello.
In Readme.md you refer to the paper Redesigning SLAM for Arbitrary Multi-Camera Systems.
, which proposes to use entropy-based KF selection instead of any ad-hoc logic, however the current code doesn't support it. Do you plan to publish it, or is it available already on the github?

svo node died

when I run the code of "roslaunch svo_ros run_from_bag.launch cam_name:=svo_test_pinhole", svo node died. Why?????

[svo-2] process has died [pid 9465, exit code -11, cmd /home/han/Desktop/code/svo_ws/devel/lib/svo_ros/svo_node __name:=svo __log:=/home/han/.ros/log/cae6b4da-e626-11ec-81b1-54bf643636fb/svo-2.log].
log file: /home/han/.ros/log/cae6b4da-e626-11ec-81b1-54bf643636fb/svo-2*.log

svo ceres backend package error?

    when i execute the command of roslaunch svo_ros fla_stereo_imu.launch, in the config file of fla_stereo_imu.yaml, the flag of use_ceres_backend was set to be "true",There will have this error as fellow:

*** Aborted at 1633594407 (unix time) try "date -d @1633594407" if you are using GNU date ***
PC: @ 0x0 (unknown)
*** SIGSEGV (@0x0) received by PID 16632 (TID 0x7f32e7fff700) from PID 0; stack trace: ***
@ 0x7f33244b8f20 (unknown)
@ 0x5618dd640875 Eigen::internal::gemm_pack_lhs<>::operator()()
@ 0x5618dd65f758 Eigen::internal::general_matrix_matrix_product<>::run()
@ 0x7f3322696a7a Eigen::internal::gemm_functor<>::operator()()
@ 0x7f33226958ea Eigen::internal::parallelize_gemm<>()
@ 0x7f332269476f Eigen::internal::generic_product_impl<>::scaleAndAddTo<>()
@ 0x7f3322692f26 Eigen::internal::generic_product_impl<>::evalTo<>()
@ 0x7f332269193c Eigen::internal::Assignment<>::run()
@ 0x7f332268fdf4 Eigen::internal::call_assignment_no_alias<>()
@ 0x7f332268e26a Eigen::PlainObjectBase<>::_set_noalias<>()
@ 0x7f332268c087 Eigen::PlainObjectBase<>::_init1<>()
@ 0x7f332268b1a6 Eigen::Matrix<>::Matrix<>()
@ 0x7f332268a82a Eigen::internal::call_assignment<>()
@ 0x7f3322689b58 Eigen::internal::call_assignment<>()
@ 0x7f332268918b Eigen::MatrixBase<>::operator=<>()
@ 0x7f33226886b0 svo::ceres_backend::SpeedAndBiasError::EvaluateWithMinimalJacobians()
@ 0x7f3322688504 svo::ceres_backend::SpeedAndBiasError::Evaluate()
@ 0x7f331a864725 ceres::internal::ResidualBlock::Evaluate()
@ 0x7f331a8868d1 _ZN5ceres8internal16ProgramEvaluatorINS0_21BlockEvaluatePreparerENS0_19BlockJacobianWriterENS0_21NullJacobianFinalizerEE8EvaluateERKNS0_9Evaluator15EvaluateOptionsEPKdPdSC_SC_PNS0_12SparseMatrixE._omp_fn.2
@ 0x7f3315476edf GOMP_parallel
@ 0x7f331a88836a ceres::internal::ProgramEvaluator<>::Evaluate()
@ 0x7f331a89b0fd ceres::internal::TrustRegionMinimizer::EvaluateGradientAndJacobian()
@ 0x7f331a89bb92 ceres::internal::TrustRegionMinimizer::IterationZero()
@ 0x7f331a8a04f4 ceres::internal::TrustRegionMinimizer::Minimize()
@ 0x7f331a87220d ceres::Solver::Solve()
@ 0x7f331a873099 ceres::Solve()
@ 0x7f33224e1bd5 svo::ceres_backend::Map::solve()
@ 0x7f33224dcd81 svo::Estimator::optimize()
@ 0x7f332246b88c svo::CeresBackendInterface::optimizationLoop()
@ 0x7f332248326c std::__invoke_impl<>()
@ 0x7f332247b4e1 std::__invoke<>()
@ 0x7f33224d564b _ZNSt6thread8_InvokerISt5tupleIJMN3svo21CeresBackendInterfaceEFvvEPS3_EEE9_M_invokeIJLm0ELm1EEEEDTcl8__invokespcl10_S_declvalIXT_EEEEESt12_Index_tupleIJXspT_EEE

The Specific error code are " return func(0,rows, 0,cols);" as fellow:
please tell me where am i wrong ? @uzh-rpg

Index threads = std::min(nbThreads(), pb_max_threads);
// if multi-threading is explicitely disabled, not useful, or if we already are in a parallel session,
// then abort multi-threading
// FIXME omp_get_num_threads()>1 only works for openmp, what if the user does not use openmp?
if((!Condition) || (threads==1) || (omp_get_num_threads()>1))
return func(0,rows, 0,cols);

Downsizing storage not implemented

The problem I met is when I run the code, the error is printing on the terminal as:
[ERROR] [1688383531.123986841]: Downsizing storage not implemented. cols = 180 , desired = 179, num features = 108
I'm not sure the reason. Thank you for your help!

catkin build error in svo_ceres_backend

/usr/local/include/eigen3/Eigen/src/Core/util/ConfigureVectorization.h:39:38:error: attribute ignored in declaration of ‘struct pcl::_PointXYZHSV’ [-Werror=attributes]
#define EIGEN_ALIGN_TO_BOUNDARY(n) alignas(n)
/usr/local/include/eigen3/Eigen/src/Core/util/ConfigureVectorization.h:39:38: note: attribute for ‘struct pcl::_PointXYZHSV’ must follow the ‘struct’ keyword
cc1plus: all warnings being treated as errors
make[2]: *** [CMakeFiles/svo_ceres_backend.dir/src/ceres_backend_interface.cpp.o] Error 1
make[1]: *** [CMakeFiles/svo_ceres_backend.dir/all] Error 2
make: *** [all] Error 2

I guess the environment causes it, i try to gcc 6,7,8,9,10,11 , but it doesn't work.

Is it redundant of svo_backend in svo_ros package.xml

    Is it redundant of svo_backend in svo_ros package.xml;because there is no svo_backend package file  in the source code ;but there is  "<depend>svo_backend</depend>" in the svo_ros package.xml,so  Is it redundant?
     if not,Will it cause the fellow problem  without the svo_backend package?

double free or corruption (out)
*** SIGSEGV (@0x0) received by PID 9900 (TID 0x7efc7bfff700) from PID 0; stack trace: ***
@ 0x7efcb8540f20 (unknown)
@ 0x55cd9567d875 Eigen::internal::gemm_pack_lhs<>::operator()()
@ 0x55cd9569c758 Eigen::internal::general_matrix_matrix_product<>::run()
@ 0x7efcb671cc62 Eigen::internal::gemm_functor<>::operator()()
@ 0x7efcb671babf Eigen::internal::parallelize_gemm<>()
@ 0x7efcb671a859 Eigen::internal::generic_product_impl<>::scaleAndAddTo<>()
@ 0x7efcb6719010 Eigen::internal::generic_product_impl<>::evalTo<>()
@ 0x7efcb6717a26 Eigen::internal::Assignment<>::run()
@ 0x7efcb6715ede Eigen::internal::call_assignment_no_alias<>()
@ 0x7efcb6714352 Eigen::PlainObjectBase<>::_set_noalias<>()
@ 0x7efcb671216f Eigen::PlainObjectBase<>::_init1<>()
@ 0x7efcb671128e Eigen::Matrix<>::Matrix<>()
@ 0x7efcb6710912 Eigen::internal::call_assignment<>()
@ 0x7efcb670fc40 Eigen::internal::call_assignment<>()
@ 0x7efcb670f273 Eigen::MatrixBase<>::operator=<>()
@ 0x7efcb670e794 svo::ceres_backend::SpeedAndBiasError::EvaluateWithMinimalJacobians()
@ 0x7efcb670e5e8 svo::ceres_backend::SpeedAndBiasError::Evaluate()
@ 0x7efcae8e3735 ceres::internal::ResidualBlock::Evaluate()
@ 0x7efcae9058e1 _ZN5ceres8internal16ProgramEvaluatorINS0_21BlockEvaluatePreparerENS0_19BlockJacobianWriterENS0_21NullJacobianFinalizerEE8EvaluateERKNS0_9Evaluator15EvaluateOptionsEPKdPdSC_SC_PNS0_12SparseMatrixE._omp_fn.2
@ 0x7efcae1a0edf GOMP_parallel
@ 0x7efcae90737a ceres::internal::ProgramEvaluator<>::Evaluate()
@ 0x7efcae91a10d ceres::internal::TrustRegionMinimizer::EvaluateGradientAndJacobian()
@ 0x7efcae91aba2 ceres::internal::TrustRegionMinimizer::IterationZero()
@ 0x7efcae91f504 ceres::internal::TrustRegionMinimizer::Minimize()
@ 0x7efcae8f121d ceres::Solver::Solve()
@ 0x7efcae8f20a9 ceres::Solve()
@ 0x7efcb6563149 svo::ceres_backend::Map::solve()
@ 0x7efcb655e2ed svo::Estimator::optimize()
@ 0x7efcb64ecd06 svo::CeresBackendInterface::optimizationLoop()
@ 0x7efcb6504766 std::__invoke_impl<>()
@ 0x7efcb64fc9c3 std::__invoke<>()
@ 0x7efcb6556b55 _ZNSt6thread8_InvokerISt5tupleIJMN3svo21CeresBackendInterfaceEFvvEPS3_EEE9_M_invokeIJLm0ELm1EEEEDTcl8__invokespcl10_S_declvalIXT_EEEEESt12_Index_tupleIJXspT_EEE

global map run EuRoc MH01 error about gtsam,help,thank you!

when i run MH01 euroc dataser use svo pro,it happens error like this,have you encount?
Indeterminant linear system detected while working near variable
8646911284551353336 (Symbol: x1016).

Thrown when a linear system is ill-posed. The most common cause for this
error is having underconstrained variables. Mathematically, the system is
underdetermined. See the GTSAM Doxygen documentation at
http://borg.cc.gatech.edu/ on gtsam::IndeterminantLinearSystemException for
more information.
crash in update: 4
terminate called after throwing an instance of 'gtsam::IndeterminantLinearSystemException'
what():
Indeterminant linear system detected while working near variable
8646911284551353336 (Symbol: x1016).

Thrown when a linear system is ill-posed. The most common cause for this
error is having underconstrained variables. Mathematically, the system is
underdetermined. See the GTSAM Doxygen documentation at
http://borg.cc.gatech.edu/ on gtsam::IndeterminantLinearSystemException for
more information.

Which part of the code contains the 'adaptive exposure control"

Hi,

I'm working on the active exposure control algorithm in Zichao Zhang's ICRA 2017 paper and I find this repo might contain the implementation of this algorithm. However, I cannot find it in this repo. Could you please point out where I can find the code for this algorithm in this repo?

Best
Taoyi

Does the current version work with Multi-Camera arrays?

Hello Authors (@Manasi94 ),
I am interested to use SVO_PRO for performing SLAM on multi-camera data (>2). I have looked into the code and tried to make it work by extending on the mono and stereo setups and using existing code stubs. But, it is failing at the initialization stage. When I probed deeper into the code I found that the Initialization method for camera arrays (ArrayInitGeometric and ArrayInitOptimization) are nlot implemented. Particularly the addFrameBundle() methods of these init classes have commented code and some methods that dont exisit (eg, trackBundleFeatures). Is there a way for the code to work with multi-camera arrays? or is it a future release? Any info on this would be great!

Thanks,
Pushyami

svo node not publishing any results using stereo+imu camera

I am attempting svo with a realsense t265, but when I launch the node with a launch file similar to the fla_stere_imu.launch, Rviz opens up but nothing is displayed, there is no error message, although the svo node in not publishing data in any of the topics.

The step I've followed are:

  • Since the camera publishes the IMU data segregated on different topics, I have made a python node that mixes them in a single IMU message.
  • In the launch file, I set the camera topics to the ones published by the t265, and the IMU topic to the one published by the python node.
  • For the calibration file I used the format of the fla_stereo_imu.yaml introducing the distortion values and intrinsics published by the t265 at /camera/fisheye1/camera_info and /camera/fisheye2/camera_info.
  • For the parameter file I used the same as in fla_stere_imu.launch
  • I have tried both running from a bag file with the three topics recorded, and running the camera node live and in either case I get the same result.
    In rqt I get the following chart:
    image
    where you can see that the svo node is subscribed to the camera and IMU topics, but none of the topics between svo and vis contain any data.
    I don't know if I have missed any steps needed to use svo with a custom stereo + imu camera.
    Thank you for the help

Recommend Projects

  • React photo React

    A declarative, efficient, and flexible JavaScript library for building user interfaces.

  • Vue.js photo Vue.js

    🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.

  • Typescript photo Typescript

    TypeScript is a superset of JavaScript that compiles to clean JavaScript output.

  • TensorFlow photo TensorFlow

    An Open Source Machine Learning Framework for Everyone

  • Django photo Django

    The Web framework for perfectionists with deadlines.

  • D3 photo D3

    Bring data to life with SVG, Canvas and HTML. 📊📈🎉

Recommend Topics

  • javascript

    JavaScript (JS) is a lightweight interpreted programming language with first-class functions.

  • web

    Some thing interesting about web. New door for the world.

  • server

    A server is a program made to process requests and deliver data to clients.

  • Machine learning

    Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.

  • Game

    Some thing interesting about game, make everyone happy.

Recommend Org

  • Facebook photo Facebook

    We are working to build community through open source technology. NB: members must have two-factor auth.

  • Microsoft photo Microsoft

    Open source projects and samples from Microsoft.

  • Google photo Google

    Google ❤️ Open Source for everyone.

  • D3 photo D3

    Data-Driven Documents codes.