New super vision filter

ateam_bringup/launch/state_tracking.launch.xml

@@ -3,7 +3,7 @@
   <arg name="vision_offset_robot_x" default="0.0"/>
   <arg name="vision_offset_robot_y" default="0.0"/>
 
-  <node name="vision_filter" pkg="ateam_vision_filter" exec="ateam_vision_filter_node" respawn="True">
+  <node name="vision_filter" pkg="new_super_vision_filter" exec="ateam_new_super_vision" respawn="True">
     <param name="gc_team_name" value="$(var team_name)"/>
     <param name="offsets.robots.x" value="$(var vision_offset_robot_x)"/>
     <param name="offsets.robots.y" value="$(var vision_offset_robot_y)"/>

new_super_vision_filter/CMakeLists.txt

@@ -0,0 +1,83 @@
+cmake_minimum_required(VERSION 3.8)
+project(new_super_vision_filter)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(ateam_msgs REQUIRED)
+find_package(ateam_common REQUIRED)
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(Boost REQUIRED)
+
+add_library(${PROJECT_NAME} SHARED
+  src/vision_filter_node.cpp
+  src/camera.cpp
+  src/filtered_robot.cpp
+  src/filtered_ball.cpp
+  src/filter/kalman_filter.cpp
+)
+
+target_include_directories(${PROJECT_NAME}
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/kalman/include>
+    $<INSTALL_INTERFACE:include>
+  PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}/src
+)
+set_target_properties(${PROJECT_NAME} PROPERTIES CXX_STANDARD 20)
+ament_target_dependencies(${PROJECT_NAME}
+  rclcpp
+  rclcpp_components
+  ateam_msgs
+  ateam_common
+  tf2
+  tf2_geometry_msgs
+  ssl_league_msgs
+  Eigen3
+)
+target_link_libraries(${PROJECT_NAME}
+  Boost::boost
+)
+
+rclcpp_components_register_node(
+  ${PROJECT_NAME}
+  PLUGIN "new_super_vision::VisionFilterNode"
+  EXECUTABLE ateam_new_super_vision
+)
+install(TARGETS ${PROJECT_NAME}
+    EXPORT ${PROJECT_NAME}
+    LIBRARY DESTINATION lib
+    ARCHIVE DESTINATION lib
+    RUNTIME DESTINATION bin
+    INCLUDES DESTINATION include
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_lint_cmake REQUIRED)
+  find_package(ament_cmake_gtest REQUIRED)
+  file(GLOB_RECURSE AMENT_LINT_AUTO_FILE_EXCLUDE
+    "kalman/*"
+    "kalman/cmake/*"
+  )
+  file(GLOB_RECURSE ALL_CMAKE_FILES
+    "${CMAKE_CURRENT_SOURCE_DIR}/*.cmake"
+    "${CMAKE_CURRENT_SOURCE_DIR}/CMakeLists.txt"
+  )
+
+  list(FILTER ALL_CMAKE_FILES EXCLUDE REGEX
+    ".*/kalman/.*"
+  )
+
+  ament_lint_cmake(${ALL_CMAKE_FILES})
+  add_subdirectory(test)
+endif()
+
+ament_package()

new_super_vision_filter/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>new_super_vision_filter</name>
+  <version>1.0.0</version>
+  <description>wahoo! we can filter vision data!</description>
+  <maintainer email="christianclark687@gmail.com">christian</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>eigen</depend>
+  <depend>ateam_msgs</depend>
+  <depend>ateam_common</depend>
+  <depend>boost</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

new_super_vision_filter/src/camera.cpp

@@ -0,0 +1,42 @@
+// Copyright 2025 A Team
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+
+#include <algorithm>
+
+#include "camera.hpp"
+#include "filtered_ball.hpp"
+#include "filtered_robot.hpp"
+
+namespace ateam_super_vision
+{
+Camera::Camera(int camera_id)
+: camera_id(camera_id) {}
+
+void Camera::process_detection_frame(
+  const ssl_league_msgs::msg::VisionDetectionFrame & detection_frame_msg)
+{
+  (void)detection_frame_msg;
+}
+
+// void Camera::process_camera_geometry(const ssl_league_msgs::msg::VisionGeometryData & geometry) {}
+
+void Camera::clear_old_messages() {}
+
+} // namespace ateam_super_vision

new_super_vision_filter/src/camera.hpp

@@ -0,0 +1,53 @@
+// Copyright 2025 A Team
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+
+#ifndef CAMERA_HPP_
+#define CAMERA_HPP_
+
+#include <vector>
+#include <ssl_league_msgs/msg/vision_geometry_data.hpp>
+#include <ssl_league_msgs/msg/vision_detection_ball.hpp>
+#include <ssl_league_msgs/msg/vision_detection_robot.hpp>
+#include <ssl_league_msgs/msg/vision_detection_frame.hpp>
+#include "filtered_ball.hpp"
+#include "filtered_robot.hpp"
+
+namespace ateam_super_vision {
+class Camera {
+public:
+        // Need to process an individual frame
+        // Set geometry from VisionGeometryCameraCalibration.msg
+        // Have a queue/buffer that we can remove old frames/have a set capacity
+  Camera(int camera_id);
+
+  void process_detection_frame(
+    const ssl_league_msgs::msg::VisionDetectionFrame & detection_frame_msg);
+
+  void process_camera_geometry(const ssl_league_msgs::msg::VisionGeometryData & geometry);
+
+  void clear_old_messages();
+
+private:
+  int camera_id;
+  std::chrono::time_point<std::chrono::steady_clock> last_updated;
+};
+} // namespace ateam_super_vision
+
+#endif // CAMERA_HPP_

