Skip to content

Commit

Permalink
Clean slam (Soldann#16)
Browse files Browse the repository at this point in the history
* New java style enums

* Compile with new java style enums

* change some locks

* Clean more and add comments

* Clean mostly iterators

* Make refactor readability changes

Co-authored-by: Nicole Jin <[email protected]>
Co-authored-by: justinmetivier <[email protected]>

* Fix compile issues

* clean up some of the iterators and formatting

* Remove unused functions and member fields

* Remove unused variables or take out more member variables

* Resolve nan issue

* fixes Soldann#14

* Merge 9 minimum cmake version (Soldann#18)

* Update cmake to be compatible with latest minor version from apt

* Add dependency installer

* Update readme to show install and build instructions

* Reverted some breaking slam 'refactoring' (Soldann#19)

SLAM would not continue to 'spam print' and would frequently have invalid_argument exceptions thrown or other segmentation faults. This was due to some changes that were accidentally made to for loops in an invalid way.

We reverted more than might have been required but not much.

---------------

Co-authored-by: Nicole Jin <[email protected]>

* Potentially fix segfault from PredictStateIMU

* fixes Soldann#22

* Add quality of life changes for cmake, build, and clean

* Handle compile warnings and issues when compiling with clang17 and c++20

* Address g2o clang compile typeid warnings

* Call PoseOptimization if there are no imu frames in track local map

---------

Co-authored-by: Nicole Jin <[email protected]>
Co-authored-by: justinmetivier <[email protected]>
Co-authored-by: Nicole Jin <[email protected]>
  • Loading branch information
4 people authored Jul 26, 2023
1 parent 98c94b9 commit 67cf5a2
Show file tree
Hide file tree
Showing 50 changed files with 1,124 additions and 2,462 deletions.
27 changes: 20 additions & 7 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,36 +1,47 @@
cmake_minimum_required(VERSION 3.24.0)
cmake_minimum_required(VERSION 3.16.0)

set(MORB_VERSION_MAJOR 0 CACHE STRING "major version" FORCE)
set(MORB_VERSION_MINOR 0 CACHE STRING "major version" FORCE)
set(MORB_VERSION_PATCH 1 CACHE STRING "major version" FORCE)
set(MORB_VERSION_SHORT ${MORB_VERSION_MAJOR}.${MORB_VERSION_MINOR} CACHE STRING "short-version" FORCE)
set(MORB_VERSION ${MORB_VERSION_SHORT}.${MORB_VERSION_PATCH} CACHE STRING "version" FORCE)

find_program(CLANGXX_17_FOUND clang++-17)
if(CLANGXX_17_FOUND)
set(CMAKE_C_COMPILER "clang-17")
set(CMAKE_CXX_COMPILER "clang++-17")
endif()

project(MORB_SLAM
VERSION ${MORB_VERSION}
DESCRIPTION "A library for visual simultanious localization and mapping (vSLAM) for autonomous vehicles"
HOMEPAGE_URL "https://github.com/DavidPetkovsek/MORB_SLAM"
LANGUAGES CXX
LANGUAGES C CXX # if you do not specify C in addition to CXX, GNU compilers will fail to find threads on some computers
)


include(GNUInstallDirs)
# Set compiler standard to C++17
set(CMAKE_CXX_STANDARD 17)
# Set compiler standard to C++20
set(CMAKE_CXX_STANDARD 20)
set(CMAKE_CXX_STANDARD_REQUIRED TRUE)
if( NOT CMAKE_BUILD_TYPE )
set(CMAKE_BUILD_TYPE Release)
ENDIF()

# https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
if(CMAKE_GENERATOR STREQUAL "Ninja")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fdiagnostics-color=always")
endif()
if(CMAKE_CXX_COMPILER_VERSION VERSION_LESS 7) # required for eigen
message(FATAL_ERROR "Insufficient gcc version must be at least 7, required for eigen")
message(FATAL_ERROR "Insufficient g++ version must be at least 7, required for eigen")
endif()
elseif(CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
if(CMAKE_GENERATOR STREQUAL "Ninja")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fdiagnostics-color=always")
endif()
if(CMAKE_CXX_COMPILER_VERSION VERSION_LESS 5) # required for eigen
message(FATAL_ERROR "Insufficient clang version must be at least 5, required for eigen")
message(FATAL_ERROR "Insufficient clang++ version must be at least 5, required for eigen")
endif()
elseif(CMAKE_CXX_COMPILER_ID STREQUAL "MSVC")
if(CMAKE_CXX_COMPILER_VERSION VERSION_LESS 19.12) # required for eigen
Expand Down Expand Up @@ -63,7 +74,7 @@ set(TARGET_NAME ${PROJECT_NAME})

find_package(Threads REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(realsense2) # realsense only needed for example
find_package(realsense2 QUIET) # realsense only needed for example
find_package(Pangolin REQUIRED)
find_package(OpenCV REQUIRED)
find_package(Boost REQUIRED COMPONENTS serialization regex)
Expand Down Expand Up @@ -145,6 +156,8 @@ if(realsense2_FOUND)
target_link_libraries( ${TARGET_NAME} PUBLIC ${OpenCV_LIBS} )
target_link_libraries( ${TARGET_NAME} PUBLIC ${Pangolin_LIBRARIES} )
target_link_libraries(stereo_inertial_realsense_D435i PRIVATE ${PROJECT_NAME})
else()
message(STATUS "** Stereo inertial realsense D435i example will not compile because librealsense2 was not found")
endif()


Expand Down
3 changes: 0 additions & 3 deletions Examples/Stereo-Inertial/RealSense_D435i.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -22,9 +22,6 @@ Camera.height: 480
# Camera frames per second
Camera.fps: 30

# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1

# Close/Far threshold. Baseline times.
Stereo.ThDepth: 40.0

Expand Down
21 changes: 10 additions & 11 deletions Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc
Original file line number Diff line number Diff line change
Expand Up @@ -95,7 +95,7 @@ static rs2_option get_sensor_option(const rs2::sensor& sensor)
}


Sophus::SE3f sendImageAndImuData(const cv::Mat& imLeft, const cv::Mat& imRight,
MORB_SLAM::StereoPacket sendImageAndImuData(const cv::Mat& imLeft, const cv::Mat& imRight,
const float& im_timestamp, std::vector<MORB_SLAM::IMU::Point>& vImuMeas, MORB_SLAM::System_ptr SLAM) {

float imageScale = SLAM->GetImageScale();
Expand All @@ -109,9 +109,10 @@ Sophus::SE3f sendImageAndImuData(const cv::Mat& imLeft, const cv::Mat& imRight,
cv::resize(imRight, imRight, cv::Size(width, height));
}

Sophus::SE3f sophusPose = SLAM->TrackStereo(imLeft, imRight, im_timestamp, vImuMeas);
MORB_SLAM::StereoPacket sophusPose = SLAM->TrackStereo(imLeft, imRight, im_timestamp, vImuMeas);

sophusPose = sophusPose.inverse();
if(sophusPose.pose)
sophusPose.pose = sophusPose.pose->inverse();
return sophusPose;
}

Expand Down Expand Up @@ -297,22 +298,20 @@ int main(int argc, char **argv) {


rs2::stream_profile imu_stream = pipe_profile.get_stream(RS2_STREAM_GYRO);
float* Rbc = cam_left.get_extrinsics_to(imu_stream).rotation;
float* tbc = cam_left.get_extrinsics_to(imu_stream).translation;
auto extrinLeft = cam_left.get_extrinsics_to(imu_stream);
std::cout << "Tbc (left) = " << std::endl;
for(int i = 0; i<3; i++){
for(int j = 0; j<3; j++)
std::cout << Rbc[i*3 + j] << ", ";
std::cout << tbc[i] << "\n";
std::cout << extrinLeft.rotation[i*3 + j] << ", ";
std::cout << extrinLeft.translation[i] << "\n";
}

float* Rlr = cam_right.get_extrinsics_to(cam_left).rotation;
float* tlr = cam_right.get_extrinsics_to(cam_left).translation;
auto extrinRight = cam_right.get_extrinsics_to(cam_left);
std::cout << "Tlr = " << std::endl;
for(int i = 0; i<3; i++){
for(int j = 0; j<3; j++)
std::cout << Rlr[i*3 + j] << ", ";
std::cout << tlr[i] << "\n";
std::cout << extrinRight.rotation[i*3 + j] << ", ";
std::cout << extrinRight.translation[i] << "\n";
}


Expand Down
12 changes: 10 additions & 2 deletions README.md
Original file line number Diff line number Diff line change
@@ -1,8 +1,16 @@
# MORB_SLAM

[![CMake Build](https://github.com/Soldann/MORB_SLAM/actions/workflows/cmake.yml/badge.svg)](https://github.com/Soldann/MORB_SLAM/actions/workflows/cmake.yml)
> This fork of [ORB_SLAM3](https://github.com/UZ-SLAMLab/ORB_SLAM3) converts it into a CMake package that can be imported into other projects. It is continued from the original now deprecated/abandoned fork [MORB_SLAM](https://github.com/Soldann/MORB_SLAM).
This fork of [MORB_SLAM](https://github.com/UZ-SLAMLab/MORB_SLAM) converts it into a CMake package that can be imported into other projects. Run `morbslam_installer.sh` to install.
Install the library dependencies:
```bash
./dependencies.sh
```

Build and install MORB_SLAM:
```bash
./build.sh
```

In your other projects, import using:
```cmake
Expand Down
2 changes: 1 addition & 1 deletion Thirdparty/DBoW2/DBoW2/FORB.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@
#include <vector>
#include <string>
#include <sstream>
#include <stdint-gcc.h>
#include <cinttypes>

#include "FORB.h"

Expand Down
23 changes: 6 additions & 17 deletions Thirdparty/Sophus/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -41,21 +41,12 @@ target_include_directories(${TARGET_NAME} INTERFACE
target_link_libraries( ${TARGET_NAME} INTERFACE Eigen3::Eigen )

# the following is from orb-sam-3
# Set compiler specific settings (FixMe: Should not cmake do this for us automatically?)
IF("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
set_target_properties(${TARGET_NAME} PROPERTIES CMAKE_CXX_FLAGS_DEBUG "-O0 -g")
set_target_properties(${TARGET_NAME} PROPERTIES CMAKE_CXX_FLAGS_RELEASE "-O3")
set_target_properties(${TARGET_NAME} PROPERTIES CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -Wextra -Wno-deprecated-register -Qunused-arguments -fcolor-diagnostics")
ELSEIF("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU")
set_target_properties(${TARGET_NAME} PROPERTIES CMAKE_CXX_FLAGS_DEBUG "-O0 -g")
set_target_properties(${TARGET_NAME} PROPERTIES CMAKE_CXX_FLAGS_RELEASE "-O3")
set_target_properties(${TARGET_NAME} PROPERTIES CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -Wextra -std=c++11 -Wno-deprecated-declarations -ftemplate-backtrace-limit=0")
set_target_properties(${TARGET_NAME} PROPERTIES CMAKE_CXX_FLAGS_COVERAGE "${CMAKE_CXX_FLAGS_DEBUG} --coverage -fno-inline -fno-inline-small-functions -fno-default-inline")
set_target_properties(${TARGET_NAME} PROPERTIES CMAKE_EXE_LINKER_FLAGS_COVERAGE "${CMAKE_EXE_LINKER_FLAGS_DEBUG} --coverage")
set_target_properties(${TARGET_NAME} PROPERTIES CMAKE_SHARED_LINKER_FLAGS_COVERAGE "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} --coverage")
ELSEIF(CMAKE_CXX_COMPILER_ID MATCHES "^MSVC$")
target_compile_definitions(${TARGET_NAME} "-D _USE_MATH_DEFINES /bigobj /wd4305 /wd4244 /MP")
ENDIF()
# There were a bunch of manually set compile options here. They were removed because they were being applied to an interface library (header only) which would have no impact on the end user.
# The only impact would be on the test and examples built by sophus itself. Which we do not use. Furthermore these specific ones were not being used by the original orb-slam-3 because
# it compiled sophus as a completely seperate library and interfaced with everything manually. Meaning it also didn't propogate the changes. Since we have updated cmake to be more
# modern in its use, we do not want to propogate to the end user so the items are removed all together as they are not required and should be up to the user to include. If the user
# who is building this library wants the examples or tests to be built with these flags add them to those targets manually in their respective cmakelist files.

if(MSVC)
# Define common math constants if we compile with MSVC
target_compile_definitions (${TARGET_NAME} INTERFACE _USE_MATH_DEFINES)
Expand Down Expand Up @@ -94,8 +85,6 @@ endif()
# https://cmake.org/cmake/help/latest/guide/importing-exporting/index.html#creating-a-package-version-file
# https://cmake.org/cmake/help/latest/manual/cmake-packages.7.html#creating-packages
# https://mail.kde.org/pipermail/kde-buildsystem/2008-April/004544.html
set_property(TARGET ${TARGET_NAME} PROPERTY VERSION 1.1.0) # x.x.x used for symlinks
set_property(TARGET ${TARGET_NAME} PROPERTY SOVERSION 1) # ABI/API compatibility at so level must be integer
set_property(TARGET ${TARGET_NAME} PROPERTY "INTERFACE_${TARGET_NAME}_MAJOR_VERSION" 1)
set_property(TARGET ${TARGET_NAME} APPEND PROPERTY COMPATIBLE_INTERFACE_STRING "${TARGET_NAME}_MAJOR_VERSION")

Expand Down
4 changes: 2 additions & 2 deletions Thirdparty/g2o/g2o/core/hyper_graph_action.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -254,13 +254,13 @@ namespace g2o {
{
for (HyperGraph::VertexIDMap::iterator it=graph->vertices().begin();
it!=graph->vertices().end(); ++it){
if ( typeName.empty() || typeid(*it->second).name()==typeName){
if ( typeName.empty() || typeid(std::remove_pointer_t<decltype(it->second)>).name()==typeName){
(*action)(it->second, params);
}
}
for (HyperGraph::EdgeSet::iterator it=graph->edges().begin();
it!=graph->edges().end(); ++it){
if ( typeName.empty() || typeid(**it).name()==typeName)
if ( typeName.empty() || typeid(std::remove_pointer_t<decltype(*it)>).name()==typeName)
(*action)(*it, params);
}
}
Expand Down
4 changes: 2 additions & 2 deletions Thirdparty/g2o/g2o/core/optimizable_graph.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -179,8 +179,8 @@ namespace g2o {
for (size_t i=0; i<_parameters.size(); i++){
int index = _parameterIds[i];
*_parameters[i] = graph()->parameter(index);
if (typeid(**_parameters[i]).name()!=_parameterTypes[i]){
std::cerr << __PRETTY_FUNCTION__ << ": FATAL, parameter type mismatch - encountered " << typeid(**_parameters[i]).name() << "; should be " << _parameterTypes[i] << std::endl;
if (typeid(std::remove_pointer_t<decltype(*_parameters[i])>).name()!=_parameterTypes[i]){
std::cerr << __PRETTY_FUNCTION__ << ": FATAL, parameter type mismatch - encountered " << typeid(std::remove_pointer_t<decltype(*_parameters[i])>).name() << "; should be " << _parameterTypes[i] << std::endl;
}
if (!*_parameters[i]) {
std::cerr << __PRETTY_FUNCTION__ << ": FATAL, *_parameters[i] == 0" << std::endl;
Expand Down
2 changes: 1 addition & 1 deletion Thirdparty/g2o/g2o/core/sparse_block_matrix.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -336,7 +336,7 @@ namespace g2o {
for (int i=1; i<n; ++i){
colIdx[i]=colIdx[i-1]+colsOfBlock(cmin+i);
}
typename SparseBlockMatrix<MatrixType>::SparseBlockMatrix* s=new SparseBlockMatrix(rowIdx, colIdx, m, n, true);
auto* s=new SparseBlockMatrix(rowIdx, colIdx, m, n, true);
for (int i=0; i<n; ++i){
int mc=cmin+i;
for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[mc].begin(); it!=_blockCols[mc].end(); ++it){
Expand Down
78 changes: 59 additions & 19 deletions build.sh
Original file line number Diff line number Diff line change
@@ -1,49 +1,89 @@
#!/bin/bash

if [ $# == "1" ]; then
jobs=$1
else
jobs="-j$(nproc)"
fi
echo "Using argument ${jobs}"

# https://stackoverflow.com/questions/24112727/relative-paths-based-on-file-location-instead-of-current-working-directory
parent_path=$( cd "$(dirname "${BASH_SOURCE[0]}")" ; pwd -P )
cd "$parent_path" # change directories so working directory is where the script is

cd Vocabulary
if [ ! -f "ORBvoc.txt" ]; then
echo "Uncompress vocabulary ..."
# Initialize lists
cmake_args=()
j_arg="-j$(nproc)"
g_arg=""
# Iterate over arguments
for arg in "$@"; do
if [[ $arg =~ ^-j[0-9]+$ ]]; then
j_arg="$arg"
elif [[ $arg =~ ^-G[0-9a-zA-Z]+$ ]]; then
g_arg="$arg"
else
cmake_args+=("$arg")
fi
done
# Check if the g_arg is empty (non-user specified generator)
if [ -z "$g_arg" ]; then
# select ninja if available, make otherwise
if which ninja >/dev/null 2>&1; then
g_arg="-GNinja"
elif which make >/dev/null 2>&1; then
g_arg="-GMake"
else
echo "Please either install Ninja (preffered), Make, or specify an installed Generator?"
echo " For ninja `sudo apt install ninja-build`"
echo " For make `sudo apt install build-essential`"
exit -1
fi
fi


if [ ! -f "Vocabulary/ORBvoc.txt" ]; then
cd Vocabulary
echo "Extracting vocabulary..."
tar -xf ORBvoc.txt.tar.gz
echo "ORB Vocabulary extracted"
cd $parent_path
else
echo "ORB Vocabulary already extracted"
fi
cd ..

if [ ! -d "build" ] || [ ! -f 'build/config-finished.bool' ]; then
echo 'Performing first time configuration'
configCompleteFile='config-finished.bool'
# https://unix.stackexchange.com/questions/31414/how-can-i-pass-a-command-line-argument-into-a-shell-script
if [ ! -d "build" ] || [ ! -f "build/${configCompleteFile}" ]; then
echo 'Performing first time configuration...'
echo "Workers: ${j_arg} Generator: ${g_arg}"
echo "User Flags: ${cmake_args[@]}"
mkdir build 2> /dev/null
cd build
# https://unix.stackexchange.com/questions/31414/how-can-i-pass-a-command-line-argument-into-a-shell-script
cmake .. -GNinja
cmake .. ${g_arg} "${cmake_args[@]}" # pass arguments on to cmake
if [ $? -ne 0 ]; then
rm 'config-finished.bool' 2> /dev/null
rm "${configCompleteFile}" 2> /dev/null
cd ..
echo "Configuration was not successful"
exit 1
fi
touch 'config-finished.bool'
touch "${configCompleteFile}"
echo -e " Generator: ${g_arg}\nUser Flags: ${cmake_args[@]}" > "${configCompleteFile}"
else
echo 'Already configured'
cd build
config=$(cat "${configCompleteFile}")
echo "Workers: ${j_arg}${config}"
fi
ninja $jobs

if [ $? -ne 0 ]; then
cd ..
echo "Configuration was not successful"
exit 1
fi
echo "Building..."
cmake --build . -- ${j_arg}
if [ $? -ne 0 ]; then
cd ..
echo "Build was not successful"
exit 2
fi
sudo ninja install
sudo cmake --install .
if [ $? -ne 0 ]; then
cd ..
echo "Install was not successful"
exit 3
fi
fi
10 changes: 5 additions & 5 deletions clean.sh
Original file line number Diff line number Diff line change
Expand Up @@ -4,12 +4,12 @@
parent_path=$( cd "$(dirname "${BASH_SOURCE[0]}")" ; pwd -P )
cd "$parent_path" # change directories so working directory is where the script is

rm Vocabulary/ORBvoc.txt 2> /dev/null
rm -r build 2> /dev/null
rm -r bin 2> /dev/null
rm -rf Vocabulary/ORBvoc.txt 2> /dev/null
rm -rf build 2> /dev/null
rm -rf bin 2> /dev/null

# Clean executable examples
rm Examples/RGB-D/rgbd_tum \
rm -rf Examples/RGB-D/rgbd_tum \
Examples/RGB-D/rgbd_realsense_D435i \
Examples/RGB-D-Inertial/rgbd_inertial_realsense_D435i \
Examples/Monocular/mono_realsense_D435i \
Expand Down Expand Up @@ -44,6 +44,6 @@ rm Examples/RGB-D/rgbd_tum \
Examples/Tests/sophus_test \
2> /dev/null

rm -r Examples/ROS/MORB_SLAM/build 2> /dev/null
rm -rf Examples/ROS/MORB_SLAM/build 2> /dev/null

echo cleaning complete
25 changes: 25 additions & 0 deletions dependencies.sh
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
parent_path=$( cd "$(dirname "${BASH_SOURCE[0]}")" ; pwd -P )
cd "$parent_path" # change directories so working directory is where the script is

#apt update for good measure
sudo apt update

sudo apt install -y build-essential cmake ninja-build libeigen3-dev libssl-dev libboost-all-dev libopencv-dev libpython3.9

#pangolin
git clone --recursive https://github.com/stevenlovegrove/Pangolin.git --depth=1 -b v0.8
cd Pangolin

./scripts/install_prerequisites.sh -m recommended

mkdir build
cd build

set -e # set to abort on error

cmake .. -DCMAKE_BUILD_TYPE=Release
make -j$(nproc)
sudo make install
sudo ldconfig > /dev/null 2> /dev/null

echo "Finished installing all of the dependencies! Now just run build.sh! You can use -jX for choosing the number of workers with build.sh. default:-j$(nproc)"
Loading

0 comments on commit 67cf5a2

Please sign in to comment.