Skip to content

Commit 22522f0

Browse files
matlabbematlabbe
committed
OptimizerG2O: adding support for priors in SBA
Fixed computeError CID-SIMS support (#1676) * cid-sims dataset support * Added cli tool to test CID-SIMS dataset * removed debug log don't fix root id roll/pitch if gravity constraints are fed (SBA) changed number_t to double forward compatibility cleanup log rtabmap-matcher: Added option to recfify raw images for convenience (#1663) Cudasift tuning and SSC supporting multicameras (#1677) * CudaSIFT: filter doubles * removed fixed threshold * SSC can be used with multicameras. Refactored CudaSIFT to support SSC. Add new parameter SIFT/MaxGaussianThreshold. DbViewer: show negative features with gray color (so that we can know which features are in the vocabulary) * Added SIFT/MaxGaussianThreshold parameter * Updated parameter description Updated CI status split cmake workflow in linux and windows OptimizerG2O: adding support for priors in SBA (#1670) * OptimizerG2O: adding support for priors in SBA * Fixed computeError * don't fix root id roll/pitch if gravity constraints are fed (SBA) * changed number_t to double * forward compatibility * cleanup * log bump release badge version uniformize name fixing android build Update CI badges (#1681) * OptimizerG2O: adding support for priors in SBA * Fixed computeError * don't fix root id roll/pitch if gravity constraints are fed (SBA) * changed number_t to double * forward compatibility * cleanup * log * Updated CI status * split cmake workflow in linux and windows * bump release badge version * uniformize name * fixing android build
1 parent 5b985f6 commit 22522f0

29 files changed

+1652
-480
lines changed

‎.github/workflows/cmake-linux.yml‎

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
name: CMake-Linux
2+
3+
on:
4+
push:
5+
branches:
6+
- master
7+
pull_request:
8+
branches:
9+
- '**'
10+
11+
env:
12+
BUILD_TYPE: Release
13+
14+
concurrency:
15+
group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }}
16+
cancel-in-progress: ${{ github.event_name == 'pull_request' }}
17+
18+
jobs:
19+
build:
20+
name: ${{ matrix.build_name }}
21+
runs-on: ${{ matrix.os }}
22+
strategy:
23+
fail-fast: true
24+
matrix:
25+
build_name: [ubuntu-22.04, ubuntu-24.04, ubuntu-24.04-with-opengv]
26+
include:
27+
- build_name: ubuntu-22.04
28+
os: ubuntu-22.04
29+
extra_deps: "libunwind-dev libceres-dev"
30+
extra_cmake_def: "-DWITH_CERES=ON"
31+
- build_name: ubuntu-24.04
32+
os: ubuntu-24.04
33+
extra_deps: "libg2o-dev libceres-dev"
34+
extra_cmake_def: "-DWITH_CERES=ON"
35+
- build_name: ubuntu-24.04-with-opengv
36+
os: ubuntu-24.04
37+
extra_deps: "libg2o-dev libceres-dev"
38+
extra_cmake_def: "-DWITH_CERES=ON -DBUILD_OPENGV=ON"
39+
40+
steps:
41+
- uses: actions/checkout@v4
42+
43+
- name: Install Linux Dependencies
44+
run: |
45+
DEBIAN_FRONTEND=noninteractive
46+
sudo apt-get update
47+
sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev ${{ matrix.extra_deps }}
48+
49+
- name: Configure CMake
50+
run: |
51+
cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{ matrix.extra_cmake_def }}
52+
53+
- name: Build
54+
run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
55+
56+
- name: Info
57+
working-directory: ${{github.workspace}}/build/bin
58+
run: |
59+
./rtabmap-console --version
60+
61+
# - name: Test
62+
# working-directory: ${{github.workspace}}/build
63+
# # Execute tests defined by the CMake configuration.
64+
# # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail
65+
# run: ctest -C ${{env.BUILD_TYPE}}
66+

.github/workflows/cmake.yml renamed to .github/workflows/cmake-windows.yml

Lines changed: 3 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
name: CMake
1+
name: CMake-Windows
22