new_super_vision_filter/src/filter/kalman_filter.cpp

@@ -0,0 +1,63 @@
+#include "filter/kalman_filter.hpp"
+
+#include <Eigen/LU>
+
+namespace ateam_super_vision {
+    KalmanFilter::KalmanFilter() {}
+
+    void KalmanFilter::init(Eigen::VectorXd & initial_state, Eigen::MatrixXd & initial_error_covar) {
+        state_estimate = initial_state;
+        error_covar = initial_error_covar;
+    }
+
+    void KalmanFilter::predict(Eigen::VectorXd & control_input) {
+        state_estimate = state_transition_model * state_estimate + control_model * control_input;
+        error_covar = state_transition_model * error_covar * state_transition_model.transpose() + process_noise_covar;
+    }
+
+    void KalmanFilter::update(Eigen::VectorXd &measurement) {
+        // NOTE: This function expects you will normalize angles first outside of this function
+        // unlike in Joe's implementation.
+        Eigen::VectorXd predicted_measurement = measurement_model * state_estimate;
+        Eigen::VectorXd measure_residual = measurement - predicted_measurement;
+
+        // S matrix
+        Eigen::MatrixXd innovation_covar = measurement_model * error_covar
+            * measurement_model.transpose() + measurement_noise_covar;
+        // K matrix
+        Eigen::MatrixXd kalman_gain =  error_covar * measurement_model.transpose() * innovation_covar.inverse();
+
+        state_estimate = state_estimate + kalman_gain * measure_residual;
+
+        Eigen::MatrixXd I = Eigen::MatrixXd::Identity(error_covar.rows(), error_covar.cols());
+        // Also have to update the error covariance here
+        error_covar = (I - kalman_gain * measurement_model) * error_covar *
+            (I - kalman_gain * measurement_model).transpose() +
+            (kalman_gain * measurement_noise_covar * kalman_gain.transpose());
+    }
+
+    void KalmanFilter::set_process_noise_covar(Eigen::MatrixXd &covar_mat) {
+        this->process_noise_covar = covar_mat;
+    }
+
+    void KalmanFilter::set_measurement_noise_covar(Eigen::MatrixXd &covar_mat) {
+        this->measurement_noise_covar = covar_mat;
+    }
+
+    void KalmanFilter::set_state_transition_model(Eigen::MatrixXd &model_mat) {
+        this->state_transition_model = model_mat;
+    }
+
+    void KalmanFilter::set_control_model(Eigen::MatrixXd &model_mat) {
+        this->control_model = model_mat;
+    }
+
+    void KalmanFilter::set_measurement_model(Eigen::MatrixXd &model_mat) {
+        this->measurement_model = model_mat;
+    }
+
+    const Eigen::VectorXd & KalmanFilter::get_state_estimate() const {
+        return state_estimate;
+    }
+}
+

new_super_vision_filter/src/filter/kalman_filter.hpp

@@ -0,0 +1,50 @@
+#ifndef FILTER__KALMAN_FILTER_HPP_
+#define FILTER__KALMAN_FILTER_HPP_
+
+#include <Eigen/Core>
+
+namespace ateam_super_vision {
+
+class KalmanFilter {
+    public:
+        KalmanFilter();
+
+        void init(Eigen::VectorXd &initial_state, Eigen::MatrixXd &initial_error_covar);
+
+        void predict(Eigen::VectorXd &control_input);
+
+        void update(Eigen::VectorXd &measurement);
+
+        void set_process_noise_covar(Eigen::MatrixXd &covar_mat);
+
+        void set_measurement_noise_covar(Eigen::MatrixXd &covar_mat);
+
+        void set_state_transition_model(Eigen::MatrixXd &model_mat);
+
+        void set_control_model(Eigen::MatrixXd &model_mat);
+
+        void set_measurement_model(Eigen::MatrixXd &model_mat);
+
+        const Eigen::VectorXd & get_state_estimate() const;
+
+    private:
+        // P matrix
+        Eigen::MatrixXd error_covar;
+        // Q matrix
+        Eigen::MatrixXd process_noise_covar;
+        // R matrix
+        Eigen::MatrixXd measurement_noise_covar;
+        // F matrix
+        Eigen::MatrixXd state_transition_model;
+        // B matrix
+        Eigen::MatrixXd control_model;
+        // H matrix
+        Eigen::MatrixXd measurement_model;
+
+        // x_hat
+        Eigen::VectorXd state_estimate;
+};
+
+} // namespace ateam_super_vision
+
+#endif // FILTER__KALMAN_FILTER_HPP_