diff --git a/urc_bringup/launch/sim.launch.py b/urc_bringup/launch/sim.launch.py index e68ba7eb..b8291fba 100644 --- a/urc_bringup/launch/sim.launch.py +++ b/urc_bringup/launch/sim.launch.py @@ -24,7 +24,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" @@ -197,12 +196,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") @@ -355,7 +348,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_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_localization/CMakeLists.txt b/urc_localization/CMakeLists.txt index 28dfd069..83a28112 100644 --- a/urc_localization/CMakeLists.txt +++ b/urc_localization/CMakeLists.txt @@ -25,7 +25,6 @@ include_directories( ) add_library(${PROJECT_NAME} SHARED - src/gps_imu_localizer.cpp src/covariances_on_imu.cpp src/covariances_on_gps.cpp src/ground_truth.cpp @@ -52,12 +51,6 @@ ament_target_dependencies(${PROJECT_NAME} ) -rclcpp_components_register_node( - ${PROJECT_NAME} - PLUGIN "gps_imu_localizer::GpsImuLocalizer" - EXECUTABLE ${PROJECT_NAME}_GpsImuLocalizer -) - rclcpp_components_register_node( ${PROJECT_NAME} PLUGIN "covariances_on_imu::CovariancesOnImu" diff --git a/urc_localization/include/urc_localization/gps_imu_localizer.hpp b/urc_localization/include/urc_localization/gps_imu_localizer.hpp deleted file mode 100644 index e0060463..00000000 --- a/urc_localization/include/urc_localization/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 c579821f..00000000 --- a/urc_localization/src/gps_imu_localizer.cpp +++ /dev/null @@ -1,71 +0,0 @@ -#include "urc_localization/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) { - 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/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..233bba81 --- /dev/null +++ b/urc_slam/config/slam_params.yaml @@ -0,0 +1,37 @@ +slam_node: + ros__parameters: + use_sim_time: true + imu_topic: /imu/fused + 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: 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/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 new file mode 100644 index 00000000..bc474d41 --- /dev/null +++ b/urc_slam/include/slam_backend.hpp @@ -0,0 +1,123 @@ +#ifndef SLAM_BACKEND_HPP_ +#define SLAM_BACKEND_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace urc_slam { + + class SlamBackend { + public: + // Constructor + SlamBackend(double keyframe_translation_threshold_m, + double keyframe_rotation_threshold_rad, + 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 &bias_prior_sigmas, + const gtsam::Vector6 &bias_between_sigmas + ); + + // 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( + std::size_t from_index, + 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 + // 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 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; + 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 + ) const; + + gtsam::ISAM2 isam; + gtsam::NonlinearFactorGraph new_factors; + gtsam::Values new_values; + gtsam::Values estimate; + + + gtsam::SharedNoiseModel pose_prior_noise; + gtsam::SharedNoiseModel velocity_prior_noise; + gtsam::SharedNoiseModel odom_noise; + gtsam::SharedNoiseModel lidar_noise; + gtsam::SharedNoiseModel bias_prior_noise; + gtsam::SharedNoiseModel bias_between_noise; + + boost::shared_ptr imu_params; + std::unique_ptr imu_preintegrator; + + + + + 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; + gtsam::Vector3 last_estimated_velocity; + gtsam::imuBias::ConstantBias last_estimated_bias; + + }; + + +} + + + +#endif diff --git a/urc_slam/include/slam_node.hpp b/urc_slam/include/slam_node.hpp new file mode 100644 index 00000000..a3322a37 --- /dev/null +++ b/urc_slam/include/slam_node.hpp @@ -0,0 +1,93 @@ +#ifndef SLAM_NODE_HPP_ +#define SLAM_NODE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "slam_backend.hpp" +#include "lidar_frontend.hpp" + +namespace urc_slam { + class SlamNode : public rclcpp::Node { + public: + explicit SlamNode( + const rclcpp::NodeOptions &options = rclcpp::NodeOptions() + ); + + 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; + + std::unique_ptr tf_broadcaster; + + 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; + + + 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); + }; + +} +#endif \ No newline at end of file diff --git a/urc_slam/launch/slam.launch.py b/urc_slam/launch/slam.launch.py new file mode 100644 index 00000000..10b0fc91 --- /dev/null +++ b/urc_slam/launch/slam.launch.py @@ -0,0 +1,40 @@ +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", + ) + + 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", + name="slam_node", + output="screen", + parameters=[parameters_file], + ) + + return LaunchDescription([ + slam_node, + navsat_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..54d6d01e --- /dev/null +++ b/urc_slam/package.xml @@ -0,0 +1,36 @@ + + + + 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 + robot_localization + + 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 new file mode 100644 index 00000000..76873e67 --- /dev/null +++ b/urc_slam/src/slam_backend.cpp @@ -0,0 +1,312 @@ +#include "slam_backend.hpp" + +#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 &pose_prior_sigmas, + const gtsam::Vector3 &velocity_prior_sigmas, + const gtsam::Vector6 &odom_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)), + // 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), + // for shouldCreateKeyframe + keyframe_translation_threshold_m( + keyframe_translation_threshold_m + ), + keyframe_rotation_threshold_rad( + keyframe_rotation_threshold_rad + ) + { + // 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; + + } + + + 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_navstate.pose(), + pose_prior_noise + ) + ); + + new_factors.add( + gtsam::PriorFactor( + velocityKey(0), + initial_navstate.v(), + velocity_prior_noise + ) + ); + // Add initial bias to the graph + new_factors.add( + gtsam::PriorFactor< + gtsam::imuBias::ConstantBias>( + biasKey(0), + initial_bias, + bias_prior_noise + ) + ); + + // 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_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 + ); + + } + + bool SlamBackend::shouldCreateKeyframe(const gtsam::Pose3 &measured_pose) const { + // 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 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::ImuFactor( + poseKey(previous_index), + velocityKey(previous_index), + poseKey(new_index), + velocityKey(new_index), + biasKey(previous_index), + *imu_preintegrator + ) + ); + + // Model IMU bias as slowly changing between keyframes + new_factors.add( + gtsam::BetweenFactor( + biasKey(previous_index), + biasKey(new_index), + gtsam::imuBias::ConstantBias(), + bias_between_noise + ) + ); + + // Predicted pose and velocity + new_values.insert( + poseKey(new_index), + predicted_state.pose() + ); + + 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; + + + imu_preintegrator->resetIntegrationAndSetBias( + last_estimated_bias + ); + + } + + 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, + const gtsam::Pose3 &relative_pose + ) { + // Constrain keyframe motion using ICP + new_factors.add( + gtsam::BetweenFactor( + poseKey(from_index), + poseKey(to_index), + relative_pose, + lidar_noise + ) + ); + optimize(); + + } + + void SlamBackend::optimize() { + isam.update(new_factors, new_values); + estimate = isam.calculateEstimate(); + + new_factors.resize(0); + new_values.clear(); + + last_estimated_pose = estimate.at( + poseKey(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 + ); + } + + + 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) + ); + + const gtsam::Vector3 velocity = estimate.at( + velocityKey(latest_index) + ); + + return gtsam::NavState(pose,velocity); + } + + gtsam::Symbol SlamBackend::poseKey(std::size_t index) const { + 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 + ) const { + return (a.translation() - b.translation()).norm(); + } + + double SlamBackend::rotationDistance( + const gtsam::Pose3 &a, + const gtsam::Pose3 &b + ) const { + return a.rotation().between(b.rotation()).rpy().norm(); + } +} \ 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..d6c407de --- /dev/null +++ b/urc_slam/src/slam_node.cpp @@ -0,0 +1,492 @@ +#include "slam_node.hpp" + +#include +#include + + +#include +#include + +#include +#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), + last_lidar_relative_pose(gtsam::Pose3()), + 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), + 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"); + 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"); + + 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 + ) + ); + + 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 + ); + + path_pub = create_publisher( + "/slam/path", + 10 + ); + + tf_broadcaster = std::make_unique(*this); + + 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) { + 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; + } + + const double dt = (current_stamp - previous_imu_stamp.value()).seconds(); + previous_imu_stamp = current_stamp; + + if (dt <= 0.0 || dt > 0.1) { + return; + } + + if (!previous_keyframe_cloud) { + return; + } + + backend.integrateImuMeasurement( + 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 + ); + + 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; + 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; + } + + 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( + imu_relative_guess + ); + + if (!backend.shouldCreateKeyframe(predicted_global_pose)) { + return; + } + + const LidarRegistrationResult registration = lidar_frontend.registerClouds( + previous_keyframe_cloud, + current_cloud, + last_lidar_relative_pose + ); + + 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; + } + + 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; + + // 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; + 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(); + 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.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; + 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); + } + + 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 + ) { + 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 +RCLCPP_COMPONENTS_REGISTER_NODE(urc_slam::SlamNode) \ No newline at end of file