33
on:
44
push:
@@ -22,20 +22,8 @@ jobs:
2222
strategy:
2323
fail-fast: true
2424
matrix:
25-
build_name: [ubuntu-22.04, ubuntu-24.04, ubuntu-24.04-with-opengv, windows-2022, windows-2022-cuda]
25+
build_name: [windows-2022, windows-2022-cuda]
2626
include:
27-
- build_name: ubuntu-22.04
28-
os: ubuntu-22.04
29-
extra_deps: "libunwind-dev libceres-dev"
30-
extra_cmake_def: "-DWITH_CERES=ON"
31-
- build_name: ubuntu-24.04
32-
os: ubuntu-24.04
33-
extra_deps: "libg2o-dev libceres-dev"
34-
extra_cmake_def: "-DWITH_CERES=ON"
35-
- build_name: ubuntu-24.04-with-opengv
36-
os: ubuntu-24.04
37-
extra_deps: "libg2o-dev libceres-dev"
38-
extra_cmake_def: "-DWITH_CERES=ON -DBUILD_OPENGV=ON"
3927
- build_name: windows-2022
4028
os: windows-2022
4129
extra_deps: ""
@@ -56,15 +44,7 @@ jobs:
5644
if: matrix.build_name == 'windows-2022-cuda'
5745
uses: ./.github/actions/install-windows-cuda-deps
5846

