merged landmark egomotion into eskf package. LandmarkESKF now inherits from ESKF directly. Fixed quaternion error to use right perturbation

Author: AntTra

navigation/eskf/CMakeLists.txt

@@ -56,8 +56,15 @@ 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}
@@ -78,14 +85,6 @@ ament_target_dependencies(${COMPONENT_LIB_NAME}
 
 target_link_libraries(${COMPONENT_LIB_NAME} ${PROJECT_NAME})
 
-find_package(landmark_egomotion QUIET)
-if(landmark_egomotion_FOUND)
-  target_compile_definitions(${COMPONENT_LIB_NAME} PRIVATE HAVE_LANDMARK_EGOMOTION=1)
-  target_link_libraries(${COMPONENT_LIB_NAME} landmark_egomotion::landmark_egomotion_lib)
-else()
-  message(STATUS "landmark_egomotion not found — building eskf without landmark egomotion support")
-endif()
-
 rclcpp_components_register_node(
   ${COMPONENT_LIB_NAME}
   PLUGIN "ESKFNode"
@@ -106,6 +105,11 @@ install(
   DESTINATION include
 )
 
+install(
+  DIRECTORY ${LANDMARK_DIR}/include/
+  DESTINATION include
+)
+
 install(DIRECTORY
   launch
   config

navigation/eskf/include/eskf/ros/eskf_ros.hpp

@@ -24,11 +24,9 @@
 #include "eskf/lib/typedefs.hpp"
 #include "spdlog/spdlog.h"
 
-#ifdef HAVE_LANDMARK_EGOMOTION
 #include <vortex_msgs/msg/landmark_array.hpp>
 #include "landmark_egomotion/lib/landmark_egomotion.hpp"
 #include "landmark_egomotion/lib/landmark_typedefs.hpp"
-#endif
 
 class ESKFNode : public rclcpp::Node {
    public:
@@ -60,7 +58,6 @@ class ESKFNode : public rclcpp::Node {
     // @brief broadcast the State as a TF
     void publish_tf(const StateQuat& nom_state);
 
-#ifdef HAVE_LANDMARK_EGOMOTION
     void setup_vo(const EskfParams& eskf_params);
 
     void landmark_callback(
@@ -76,7 +73,6 @@ class ESKFNode : public rclcpp::Node {
     bool have_last_marker_{false};
     uint16_t last_marker_id_{0};
     int vo_rejects_limit_{0};
-#endif
 
     // Subscribers and Publishers
 

navigation/eskf/src/ros/eskf_ros.cpp

@@ -132,17 +132,13 @@ void ESKFNode::set_parameters() {
     EskfParams eskf_params{.Q = Q, .P = P};
 
     eskf_ = std::make_unique<ESKF>(eskf_params);
-    spdlog::info("Landmark egomotion enabled?");
-#ifdef HAVE_LANDMARK_EGOMOTION
-    spdlog::info("Landmark egomotion enabled");
+
     this->declare_parameter<bool>("use_landmark_egomotion");
     if (this->get_parameter("use_landmark_egomotion").as_bool()) {
         setup_vo(eskf_params);
     }
-#endif
 }
 
-#ifdef HAVE_LANDMARK_EGOMOTION
 void ESKFNode::setup_vo(const EskfParams& eskf_params) {
     auto landmark_eskf = std::make_unique<LandmarkESKF>(eskf_params);
     landmark_eskf_ = landmark_eskf.get();
@@ -289,7 +285,6 @@ void ESKFNode::landmark_callback(
         spdlog::warn("VO gating: {} consecutive reject(s)", rejects_after);
     }
 }
-#endif  // HAVE_LANDMARK_EGOMOTION
 
 void ESKFNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) {
     rclcpp::Time current_time = msg->header.stamp;

navigation/landmark_egomotion/CMakeLists.txt

@@ -22,7 +22,7 @@ LandmarkESKF::LandmarkESKF(const EskfParams& params) : ESKF(params) {
 Eigen::Vector3d LandmarkESKF::compute_quaternion_error(
     const Eigen::Quaterniond& q_meas,
     const Eigen::Quaterniond& q_est) {
-    Eigen::Quaterniond q_err = (q_meas * q_est.inverse()).normalized();
+    Eigen::Quaterniond q_err = (q_est.inverse() * q_meas).normalized();
 
     if (q_err.w() < 0.0) {
         q_err.coeffs() = -q_err.coeffs();
@@ -202,10 +202,42 @@ void LandmarkESKF::landmark_update(const LandmarkMeasurement& landmark_meas) {
         return;
     }
 
-    // Transform VO measurement to navigation frame
+    // 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<double, 6, 15> H = Eigen::Matrix<double, 6, 15>::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<double, 6, 6> 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<double, 6, 6> S = H * P * H.transpose() + R;
+
+    Eigen::Matrix<double, 6, 1> 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);
@@ -239,41 +271,9 @@ void LandmarkESKF::landmark_update(const LandmarkMeasurement& landmark_meas) {
     have_prev_ = true;
 
     Eigen::Matrix<double, 6, 1> y;
-
-    // Position innovation
     y.head<3>() = p_nav - current_nom_state_.pos;
     y.segment<3>(3) = compute_quaternion_error(q_nav, current_nom_state_.quat);
 
-    // Measurement Jacobian
-    Eigen::Matrix<double, 6, 15> H = Eigen::Matrix<double, 6, 15>::Zero();
-    H.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
-    H.block<3, 3>(3, 6) = Eigen::Matrix3d::Identity();
-
-    // Measurement noise floors
-    Eigen::Matrix<double, 6, 6> 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);
-    }
-
-    // Innovation covariance and NIS
-    const Eigen::Matrix15d P = current_error_state_.covariance;
-    const Eigen::Matrix<double, 6, 6> S = H * P * H.transpose() + R;
-    const double nis = compute_nis(y, S);
-
-    const double gate = !gating_enabled_ ? 1e9 : vo_cfg_.nis_gate_pose;
-
-    if (nis > gate) {
-        consecutive_rejects_++;
-        return;
-    }
-
     Eigen::Matrix<double, 15, 6> K = P * H.transpose() * S.inverse();
     current_error_state_.set_from_vector(K * y);