From ec7f14e04fbabcefe3492451e55af45c24fda2dc Mon Sep 17 00:00:00 2001 From: shayaf84 Date: Fri, 17 Apr 2026 22:42:46 +0000 Subject: [PATCH 1/9] SLAM backend hpp with gtsam --- .../include/gps_imu_localizer.hpp | 44 ------------ urc_localization/src/gps_imu_localizer.cpp | 71 ------------------- urc_slam/include/slam_backend.hpp | 70 ++++++++++++++++++ urc_slam/src/slam_backend.cpp | 0 4 files changed, 70 insertions(+), 115 deletions(-) delete mode 100644 urc_localization/include/gps_imu_localizer.hpp delete mode 100644 urc_localization/src/gps_imu_localizer.cpp create mode 100644 urc_slam/include/slam_backend.hpp create mode 100644 urc_slam/src/slam_backend.cpp diff --git a/urc_localization/include/gps_imu_localizer.hpp b/urc_localization/include/gps_imu_localizer.hpp deleted file mode 100644 index e0060463..00000000 --- a/urc_localization/include/gps_imu_localizer.hpp +++ /dev/null @@ -1,44 +0,0 @@ -#ifndef GPS_IMU_LOCALIZER_H -#define GPS_IMU_LOCALIZER_H - -#include "nav_msgs/msg/odometry.hpp" -#include "sensor_msgs/msg/imu.hpp" -#include "sensor_msgs/msg/nav_sat_fix.hpp" -#include "geographic_msgs/msg/geo_point.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "tf2_ros/transform_broadcaster.h" -#include "math.h" -#include -#include -#include -#include -#include -#include -#include -#include - -namespace gps_imu_localizer -{ -class GpsImuLocalizer : public rclcpp::Node -{ -public: - explicit GpsImuLocalizer(const rclcpp::NodeOptions & options); - -private: - rclcpp::Subscription::SharedPtr gps_subscriber_; - rclcpp::Subscription::SharedPtr imu_subscriber_; - rclcpp::Subscription::SharedPtr set_base_subscriber_; - std::shared_ptr> - odometry_publisher_; - - void GpsCallback(const sensor_msgs::msg::NavSatFix & msg); - void ImuCallback(const sensor_msgs::msg::Imu & msg); - - std::pair base; - nav_msgs::msg::Odometry odometry_msg_; - - std::unique_ptr br; -}; -} // namespace gps_imu_localizer - -#endif diff --git a/urc_localization/src/gps_imu_localizer.cpp b/urc_localization/src/gps_imu_localizer.cpp deleted file mode 100644 index 2275978b..00000000 --- a/urc_localization/src/gps_imu_localizer.cpp +++ /dev/null @@ -1,71 +0,0 @@ -#include "gps_imu_localizer.hpp" - -namespace gps_imu_localizer -{ - -GpsImuLocalizer::GpsImuLocalizer(const rclcpp::NodeOptions & options) -: rclcpp::Node("gps_imu_localizer", options) -{ - declare_parameter("gps_topic", "/gps/data"); - declare_parameter("imu_topic", "/imu/data"); - declare_parameter("set_base_topic", "/set_base"); - declare_parameter("odometry_topic", "/odometry/filtered_global"); - - odometry_msg_.header.frame_id = "map"; - odometry_msg_.child_frame_id = "base_link"; - - gps_subscriber_ = create_subscription( - get_parameter("gps_topic").as_string(), rclcpp::SystemDefaultsQoS(), - [this](const sensor_msgs::msg::NavSatFix msg) {GpsCallback(msg);}); - - imu_subscriber_ = create_subscription( - get_parameter("imu_topic").as_string(), rclcpp::SystemDefaultsQoS(), - [this](const sensor_msgs::msg::Imu msg) {ImuCallback(msg);}); - - set_base_subscriber_ = create_subscription( - get_parameter("set_base_topic").as_string(), rclcpp::SystemDefaultsQoS(), - [this](const std_msgs::msg::Empty::SharedPtr msg) { - base.first = odometry_msg_.pose.pose.position.x; - base.second = odometry_msg_.pose.pose.position.y; - } - ); - - odometry_publisher_ = create_publisher( - get_parameter("odometry_topic").as_string(), rclcpp::SystemDefaultsQoS()); - - br = std::make_unique(*this); -} - -void GpsImuLocalizer::GpsCallback(const sensor_msgs::msg::NavSatFix & msg) -{ - geographic_msgs::msg::GeoPoint geo_point; - geo_point.latitude = msg.latitude; - geo_point.longitude = msg.longitude; - geo_point.altitude = msg.altitude; - geodesy::UTMPoint utm_point; - geodesy::fromMsg(geo_point, utm_point); - odometry_msg_.pose.pose.position.x = utm_point.easting - base.first; - odometry_msg_.pose.pose.position.y = utm_point.northing - base.second; -} - -void GpsImuLocalizer::ImuCallback(const sensor_msgs::msg::Imu & msg) -{ - odometry_msg_.header.stamp = msg.header.stamp; - odometry_msg_.pose.pose.orientation = msg.orientation; - - odometry_publisher_->publish(odometry_msg_); - - geometry_msgs::msg::TransformStamped t; - t.header.stamp = msg.header.stamp; - t.header.frame_id = "map"; - t.child_frame_id = "base_link"; - t.transform.translation.x = odometry_msg_.pose.pose.position.x; - t.transform.translation.y = odometry_msg_.pose.pose.position.y; - t.transform.translation.z = 0.0; - t.transform.rotation = odometry_msg_.pose.pose.orientation; - br->sendTransform(t); -} - -} // namespace gps_imu_localizer - -RCLCPP_COMPONENTS_REGISTER_NODE(gps_imu_localizer::GpsImuLocalizer) diff --git a/urc_slam/include/slam_backend.hpp b/urc_slam/include/slam_backend.hpp new file mode 100644 index 00000000..1f1b5f3a --- /dev/null +++ b/urc_slam/include/slam_backend.hpp @@ -0,0 +1,70 @@ +#ifndef SLAM_BACKEND_HPP_ +#define SLAM_BACKEND_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include + + +namespace urc_slam { + + class SlamBackend { + public: + SlamBackend(double keyframe_translation_threshold_m, + double keyframe_rotation_threshold_rad, + const gtsam::Vector6 &prior_sigmas, + const gtsam::Vector6 &odom_sigmas); + + void initialize(const gtsam::Pose3 &initial_pose); + + bool shouldCreateKeyframe(const gtsam::Pose3 &measured_pose) const; + + bool addKeyframe(const gtsam::Pose3 &measured_pose); + + gtsam::Pose3 latestEstimate() const; + std::vector trajectory() const; + + private: + gtsam::Symbol poseKey(std::size_t index) const; + + double translationDistance( + const gtsam::Pose3 &a, + const gtsam::Pose3 &b + ) const; + + double rotationDistance( + const gtsam::Pose3 &a, + const gtsam::Pose3 &b + ) const; + + gtsam::ISAM2 isam; + gtsam::NonlinearFactorGraph new_factors; + gtsam::Values new_values; + gtsam::Values estimate; + + gtsam::SharedNoiseModel prior_noise; + gtsam::SharedNoiseModel odom_noise; + + std::size_t latest_index = 0; + + double keyframe_translation_threshold_m; + double keyframe_rotation_threshold_rad; + + gtsam::Pose3 last_measured_pose; + gtsam::Pose3 last_estimated_pose; + std::vector trajectory; + + }; + + +} + + + +#endif \ No newline at end of file diff --git a/urc_slam/src/slam_backend.cpp b/urc_slam/src/slam_backend.cpp new file mode 100644 index 00000000..e69de29b From 24b4651fd4e087c488e808325135d90ceca2429f Mon Sep 17 00:00:00 2001 From: shayaf84 Date: Sun, 19 Apr 2026 21:59:19 +0000 Subject: [PATCH 2/9] cpp file for iSAM backend --- urc_slam/src/slam_backend.cpp | 109 ++++++++++++++++++++++++++++++++++ 1 file changed, 109 insertions(+) diff --git a/urc_slam/src/slam_backend.cpp b/urc_slam/src/slam_backend.cpp index e69de29b..b14f4502 100644 --- a/urc_slam/src/slam_backend.cpp +++ b/urc_slam/src/slam_backend.cpp @@ -0,0 +1,109 @@ +#include "slam_backend.hpp" + +#include +#include + +#include +#include +#include +#include + +namespace urc_slam { + + SlamBackend::SlamBackend( + double keyframe_translation_threshold_m, + double keyframe_rotation_threshold_rad, + const gtsam::Vector6 &prior_sigmas, + const gtsam::Vector6 &odom_sigmas) + : isam(gtsam::ISAM2(gtsam::ISAM2Params())), + prior_noise(gtsam::noiseModel::Diagonal::Sigmas(prior_sigmas)), + odom_noise(gtsam::noiseModel::Diagonal::Sigmas(odom_sigmas)), + latest_index(0), + keyframe_translation_threshold_m(keyframe_translation_threshold_m), + keyframe_rotation_threshold_rad(keyframe_rotation_threshold_rad) + { + + } + + void SlamBackend::initialize(const gtsam::Pose3 &initial_pose) { + new_factors.add( + gtsam::PriorFactor(poseKey(0), initial_pose, prior_noise) + ); + new_values.insert(poseKey(0), initial_pose); + + isam.update(new_factors, new_values); + estimate = isam.calculateEstimate(); + + new_factors.resize(0); + new_values.clear(); + + latest_index = 0; + last_measured_pose = initial_pose; + last_estimated_pose = estimate.at(poseKey(0)); + + trajectory.clear(); + trajectory.push_back(last_estimated_pose); + } + + bool SlamBackend::shouldCreateKeyframe(const gtsam::Pose3 &measured_pose) const { + return translationDistance(last_measured_pose, measured_pose) >= keyframe_translation_threshold_m + || rotationDistance(last_measured_pose, measured_pose) >= keyframe_rotation_threshold_rad; + } + + bool SlamBackend::addKeyframe(const gtsam::Pose3 &measured_pose) { + const std::size_t new_index = latest_index + 1; + const gtsam::Pose3 delta = last_measured_pose.between(measured_pose); + const gtsam::Pose3 predicted_pose = last_estimated_pose.compose(delta); + + new_factors.add( + gtsam::BetweenFactor( + poseKey(latest_index), + poseKey(new_index), + delta, + odom_noise + ) + ); + + new_values.insert(poseKey(new_index), predicted_pose); + + isam.update(new_factors, new_values); + estimate = isam.calculateEstimate(); + + new_factors.resize(0); + new_values.clear(); + + latest_index = new_index; + last_measured_pose = measured_pose; + // Figure ts syntax out later + last_estimated_pose = estimate.at(poseKey(latest_index)); + trajectory.push_back(last_estimated_pose); + return true; + } + + gtsam::Pose3 SlamBackend::latestEstimate() const { + return estimate.at(poseKey(latest_index)); + } + + std::vector SlamBackend::trajectory() const { + return trajectory; + } + + gtsam::Symbol SlamBackend::poseKey(std::size_t index) const { + return gtsam::Symbol('x', index); + } + + double SlamBackend::translationDistance( + const gtsam::Pose3 &a, + const gtsam::Pose3 &b + ) const { + return a.translation().distance(b.translation()); + } + + double SlamBackend::rotationDistance( + const gtsam::Pose3 &a, + const gtsam::Pose3 &b + ) const { + return a.rotation().between(b.rotation()).rpy().norm(); + } + +} // namespace urc_slam \ No newline at end of file From b9e1d3f14177ddc3b5471e101f926c66f6ce5844 Mon Sep 17 00:00:00 2001 From: shayaf84 Date: Sun, 19 Apr 2026 22:41:12 +0000 Subject: [PATCH 3/9] support lidar in backend and ros2 node header --- urc_slam/include/slam_backend.hpp | 19 +++++----- urc_slam/include/slam_node.hpp | 58 +++++++++++++++++++++++++++++++ urc_slam/src/slam_backend.cpp | 38 ++++++++++++++------ 3 files changed, 96 insertions(+), 19 deletions(-) create mode 100644 urc_slam/include/slam_node.hpp diff --git a/urc_slam/include/slam_backend.hpp b/urc_slam/include/slam_backend.hpp index 1f1b5f3a..532e6524 100644 --- a/urc_slam/include/slam_backend.hpp +++ b/urc_slam/include/slam_backend.hpp @@ -2,8 +2,6 @@ #define SLAM_BACKEND_HPP_ #include -#include - #include #include #include @@ -19,17 +17,22 @@ namespace urc_slam { SlamBackend(double keyframe_translation_threshold_m, double keyframe_rotation_threshold_rad, const gtsam::Vector6 &prior_sigmas, - const gtsam::Vector6 &odom_sigmas); + const gtsam::Vector6 &odom_sigmas, + const gtsam::Vector6 &lidar_sigmas); void initialize(const gtsam::Pose3 &initial_pose); bool shouldCreateKeyframe(const gtsam::Pose3 &measured_pose) const; - bool addKeyframe(const gtsam::Pose3 &measured_pose); + void addKeyframe(const gtsam::Pose3 &measured_pose); + + void addLidarFactor( + std::size_t from_index, + std::size_t to_index, + const gtsam::Pose3 &relative_pose + ); gtsam::Pose3 latestEstimate() const; - std::vector trajectory() const; - private: gtsam::Symbol poseKey(std::size_t index) const; @@ -50,6 +53,7 @@ namespace urc_slam { gtsam::SharedNoiseModel prior_noise; gtsam::SharedNoiseModel odom_noise; + gtsam::SharedNoiseModel lidar_noise; std::size_t latest_index = 0; @@ -58,7 +62,6 @@ namespace urc_slam { gtsam::Pose3 last_measured_pose; gtsam::Pose3 last_estimated_pose; - std::vector trajectory; }; @@ -67,4 +70,4 @@ namespace urc_slam { -#endif \ No newline at end of file +#endif diff --git a/urc_slam/include/slam_node.hpp b/urc_slam/include/slam_node.hpp new file mode 100644 index 00000000..ed2e9d6d --- /dev/null +++ b/urc_slam/include/slam_node.hpp @@ -0,0 +1,58 @@ +#ifndef SLAM_NODE_HPP_ +#define SLAM_NODE_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "slam_backend.hpp" + + +namespace urc_slam { + class SlamNode: public rclcpp::Node { + public: + explicit SlamNode( + const rclcpp::NodeOptions &options = rclcpp::NodeOptions() + ); + + private: + void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg); + void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg); + void lidarCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg); + + gtsam::Pose3 poseFromOdom(const nav_msgs::msg::Odometry &msg) const; + void publishOutputs(const rclcpp::Time &stamp); + + SlamBackend backend; + + rclcpp::Subscription::SharedPtr odom_sub; + rclcpp::Subscription::SharedPtr imu_sub; + rclcpp::Subscription::SharedPtr lidar_sub; + + rclcpp::Publisher::SharedPtr slam_odom_pub; + rclcpp::Publisher::SharedPtr path_pub; + + std::unique_ptr tf_broadcaster; + + nav_msgs::msg::Path path_msg; + sensor_msgs::msg::Imu last_imu_msg; + sensor_msgs::msg::PointCloud2::SharedPtr last_lidar_msg; + + std::string odom_topic; + std::string imu_topic; + std::string lidar_topic; + std::string map_frame; + std::string odom_frame; + std::string base_link_frame; + + }; + +} +#endif \ No newline at end of file diff --git a/urc_slam/src/slam_backend.cpp b/urc_slam/src/slam_backend.cpp index b14f4502..1a7d6f6a 100644 --- a/urc_slam/src/slam_backend.cpp +++ b/urc_slam/src/slam_backend.cpp @@ -14,10 +14,12 @@ namespace urc_slam { double keyframe_translation_threshold_m, double keyframe_rotation_threshold_rad, const gtsam::Vector6 &prior_sigmas, - const gtsam::Vector6 &odom_sigmas) + const gtsam::Vector6 &odom_sigmas, + const gtsam::Vector6 &lidar_sigmas) : isam(gtsam::ISAM2(gtsam::ISAM2Params())), prior_noise(gtsam::noiseModel::Diagonal::Sigmas(prior_sigmas)), odom_noise(gtsam::noiseModel::Diagonal::Sigmas(odom_sigmas)), + lidar_noise(gtsam::noiseModel::Diagonal::Sigmas(lidar_sigmas)), latest_index(0), keyframe_translation_threshold_m(keyframe_translation_threshold_m), keyframe_rotation_threshold_rad(keyframe_rotation_threshold_rad) @@ -41,8 +43,6 @@ namespace urc_slam { last_measured_pose = initial_pose; last_estimated_pose = estimate.at(poseKey(0)); - trajectory.clear(); - trajectory.push_back(last_estimated_pose); } bool SlamBackend::shouldCreateKeyframe(const gtsam::Pose3 &measured_pose) const { @@ -50,7 +50,7 @@ namespace urc_slam { || rotationDistance(last_measured_pose, measured_pose) >= keyframe_rotation_threshold_rad; } - bool SlamBackend::addKeyframe(const gtsam::Pose3 &measured_pose) { + void SlamBackend::addKeyframe(const gtsam::Pose3 &measured_pose) { const std::size_t new_index = latest_index + 1; const gtsam::Pose3 delta = last_measured_pose.between(measured_pose); const gtsam::Pose3 predicted_pose = last_estimated_pose.compose(delta); @@ -76,16 +76,32 @@ namespace urc_slam { last_measured_pose = measured_pose; // Figure ts syntax out later last_estimated_pose = estimate.at(poseKey(latest_index)); - trajectory.push_back(last_estimated_pose); - return true; } - gtsam::Pose3 SlamBackend::latestEstimate() const { - return estimate.at(poseKey(latest_index)); + void SlamBackend::addLidarFactor( + std::size_t from_index, + std::size_t to_index, + const gtsam::Pose3 &relative_pose + ) { + new_factors.add( + gtsam::BetweenFactor( + poseKey(from_index), + poseKey(to_index), + relative_pose, + lidar_noise + ) + ); + isam.update(new_factors, new_values); + estimate = isam.calculateEstimate(); + + new_factors.resize(0); + new_values.clear(); + + last_estimated_pose = estimate.at(poseKey(latest_index)); } - std::vector SlamBackend::trajectory() const { - return trajectory; + gtsam::Pose3 SlamBackend::latestEstimate() const { + return estimate.at(poseKey(latest_index)); } gtsam::Symbol SlamBackend::poseKey(std::size_t index) const { @@ -106,4 +122,4 @@ namespace urc_slam { return a.rotation().between(b.rotation()).rpy().norm(); } -} // namespace urc_slam \ No newline at end of file +} // namespace urc_slam From afa3f7d320c0e05911a95b5deab5823af8eae959 Mon Sep 17 00:00:00 2001 From: shayaf84 Date: Fri, 24 Apr 2026 21:58:50 +0000 Subject: [PATCH 4/9] Add imu preintegration to factor graph --- urc_slam/include/slam_backend.hpp | 26 +++++++++- urc_slam/src/slam_backend.cpp | 83 ++++++++++++++++++++++++++++--- 2 files changed, 101 insertions(+), 8 deletions(-) diff --git a/urc_slam/include/slam_backend.hpp b/urc_slam/include/slam_backend.hpp index 532e6524..e525d378 100644 --- a/urc_slam/include/slam_backend.hpp +++ b/urc_slam/include/slam_backend.hpp @@ -1,6 +1,7 @@ #ifndef SLAM_BACKEND_HPP_ #define SLAM_BACKEND_HPP_ +#include #include #include #include @@ -8,6 +9,10 @@ #include #include #include +#include +#include +#include +#include namespace urc_slam { @@ -32,9 +37,17 @@ namespace urc_slam { const gtsam::Pose3 &relative_pose ); + void integrateImuMeasurement( + const gtsam::Vector3 &measured_acc, + const gtsam::Vector3 &measured_omega, + double dt + ); + gtsam::Pose3 latestEstimate() const; private: gtsam::Symbol poseKey(std::size_t index) const; + gtsam::Symbol velocityKey(std::size_t index) const; + gtsam::Symbol biasKey(std::size_t index) const; double translationDistance( const gtsam::Pose3 &a, @@ -51,9 +64,20 @@ namespace urc_slam { gtsam::Values new_values; gtsam::Values estimate; + gtsam::SharedNoiseModel prior_noise; gtsam::SharedNoiseModel odom_noise; gtsam::SharedNoiseModel lidar_noise; + gtsam::SharedNoiseModel velocity_prior_noise; + gtsam::SharedNoiseModel bias_prior_noise; + + std::shared_ptr imu_params; + std::unique_ptr imu_preintegrator; + + + gtsam::imuBias::ConstantBias last_estimated_bias; + + std::size_t latest_index = 0; @@ -61,7 +85,7 @@ namespace urc_slam { double keyframe_rotation_threshold_rad; gtsam::Pose3 last_measured_pose; - gtsam::Pose3 last_estimated_pose; + gtsam::NavState last_estimated_navstate; }; diff --git a/urc_slam/src/slam_backend.cpp b/urc_slam/src/slam_backend.cpp index 1a7d6f6a..b1bfd6c2 100644 --- a/urc_slam/src/slam_backend.cpp +++ b/urc_slam/src/slam_backend.cpp @@ -4,9 +4,13 @@ #include #include +#include #include +#include #include #include +#include +#include namespace urc_slam { @@ -20,18 +24,37 @@ namespace urc_slam { prior_noise(gtsam::noiseModel::Diagonal::Sigmas(prior_sigmas)), odom_noise(gtsam::noiseModel::Diagonal::Sigmas(odom_sigmas)), lidar_noise(gtsam::noiseModel::Diagonal::Sigmas(lidar_sigmas)), + velocity_prior_noise(gtsam::noiseModel::Isotropic::Sigma(3, 1.0)), + bias_prior_noise(gtsam::noiseModel::Isotropic::Sigma(6, 1e-3)), latest_index(0), keyframe_translation_threshold_m(keyframe_translation_threshold_m), keyframe_rotation_threshold_rad(keyframe_rotation_threshold_rad) { - + imu_params = gtsam::PreintegrationCombinedParams::MakeSharedU(9.81); + imu_params->accelerometerCovariance = gtsam::I_3x3 * std::pow(0.1, 2); + imu_params->gyroscopeCovariance = gtsam::I_3x3 * std::pow(0.01, 2); + imu_params->integrationCovariance = gtsam::I_3x3 * 1e-8; + imu_params->biasAccCovariance = gtsam::I_3x3 * std::pow(0.0001, 2); + imu_params->biasOmegaCovariance = gtsam::I_3x3 * std::pow(0.0001, 2); + imu_params->biasAccOmegaInt = gtsam::I_6x6 * 1e-5; } void SlamBackend::initialize(const gtsam::Pose3 &initial_pose) { + const gtsam::Vector3 initial_velocity(0.0, 0.0, 0.0); + const gtsam::imuBias::ConstantBias initial_bias; + new_factors.add( gtsam::PriorFactor(poseKey(0), initial_pose, prior_noise) ); + new_factors.add( + gtsam::PriorFactor(velocityKey(0), initial_velocity, velocity_prior_noise) + ); + new_factors.add( + gtsam::PriorFactor(biasKey(0), initial_bias, bias_prior_noise) + ); new_values.insert(poseKey(0), initial_pose); + new_values.insert(velocityKey(0), initial_velocity); + new_values.insert(biasKey(0), initial_bias); isam.update(new_factors, new_values); estimate = isam.calculateEstimate(); @@ -41,7 +64,14 @@ namespace urc_slam { latest_index = 0; last_measured_pose = initial_pose; - last_estimated_pose = estimate.at(poseKey(0)); + last_estimated_navstate = gtsam::NavState( + estimate.at(poseKey(0)), + estimate.at(velocityKey(0)) + ); + last_estimated_bias = estimate.at(biasKey(0)); + imu_preintegrator = std::make_unique( + imu_params, last_estimated_bias + ); } @@ -53,7 +83,7 @@ namespace urc_slam { void SlamBackend::addKeyframe(const gtsam::Pose3 &measured_pose) { const std::size_t new_index = latest_index + 1; const gtsam::Pose3 delta = last_measured_pose.between(measured_pose); - const gtsam::Pose3 predicted_pose = last_estimated_pose.compose(delta); + const gtsam::Pose3 predicted_pose = last_estimated_navstate.pose().compose(delta); new_factors.add( gtsam::BetweenFactor( @@ -63,9 +93,21 @@ namespace urc_slam { odom_noise ) ); + new_factors.add( + gtsam::CombinedImuFactor( + poseKey(latest_index), + velocityKey(latest_index), + poseKey(new_index), + velocityKey(new_index), + biasKey(latest_index), + biasKey(new_index), + *imu_preintegrator + ) + ); new_values.insert(poseKey(new_index), predicted_pose); - + new_values.insert(velocityKey(new_index), last_estimated_navstate.v()); + new_values.insert(biasKey(new_index), last_estimated_bias); isam.update(new_factors, new_values); estimate = isam.calculateEstimate(); @@ -74,8 +116,14 @@ namespace urc_slam { latest_index = new_index; last_measured_pose = measured_pose; - // Figure ts syntax out later - last_estimated_pose = estimate.at(poseKey(latest_index)); + last_estimated_navstate = gtsam::NavState( + estimate.at(poseKey(latest_index)), + estimate.at(velocityKey(latest_index)) + ); + last_estimated_bias = estimate.at(biasKey(latest_index)); + imu_preintegrator = std::make_unique( + imu_params, last_estimated_bias + ); } void SlamBackend::addLidarFactor( @@ -97,7 +145,20 @@ namespace urc_slam { new_factors.resize(0); new_values.clear(); - last_estimated_pose = estimate.at(poseKey(latest_index)); + last_estimated_navstate = gtsam::NavState( + estimate.at(poseKey(latest_index)), + estimate.at(velocityKey(latest_index)) + ); + last_estimated_bias = estimate.at(biasKey(latest_index)); + } + + + void SlamBackend::integrateImuMeasurement( + const gtsam::Vector3 &measured_acc, + const gtsam::Vector3 &measured_omega, + double dt + ) { + imu_preintegrator->integrateMeasurement(measured_acc, measured_omega, dt); } gtsam::Pose3 SlamBackend::latestEstimate() const { @@ -108,6 +169,14 @@ namespace urc_slam { return gtsam::Symbol('x', index); } + gtsam::Symbol SlamBackend::velocityKey(std::size_t index) const { + return gtsam::Symbol('v', index); + } + + gtsam::Symbol SlamBackend::biasKey(std::size_t index) const { + return gtsam::Symbol('b', index); + } + double SlamBackend::translationDistance( const gtsam::Pose3 &a, const gtsam::Pose3 &b From c41d57b951fc8fb9e989e6ea665c89bb3236a409 Mon Sep 17 00:00:00 2001 From: shayaf84 Date: Fri, 28 Aug 2026 20:09:04 +0000 Subject: [PATCH 5/9] Lidar front integrated with backend --- urc_slam/CMakeLists.txt | 106 ++++++++++++ urc_slam/config/slam_params.yaml | 22 +++ urc_slam/include/lidar_frontend.hpp | 58 +++++++ urc_slam/include/slam_backend.hpp | 55 ++++-- urc_slam/include/slam_node.hpp | 23 ++- urc_slam/launch/slam.launch.py | 25 +++ urc_slam/package.xml | 35 ++++ urc_slam/src/lidar_frontend.cpp | 118 +++++++++++++ urc_slam/src/slam_backend.cpp | 253 ++++++++++++++++++--------- urc_slam/src/slam_node.cpp | 258 ++++++++++++++++++++++++++++ 10 files changed, 846 insertions(+), 107 deletions(-) create mode 100644 urc_slam/CMakeLists.txt create mode 100644 urc_slam/config/slam_params.yaml create mode 100644 urc_slam/include/lidar_frontend.hpp create mode 100644 urc_slam/launch/slam.launch.py create mode 100644 urc_slam/package.xml create mode 100644 urc_slam/src/lidar_frontend.cpp create mode 100644 urc_slam/src/slam_node.cpp diff --git a/urc_slam/CMakeLists.txt b/urc_slam/CMakeLists.txt new file mode 100644 index 00000000..a0abc4fb --- /dev/null +++ b/urc_slam/CMakeLists.txt @@ -0,0 +1,106 @@ +cmake_minimum_required(VERSION 3.5) +project(urc_slam) + +include(../cmake/default_settings.cmake) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(pcl_conversions REQUIRED) + +find_package(Eigen3 REQUIRED) +cmake_policy(SET CMP0074 NEW) +find_package(PCL REQUIRED COMPONENTS + common + filters + registration +) +find_package(GTSAM REQUIRED) + +add_library(${PROJECT_NAME} SHARED + src/slam_backend.cpp + src/lidar_frontend.cpp + src/slam_node.cpp +) + +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ${PCL_INCLUDE_DIRS} +) + +target_compile_definitions(${PROJECT_NAME} + PRIVATE + ${PCL_DEFINITIONS} +) + +ament_target_dependencies(${PROJECT_NAME} + rclcpp + rclcpp_components + sensor_msgs + nav_msgs + geometry_msgs + tf2_ros + pcl_conversions +) + +target_link_libraries(${PROJECT_NAME} + Eigen3::Eigen + gtsam + ${PCL_LIBRARIES} +) + +rclcpp_components_register_node( + ${PROJECT_NAME} + PLUGIN "urc_slam::SlamNode" + EXECUTABLE SlamNode +) + +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + DIRECTORY config launch + DESTINATION share/${PROJECT_NAME} +) + +ament_export_targets( + export_${PROJECT_NAME} + HAS_LIBRARY_TARGET +) + +ament_export_include_directories(include) + +ament_export_dependencies( + rclcpp + rclcpp_components + sensor_msgs + nav_msgs + geometry_msgs + tf2_ros + pcl_conversions +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/urc_slam/config/slam_params.yaml b/urc_slam/config/slam_params.yaml new file mode 100644 index 00000000..eaa2f986 --- /dev/null +++ b/urc_slam/config/slam_params.yaml @@ -0,0 +1,22 @@ +slam_node: + ros__parameters: + imu_topic: /imu/data + lidar_topic: /scan/points + slam_odom_topic: /slam/odometry + + map_frame: map + base_link_frame: base_link + + keyframe: + translation_threshold_m: 0.5 + rotation_threshold_rad: 0.174533 + + lidar_frontend: + voxel_size_m: 0.2 + minimum_range_m: 1.0 + maximum_range_m: 80.0 + maximum_correspondence_distance_m: 2.0 + maximum_iterations: 64 + transformation_epsilon: 1.0e-6 + fitness_epsilon: 1.0e-6 + maximum_fitness_score: 0.5 \ No newline at end of file diff --git a/urc_slam/include/lidar_frontend.hpp b/urc_slam/include/lidar_frontend.hpp new file mode 100644 index 00000000..d90949ed --- /dev/null +++ b/urc_slam/include/lidar_frontend.hpp @@ -0,0 +1,58 @@ +#ifndef LIDAR_FRONTEND_HPP_ +#define LIDAR_FRONTEND_HPP_ + +#include + +#include +#include +#include + +namespace urc_slam { + + struct LidarRegistrationResult { + bool converged = false; + double fitness_score = std::numeric_limits::infinity(); + gtsam::Pose3 relative_pose; + }; + + class LidarFrontend { + public: + using Point = pcl::PointXYZ; + using Cloud = pcl::PointCloud; + + LidarFrontend( + double voxel_size_m, + double minimum_range_m, + double maximum_range_m, + double maximum_correspondence_distance_m, + int maximum_iterations, + double transformation_epsilon, + double fitness_epsilon, + const gtsam::Pose3 &base_T_lidar + ); + + Cloud::Ptr preprocess( + const Cloud::ConstPtr &cloud + ) const; + + LidarRegistrationResult registerClouds( + const Cloud::ConstPtr &previous_cloud, + const Cloud::ConstPtr ¤t_cloud, + const gtsam::Pose3 &initial_base_relative_pose + ) const; + private: + double voxel_size_m_; + double minimum_range_m_; + double maximum_range_m_; + double maximum_correspondence_distance_m_; + int maximum_iterations_; + double transformation_epsilon_; + double fitness_epsilon_; + + // Convert between base link and Lidar frame + gtsam::Pose3 base_T_lidar_; + }; + + +} +#endif \ No newline at end of file diff --git a/urc_slam/include/slam_backend.hpp b/urc_slam/include/slam_backend.hpp index e525d378..37297721 100644 --- a/urc_slam/include/slam_backend.hpp +++ b/urc_slam/include/slam_backend.hpp @@ -8,10 +8,10 @@ #include #include #include -#include -#include +#include +#include #include -#include +#include #include @@ -19,16 +19,25 @@ namespace urc_slam { class SlamBackend { public: + // Constructor SlamBackend(double keyframe_translation_threshold_m, double keyframe_rotation_threshold_rad, - const gtsam::Vector6 &prior_sigmas, + const gtsam::Vector6 &pose_prior_sigmas, + const gtsam::Vector3 &velocity_prior_sigmas, const gtsam::Vector6 &odom_sigmas, - const gtsam::Vector6 &lidar_sigmas); + const gtsam::Vector6 &lidar_sigmas, + const gtsam::Vector6 &bias_prior_sigmas, + const gtsam::Vector6 &bias_between_sigmas + ); - void initialize(const gtsam::Pose3 &initial_pose); - + // Creates first node in the SLAM factor graph + void initialize(const gtsam::NavState &initial_navstate, + const gtsam::imuBias::ConstantBias &initial_bias); + + // Whether to add a new slam state to the graph bool shouldCreateKeyframe(const gtsam::Pose3 &measured_pose) const; + //Adds node to graph void addKeyframe(const gtsam::Pose3 &measured_pose); void addLidarFactor( @@ -36,24 +45,34 @@ namespace urc_slam { std::size_t to_index, const gtsam::Pose3 &relative_pose ); - + + // Adds a raw imu sample to the preintegrator + // Every time an imu measurement arrives, accumulate it into + // a compressed IMU measurement between current and next frame void integrateImuMeasurement( const gtsam::Vector3 &measured_acc, const gtsam::Vector3 &measured_omega, double dt ); + + // Returns the backend's best estimate of robot state + gtsam::NavState latestEstimate() const; - gtsam::Pose3 latestEstimate() const; + gtsam::Pose3 predictedRelativePose() const; private: + + // Factor graph symbols gtsam::Symbol poseKey(std::size_t index) const; gtsam::Symbol velocityKey(std::size_t index) const; gtsam::Symbol biasKey(std::size_t index) const; + // Translation distance double translationDistance( const gtsam::Pose3 &a, const gtsam::Pose3 &b ) const; - + + // Rotation Distance double rotationDistance( const gtsam::Pose3 &a, const gtsam::Pose3 &b @@ -65,27 +84,27 @@ namespace urc_slam { gtsam::Values estimate; - gtsam::SharedNoiseModel prior_noise; + gtsam::SharedNoiseModel pose_prior_noise; + gtsam::SharedNoiseModel velocity_prior_noise; gtsam::SharedNoiseModel odom_noise; gtsam::SharedNoiseModel lidar_noise; - gtsam::SharedNoiseModel velocity_prior_noise; gtsam::SharedNoiseModel bias_prior_noise; + gtsam::SharedNoiseModel bias_between_noise; - std::shared_ptr imu_params; - std::unique_ptr imu_preintegrator; + boost::shared_ptr imu_params; + std::unique_ptr imu_preintegrator; - gtsam::imuBias::ConstantBias last_estimated_bias; - std::size_t latest_index = 0; double keyframe_translation_threshold_m; double keyframe_rotation_threshold_rad; - gtsam::Pose3 last_measured_pose; - gtsam::NavState last_estimated_navstate; + gtsam::Pose3 last_estimated_pose; + gtsam::Vector3 last_estimated_velocity; + gtsam::imuBias::ConstantBias last_estimated_bias; }; diff --git a/urc_slam/include/slam_node.hpp b/urc_slam/include/slam_node.hpp index ed2e9d6d..2202c548 100644 --- a/urc_slam/include/slam_node.hpp +++ b/urc_slam/include/slam_node.hpp @@ -3,8 +3,7 @@ #include #include - -#include +#include #include #include #include @@ -13,26 +12,28 @@ #include #include "slam_backend.hpp" - +#include "lidar_frontend.hpp" namespace urc_slam { - class SlamNode: public rclcpp::Node { + class SlamNode : public rclcpp::Node { public: explicit SlamNode( const rclcpp::NodeOptions &options = rclcpp::NodeOptions() ); private: - void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg); + LidarFrontend lidar_frontend; + LidarFrontend::Cloud::Ptr previous_keyframe_cloud; + double maximum_fitness_score; + void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg); void lidarCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg); - - gtsam::Pose3 poseFromOdom(const nav_msgs::msg::Odometry &msg) const; void publishOutputs(const rclcpp::Time &stamp); SlamBackend backend; + std::optional previous_imu_stamp; + std::size_t latest_keyframe_index = 0; - rclcpp::Subscription::SharedPtr odom_sub; rclcpp::Subscription::SharedPtr imu_sub; rclcpp::Subscription::SharedPtr lidar_sub; @@ -42,14 +43,12 @@ namespace urc_slam { std::unique_ptr tf_broadcaster; nav_msgs::msg::Path path_msg; - sensor_msgs::msg::Imu last_imu_msg; - sensor_msgs::msg::PointCloud2::SharedPtr last_lidar_msg; - std::string odom_topic; + + std::string slam_odom_topic; std::string imu_topic; std::string lidar_topic; std::string map_frame; - std::string odom_frame; std::string base_link_frame; }; diff --git a/urc_slam/launch/slam.launch.py b/urc_slam/launch/slam.launch.py new file mode 100644 index 00000000..094fc4d6 --- /dev/null +++ b/urc_slam/launch/slam.launch.py @@ -0,0 +1,25 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + parameters_file = os.path.join( + get_package_share_directory("urc_slam"), + "config", + "slam_params.yaml", + ) + + slam_node = Node( + package="urc_slam", + executable="SlamNode", + name="slam_node", + output="screen", + parameters=[parameters_file], + ) + + return LaunchDescription([ + slam_node, + ]) \ No newline at end of file diff --git a/urc_slam/package.xml b/urc_slam/package.xml new file mode 100644 index 00000000..3b1ca951 --- /dev/null +++ b/urc_slam/package.xml @@ -0,0 +1,35 @@ + + + + urc_slam + 0.0.0 + LiDAR-inertial SLAM using GTSAM and ROS 2. + + Shaya Farahmand + MIT + + ament_cmake + + rclcpp + rclcpp_components + sensor_msgs + nav_msgs + geometry_msgs + tf2_ros + pcl_conversions + libpcl-all-dev + eigen + gtsam + + ament_index_python + launch + launch_ros + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/urc_slam/src/lidar_frontend.cpp b/urc_slam/src/lidar_frontend.cpp new file mode 100644 index 00000000..6aec2f56 --- /dev/null +++ b/urc_slam/src/lidar_frontend.cpp @@ -0,0 +1,118 @@ +#include "lidar_frontend.hpp" + +#include +#include +#include +#include +#include + +namespace urc_slam { + LidarFrontend::LidarFrontend( + double voxel_size_m, + double minimum_range_m, + double maximum_range_m, + double maximum_correspondence_distance_m, + int maximum_iterations, + double transformation_epsilon, + double fitness_epsilon, + const gtsam::Pose3 &base_T_lidar + ) + : voxel_size_m_(voxel_size_m), + minimum_range_m_(minimum_range_m), + maximum_range_m_(maximum_range_m), + maximum_correspondence_distance_m_( + maximum_correspondence_distance_m + ), + maximum_iterations_(maximum_iterations), + transformation_epsilon_(transformation_epsilon), + fitness_epsilon_(fitness_epsilon), + base_T_lidar_(base_T_lidar) +{ + +} + +LidarFrontend::Cloud::Ptr LidarFrontend::preprocess(const Cloud::ConstPtr &cloud) const { + Cloud::Ptr range_filtered(new Cloud); + + range_filtered->reserve(cloud->size()); + + const double minimum_range_squared = minimum_range_m_ * minimum_range_m_; + const double maximum_range_squared = maximum_range_m_ * maximum_range_m_; + + // Remove invalid points and points outside range + for (const Point &point : cloud->points) { + if (!pcl::isFinite(point)) { + continue; + } + + const double range_squared = point.x * point.x + point.y * point.y + point.z * point.z; + + if (range_squared < minimum_range_squared || range_squared > maximum_range_squared) { + continue; + } + + range_filtered->push_back(point); + } + Cloud::Ptr downsampled(new Cloud); + + pcl::VoxelGrid voxel_filter; + voxel_filter.setInputCloud(range_filtered); + voxel_filter.setLeafSize(voxel_size_m_, voxel_size_m_, voxel_size_m_); + voxel_filter.filter(*downsampled); + return downsampled; +} + +LidarRegistrationResult LidarFrontend::registerClouds( + const Cloud::ConstPtr &previous_cloud, + const Cloud::ConstPtr ¤t_cloud, + const gtsam::Pose3 &initial_base_relative_pose +) const { + LidarRegistrationResult result; + if (!previous_cloud || !current_cloud || previous_cloud->empty() || + current_cloud->empty()) { + return result; + } + + // Convert imu prediction from base_link to lidar frame + const gtsam::Pose3 initial_lidar_relative_pose = + base_T_lidar_.inverse().compose(initial_base_relative_pose).compose(base_T_lidar_); + + const Eigen::Matrix4f initial_guess = initial_lidar_relative_pose.matrix().cast(); + + pcl::GeneralizedIterativeClosestPoint gicp; + + gicp.setInputSource(current_cloud); + gicp.setInputTarget(previous_cloud); + gicp.setMaxCorrespondenceDistance(maximum_correspondence_distance_m_); + gicp.setMaximumIterations(maximum_iterations_); + gicp.setTransformationEpsilon(transformation_epsilon_); + gicp.setEuclideanFitnessEpsilon(fitness_epsilon_); + + Cloud aligned_cloud; + + gicp.align(aligned_cloud, initial_guess); + + result.converged = gicp.hasConverged(); + result.fitness_score = gicp.getFitnessScore(); + + if (!result.converged) { + return result; + } + + const Eigen::Matrix4f lidar_transform = gicp.getFinalTransformation(); + const gtsam::Matrix3 rotation_matrix = lidar_transform.block<3, 3>(0, 0).cast(); + const gtsam::Vector3 translation_vector = lidar_transform.block<3, 1>(0, 3).cast(); + + const gtsam::Pose3 lidar_relative_pose( + gtsam::Rot3(rotation_matrix), + translation_vector + ); + + // Convert lidar relative pose back to base_link frame + result.relative_pose = base_T_lidar_.compose(lidar_relative_pose).compose(base_T_lidar_.inverse()); + return result; + + +} + +} \ No newline at end of file diff --git a/urc_slam/src/slam_backend.cpp b/urc_slam/src/slam_backend.cpp index b1bfd6c2..21e08ecf 100644 --- a/urc_slam/src/slam_backend.cpp +++ b/urc_slam/src/slam_backend.cpp @@ -1,129 +1,208 @@ #include "slam_backend.hpp" #include -#include - +#include #include -#include #include -#include #include #include #include -#include +#include +#include namespace urc_slam { - + + // Constructor SlamBackend::SlamBackend( double keyframe_translation_threshold_m, double keyframe_rotation_threshold_rad, - const gtsam::Vector6 &prior_sigmas, + const gtsam::Vector6 &pose_prior_sigmas, + const gtsam::Vector3 &velocity_prior_sigmas, const gtsam::Vector6 &odom_sigmas, - const gtsam::Vector6 &lidar_sigmas) - : isam(gtsam::ISAM2(gtsam::ISAM2Params())), - prior_noise(gtsam::noiseModel::Diagonal::Sigmas(prior_sigmas)), + const gtsam::Vector6 &lidar_sigmas, + const gtsam::Vector6 &bias_prior_sigmas, + const gtsam::Vector6 &bias_between_sigmas + ): + // ISAM factor graph + isam(gtsam::ISAM2(gtsam::ISAM2Params())), + // Prior for first measurement, wheel odom noise, lidar noise + pose_prior_noise(gtsam::noiseModel::Diagonal::Sigmas(pose_prior_sigmas)), + velocity_prior_noise(gtsam::noiseModel::Diagonal::Sigmas(velocity_prior_sigmas)), odom_noise(gtsam::noiseModel::Diagonal::Sigmas(odom_sigmas)), lidar_noise(gtsam::noiseModel::Diagonal::Sigmas(lidar_sigmas)), - velocity_prior_noise(gtsam::noiseModel::Isotropic::Sigma(3, 1.0)), - bias_prior_noise(gtsam::noiseModel::Isotropic::Sigma(6, 1e-3)), + // Bias noises + bias_prior_noise(gtsam::noiseModel::Diagonal::Sigmas(bias_prior_sigmas)), + bias_between_noise(gtsam::noiseModel::Diagonal::Sigmas(bias_between_sigmas)), + // Latest index latest_index(0), - keyframe_translation_threshold_m(keyframe_translation_threshold_m), - keyframe_rotation_threshold_rad(keyframe_rotation_threshold_rad) + // for shouldCreateKeyframe + keyframe_translation_threshold_m( + keyframe_translation_threshold_m + ), + keyframe_rotation_threshold_rad( + keyframe_rotation_threshold_rad + ) { - imu_params = gtsam::PreintegrationCombinedParams::MakeSharedU(9.81); + // IMU covariance and gravity params + imu_params = gtsam::PreintegrationParams::MakeSharedU(9.81); imu_params->accelerometerCovariance = gtsam::I_3x3 * std::pow(0.1, 2); imu_params->gyroscopeCovariance = gtsam::I_3x3 * std::pow(0.01, 2); imu_params->integrationCovariance = gtsam::I_3x3 * 1e-8; - imu_params->biasAccCovariance = gtsam::I_3x3 * std::pow(0.0001, 2); - imu_params->biasOmegaCovariance = gtsam::I_3x3 * std::pow(0.0001, 2); - imu_params->biasAccOmegaInt = gtsam::I_6x6 * 1e-5; + } - void SlamBackend::initialize(const gtsam::Pose3 &initial_pose) { - const gtsam::Vector3 initial_velocity(0.0, 0.0, 0.0); - const gtsam::imuBias::ConstantBias initial_bias; + void SlamBackend::initialize( + const gtsam::NavState &initial_navstate, + const gtsam::imuBias::ConstantBias &initial_bias + ) { + // Add prior factor with initial navstate and prior noise new_factors.add( - gtsam::PriorFactor(poseKey(0), initial_pose, prior_noise) + gtsam::PriorFactor( + poseKey(0), + initial_navstate.pose(), + pose_prior_noise + ) ); + new_factors.add( - gtsam::PriorFactor(velocityKey(0), initial_velocity, velocity_prior_noise) + gtsam::PriorFactor( + velocityKey(0), + initial_navstate.v(), + velocity_prior_noise + ) ); + // Add initial bias to the graph new_factors.add( - gtsam::PriorFactor(biasKey(0), initial_bias, bias_prior_noise) + gtsam::PriorFactor< + gtsam::imuBias::ConstantBias>( + biasKey(0), + initial_bias, + bias_prior_noise + ) ); - new_values.insert(poseKey(0), initial_pose); - new_values.insert(velocityKey(0), initial_velocity); + + // Add prior values to graph + new_values.insert(poseKey(0), initial_navstate.pose()); + new_values.insert(velocityKey(0), initial_navstate.v()); new_values.insert(biasKey(0), initial_bias); isam.update(new_factors, new_values); estimate = isam.calculateEstimate(); - + + // Empty out the new factors and new values new_factors.resize(0); new_values.clear(); + latest_index = 0; - last_measured_pose = initial_pose; - last_estimated_navstate = gtsam::NavState( - estimate.at(poseKey(0)), - estimate.at(velocityKey(0)) - ); + last_measured_pose = initial_navstate.pose(); + + last_estimated_pose = estimate.at(poseKey(0)); + last_estimated_velocity = estimate.at(velocityKey(0)); last_estimated_bias = estimate.at(biasKey(0)); - imu_preintegrator = std::make_unique( - imu_params, last_estimated_bias + + imu_preintegrator = std::make_unique( + imu_params, + last_estimated_bias ); } bool SlamBackend::shouldCreateKeyframe(const gtsam::Pose3 &measured_pose) const { - return translationDistance(last_measured_pose, measured_pose) >= keyframe_translation_threshold_m - || rotationDistance(last_measured_pose, measured_pose) >= keyframe_rotation_threshold_rad; + // Check if current measurements exceed thresholds + return translationDistance( + last_measured_pose, + measured_pose + ) >= keyframe_translation_threshold_m + || + rotationDistance( + last_measured_pose, + measured_pose + ) >= keyframe_rotation_threshold_rad; + } + + gtsam::Pose3 SlamBackend::predictedRelativePose() const { + const gtsam::NavState previous_state( + last_estimated_pose, + last_estimated_velocity + ); + + const gtsam::NavState predicted_state = imu_preintegrator->predict( + previous_state, + last_estimated_bias + ); + + return last_estimated_pose.between( + predicted_state.pose() + ); } void SlamBackend::addKeyframe(const gtsam::Pose3 &measured_pose) { - const std::size_t new_index = latest_index + 1; - const gtsam::Pose3 delta = last_measured_pose.between(measured_pose); - const gtsam::Pose3 predicted_pose = last_estimated_navstate.pose().compose(delta); + const std::size_t previous_index = latest_index; + const std::size_t new_index = previous_index + 1; + + // Reconstruct previous state for IMU prediction + const gtsam::NavState previous_state( + last_estimated_pose, + last_estimated_velocity + ); + + const gtsam::NavState predicted_state = + imu_preintegrator->predict( + previous_state, + last_estimated_bias + ); + + // Preintegrated IMU constraint on pose and velocity new_factors.add( - gtsam::BetweenFactor( - poseKey(latest_index), + gtsam::ImuFactor( + poseKey(previous_index), + velocityKey(previous_index), poseKey(new_index), - delta, - odom_noise + velocityKey(new_index), + biasKey(previous_index), + *imu_preintegrator ) ); + + // Model IMU bias as slowly changing between keyframes new_factors.add( - gtsam::CombinedImuFactor( - poseKey(latest_index), - velocityKey(latest_index), - poseKey(new_index), - velocityKey(new_index), - biasKey(latest_index), + gtsam::BetweenFactor( + biasKey(previous_index), biasKey(new_index), - *imu_preintegrator + gtsam::imuBias::ConstantBias(), + bias_between_noise ) ); - new_values.insert(poseKey(new_index), predicted_pose); - new_values.insert(velocityKey(new_index), last_estimated_navstate.v()); - new_values.insert(biasKey(new_index), last_estimated_bias); - isam.update(new_factors, new_values); - estimate = isam.calculateEstimate(); + // Predicted pose and velocity + new_values.insert( + poseKey(new_index), + predicted_state.pose() + ); - new_factors.resize(0); - new_values.clear(); + new_values.insert( + velocityKey(new_index), + predicted_state.v() + ); + + new_values.insert( + biasKey(new_index), + last_estimated_bias + ); + + latest_index = new_index; last_measured_pose = measured_pose; - last_estimated_navstate = gtsam::NavState( - estimate.at(poseKey(latest_index)), - estimate.at(velocityKey(latest_index)) - ); - last_estimated_bias = estimate.at(biasKey(latest_index)); - imu_preintegrator = std::make_unique( - imu_params, last_estimated_bias + + + imu_preintegrator->resetIntegrationAndSetBias( + last_estimated_bias ); + } void SlamBackend::addLidarFactor( @@ -131,6 +210,7 @@ namespace urc_slam { std::size_t to_index, const gtsam::Pose3 &relative_pose ) { + // Constrain keyframe motion using ICP new_factors.add( gtsam::BetweenFactor( poseKey(from_index), @@ -139,30 +219,51 @@ namespace urc_slam { lidar_noise ) ); + isam.update(new_factors, new_values); estimate = isam.calculateEstimate(); new_factors.resize(0); new_values.clear(); - last_estimated_navstate = gtsam::NavState( - estimate.at(poseKey(latest_index)), - estimate.at(velocityKey(latest_index)) + last_estimated_pose = estimate.at( + poseKey(latest_index) ); - last_estimated_bias = estimate.at(biasKey(latest_index)); - } + last_estimated_velocity = estimate.at( + velocityKey(latest_index) + ); + + last_estimated_bias = + estimate.at( + biasKey(latest_index) + ); + } + // Integrate IMU measurement void SlamBackend::integrateImuMeasurement( const gtsam::Vector3 &measured_acc, const gtsam::Vector3 &measured_omega, double dt ) { - imu_preintegrator->integrateMeasurement(measured_acc, measured_omega, dt); + imu_preintegrator->integrateMeasurement( + measured_acc, + measured_omega, + dt + ); } - gtsam::Pose3 SlamBackend::latestEstimate() const { - return estimate.at(poseKey(latest_index)); + + gtsam::NavState SlamBackend::latestEstimate() const { + const gtsam::Pose3 pose = estimate.at( + poseKey(latest_index) + ); + + const gtsam::Vector3 velocity = estimate.at( + velocityKey(latest_index) + ); + + return gtsam::NavState(pose,velocity); } gtsam::Symbol SlamBackend::poseKey(std::size_t index) const { @@ -177,11 +278,10 @@ namespace urc_slam { return gtsam::Symbol('b', index); } - double SlamBackend::translationDistance( - const gtsam::Pose3 &a, + double SlamBackend::translationDistance(const gtsam::Pose3 &a, const gtsam::Pose3 &b ) const { - return a.translation().distance(b.translation()); + return (a.translation() - b.translation()).norm(); } double SlamBackend::rotationDistance( @@ -190,5 +290,4 @@ namespace urc_slam { ) const { return a.rotation().between(b.rotation()).rpy().norm(); } - -} // namespace urc_slam +} \ No newline at end of file diff --git a/urc_slam/src/slam_node.cpp b/urc_slam/src/slam_node.cpp new file mode 100644 index 00000000..99260895 --- /dev/null +++ b/urc_slam/src/slam_node.cpp @@ -0,0 +1,258 @@ +#include "slam_node.hpp" +#include +#include +#include +#include + +#include + +#include +#include + +namespace urc_slam { + SlamNode::SlamNode(const rclcpp::NodeOptions &options) + : rclcpp::Node("slam_node", options), + lidar_frontend( + declare_parameter("lidar_frontend.voxel_size_m", 0.2), + declare_parameter("lidar_frontend.minimum_range_m", 1.0), + declare_parameter("lidar_frontend.maximum_range_m", 80.0), + declare_parameter("lidar_frontend.maximum_correspondence_distance_m", 2.0), + declare_parameter("lidar_frontend.maximum_iterations", 64), + declare_parameter("lidar_frontend.transformation_epsilon", 1e-6), + declare_parameter("lidar_frontend.fitness_epsilon", 1e-6), + gtsam::Pose3() + + ), + previous_keyframe_cloud(nullptr), + maximum_fitness_score( + declare_parameter("lidar_frontend.maximum_fitness_score", 0.5) + ), + backend( + declare_parameter("keyframe.translation_threshold_m", 0.5), + declare_parameter("keyframe.rotation_threshold_rad", 0.174533), + gtsam::Vector6::Constant(0.1), + gtsam::Vector3::Constant(1.0), + gtsam::Vector6::Constant(1.0), + gtsam::Vector6::Constant(0.2), + gtsam::Vector6::Constant(1e-3), + gtsam::Vector6::Constant(1e-4) + ), + previous_imu_stamp(std::nullopt), + latest_keyframe_index(0) + { + imu_topic = declare_parameter("imu_topic", "/imu/data"); + lidar_topic = declare_parameter("lidar_topic", "/points"); + slam_odom_topic = declare_parameter("slam_odom_topic", "/slam/odometry"); + map_frame = declare_parameter("map_frame", "map"); + base_link_frame = declare_parameter("base_link_frame", "base_link"); + + backend.initialize( + gtsam::NavState( + gtsam::Pose3(), + gtsam::Vector3::Zero() + ), + gtsam::imuBias::ConstantBias() + ); + + imu_sub = create_subscription( + imu_topic, + rclcpp::SensorDataQoS(), + std::bind( + &SlamNode::imuCallback, + this, + std::placeholders::_1 + ) + ); + + lidar_sub = create_subscription( + lidar_topic, + rclcpp::SensorDataQoS(), + std::bind( + &SlamNode::lidarCallback, + this, + std::placeholders::_1 + ) + ); + + slam_odom_pub = create_publisher( + slam_odom_topic, + 10 + ); + + path_pub = create_publisher( + "/slam/path", + 10 + ); + + tf_broadcaster = std::make_unique(*this); + + path_msg.header.frame_id = map_frame; + + } + + void SlamNode::imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg) { + const rclcpp::Time current_stamp(msg->header.stamp); + + if (!previous_imu_stamp.has_value()) { + previous_imu_stamp = current_stamp; + return; + } + + const double dt = (current_stamp - previous_imu_stamp.value()).seconds(); + previous_imu_stamp = current_stamp; + + if (dt <= 0.0) { + return; + } + + // Keyframe zero begins at first LiDAR cloud + if (!previous_keyframe_cloud) { + return; + } + + const gtsam::Vector3 measured_acceleration( + msg->linear_acceleration.x, + msg->linear_acceleration.y, + msg->linear_acceleration.z + ); + + const gtsam::Vector3 measured_angular_velocity( + msg->angular_velocity.x, + msg->angular_velocity.y, + msg->angular_velocity.z + ); + + backend.integrateImuMeasurement( + measured_acceleration, + measured_angular_velocity, + dt + ); + } + + void SlamNode::lidarCallback( + const sensor_msgs::msg::PointCloud2::SharedPtr msg + ) { + LidarFrontend::Cloud::Ptr raw_cloud( + new LidarFrontend::Cloud + ); + + pcl::fromROSMsg(*msg, *raw_cloud); + + const LidarFrontend::Cloud::Ptr current_cloud = lidar_frontend.preprocess(raw_cloud); + + if (!current_cloud || current_cloud->empty()) { + RCLCPP_WARN(get_logger(), "Discarding empty LiDAR cloud"); + return; + } + + // First cloud is keyframe zero + if (!previous_keyframe_cloud) { + previous_keyframe_cloud = current_cloud; + latest_keyframe_index = 0; + publishOutputs(rclcpp::Time(msg->header.stamp)); + return; + } + + // Use accumulated IMU motion as ICP initial guess + const gtsam::Pose3 imu_relative_guess = backend.predictedRelativePose(); + const gtsam::Pose3 predicted_global_pose = backend.latestEstimate().pose().compose( + imu_relative_guess + ); + + if (!backend.shouldCreateKeyframe(predicted_global_pose)) { + return; + } + + const LidarRegistrationResult registration = lidar_frontend.registerClouds( + previous_keyframe_cloud, + current_cloud, + imu_relative_guess + ); + + if (!registration.converged) { + RCLCPP_WARN(get_logger(), "LiDAR registration did not converge"); + return; + } + + if (registration.fitness_score > maximum_fitness_score) { + RCLCPP_WARN(get_logger(), "Rejected LiDAR registration with fitness score %.3f", + registration.fitness_score + ); + return; + } + + const std::size_t previous_index = latest_keyframe_index; + const std::size_t current_index = previous_index + 1; + + // Convert icp relative transform to global pose + const gtsam::Pose3 current_global_pose = backend.latestEstimate().pose().compose( + registration.relative_pose + ); + + // Stage imu state, then optimize with LiDAR constraint + backend.addKeyframe(current_global_pose); + backend.addLidarFactor( + previous_index, + current_index, + registration.relative_pose + ); + + latest_keyframe_index = current_index; + previous_keyframe_cloud = current_cloud; + + publishOutputs(rclcpp::Time(msg->header.stamp)); + } + + void SlamNode::publishOutputs(const rclcpp::Time &stamp) { + const gtsam::NavState state = backend.latestEstimate(); + const gtsam::Pose3 pose = state.pose(); + const gtsam::Point3 position = pose.translation(); + const auto orientation = pose.rotation().toQuaternion(); + + // gtsam velocity is in the map frame, odometry twist uses base link + const gtsam::Vector3 body_velocity = pose.rotation().unrotate(state.v()); + + nav_msgs::msg::Odometry odometry_msg; + odometry_msg.header.stamp = stamp; + odometry_msg.header.frame_id = map_frame; + odometry_msg.child_frame_id = base_link_frame; + + odometry_msg.pose.pose.position.x = position.x(); + odometry_msg.pose.pose.position.y = position.y(); + odometry_msg.pose.pose.position.z = position.z(); + + odometry_msg.pose.pose.orientation.w = orientation.w(); + odometry_msg.pose.pose.orientation.x = orientation.x(); + odometry_msg.pose.pose.orientation.y = orientation.y(); + odometry_msg.pose.pose.orientation.z = orientation.z(); + + odometry_msg.twist.twist.linear.x = body_velocity.x(); + odometry_msg.twist.twist.linear.y = body_velocity.y(); + odometry_msg.twist.twist.linear.z = body_velocity.z(); + + slam_odom_pub->publish(odometry_msg); + + geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.header = odometry_msg.header; + pose_msg.pose = odometry_msg.pose.pose; + + path_msg.header.stamp = stamp; + path_msg.poses.push_back(pose_msg); + path_pub->publish(path_msg); + + geometry_msgs::msg::TransformStamped transform_msg; + transform_msg.header.stamp = stamp; + transform_msg.header.frame_id = map_frame; + transform_msg.child_frame_id = base_link_frame; + + transform_msg.transform.translation.x = position.x(); + transform_msg.transform.translation.y = position.y(); + transform_msg.transform.translation.z = position.z(); + + transform_msg.transform.rotation = odometry_msg.pose.pose.orientation; + tf_broadcaster->sendTransform(transform_msg); + } +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(urc_slam::SlamNode) \ No newline at end of file From 19002e4057b31e97beb6b9039a73dacf0698d0b8 Mon Sep 17 00:00:00 2001 From: shayaf84 Date: Sun, 30 Aug 2026 21:38:35 +0000 Subject: [PATCH 6/9] fix sensor configuraiton in gazebo for improved sensor frequency --- .../simplified_swerve_sensors.xacro | 4 +- urc_slam/config/slam_params.yaml | 4 +- urc_slam/include/slam_node.hpp | 1 + urc_slam/src/slam_node.cpp | 44 ++++++++++++------- 4 files changed, 32 insertions(+), 21 deletions(-) diff --git a/urc_hw_description/urdf/simplified_swerve/simplified_swerve_sensors.xacro b/urc_hw_description/urdf/simplified_swerve/simplified_swerve_sensors.xacro index d988aae1..f057f8bf 100644 --- a/urc_hw_description/urdf/simplified_swerve/simplified_swerve_sensors.xacro +++ b/urc_hw_description/urdf/simplified_swerve/simplified_swerve_sensors.xacro @@ -50,13 +50,13 @@ - 2880 + 360 1 -3.14 3.14 - 144 + 16 1 -0.366519 0.366519 diff --git a/urc_slam/config/slam_params.yaml b/urc_slam/config/slam_params.yaml index eaa2f986..96d0665a 100644 --- a/urc_slam/config/slam_params.yaml +++ b/urc_slam/config/slam_params.yaml @@ -1,6 +1,6 @@ slam_node: ros__parameters: - imu_topic: /imu/data + imu_topic: /imu/fused lidar_topic: /scan/points slam_odom_topic: /slam/odometry @@ -19,4 +19,4 @@ slam_node: maximum_iterations: 64 transformation_epsilon: 1.0e-6 fitness_epsilon: 1.0e-6 - maximum_fitness_score: 0.5 \ No newline at end of file + maximum_fitness_score: 2.5 \ No newline at end of file diff --git a/urc_slam/include/slam_node.hpp b/urc_slam/include/slam_node.hpp index 2202c548..529f64d1 100644 --- a/urc_slam/include/slam_node.hpp +++ b/urc_slam/include/slam_node.hpp @@ -31,6 +31,7 @@ namespace urc_slam { void publishOutputs(const rclcpp::Time &stamp); SlamBackend backend; + bool imu_integrated_since_keyframe = false; std::optional previous_imu_stamp; std::size_t latest_keyframe_index = 0; diff --git a/urc_slam/src/slam_node.cpp b/urc_slam/src/slam_node.cpp index 99260895..05b10282 100644 --- a/urc_slam/src/slam_node.cpp +++ b/urc_slam/src/slam_node.cpp @@ -93,6 +93,18 @@ namespace urc_slam { void SlamNode::imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg) { const rclcpp::Time current_stamp(msg->header.stamp); + const gtsam::Vector3 acceleration( + msg->linear_acceleration.x, + msg->linear_acceleration.y, + msg->linear_acceleration.z + ); + + const gtsam::Vector3 angular_velocity( + msg->angular_velocity.x, + msg->angular_velocity.y, + msg->angular_velocity.z + ); + if (!previous_imu_stamp.has_value()) { previous_imu_stamp = current_stamp; return; @@ -101,37 +113,28 @@ namespace urc_slam { const double dt = (current_stamp - previous_imu_stamp.value()).seconds(); previous_imu_stamp = current_stamp; - if (dt <= 0.0) { + if (dt <= 0.0 || dt > 0.1) { return; } - // Keyframe zero begins at first LiDAR cloud if (!previous_keyframe_cloud) { return; } - const gtsam::Vector3 measured_acceleration( - msg->linear_acceleration.x, - msg->linear_acceleration.y, - msg->linear_acceleration.z - ); - - const gtsam::Vector3 measured_angular_velocity( - msg->angular_velocity.x, - msg->angular_velocity.y, - msg->angular_velocity.z - ); - backend.integrateImuMeasurement( - measured_acceleration, - measured_angular_velocity, + acceleration, + angular_velocity, dt ); + + imu_integrated_since_keyframe = true; } void SlamNode::lidarCallback( const sensor_msgs::msg::PointCloud2::SharedPtr msg ) { + + LidarFrontend::Cloud::Ptr raw_cloud( new LidarFrontend::Cloud ); @@ -149,10 +152,15 @@ namespace urc_slam { if (!previous_keyframe_cloud) { previous_keyframe_cloud = current_cloud; latest_keyframe_index = 0; + imu_integrated_since_keyframe = false; publishOutputs(rclcpp::Time(msg->header.stamp)); return; } + if (!imu_integrated_since_keyframe) { + return; + } + // Use accumulated IMU motion as ICP initial guess const gtsam::Pose3 imu_relative_guess = backend.predictedRelativePose(); const gtsam::Pose3 predicted_global_pose = backend.latestEstimate().pose().compose( @@ -166,7 +174,8 @@ namespace urc_slam { const LidarRegistrationResult registration = lidar_frontend.registerClouds( previous_keyframe_cloud, current_cloud, - imu_relative_guess + //imu_relative_guess + gtsam::Pose3() ); if (!registration.converged) { @@ -199,6 +208,7 @@ namespace urc_slam { latest_keyframe_index = current_index; previous_keyframe_cloud = current_cloud; + imu_integrated_since_keyframe = false; publishOutputs(rclcpp::Time(msg->header.stamp)); } From df11c56262cb6695eab4a394ed7ffc2fe1ead3bc Mon Sep 17 00:00:00 2001 From: shayaf84 Date: Sun, 30 Aug 2026 22:03:32 +0000 Subject: [PATCH 7/9] create an accumulated point cloud map --- urc_slam/include/slam_node.hpp | 7 ++++++ urc_slam/src/slam_node.cpp | 46 +++++++++++++++++++++++++++++++++- 2 files changed, 52 insertions(+), 1 deletion(-) diff --git a/urc_slam/include/slam_node.hpp b/urc_slam/include/slam_node.hpp index 529f64d1..bfcb8ddf 100644 --- a/urc_slam/include/slam_node.hpp +++ b/urc_slam/include/slam_node.hpp @@ -26,6 +26,7 @@ namespace urc_slam { LidarFrontend::Cloud::Ptr previous_keyframe_cloud; double maximum_fitness_score; + void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg); void lidarCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg); void publishOutputs(const rclcpp::Time &stamp); @@ -35,6 +36,7 @@ namespace urc_slam { std::optional previous_imu_stamp; std::size_t latest_keyframe_index = 0; + rclcpp::Subscription::SharedPtr imu_sub; rclcpp::Subscription::SharedPtr lidar_sub; @@ -52,6 +54,11 @@ namespace urc_slam { std::string map_frame; std::string base_link_frame; + + LidarFrontend::Cloud::Ptr accumulated_map; + double map_voxel_size_m; + rclcpp::Publisher::SharedPtr map_pub; + void addKeyframeToMap(const LidarFrontend::Cloud::ConstPtr &cloud, const rclcpp::Time &stamp); }; } diff --git a/urc_slam/src/slam_node.cpp b/urc_slam/src/slam_node.cpp index 05b10282..e3d4045d 100644 --- a/urc_slam/src/slam_node.cpp +++ b/urc_slam/src/slam_node.cpp @@ -5,10 +5,13 @@ #include #include +#include +#include #include #include + namespace urc_slam { SlamNode::SlamNode(const rclcpp::NodeOptions &options) : rclcpp::Node("slam_node", options), @@ -38,7 +41,12 @@ namespace urc_slam { gtsam::Vector6::Constant(1e-4) ), previous_imu_stamp(std::nullopt), - latest_keyframe_index(0) + latest_keyframe_index(0), + accumulated_map(new LidarFrontend::Cloud), + map_voxel_size_m( + declare_parameter("map_voxel_size_m", 0.5) + ) + { imu_topic = declare_parameter("imu_topic", "/imu/data"); lidar_topic = declare_parameter("lidar_topic", "/points"); @@ -88,6 +96,10 @@ namespace urc_slam { path_msg.header.frame_id = map_frame; + map_pub = create_publisher( + "/slam/map_points", + rclcpp::QoS(1).reliable().transient_local() + ); } void SlamNode::imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg) { @@ -153,6 +165,8 @@ namespace urc_slam { previous_keyframe_cloud = current_cloud; latest_keyframe_index = 0; imu_integrated_since_keyframe = false; + + addKeyframeToMap(current_cloud, rclcpp::Time(msg->header.stamp)); publishOutputs(rclcpp::Time(msg->header.stamp)); return; } @@ -210,6 +224,7 @@ namespace urc_slam { previous_keyframe_cloud = current_cloud; imu_integrated_since_keyframe = false; + addKeyframeToMap(current_cloud, rclcpp::Time(msg->header.stamp)); publishOutputs(rclcpp::Time(msg->header.stamp)); } @@ -262,6 +277,35 @@ namespace urc_slam { transform_msg.transform.rotation = odometry_msg.pose.pose.orientation; tf_broadcaster->sendTransform(transform_msg); } + + void SlamNode::addKeyframeToMap( + const LidarFrontend::Cloud::ConstPtr &cloud, + const rclcpp::Time &stamp + ) { + const gtsam::Pose3 map_T_base = backend.latestEstimate().pose(); + + // FIX FOR ACTUAL - base_T_lidar is identity rn + const gtsam::Pose3 map_T_lidar = map_T_base; + + LidarFrontend::Cloud transformed_cloud; + pcl::transformPointCloud(*cloud, transformed_cloud, map_T_lidar.matrix()); + + *accumulated_map += transformed_cloud; + LidarFrontend::Cloud::Ptr filtered_map(new LidarFrontend::Cloud); + pcl::VoxelGrid voxel_filter; + voxel_filter.setInputCloud(accumulated_map); + voxel_filter.setLeafSize(map_voxel_size_m, map_voxel_size_m, map_voxel_size_m); + voxel_filter.filter(*filtered_map); + + accumulated_map = filtered_map; + + sensor_msgs::msg::PointCloud2 map_msg; + pcl::toROSMsg(*accumulated_map, map_msg); + map_msg.header.stamp = stamp; + map_msg.header.frame_id = map_frame; + map_pub->publish(map_msg); + } + } #include From bce11daf16d39fb8268475a957ad8f99066667cf Mon Sep 17 00:00:00 2001 From: shayaf84 Date: Sun, 30 Aug 2026 22:24:46 +0000 Subject: [PATCH 8/9] lidar accumulated point map --- urc_slam/include/slam_node.hpp | 1 + urc_slam/src/slam_node.cpp | 19 ++++++++++--------- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/urc_slam/include/slam_node.hpp b/urc_slam/include/slam_node.hpp index bfcb8ddf..3e72c4a6 100644 --- a/urc_slam/include/slam_node.hpp +++ b/urc_slam/include/slam_node.hpp @@ -24,6 +24,7 @@ namespace urc_slam { private: LidarFrontend lidar_frontend; LidarFrontend::Cloud::Ptr previous_keyframe_cloud; + gtsam::Pose3 last_lidar_relative_pose; double maximum_fitness_score; diff --git a/urc_slam/src/slam_node.cpp b/urc_slam/src/slam_node.cpp index e3d4045d..c5feccb4 100644 --- a/urc_slam/src/slam_node.cpp +++ b/urc_slam/src/slam_node.cpp @@ -27,6 +27,7 @@ namespace urc_slam { ), previous_keyframe_cloud(nullptr), + last_lidar_relative_pose(gtsam::Pose3()), maximum_fitness_score( declare_parameter("lidar_frontend.maximum_fitness_score", 0.5) ), @@ -188,8 +189,7 @@ namespace urc_slam { const LidarRegistrationResult registration = lidar_frontend.registerClouds( previous_keyframe_cloud, current_cloud, - //imu_relative_guess - gtsam::Pose3() + last_lidar_relative_pose ); if (!registration.converged) { @@ -204,6 +204,8 @@ namespace urc_slam { return; } + last_lidar_relative_pose = registration.relative_pose; + const std::size_t previous_index = latest_keyframe_index; const std::size_t current_index = previous_index + 1; @@ -291,13 +293,12 @@ namespace urc_slam { pcl::transformPointCloud(*cloud, transformed_cloud, map_T_lidar.matrix()); *accumulated_map += transformed_cloud; - LidarFrontend::Cloud::Ptr filtered_map(new LidarFrontend::Cloud); - pcl::VoxelGrid voxel_filter; - voxel_filter.setInputCloud(accumulated_map); - voxel_filter.setLeafSize(map_voxel_size_m, map_voxel_size_m, map_voxel_size_m); - voxel_filter.filter(*filtered_map); - - accumulated_map = filtered_map; + // LidarFrontend::Cloud::Ptr filtered_map(new LidarFrontend::Cloud); + // pcl::VoxelGrid voxel_filter; + // voxel_filter.setInputCloud(accumulated_map); + // voxel_filter.setLeafSize(map_voxel_size_m, map_voxel_size_m, map_voxel_size_m); + // voxel_filter.filter(*filtered_map); + // accumulated_map = filtered_map; sensor_msgs::msg::PointCloud2 map_msg; pcl::toROSMsg(*accumulated_map, map_msg); From 2e771235012d87fb069436253f4feb061a49c5de Mon Sep 17 00:00:00 2001 From: shayaf84 Date: Sun, 13 Sep 2026 20:27:40 +0000 Subject: [PATCH 9/9] Integrate gps with slam --- urc_bringup/launch/sim.launch.py | 8 -- urc_slam/config/slam_params.yaml | 17 ++- urc_slam/include/slam_backend.hpp | 9 +- urc_slam/include/slam_node.hpp | 27 +++++ urc_slam/launch/slam.launch.py | 15 +++ urc_slam/package.xml | 1 + urc_slam/src/slam_backend.cpp | 19 ++++ urc_slam/src/slam_node.cpp | 181 +++++++++++++++++++++++++++++- 8 files changed, 266 insertions(+), 11 deletions(-) diff --git a/urc_bringup/launch/sim.launch.py b/urc_bringup/launch/sim.launch.py index f3dc0988..1736f7c9 100644 --- a/urc_bringup/launch/sim.launch.py +++ b/urc_bringup/launch/sim.launch.py @@ -14,7 +14,6 @@ def generate_launch_description(): path_ros_gazebo_sim = get_package_share_directory("ros_gz_sim") path_urc_hw_description = get_package_share_directory("urc_hw_description") path_urc_bringup = get_package_share_directory("urc_bringup") - path_urc_localization = get_package_share_directory("urc_localization") controller_config_file_dir = os.path.join(path_urc_bringup, "config", "test_controllers.yaml") @@ -180,12 +179,6 @@ def generate_launch_description(): output="screen", ) - launch_ekf = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(path_urc_localization, "launch", "ekf.launch.py") - ) - ) - launch_autonomy = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(path_urc_bringup, "launch", "autonomy.launch.py") @@ -341,7 +334,6 @@ def generate_launch_description(): robot_state_publisher_node, covariances_on_imu, covariances_on_gps, - launch_ekf, launch_autonomy, rocker_tf_broadcaster, rocker_effort_pid_node, diff --git a/urc_slam/config/slam_params.yaml b/urc_slam/config/slam_params.yaml index 96d0665a..233bba81 100644 --- a/urc_slam/config/slam_params.yaml +++ b/urc_slam/config/slam_params.yaml @@ -1,5 +1,6 @@ slam_node: ros__parameters: + use_sim_time: true imu_topic: /imu/fused lidar_topic: /scan/points slam_odom_topic: /slam/odometry @@ -19,4 +20,18 @@ slam_node: maximum_iterations: 64 transformation_epsilon: 1.0e-6 fitness_epsilon: 1.0e-6 - maximum_fitness_score: 2.5 \ No newline at end of file + maximum_fitness_score: 2.5 + +slam_navsat_transform: + ros__parameters: + use_sim_time: true + frequency: 10.0 + delay: 0.0 + zero_altitude: false + magnetic_declination_radians: 0.0 + yaw_offset: 0.0 + use_odometry_yaw: false + wait_for_datum: false + publish_filtered_gps: false + broadcast_utm_transform: false + broadcast_cartesian_transform: false \ No newline at end of file diff --git a/urc_slam/include/slam_backend.hpp b/urc_slam/include/slam_backend.hpp index 37297721..bc474d41 100644 --- a/urc_slam/include/slam_backend.hpp +++ b/urc_slam/include/slam_backend.hpp @@ -45,6 +45,12 @@ namespace urc_slam { std::size_t to_index, const gtsam::Pose3 &relative_pose ); + + void addGpsFactor( + std::size_t index, + const gtsam::Point3 &position, + const gtsam::Matrix3 &covariance + ); // Adds a raw imu sample to the preintegrator // Every time an imu measurement arrives, accumulate it into @@ -57,10 +63,11 @@ namespace urc_slam { // Returns the backend's best estimate of robot state gtsam::NavState latestEstimate() const; + gtsam::Pose3 poseAt(std::size_t index) const; gtsam::Pose3 predictedRelativePose() const; private: - + void optimize(); // Factor graph symbols gtsam::Symbol poseKey(std::size_t index) const; gtsam::Symbol velocityKey(std::size_t index) const; diff --git a/urc_slam/include/slam_node.hpp b/urc_slam/include/slam_node.hpp index 3e72c4a6..a3322a37 100644 --- a/urc_slam/include/slam_node.hpp +++ b/urc_slam/include/slam_node.hpp @@ -4,6 +4,8 @@ #include #include #include +#include +#include #include #include #include @@ -24,22 +26,36 @@ namespace urc_slam { private: LidarFrontend lidar_frontend; LidarFrontend::Cloud::Ptr previous_keyframe_cloud; + std::vector keyframe_clouds; gtsam::Pose3 last_lidar_relative_pose; double maximum_fitness_score; void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg); void lidarCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg); + void gpsCallback(const nav_msgs::msg::Odometry::SharedPtr msg); + void processGPSMeasurements(); void publishOutputs(const rclcpp::Time &stamp); + void rebuildMap(); + + SlamBackend backend; bool imu_integrated_since_keyframe = false; std::optional previous_imu_stamp; std::size_t latest_keyframe_index = 0; + struct KeyframeStamp { + std::size_t index; + rclcpp::Time stamp; + }; + + std::deque keyframe_stamps; + rclcpp::Subscription::SharedPtr imu_sub; rclcpp::Subscription::SharedPtr lidar_sub; + rclcpp::Subscription::SharedPtr gps_sub; rclcpp::Publisher::SharedPtr slam_odom_pub; rclcpp::Publisher::SharedPtr path_pub; @@ -48,10 +64,21 @@ namespace urc_slam { nav_msgs::msg::Path path_msg; + struct MatchedGPS { + std::size_t keyframe_index; + nav_msgs::msg::Odometry measurement; + }; + std::deque gps_buffer; + std::optional last_accepted_gps_stamp; + std::deque matched_gps; + double gps_time_tolerance_sec = 0.1; + + std::string slam_odom_topic; std::string imu_topic; std::string lidar_topic; + std::string gps_topic; std::string map_frame; std::string base_link_frame; diff --git a/urc_slam/launch/slam.launch.py b/urc_slam/launch/slam.launch.py index 094fc4d6..10b0fc91 100644 --- a/urc_slam/launch/slam.launch.py +++ b/urc_slam/launch/slam.launch.py @@ -12,6 +12,20 @@ def generate_launch_description(): "slam_params.yaml", ) + navsat_node = Node( + package = "robot_localization", + executable = "navsat_transform_node", + name = "slam_navsat_transform", + output = "screen", + parameters = [parameters_file], + remappings = [ + ("gps/fix", "/gps"), + ("imu", "/imu/fused"), + ("odometry/filtered", "/slam/odometry"), + ("odometry/gps", "/slam/gps_odometry"), + ], + ) + slam_node = Node( package="urc_slam", executable="SlamNode", @@ -22,4 +36,5 @@ def generate_launch_description(): return LaunchDescription([ slam_node, + navsat_node, ]) \ No newline at end of file diff --git a/urc_slam/package.xml b/urc_slam/package.xml index 3b1ca951..54d6d01e 100644 --- a/urc_slam/package.xml +++ b/urc_slam/package.xml @@ -25,6 +25,7 @@ ament_index_python launch launch_ros + robot_localization ament_lint_auto ament_lint_common diff --git a/urc_slam/src/slam_backend.cpp b/urc_slam/src/slam_backend.cpp index 21e08ecf..76873e67 100644 --- a/urc_slam/src/slam_backend.cpp +++ b/urc_slam/src/slam_backend.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include #include #include @@ -205,6 +206,16 @@ namespace urc_slam { } + void SlamBackend::addGpsFactor( + std::size_t index, + const gtsam::Point3 &position, + const gtsam::Matrix3 &covariance + ) { + const auto noise = gtsam::noiseModel::Gaussian::Covariance(covariance); + new_factors.add(gtsam::GPSFactor(poseKey(index), position, noise)); + optimize(); + } + void SlamBackend::addLidarFactor( std::size_t from_index, std::size_t to_index, @@ -219,7 +230,11 @@ namespace urc_slam { lidar_noise ) ); + optimize(); + + } + void SlamBackend::optimize() { isam.update(new_factors, new_values); estimate = isam.calculateEstimate(); @@ -254,6 +269,10 @@ namespace urc_slam { } + gtsam::Pose3 SlamBackend::poseAt(std::size_t index) const { + return estimate.at(poseKey(index)); + } + gtsam::NavState SlamBackend::latestEstimate() const { const gtsam::Pose3 pose = estimate.at( poseKey(latest_index) diff --git a/urc_slam/src/slam_node.cpp b/urc_slam/src/slam_node.cpp index c5feccb4..d6c407de 100644 --- a/urc_slam/src/slam_node.cpp +++ b/urc_slam/src/slam_node.cpp @@ -1,5 +1,12 @@ #include "slam_node.hpp" + #include +#include + + +#include +#include + #include #include #include @@ -51,6 +58,7 @@ namespace urc_slam { { imu_topic = declare_parameter("imu_topic", "/imu/data"); lidar_topic = declare_parameter("lidar_topic", "/points"); + gps_topic = declare_parameter("gps_topic", "/slam/gps_odometry"); slam_odom_topic = declare_parameter("slam_odom_topic", "/slam/odometry"); map_frame = declare_parameter("map_frame", "map"); base_link_frame = declare_parameter("base_link_frame", "base_link"); @@ -83,6 +91,16 @@ namespace urc_slam { ) ); + gps_sub = create_subscription( + gps_topic, + rclcpp::SensorDataQoS(), + std::bind( + &SlamNode::gpsCallback, + this, + std::placeholders::_1 + ) + ); + slam_odom_pub = create_publisher( slam_odom_topic, 10 @@ -167,7 +185,12 @@ namespace urc_slam { latest_keyframe_index = 0; imu_integrated_since_keyframe = false; + keyframe_clouds.push_back(current_cloud); + keyframe_stamps.push_back({ + 0, rclcpp::Time(msg->header.stamp) + }); addKeyframeToMap(current_cloud, rclcpp::Time(msg->header.stamp)); + processGPSMeasurements(); publishOutputs(rclcpp::Time(msg->header.stamp)); return; } @@ -226,10 +249,120 @@ namespace urc_slam { previous_keyframe_cloud = current_cloud; imu_integrated_since_keyframe = false; + keyframe_clouds.push_back(current_cloud); + keyframe_stamps.push_back({ + current_index, rclcpp::Time(msg->header.stamp) + }); addKeyframeToMap(current_cloud, rclcpp::Time(msg->header.stamp)); + processGPSMeasurements(); publishOutputs(rclcpp::Time(msg->header.stamp)); } + void SlamNode::gpsCallback(nav_msgs::msg::Odometry::SharedPtr msg) { + if (msg->header.frame_id != map_frame) { + return; + } + + const auto &p = msg->pose.pose.position; + if (!std::isfinite(p.x) || + !std::isfinite(p.y) || + !std::isfinite(p.z)) { + return; + } + + // Extract position block from 6x6 pose covariance + Eigen::Matrix3d covariance; + for (int row = 0; row < 3; ++row) { + for (int col = 0; col < 3; ++col) { + covariance(row, col) = msg->pose.covariance[row * 6 + col]; + } + } + + if (!covariance.allFinite() || !covariance.isApprox(covariance.transpose())) { + return; + } + + Eigen::LLT decomposition(covariance); + if (decomposition.info() != Eigen::Success) { + return; + } + + // Keep timestamps strictly increasing; discard duplicates + const rclcpp::Time stamp(msg->header.stamp); + if (last_accepted_gps_stamp && stamp <= *last_accepted_gps_stamp) { + return; + } + + gps_buffer.push_back(*msg); + last_accepted_gps_stamp = stamp; + while (gps_buffer.size() > 200) { + gps_buffer.pop_front(); + } + + processGPSMeasurements(); + + } + + void SlamNode::processGPSMeasurements() { + if (keyframe_stamps.empty()) { + return; + } + + while (!gps_buffer.empty()) { + const auto &gps = gps_buffer.front(); + const rclcpp::Time gps_stamp(gps.header.stamp); + + // Wait until keyframes bracket this measurement's time + if (gps_stamp > keyframe_stamps.back().stamp) { + break; + } + + double best_dt = std::numeric_limits::infinity(); + std::size_t best_index = 0; + + for (const auto &keyframe : keyframe_stamps) { + const double dt = std::abs((keyframe.stamp - gps_stamp).seconds()); + + if (dt < best_dt) { + best_dt = dt; + best_index = keyframe.index; + } + } + + if (best_dt <= gps_time_tolerance_sec) { + matched_gps.push_back({best_index, gps}); + } + + // Consume matched fixes and discard those outside tolerance + gps_buffer.pop_front(); + } + + const bool has_gps_updates = !matched_gps.empty(); + while (!matched_gps.empty()) { + const auto &match = matched_gps.front(); + const auto &p = match.measurement.pose.pose.position; + + gtsam::Matrix3 covariance; + for (int row = 0; row < 3; ++row) { + for (int col = 0; col < 3; ++col) { + covariance(row, col) = match.measurement.pose.covariance[row*6 + col]; + } + } + + backend.addGpsFactor( + match.keyframe_index, + gtsam::Point3(p.x, p.y, p.z), + covariance + ); + + matched_gps.pop_front(); + } + if (has_gps_updates) { + rebuildMap(); + publishOutputs(keyframe_stamps.back().stamp); + } + } + void SlamNode::publishOutputs(const rclcpp::Time &stamp) { const gtsam::NavState state = backend.latestEstimate(); const gtsam::Pose3 pose = state.pose(); @@ -264,7 +397,8 @@ namespace urc_slam { pose_msg.pose = odometry_msg.pose.pose; path_msg.header.stamp = stamp; - path_msg.poses.push_back(pose_msg); + path_msg.poses.resize(latest_keyframe_index + 1); + path_msg.poses.at(latest_keyframe_index) = pose_msg; path_pub->publish(path_msg); geometry_msgs::msg::TransformStamped transform_msg; @@ -280,6 +414,51 @@ namespace urc_slam { tf_broadcaster->sendTransform(transform_msg); } + void SlamNode::rebuildMap() { + if (keyframe_stamps.empty()) { + return; + } + + accumulated_map->clear(); + path_msg.poses.clear(); + path_msg.header.frame_id = map_frame; + path_msg.header.stamp = keyframe_stamps.back().stamp; + path_msg.poses.reserve(keyframe_stamps.size()); + + for (const auto &keyframe : keyframe_stamps) { + const gtsam::Pose3 pose = backend.poseAt(keyframe.index); + + // Match the frontend's current identity base_T_lidar assumption. + LidarFrontend::Cloud transformed; + pcl::transformPointCloud( + *keyframe_clouds.at(keyframe.index), + transformed, + pose.matrix() + ); + *accumulated_map += transformed; + + geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.header.frame_id = map_frame; + pose_msg.header.stamp = keyframe.stamp; + const auto &position = pose.translation(); + const auto rotation = pose.rotation().toQuaternion(); + pose_msg.pose.position.x = position.x(); + pose_msg.pose.position.y = position.y(); + pose_msg.pose.position.z = position.z(); + pose_msg.pose.orientation.x = rotation.x(); + pose_msg.pose.orientation.y = rotation.y(); + pose_msg.pose.orientation.z = rotation.z(); + pose_msg.pose.orientation.w = rotation.w(); + path_msg.poses.push_back(pose_msg); + } + + sensor_msgs::msg::PointCloud2 map_msg; + pcl::toROSMsg(*accumulated_map, map_msg); + map_msg.header = path_msg.header; + map_pub->publish(map_msg); + path_pub->publish(path_msg); + } + void SlamNode::addKeyframeToMap( const LidarFrontend::Cloud::ConstPtr &cloud, const rclcpp::Time &stamp