i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-09 19:19:49 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
91856ca2dd3e1514f1751a74dd63562e9c192031
rtabmap_ros/rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp
T
matlabbeandmathieu86 11edc01d6a rtabmap_odom tests and doc (#1456) 
* rtabmap_odom tests and doc

* opengv note

* added ci checks or humble-latest flaky dep cmake errors

* Added real data tests for rgbd_odom and stereo_odom

* added real data for icp_odometry's deskewing test

* fixing json cmake error on lyrical/rolling

* test 2d icp odom deskewing branch

* first review of existing OdometryROS tests

* testing with imu used as guess

* tested imu arrivals sync

* Fixed odom reset on right pose when guess frame id is used

* fixing header errors in ci >=lyrical

* Added support for input rgbd_image topic with features for odom, added multicam rgbd_odometry test

* Added stereo odom support for features-only frames. Added multicam stereo tests.

* forcing latest rtabmap version

* updated OdometryROS API

* ci: dont build non-latest docker in pull requests

* splitting docker jobs

* doc edit

* Making publish_null_when_lost:=false continous when guess is provided (using guess covariance when we cannot register yet)

* updated stereo doc

* ficing rolling ci (rviz Ogre header)

* Added test coverage of alll rgbd_image callbacks

* fixing rolling ci

* making docker ci build/run the tests on pull requests

* fixing ros2 ci testing

* improved sync callback coverage

* improving stereo_odometry test coverage

* improved icp_odometry test coverage

* lyrical voxel_grid ptr error

* make multicam tests working as well without opengv

* removing deps of missing packages on rolling

* PCL empty cloud  conversion compiler errors fix

* fixing icp_odometry test failure on ci witohut libpointmatcher

* fixing nav2 costmap plugin build on lyrical

* joining thread when exiting

* updating icp test to work the same on pcl 1.15 (lyrical)

* Fix parallel tests seg fault

---------

Co-authored-by: mathieu86 <[email protected]>
2026-09-21 17:02:45 -07:00

181 lines
9.4 KiB
C++
Raw Blame History

/*
Copyright (c) 2010-2016, 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 "rclcpp/rclcpp.hpp"
#include <rtabmap_odom/OdometryROS.h>
#include <rtabmap_odom/visibility.h>
#include <message_filters/subscriber.hpp>
#include <message_filters/time_synchronizer.hpp>
#include <message_filters/sync_policies/approximate_time.hpp>
#include <image_transport/image_transport.hpp>
#include <image_transport/subscriber_filter.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <rtabmap_msgs/msg/key_point.hpp>
#include <rtabmap_msgs/msg/point3f.hpp>
#include <rtabmap_msgs/msg/rgbd_image.hpp>
#include <rtabmap_msgs/msg/rgbd_images.hpp>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#endif
namespace rtabmap_odom
{
/**
* @brief Odometry from an RGB-D camera, or from several on one rig.
*
* Takes either the three raw topics of a camera (`rgb/image`, `depth/image`,
* `rgb/camera_info`) or pre-synchronized rtabmap_msgs::msg::RGBDImage messages, one per
* camera, and registers each frame's visual features against a local feature map. A
* frame that arrives with its own keypoints, 3D points and descriptors is registered
* with those rather than having them extracted again.
*
* @see doc/rgbd_odometry.md for the topics, the parameters and what to do when it loses
* tracking.
*/
class RGBDOdometry : public rtabmap_odom::OdometryROS
{
public:
RTABMAP_ODOM_PUBLIC
explicit RGBDOdometry(const rclcpp::NodeOptions & options);
virtual ~RGBDOdometry();
private:
virtual void updateParameters(rtabmap::ParametersMap & parameters);
virtual void onOdomInit();
/**
* Local features, when the input topic carries them, are indexed per camera like
* the images are: one entry per camera, in the same order. They are optional, and
* a frame that comes without them is processed exactly as before, the features
* being extracted from the images downstream.
*/
void commonCallback(
const std::vector<cv_bridge::CvImageConstPtr> & rgbImages,
const std::vector<cv_bridge::CvImageConstPtr> & depthImages,
const std::vector<sensor_msgs::msg::CameraInfo>& cameraInfos,
const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPointsMsgs = {},
const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3dMsgs = {},
const std::vector<cv::Mat> & localDescriptorsMsgs = {});
void callback(
const sensor_msgs::msg::Image::ConstSharedPtr image,
const sensor_msgs::msg::Image::ConstSharedPtr depth,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo);
void callbackRGBDX(
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images);
void callbackRGBD(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image);
void callbackRGBD2(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2);
void callbackRGBD3(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3);
void callbackRGBD4(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4);
void callbackRGBD5(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5);
void callbackRGBD6(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6);
protected:
virtual void flushCallbacks();
private:
image_transport::SubscriberFilter image_mono_sub_;
image_transport::SubscriberFilter image_depth_sub_;
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> info_sub_;
rclcpp::Subscription<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdSub_;
rclcpp::Subscription<rtabmap_msgs::msg::RGBDImages>::SharedPtr rgbdxSub_;
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image1_sub_;
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image2_sub_;
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image3_sub_;
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image4_sub_;
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image5_sub_;
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image6_sub_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo> MyApproxSyncPolicy;
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo> MyExactSyncPolicy;
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync2Policy;
message_filters::Synchronizer<MyApproxSync2Policy> * approxSync2_;
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync2Policy;
message_filters::Synchronizer<MyExactSync2Policy> * exactSync2_;
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync3Policy;
message_filters::Synchronizer<MyApproxSync3Policy> * approxSync3_;
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync3Policy;
message_filters::Synchronizer<MyExactSync3Policy> * exactSync3_;
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync4Policy;
message_filters::Synchronizer<MyApproxSync4Policy> * approxSync4_;
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync4Policy;
message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_;
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync5Policy;
message_filters::Synchronizer<MyApproxSync5Policy> * approxSync5_;
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync5Policy;
message_filters::Synchronizer<MyExactSync5Policy> * exactSync5_;
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync6Policy;
message_filters::Synchronizer<MyApproxSync6Policy> * approxSync6_;
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync6Policy;
message_filters::Synchronizer<MyExactSync6Policy> * exactSync6_;
int topicQueueSize_;
int syncQueueSize_;
bool keepColor_;
double approxSyncMaxInterval_;
};
}
Reference in New Issue View Git Blame Copy Permalink