From 887695298a54ce01e53dfa9396ac958ff5161c5f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 21 Feb 2025 16:31:16 -0800 Subject: [PATCH 1/4] Updated OdomInfo msg with upstream new stats --- rtabmap_conversions/src/MsgConversion.cpp | 9 +++++++++ rtabmap_msgs/msg/OdomInfo.msg | 2 ++ 2 files changed, 11 insertions(+) diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index b41c7ae5..aacdcc4f 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -1598,6 +1598,11 @@ std::map odomInfoToStatistics(const rtabmap::OdometryInfo & stats.insert(std::make_pair("Odometry/LocalBundleOutliers/", info.localBundleOutliers)); stats.insert(std::make_pair("Odometry/LocalBundleConstraints/", info.localBundleConstraints)); stats.insert(std::make_pair("Odometry/LocalBundleTime/ms", info.localBundleTime*1000.0f)); + stats.insert(std::make_pair("Odometry/localBundleAvgInlierDistance/pix", info.localBundleAvgInlierDistance)); + stats.insert(std::make_pair("Odometry/localBundleMaxKeyFramesForInlier/", info.localBundleMaxKeyFramesForInlier)); + + float32 localBundleAvgInlierDistance +int32 localBundleMaxKeyFramesForInlier stats.insert(std::make_pair("Odometry/KeyFrameAdded/", info.keyFrameAdded?1.0f:0.0f)); stats.insert(std::make_pair("Odometry/Interval/ms", (float)info.interval)); stats.insert(std::make_pair("Odometry/Distance/m", info.distanceTravelled)); @@ -1674,6 +1679,8 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_msgs::OdomInfo & msg, bool i info.localBundleOutliers = msg.localBundleOutliers; info.localBundleConstraints = msg.localBundleConstraints; info.localBundleTime = msg.localBundleTime; + info.localBundleAvgInlierDistance = msg.localBundleAvgInlierDistance; + info.localBundleMaxKeyFramesForInlier = msg.localBundleMaxKeyFramesForInlier; UASSERT(msg.localBundleModels.size() == msg.localBundleIds.size()); UASSERT(msg.localBundleModels.size() == msg.localBundlePoses.size()); for(size_t i=0; i >::const_iterator iter=info.localBundleModels.begin(); iter!=info.localBundleModels.end(); diff --git a/rtabmap_msgs/msg/OdomInfo.msg b/rtabmap_msgs/msg/OdomInfo.msg index 473b4314..094cf3e1 100644 --- a/rtabmap_msgs/msg/OdomInfo.msg +++ b/rtabmap_msgs/msg/OdomInfo.msg @@ -18,6 +18,8 @@ int32 localKeyFrames int32 localBundleOutliers int32 localBundleConstraints float32 localBundleTime +float32 localBundleAvgInlierDistance +int32 localBundleMaxKeyFramesForInlier bool keyFrameAdded float32 timeEstimation float32 timeParticleFiltering From a57b2971f2079d375f867bdb6927efefd75ddd38 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 21 Feb 2025 16:41:24 -0800 Subject: [PATCH 2/4] fixed last commit --- rtabmap_conversions/src/MsgConversion.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index aacdcc4f..e8cb1ee0 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -1600,9 +1600,6 @@ std::map odomInfoToStatistics(const rtabmap::OdometryInfo & stats.insert(std::make_pair("Odometry/LocalBundleTime/ms", info.localBundleTime*1000.0f)); stats.insert(std::make_pair("Odometry/localBundleAvgInlierDistance/pix", info.localBundleAvgInlierDistance)); stats.insert(std::make_pair("Odometry/localBundleMaxKeyFramesForInlier/", info.localBundleMaxKeyFramesForInlier)); - - float32 localBundleAvgInlierDistance -int32 localBundleMaxKeyFramesForInlier stats.insert(std::make_pair("Odometry/KeyFrameAdded/", info.keyFrameAdded?1.0f:0.0f)); stats.insert(std::make_pair("Odometry/Interval/ms", (float)info.interval)); stats.insert(std::make_pair("Odometry/Distance/m", info.distanceTravelled)); From c4a596f8008428717ce5c45e48a5798e4e079235 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 1 Mar 2025 15:50:39 -0800 Subject: [PATCH 3/4] Support any frame_id in initialpose #1273 --- rtabmap_slam/src/CoreWrapper.cpp | 37 +++++++++++++++++++++++++++----- 1 file changed, 32 insertions(+), 5 deletions(-) diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 08b56b4b..a26a58e3 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -2526,14 +2526,41 @@ void CoreWrapper::interOdomInfoCallback(const nav_msgs::OdometryConstPtr & msg1, void CoreWrapper::initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg) { - Transform intialPose = rtabmap_conversions::transformFromPoseMsg(msg->pose.pose); - if(intialPose.isNull()) + Transform mapToPose = Transform::getIdentity(); + if(msg->header.frame_id.empty()) { - NODELET_ERROR("Pose received is null!"); - return; + NODELET_WARN("Received initialpose doesn't have frame_id set, assuming it is in %s frame.", mapFrameId_.c_str()); + } + else if(msg->header.frame_id != mapFrameId_) + { + mapToPose = rtabmap_conversions::getTransform(mapFrameId_, msg->header.frame_id, msg->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0); + if(mapToPose.isNull()) + { + NODELET_ERROR("Failed to transform initialpose from frame %s to map frame %s", msg->header.frame_id.c_str(), mapFrameId_.c_str()); + return; + } } - rtabmap_.setInitialPose(intialPose); + Transform initialPose = rtabmap_conversions::transformFromPoseMsg(msg->pose.pose); + if(initialPose.isNull()) + { + NODELET_ERROR("initialpose received is null!"); + return; + } + if(mapToPose.isIdentity()) + { + NODELET_INFO("initialpose received: %s", initialPose.prettyPrint().c_str()); + rtabmap_.setInitialPose(initialPose); + } + else + { + NODELET_INFO("initialpose received: %s in %s frame, transformed to %s in %s frame.", + initialPose.prettyPrint().c_str(), + msg->header.frame_id.c_str(), + (mapToPose * initialPose).prettyPrint().c_str(), + mapFrameId_.c_str()); + rtabmap_.setInitialPose(mapToPose*initialPose); + } } void CoreWrapper::goalCommonCallback( From 12e61a952e1ee977885009c006bd71dfb752857a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 1 Mar 2025 15:53:54 -0800 Subject: [PATCH 4/4] bump required min rtabmap version --- rtabmap_conversions/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rtabmap_conversions/CMakeLists.txt b/rtabmap_conversions/CMakeLists.txt index 166b0abb..b722d77d 100644 --- a/rtabmap_conversions/CMakeLists.txt +++ b/rtabmap_conversions/CMakeLists.txt @@ -7,7 +7,7 @@ find_package(catkin REQUIRED COMPONENTS image_geometry rtabmap_msgs ) -find_package(RTABMap 0.21.5 REQUIRED) +find_package(RTABMap 0.21.11 REQUIRED) catkin_package( INCLUDE_DIRS include