diff --git a/mission/landmark_server/config/landmark_server_config.yaml b/mission/landmark_server/config/landmark_server_config.yaml index 9128974cd..09f4d414b 100644 --- a/mission/landmark_server/config/landmark_server_config.yaml +++ b/mission/landmark_server/config/landmark_server_config.yaml @@ -1,8 +1,7 @@ /**: ros__parameters: - target_frame: "odom" + target_frame: "orca/odom" timer_rate_ms: 200 - enu_ned_rotation: true track_config: default: diff --git a/mission/landmark_server/include/landmark_server/landmark_server_ros.hpp b/mission/landmark_server/include/landmark_server/landmark_server_ros.hpp index c714a5327..13ac5733a 100644 --- a/mission/landmark_server/include/landmark_server/landmark_server_ros.hpp +++ b/mission/landmark_server/include/landmark_server/landmark_server_ros.hpp @@ -170,8 +170,6 @@ class LandmarkServerNode : public rclcpp::Node { std::shared_ptr tf2_buffer_; std::shared_ptr tf2_listener_; - bool enu_ned_rotation_{false}; - std::shared_ptr active_landmark_polling_goal_; std::shared_ptr active_landmark_convergence_goal_; diff --git a/mission/landmark_server/src/landmark_server_convergence.cpp b/mission/landmark_server/src/landmark_server_convergence.cpp index f26492fa5..329850269 100644 --- a/mission/landmark_server/src/landmark_server_convergence.cpp +++ b/mission/landmark_server/src/landmark_server_convergence.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include #include #include "landmark_server/landmark_server_ros.hpp" @@ -464,27 +465,13 @@ geometry_msgs::msg::Pose LandmarkServerNode::compute_target_pose( const vortex::filtering::Track& track, const geometry_msgs::msg::Pose& convergence_offset) { const auto landmark_pose = track.to_pose(); - - const Eigen::Vector3d p_landmark = landmark_pose.pos_vector(); - const Eigen::Quaterniond q_landmark = - landmark_pose.ori_quaternion().normalized(); - - const Eigen::Vector3d p_offset(convergence_offset.position.x, - convergence_offset.position.y, - convergence_offset.position.z); - - const Eigen::Quaterniond q_offset = - Eigen::Quaterniond( - convergence_offset.orientation.w, convergence_offset.orientation.x, - convergence_offset.orientation.y, convergence_offset.orientation.z) - .normalized(); - - const Eigen::Vector3d p_target = p_landmark + p_offset; - - const Eigen::Quaterniond q_target = (q_landmark * q_offset).normalized(); - - return vortex::utils::ros_conversions::eigen_to_pose_msg(p_target, - q_target); + const auto base = vortex::utils::types::Pose::from_eigen( + landmark_pose.pos_vector(), landmark_pose.ori_quaternion()); + const auto offset = + vortex::utils::ros_conversions::ros_pose_to_pose(convergence_offset); + const auto target = + vortex::utils::waypoints::apply_pose_offset(base, offset); + return vortex::utils::ros_conversions::to_pose_msg(target); } } // namespace vortex::mission diff --git a/mission/landmark_server/src/landmark_server_ros.cpp b/mission/landmark_server/src/landmark_server_ros.cpp index 345c61f6a..9c42152ae 100644 --- a/mission/landmark_server/src/landmark_server_ros.cpp +++ b/mission/landmark_server/src/landmark_server_ros.cpp @@ -65,7 +65,6 @@ void LandmarkServerNode::create_reference_publisher() { void LandmarkServerNode::create_pose_subscription() { std::string landmark_topic = this->declare_parameter("topics.landmarks"); - enu_ned_rotation_ = this->declare_parameter("enu_ned_rotation"); target_frame_ = this->declare_parameter("target_frame"); auto qos_sensor_profile = vortex::utils::qos_profiles::sensor_data_profile(1); @@ -98,12 +97,6 @@ void LandmarkServerNode::create_pose_subscription() { } auto new_measurements = ros_msg_to_landmarks(pose_tf); - if (enu_ned_rotation_) { - std::ranges::for_each(new_measurements, [](auto& m) { - m.pose.set_ori(vortex::utils::math::enu_ned_rotation( - m.pose.ori_quaternion())); - }); - } { std::lock_guard lock(measurements_mtx_); measurements_ = std::move(new_measurements); @@ -273,6 +266,15 @@ void LandmarkServerNode::timer_callback() { convergence_update(); + if (active_landmark_polling_goal_ && + active_landmark_polling_goal_->is_canceling()) { + auto result = + std::make_shared(); + active_landmark_polling_goal_->canceled(result); + active_landmark_polling_goal_ = nullptr; + return; + } + if (active_landmark_polling_goal_ && active_landmark_polling_goal_->is_active()) { const auto goal = active_landmark_polling_goal_->get_goal(); @@ -294,6 +296,9 @@ void LandmarkServerNode::timer_callback() { if (!found) { return; } + spdlog::info( + "LandmarkPolling: found landmark(s) for type={}, subtype={}", type, + subtype); vortex_msgs::msg::LandmarkArray landmarks = tracks_to_landmark_msgs(type, subtype); auto polling_result = diff --git a/navigation/eskf/CMakeLists.txt b/navigation/eskf/CMakeLists.txt index b292b256d..cde391fd9 100644 --- a/navigation/eskf/CMakeLists.txt +++ b/navigation/eskf/CMakeLists.txt @@ -36,39 +36,65 @@ include_directories(${EIGEN3_INCLUDE_DIR}) include_directories(include) -set(LIB_NAME "${PROJECT_NAME}_component") +set(CORE_LIB_NAME "${PROJECT_NAME}") -add_library(${LIB_NAME} SHARED - src/eskf.cpp - src/eskf_ros.cpp +add_library(${CORE_LIB_NAME} SHARED + src/lib/eskf.cpp ) -ament_target_dependencies(${LIB_NAME} PUBLIC +target_include_directories(${CORE_LIB_NAME} PUBLIC + $ + $ +) + +ament_target_dependencies(${CORE_LIB_NAME} + Eigen3 + vortex_utils +) + +add_library(${PROJECT_NAME}::${CORE_LIB_NAME} ALIAS ${CORE_LIB_NAME}) + +set(COMPONENT_LIB_NAME "${PROJECT_NAME}_component") + +set(LANDMARK_DIR "${CMAKE_CURRENT_SOURCE_DIR}/../landmark_egomotion") + +add_library(${COMPONENT_LIB_NAME} SHARED + src/ros/eskf_ros.cpp + ${LANDMARK_DIR}/src/lib/landmark_egomotion.cpp +) + +target_include_directories(${COMPONENT_LIB_NAME} PRIVATE + ${LANDMARK_DIR}/include +) + +ament_target_dependencies(${COMPONENT_LIB_NAME} rclcpp rclcpp_components + Eigen3 geometry_msgs nav_msgs - Eigen3 tf2 + tf2_ros + tf2_eigen vortex_msgs vortex_utils vortex_utils_ros spdlog fmt - tf2_ros - tf2_eigen ) +target_link_libraries(${COMPONENT_LIB_NAME} ${PROJECT_NAME}) + rclcpp_components_register_node( - ${LIB_NAME} + ${COMPONENT_LIB_NAME} PLUGIN "ESKFNode" EXECUTABLE ${PROJECT_NAME}_node ) -ament_export_targets(export_${LIB_NAME}) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -install(TARGETS ${LIB_NAME} - EXPORT export_${LIB_NAME} +install(TARGETS ${CORE_LIB_NAME} ${COMPONENT_LIB_NAME} + EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -79,10 +105,22 @@ install( DESTINATION include ) +install( + DIRECTORY ${LANDMARK_DIR}/include/ + DESTINATION include +) + install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}/ ) +ament_export_dependencies( + Eigen3 + vortex_utils +) +ament_export_include_directories(include) +ament_export_libraries(${CORE_LIB_NAME}) + ament_package() diff --git a/navigation/eskf/config/eskf_params.yaml b/navigation/eskf/config/eskf_params.yaml index 001ede019..fda0e3214 100644 --- a/navigation/eskf/config/eskf_params.yaml +++ b/navigation/eskf/config/eskf_params.yaml @@ -1,6 +1,8 @@ /**: eskf_node: ros__parameters: + dvl_topic: /dvl/sim + odom_topic: odom_ESKF diag_Q_std: [0.05, 0.05, 0.1, 0.01, 0.01, 0.02, 0.001, 0.001, 0.001, 0.0001, 0.0001, 0.0001] diag_p_init: [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001] transform: @@ -30,3 +32,29 @@ atmospheric_pressure: 101000.0 # [Pa] water_density: 1026.0 # [kg/m3] gravity: 9.82841 + + use_landmark_egomotion: true + vo: + landmarks_topic: /aruco_detector/landmarks + + base_frame: orca/base_link + camera_frame: orca/front_camera_optical + + nis_gate_pose: 16.812 + nis_gate_velocity: 11.345 + disable_gating: true + dropout_timeout: 10.0 + rejects_limit: 5 + + pos_floor: 0.05 + att_floor: 0.02 + vel_floor: 0.2 + dt_min: 0.02 + dt_max: 15.0 + + # Sliding-window smoother + use_sw: true + window_size: 20 + max_age: 15.0 + huber_deg: 20.0 + gate_deg: 60.0 diff --git a/navigation/eskf/config/eskf_params_real_world.yaml b/navigation/eskf/config/eskf_params_real_world.yaml index 0f82da7d7..688c2b287 100644 --- a/navigation/eskf/config/eskf_params_real_world.yaml +++ b/navigation/eskf/config/eskf_params_real_world.yaml @@ -31,6 +31,32 @@ water_density: 1026.0 # [kg/m3] gravity: 9.82841 + use_landmark_egomotion: true + vo: + landmarks_topic: /aruco_detector/landmarks + + base_frame: nautilus/base_link + camera_frame: "/cam/camera_info" + + nis_gate_pose: 16.812 + nis_gate_velocity: 11.345 + disable_gating: true + dropout_timeout: 10.0 + rejects_limit: 5 + + pos_floor: 0.1 + att_floor: 0.05 + vel_floor: 0.4 + dt_min: 0.02 + dt_max: 15.0 + + # Sliding-window smoother + use_sw: true + window_size: 20 + max_age: 15.0 + huber_deg: 20.0 + gate_deg: 60.0 + # USED SENSORS # IMU: Kongsberg mini MRU # DVL & Depth: Nucleus 1000 diff --git a/navigation/eskf/include/eskf/eskf.hpp b/navigation/eskf/include/eskf/lib/eskf.hpp similarity index 95% rename from navigation/eskf/include/eskf/eskf.hpp rename to navigation/eskf/include/eskf/lib/eskf.hpp index 96d354bdc..91c644fdd 100644 --- a/navigation/eskf/include/eskf/eskf.hpp +++ b/navigation/eskf/include/eskf/lib/eskf.hpp @@ -1,14 +1,15 @@ -#ifndef ESKF__ESKF_HPP_ -#define ESKF__ESKF_HPP_ +#ifndef ESKF__LIB__ESKF_HPP_ +#define ESKF__LIB__ESKF_HPP_ #include #include -#include "eskf/typedefs.hpp" +#include "eskf/lib/typedefs.hpp" #include "typedefs.hpp" class ESKF { public: explicit ESKF(const EskfParams& params); + virtual ~ESKF() = default; // @brief Update the nominal state and error state // @param imu_meas: IMU measurement @@ -29,6 +30,16 @@ class ESKF { inline Eigen::Vector3d get_gravity() const { return g_; } + protected: + // @brief Inject the error state into the nominal state and reset the error + void injection_and_reset(); + + // Member variable for the current error state + StateEuler current_error_state_{}; + + // Member variable for the current nominal state + StateQuat current_nom_state_{}; + private: // @brief Predict the nominal state // @param imu_meas: IMU measurement @@ -56,9 +67,6 @@ class ESKF { template void measurement_update(const SensorT& meas); - // @brief Inject the error state into the nominal state and reset the error - void injection_and_reset(); - // @brief Van Loan discretization // @param A_c: Continuous state transition matrix // @param G_c: Continuous input matrix @@ -74,12 +82,6 @@ class ESKF { // Normalized Innovation Squared double nis_{}; - // Member variable for the current error state - StateEuler current_error_state_{}; - - // Member variable for the current nominal state - StateQuat current_nom_state_{}; - // gravity Eigen::Vector3d g_{0.0, 0.0, -9.82841}; @@ -107,4 +109,4 @@ double compute_nis(const Eigen::VectorXd& innovation, const Eigen::MatrixXd& S); #include "eskf.tpp" // including template implementation -#endif // ESKF__ESKF_HPP_ +#endif // ESKF__LIB__ESKF_HPP_ diff --git a/navigation/eskf/include/eskf/eskf.tpp b/navigation/eskf/include/eskf/lib/eskf.tpp similarity index 100% rename from navigation/eskf/include/eskf/eskf.tpp rename to navigation/eskf/include/eskf/lib/eskf.tpp diff --git a/navigation/eskf/include/eskf/typedefs.hpp b/navigation/eskf/include/eskf/lib/typedefs.hpp similarity index 97% rename from navigation/eskf/include/eskf/typedefs.hpp rename to navigation/eskf/include/eskf/lib/typedefs.hpp index ee04e8911..47947b865 100644 --- a/navigation/eskf/include/eskf/typedefs.hpp +++ b/navigation/eskf/include/eskf/lib/typedefs.hpp @@ -2,8 +2,8 @@ * @file typedefs.hpp * @brief Contains the typedef and structs for the eskf. */ -#ifndef ESKF__TYPEDEFS_HPP_ -#define ESKF__TYPEDEFS_HPP_ +#ifndef ESKF__LIB__TYPEDEFS_HPP_ +#define ESKF__LIB__TYPEDEFS_HPP_ #include #include @@ -135,4 +135,4 @@ struct SensorDepth { Eigen::MatrixXd noise_covariance() const; }; -#endif // ESKF__TYPEDEFS_HPP_ +#endif // ESKF__LIB__TYPEDEFS_HPP_ diff --git a/navigation/eskf/include/eskf/eskf_ros.hpp b/navigation/eskf/include/eskf/ros/eskf_ros.hpp similarity index 82% rename from navigation/eskf/include/eskf/eskf_ros.hpp rename to navigation/eskf/include/eskf/ros/eskf_ros.hpp index 03d4c7266..3f1da2aad 100644 --- a/navigation/eskf/include/eskf/eskf_ros.hpp +++ b/navigation/eskf/include/eskf/ros/eskf_ros.hpp @@ -1,10 +1,12 @@ -#ifndef ESKF__ESKF_ROS_HPP_ -#define ESKF__ESKF_ROS_HPP_ +#ifndef ESKF__ROS__ESKF_ROS_HPP_ +#define ESKF__ROS__ESKF_ROS_HPP_ #include #include #include +#include #include +#include #include #include #include @@ -20,10 +22,14 @@ #include #include #include -#include "eskf/eskf.hpp" -#include "eskf/typedefs.hpp" +#include "eskf/lib/eskf.hpp" +#include "eskf/lib/typedefs.hpp" #include "spdlog/spdlog.h" +#include +#include "landmark_egomotion/lib/landmark_egomotion.hpp" +#include "landmark_egomotion/lib/landmark_typedefs.hpp" + class ESKFNode : public rclcpp::Node { public: explicit ESKFNode( @@ -57,6 +63,13 @@ class ESKFNode : public rclcpp::Node { // transforms are available (or immediately if use_tf_transforms_ is false). void complete_initialization(); + // @brief Set up visual odometry/landmark egomotion + void setup_vo(const EskfParams& eskf_params); + + // @brief Callback function for the landmark topic + void landmark_callback( + const vortex_msgs::msg::LandmarkArray::SharedPtr msg); + // @brief broadcast the State as a TF void publish_tf(const StateQuat& nom_state, const rclcpp::Time& current_time); @@ -96,6 +109,18 @@ class ESKFNode : public rclcpp::Node { std::unique_ptr eskf_; + // Non-owning pointer to LandmarkESKF + LandmarkESKF* landmark_eskf_{nullptr}; + + rclcpp::Subscription::SharedPtr + landmark_sub_; + + std::string vo_base_frame_{}; + std::string vo_cam_frame_{}; + int vo_rejects_limit_{0}; + uint16_t last_marker_id_{0}; + bool have_last_marker_{false}; + bool first_imu_msg_received_ = false; Eigen::Matrix3d R_imu_eskf_{}; @@ -142,4 +167,4 @@ class ESKFNode : public rclcpp::Node { double atmospheric_pressure; }; -#endif // ESKF__ESKF_ROS_HPP_ +#endif // ESKF__ROS__ESKF_ROS_HPP_ diff --git a/navigation/eskf/src/eskf.cpp b/navigation/eskf/src/lib/eskf.cpp similarity index 99% rename from navigation/eskf/src/eskf.cpp rename to navigation/eskf/src/lib/eskf.cpp index 3db940929..68cf83b1a 100644 --- a/navigation/eskf/src/eskf.cpp +++ b/navigation/eskf/src/lib/eskf.cpp @@ -1,9 +1,9 @@ -#include "eskf/eskf.hpp" +#include "eskf/lib/eskf.hpp" #include #include #include #include -#include "eskf/typedefs.hpp" +#include "eskf/lib/typedefs.hpp" double compute_nis(const Eigen::VectorXd& innovation, const Eigen::MatrixXd& S) { diff --git a/navigation/eskf/src/eskf_ros.cpp b/navigation/eskf/src/ros/eskf_ros.cpp similarity index 72% rename from navigation/eskf/src/eskf_ros.cpp rename to navigation/eskf/src/ros/eskf_ros.cpp index ed70aee78..8f57d7fe6 100644 --- a/navigation/eskf/src/eskf_ros.cpp +++ b/navigation/eskf/src/ros/eskf_ros.cpp @@ -1,8 +1,8 @@ -#include "eskf/eskf_ros.hpp" +#include "eskf/ros/eskf_ros.hpp" #include #include #include -#include "eskf/typedefs.hpp" +#include "eskf/lib/typedefs.hpp" auto start_message{R"( ________ ______ ___ ____ ________ @@ -200,6 +200,73 @@ void ESKFNode::set_parameters() { add_gravity_to_imu_ = this->declare_parameter("add_gravity_to_imu"); spdlog::info("add_gravity_to_imu: {}", add_gravity_to_imu_); + + this->declare_parameter("use_landmark_egomotion"); + if (this->get_parameter("use_landmark_egomotion").as_bool()) { + setup_vo(eskf_params); + } +} + +void ESKFNode::setup_vo(const EskfParams& eskf_params) { + auto landmark_eskf = std::make_unique(eskf_params); + landmark_eskf_ = landmark_eskf.get(); + eskf_ = std::move(landmark_eskf); + + this->declare_parameter("vo.landmarks_topic"); + this->declare_parameter("vo.base_frame"); + this->declare_parameter("vo.camera_frame"); + this->declare_parameter("vo.disable_gating"); + + this->declare_parameter("vo.nis_gate_pose"); + this->declare_parameter("vo.nis_gate_velocity"); + this->declare_parameter("vo.dropout_timeout"); + this->declare_parameter("vo.rejects_limit"); + this->declare_parameter("vo.pos_floor"); + this->declare_parameter("vo.att_floor"); + this->declare_parameter("vo.vel_floor"); + this->declare_parameter("vo.dt_min"); + this->declare_parameter("vo.dt_max"); + this->declare_parameter("vo.use_sw"); + this->declare_parameter("vo.window_size"); + this->declare_parameter("vo.max_age"); + this->declare_parameter("vo.huber_deg"); + this->declare_parameter("vo.gate_deg"); + + vo_base_frame_ = this->get_parameter("vo.base_frame").as_string(); + vo_cam_frame_ = this->get_parameter("vo.camera_frame").as_string(); + + VoConfig vo_cfg; + vo_cfg.nis_gate_pose = this->get_parameter("vo.nis_gate_pose").as_double(); + vo_cfg.nis_gate_vel = + this->get_parameter("vo.nis_gate_velocity").as_double(); + vo_cfg.dropout_timeout = + this->get_parameter("vo.dropout_timeout").as_double(); + vo_cfg.rejects_limit = this->get_parameter("vo.rejects_limit").as_int(); + vo_cfg.pos_floor = this->get_parameter("vo.pos_floor").as_double(); + vo_cfg.att_floor = this->get_parameter("vo.att_floor").as_double(); + vo_cfg.vel_floor = this->get_parameter("vo.vel_floor").as_double(); + vo_cfg.dt_min = this->get_parameter("vo.dt_min").as_double(); + vo_cfg.dt_max = this->get_parameter("vo.dt_max").as_double(); + vo_cfg.use_sw = this->get_parameter("vo.use_sw").as_bool(); + vo_cfg.sw_window_size = this->get_parameter("vo.window_size").as_int(); + vo_cfg.sw_max_age = this->get_parameter("vo.max_age").as_double(); + vo_cfg.sw_huber_deg = this->get_parameter("vo.huber_deg").as_double(); + vo_cfg.sw_gate_deg = this->get_parameter("vo.gate_deg").as_double(); + + landmark_eskf_->set_vo_enabled(true); + landmark_eskf_->set_nis_gating_enabled( + !this->get_parameter("vo.disable_gating").as_bool()); + landmark_eskf_->set_vo_config(vo_cfg); + vo_rejects_limit_ = vo_cfg.rejects_limit; + + std::string landmarks_topic = + this->get_parameter("vo.landmarks_topic").as_string(); + landmark_sub_ = this->create_subscription( + landmarks_topic, rclcpp::SensorDataQoS(), + std::bind(&ESKFNode::landmark_callback, this, std::placeholders::_1)); + + spdlog::info("Landmark egomotion enabled: {} -> {}", vo_cam_frame_, + vo_base_frame_); } void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { @@ -306,6 +373,91 @@ void ESKFNode::depth_callback( #endif } +void ESKFNode::landmark_callback( + const vortex_msgs::msg::LandmarkArray::SharedPtr msg) { + if (!msg || msg->landmarks.empty()) + return; + + std::vector markers; + for (const auto& lm : msg->landmarks) { + if (lm.type.value == vortex_msgs::msg::LandmarkType::ARUCO_MARKER) { + markers.push_back(&lm); + } + } + if (markers.empty()) + return; + + auto it = std::min_element( + markers.begin(), markers.end(), [](const auto* a, const auto* b) { + const double da = Eigen::Vector3d(a->pose.pose.position.x, + a->pose.pose.position.y, + a->pose.pose.position.z) + .norm(); + const double db = Eigen::Vector3d(b->pose.pose.position.x, + b->pose.pose.position.y, + b->pose.pose.position.z) + .norm(); + return da < db; + }); + const auto* chosen = *it; + uint16_t chosen_id = chosen->subtype.value; + spdlog::debug("Received {} markers, chosen id: {}", markers.size(), + chosen_id); + + geometry_msgs::msg::TransformStamped tf_msg; + try { + tf_msg = tf_buffer_->lookupTransform(vo_base_frame_, vo_cam_frame_, + tf2::TimePointZero); + } catch (const tf2::TransformException& ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, + "TF %s <- %s: %s", vo_base_frame_.c_str(), + vo_cam_frame_.c_str(), ex.what()); + return; + } + + Eigen::Vector3d t_base_cam(tf_msg.transform.translation.x, + tf_msg.transform.translation.y, + tf_msg.transform.translation.z); + Eigen::Quaterniond q_base_cam( + tf_msg.transform.rotation.w, tf_msg.transform.rotation.x, + tf_msg.transform.rotation.y, tf_msg.transform.rotation.z); + + Eigen::Vector3d t_cam_marker(chosen->pose.pose.position.x, + chosen->pose.pose.position.y, + chosen->pose.pose.position.z); + Eigen::Quaterniond q_cam_marker( + chosen->pose.pose.orientation.w, chosen->pose.pose.orientation.x, + chosen->pose.pose.orientation.y, chosen->pose.pose.orientation.z); + + Eigen::Vector3d t_base = t_base_cam + q_base_cam * t_cam_marker; + Eigen::Quaterniond q_base = (q_base_cam * q_cam_marker).normalized(); + + if (have_last_marker_ && chosen_id != last_marker_id_) { + spdlog::info("Marker switch {} -> {}", last_marker_id_, chosen_id); + landmark_eskf_->handle_marker_switch(t_base, q_base); + } + last_marker_id_ = chosen_id; + have_last_marker_ = true; + + LandmarkMeasurement meas; + meas.pos = t_base; + meas.quat = q_base; + meas.stamp = rclcpp::Time(msg->header.stamp).seconds(); + meas.R = Eigen::Map>( + chosen->pose.covariance.data()); + + const int rejects_before = landmark_eskf_->get_consecutive_rejects(); + landmark_eskf_->landmark_update(meas); + const int rejects_after = landmark_eskf_->get_consecutive_rejects(); + + if (rejects_before >= vo_rejects_limit_ && rejects_after == 0) { + spdlog::warn("VO anchor reset after {} consecutive rejects", + rejects_before); + } else if (rejects_after > 0) { + spdlog::warn("VO gating: {} consecutive reject(s)", rejects_after); + } +} + void ESKFNode::publish_odom() { nav_msgs::msg::Odometry odom_msg; StateQuat nom_state = eskf_->get_nominal_state(); diff --git a/navigation/landmark_egomotion/include/landmark_egomotion/lib/landmark_egomotion.hpp b/navigation/landmark_egomotion/include/landmark_egomotion/lib/landmark_egomotion.hpp new file mode 100644 index 000000000..8c868c6dc --- /dev/null +++ b/navigation/landmark_egomotion/include/landmark_egomotion/lib/landmark_egomotion.hpp @@ -0,0 +1,69 @@ +#ifndef LANDMARK_EGOMOTION__LIB__LANDMARK_EGOMOTION_HPP_ +#define LANDMARK_EGOMOTION__LIB__LANDMARK_EGOMOTION_HPP_ + +#include +#include +#include "landmark_typedefs.hpp" + +class LandmarkESKF : public ESKF { + public: + explicit LandmarkESKF(const EskfParams& params); + ~LandmarkESKF() override = default; + + void landmark_update(const LandmarkMeasurement& landmark_meas); + void set_vo_config(const VoConfig& cfg); + void set_vo_enabled(bool enabled); + void set_nis_gating_enabled(bool enabled); + void force_anchor_reset(); + void handle_marker_switch(const Eigen::Vector3d& pos, + const Eigen::Quaterniond& quat); + int get_consecutive_rejects() const; + bool is_anchor_valid() const; + + private: + void landmark_vel_update_(const Eigen::Vector3d& v_meas_nav, + const Eigen::Matrix3d& Rv); + + static Eigen::Vector3d compute_quaternion_error( + const Eigen::Quaterniond& q_meas, + const Eigen::Quaterniond& q_est); + static Eigen::Vector3d log_quat(const Eigen::Quaterniond& q); + static Eigen::Quaterniond exp_rotvec(const Eigen::Vector3d& w); + static double angle_dist(const Eigen::Quaterniond& a, + const Eigen::Quaterniond& b); + + static double compute_nis(const Eigen::VectorXd& innovation, + const Eigen::MatrixXd& S); + + // Sliding-window SO(3) smoother + struct Sample { + double stamp; + Eigen::Vector3d pos; + Eigen::Quaterniond quat; + }; + std::deque sw_buf_; + + void sw_add(double stamp, + const Eigen::Vector3d& p, + const Eigen::Quaterniond& q); + void sw_prune(double now); + bool sw_estimate(Eigen::Vector3d& p_out, Eigen::Quaterniond& q_out); + + bool use_vo_; + bool gating_enabled_; + + VoConfig vo_cfg_; + + bool have_anchor_; + Eigen::Quaterniond q_nav_vo_; + Eigen::Vector3d p_nav_vo_; + + double last_stamp_; + int consecutive_rejects_; + + bool have_prev_; + double prev_stamp_; + Eigen::Vector3d prev_p_nav_; +}; + +#endif // LANDMARK_EGOMOTION__LIB__LANDMARK_EGOMOTION_HPP_ diff --git a/navigation/landmark_egomotion/include/landmark_egomotion/lib/landmark_typedefs.hpp b/navigation/landmark_egomotion/include/landmark_egomotion/lib/landmark_typedefs.hpp new file mode 100644 index 000000000..ff30450e6 --- /dev/null +++ b/navigation/landmark_egomotion/include/landmark_egomotion/lib/landmark_typedefs.hpp @@ -0,0 +1,37 @@ +#ifndef LANDMARK_EGOMOTION__LIB__LANDMARK_TYPEDEFS_HPP_ +#define LANDMARK_EGOMOTION__LIB__LANDMARK_TYPEDEFS_HPP_ + +#include +#include + +struct LandmarkMeasurement { + Eigen::Vector3d pos; + Eigen::Quaterniond quat; + Eigen::Matrix R; + double stamp = 0.0; +}; + +struct VoConfig { + double nis_gate_pose; + double nis_gate_vel; + double dropout_timeout; + double pos_floor; + double att_floor; + double vel_floor; + double dt_min; + double dt_max; + double sw_max_age; + double sw_huber_deg; + double sw_gate_deg; + int sw_window_size; + int rejects_limit; + bool use_sw; +}; + +namespace Eigen { +typedef Matrix Vector6d; +typedef Matrix Matrix15x6d; +typedef Matrix Matrix6x15d; +} // namespace Eigen + +#endif // LANDMARK_EGOMOTION__LIB__LANDMARK_TYPEDEFS_HPP_ diff --git a/navigation/landmark_egomotion/src/lib/landmark_egomotion.cpp b/navigation/landmark_egomotion/src/lib/landmark_egomotion.cpp new file mode 100644 index 000000000..9c57f53dd --- /dev/null +++ b/navigation/landmark_egomotion/src/lib/landmark_egomotion.cpp @@ -0,0 +1,321 @@ +#include "landmark_egomotion/lib/landmark_egomotion.hpp" +#include +#include + +LandmarkESKF::LandmarkESKF(const EskfParams& params) : ESKF(params) { + use_vo_ = false; + gating_enabled_ = true; + consecutive_rejects_ = 0; + + have_anchor_ = false; + q_nav_vo_ = Eigen::Quaterniond::Identity(); + p_nav_vo_ = Eigen::Vector3d::Zero(); + + last_stamp_ = 0.0; + have_prev_ = false; + prev_stamp_ = 0.0; + prev_p_nav_ = Eigen::Vector3d::Zero(); + + vo_cfg_ = {}; +} + +Eigen::Vector3d LandmarkESKF::compute_quaternion_error( + const Eigen::Quaterniond& q_meas, + const Eigen::Quaterniond& q_est) { + Eigen::Quaterniond q_err = (q_est.inverse() * q_meas).normalized(); + + if (q_err.w() < 0.0) { + q_err.coeffs() = -q_err.coeffs(); + } + + const double w = q_err.w(); + const Eigen::Vector3d v(q_err.x(), q_err.y(), q_err.z()); + const double v_norm = v.norm(); + + if (v_norm < 1e-10) { + return 2.0 * v; + } + + const double theta = 2.0 * std::atan2(v_norm, w); + + return (theta / v_norm) * v; +} + +Eigen::Vector3d LandmarkESKF::log_quat(const Eigen::Quaterniond& q) { + const double w = std::clamp(static_cast(q.w()), -1.0, 1.0); + const double theta = 2.0 * std::acos(w); + const double s = std::sqrt(std::max(0.0, 1.0 - w * w)); + + if (s < 1e-9) { + return Eigen::Vector3d(2 * q.x(), 2 * q.y(), 2 * q.z()); + } + + return Eigen::Vector3d(q.x(), q.y(), q.z()) * (theta / s); +} + +Eigen::Quaterniond LandmarkESKF::exp_rotvec(const Eigen::Vector3d& w) { + const double theta = w.norm(); + if (theta < 1e-9) { + return Eigen::Quaterniond(1.0, 0.5 * w.x(), 0.5 * w.y(), 0.5 * w.z()) + .normalized(); + } + const Eigen::Vector3d a = w / theta; + const double s = std::sin(0.5 * theta); + + return Eigen::Quaterniond(std::cos(0.5 * theta), a.x() * s, a.y() * s, + a.z() * s); +} + +double LandmarkESKF::angle_dist(const Eigen::Quaterniond& a, + const Eigen::Quaterniond& b) { + const double d = std::abs(a.dot(b)); + + return 2.0 * std::acos(std::clamp(d, -1.0, 1.0)); +} + +double LandmarkESKF::compute_nis(const Eigen::VectorXd& innovation, + const Eigen::MatrixXd& S) { + return innovation.transpose() * S.inverse() * innovation; +} + +void LandmarkESKF::sw_add(double stamp, + const Eigen::Vector3d& p, + const Eigen::Quaterniond& q) { + sw_buf_.push_back({stamp, p, q}); +} + +void LandmarkESKF::sw_prune(double now) { + while (!sw_buf_.empty() && + (now - sw_buf_.front().stamp) > vo_cfg_.sw_max_age) { + sw_buf_.pop_front(); + } + while (static_cast(sw_buf_.size()) > vo_cfg_.sw_window_size) { + sw_buf_.pop_front(); + } +} + +bool LandmarkESKF::sw_estimate(Eigen::Vector3d& p_out, + Eigen::Quaterniond& q_out) { + if (sw_buf_.empty()) + return false; + + q_out = sw_buf_.back().quat; + p_out = Eigen::Vector3d::Zero(); + + const double gate = vo_cfg_.sw_gate_deg * M_PI / 180.0; + const double huber = vo_cfg_.sw_huber_deg * M_PI / 180.0; + + for (int iter = 0; iter < 5; ++iter) { + Eigen::Vector3d sum_p = Eigen::Vector3d::Zero(); + Eigen::Vector3d sum_w = Eigen::Vector3d::Zero(); + double w_total = 0.0; + + for (const auto& s : sw_buf_) { + const double dist = angle_dist(q_out, s.quat); + if (dist > gate) + continue; + + const Eigen::Vector3d w_i = log_quat(q_out.inverse() * s.quat); + + double weight = 1.0; + if (huber > 0.0) { + const double a = w_i.norm(); + if (a > huber && a > 1e-12) { + weight = huber / a; + } + } + + sum_p += s.pos * weight; + sum_w += w_i * weight; + w_total += weight; + } + + if (w_total < 1e-9) + return false; + + p_out = sum_p / w_total; + + const Eigen::Vector3d dw = sum_w / w_total; + q_out = (q_out * exp_rotvec(dw)).normalized(); + + if (dw.norm() < 1e-4) + break; + } + + return true; +} + +void LandmarkESKF::landmark_vel_update_(const Eigen::Vector3d& v_meas_nav, + const Eigen::Matrix3d& Rv) { + Eigen::Matrix H = Eigen::Matrix::Zero(); + H.block<3, 3>(0, 3) = Eigen::Matrix3d::Identity(); + + const Eigen::Matrix15d P = current_error_state_.covariance; + const Eigen::Vector3d y = v_meas_nav - current_nom_state_.vel; + + const Eigen::Matrix3d S = H * P * H.transpose() + Rv; + const double nis = compute_nis(y, S); + + const double gate = !gating_enabled_ ? 1e9 : vo_cfg_.nis_gate_vel; + if (nis > gate) { + return; + } + + Eigen::Matrix K = P * H.transpose() * S.inverse(); + + current_error_state_.set_from_vector(K * y); + + Eigen::Matrix15d I_KH = Eigen::Matrix15d::Identity() - K * H; + current_error_state_.covariance = + I_KH * P * I_KH.transpose() + K * Rv * K.transpose(); + + injection_and_reset(); +} + +void LandmarkESKF::landmark_update(const LandmarkMeasurement& landmark_meas) { + if (!use_vo_) { + return; + } + + if (last_stamp_ > 0.0) { + const double gap = landmark_meas.stamp - last_stamp_; + if (gap > vo_cfg_.dropout_timeout) { + have_anchor_ = false; + have_prev_ = false; + consecutive_rejects_ = 0; + sw_buf_.clear(); + } + } + last_stamp_ = landmark_meas.stamp; + + const bool need_reset = (consecutive_rejects_ >= vo_cfg_.rejects_limit); + + if (!have_anchor_ || need_reset) { + q_nav_vo_ = (current_nom_state_.quat * landmark_meas.quat.inverse()) + .normalized(); + p_nav_vo_ = current_nom_state_.pos - q_nav_vo_ * landmark_meas.pos; + + have_anchor_ = true; + have_prev_ = false; + consecutive_rejects_ = 0; + sw_buf_.clear(); + return; + } + + // Transform raw VO measurement to navigation frame + Eigen::Vector3d p_nav = p_nav_vo_ + q_nav_vo_ * landmark_meas.pos; + Eigen::Quaterniond q_nav = (q_nav_vo_ * landmark_meas.quat).normalized(); + + // Measurement Jacobian + Eigen::Matrix H = Eigen::Matrix::Zero(); + H.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); + H.block<3, 3>(3, 6) = Eigen::Matrix3d::Identity(); + + // Measurement noise with floors applied + Eigen::Matrix R = landmark_meas.R; + const double p_floor_sq = vo_cfg_.pos_floor * vo_cfg_.pos_floor; + const double a_floor_sq = vo_cfg_.att_floor * vo_cfg_.att_floor; + for (int i = 0; i < 3; ++i) { + R(i, i) = std::max(R(i, i), p_floor_sq); + } + for (int i = 3; i < 6; ++i) { + R(i, i) = std::max(R(i, i), a_floor_sq); + } + + // Gate on raw measurement + const Eigen::Matrix15d P = current_error_state_.covariance; + const Eigen::Matrix S = H * P * H.transpose() + R; + + Eigen::Matrix y_raw; + y_raw.head<3>() = p_nav - current_nom_state_.pos; + y_raw.segment<3>(3) = + compute_quaternion_error(q_nav, current_nom_state_.quat); + + const double gate = !gating_enabled_ ? 1e9 : vo_cfg_.nis_gate_pose; + if (compute_nis(y_raw, S) > gate) { + consecutive_rejects_++; + return; + } + + // Measurement passed gating + if (vo_cfg_.use_sw) { + sw_add(landmark_meas.stamp, p_nav, q_nav); + sw_prune(landmark_meas.stamp); + + Eigen::Vector3d p_smooth; + Eigen::Quaterniond q_smooth; + if (sw_estimate(p_smooth, q_smooth)) { + p_nav = p_smooth; + q_nav = q_smooth; + } + } + + if (have_prev_) { + const double dt = landmark_meas.stamp - prev_stamp_; + if (dt > vo_cfg_.dt_min && dt < vo_cfg_.dt_max) { + const Eigen::Vector3d v_meas_nav = (p_nav - prev_p_nav_) / dt; + + Eigen::Matrix3d Rp = landmark_meas.R.topLeftCorner<3, 3>(); + Eigen::Matrix3d Rv = 2.0 * Rp / (dt * dt); + + const double v_floor_sq = vo_cfg_.vel_floor * vo_cfg_.vel_floor; + for (int i = 0; i < 3; ++i) { + Rv(i, i) = std::max(Rv(i, i), v_floor_sq); + } + + landmark_vel_update_(v_meas_nav, Rv); + } + } + prev_stamp_ = landmark_meas.stamp; + prev_p_nav_ = p_nav; + have_prev_ = true; + + Eigen::Matrix y; + y.head<3>() = p_nav - current_nom_state_.pos; + y.segment<3>(3) = compute_quaternion_error(q_nav, current_nom_state_.quat); + + Eigen::Matrix K = P * H.transpose() * S.inverse(); + current_error_state_.set_from_vector(K * y); + + // Joseph form covariance update + Eigen::Matrix15d I_KH = Eigen::Matrix15d::Identity() - K * H; + current_error_state_.covariance = + I_KH * P * I_KH.transpose() + K * R * K.transpose(); + + injection_and_reset(); + consecutive_rejects_ = 0; +} + +void LandmarkESKF::set_vo_enabled(bool enabled) { + use_vo_ = enabled; +} + +void LandmarkESKF::set_nis_gating_enabled(bool enabled) { + gating_enabled_ = enabled; +} + +void LandmarkESKF::handle_marker_switch(const Eigen::Vector3d& p_base, + const Eigen::Quaterniond& q_base) { + q_nav_vo_ = (current_nom_state_.quat * q_base.inverse()).normalized(); + p_nav_vo_ = current_nom_state_.pos - q_nav_vo_ * p_base; + sw_buf_.clear(); +} + +void LandmarkESKF::force_anchor_reset() { + have_anchor_ = false; + have_prev_ = false; + consecutive_rejects_ = 0; + sw_buf_.clear(); +} + +int LandmarkESKF::get_consecutive_rejects() const { + return consecutive_rejects_; +} + +bool LandmarkESKF::is_anchor_valid() const { + return have_anchor_; +} + +void LandmarkESKF::set_vo_config(const VoConfig& cfg) { + vo_cfg_ = cfg; +}