mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-05 17:47:49 +08:00
Compare commits
6
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 matlabbe
|
d1e5c61c1b | ||
|
matlabbe
|
74c0095c2c | ||
|
matlabbe
|
d10f2d9738 | ||
|
matlabbe
|
33c433baba | ||
|
matlabbe
|
a58024e0c5 | ||
|
matlabbe
|
bdb3e3fab8 |
@@ -3,7 +3,7 @@ name: CMake-ROS
|
||||
on:
|
||||
push:
|
||||
branches:
|
||||
- master
|
||||
- rolling-devel
|
||||
paths-ignore: &platform_only
|
||||
- '.github/workflows/android.yml'
|
||||
- '.github/workflows/ios.yml'
|
||||
@@ -21,31 +21,25 @@ env:
|
||||
|
||||
concurrency:
|
||||
group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }}
|
||||
cancel-in-progress: ${{ github.event_name == 'pull_request' }}
|
||||
cancel-in-progress: true
|
||||
|
||||
jobs:
|
||||
build:
|
||||
name: ${{ matrix.ros_distribution }}
|
||||
name: ${{ matrix.ros_distribution }}${{ matrix.use_ros2_testing && '-testing' || '' }}
|
||||
runs-on: ubuntu-latest
|
||||
concurrency:
|
||||
group: ${{ github.workflow }}-${{ github.ref }}-${{ matrix.ros_distribution }}
|
||||
group: ${{ github.workflow }}-${{ github.ref }}-${{ matrix.ros_distribution }}-${{ matrix.use_ros2_testing }}
|
||||
cancel-in-progress: true
|
||||
strategy:
|
||||
fail-fast: false
|
||||
matrix:
|
||||
ros_distribution: [ humble, jazzy, kilted, lyrical, rolling]
|
||||
ros_distribution: [ rolling ]
|
||||
# Testing only: the osrf/ros:rolling image is already set up on ros2-testing,
|
||||
# and rolling's main repo is just a periodic snapshot of it.
|
||||
use_ros2_testing: [ true ]
|
||||
include:
|
||||
- ros_distribution: 'humble'
|
||||
skip_keys: ""
|
||||
- ros_distribution: 'jazzy'
|
||||
skip_keys: ""
|
||||
- ros_distribution: 'kilted'
|
||||
skip_keys: ""
|
||||
- ros_distribution: 'lyrical'
|
||||
skip_keys: "libpointmatcher"
|
||||
- ros_distribution: 'rolling'
|
||||
skip_keys: "libpointmatcher gtsam"
|
||||
use_ros2_testing: true # Rolling is using ros2-testing (nightly)
|
||||
skip_keys: "" # When releasing to ROS2, the skip_keys should be empty, patch these deps in package.xml instead.
|
||||
container:
|
||||
image: osrf/ros:${{ matrix.ros_distribution }}-desktop-full
|
||||
steps:
|
||||
@@ -94,10 +88,23 @@ jobs:
|
||||
ls "$root"/tests/*.db >/dev/null || { echo "::error::no test databases in $root/tests"; exit 1; }
|
||||
echo "root=$root" >> "$GITHUB_OUTPUT"
|
||||
|
||||
# The osrf/ros image ships ros2-apt-source (main), which conflicts with the
|
||||
# ros2-testing-apt-source package setup-ros tries to install. Remove it so
|
||||
# setup-ros can switch the image to the testing repo.
|
||||
- name: Remove ROS main apt source
|
||||
if: matrix.use_ros2_testing
|
||||
run: dpkg --purge ros2-apt-source
|
||||
|
||||
- uses: ros-tooling/[email protected]
|
||||
with:
|
||||
required-ros-distributions: ${{ matrix.ros_distribution }}
|
||||
use-ros2-testing: ${{ matrix.use_ros2_testing || false }}
|
||||
use-ros2-testing: ${{ matrix.use_ros2_testing }}
|
||||
|
||||
# setup-ros doesn't upgrade on noble/resolute, so the packages preinstalled
|
||||
# in the image would stay at their main versions. Upgrade them to testing.
|
||||
- name: Upgrade ROS packages to testing
|
||||
if: matrix.use_ros2_testing
|
||||
run: apt-get update && apt-get dist-upgrade -y
|
||||
- uses: ros-tooling/[email protected]
|
||||
with:
|
||||
package-name: rtabmap
|
||||
|
||||
+2
-2
@@ -21,8 +21,8 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
|
||||
# VERSION
|
||||
#######################
|
||||
SET(RTABMAP_MAJOR_VERSION 0)
|
||||
SET(RTABMAP_MINOR_VERSION 24)
|
||||
SET(RTABMAP_PATCH_VERSION 0)
|
||||
SET(RTABMAP_MINOR_VERSION 23)
|
||||
SET(RTABMAP_PATCH_VERSION 13)
|
||||
SET(RTABMAP_VERSION
|
||||
${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
|
||||
|
||||
|
||||
@@ -8,7 +8,7 @@ rtabmap
|
||||
[](https://codecov.io/gh/introlab/rtabmap)
|
||||
[![License][license-image]][license]
|
||||
|
||||
[release-image]: https://img.shields.io/github/v/release/introlab/rtabmap?color=green&style=flat
|
||||
[release-image]: https://img.shields.io/badge/release-0.23.1-green.svg?style=flat
|
||||
[releases]: https://github.com/introlab/rtabmap/releases
|
||||
|
||||
[downloads-image]: https://img.shields.io/github/downloads/introlab/rtabmap/total?label=downloads
|
||||
@@ -34,26 +34,59 @@ This project is supported by [IntRoLab - Intelligent / Interactive / Integrated
|
||||
|
||||
#### CI Latest
|
||||
|
||||
| | Build |
|
||||
|---|---|
|
||||
| Desktop | [](https://github.com/introlab/rtabmap/actions/workflows/cmake-linux.yml) [](https://github.com/introlab/rtabmap/actions/workflows/cmake-windows.yml) [](https://github.com/introlab/rtabmap/actions/workflows/cmake-macos.yml) |
|
||||
| ROS | [](https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml) [](https://github.com/introlab/rtabmap/actions/workflows/docker-ros.yml) |
|
||||
| Mobile | [](https://github.com/introlab/rtabmap/actions/workflows/android.yml) [](https://github.com/introlab/rtabmap/actions/workflows/ios.yml) |
|
||||
| Quality | [](https://github.com/introlab/rtabmap/actions/workflows/coverage.yml) [](https://github.com/introlab/rtabmap/actions/workflows/docs.yml) |
|
||||
<table>
|
||||
<tbody>
|
||||
<tr>
|
||||
<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-macos.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/cmake-macos.yml/badge.svg" alt="CMake MaCOS 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-ros.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/docker-ros.yml/badge.svg" alt="Docker ROS Build Status"/> <br>
|
||||
<a href="https://github.com/introlab/rtabmap/actions/workflows/android.yml"><img src="https://github.com/introlab/rtabmap/actions/workflows/android.yml/badge.svg" alt="Android Build Status"/>
|
||||
</td>
|
||||
</tr>
|
||||
</tbody>
|
||||
</table>
|
||||
|
||||
#### ROS Binaries
|
||||
|
||||
`ros-$ROS_DISTRO-rtabmap`
|
||||
|
||||
| | Distro | Ubuntu | Released | In apt | Build |
|
||||
|---|---|---|---|---|---|
|
||||
| ROS 1 | Noetic (EOL) | 20.04 | [](https://github.com/ros/rosdistro/blob/master/noetic/distribution.yaml) | [](https://index.ros.org/p/rtabmap/#noetic) | |
|
||||
| ROS 2 | Humble | 22.04 | [](https://github.com/ros/rosdistro/blob/master/humble/distribution.yaml) | [](https://index.ros.org/p/rtabmap/#humble) | [](http://build.ros2.org/job/Hbin_uJ64__rtabmap__ubuntu_jammy_amd64__binary/) |
|
||||
| ROS 2 | Iron (EOL) | 22.04 | [](https://github.com/ros/rosdistro/blob/master/iron/distribution.yaml) | [](https://index.ros.org/p/rtabmap/#iron) | |
|
||||
| ROS 2 | Jazzy | 24.04 | [](https://github.com/ros/rosdistro/blob/master/jazzy/distribution.yaml) | [](https://index.ros.org/p/rtabmap/#jazzy) | [](http://build.ros2.org/job/Jbin_uN64__rtabmap__ubuntu_noble_amd64__binary/) |
|
||||
| ROS 2 | Kilted | 24.04 | [](https://github.com/ros/rosdistro/blob/master/kilted/distribution.yaml) | [](https://index.ros.org/p/rtabmap/#kilted) | [](http://build.ros2.org/job/Kbin_uN64__rtabmap__ubuntu_noble_amd64__binary/) |
|
||||
| ROS 2 | Lyrical | 26.04 | [](https://github.com/ros/rosdistro/blob/master/lyrical/distribution.yaml) | [](https://index.ros.org/p/rtabmap/#lyrical) | [](http://build.ros2.org/job/Lbin_uR64__rtabmap__ubuntu_resolute_amd64__binary/) |
|
||||
| ROS 2 | Rolling | 26.04 | [](https://github.com/ros/rosdistro/blob/master/rolling/distribution.yaml) | [](https://index.ros.org/p/rtabmap/#rolling) | [](http://build.ros2.org/job/Rbin_uR64__rtabmap__ubuntu_resolute_amd64__binary/) |
|
||||
| Docker | [rtabmap](https://hub.docker.com/r/introlab3it/rtabmap) | | |  | |
|
||||
|
||||
*Released* is the version bloomed into [rosdistro](https://github.com/ros/rosdistro); *In apt* is what `apt install` actually gives you today. They differ while a release is waiting on a buildfarm sync.
|
||||
<table>
|
||||
<tbody>
|
||||
<tr>
|
||||
<td rowspan="1">ROS 1</td>
|
||||
<td>Noetic</td>
|
||||
<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>
|
||||
</tr>
|
||||
<tr>
|
||||
<td rowspan="5">ROS 2</td>
|
||||
<td>Humble</td>
|
||||
<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>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>Jazzy</td>
|
||||
<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>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>Kilted</td>
|
||||
<td><a href="http://build.ros2.org/job/Lbin_uR64__rtabmap__ubuntu_resolute_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Lbin_uR64__rtabmap__ubuntu_resolute_amd64__binary" alt="Build Status"/></td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>Lyrical</td>
|
||||
<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>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>Rolling</td>
|
||||
<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>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>Docker</td>
|
||||
<td>
|
||||
<a href="https://hub.docker.com/r/introlab3it/rtabmap">rtabmap</a>
|
||||
</td>
|
||||
<td><img src="https://img.shields.io/docker/pulls/introlab3it/rtabmap" alt="Docker Pulls"/></td>
|
||||
</tr>
|
||||
</tbody>
|
||||
</table>
|
||||
|
||||
@@ -1078,7 +1078,7 @@
|
||||
"\"$(SRCROOT)/RTABMapApp/Libraries/include\"",
|
||||
"\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"",
|
||||
"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.15\"",
|
||||
"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.24\"",
|
||||
"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.23\"",
|
||||
"\"$(SRCROOT)/../android/jni/tango-gl/include\"",
|
||||
"\"$(SRCROOT)/../android/jni/third-party/include\"",
|
||||
"\"$(SRCROOT)/RTABMapApp/Libraries/lib/vtk.framework/Headers\"",
|
||||
@@ -1139,7 +1139,7 @@
|
||||
"\"$(SRCROOT)/RTABMapApp/Libraries/include\"",
|
||||
"\"$(SRCROOT)/RTABMapApp/Libraries/include/eigen3\"",
|
||||
"\"$(SRCROOT)/RTABMapApp/Libraries/include/pcl-1.15\"",
|
||||
"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.24\"",
|
||||
"\"$(SRCROOT)/RTABMapApp/Libraries/include/rtabmap-0.23\"",
|
||||
"\"$(SRCROOT)/../android/jni/tango-gl/include\"",
|
||||
"\"$(SRCROOT)/../android/jni/third-party/include\"",
|
||||
"\"$(SRCROOT)/RTABMapApp/Libraries/lib/vtk.framework/Headers\"",
|
||||
|
||||
@@ -41,8 +41,7 @@ namespace rtabmap {
|
||||
* @brief Background thread to compress or uncompress images and generic matrices.
|
||||
*
|
||||
* In compress mode, pass a source matrix to the constructor with an optional image
|
||||
* format (".png", ".jpg", ".rvl", or empty for zlib data, see @ref compressImage() for
|
||||
* depth options). In uncompress mode, pass
|
||||
* format (".png", ".jpg", ".rvl", or empty for zlib data). In uncompress mode, pass
|
||||
* compressed bytes and set @c isImage accordingly. Call @ref UThread::start() then
|
||||
* @ref UThread::join() to obtain the result from @ref getCompressedData() or
|
||||
* @ref getUncompressedData().
|
||||
@@ -71,7 +70,7 @@ public:
|
||||
/**
|
||||
* @brief Constructs a thread in compress mode.
|
||||
* @param mat Source image or data matrix to compress.
|
||||
* @param format Image format: @c ".png", @c ".jpg", @c ".rvl" (see @ref compressImage()), or empty for zlib (@ref compressData2).
|
||||
* @param format Image format: @c ".png", @c ".jpg", @c ".rvl", or empty for zlib (@ref compressData2).
|
||||
*/
|
||||
CompressionThread(const cv::Mat & mat, const std::string & format = "");
|
||||
/**
|
||||
@@ -94,63 +93,7 @@ private:
|
||||
bool compressMode_;
|
||||
};
|
||||
|
||||
/*
|
||||
* Compressed depth image layouts (all values little-endian). They are stable: they are
|
||||
* saved in databases, and rtabmap_ros converts them to and from ROS's
|
||||
* compressed_depth_image_transport messages without decompressing the images.
|
||||
*
|
||||
* - ".png": a standard PNG file. 16UC1 depth images are 16 bits grayscale PNGs,
|
||||
* 32FC1 depth images (legacy) are 4-channel 8 bits PNGs holding the float bytes.
|
||||
* - ".rvl" (16UC1):
|
||||
* [0..7] "DEPTHRVL"
|
||||
* [8..11] uint32 cols
|
||||
* [12..15] uint32 rows
|
||||
* [16..] RVL data (see RvlCodec)
|
||||
* - ".png:<maxDepth>:<quantization>" or ".rvl:<maxDepth>:<quantization>" (32FC1):
|
||||
* [0..7] "DEPTHINV"
|
||||
* [8..11] float depthQuantA = quantization*(quantization+1)
|
||||
* [12..15] float depthQuantB = 1 - depthQuantA/maxDepth
|
||||
* [16..] the 16UC1 inverse depth image in ".png" or ".rvl" layout above, where
|
||||
* 0 is invalid and v>0 is the depth depthQuantA/(v-depthQuantB).
|
||||
*/
|
||||
|
||||
/** @brief Signature of the ".rvl" layout (8 bytes, not null-terminated). */
|
||||
const char kCompressedDepthRvlSignature[8] = {'D', 'E', 'P', 'T', 'H', 'R', 'V', 'L'};
|
||||
/** @brief Size of the ".rvl" header: signature, uint32 cols, uint32 rows. */
|
||||
const size_t kCompressedDepthRvlHeaderSize = 16;
|
||||
/** @brief Signature of the inverse depth layout (8 bytes, not null-terminated). */
|
||||
const char kCompressedDepthInvSignature[8] = {'D', 'E', 'P', 'T', 'H', 'I', 'N', 'V'};
|
||||
/** @brief Size of the inverse depth header: signature, float depthQuantA, float depthQuantB. */
|
||||
const size_t kCompressedDepthInvHeaderSize = 16;
|
||||
|
||||
/**
|
||||
* @brief Parses an image compression format "<codec>[:<maxDepth>[:<quantization>]]".
|
||||
*
|
||||
* @param format Format, e.g., @c ".jpg", @c ".png", @c ".rvl", @c ".png:10" or @c ".rvl:20:100".
|
||||
* The empty format is valid (general zlib compression, see @ref CompressionThread).
|
||||
* @param codec Output codec (e.g., @c ".png").
|
||||
* @param maxDepth Output maximum depth (m) of the inverse depth format, 0 if not set.
|
||||
* @param quantization Output depth quantization of the inverse depth format
|
||||
* (100 if not set but @p maxDepth is), 0 if @p maxDepth is not set.
|
||||
* @return false if the format is invalid. The inverse depth parameters are only
|
||||
* valid with @c ".png" and @c ".rvl", and should be positive.
|
||||
*/
|
||||
bool RTABMAP_CORE_EXPORT parseImageCompressionFormat(const std::string & format, std::string & codec, float & maxDepth, float & quantization);
|
||||
|
||||
/**
|
||||
* @brief Encodes @p image to a byte buffer (OpenCV @c imencode or RVL for depth).
|
||||
*
|
||||
* @param format @c ".png", @c ".jpg" or @c ".rvl" (16UC1 only), optionally followed by
|
||||
* @c ":<maxDepth>[:<quantization>]" (see @ref parseImageCompressionFormat()).
|
||||
* For 32FC1 depth images, if @c maxDepth is set, depth is quantized on 16 bits
|
||||
* as inverse depth (as ROS's @c compressed_depth_image_transport) and compressed
|
||||
* with the codec. With A=quantization*(quantization+1) and B=1-A/maxDepth, the
|
||||
* precision is ~d^2/(2A), and depth values over @c maxDepth or under A/(65535-B)
|
||||
* are lost (set to 0). For example, ".png:10:100" keeps depth between 0.15 and
|
||||
* 10 m with errors of 0.05 mm at 1 m and 5 mm at 10 m. Otherwise, 32FC1 depth
|
||||
* images are compressed losslessly as 4-channel 8 bits PNG (legacy format). Other
|
||||
* image types ignore the depth parameters.
|
||||
*/
|
||||
/** @brief Encodes @p image to a byte buffer (OpenCV @c imencode or RVL for depth). */
|
||||
std::vector<unsigned char> RTABMAP_CORE_EXPORT compressImage(const cv::Mat & image, const std::string & format = ".png");
|
||||
/** @brief Same as @ref compressImage() but returns a @c CV_8UC1 row matrix. */
|
||||
cv::Mat RTABMAP_CORE_EXPORT compressImage2(const cv::Mat & image, const std::string & format = ".png");
|
||||
@@ -159,8 +102,6 @@ cv::Mat RTABMAP_CORE_EXPORT compressImage2(const cv::Mat & image, const std::str
|
||||
cv::Mat RTABMAP_CORE_EXPORT uncompressImage(const cv::Mat & bytes);
|
||||
/** @brief Decodes compressed image bytes to a @cv::Mat. */
|
||||
cv::Mat RTABMAP_CORE_EXPORT uncompressImage(const std::vector<unsigned char> & bytes);
|
||||
/** @brief Decodes compressed image bytes to a @cv::Mat. */
|
||||
cv::Mat RTABMAP_CORE_EXPORT uncompressImage(const unsigned char * bytes, size_t size);
|
||||
|
||||
/** @brief Compresses a matrix with zlib; appends rows, cols and type at the end. */
|
||||
std::vector<unsigned char> RTABMAP_CORE_EXPORT compressData(const cv::Mat & data);
|
||||
@@ -181,10 +122,7 @@ std::string RTABMAP_CORE_EXPORT uncompressString(const cv::Mat & bytes);
|
||||
|
||||
/**
|
||||
* @brief Detects the compression format of depth image bytes.
|
||||
* @return @c ".rvl" if the buffer has an RVL signature, @c ".png:<maxDepth>:<quantization>"
|
||||
* or @c ".rvl:<maxDepth>:<quantization>" for inverse depth images (see
|
||||
* @ref compressImage()), otherwise @c ".png". The returned format can be passed
|
||||
* back to @ref compressImage() to compress in the same format.
|
||||
* @return @c ".rvl" if the buffer has an RVL signature, otherwise @c ".png".
|
||||
*/
|
||||
std::string RTABMAP_CORE_EXPORT compressedDepthFormat(const cv::Mat & bytes);
|
||||
/** @overload */
|
||||
@@ -192,6 +130,5 @@ std::string RTABMAP_CORE_EXPORT compressedDepthFormat(const std::vector<unsigned
|
||||
/** @overload */
|
||||
std::string RTABMAP_CORE_EXPORT compressedDepthFormat(const unsigned char * bytes, size_t size);
|
||||
|
||||
|
||||
} /* namespace rtabmap */
|
||||
#endif /* COMPRESSION_H_ */
|
||||
|
||||
@@ -848,7 +848,6 @@ private:
|
||||
unsigned int _imagePreDecimation;
|
||||
unsigned int _imagePostDecimation;
|
||||
bool _legacyDecimatedOctave;
|
||||
bool _inverseDepthCompressionAllowed; // database version >= 0.24
|
||||
bool _compressionParallelized;
|
||||
float _laserScanDownsampleStepSize;
|
||||
float _laserScanVoxelSize;
|
||||
|
||||
@@ -222,7 +222,7 @@ class RTABMAP_CORE_EXPORT Parameters
|
||||
RTABMAP_PARAM(Mem, NotLinkedNodesKept, bool, true, "Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
|
||||
RTABMAP_PARAM(Mem, IntermediateNodeDataKept, bool, false, "Keep intermediate node data in db.");
|
||||
RTABMAP_PARAM_STR(Mem, ImageCompressionFormat, ".jpg", "RGB image compression format. It should be \".jpg\" or \".png\".");
|
||||
RTABMAP_PARAM_STR(Mem, DepthCompressionFormat, ".rvl", "Depth image compression format. It should be \".png\" or \".rvl\", optionally followed by \":maxDepth[:quantization]\" (e.g., \".rvl:10:100\", quantization is 100 by default) to compress 32FC1 depth images as 16 bits inverse depth with that codec (same quantization than ROS's compressed_depth_image_transport). 16UC1 depth images are always compressed losslessly with the codec. Warning: the inverse depth format is lossy, the precision is ~d^2/(2*q*(q+1)) (q=quantization, e.g., 0.05 mm at 1 m and 5 mm at 10 m with q=100) and depth values over maxDepth or under ~q*(q+1)/65535 meters (0.15 m with q=100) are lost. Without depth parameters, 32FC1 depth images are compressed losslessly in \".png\" format (4 channels 8 bits). Databases with depth images compressed in inverse depth format cannot be opened by rtabmap versions under 0.24.");
|
||||
RTABMAP_PARAM_STR(Mem, DepthCompressionFormat, ".rvl", "Depth image compression format for 16UC1 depth type. It should be \".png\" or \".rvl\". If depth type is 32FC1, \".png\" is used.");
|
||||
RTABMAP_PARAM(Mem, STMSize, unsigned int, 10, "Short-term memory size.");
|
||||
RTABMAP_PARAM(Mem, IncrementalMemory, bool, true, "SLAM mode, otherwise it is Localization mode.");
|
||||
RTABMAP_PARAM(Mem, LocalizationReadOnly, bool, false, uFormat("In localization mode, open the database in read-only mode (ignored if %s=true). Currrenty incompatible with memory management (%s and %s cannot be used) and if there are disjoint sessions in working memory. Last localization pose won't be saved back in the database at the end of the session, so the robot will always restart to original last localization pose, unless %s is used or an external initial pose is provided on initialization.", kMemIncrementalMemory().c_str(), kRtabmapLoopThr().c_str(), kRtabmapMemoryThr().c_str(), kRGBDStartAtOrigin().c_str()).c_str());
|
||||
|
||||
@@ -598,8 +598,6 @@ public:
|
||||
/**
|
||||
* Set image data. Detect automatically if raw or compressed.
|
||||
* A matrix of type CV_8UC1 with 1 row is considered as compressed.
|
||||
* An invalid @p model (not CameraModel::isValidForProjection()) without any image is
|
||||
* a placeholder (e.g., scan-only data): it is not added, so cameraModels() is empty.
|
||||
* @param clearPreviousData, clear previous raw and compressed images before setting the new ones.
|
||||
*/
|
||||
void setRGBDImage(const cv::Mat & rgb, const cv::Mat & depth, const CameraModel & model, bool clearPreviousData = true);
|
||||
@@ -1027,9 +1025,6 @@ public:
|
||||
#endif
|
||||
|
||||
private:
|
||||
/// Whether setRGBDImage() keeps @p model: not an invalid model without any image.
|
||||
bool keepCameraModel(const CameraModel & model, const cv::Mat & rgb, const cv::Mat & depth, bool clearPreviousData) const;
|
||||
|
||||
int _id; ///< Unique sensor data ID (0 if invalid)
|
||||
double _stamp; ///< Timestamp in seconds
|
||||
|
||||
|
||||
+51
-194
@@ -28,12 +28,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include "rtabmap/core/Compression.h"
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <rtabmap/utilite/UStl.h>
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
#include <zlib.h>
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
@@ -70,117 +67,16 @@ int deserializeMatType(int serializedType)
|
||||
((serializedType >> kSerializedCnShift) & 511) + 1);
|
||||
}
|
||||
|
||||
// Default quantization when only the maximum depth is set in the format.
|
||||
const float kDefaultDepthQuantization = 100.0f;
|
||||
|
||||
bool hasSignature(const unsigned char * bytes, size_t size, const void * signature)
|
||||
{
|
||||
return bytes && size >= 8 && memcmp(bytes, signature, 8) == 0;
|
||||
}
|
||||
|
||||
// Values over maxDepth, NaN, inf, 0 and negative values are set to 0 (invalid),
|
||||
// as well as values too close to be represented on 16 bits (under
|
||||
// depthQuantA / (65535 - depthQuantB) meters).
|
||||
cv::Mat depthToInvDepth(const cv::Mat & depth, float maxDepth, float quantization, float & depthQuantA, float & depthQuantB)
|
||||
{
|
||||
UASSERT(depth.type() == CV_32FC1);
|
||||
depthQuantA = quantization * (quantization + 1.0f);
|
||||
depthQuantB = 1.0f - depthQuantA / maxDepth;
|
||||
cv::Mat invDepth(depth.size(), CV_16UC1);
|
||||
for(int i=0; i<depth.rows; ++i)
|
||||
{
|
||||
const float * in = depth.ptr<float>(i);
|
||||
uint16_t * out = invDepth.ptr<uint16_t>(i);
|
||||
for(int j=0; j<depth.cols; ++j)
|
||||
{
|
||||
const float d = in[j];
|
||||
if(d > 0.0f && d < maxDepth) // false for NaN
|
||||
{
|
||||
// Rounded (ROS truncates), the decoding is the same.
|
||||
const float v = depthQuantA / d + depthQuantB + 0.5f;
|
||||
out[j] = v < 65536.0f ? (uint16_t)v : 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
out[j] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
return invDepth;
|
||||
}
|
||||
|
||||
cv::Mat invDepthToDepth(const cv::Mat & invDepth, float depthQuantA, float depthQuantB)
|
||||
{
|
||||
UASSERT(invDepth.type() == CV_16UC1);
|
||||
cv::Mat depth(invDepth.size(), CV_32FC1);
|
||||
for(int i=0; i<invDepth.rows; ++i)
|
||||
{
|
||||
const uint16_t * in = invDepth.ptr<uint16_t>(i);
|
||||
float * out = depth.ptr<float>(i);
|
||||
for(int j=0; j<invDepth.cols; ++j)
|
||||
{
|
||||
out[j] = in[j] ? depthQuantA / (float(in[j]) - depthQuantB) : 0.0f;
|
||||
}
|
||||
}
|
||||
return depth;
|
||||
}
|
||||
|
||||
void invDepthParameters(float depthQuantA, float depthQuantB, float & maxDepth, float & quantization)
|
||||
{
|
||||
// inverse of depthQuantA = q*(q+1) and depthQuantB = 1 - depthQuantA/maxDepth
|
||||
quantization = (std::sqrt(1.0f + 4.0f*depthQuantA) - 1.0f) / 2.0f;
|
||||
maxDepth = depthQuantA / (1.0f - depthQuantB);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
bool parseImageCompressionFormat(const std::string & format, std::string & codec, float & maxDepth, float & quantization)
|
||||
{
|
||||
codec.clear();
|
||||
maxDepth = 0.0f;
|
||||
quantization = 0.0f;
|
||||
std::vector<std::string> fields = uListToVector(uSplit(format, ':'));
|
||||
if(fields.empty())
|
||||
{
|
||||
return format.empty(); // empty is general (zlib)
|
||||
}
|
||||
if(fields[0].size() < 2 || fields[0][0] != '.')
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if(fields.size() > 1)
|
||||
{
|
||||
// Inverse depth parameters only for formats supporting 16UC1
|
||||
if((fields[0] != ".png" && fields[0] != ".rvl") || fields.size() > 3)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
for(size_t i=1; i<fields.size(); ++i)
|
||||
{
|
||||
if(!uIsNumber(fields[i]) || uStr2Float(fields[i]) <= 0.0f)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
maxDepth = uStr2Float(fields[1]);
|
||||
quantization = fields.size() == 3 ? uStr2Float(fields[2]) : kDefaultDepthQuantization;
|
||||
}
|
||||
codec = fields[0];
|
||||
return true;
|
||||
}
|
||||
|
||||
// format : ".jpg" ".png" ".rvl" "" (empty is general), see parseImageCompressionFormat()
|
||||
// format : ".jpg" ".png" ".rvl" "" (empty is general)
|
||||
CompressionThread::CompressionThread(const cv::Mat & mat, const std::string & format) :
|
||||
uncompressedData_(mat),
|
||||
format_(format),
|
||||
image_(!format.empty()),
|
||||
compressMode_(true)
|
||||
{
|
||||
std::string codec;
|
||||
float maxDepth, quantization;
|
||||
UASSERT_MSG(parseImageCompressionFormat(format, codec, maxDepth, quantization) &&
|
||||
(codec.empty() || codec == ".jpg" || codec == ".png" || codec == ".rvl"),
|
||||
uFormat("Invalid compression format \"%s\"", format.c_str()).c_str());
|
||||
UASSERT(format.empty() || format.compare(".jpg") == 0 || format.compare(".png") == 0 || format.compare(".rvl") == 0);
|
||||
}
|
||||
// assume image
|
||||
CompressionThread::CompressionThread(const cv::Mat & bytes, bool isImage) :
|
||||
@@ -235,43 +131,21 @@ void CompressionThread::mainLoop()
|
||||
this->kill();
|
||||
}
|
||||
|
||||
// ".jpg" or ".png" or ".rvl", with optional ":maxDepth[:quantization]" for 32FC1 depth images
|
||||
// ".jpg" or ".png" or ".rvl"
|
||||
std::vector<unsigned char> compressImage(const cv::Mat & image, const std::string & format)
|
||||
{
|
||||
std::vector<unsigned char> bytes;
|
||||
if(!image.empty())
|
||||
{
|
||||
std::string codec;
|
||||
float maxDepth, quantization;
|
||||
if(!parseImageCompressionFormat(format, codec, maxDepth, quantization) || codec.empty())
|
||||
{
|
||||
UERROR("Invalid image compression format \"%s\"", format.c_str());
|
||||
return bytes;
|
||||
}
|
||||
|
||||
if(image.type() == CV_32FC1 && maxDepth > 0.0f)
|
||||
{
|
||||
float depthQuantA, depthQuantB;
|
||||
cv::Mat invDepth = depthToInvDepth(image, maxDepth, quantization, depthQuantA, depthQuantB);
|
||||
std::vector<unsigned char> invDepthBytes = compressImage(invDepth, codec);
|
||||
if(!invDepthBytes.empty())
|
||||
{
|
||||
bytes.resize(kCompressedDepthInvHeaderSize + invDepthBytes.size());
|
||||
memcpy(&bytes[0], kCompressedDepthInvSignature, 8);
|
||||
memcpy(&bytes[8], &depthQuantA, 4);
|
||||
memcpy(&bytes[12], &depthQuantB, 4);
|
||||
memcpy(&bytes[kCompressedDepthInvHeaderSize], invDepthBytes.data(), invDepthBytes.size());
|
||||
}
|
||||
}
|
||||
else if(image.type() == CV_32FC1)
|
||||
if(image.type() == CV_32FC1)
|
||||
{
|
||||
//save in 8bits-4channel
|
||||
cv::Mat bgra(image.size(), CV_8UC4, image.data);
|
||||
cv::imencode(".png", bgra, bytes);
|
||||
}
|
||||
else if(codec == ".rvl")
|
||||
else if(format == ".rvl")
|
||||
{
|
||||
bytes.assign(kCompressedDepthRvlSignature, kCompressedDepthRvlSignature+8);
|
||||
bytes = {'D', 'E', 'P', 'T', 'H', 'R', 'V', 'L'};
|
||||
int numPixels = image.rows * image.cols;
|
||||
// In the worst case, RVL compression results in ~1.5x larger data.
|
||||
bytes.resize(3 * numPixels + 20);
|
||||
@@ -280,18 +154,18 @@ std::vector<unsigned char> compressImage(const cv::Mat & image, const std::strin
|
||||
memcpy(&bytes[8], &cols, 4);
|
||||
memcpy(&bytes[12], &rows, 4);
|
||||
RvlCodec rvl;
|
||||
int compressedSize = rvl.CompressRVL(image.ptr<uint16_t>(), &bytes[kCompressedDepthRvlHeaderSize], numPixels);
|
||||
bytes.resize(kCompressedDepthRvlHeaderSize + compressedSize);
|
||||
int compressedSize = rvl.CompressRVL(image.ptr<uint16_t>(), &bytes[16], numPixels);
|
||||
bytes.resize(16 + compressedSize);
|
||||
}
|
||||
else
|
||||
{
|
||||
cv::imencode(codec, image, bytes);
|
||||
cv::imencode(format, image, bytes);
|
||||
}
|
||||
}
|
||||
return bytes;
|
||||
}
|
||||
|
||||
// ".jpg" or ".png" or ".rvl", with optional ":maxDepth[:quantization]" for 32FC1 depth images
|
||||
// ".jpg" or ".png" or ".rvl"
|
||||
cv::Mat compressImage2(const cv::Mat & image, const std::string & format)
|
||||
{
|
||||
std::vector<unsigned char> bytes = compressImage(image, format);
|
||||
@@ -303,65 +177,25 @@ cv::Mat compressImage2(const cv::Mat & image, const std::string & format)
|
||||
}
|
||||
|
||||
cv::Mat uncompressImage(const cv::Mat & bytes)
|
||||
{
|
||||
if(bytes.empty())
|
||||
{
|
||||
return cv::Mat();
|
||||
}
|
||||
return uncompressImage(bytes.data, bytes.total()*bytes.elemSize());
|
||||
}
|
||||
|
||||
cv::Mat uncompressImage(const std::vector<unsigned char> & bytes)
|
||||
{
|
||||
return uncompressImage(bytes.data(), bytes.size());
|
||||
}
|
||||
|
||||
cv::Mat uncompressImage(const unsigned char * bytes, size_t size)
|
||||
{
|
||||
cv::Mat image;
|
||||
if(bytes && size)
|
||||
if(!bytes.empty())
|
||||
{
|
||||
if(hasSignature(bytes, size, kCompressedDepthInvSignature))
|
||||
if (compressedDepthFormat(bytes) == ".rvl")
|
||||
{
|
||||
if(size <= kCompressedDepthInvHeaderSize)
|
||||
{
|
||||
UERROR("Inverse depth image is truncated (%d bytes).", (int)size);
|
||||
return image;
|
||||
}
|
||||
float depthQuantA, depthQuantB;
|
||||
memcpy(&depthQuantA, &bytes[8], 4);
|
||||
memcpy(&depthQuantB, &bytes[12], 4);
|
||||
cv::Mat invDepth = uncompressImage(&bytes[kCompressedDepthInvHeaderSize], size - kCompressedDepthInvHeaderSize);
|
||||
if(invDepth.type() == CV_16UC1)
|
||||
{
|
||||
image = invDepthToDepth(invDepth, depthQuantA, depthQuantB);
|
||||
}
|
||||
else if(!invDepth.empty())
|
||||
{
|
||||
UERROR("Inverse depth image should be 16UC1 (type=%d).", invDepth.type());
|
||||
}
|
||||
}
|
||||
else if(hasSignature(bytes, size, kCompressedDepthRvlSignature))
|
||||
{
|
||||
if(size < kCompressedDepthRvlHeaderSize)
|
||||
{
|
||||
UERROR("RVL depth image is truncated (%d bytes).", (int)size);
|
||||
return image;
|
||||
}
|
||||
uint32_t cols, rows;
|
||||
memcpy(&cols, &bytes[8], 4);
|
||||
memcpy(&rows, &bytes[12], 4);
|
||||
memcpy(&cols, &bytes.data[8], 4);
|
||||
memcpy(&rows, &bytes.data[12], 4);
|
||||
image = cv::Mat(rows, cols, CV_16UC1);
|
||||
RvlCodec rvl;
|
||||
rvl.DecompressRVL(&bytes[kCompressedDepthRvlHeaderSize], image.ptr<uint16_t>(), cols * rows);
|
||||
rvl.DecompressRVL(&bytes.data[16], image.ptr<uint16_t>(), cols * rows);
|
||||
}
|
||||
else
|
||||
{
|
||||
const cv::Mat buf(1, (int)size, CV_8UC1, (void *)bytes);
|
||||
#if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
|
||||
image = cv::imdecode(buf, cv::IMREAD_UNCHANGED);
|
||||
image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
|
||||
#else
|
||||
image = cv::imdecode(buf, -1);
|
||||
image = cv::imdecode(bytes, -1);
|
||||
#endif
|
||||
if(image.type() == CV_8UC4)
|
||||
{
|
||||
@@ -376,6 +210,36 @@ cv::Mat uncompressImage(const unsigned char * bytes, size_t size)
|
||||
return image;
|
||||
}
|
||||
|
||||
cv::Mat uncompressImage(const std::vector<unsigned char> & bytes)
|
||||
{
|
||||
cv::Mat image;
|
||||
if(bytes.size())
|
||||
{
|
||||
if (compressedDepthFormat(bytes) == ".rvl")
|
||||
{
|
||||
uint32_t cols, rows;
|
||||
memcpy(&cols, &bytes[8], 4);
|
||||
memcpy(&rows, &bytes[12], 4);
|
||||
image = cv::Mat(rows, cols, CV_16UC1);
|
||||
RvlCodec rvl;
|
||||
rvl.DecompressRVL(&bytes[16], image.ptr<uint16_t>(), cols * rows);
|
||||
}
|
||||
else
|
||||
{
|
||||
#if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
|
||||
image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
|
||||
#else
|
||||
image = cv::imdecode(bytes, -1);
|
||||
#endif
|
||||
if(image.type() == CV_8UC4)
|
||||
{
|
||||
image = cv::Mat(image.size(), CV_32FC1, image.data).clone();
|
||||
}
|
||||
}
|
||||
}
|
||||
return image;
|
||||
}
|
||||
|
||||
std::vector<unsigned char> compressData(const cv::Mat & data)
|
||||
{
|
||||
std::vector<unsigned char> bytes;
|
||||
@@ -517,17 +381,10 @@ std::string compressedDepthFormat(const unsigned char * bytes, size_t size)
|
||||
std::string format;
|
||||
if(bytes && size)
|
||||
{
|
||||
if(hasSignature(bytes, size, kCompressedDepthInvSignature) && size > kCompressedDepthInvHeaderSize)
|
||||
{
|
||||
float depthQuantA, depthQuantB, maxDepth, quantization;
|
||||
memcpy(&depthQuantA, &bytes[8], 4);
|
||||
memcpy(&depthQuantB, &bytes[12], 4);
|
||||
invDepthParameters(depthQuantA, depthQuantB, maxDepth, quantization);
|
||||
format = uFormat("%s:%g:%g",
|
||||
compressedDepthFormat(&bytes[kCompressedDepthInvHeaderSize], size - kCompressedDepthInvHeaderSize).c_str(),
|
||||
maxDepth, quantization);
|
||||
}
|
||||
else if(hasSignature(bytes, size, kCompressedDepthRvlSignature))
|
||||
size_t maxlen = std::min(size, size_t(8));
|
||||
std::vector<unsigned char> signature(maxlen);
|
||||
memcpy(&signature[0], bytes, maxlen);
|
||||
if (std::string(signature.begin(), signature.end()) == "DEPTHRVL")
|
||||
{
|
||||
format = ".rvl";
|
||||
}
|
||||
|
||||
+11
-72
@@ -102,7 +102,6 @@ Memory::Memory(const ParametersMap & parameters) :
|
||||
_imagePreDecimation(Parameters::defaultMemImagePreDecimation()),
|
||||
_imagePostDecimation(Parameters::defaultMemImagePostDecimation()),
|
||||
_legacyDecimatedOctave(false),
|
||||
_inverseDepthCompressionAllowed(true),
|
||||
_compressionParallelized(Parameters::defaultMemCompressionParallelized()),
|
||||
_laserScanDownsampleStepSize(Parameters::defaultMemLaserScanDownsampleStepSize()),
|
||||
_laserScanVoxelSize(Parameters::defaultMemLaserScanVoxelSize()),
|
||||
@@ -229,11 +228,6 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
|
||||
// filling it that way; a new one gets the corrected scaling.
|
||||
_legacyDecimatedOctave =
|
||||
uStrNumCmp(_dbDriver->getDatabaseVersion(), "0.23.12") < 0;
|
||||
// Depth images compressed as inverse depth cannot be read before 0.24, which
|
||||
// would still open databases created with Db/TargetVersion < 0.24 or by an
|
||||
// older version.
|
||||
_inverseDepthCompressionAllowed =
|
||||
uStrNumCmp(_dbDriver->getDatabaseVersion(), "0.24.0") >= 0;
|
||||
// Only where the descriptors stored in the map end up different: keypoints
|
||||
// from odometry, scaled into the pre-decimated image before being described.
|
||||
if(_legacyDecimatedOctave && _useOdometryFeatures && _imagePreDecimation > 1)
|
||||
@@ -832,19 +826,6 @@ void Memory::parseParameters(const ParametersMap & parameters)
|
||||
Parameters::parse(params, Parameters::kMemIntermediateNodeDataKept(), _saveIntermediateNodeData);
|
||||
Parameters::parse(params, Parameters::kMemImageCompressionFormat(), _rgbCompressionFormat);
|
||||
Parameters::parse(params, Parameters::kMemDepthCompressionFormat(), _depthCompressionFormat);
|
||||
{
|
||||
std::string codec;
|
||||
float maxDepth, quantization;
|
||||
if(!parseImageCompressionFormat(_depthCompressionFormat, codec, maxDepth, quantization) ||
|
||||
(codec != ".png" && codec != ".rvl"))
|
||||
{
|
||||
UWARN("Invalid %s=\"%s\", using default \"%s\".",
|
||||
Parameters::kMemDepthCompressionFormat().c_str(),
|
||||
_depthCompressionFormat.c_str(),
|
||||
Parameters::defaultMemDepthCompressionFormat().c_str());
|
||||
_depthCompressionFormat = Parameters::defaultMemDepthCompressionFormat();
|
||||
}
|
||||
}
|
||||
Parameters::parse(params, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal);
|
||||
Parameters::parse(params, Parameters::kMemGenerateIds(), _generateIds);
|
||||
Parameters::parse(params, Parameters::kMemBadSignaturesIgnored(), _badSignaturesIgnored);
|
||||
@@ -5246,7 +5227,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
|
||||
if(!isIntermediateNode)
|
||||
{
|
||||
// We need raw images if we need to extract features and/or do tag detection
|
||||
bool needRawImages = (_feature2D->getMaxFeatures() >= 0 &&
|
||||
bool needRawImages = _feature2D->getMaxFeatures() >= 0 &&
|
||||
(!_useOdometryFeatures ||
|
||||
data.keypoints().empty() ||
|
||||
(int)data.keypoints().size() != data.descriptors().rows ||
|
||||
@@ -5254,9 +5235,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
|
||||
_detectMarkers ||
|
||||
_rotateImagesUpsideUp ||
|
||||
_imagePostDecimation > 1 ||
|
||||
(_createOccupancyGrid && _localMapMaker->isGridFromDepth()))) ||
|
||||
// Images rectified below: stereo always, RGB-D unless only its features are
|
||||
(!_imagesAlreadyRectified && !(_rectifyOnlyFeatures && data.stereoCameraModels().empty()));
|
||||
(_createOccupancyGrid && _localMapMaker->isGridFromDepth()));
|
||||
|
||||
// Note: we could avoid uncompressing scan if we don't do any filtering
|
||||
// and if we don't use it for local occupancy grid
|
||||
@@ -6646,44 +6625,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
|
||||
std::vector<unsigned char> imageBytes;
|
||||
std::vector<unsigned char> depthBytes;
|
||||
|
||||
std::string depthCompressionFormat = _depthCompressionFormat;
|
||||
bool reuseCompressedDepth =
|
||||
depthOrRightImage.data == data.depthOrRightRaw().data &&
|
||||
!data.depthOrRightCompressed().empty();
|
||||
if(!_inverseDepthCompressionAllowed)
|
||||
{
|
||||
std::string codec;
|
||||
float maxDepth, quantization;
|
||||
if(parseImageCompressionFormat(depthCompressionFormat, codec, maxDepth, quantization) && maxDepth > 0.0f)
|
||||
{
|
||||
static bool warned = false;
|
||||
if(!warned)
|
||||
{
|
||||
UWARN("%s=\"%s\": inverse depth compression format is not compatible with database "
|
||||
"version %s (requires >= 0.24, see %s), \"%s\" format is used instead. This "
|
||||
"warning is only printed once.",
|
||||
Parameters::kMemDepthCompressionFormat().c_str(),
|
||||
depthCompressionFormat.c_str(),
|
||||
_dbDriver?_dbDriver->getDatabaseVersion().c_str():"",
|
||||
Parameters::kDbTargetVersion().c_str(),
|
||||
codec.c_str());
|
||||
warned = true;
|
||||
}
|
||||
depthCompressionFormat = codec;
|
||||
}
|
||||
if(reuseCompressedDepth &&
|
||||
compressedDepthFormat(data.depthOrRightCompressed()).find(':') != std::string::npos)
|
||||
{
|
||||
// Already compressed as inverse depth (e.g., received from ROS's
|
||||
// compressed_depth_image_transport), re-compress it.
|
||||
reuseCompressedDepth = false;
|
||||
if(depthOrRightImage.empty())
|
||||
{
|
||||
depthOrRightImage = uncompressImage(data.depthOrRightCompressed());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(!depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1)
|
||||
{
|
||||
if(_saveDepth16Format)
|
||||
@@ -6699,7 +6640,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
|
||||
}
|
||||
depthOrRightImage = util2d::cvtDepthFromFloat(depthOrRightImage);
|
||||
}
|
||||
else if(depthCompressionFormat == ".rvl")
|
||||
else if(_depthCompressionFormat == ".rvl")
|
||||
{
|
||||
static bool warned = false;
|
||||
if(!warned)
|
||||
@@ -6709,16 +6650,13 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
|
||||
"images will be compressed in \".png\" format instead. Explicitly "
|
||||
"set %s to true to keep using \"%s\" format and images will be "
|
||||
"converted to 16bits for convenience (warning: that would "
|
||||
"remove all depth values over 65 meters). Set %s=\".rvl:<maxDepth>:<quantization>\" "
|
||||
"(e.g., \".rvl:10:100\") to compress them in RVL as 16 bits inverse depth "
|
||||
"(lossy, see parameter's description). Explicitly set %s=\".png\" "
|
||||
"remove all depth values over 65 meters). Explicitly set %s=\".png\" "
|
||||
"to suppress this warning. This warning is only printed once.",
|
||||
Parameters::kMemSaveDepth16Format().c_str(),
|
||||
Parameters::kMemDepthCompressionFormat().c_str(),
|
||||
depthCompressionFormat.c_str(),
|
||||
_depthCompressionFormat.c_str(),
|
||||
Parameters::kMemSaveDepth16Format().c_str(),
|
||||
depthCompressionFormat.c_str(),
|
||||
Parameters::kMemDepthCompressionFormat().c_str(),
|
||||
_depthCompressionFormat.c_str(),
|
||||
Parameters::kMemDepthCompressionFormat().c_str());
|
||||
warned = true;
|
||||
}
|
||||
@@ -6728,8 +6666,9 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
|
||||
bool reuseCompressedImage =
|
||||
image.data == data.imageRaw().data &&
|
||||
!data.imageCompressed().empty();
|
||||
reuseCompressedDepth = reuseCompressedDepth &&
|
||||
depthOrRightImage.data == data.depthOrRightRaw().data;
|
||||
bool reuseCompressedDepth =
|
||||
depthOrRightImage.data == data.depthOrRightRaw().data &&
|
||||
!data.depthOrRightCompressed().empty();
|
||||
bool reuseCompressedDepthConfidence =
|
||||
depthConfidence.data == data.depthConfidenceRaw().data &&
|
||||
!data.depthConfidenceCompressed().empty();
|
||||
@@ -6746,7 +6685,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
|
||||
if(_compressionParallelized)
|
||||
{
|
||||
rtabmap::CompressionThread ctImage(image, _rgbCompressionFormat);
|
||||
rtabmap::CompressionThread ctDepth(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?depthCompressionFormat:_rgbCompressionFormat);
|
||||
rtabmap::CompressionThread ctDepth(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?_depthCompressionFormat:_rgbCompressionFormat);
|
||||
rtabmap::CompressionThread ctDepthConfidence(depthConfidence);
|
||||
rtabmap::CompressionThread ctLaserScan(laserScan.data());
|
||||
rtabmap::CompressionThread ctUserData(data.userDataRaw());
|
||||
@@ -6785,7 +6724,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
|
||||
else
|
||||
{
|
||||
compressedImage = reuseCompressedImage?cv::Mat():compressImage2(image, _rgbCompressionFormat);
|
||||
compressedDepth = reuseCompressedDepth?cv::Mat():compressImage2(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?depthCompressionFormat:_rgbCompressionFormat);
|
||||
compressedDepth = reuseCompressedDepth?cv::Mat():compressImage2(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?_depthCompressionFormat:_rgbCompressionFormat);
|
||||
compressedDepthConfidence = reuseCompressedDepthConfidence?cv::Mat():compressData2(depthConfidence);
|
||||
compressedScan = reuseCompressedScan?data.laserScanCompressed().data():compressData2(laserScan.data());
|
||||
compressedUserData = reuseCompressedUserData?data.userDataCompressed():compressData2(data.userDataRaw());
|
||||
|
||||
@@ -313,24 +313,6 @@ SensorData::~SensorData()
|
||||
{
|
||||
}
|
||||
|
||||
bool SensorData::keepCameraModel(
|
||||
const CameraModel & model,
|
||||
const cv::Mat & rgb,
|
||||
const cv::Mat & depth,
|
||||
bool clearPreviousData) const
|
||||
{
|
||||
// An invalid model without any image is only a placeholder (e.g., scan-only data
|
||||
// created with CameraModel()): it is not kept, so that cameraModels() is empty when
|
||||
// there is no camera. An invalid model with an image is kept: images can be used
|
||||
// without calibration, and they are split per camera model.
|
||||
return model.isValidForProjection() ||
|
||||
!rgb.empty() ||
|
||||
!depth.empty() ||
|
||||
(!clearPreviousData && (
|
||||
!_imageRaw.empty() || !_imageCompressed.empty() ||
|
||||
!_depthOrRightRaw.empty() || !_depthOrRightCompressed.empty()));
|
||||
}
|
||||
|
||||
void SensorData::setRGBDImage(
|
||||
const cv::Mat & rgb,
|
||||
const cv::Mat & depth,
|
||||
@@ -338,10 +320,7 @@ void SensorData::setRGBDImage(
|
||||
bool clearPreviousData)
|
||||
{
|
||||
std::vector<CameraModel> models;
|
||||
if(keepCameraModel(model, rgb, depth, clearPreviousData))
|
||||
{
|
||||
models.push_back(model);
|
||||
}
|
||||
models.push_back(model);
|
||||
setRGBDImage(rgb, depth, models, clearPreviousData);
|
||||
}
|
||||
void SensorData::setRGBDImage(
|
||||
@@ -352,10 +331,7 @@ void SensorData::setRGBDImage(
|
||||
bool clearPreviousData)
|
||||
{
|
||||
std::vector<CameraModel> models;
|
||||
if(keepCameraModel(model, rgb, depth, clearPreviousData))
|
||||
{
|
||||
models.push_back(model);
|
||||
}
|
||||
models.push_back(model);
|
||||
setRGBDImage(rgb, depth, depthConfidence, models, clearPreviousData);
|
||||
}
|
||||
void SensorData::setRGBDImage(
|
||||
|
||||
@@ -73,7 +73,7 @@ unsigned long VisualWord::getMemoryUsed() const
|
||||
{
|
||||
unsigned long memoryUsage = sizeof(VisualWord);
|
||||
memoryUsage += _references.size() * (sizeof(int)*2+sizeof(std::map<int ,int>::iterator)) + sizeof(std::map<int ,int>);
|
||||
memoryUsage += _descriptor.empty()?0:_descriptor.total() * _descriptor.elemSize();
|
||||
memoryUsage += _descriptor.total() * _descriptor.elemSize();
|
||||
return memoryUsage;
|
||||
}
|
||||
|
||||
|
||||
@@ -162,20 +162,6 @@ IF(BUILD_PERF_TESTS)
|
||||
set_tests_properties(test_graph_perf PROPERTIES
|
||||
TIMEOUT ${_perf_timeout}
|
||||
LABELS "performance")
|
||||
|
||||
# Comparison of the depth image compression approaches (sizes, times, errors) for
|
||||
# 16UC1 and 32FC1 depth images: PNG, RVL, zlib, the legacy 4-channel PNG of 32FC1
|
||||
# images, their conversion to 16UC1 millimeters, and their quantization as 16 bits
|
||||
# inverse depth (Mem/DepthCompressionFormat=".png:max:q" or ".rvl:max:q"):
|
||||
# bin/test_compression_perf
|
||||
# bin/test_compression_perf --gtest_filter=*Synthetic*
|
||||
add_executable(test_compression_perf perf_compression.cpp)
|
||||
target_link_libraries(test_compression_perf gtest_main rtabmap_core)
|
||||
|
||||
add_test(NAME test_compression_perf COMMAND test_compression_perf)
|
||||
set_tests_properties(test_compression_perf PROPERTIES
|
||||
TIMEOUT ${_perf_timeout}
|
||||
LABELS "performance")
|
||||
ENDIF(BUILD_PERF_TESTS)
|
||||
|
||||
# Rtabmap end-to-end replay of sample DBs (test data fetched by
|
||||
|
||||
@@ -1,342 +0,0 @@
|
||||
// Comparison of the depth image compression approaches of Compression.h, for each
|
||||
// depth type rtabmap receives:
|
||||
//
|
||||
// 16UC1 (millimeters):
|
||||
// - ".png" lossless, 16 bits grayscale PNG
|
||||
// - ".rvl" lossless, RVL (Mem/DepthCompressionFormat default)
|
||||
// - zlib lossless, compressData2(), as a reference
|
||||
// 32FC1 (meters):
|
||||
// - ".png" lossless, float bytes as a 4-channel 8 bits PNG (legacy)
|
||||
// - zlib lossless, compressData2(), as a reference
|
||||
// - 16UC1 mm + ".png/.rvl" lossy, util2d::cvtDepthFromFloat() then 16 bits codec,
|
||||
// what Mem/SaveDepth16Format=true does
|
||||
// - ".png:max:q/.rvl:max:q" lossy, 16 bits quantized inverse depth (same
|
||||
// quantization than ROS's compressed_depth_image_transport)
|
||||
//
|
||||
// over the depth images of data/rgbd/depth (a structured light camera, millimeters),
|
||||
// the same images converted to meters in 32FC1 (as many drivers publish them), and a
|
||||
// synthetic 32FC1 image with continuous values, like stereo or lidar projected depth.
|
||||
//
|
||||
// Its own executable, run by ctest under the "performance" label, so that its seconds
|
||||
// of benchmarking stay out of the unit test shards:
|
||||
// ctest -L performance to run them
|
||||
// ctest -LE performance to skip them
|
||||
// bin/test_compression_perf --gtest_filter=*Synthetic*
|
||||
//
|
||||
// The times are reported rather than asserted on, as they depend on the machine. What
|
||||
// is asserted is that the lossless approaches give back the same image, and that the
|
||||
// lossy ones stay within their error bounds for the depth range they keep.
|
||||
#include <gtest/gtest.h>
|
||||
#include <rtabmap/core/Compression.h>
|
||||
#include <rtabmap/core/util2d.h>
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <rtabmap/utilite/UTimer.h>
|
||||
#include <opencv2/imgcodecs.hpp>
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <cstdio>
|
||||
#include <functional>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
namespace {
|
||||
|
||||
static const int ITERATIONS = 15;
|
||||
|
||||
struct Approach
|
||||
{
|
||||
std::string name;
|
||||
std::function<std::vector<unsigned char>(const cv::Mat &)> encode;
|
||||
std::function<cv::Mat(const std::vector<unsigned char> &)> decode;
|
||||
bool lossless;
|
||||
float maxDepth; // meters, lossy approaches only: depth kept under it
|
||||
float minDepth; // meters, lossy approaches only: depth kept over it
|
||||
std::function<float(float)> tolerance; // meters, lossy approaches only, for a depth in meters
|
||||
};
|
||||
|
||||
struct Result
|
||||
{
|
||||
size_t bytes = 0;
|
||||
double encodeMs = 0.0;
|
||||
double decodeMs = 0.0;
|
||||
double maxError = 0.0; // mm, over the depth range kept
|
||||
double rmse = 0.0; // mm, over the depth range kept
|
||||
double lost = 0.0; // % of the valid pixels set to 0
|
||||
int outOfTolerance = 0; // pixels with an error over the tolerance
|
||||
};
|
||||
|
||||
double median(std::vector<double> v)
|
||||
{
|
||||
std::sort(v.begin(), v.end());
|
||||
return v[v.size()/2];
|
||||
}
|
||||
|
||||
float toMeters(const cv::Mat & depth, int r, int c)
|
||||
{
|
||||
return depth.type() == CV_16UC1 ? float(depth.at<uint16_t>(r, c)) * 0.001f : depth.at<float>(r, c);
|
||||
}
|
||||
|
||||
Result run(const cv::Mat & depth, const Approach & approach)
|
||||
{
|
||||
Result result;
|
||||
std::vector<unsigned char> bytes;
|
||||
cv::Mat restored;
|
||||
std::vector<double> encodeTimes, decodeTimes;
|
||||
for(int i=0; i<ITERATIONS; ++i)
|
||||
{
|
||||
UTimer timer;
|
||||
bytes = approach.encode(depth);
|
||||
encodeTimes.push_back(timer.restart() * 1000.0);
|
||||
restored = approach.decode(bytes);
|
||||
decodeTimes.push_back(timer.ticks() * 1000.0);
|
||||
}
|
||||
result.bytes = bytes.size();
|
||||
result.encodeMs = median(encodeTimes);
|
||||
result.decodeMs = median(decodeTimes);
|
||||
|
||||
EXPECT_EQ(restored.size(), depth.size());
|
||||
EXPECT_EQ(restored.type(), approach.lossless ? depth.type() : restored.type());
|
||||
if(restored.size() != depth.size())
|
||||
{
|
||||
return result;
|
||||
}
|
||||
|
||||
if(approach.lossless)
|
||||
{
|
||||
EXPECT_EQ(memcmp(restored.data, depth.data, depth.total()*depth.elemSize()), 0);
|
||||
return result;
|
||||
}
|
||||
|
||||
int valid = 0, lost = 0, kept = 0;
|
||||
double sumSq = 0.0;
|
||||
for(int r=0; r<depth.rows; ++r)
|
||||
{
|
||||
for(int c=0; c<depth.cols; ++c)
|
||||
{
|
||||
const float d = toMeters(depth, r, c);
|
||||
if(!(std::isfinite(d) && d > 0.0f))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
++valid;
|
||||
const float out = toMeters(restored, r, c);
|
||||
if(out == 0.0f)
|
||||
{
|
||||
++lost;
|
||||
// Only allowed outside the kept range
|
||||
if(d >= approach.minDepth && d < approach.maxDepth)
|
||||
{
|
||||
++result.outOfTolerance;
|
||||
}
|
||||
continue;
|
||||
}
|
||||
const double err = std::fabs(out - d);
|
||||
result.maxError = std::max(result.maxError, err*1000.0);
|
||||
sumSq += err*err*1e6;
|
||||
++kept;
|
||||
if(err > approach.tolerance(d))
|
||||
{
|
||||
++result.outOfTolerance;
|
||||
}
|
||||
}
|
||||
}
|
||||
result.rmse = kept ? std::sqrt(sumSq / kept) : 0.0;
|
||||
result.lost = valid ? 100.0 * lost / valid : 0.0;
|
||||
EXPECT_EQ(result.outOfTolerance, 0) << approach.name;
|
||||
return result;
|
||||
}
|
||||
|
||||
void report(const std::string & title, const cv::Mat & depth, const std::vector<Approach> & approaches)
|
||||
{
|
||||
const size_t raw = depth.total() * depth.elemSize();
|
||||
std::printf("\n%s: %dx%d %s, %zu bytes raw\n", title.c_str(), depth.cols, depth.rows,
|
||||
depth.type() == CV_16UC1 ? "16UC1" : "32FC1", raw);
|
||||
std::printf(" %-22s %10s %7s %10s %10s %11s %10s %8s\n",
|
||||
"approach", "bytes", "ratio", "encode ms", "decode ms", "max err mm", "rmse mm", "lost %");
|
||||
for(const Approach & approach : approaches)
|
||||
{
|
||||
SCOPED_TRACE(title + " " + approach.name);
|
||||
const Result r = run(depth, approach);
|
||||
if(approach.lossless)
|
||||
{
|
||||
std::printf(" %-22s %10zu %6.1fx %10.2f %10.2f %11s %10s %8s\n",
|
||||
approach.name.c_str(), r.bytes, double(raw)/double(r.bytes), r.encodeMs, r.decodeMs,
|
||||
"lossless", "-", "-");
|
||||
}
|
||||
else
|
||||
{
|
||||
std::printf(" %-22s %10zu %6.1fx %10.2f %10.2f %11.3f %10.3f %8.2f\n",
|
||||
approach.name.c_str(), r.bytes, double(raw)/double(r.bytes), r.encodeMs, r.decodeMs,
|
||||
r.maxError, r.rmse, r.lost);
|
||||
}
|
||||
}
|
||||
std::fflush(stdout);
|
||||
}
|
||||
|
||||
std::vector<unsigned char> encode(const cv::Mat & depth, const std::string & format)
|
||||
{
|
||||
return compressImage(depth, format);
|
||||
}
|
||||
|
||||
cv::Mat decode(const std::vector<unsigned char> & bytes)
|
||||
{
|
||||
return uncompressImage(bytes);
|
||||
}
|
||||
|
||||
std::vector<unsigned char> encodeZlib(const cv::Mat & depth)
|
||||
{
|
||||
return compressData(depth);
|
||||
}
|
||||
|
||||
cv::Mat decodeZlib(const std::vector<unsigned char> & bytes)
|
||||
{
|
||||
return uncompressData(bytes);
|
||||
}
|
||||
|
||||
std::vector<Approach> approaches16U()
|
||||
{
|
||||
using namespace std::placeholders;
|
||||
return {
|
||||
{".png", std::bind(encode, _1, ".png"), decode, true, 0, 0, nullptr},
|
||||
{".rvl", std::bind(encode, _1, ".rvl"), decode, true, 0, 0, nullptr},
|
||||
{"zlib", encodeZlib, decodeZlib, true, 0, 0, nullptr}};
|
||||
}
|
||||
|
||||
Approach invDepth(const std::string & codec, float maxDepth, float quantization)
|
||||
{
|
||||
using namespace std::placeholders;
|
||||
const float A = quantization * (quantization + 1.0f);
|
||||
const float B = 1.0f - A / maxDepth;
|
||||
const std::string format = uFormat("%s:%g:%g", codec.c_str(), maxDepth, quantization);
|
||||
return {format, std::bind(encode, _1, format), decode, false,
|
||||
maxDepth,
|
||||
A / (65535.0f - B) * 1.001f,
|
||||
[A](float d) { return 0.51f * d * d / A + 1e-6f; }};
|
||||
}
|
||||
|
||||
Approach depth16(const std::string & codec)
|
||||
{
|
||||
return {"16UC1 mm + " + codec,
|
||||
[codec](const cv::Mat & depth) { return compressImage(util2d::cvtDepthFromFloat(depth), codec); },
|
||||
[](const std::vector<unsigned char> & bytes) { return util2d::cvtDepthToFloat(uncompressImage(bytes)); },
|
||||
false,
|
||||
65.535f,
|
||||
0.0f,
|
||||
[](float) { return 0.001f + 1e-6f; }}; // truncated to millimeters
|
||||
}
|
||||
|
||||
std::vector<Approach> approaches32F()
|
||||
{
|
||||
using namespace std::placeholders;
|
||||
return {
|
||||
{".png (legacy RGBA)", std::bind(encode, _1, ".png"), decode, true, 0, 0, nullptr},
|
||||
{"zlib", encodeZlib, decodeZlib, true, 0, 0, nullptr},
|
||||
depth16(".png"),
|
||||
depth16(".rvl"),
|
||||
invDepth(".png", 10.0f, 100.0f),
|
||||
invDepth(".rvl", 10.0f, 100.0f),
|
||||
invDepth(".png", 40.0f, 100.0f),
|
||||
invDepth(".rvl", 40.0f, 100.0f),
|
||||
invDepth(".rvl", 40.0f, 200.0f)};
|
||||
}
|
||||
|
||||
std::vector<cv::Mat> loadSampleDepths()
|
||||
{
|
||||
std::vector<cv::Mat> depths;
|
||||
for(const std::string & name : {"17.png", "154.png"})
|
||||
{
|
||||
const std::string path = std::string(RTABMAP_TEST_DATA_ROOT) + "/rgbd/depth/" + name;
|
||||
cv::Mat depth = cv::imread(path, cv::IMREAD_UNCHANGED);
|
||||
if(depth.type() == CV_16UC1)
|
||||
{
|
||||
depths.push_back(depth);
|
||||
}
|
||||
else
|
||||
{
|
||||
std::printf("Cannot load 16UC1 depth image \"%s\", skipped.\n", path.c_str());
|
||||
}
|
||||
}
|
||||
return depths;
|
||||
}
|
||||
|
||||
// Ground plane, walls and boxes seen by a 640x480 camera, with continuous
|
||||
// values up to ~35 m, noise growing with depth (as stereo) and holes.
|
||||
cv::Mat makeSyntheticDepth(int cols = 640, int rows = 480)
|
||||
{
|
||||
cv::RNG rng(42);
|
||||
const float fx = 0.75f * cols, cx = cols / 2.0f, cy = rows / 2.0f;
|
||||
const float cameraHeight = 1.0f;
|
||||
cv::Mat depth(rows, cols, CV_32FC1);
|
||||
for(int v=0; v<rows; ++v)
|
||||
{
|
||||
for(int u=0; u<cols; ++u)
|
||||
{
|
||||
const float x = (u - cx) / fx; // ray direction, z = 1
|
||||
const float y = (v - cy) / fx;
|
||||
float d = 35.0f; // far wall
|
||||
if(y > 0.0f)
|
||||
{
|
||||
d = std::min(d, cameraHeight / y); // ground
|
||||
}
|
||||
if(x < 0.0f)
|
||||
{
|
||||
d = std::min(d, 3.0f / -x); // left wall, 3 m away
|
||||
}
|
||||
// boxes
|
||||
if(x > 0.05f && x < 0.25f && y > -0.1f && y < cameraHeight / 2.5f)
|
||||
{
|
||||
d = std::min(d, 2.5f - 1.5f * x);
|
||||
}
|
||||
if(x > -0.35f && x < -0.15f && y > -0.2f && y < cameraHeight / 12.0f)
|
||||
{
|
||||
d = std::min(d, 12.0f);
|
||||
}
|
||||
d += (float)rng.gaussian(0.002 * d * d); // stereo-like noise
|
||||
depth.at<float>(v, u) = d;
|
||||
}
|
||||
}
|
||||
// Holes
|
||||
for(int i=0; i<40; ++i)
|
||||
{
|
||||
const int u = rng.uniform(0, cols - 20), v = rng.uniform(0, rows - 20);
|
||||
depth(cv::Rect(u, v, rng.uniform(2, 20), rng.uniform(2, 20))).setTo(0.0f);
|
||||
}
|
||||
return depth;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
TEST(CompressionPerf, SampleDepth16UC1)
|
||||
{
|
||||
const std::vector<cv::Mat> depths = loadSampleDepths();
|
||||
if(depths.empty())
|
||||
{
|
||||
GTEST_SKIP() << "No sample depth images in " << RTABMAP_TEST_DATA_ROOT << "/rgbd/depth";
|
||||
}
|
||||
for(size_t i=0; i<depths.size(); ++i)
|
||||
{
|
||||
report(uFormat("Sample depth %d", (int)i), depths[i], approaches16U());
|
||||
}
|
||||
}
|
||||
|
||||
TEST(CompressionPerf, SampleDepth32FC1)
|
||||
{
|
||||
const std::vector<cv::Mat> depths = loadSampleDepths();
|
||||
if(depths.empty())
|
||||
{
|
||||
GTEST_SKIP() << "No sample depth images in " << RTABMAP_TEST_DATA_ROOT << "/rgbd/depth";
|
||||
}
|
||||
for(size_t i=0; i<depths.size(); ++i)
|
||||
{
|
||||
report(uFormat("Sample depth %d in meters", (int)i), util2d::cvtDepthToFloat(depths[i]), approaches32F());
|
||||
}
|
||||
}
|
||||
|
||||
TEST(CompressionPerf, Synthetic32FC1)
|
||||
{
|
||||
report("Synthetic continuous depth", makeSyntheticDepth(), approaches32F());
|
||||
report("Synthetic continuous depth HD", makeSyntheticDepth(1280, 720), approaches32F());
|
||||
}
|
||||
@@ -1,9 +1,6 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <rtabmap/core/Compression.h>
|
||||
#include <rtabmap/utilite/UException.h>
|
||||
#include <opencv2/core.hpp>
|
||||
#include <cstring>
|
||||
#include <limits>
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
@@ -186,228 +183,3 @@ TEST(CompressionTest, CompressionThreadDataRoundTrip)
|
||||
|
||||
expectMatEqual(uncompressThread.getUncompressedData(), data);
|
||||
}
|
||||
|
||||
namespace {
|
||||
|
||||
// 32FC1 depth image covering [minDepth, maxDepth[ with sub-millimeter values,
|
||||
// and the invalid values of the inverse depth format on the first row.
|
||||
cv::Mat makeFloatDepth(int rows, int cols, float minDepth, float maxDepth)
|
||||
{
|
||||
cv::Mat depth(rows, cols, CV_32FC1);
|
||||
for(int r = 0; r < rows; ++r)
|
||||
{
|
||||
for(int c = 0; c < cols; ++c)
|
||||
{
|
||||
depth.at<float>(r, c) = minDepth + (maxDepth - minDepth) * float(r * cols + c) / float(rows * cols);
|
||||
}
|
||||
}
|
||||
return depth;
|
||||
}
|
||||
|
||||
// Error bound of the inverse depth format: half a quantization step.
|
||||
float invDepthTolerance(float d, float quantization)
|
||||
{
|
||||
// (with some margin for the float rounding of A/d + B, up to ~66000)
|
||||
return 0.51f * d * d / (quantization * (quantization + 1.0f)) + 1e-6f;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
TEST(CompressionTest, ParseImageCompressionFormat)
|
||||
{
|
||||
std::string codec;
|
||||
float maxDepth, quantization;
|
||||
|
||||
EXPECT_TRUE(parseImageCompressionFormat("", codec, maxDepth, quantization));
|
||||
EXPECT_TRUE(codec.empty());
|
||||
EXPECT_EQ(maxDepth, 0.0f);
|
||||
|
||||
EXPECT_TRUE(parseImageCompressionFormat(".jpg", codec, maxDepth, quantization));
|
||||
EXPECT_EQ(codec, ".jpg");
|
||||
EXPECT_EQ(maxDepth, 0.0f);
|
||||
EXPECT_EQ(quantization, 0.0f);
|
||||
|
||||
EXPECT_TRUE(parseImageCompressionFormat(".rvl", codec, maxDepth, quantization));
|
||||
EXPECT_EQ(codec, ".rvl");
|
||||
EXPECT_EQ(maxDepth, 0.0f);
|
||||
|
||||
EXPECT_TRUE(parseImageCompressionFormat(".png:20", codec, maxDepth, quantization));
|
||||
EXPECT_EQ(codec, ".png");
|
||||
EXPECT_FLOAT_EQ(maxDepth, 20.0f);
|
||||
EXPECT_FLOAT_EQ(quantization, 100.0f);
|
||||
|
||||
EXPECT_TRUE(parseImageCompressionFormat(".rvl:10.5:50", codec, maxDepth, quantization));
|
||||
EXPECT_EQ(codec, ".rvl");
|
||||
EXPECT_FLOAT_EQ(maxDepth, 10.5f);
|
||||
EXPECT_FLOAT_EQ(quantization, 50.0f);
|
||||
|
||||
EXPECT_FALSE(parseImageCompressionFormat("png", codec, maxDepth, quantization));
|
||||
EXPECT_FALSE(parseImageCompressionFormat(".jpg:10:100", codec, maxDepth, quantization));
|
||||
EXPECT_FALSE(parseImageCompressionFormat(".png:abc", codec, maxDepth, quantization));
|
||||
EXPECT_FALSE(parseImageCompressionFormat(".png:0:100", codec, maxDepth, quantization));
|
||||
EXPECT_FALSE(parseImageCompressionFormat(".png:-10:100", codec, maxDepth, quantization));
|
||||
EXPECT_FALSE(parseImageCompressionFormat(".png:10:0", codec, maxDepth, quantization));
|
||||
EXPECT_FALSE(parseImageCompressionFormat(".png:10:100:1", codec, maxDepth, quantization));
|
||||
}
|
||||
|
||||
TEST(CompressionTest, InvalidFormatReturnsEmpty)
|
||||
{
|
||||
const cv::Mat depth = makeFloatDepth(4, 4, 1.0f, 2.0f);
|
||||
EXPECT_TRUE(compressImage(depth, ".jpg:10").empty());
|
||||
EXPECT_TRUE(compressImage(depth, ".png:x").empty());
|
||||
}
|
||||
|
||||
TEST(CompressionTest, InverseDepthRoundTrip)
|
||||
{
|
||||
const float maxDepth = 10.0f;
|
||||
const float quantization = 100.0f;
|
||||
const float minDepth = quantization * (quantization + 1.0f) / (65535.0f + quantization * (quantization + 1.0f) / maxDepth);
|
||||
cv::Mat depth = makeFloatDepth(48, 64, minDepth * 1.001f, maxDepth * 0.999f);
|
||||
const float invalid[] = {
|
||||
0.0f, -1.0f, maxDepth, maxDepth * 2.0f, minDepth * 0.9f,
|
||||
std::numeric_limits<float>::quiet_NaN(),
|
||||
std::numeric_limits<float>::infinity(),
|
||||
-std::numeric_limits<float>::infinity()};
|
||||
const int nInvalid = sizeof(invalid) / sizeof(float);
|
||||
for(int i = 0; i < nInvalid; ++i)
|
||||
{
|
||||
depth.at<float>(0, i) = invalid[i];
|
||||
}
|
||||
|
||||
for(const std::string codec : {".png", ".rvl"})
|
||||
{
|
||||
SCOPED_TRACE(codec);
|
||||
const std::string format = codec + ":10:100";
|
||||
const std::vector<unsigned char> bytes = compressImage(depth, format);
|
||||
ASSERT_FALSE(bytes.empty());
|
||||
EXPECT_LT(bytes.size(), depth.total() * depth.elemSize() / 2);
|
||||
EXPECT_EQ(compressedDepthFormat(bytes), format);
|
||||
|
||||
const cv::Mat restored = uncompressImage(bytes);
|
||||
ASSERT_EQ(restored.type(), CV_32FC1);
|
||||
ASSERT_EQ(restored.size(), depth.size());
|
||||
for(int r = 0; r < depth.rows; ++r)
|
||||
{
|
||||
for(int c = 0; c < depth.cols; ++c)
|
||||
{
|
||||
const float d = depth.at<float>(r, c);
|
||||
if(r == 0 && c < nInvalid)
|
||||
{
|
||||
EXPECT_EQ(restored.at<float>(r, c), 0.0f) << "input=" << d;
|
||||
}
|
||||
else
|
||||
{
|
||||
ASSERT_NEAR(restored.at<float>(r, c), d, invDepthTolerance(d, quantization)) << "r=" << r << " c=" << c;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Re-compressing with the detected format gives back the same bytes
|
||||
// (e.g., DatabaseViewer saving an edited depth image).
|
||||
EXPECT_EQ(compressImage(restored, compressedDepthFormat(bytes)), compressImage(restored, format));
|
||||
|
||||
// Same through cv::Mat and thread overloads
|
||||
CompressionThread compressThread(depth, format);
|
||||
compressThread.start();
|
||||
compressThread.join();
|
||||
const cv::Mat bytesMat = compressThread.getCompressedData();
|
||||
ASSERT_EQ(bytesMat.total(), bytes.size());
|
||||
EXPECT_EQ(memcmp(bytesMat.data, bytes.data(), bytes.size()), 0);
|
||||
CompressionThread uncompressThread(bytesMat, true);
|
||||
uncompressThread.start();
|
||||
uncompressThread.join();
|
||||
expectMatEqual(uncompressThread.getUncompressedData(), restored);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(CompressionTest, InverseDepthQuantizationParameters)
|
||||
{
|
||||
const cv::Mat depth = makeFloatDepth(32, 32, 1.0f, 39.0f);
|
||||
const std::vector<unsigned char> bytes = compressImage(depth, ".png:40:50");
|
||||
EXPECT_EQ(compressedDepthFormat(bytes), ".png:40:50");
|
||||
const cv::Mat restored = uncompressImage(bytes);
|
||||
ASSERT_EQ(restored.type(), CV_32FC1);
|
||||
for(int r = 0; r < depth.rows; ++r)
|
||||
{
|
||||
for(int c = 0; c < depth.cols; ++c)
|
||||
{
|
||||
const float d = depth.at<float>(r, c);
|
||||
ASSERT_NEAR(restored.at<float>(r, c), d, invDepthTolerance(d, 50.0f));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST(CompressionTest, InverseDepthNonContinuousImage)
|
||||
{
|
||||
const cv::Mat depth = makeFloatDepth(20, 30, 1.0f, 5.0f);
|
||||
const cv::Mat roi = depth(cv::Rect(3, 2, 10, 8));
|
||||
ASSERT_FALSE(roi.isContinuous());
|
||||
const cv::Mat restored = uncompressImage(compressImage(roi, ".rvl:10:100"));
|
||||
ASSERT_EQ(restored.size(), roi.size());
|
||||
for(int r = 0; r < roi.rows; ++r)
|
||||
{
|
||||
for(int c = 0; c < roi.cols; ++c)
|
||||
{
|
||||
const float d = roi.at<float>(r, c);
|
||||
ASSERT_NEAR(restored.at<float>(r, c), d, invDepthTolerance(d, 100.0f));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST(CompressionTest, DepthParametersIgnoredFor16UC1)
|
||||
{
|
||||
cv::Mat depth(24, 32, CV_16UC1);
|
||||
cv::randu(depth, 0, 20000); // includes values over the max depth below
|
||||
for(const std::string codec : {".png", ".rvl"})
|
||||
{
|
||||
SCOPED_TRACE(codec);
|
||||
const std::vector<unsigned char> bytes = compressImage(depth, codec + ":10:100");
|
||||
EXPECT_EQ(bytes, compressImage(depth, codec));
|
||||
EXPECT_EQ(compressedDepthFormat(bytes), codec);
|
||||
expectMatEqual(uncompressImage(bytes), depth);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(CompressionTest, LegacyFloatDepthIsLossless)
|
||||
{
|
||||
const cv::Mat depth = makeFloatDepth(16, 16, 0.01f, 100.0f);
|
||||
for(const std::string format : {".png", ".rvl"})
|
||||
{
|
||||
SCOPED_TRACE(format);
|
||||
const std::vector<unsigned char> bytes = compressImage(depth, format);
|
||||
EXPECT_EQ(compressedDepthFormat(bytes), ".png");
|
||||
const cv::Mat restored = uncompressImage(bytes);
|
||||
ASSERT_EQ(restored.type(), CV_32FC1);
|
||||
EXPECT_EQ(memcmp(restored.data, depth.data, depth.total() * depth.elemSize()), 0);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(CompressionTest, MalformedDepthFormatsDecodeToEmpty)
|
||||
{
|
||||
// Signature and header only, no payload
|
||||
std::vector<unsigned char> invDepth = {'D', 'E', 'P', 'T', 'H', 'I', 'N', 'V'};
|
||||
invDepth.resize(16, 0);
|
||||
EXPECT_TRUE(uncompressImage(invDepth).empty());
|
||||
EXPECT_EQ(compressedDepthFormat(invDepth), ".png") << "too short to be inverse depth";
|
||||
|
||||
// Inverse depth header followed by an 8 bits image instead of a 16 bits one
|
||||
const std::vector<unsigned char> png8 = compressImage(cv::Mat(4, 4, CV_8UC1, cv::Scalar(1)), ".png");
|
||||
invDepth.insert(invDepth.end(), png8.begin(), png8.end());
|
||||
EXPECT_TRUE(uncompressImage(invDepth).empty());
|
||||
|
||||
// RVL signature without its size
|
||||
const std::vector<unsigned char> rvl = {'D', 'E', 'P', 'T', 'H', 'R', 'V', 'L', 4, 0};
|
||||
EXPECT_TRUE(uncompressImage(rvl).empty());
|
||||
EXPECT_EQ(compressedDepthFormat(rvl), ".rvl");
|
||||
|
||||
EXPECT_TRUE(uncompressImage(nullptr, 0).empty());
|
||||
}
|
||||
|
||||
TEST(CompressionTest, CompressionThreadRejectsInvalidFormat)
|
||||
{
|
||||
// std::string: a string literal would select the (bytes, isImage) constructor
|
||||
const cv::Mat depth(4, 4, CV_32FC1, cv::Scalar(1.0f));
|
||||
EXPECT_THROW(CompressionThread(depth, std::string(".jpg:10")), UException);
|
||||
EXPECT_THROW(CompressionThread(depth, std::string(".bmp")), UException);
|
||||
EXPECT_NO_THROW(CompressionThread(depth, std::string(".rvl:10:100")));
|
||||
}
|
||||
|
||||
@@ -4681,128 +4681,3 @@ TEST(MemoryTest, CreateSignatureRecompressesStereoPairAfterRectification)
|
||||
EXPECT_GT(cv::countNonZero(uncompressImage(stored.depthOrRightCompressed()) != right), 0)
|
||||
<< "stored right image still holds the unrectified pixels";
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Mem/DepthCompressionFormat with inverse depth (".rvl:max:q"), which databases
|
||||
// older than 0.24 cannot hold: rtabmap 0.23 would still open them (e.g., created
|
||||
// with Db/TargetVersion=0.23.0) but could not decode their depth images.
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
namespace {
|
||||
|
||||
enum DepthInput
|
||||
{
|
||||
kRawDepth,
|
||||
kCompressedDepthWithRaw, // e.g., received from ROS and decoded
|
||||
kCompressedDepthOnly // raw depth not needed (no features extracted here)
|
||||
};
|
||||
|
||||
struct InverseDepthCase
|
||||
{
|
||||
const char * targetVersion;
|
||||
DepthInput input;
|
||||
const char * depthCompressionFormat;
|
||||
bool parallelCompression;
|
||||
const char * expectedFormat;
|
||||
};
|
||||
|
||||
class MemoryInverseDepthTest : public ::testing::TestWithParam<InverseDepthCase> {};
|
||||
|
||||
} // namespace
|
||||
|
||||
TEST_P(MemoryInverseDepthTest, StoredDepthFormatFollowsDatabaseVersion)
|
||||
{
|
||||
const InverseDepthCase & cs = GetParam();
|
||||
ParametersMap params = defaultMemoryParams();
|
||||
params[Parameters::kMemBinDataKept()] = "true";
|
||||
params[Parameters::kMemDepthCompressionFormat()] = cs.depthCompressionFormat;
|
||||
params[Parameters::kMemCompressionParallelized()] = cs.parallelCompression ? "true" : "false";
|
||||
params[Parameters::kDbTargetVersion()] = cs.targetVersion;
|
||||
Memory memory(params);
|
||||
const std::string dbPath = uniqueDbPath();
|
||||
ASSERT_TRUE(memory.init(dbPath, true, params));
|
||||
|
||||
const cv::Mat rgb(16, 16, CV_8UC3, cv::Scalar(10, 20, 30));
|
||||
cv::Mat depth(16, 16, CV_32FC1);
|
||||
cv::randu(depth, 0.5f, 8.0f);
|
||||
const CameraModel model(10.0, 10.0, 8.0, 8.0, CameraModel::opticalRotation());
|
||||
SensorData data;
|
||||
if(cs.input == kRawDepth)
|
||||
{
|
||||
data = SensorData(rgb, depth, model);
|
||||
}
|
||||
else
|
||||
{
|
||||
data = SensorData(compressImage2(rgb, ".png"), compressImage2(depth, ".png:10:100"), model);
|
||||
if(cs.input == kCompressedDepthWithRaw)
|
||||
{
|
||||
data.uncompressData();
|
||||
ASSERT_FALSE(data.depthRaw().empty());
|
||||
}
|
||||
}
|
||||
|
||||
ASSERT_TRUE(memory.update(data, Transform(0, 0, 0, 0, 0, 0), cv::Mat::eye(6, 6, CV_64FC1) * 0.01));
|
||||
const Signature * s = memory.getSignature(memory.getLastSignatureId());
|
||||
ASSERT_NE(s, nullptr);
|
||||
const cv::Mat & stored = s->sensorData().depthOrRightCompressed();
|
||||
ASSERT_FALSE(stored.empty());
|
||||
EXPECT_EQ(compressedDepthFormat(stored), cs.expectedFormat);
|
||||
|
||||
const cv::Mat restored = uncompressImage(stored);
|
||||
ASSERT_EQ(restored.type(), CV_32FC1);
|
||||
ASSERT_EQ(restored.size(), depth.size());
|
||||
EXPECT_LT(cv::norm(restored, depth, cv::NORM_INF), 0.01);
|
||||
|
||||
memory.close(false);
|
||||
UFile::erase(dbPath);
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_SUITE_P(
|
||||
DatabaseVersions,
|
||||
MemoryInverseDepthTest,
|
||||
::testing::Values(
|
||||
InverseDepthCase{"", kRawDepth, ".rvl:10:100", true, ".rvl:10:100"},
|
||||
InverseDepthCase{"", kRawDepth, ".rvl:10:100", false, ".rvl:10:100"},
|
||||
InverseDepthCase{"", kCompressedDepthWithRaw, ".rvl:10:100", true, ".png:10:100"}, // reused as is
|
||||
InverseDepthCase{"", kCompressedDepthOnly, ".rvl:10:100", true, ".png:10:100"}, // reused as is
|
||||
InverseDepthCase{"0.23.0", kRawDepth, ".rvl:10:100", true, ".png"}, // legacy 32FC1 format
|
||||
InverseDepthCase{"0.23.0", kCompressedDepthWithRaw, ".rvl:10:100", true, ".png"}, // re-compressed
|
||||
InverseDepthCase{"0.23.0", kCompressedDepthOnly, ".rvl:10:100", true, ".png"}, // decompressed, re-compressed
|
||||
InverseDepthCase{"", kRawDepth, ".rvl", true, ".png"}, // RVL is 16UC1 only: legacy
|
||||
InverseDepthCase{"", kRawDepth, ".jpg", true, ".png"})); // invalid: default ".rvl"
|
||||
|
||||
// Compressed images that Memory rectifies (Rtabmap/ImagesAlreadyRectified=false) are
|
||||
// decoded for it, even when nothing else needs them (no feature extraction here): they
|
||||
// are stored rectified, not as received.
|
||||
TEST(MemoryTest, DecodesCompressedImagesToRectifyThem)
|
||||
{
|
||||
for(bool alreadyRectified : {true, false})
|
||||
{
|
||||
SCOPED_TRACE(alreadyRectified ? "already rectified" : "rectified by Memory");
|
||||
ParametersMap params = defaultMemoryParams();
|
||||
params[Parameters::kMemBinDataKept()] = "true";
|
||||
params[Parameters::kRtabmapImagesAlreadyRectified()] = alreadyRectified ? "true" : "false";
|
||||
Memory memory(params);
|
||||
ASSERT_TRUE(memory.init(""));
|
||||
|
||||
cv::Mat rgb(48, 64, CV_8UC3);
|
||||
cv::randu(rgb, 0, 255);
|
||||
const cv::Mat K = (cv::Mat_<double>(3, 3) << 50, 0, 32, 0, 50, 24, 0, 0, 1);
|
||||
const cv::Mat D = (cv::Mat_<double>(1, 5) << -0.3, 0.1, 0, 0, 0);
|
||||
const cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
|
||||
const cv::Mat P = (cv::Mat_<double>(3, 4) << 50, 0, 32, 0, 0, 50, 24, 0, 0, 0, 1, 0);
|
||||
const CameraModel model("cam", cv::Size(64, 48), K, D, R, P, CameraModel::opticalRotation());
|
||||
ASSERT_TRUE(model.isValidForRectification());
|
||||
const cv::Mat compressed = compressImage2(rgb, ".png");
|
||||
SensorData data(compressed, cv::Mat(), model);
|
||||
|
||||
ASSERT_TRUE(memory.update(data, Transform(0, 0, 0, 0, 0, 0), cv::Mat::eye(6, 6, CV_64FC1) * 0.01));
|
||||
const Signature * s = memory.getSignature(memory.getLastSignatureId());
|
||||
ASSERT_NE(s, nullptr);
|
||||
const cv::Mat & stored = s->sensorData().imageCompressed();
|
||||
ASSERT_FALSE(stored.empty());
|
||||
const bool sameBytes = stored.total() == compressed.total() &&
|
||||
memcmp(stored.data, compressed.data, compressed.total()) == 0;
|
||||
EXPECT_EQ(sameBytes, alreadyRectified);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -199,7 +199,7 @@ TEST(SensorDataTest, IsValidWithCameraModel)
|
||||
{
|
||||
SensorData data;
|
||||
data.setRGBDImage(cv::Mat(), cv::Mat(), CameraModel());
|
||||
EXPECT_TRUE(data.cameraModels().empty()); // invalid without image: placeholder not kept
|
||||
EXPECT_FALSE(data.cameraModels().empty());
|
||||
EXPECT_FALSE(data.isValid()); // not valid for projection
|
||||
|
||||
data.setRGBDImage(cv::Mat(), cv::Mat(), CameraModel(525.0, 525.0, 320.0, 240.0));
|
||||
@@ -986,32 +986,3 @@ TEST(SensorDataTest, DifferentDepthTypes)
|
||||
EXPECT_EQ(data.depthOrRightRaw().type(), CV_32FC1);
|
||||
}
|
||||
|
||||
|
||||
// An invalid CameraModel without any image is a placeholder (e.g., lidar odometry
|
||||
// creating scan-only data with CameraModel()): it is not kept. With an image, it is kept,
|
||||
// as images can be used without calibration.
|
||||
TEST(SensorDataTest, InvalidCameraModelIsKeptOnlyWithImages)
|
||||
{
|
||||
const LaserScan scan(cv::Mat(1, 3, CV_32FC2, cv::Scalar(1.0f, 0.0f)), 0, 10.0f, LaserScan::kXY);
|
||||
const SensorData scanOnly(scan, cv::Mat(), cv::Mat(), CameraModel(), 1, 1.0);
|
||||
EXPECT_TRUE(scanOnly.cameraModels().empty());
|
||||
EXPECT_TRUE(scanOnly.isValid());
|
||||
|
||||
const cv::Mat image(4, 6, CV_8UC1, cv::Scalar(1));
|
||||
const SensorData uncalibrated(image, CameraModel(), 1, 1.0);
|
||||
EXPECT_EQ(uncalibrated.cameraModels().size(), 1u);
|
||||
|
||||
const SensorData compressedOnly(compressImage2(image, ".png"), CameraModel(), 1, 1.0);
|
||||
EXPECT_EQ(compressedOnly.cameraModels().size(), 1u);
|
||||
|
||||
const CameraModel valid(10.0, 10.0, 3.0, 2.0);
|
||||
const SensorData calibratedNoImage(scan, cv::Mat(), cv::Mat(), valid, 1, 1.0);
|
||||
EXPECT_EQ(calibratedNoImage.cameraModels().size(), 1u) << "valid models are always kept";
|
||||
|
||||
// Keeping the images already there: they still need their model
|
||||
SensorData data(image, CameraModel(), 1, 1.0);
|
||||
data.setRGBDImage(cv::Mat(), cv::Mat(), CameraModel(), false);
|
||||
EXPECT_EQ(data.cameraModels().size(), 1u);
|
||||
data.setRGBDImage(cv::Mat(), cv::Mat(), CameraModel(), true);
|
||||
EXPECT_TRUE(data.cameraModels().empty()) << "images cleared, nothing left to describe";
|
||||
}
|
||||
|
||||
@@ -12391,7 +12391,7 @@ see Sqlite3 doc 'PRAGMA synchronous'.</string>
|
||||
<item row="5" column="1">
|
||||
<widget class="QLabel" name="label_760">
|
||||
<property name="text">
|
||||
<string>Depth image compression format (should be ".png" or ".rvl"). Add ":maxDepth[:quantization]" (e.g., ".rvl:10:100") to compress 32FC1 depth images as 16 bits inverse depth (lossy: precision ~d²/(2q(q+1)), depth over maxDepth is lost, databases cannot be opened by versions < 0.24). 16UC1 depth images are always compressed losslessly.</string>
|
||||
<string>Depth image compression format (should be ".png" or ".rvl").</string>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
|
||||
+4
-4
@@ -1,7 +1,7 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>rtabmap</name>
|
||||
<version>0.24.0</version>
|
||||
<version>0.23.13</version>
|
||||
<description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
|
||||
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
|
||||
<author>Mathieu Labbe</author>
|
||||
@@ -18,12 +18,12 @@
|
||||
<depend>gtsam</depend>
|
||||
<!-- <depend>libopenni2-dev</depend> --> <!-- not available on Jessie -->
|
||||
<depend>libpcl-all-dev</depend> <!-- include libvtk-qt -->
|
||||
<depend>libpointmatcher</depend> <!-- optional but recommended if lidar is used, also not available on 32 bits system, but rtabmap can be built without it -->
|
||||
<!-- <depend>libpointmatcher</depend> Not available on rolling--> <!-- optional but recommended if lidar is used, also not available on 32 bits system, but rtabmap can be built without it -->
|
||||
<!-- <depend>libproj-dev</depend> needed due to error in vtk6 (kinetic)-->
|
||||
<depend>libsqlite3-dev</depend>
|
||||
<depend>liboctomap-dev</depend>
|
||||
<!-- <depend>grid_map_core</depend> --> <!-- till this PR is released https://github.com/ANYbotics/grid_map/pull/499 -->
|
||||
<depend>qt_gui_cpp</depend> <!-- libqt4-dev or libqt5-dev -->
|
||||
<!-- <depend>grid_map_core</depend> # not available on Rolling -->
|
||||
<depend>qtbase5-dev</depend>
|
||||
<depend>zlib</depend>
|
||||
|
||||
<export>
|
||||
|
||||
@@ -17,7 +17,6 @@ ADD_SUBDIRECTORY( Info )
|
||||
ADD_SUBDIRECTORY( CleanupLocalGrids )
|
||||
ADD_SUBDIRECTORY( GlobalBundleAdjustment )
|
||||
ADD_SUBDIRECTORY( ReduceGraph )
|
||||
ADD_SUBDIRECTORY( LidarCameraCalibration )
|
||||
|
||||
IF(OPENCV_NONFREE_FOUND)
|
||||
ADD_SUBDIRECTORY( VocabularyComparison )
|
||||
|
||||
@@ -1,33 +0,0 @@
|
||||
|
||||
ADD_EXECUTABLE(lidarCameraCalibration
|
||||
main.cpp
|
||||
CalibrationProblem.cpp
|
||||
CorrectionSolver.cpp
|
||||
PatternSearchSolver.cpp
|
||||
DownhillSimplexSolver.cpp
|
||||
G2oSolver.cpp
|
||||
GtsamSolver.cpp)
|
||||
|
||||
TARGET_LINK_LIBRARIES(lidarCameraCalibration rtabmap_core)
|
||||
|
||||
# For the g2o solver: rtabmap_core links g2o privately.
|
||||
IF(G2O_FOUND)
|
||||
IF(g2o_FOUND)
|
||||
TARGET_LINK_LIBRARIES(lidarCameraCalibration g2o::core g2o::solver_eigen)
|
||||
ELSE()
|
||||
TARGET_INCLUDE_DIRECTORIES(lidarCameraCalibration PRIVATE ${G2O_INCLUDE_DIRS})
|
||||
TARGET_LINK_LIBRARIES(lidarCameraCalibration ${G2O_LIBRARIES})
|
||||
ENDIF()
|
||||
ENDIF(G2O_FOUND)
|
||||
|
||||
# For the GTSAM solver: rtabmap_core links GTSAM privately.
|
||||
IF(GTSAM_FOUND)
|
||||
TARGET_LINK_LIBRARIES(lidarCameraCalibration gtsam)
|
||||
ENDIF(GTSAM_FOUND)
|
||||
|
||||
SET_TARGET_PROPERTIES( lidarCameraCalibration
|
||||
PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-lidarCameraCalibration)
|
||||
|
||||
INSTALL(TARGETS lidarCameraCalibration
|
||||
RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
|
||||
BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
|
||||
@@ -1,130 +0,0 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "CalibrationProblem.h"
|
||||
|
||||
#include <rtabmap/core/util3d_transforms.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
Transform correctionFrom(const double p[6])
|
||||
{
|
||||
return Transform(p[0], p[1], p[2], p[3]*M_PI/180.0, p[4]*M_PI/180.0, p[5]*M_PI/180.0);
|
||||
}
|
||||
|
||||
Eigen::Isometry3d correctionFromDouble(const double p[6])
|
||||
{
|
||||
// As Transform(x, y, z, roll, pitch, yaw): yaw * pitch * roll.
|
||||
Eigen::Isometry3d C = Eigen::Isometry3d::Identity();
|
||||
C.translation() = Eigen::Vector3d(p[0], p[1], p[2]);
|
||||
C.linear() = (Eigen::AngleAxisd(p[5]*M_PI/180.0, Eigen::Vector3d::UnitZ()) *
|
||||
Eigen::AngleAxisd(p[4]*M_PI/180.0, Eigen::Vector3d::UnitY()) *
|
||||
Eigen::AngleAxisd(p[3]*M_PI/180.0, Eigen::Vector3d::UnitX())).toRotationMatrix();
|
||||
return C;
|
||||
}
|
||||
|
||||
CalibrationProblem::CalibrationProblem(const std::vector<Frame> & frames, const std::vector<int> & nodes, float minDepth) :
|
||||
frames_(frames), nodes_(nodes), minDepth_(minDepth), evaluations_(0)
|
||||
{
|
||||
}
|
||||
|
||||
void CalibrationProblem::project(const Transform & C,
|
||||
const std::function<void(const Frame &, size_t, float, float)> & visit) const
|
||||
{
|
||||
for(int k : nodes_)
|
||||
{
|
||||
const Frame & f = frames_[k];
|
||||
const Transform scanInCam = (f.scanToCam * C).inverse();
|
||||
const double fx = f.K.at<double>(0,0), fy = f.K.at<double>(1,1);
|
||||
const double cx = f.K.at<double>(0,2), cy = f.K.at<double>(1,2);
|
||||
for(size_t i = 0; i < f.edgePoints.size(); ++i)
|
||||
{
|
||||
const cv::Point3f pc = util3d::transformPoint(f.edgePoints[i], scanInCam);
|
||||
if(pc.z < minDepth_)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
const float u = fx * pc.x / pc.z + cx, v = fy * pc.y / pc.z + cy;
|
||||
if(u < 0 || v < 0 || u >= f.gray.cols - 1 || v >= f.gray.rows - 1)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
visit(f, i, u, v);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
double CalibrationProblem::score(const Transform & C) const
|
||||
{
|
||||
++evaluations_;
|
||||
double sum = 0.0;
|
||||
project(C, [&](const Frame & f, size_t i, float u, float v) {
|
||||
const int u0 = int(u), v0 = int(v);
|
||||
const float a = u - u0, b = v - v0;
|
||||
const float s =
|
||||
(1-a)*(1-b)*f.edgeScore.at<float>(v0, u0) + a*(1-b)*f.edgeScore.at<float>(v0, u0+1) +
|
||||
(1-a)*b*f.edgeScore.at<float>(v0+1, u0) + a*b*f.edgeScore.at<float>(v0+1, u0+1);
|
||||
sum += f.edgeWeights[i] * s;
|
||||
});
|
||||
// The frames' edge points are selected again between passes: not cached.
|
||||
double weights = 0.0;
|
||||
for(int k : nodes_)
|
||||
{
|
||||
for(float w : frames_[k].edgeWeights)
|
||||
{
|
||||
weights += w;
|
||||
}
|
||||
}
|
||||
return weights > 0.0 ? sum / weights : 0.0;
|
||||
}
|
||||
|
||||
double CalibrationProblem::edgeDistance(const Frame & f, size_t i, const double p[6], double cap) const
|
||||
{
|
||||
const cv::Point3f & pt = f.edgePoints[i];
|
||||
const Eigen::Vector3d pc =
|
||||
(f.scanToCam.toEigen3d() * correctionFromDouble(p)).inverse() * Eigen::Vector3d(pt.x, pt.y, pt.z);
|
||||
if(pc.z() < minDepth_)
|
||||
{
|
||||
return cap;
|
||||
}
|
||||
const double u = f.K.at<double>(0,0) * pc.x() / pc.z() + f.K.at<double>(0,2);
|
||||
const double v = f.K.at<double>(1,1) * pc.y() / pc.z() + f.K.at<double>(1,2);
|
||||
if(u < 0 || v < 0 || u >= f.edgeDistance.cols - 1 || v >= f.edgeDistance.rows - 1)
|
||||
{
|
||||
return cap;
|
||||
}
|
||||
const int u0 = int(u), v0 = int(v);
|
||||
const double a = u - u0, b = v - v0;
|
||||
const cv::Mat & d = f.edgeDistance;
|
||||
const double distance =
|
||||
(1-a)*(1-b)*d.at<float>(v0, u0) + a*(1-b)*d.at<float>(v0, u0+1) +
|
||||
(1-a)*b*d.at<float>(v0+1, u0) + a*b*d.at<float>(v0+1, u0+1);
|
||||
return std::min(distance, cap);
|
||||
}
|
||||
@@ -1,113 +0,0 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef LIDARCAMERACALIBRATION_CALIBRATIONPROBLEM_H_
|
||||
#define LIDARCAMERACALIBRATION_CALIBRATIONPROBLEM_H_
|
||||
|
||||
#include <rtabmap/core/Transform.h>
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
#include <functional>
|
||||
#include <vector>
|
||||
|
||||
// What makes a lidar point an edge point, in order of precedence.
|
||||
enum EdgeType
|
||||
{
|
||||
kEdgeDepth = 0, // in front of a depth discontinuity
|
||||
kEdgeCrease = 1, // where the surface's orientation changes (e.g., wall and floor)
|
||||
kEdgeIntensity = 2 // where the surface's reflectance changes
|
||||
};
|
||||
|
||||
// One node of the database: its image, its lidar scan, and the lidar's edges.
|
||||
struct Frame
|
||||
{
|
||||
int id;
|
||||
cv::Mat gray; // as stored, matching the camera model
|
||||
cv::Mat edges; // CV_8U: the image's edges (Canny), non-zero on an edge
|
||||
cv::Mat edgeDistance; // CV_32F: distance (pixels) to the image's nearest edge
|
||||
cv::Mat edgeScore; // CV_32F: 1 on the image's edges, decaying with the distance to them
|
||||
cv::Mat K; // CV_64F 3x3
|
||||
rtabmap::Transform scanToCam; // camera pose in the scan frame, before correction
|
||||
cv::Mat cloud; // Nx4 CV_32F, scan frame: x y z intensity (0 without intensity)
|
||||
cv::Mat normals; // Mx6 CV_32F, scan frame: voxelized points and their normals (for creases)
|
||||
bool hasIntensity;
|
||||
// The robot's speeds (m/s, deg/s; negative if unknown): instantaneous, odometry's when the
|
||||
// node was added, and mean along odometry over the meanWindow (s) since the previous
|
||||
// node, which is when an assembled scan was taken.
|
||||
float linearSpeed = -1.0f, angularSpeed = -1.0f;
|
||||
float meanLinearSpeed = -1.0f, meanAngularSpeed = -1.0f, meanWindow = 0.0f;
|
||||
int meanPoses = 0; // odometry steps the mean speed is from: more than 1 with intermediate nodes
|
||||
std::vector<cv::Point3f> edgePoints; // depth and intensity edge points, scan frame
|
||||
std::vector<float> edgeWeights;
|
||||
std::vector<unsigned char> edgeTypes; // EdgeType of each edge point
|
||||
};
|
||||
|
||||
// The correction of parameters p = tx ty tz (m), rx ry rz (deg), in the camera frame.
|
||||
rtabmap::Transform correctionFrom(const double p[6]);
|
||||
// The same, in double precision: rtabmap's Transform is float, too coarse for the small
|
||||
// steps of numerical derivatives.
|
||||
Eigen::Isometry3d correctionFromDouble(const double p[6]);
|
||||
|
||||
// The lidar edge points of some nodes, and how well a candidate correction C lines them
|
||||
// up with their images' edges. This is all a solver sees of the calibration.
|
||||
class CalibrationProblem
|
||||
{
|
||||
public:
|
||||
// The frames' edge points are read at each call, so they can be selected again after
|
||||
// the problem is made. minDepth: lidar points closer than this to the camera (m) are
|
||||
// ignored.
|
||||
CalibrationProblem(const std::vector<Frame> & frames, const std::vector<int> & nodes, float minDepth);
|
||||
|
||||
const std::vector<Frame> & frames() const {return frames_;}
|
||||
const std::vector<int> & nodes() const {return nodes_;}
|
||||
float minDepth() const {return minDepth_;}
|
||||
|
||||
// Calls visit(frame, point index, u, v) for each edge point that C projects in its
|
||||
// image, at full resolution.
|
||||
void project(const rtabmap::Transform & C,
|
||||
const std::function<void(const Frame &, size_t, float, float)> & visit) const;
|
||||
|
||||
// What the direct solvers maximize: the weighted mean of the image edges' score under
|
||||
// the projected lidar edge points, in [0, 1]. A point out of its image counts as 0.
|
||||
double score(const rtabmap::Transform & C) const;
|
||||
int evaluations() const {return evaluations_;}
|
||||
|
||||
// What the least-squares solvers minimize, per point: the distance (pixels) from
|
||||
// where p projects edge point i of f to f's nearest image edge, capped; the cap where
|
||||
// it does not project in the image. In double precision, for numerical derivatives.
|
||||
double edgeDistance(const Frame & f, size_t i, const double p[6], double cap) const;
|
||||
|
||||
private:
|
||||
const std::vector<Frame> & frames_;
|
||||
std::vector<int> nodes_;
|
||||
float minDepth_;
|
||||
mutable int evaluations_;
|
||||
};
|
||||
|
||||
#endif /* LIDARCAMERACALIBRATION_CALIBRATIONPROBLEM_H_ */
|
||||
@@ -1,74 +0,0 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "CorrectionSolver.h"
|
||||
#include "DownhillSimplexSolver.h"
|
||||
#include "G2oSolver.h"
|
||||
#include "GtsamSolver.h"
|
||||
#include "PatternSearchSolver.h"
|
||||
|
||||
#include <rtabmap/core/Version.h>
|
||||
|
||||
const double kInitialSteps[6] = {0.04, 0.04, 0.04, 2.0, 2.0, 2.0};
|
||||
|
||||
std::unique_ptr<CorrectionSolver> createSolver(const std::string & name, float sigma)
|
||||
{
|
||||
(void)sigma; // used only by the least-squares solvers
|
||||
if(name == "pattern")
|
||||
{
|
||||
return std::unique_ptr<CorrectionSolver>(new PatternSearchSolver());
|
||||
}
|
||||
if(name == "simplex")
|
||||
{
|
||||
return std::unique_ptr<CorrectionSolver>(new DownhillSimplexSolver());
|
||||
}
|
||||
#ifdef RTABMAP_G2O
|
||||
if(name == "g2o")
|
||||
{
|
||||
return std::unique_ptr<CorrectionSolver>(new G2oSolver(sigma));
|
||||
}
|
||||
#endif
|
||||
#ifdef RTABMAP_GTSAM
|
||||
if(name == "gtsam")
|
||||
{
|
||||
return std::unique_ptr<CorrectionSolver>(new GtsamSolver(sigma));
|
||||
}
|
||||
#endif
|
||||
return std::unique_ptr<CorrectionSolver>();
|
||||
}
|
||||
|
||||
std::vector<std::string> availableSolvers()
|
||||
{
|
||||
std::vector<std::string> names = {"simplex", "pattern"};
|
||||
#ifdef RTABMAP_G2O
|
||||
names.push_back("g2o");
|
||||
#endif
|
||||
#ifdef RTABMAP_GTSAM
|
||||
names.push_back("gtsam");
|
||||
#endif
|
||||
return names;
|
||||
}
|
||||
@@ -1,58 +0,0 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef LIDARCAMERACALIBRATION_CORRECTIONSOLVER_H_
|
||||
#define LIDARCAMERACALIBRATION_CORRECTIONSOLVER_H_
|
||||
|
||||
#include "CalibrationProblem.h"
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
// Parameters: p = tx ty tz (m), rx ry rz (deg), in the camera frame (see correctionFrom()).
|
||||
// Initial search steps, also the scale of each parameter.
|
||||
extern const double kInitialSteps[6];
|
||||
|
||||
// Finds the correction best aligning the problem's lidar edges with its images' edges.
|
||||
// A new solver implements this and is added to createSolver().
|
||||
class CorrectionSolver
|
||||
{
|
||||
public:
|
||||
virtual ~CorrectionSolver() {}
|
||||
virtual const char * name() const = 0;
|
||||
// Improves p from its value. Without estimateTranslation, p's translation is left as is.
|
||||
virtual void solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const = 0;
|
||||
};
|
||||
|
||||
// The solver named "pattern", "simplex", "g2o" or "gtsam" (the last two if rtabmap was
|
||||
// built with them), or null. sigma: the image edges' fall off (pixels).
|
||||
std::unique_ptr<CorrectionSolver> createSolver(const std::string & name, float sigma);
|
||||
// The names createSolver() knows in this build.
|
||||
std::vector<std::string> availableSolvers();
|
||||
|
||||
#endif /* LIDARCAMERACALIBRATION_CORRECTIONSOLVER_H_ */
|
||||
@@ -1,95 +0,0 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "DownhillSimplexSolver.h"
|
||||
|
||||
#include <opencv2/core/optim.hpp>
|
||||
|
||||
#include <algorithm>
|
||||
#include <vector>
|
||||
|
||||
namespace {
|
||||
|
||||
// The score to minimize, as a function of the solved parameters x; the others stay as
|
||||
// in p.
|
||||
class NegativeScore : public cv::MinProblemSolver::Function
|
||||
{
|
||||
public:
|
||||
NegativeScore(const CalibrationProblem & problem, const std::vector<int> & solved, const double p[6]) :
|
||||
problem_(problem), solved_(solved)
|
||||
{
|
||||
std::copy(p, p + 6, p_);
|
||||
}
|
||||
int getDims() const override {return (int)solved_.size();}
|
||||
double calc(const double * x) const override
|
||||
{
|
||||
double q[6];
|
||||
std::copy(p_, p_ + 6, q);
|
||||
for(size_t i = 0; i < solved_.size(); ++i)
|
||||
{
|
||||
q[solved_[i]] = x[i];
|
||||
}
|
||||
return -problem_.score(correctionFrom(q));
|
||||
}
|
||||
private:
|
||||
const CalibrationProblem & problem_;
|
||||
std::vector<int> solved_;
|
||||
double p_[6];
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
void DownhillSimplexSolver::solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const
|
||||
{
|
||||
std::vector<int> solved;
|
||||
for(int k = estimateTranslation ? 0 : 3; k < 6; ++k)
|
||||
{
|
||||
solved.push_back(k);
|
||||
}
|
||||
|
||||
cv::Ptr<cv::DownhillSolver> solver = cv::DownhillSolver::create(
|
||||
cv::makePtr<NegativeScore>(problem, solved, p),
|
||||
cv::noArray(),
|
||||
cv::TermCriteria(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 5000, 1e-9));
|
||||
cv::Mat x(1, (int)solved.size(), CV_64F), step(1, (int)solved.size(), CV_64F);
|
||||
for(size_t i = 0; i < solved.size(); ++i)
|
||||
{
|
||||
x.at<double>(0, i) = p[solved[i]];
|
||||
step.at<double>(0, i) = kInitialSteps[solved[i]];
|
||||
}
|
||||
// Started again from its result: a simplex can shrink before it reaches the
|
||||
// optimum, the restart gives it its full size back.
|
||||
for(int restart = 0; restart < 2; ++restart)
|
||||
{
|
||||
solver->setInitStep(step);
|
||||
solver->minimize(x);
|
||||
}
|
||||
for(size_t i = 0; i < solved.size(); ++i)
|
||||
{
|
||||
p[solved[i]] = x.at<double>(0, i);
|
||||
}
|
||||
}
|
||||
@@ -1,42 +0,0 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef LIDARCAMERACALIBRATION_DOWNHILLSIMPLEXSOLVER_H_
|
||||
#define LIDARCAMERACALIBRATION_DOWNHILLSIMPLEXSOLVER_H_
|
||||
|
||||
#include "CorrectionSolver.h"
|
||||
|
||||
// OpenCV's Nelder-Mead simplex: moves all the parameters together, so it follows
|
||||
// directions in which they are coupled.
|
||||
class DownhillSimplexSolver : public CorrectionSolver
|
||||
{
|
||||
public:
|
||||
const char * name() const override {return "simplex";}
|
||||
void solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const override;
|
||||
};
|
||||
|
||||
#endif /* LIDARCAMERACALIBRATION_DOWNHILLSIMPLEXSOLVER_H_ */
|
||||
@@ -1,188 +0,0 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "G2oSolver.h"
|
||||
|
||||
#ifdef RTABMAP_G2O
|
||||
|
||||
#include <g2o/core/base_unary_edge.h>
|
||||
#include <g2o/core/base_vertex.h>
|
||||
#include <g2o/core/block_solver.h>
|
||||
#include <g2o/core/optimization_algorithm_levenberg.h>
|
||||
#include <g2o/core/robust_kernel_impl.h>
|
||||
#include <g2o/core/sparse_optimizer.h>
|
||||
#include <g2o/solvers/eigen/linear_solver_eigen.h>
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
namespace {
|
||||
|
||||
// The solved parameters of p (the rotation, and the translation if estimated).
|
||||
template<int D>
|
||||
class VertexCorrection : public g2o::BaseVertex<D, Eigen::Matrix<double, D, 1> >
|
||||
{
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
void setToOriginImpl() override {this->_estimate.setZero();}
|
||||
void oplusImpl(const double * update) override // as rtabmap's own g2o vertices
|
||||
{
|
||||
for(int i = 0; i < D; ++i)
|
||||
{
|
||||
this->_estimate[i] += update[i];
|
||||
}
|
||||
}
|
||||
bool read(std::istream &) override {return false;}
|
||||
bool write(std::ostream &) const override {return true;}
|
||||
};
|
||||
|
||||
// One lidar edge point: CalibrationProblem::edgeDistance() for it.
|
||||
template<int D>
|
||||
class EdgeToImageEdge : public g2o::BaseUnaryEdge<1, double, VertexCorrection<D> >
|
||||
{
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
EdgeToImageEdge(const CalibrationProblem & problem, const Frame & frame, size_t point,
|
||||
const std::vector<int> & solved, const double p[6], double cap) :
|
||||
problem_(problem), frame_(frame), point_(point), solved_(solved), cap_(cap)
|
||||
{
|
||||
std::copy(p, p + 6, p_);
|
||||
}
|
||||
|
||||
void computeError() override
|
||||
{
|
||||
this->_error[0] = residual(static_cast<const VertexCorrection<D> *>(this->_vertices[0])->estimate());
|
||||
}
|
||||
|
||||
// Central differences, with steps of the precision the projection needs rather than
|
||||
// g2o's default (1e-9), lost in the image's distance map.
|
||||
void linearizeOplus() override
|
||||
{
|
||||
const Eigen::Matrix<double, D, 1> x = static_cast<const VertexCorrection<D> *>(this->_vertices[0])->estimate();
|
||||
for(int k = 0; k < D; ++k)
|
||||
{
|
||||
const double h = solved_[k] < 3 ? 1e-4 : 1e-3; // m, deg
|
||||
Eigen::Matrix<double, D, 1> plus = x, minus = x;
|
||||
plus[k] += h;
|
||||
minus[k] -= h;
|
||||
this->_jacobianOplusXi(0, k) = (residual(plus) - residual(minus)) / (2.0 * h);
|
||||
}
|
||||
}
|
||||
|
||||
bool read(std::istream &) override {return false;}
|
||||
bool write(std::ostream &) const override {return true;}
|
||||
|
||||
private:
|
||||
double residual(const Eigen::Matrix<double, D, 1> & x) const
|
||||
{
|
||||
double q[6];
|
||||
std::copy(p_, p_ + 6, q);
|
||||
for(int k = 0; k < D; ++k)
|
||||
{
|
||||
q[solved_[k]] = x[k];
|
||||
}
|
||||
return problem_.edgeDistance(frame_, point_, q, cap_);
|
||||
}
|
||||
|
||||
const CalibrationProblem & problem_;
|
||||
const Frame & frame_;
|
||||
size_t point_;
|
||||
std::vector<int> solved_;
|
||||
double p_[6];
|
||||
double cap_;
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
void G2oSolver::solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const
|
||||
{
|
||||
coarse_.solve(problem, estimateTranslation, p);
|
||||
if(estimateTranslation)
|
||||
{
|
||||
solveWith<6>(problem, {0, 1, 2, 3, 4, 5}, p);
|
||||
}
|
||||
else
|
||||
{
|
||||
solveWith<3>(problem, {3, 4, 5}, p);
|
||||
}
|
||||
}
|
||||
|
||||
template<int D>
|
||||
void G2oSolver::solveWith(const CalibrationProblem & problem, const std::vector<int> & solved, double p[6]) const
|
||||
{
|
||||
typedef g2o::BlockSolver<g2o::BlockSolverTraits<D, 1> > Block;
|
||||
typedef g2o::LinearSolverEigen<typename Block::PoseMatrixType> Linear;
|
||||
|
||||
Eigen::Matrix<double, D, 1> x;
|
||||
for(int k = 0; k < D; ++k)
|
||||
{
|
||||
x[k] = p[solved[k]];
|
||||
}
|
||||
for(double scale : {9.0, 3.0, 1.0})
|
||||
{
|
||||
const double delta = scale * sigma_;
|
||||
g2o::SparseOptimizer optimizer;
|
||||
#ifdef RTABMAP_G2O_CPP11
|
||||
optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(
|
||||
std::unique_ptr<Block>(new Block(std::unique_ptr<Linear>(new Linear())))));
|
||||
#else
|
||||
optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(new Block(new Linear())));
|
||||
#endif
|
||||
VertexCorrection<D> * vertex = new VertexCorrection<D>();
|
||||
vertex->setEstimate(x);
|
||||
vertex->setId(0);
|
||||
optimizer.addVertex(vertex);
|
||||
|
||||
// Beyond a few times the kernel's scale, a point does not count anyway.
|
||||
const double cap = 10.0 * delta;
|
||||
int id = 1;
|
||||
for(int k : problem.nodes())
|
||||
{
|
||||
const Frame & f = problem.frames()[k];
|
||||
for(size_t i = 0; i < f.edgePoints.size(); ++i)
|
||||
{
|
||||
EdgeToImageEdge<D> * edge = new EdgeToImageEdge<D>(problem, f, i, solved, p, cap);
|
||||
edge->setId(id++);
|
||||
edge->setVertex(0, vertex);
|
||||
edge->setMeasurement(0.0);
|
||||
edge->setInformation(Eigen::Matrix<double, 1, 1>::Constant(f.edgeWeights[i]));
|
||||
g2o::RobustKernelWelsch * kernel = new g2o::RobustKernelWelsch();
|
||||
kernel->setDelta(delta);
|
||||
edge->setRobustKernel(kernel);
|
||||
optimizer.addEdge(edge);
|
||||
}
|
||||
}
|
||||
optimizer.initializeOptimization();
|
||||
optimizer.optimize(10); // close already, after the coarse search
|
||||
x = vertex->estimate();
|
||||
}
|
||||
for(int k = 0; k < D; ++k)
|
||||
{
|
||||
p[solved[k]] = x[k];
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -1,65 +0,0 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef LIDARCAMERACALIBRATION_G2OSOLVER_H_
|
||||
#define LIDARCAMERACALIBRATION_G2OSOLVER_H_
|
||||
|
||||
#include "CorrectionSolver.h"
|
||||
#include "PatternSearchSolver.h"
|
||||
|
||||
#include <rtabmap/core/Version.h>
|
||||
|
||||
#ifdef RTABMAP_G2O
|
||||
|
||||
// Least squares with g2o: Levenberg-Marquardt on the distances of the lidar edge points
|
||||
// to the images' edges (chamfer matching), weighted by the points' weights.
|
||||
//
|
||||
// - A redescending robust kernel (Welsch) makes a point far from any image edge count for
|
||||
// nothing, as the score does: many lidar edges have no counterpart in the image
|
||||
// (speckle, surfaces the camera does not see the same way).
|
||||
// - Levenberg-Marquardt only follows the local slope, and each point is pulled toward its
|
||||
// nearest image edge, often not its own when far from the solution: from a few degrees
|
||||
// away, it stops in a local minimum. So a coarse pattern search gets close first, then
|
||||
// the kernel's scale goes from wide to narrow (9, 3, then 1 x sigma).
|
||||
class G2oSolver : public CorrectionSolver
|
||||
{
|
||||
public:
|
||||
G2oSolver(double sigma) : sigma_(sigma), coarse_(4) {}
|
||||
const char * name() const override {return "g2o";}
|
||||
void solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const override;
|
||||
|
||||
private:
|
||||
template<int D>
|
||||
void solveWith(const CalibrationProblem & problem, const std::vector<int> & solved, double p[6]) const;
|
||||
|
||||
double sigma_;
|
||||
PatternSearchSolver coarse_; // steps of 2 down to 0.25 deg
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
#endif /* LIDARCAMERACALIBRATION_G2OSOLVER_H_ */
|
||||
@@ -1,159 +0,0 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "GtsamSolver.h"
|
||||
|
||||
#ifdef RTABMAP_GTSAM
|
||||
|
||||
#include <gtsam/linear/NoiseModel.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
|
||||
namespace {
|
||||
|
||||
// One lidar edge point: CalibrationProblem::edgeDistance() for it, as a function of the
|
||||
// solved parameters x of p.
|
||||
template<int D>
|
||||
class EdgeDistanceFactor : public gtsam::NoiseModelFactor1<Eigen::Matrix<double, D, 1> >
|
||||
{
|
||||
typedef Eigen::Matrix<double, D, 1> X;
|
||||
public:
|
||||
EdgeDistanceFactor(gtsam::Key key, const gtsam::SharedNoiseModel & model,
|
||||
const CalibrationProblem & problem, const Frame & frame, size_t point,
|
||||
const std::vector<int> & solved, const double p[6], double cap) :
|
||||
gtsam::NoiseModelFactor1<X>(model, key),
|
||||
problem_(problem), frame_(frame), point_(point), solved_(solved), cap_(cap)
|
||||
{
|
||||
std::copy(p, p + 6, p_);
|
||||
}
|
||||
|
||||
gtsam::Vector evaluateError(const X & x,
|
||||
#if GTSAM_VERSION_NUMERIC >= 40300
|
||||
gtsam::OptionalMatrixType H = OptionalNone) const override
|
||||
#else
|
||||
boost::optional<gtsam::Matrix &> H = boost::none) const override
|
||||
#endif
|
||||
{
|
||||
if(H)
|
||||
{
|
||||
// Central differences, with steps of the precision the projection needs.
|
||||
gtsam::Matrix J(1, D);
|
||||
for(int k = 0; k < D; ++k)
|
||||
{
|
||||
const double h = solved_[k] < 3 ? 1e-4 : 1e-3; // m, deg
|
||||
X plus = x, minus = x;
|
||||
plus[k] += h;
|
||||
minus[k] -= h;
|
||||
J(0, k) = (residual(plus) - residual(minus)) / (2.0 * h);
|
||||
}
|
||||
*H = J;
|
||||
}
|
||||
return gtsam::Vector1(residual(x));
|
||||
}
|
||||
|
||||
private:
|
||||
double residual(const X & x) const
|
||||
{
|
||||
double q[6];
|
||||
std::copy(p_, p_ + 6, q);
|
||||
for(int k = 0; k < D; ++k)
|
||||
{
|
||||
q[solved_[k]] = x[k];
|
||||
}
|
||||
return problem_.edgeDistance(frame_, point_, q, cap_);
|
||||
}
|
||||
|
||||
const CalibrationProblem & problem_;
|
||||
const Frame & frame_;
|
||||
size_t point_;
|
||||
std::vector<int> solved_;
|
||||
double p_[6];
|
||||
double cap_;
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
void GtsamSolver::solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const
|
||||
{
|
||||
coarse_.solve(problem, estimateTranslation, p);
|
||||
if(estimateTranslation)
|
||||
{
|
||||
solveWith<6>(problem, {0, 1, 2, 3, 4, 5}, p);
|
||||
}
|
||||
else
|
||||
{
|
||||
solveWith<3>(problem, {3, 4, 5}, p);
|
||||
}
|
||||
}
|
||||
|
||||
template<int D>
|
||||
void GtsamSolver::solveWith(const CalibrationProblem & problem, const std::vector<int> & solved, double p[6]) const
|
||||
{
|
||||
typedef Eigen::Matrix<double, D, 1> X;
|
||||
const gtsam::Key key = 0;
|
||||
|
||||
X x;
|
||||
for(int k = 0; k < D; ++k)
|
||||
{
|
||||
x[k] = p[solved[k]];
|
||||
}
|
||||
for(double scale : {9.0, 3.0, 1.0})
|
||||
{
|
||||
const double delta = scale * sigma_;
|
||||
const double cap = 10.0 * delta; // beyond a few times the scale, a point does not count anyway
|
||||
const gtsam::noiseModel::mEstimator::Welsch::shared_ptr welsch =
|
||||
gtsam::noiseModel::mEstimator::Welsch::Create(delta);
|
||||
gtsam::NonlinearFactorGraph graph;
|
||||
for(int k : problem.nodes())
|
||||
{
|
||||
const Frame & f = problem.frames()[k];
|
||||
for(size_t i = 0; i < f.edgePoints.size(); ++i)
|
||||
{
|
||||
// Information = the point's weight, as with g2o.
|
||||
const gtsam::SharedNoiseModel model = gtsam::noiseModel::Robust::Create(
|
||||
welsch, gtsam::noiseModel::Isotropic::Sigma(1, 1.0 / std::sqrt(f.edgeWeights[i])));
|
||||
graph.emplace_shared<EdgeDistanceFactor<D> >(key, model, problem, f, i, solved, p, cap);
|
||||
}
|
||||
}
|
||||
gtsam::Values initial;
|
||||
initial.insert(key, x);
|
||||
gtsam::LevenbergMarquardtParams params;
|
||||
params.setMaxIterations(10); // close already, after the coarse search
|
||||
x = gtsam::LevenbergMarquardtOptimizer(graph, initial, params).optimize().template at<X>(key);
|
||||
}
|
||||
for(int k = 0; k < D; ++k)
|
||||
{
|
||||
p[solved[k]] = x[k];
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -1,58 +0,0 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef LIDARCAMERACALIBRATION_GTSAMSOLVER_H_
|
||||
#define LIDARCAMERACALIBRATION_GTSAMSOLVER_H_
|
||||
|
||||
#include "CorrectionSolver.h"
|
||||
#include "PatternSearchSolver.h"
|
||||
|
||||
#include <rtabmap/core/Version.h>
|
||||
|
||||
#ifdef RTABMAP_GTSAM
|
||||
|
||||
// The same least squares as G2oSolver (see there), with GTSAM: one factor per lidar edge
|
||||
// point on the correction's parameters, a Welsch robust noise model, Levenberg-Marquardt
|
||||
// after a coarse pattern search, with the robust scale going from wide to narrow.
|
||||
class GtsamSolver : public CorrectionSolver
|
||||
{
|
||||
public:
|
||||
GtsamSolver(double sigma) : sigma_(sigma), coarse_(4) {}
|
||||
const char * name() const override {return "gtsam";}
|
||||
void solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const override;
|
||||
|
||||
private:
|
||||
template<int D>
|
||||
void solveWith(const CalibrationProblem & problem, const std::vector<int> & solved, double p[6]) const;
|
||||
|
||||
double sigma_;
|
||||
PatternSearchSolver coarse_; // steps of 2 down to 0.25 deg
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
#endif /* LIDARCAMERACALIBRATION_GTSAMSOLVER_H_ */
|
||||
@@ -1,73 +0,0 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "PatternSearchSolver.h"
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
void PatternSearchSolver::solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const
|
||||
{
|
||||
double steps[6];
|
||||
std::copy(kInitialSteps, kInitialSteps + 6, steps);
|
||||
if(!estimateTranslation)
|
||||
{
|
||||
steps[0] = steps[1] = steps[2] = 0.0;
|
||||
}
|
||||
double best = problem.score(correctionFrom(p));
|
||||
for(int level = 0; level < levels_; ++level)
|
||||
{
|
||||
bool improved = true;
|
||||
while(improved)
|
||||
{
|
||||
improved = false;
|
||||
for(int k = 0; k < 6; ++k)
|
||||
{
|
||||
if(steps[k] == 0.0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for(int sign = -1; sign <= 1; sign += 2)
|
||||
{
|
||||
double q[6];
|
||||
std::copy(p, p + 6, q);
|
||||
q[k] += sign * steps[k];
|
||||
const double s = problem.score(correctionFrom(q));
|
||||
if(s > best + 1e-7)
|
||||
{
|
||||
best = s;
|
||||
std::copy(q, q + 6, p);
|
||||
improved = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
for(double & s : steps)
|
||||
{
|
||||
s /= 2.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,47 +0,0 @@
|
||||
/*
|
||||
Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef LIDARCAMERACALIBRATION_PATTERNSEARCHSOLVER_H_
|
||||
#define LIDARCAMERACALIBRATION_PATTERNSEARCHSOLVER_H_
|
||||
|
||||
#include "CorrectionSolver.h"
|
||||
|
||||
// One parameter at a time, trying a step each way, until no step improves, then with
|
||||
// half the steps. Simple and robust, but can only move along the parameters' axes.
|
||||
class PatternSearchSolver : public CorrectionSolver
|
||||
{
|
||||
public:
|
||||
// levels: how many times the steps are halved, from 2 deg (7: down to about 0.03 deg)
|
||||
PatternSearchSolver(int levels = 7) : levels_(levels) {}
|
||||
const char * name() const override {return "pattern";}
|
||||
void solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const override;
|
||||
|
||||
private:
|
||||
int levels_;
|
||||
};
|
||||
|
||||
#endif /* LIDARCAMERACALIBRATION_PATTERNSEARCHSOLVER_H_ */
|
||||
@@ -1,137 +0,0 @@
|
||||
# rtabmap-lidarCameraCalibration
|
||||
|
||||
Refines the rotation between a camera and a lidar from a mapping session, without a calibration target. It aligns the edges the lidar sees (depth discontinuities, creases and reflectance changes) with the edges in the camera images, over all the nodes of an RTAB-Map database at once.
|
||||
|
||||
```bash
|
||||
rtabmap-lidarCameraCalibration [options] map.db
|
||||
```
|
||||
|
||||
The database is opened read-only. The tool prints a correction to apply to the camera's transform; it does not modify anything.
|
||||
|
||||
## What the database needs
|
||||
|
||||
Each node must have an image from one camera and a lidar scan taken at about the same time, with the camera's and the lidar's local transforms (from TF when mapping with ROS).
|
||||
|
||||
What helps:
|
||||
|
||||
- **Dense scans.** Edges are found where the scan, projected in the image, leaves no holes. An assembled cloud (a few lidar sweeps per node) at full resolution, or with a small voxel (2 cm), works better than a sparse one.
|
||||
- **Images and scans taken together.** The tool assumes that the camera's and the lidar's clocks are synchronized: it does not estimate a time offset between them, which would look like a rotation while the robot turns. With `odom_sensor_sync`, the camera's transform stored in each node already compensates the motion between the image's and the scan's stamps. The tool estimates its correction on top of that.
|
||||
- **Many nodes from varied viewpoints.** The correction is the one that fits all the nodes; a single view constrains it much less.
|
||||
- **Intensity in the scans** (format `XYZI`). It is used when there; see below.
|
||||
|
||||
## How it works
|
||||
|
||||
For every node:
|
||||
|
||||
1. **Image edges.** Canny edges of the image, turned into a score with a distance transform: 1 on an edge, decreasing with the distance to it (`exp(-d / sigma)`, `--sigma`, default 3 pixels). The score is smooth enough for a search to follow it toward the edges.
|
||||
2. **Lidar edges.** The scan is projected in the camera at a coarse resolution (`--decimation`, default 1/4 of the image), where the projected points are dense, keeping the nearest point per cell so that surfaces hidden from the camera do not count. A cell's point is a lidar edge point when:
|
||||
- it is in front of a depth discontinuity: a neighboring cell is at least 15% farther (`--jump`) than where this cell's surface would continue, as at the border of an object in front of a farther background. The continuation is predicted from the opposite neighbor (on a plane, inverse depth is linear in the image), so that a surface seen at a grazing angle, such as the floor ahead of a low camera, is not taken for a discontinuity: its depth changes fast from one cell to the next, but as its continuation predicts; or
|
||||
- it is on an intensity edge: the lidar's intensity changes by more than 40% to a neighboring cell (`--intensity_jump`), as at a change of paint or material. A cell's intensity is the mean (of the logarithm) over all the points of the surface it sees, not its nearest point's: a lidar's beams do not return the same intensity from the same surface, and a node's scan assembles several sweeps, so cells seen by different beams would otherwise differ and make false edges along the beams' traces, even on a flat uniform wall. Then it is median filtered against what speckle remains. As for creases (below), the cells only say where there is an intensity edge: where exactly is found at the image's full resolution. This is what finds edges on flat surfaces, where the depth does not jump: holds on a climbing wall, window frames, panels. `--no_intensity` turns it off; or
|
||||
- it is on a crease: the surface normals of neighboring cells differ by more than 45 degrees (`--crease_angle`, 0 disables), as between a wall and the floor, where the depth does not jump. Normals are computed on the scan voxelized (`--crease_voxel`, 10 cm), smoother than at full resolution; each voxel's normal is spread over the cells it covers, and averaged per cell like intensity. As the normals then change over a band of a few cells across a crease (and intensity across an intensity edge), where exactly it is is found at the image's full resolution, as Canny finds edges: the normals (or intensity) are interpolated and smoothed, and the edge is where their change is the largest across it, to a fraction of a pixel. Its depth is interpolated there (these edges are on continuous surfaces), and the point is put back in 3D, one per cell. Taking the nearest lidar point of each cell instead would put them anywhere in a band a few cells wide on both sides of the edge. They help most where surfaces all reflect alike, with few intensity edges.
|
||||
|
||||
Each point is weighted by the size of its jump. The edge points are kept in 3D, in the scan's frame, so that they can then be projected at full resolution.
|
||||
|
||||
Then a solver finds the correction `C` of the camera's transform that best lines the lidar edge points up with the images' edges, over all the nodes, starting from no correction (`--solver`). The direct solvers maximize a score: the weighted average, over all the lidar edge points, of the image edge score where the point projects at full resolution (interpolated).
|
||||
|
||||
- `simplex` (default): OpenCV's Nelder-Mead simplex (`cv::DownhillSolver`), started again once from its result. It moves all the parameters together, so it can follow directions in which they are coupled (which is likely between translation and rotation).
|
||||
- `pattern`: one parameter at a time, a step each way, until no step improves, then with half the steps (2 degrees down to about 0.03 degree). The fastest, but it moves only along the parameters' axes.
|
||||
|
||||
The least-squares solvers (if RTAB-Map is built with g2o or GTSAM) minimize instead, for each lidar edge point, its distance in pixels to the nearest image edge where it projects (chamfer matching), weighted by the point's weight:
|
||||
|
||||
- `g2o`, `gtsam`: Levenberg-Marquardt, with a Welsch robust kernel, which makes a point far from any image edge count for nothing, as the score does: many lidar edges have no counterpart in the image. Levenberg-Marquardt only follows the local slope, and each point is pulled toward its nearest image edge, often not its own when far from the solution, so on its own it stops in a local minimum a few degrees away. A coarse `pattern` search (2 down to 0.25 degree) gets close first, then the kernel's scale goes from wide to narrow (9, 3, then 1 x `--sigma`). Several times longer than `simplex`, and about 2 GB of memory for 500k lidar edge points.
|
||||
|
||||
The lidar edge points are then selected again with the result, and the solver runs a second time from there.
|
||||
|
||||
### Code
|
||||
|
||||
- `main.cpp`: loading the database, the image edges, the lidar edge points (`selectEdgePoints()`), the passes, the checks and the report.
|
||||
- `CalibrationProblem.h/.cpp`: what a solver sees: the nodes' lidar edge points, where a correction projects them (`project()`), the score (`score()`), and the distance to the image edges for least squares (`edgeDistance()`).
|
||||
- `CorrectionSolver.h/.cpp`: the solvers' interface and `createSolver()`.
|
||||
- `PatternSearchSolver`, `DownhillSimplexSolver`, `G2oSolver`, `GtsamSolver` (`.h/.cpp`): the solvers. The last two compile to nothing when RTAB-Map is built without g2o or GTSAM.
|
||||
|
||||
A new solver implements `CorrectionSolver` and is added to `createSolver()` and `availableSolvers()`.
|
||||
|
||||
This is the approach of Levinson and Thrun (see [Reference](#reference)), with intensity edges added.
|
||||
|
||||
## The result
|
||||
|
||||
The tool prints a correction `X` of the camera's mount, in the camera's body frame (x forward, y left, z up): the robot base to camera transform `B` (e.g., `base_link -> camera_link`) becomes `B * X`. It is a multiplication of transforms, not a sum of angles. Its roll is about the camera's viewing axis, its pitch is the camera's tilt and its yaw its pan. It prints it in radians and degrees, and as a quaternion. In TF, `X` can also be inserted after `B`, leaving the other transforms as they are: `base_link -[B]-> camera_link_measured -[X]-> camera_link`.
|
||||
|
||||
Internally, the lidar is projected in the camera's optical frame (x right, y down, z forward), under the body frame through the optical rotation `R` (roll -pi/2, pitch 0, yaw -pi/2): the solvers search for the same correction there, `C = R^-1 * X * R`, each node's camera local transform `T` becoming `T * C`.
|
||||
|
||||
### Rotation only, by default
|
||||
|
||||
Only the rotation is estimated unless `--translation` is given. The translation between a camera and a lidar is usually a few centimeters, which moves the edges in the images very little when the scene is several meters away: it is not observable, and estimating it anyway gives values that change from one subset of the nodes to another. Measure it instead, or estimate it with `--translation` only on data with close surfaces, and check it as below.
|
||||
|
||||
### Checking it
|
||||
|
||||
The tool prints, for each kind of lidar edge, the share of its points within 2 pixels of an image edge once corrected: how much each brings, and how much of it is noise. Then:
|
||||
|
||||
- **Each half of the nodes on its own.** The even and the odd nodes are calibrated separately. If they agree, the result is supported by the data; if they differ, the difference is about how much the result can be trusted.
|
||||
- **The sensitivity**, with `--verbose`: how much the score drops, in percent, with the result off by 1 degree (roll, pitch, yaw) or 2 cm (x, y, z) along or about each axis of the camera, the mean of both directions. Think of the score as a valley with the result at its bottom: the sensitivity is how steep its sides are along each axis.
|
||||
- A large drop (several percent or more for 1 degree) means the data determines that axis well: a small error on it would misalign many edges, so the solver cannot be far off, and a correction on that axis can be trusted, however large.
|
||||
- Almost none (around 1% or less) means the axis is not observable from this data: the edges hardly move with it, so any value the solver finds on it, large or small, is not reliable.
|
||||
- It says how sure the result is, not how far off the camera was: that is the correction itself. It depends on the scene and the sensors, not on the error: e.g., many vertical and horizontal edges make yaw and pitch steep, roll (about the viewing axis) moves edges little near the image center so it is usually less steep, and translation is flat unless surfaces are close (a 2 cm shift moves edges a fraction of a pixel at several meters).
|
||||
|
||||
With `--images dir`, the image of every node is saved darkened, with the image edges the alignment uses (Canny) in green and the lidar edge points projected over them, depth discontinuities in red, creases in blue and intensity edges in yellow, before (`<id>_edges_1_before.png`, from where the search started: the database's camera transform, with `--initial_rotation` if given) and after (`<id>_edges_2_after.png`) the correction, with the node's id and speeds in the top left corner: the mean since the previous node (over which an assembled scan is taken) and the instantaneous one when the node was added. After, the lidar points should lie on green wherever both sensors see an edge. Points away from any green, and green edges without points, are edges only one of the sensors sees (e.g., lidar intensity through glass, or shadows in the image).
|
||||
|
||||
For example, a node of an indoor climbing gym, before (left) and after (right) the correction: before, the creases along the floor and the climbing holds' outlines are off their green edges; after, they lie on them.
|
||||
|
||||
| Before (`<id>_edges_1_before.png`) | After (`<id>_edges_2_after.png`) |
|
||||
|---|---|
|
||||
|  |  |
|
||||
|
||||
With each of them, two images show the maps the lidar edges are found from, at the decimated resolution, half transparent over the image, with the image's edges in green and the map's own edges (Canny, as for the image) in blue:
|
||||
|
||||
- `<id>_intensity_1_before.png`, `<id>_intensity_2_after.png`: the lidar's intensity per cell, as used (mean log-intensity, median filtered), from red (dark) to yellow (bright), with the contrast stretched for each image. The intensity edges are where it changes; they should line up with green where the image shows the same change of material.
|
||||
- `<id>_normals_1_before.png`, `<id>_normals_2_after.png`: how each cell's surface faces the camera, from yellow (facing it) to red (seen edge on, at a grazing angle). Surfaces at a grazing angle are where depth changes fast without a discontinuity, and where the intensity drops. The normals are computed on the scans voxelized at `--crease_voxel`, so this image is made whether or not creases are used.
|
||||
|
||||
Before (left) and after (right) the correction: the intensity of the same node, where the holds stand out in yellow, and the normals of another, where the climbing wall seen at a grazing angle is red against the walls facing the camera in yellow. Once corrected, the map's edges (blue) lie on the image's (green).
|
||||
|
||||
| Before | After |
|
||||
|---|---|
|
||||
|  |  |
|
||||
|  |  |
|
||||
|
||||
It is also worth running it again with other values of `--decimation`, `--intensity_jump` or `--voxel` (which voxel filters the scans first, to compare densities): a result that does not move with them is more trustworthy than one that does.
|
||||
|
||||
## Options
|
||||
|
||||
| Option | Default | Description |
|
||||
|---|---|---|
|
||||
| `--solver "name"` | `simplex` | `simplex`, `pattern`, `g2o` or `gtsam`, see above. `--help` lists those in this build. |
|
||||
| `--verbose` | off | Also print the sensitivity (see above). |
|
||||
| `--translation` | off | Also estimate the translation (see above). |
|
||||
| `--images "dir"` | | Save the image of every node with its edges (green) and the lidar edge points (depth: red, crease: blue, intensity: yellow), before and after the correction, and the lidar's intensity and surface orientation (see above). |
|
||||
| `--decimation #` | `4` | Image decimation at which the lidar edges are found. Lower is finer, but needs denser scans. |
|
||||
| `--jump #.#` | `0.15` | Relative depth jump for a depth discontinuity, as a fraction of the point's depth: 0.15 means a neighbor at least 15% of its depth farther than where its surface would continue (e.g., 0.6 m behind a point at 4 m). |
|
||||
| `--intensity_jump #.#` | `0.4` | Relative intensity change for an intensity edge, as a fraction: 0.4 means a neighbor at least 1.4 times brighter or darker (compared on log-intensity). Lower finds more edges, and more speckle. |
|
||||
| `--intensity_weight #.#` | `1` | Weight of the intensity edges relative to the depth discontinuities. Lower it where intensity is less reliable than geometry (e.g., much glass). |
|
||||
| `--crease_angle #.#` | `45` | Creases: where the surface normals differ by this angle (deg). 0: off. |
|
||||
| `--crease_voxel #.#` | `0.1` | Voxel size (m) of the scans on which the normals are computed. |
|
||||
| `--no_intensity` | | Use only depth discontinuities. |
|
||||
| `--sigma #.#` | `3` | Fall off, in pixels, of the image edge score. |
|
||||
| `--min_depth #.#` | `0.5` | Ignore lidar points closer than this to the camera (m). |
|
||||
| `--initial_rotation #.# #.# #.#` | `0 0 0` | Start from this correction (roll, pitch, yaw in deg, in the camera's body frame) instead of none. The correction printed stays relative to the database's camera transform (the same as without this option if the result is found again); the one relative to the starting point is printed too: to see from how far off the result is found again, or to start closer when the stored transform is known to be far off. The two halves of the nodes start from it too. |
|
||||
| `--max_angular_speed #.#` | `0` | Skip the nodes rotating faster than this (deg/s): the mean since the previous node, from their odometry poses, over which an assembled scan is taken (else the instantaneous odometry velocity stored in the node). 0: keep all. |
|
||||
| `--max_linear_speed #.#` | `0` | Skip the nodes moving faster than this (m/s). 0: keep all. |
|
||||
| `--voxel #.#` | `0` | Voxel filter the scans first (m), to compare the result at several lidar densities. |
|
||||
|
||||
## Limits
|
||||
|
||||
- **Time offset.** A delay between the camera's and the lidar's clocks looks like a rotation while the robot turns. It is not estimated: use well synchronized sensors, or data where the robot turns slowly (`--max_angular_speed` skips the nodes where it turns fast).
|
||||
- **Motion within a node.** An assembled cloud spans a few sweeps; the points from the earlier ones are moved by odometry, whose error adds noise.
|
||||
- **Rolling shutter.** It is not modeled; fast rotations distort the images.
|
||||
- **Intrinsics.** The camera's calibration (focal lengths, center, distortion) is assumed right; an error there biases the rotation.
|
||||
- **One camera per node.** Nodes with several cameras are skipped.
|
||||
|
||||
## Reference
|
||||
|
||||
J. Levinson and S. Thrun, "Automatic Online Calibration of Cameras and Lasers", *Robotics: Science and Systems IX* (RSS), 2013. [PDF](http://www.roboticsproceedings.org/rss09/p29.pdf)
|
||||
|
||||
What this tool takes from it: lidar points at depth discontinuities as the lidar's edges, the image's edges spread out by a distance transform so that the alignment score is smooth, and that score maximized over many frames at once. What it does differently:
|
||||
|
||||
- The depth discontinuities are found in the scan projected in the camera, at a coarse resolution, rather than between consecutive points of a scan line: the scans of a node can be an assembly of several sweeps, voxel filtered, without scan lines. They are measured against the continuation of the surface, so that surfaces seen at a grazing angle do not make false ones.
|
||||
- Intensity edges and creases are added to the depth discontinuities, for surfaces without depth jumps.
|
||||
- The search is local, from the current transform, with a choice of solvers; the rotation only by default.
|
||||
- The result is checked on two halves of the nodes estimated separately.
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 143 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 136 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 153 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 142 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 166 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 154 KiB |
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user