59-
- name: Install Linux Dependencies
60-
if: matrix.os != 'windows-2022'
61-
run: |
62-
DEBIAN_FRONTEND=noninteractive
63-
sudo apt-get update
64-
sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common libyaml-cpp-dev ${{ matrix.extra_deps }}
65-
6647
- name: Configure CMake
67-
if: matrix.os == 'windows-2022'
6848
run: |
6949
cmake `
7050
-B ${{github.workspace}}/build `
@@ -76,16 +56,10 @@ jobs:
7656
-DCMAKE_TOOLCHAIN_FILE=${{env.VCPKG_EXPORT_PATH}}/scripts/buildsystems/vcpkg.cmake `
7757
-DTorch_DIR=${{env.VCPKG_EXPORT_PATH}}/installed/x64-windows-release/tools/python3/Lib/site-packages/torch/share/cmake/Torch
7858
79-
- name: Configure CMake
80-
if: matrix.os != 'windows-2022'
81-
run: |
82-
cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} ${{ matrix.extra_cmake_def }}
83-
8459
- name: Build
8560
run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
8661

8762
- name: Build Windows Package
88-
if: matrix.os == 'windows-2022'
8963
shell: pwsh
9064
run: |
9165
if ("${{ github.event_name }}" -eq "pull_request") {
@@ -105,7 +79,6 @@ jobs:
10579
./rtabmap-console --version
10680
10781
- name: Upload RTABMap Artifacts (ZIP)
108-
if: matrix.os == 'windows-2022'
10982
uses: actions/upload-artifact@v4
11083
with:
11184
name: RTABMap-Binaries-${{ matrix.build_name }}-zip
@@ -116,7 +89,7 @@ jobs:
11689
retention-days: ${{ github.event_name == 'pull_request' && 1 || 90 }}
11790

11891
- name: Upload RTABMap Artifacts (Installer)
119-
if: matrix.os == 'windows-2022' && github.event_name != 'pull_request'
92+
if: github.event_name != 'pull_request'
12093
uses: actions/upload-artifact@v4
12194
with:
12295
name: RTABMap-Binaries-${{ matrix.build_name }}-exe

‎README.md‎

Lines changed: 7 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@ rtabmap
77
[![Downloads][downloads-image]][downloads]
88
[![License][license-image]][license]
99

10-
[release-image]: https://img.shields.io/badge/release-0.21.4-green.svg?style=flat
10+
[release-image]: https://img.shields.io/badge/release-0.23.1-green.svg?style=flat
1111
[releases]: https://github.com/introlab/rtabmap/releases
1212

1313
[downloads-image]: https://img.shields.io/github/downloads/introlab/rtabmap/total?label=downloads
@@ -35,13 +35,7 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated
3535
<table>
3636
<tbody>
3737
<tr>
38-
<td>Linux</td>
39-
<td><a href="https://github.com/introlab/rtabmap/actions/workflows/cmake.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake.yml/badge.svg" alt="Build Status"/> <br> <a href="https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml/badge.svg" alt="Build Status"/> <br> <a href="https://github.com/introlab/rtabmap/actions/workflows/docker.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/docker.yml/badge.svg" alt="Build Status"/>
40-
</td>
41-
</tr>
42-
<tr>
43-
<td>Windows</td>
44-
<td><a href="https://ci.appveyor.com/project/matlabbe/rtabmap/branch/master"><img src="https://ci.appveyor.com/api/projects/status/hr73xspix9oqa26h/branch/master?svg=true" alt="Build Status"/>
38+
<td><a href="https://github.com/introlab/rtabmap/actions/workflows/cmake-linux.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-linux.yml/badge.svg" alt="CMake Linux Build Status"/> <br> <a href="https://github.com/introlab/rtabmap/actions/workflows/cmake-windows.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-windows.yml/badge.svg" alt="CMake Windows Build Status"/> <br> <a href="https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml/badge.svg" alt="CMake ROS Build Status"/> <br> <a href="https://github.com/introlab/rtabmap/actions/workflows/docker.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/docker.yml/badge.svg" alt="Docker Build Status"/>
4539
</td>
4640
</tr>
4741
</tbody>
@@ -59,14 +53,18 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated
5953
<td><a href="http://build.ros.org/job/Nbin_ufv8_uFv8__rtabmap__ubuntu_focal_arm64__binary/"><img src="http://build.ros.org/buildStatus/icon?job=Nbin_ufv8_uFv8__rtabmap__ubuntu_focal_arm64__binary" alt="Build Status"/></td>
6054
</tr>
6155
<tr>
62-
<td rowspan="3">ROS 2</td>
56+
<td rowspan="4">ROS 2</td>
6357
<td>Humble</td>
6458
<td><a href="http://build.ros2.org/job/Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
6559
</tr>
6660
<tr>
6761
<td>Jazzy</td>
6862
<td><a href="http://build.ros2.org/job/Jbin_uN64__rtabmap__ubuntu_noble_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Jbin_uN64__rtabmap__ubuntu_noble_amd64__binary" alt="Build Status"/></td>
6963
</tr>
64+
<tr>
65+
<td>Kilted</td>
66+
<td><a href="http://build.ros2.org/job/Kbin_uN64__rtabmap__ubuntu_noble_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Kbin_uN64__rtabmap__ubuntu_noble_amd64__binary" alt="Build Status"/></td>
67+
</tr>
7068
<tr>
7169
<td>Rolling</td>
7270
<td><a href="http://build.ros2.org/job/Rbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>

‎corelib/include/rtabmap/core/Features2d.h‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -309,7 +309,8 @@ class RTABMAP_CORE_EXPORT SIFT : public Feature2D
309309
bool preciseUpscale_;
310310
bool rootSIFT_;
311311
bool gpu_;
312-
float guaussianThreshold_;
312+
float gaussianThreshold_;
313+
float maxGaussianThreshold_;
313314
bool upscale_;
314315

315316
cv::Ptr<CV_SIFT> sift_;

‎corelib/include/rtabmap/core/Parameters.h‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -224,7 +224,7 @@ class RTABMAP_CORE_EXPORT Parameters
224224
RTABMAP_PARAM(Mem, BadSignaturesIgnored, bool, false, "Bad signatures are ignored.");
225225
RTABMAP_PARAM(Mem, InitWMWithAllNodes, bool, false, "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
226226
RTABMAP_PARAM(Mem, DepthAsMask, bool, true, "Use depth image as mask when extracting features for vocabulary.");
227-
RTABMAP_PARAM(Mem, DepthMaskFloorThr, float, 0.0, uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled, negative means remove all objects above the floor threshold instead. Ignored if %s is false.", kMemDepthAsMask().c_str()));
227+
RTABMAP_PARAM(Mem, DepthMaskFloorThr, float, 0.0, uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled. Ignored if %s is false.", kMemDepthAsMask().c_str()));
228228
RTABMAP_PARAM(Mem, StereoFromMotion, bool, false, uFormat("Triangulate features without depth using stereo from motion (odometry). It would be ignored if %s is true and the feature detector used supports masking.", kMemDepthAsMask().c_str()));
229229
RTABMAP_PARAM(Mem, ImagePreDecimation, unsigned int, 1, uFormat("Decimation of the RGB image before visual feature detection. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If %s is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.",kMemDepthAsMask().c_str()));
230230
RTABMAP_PARAM(Mem, ImagePostDecimation, unsigned int, 1, uFormat("Decimation of the RGB image before saving it to database. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. Decimation is done from the original image. If set to same value than %s, data already decimated is saved (no need to re-decimate the image).", kMemImagePreDecimation().c_str()));
@@ -294,7 +294,8 @@ class RTABMAP_CORE_EXPORT Parameters
294294
RTABMAP_PARAM(SIFT, PreciseUpscale, bool, false, "Whether to enable precise upscaling in the scale pyramid (OpenCV >= 4.8).");
295295
RTABMAP_PARAM(SIFT, RootSIFT, bool, false, "Apply RootSIFT normalization of the descriptors.");
296296
RTABMAP_PARAM(SIFT, Gpu, bool, false, "CudaSift: Use GPU version of SIFT. This option is enabled only if RTAB-Map is built with CudaSift dependency and GPUs are detected.");
297-
RTABMAP_PARAM(SIFT, GaussianThreshold, float, 2.0, "CudaSift: Threshold on difference of Gaussians for feature pruning. The higher the threshold, the less features are produced by the detector.");
297+
RTABMAP_PARAM(SIFT, GaussianThreshold, float, 2.0, "CudaSift: Threshold on difference of Gaussians for feature pruning. The higher the threshold, the less features with low response/hessian are produced by the detector.");
298+
RTABMAP_PARAM(SIFT, MaxGaussianThreshold, float, 0.0, uFormat("CudaSift: Maximum threshold on difference of Gaussians for feature pruning (ignored if smaller or equal than %s). The lower the threshold, the less features with high response/hessian are produced by the detector.", kSIFTGaussianThreshold().c_str()));
298299
RTABMAP_PARAM(SIFT, Upscale, bool, false, "CudaSift: Whether to enable upscaling.");
299300

300301
RTABMAP_PARAM(BRIEF, Bytes, int, 32, "Bytes is a length of descriptor in bytes. It can be equal 16, 32 or 64 bytes.");
@@ -724,7 +725,7 @@ class RTABMAP_CORE_EXPORT Parameters
724725
RTABMAP_PARAM(Vis, MaxDepth, float, 0, "Max depth of the features (0 means no limit).");
725726
RTABMAP_PARAM(Vis, MinDepth, float, 0, "Min depth of the features (0 means no limit).");
726727
RTABMAP_PARAM(Vis, DepthAsMask, bool, true, "Use depth image as mask when extracting features.");
727-
RTABMAP_PARAM(Vis, DepthMaskFloorThr, float, 0.0, uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled, negative means remove all objects above the floor threshold instead. Ignored if %s is false.", kVisDepthAsMask().c_str()));
728+
RTABMAP_PARAM(Vis, DepthMaskFloorThr, float, 0.0, uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled. Ignored if %s is false.", kVisDepthAsMask().c_str()));
728729
RTABMAP_PARAM_STR(Vis, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
729730
RTABMAP_PARAM(Vis, SubPixWinSize, int, 3, "See cv::cornerSubPix().");
730731
RTABMAP_PARAM(Vis, SubPixIterations, int, 0, "See cv::cornerSubPix(). 0 disables sub pixel refining.");

‎corelib/include/rtabmap/core/camera/CameraImages.h‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -102,10 +102,11 @@ class RTABMAP_CORE_EXPORT CameraImages :
102102
}
103103

104104
// 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV, 12=rgbd_bonn
105-
void setGroundTruthPath(const std::string & filePath, int format = 0)
105+
void setGroundTruthPath(const std::string & filePath, int format = 0, const Transform & localTransform = Transform::getIdentity())
106106
{
107107
_groundTruthPath = filePath;
108108
_groundTruthFormat = format;
109+
_groundTruthLocalTransform = localTransform;
109110
}
110111

111112
void setMaxPoseTimeDiff(double diff) {_maxPoseTimeDiff = diff;}
@@ -164,6 +165,7 @@ class RTABMAP_CORE_EXPORT CameraImages :
164165
int _odometryFormat;
165166
std::string _groundTruthPath;
166167
int _groundTruthFormat;
168+
Transform _groundTruthLocalTransform;
167169
double _maxPoseTimeDiff;
168170

169171
std::list<double> _stamps;

‎corelib/src/CameraModel.cpp‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -353,6 +353,22 @@ bool CameraModel::load(const std::string & filePath)
353353
data[0], data[1], data[2], data[3],
354354
data[4], data[5], data[6], data[7],
355355
data[8], data[9], data[10], data[11]);
356+
Transform detCheck = localTransform_.clone();
357+
localTransform_.normalizeRotation(); /// Normalize by default
358+
float det = detCheck.toEigen3f().linear().determinant();
359+
if(fabs(det - 1.0f) > 0.0001)
360+
{
361+
std::stringstream streamBefore, streamAfter;
362+
streamBefore << detCheck << std::endl;
363+
streamAfter << localTransform_ << std::endl;
364+
UWARN("The camera model's local_transform from \"%s\" doesn't "
365+
"have a normalized rotation matrix (dertminant=%f). We will normalize "
366+
"it for convenience.\nWas:\n%sNow\n%s",
367+
filePath.c_str(),
368+
det,
369+
streamBefore.str().c_str(),
370+
streamAfter.str().c_str());
371+
}
356372
}
357373
else
358374
{

0 commit comments

Comments
 (0)