Merge pull request #24 from KumarRobotics/feature/object-tracking

added generic version of object tracking from scenic

Author: jhughes50

glider/CMakeLists.txt

@@ -62,6 +62,7 @@ add_library(${PROJECT_NAME} SHARED
   src/differential_gps.cpp
   src/odometry.cpp
   src/odometry_with_covariance.cpp
+  src/point.cpp
   src/factor_manager.cpp
   src/glider.cpp
 )

glider/include/glider/core/factor_manager.hpp

@@ -31,6 +31,7 @@
 
 #include "odometry_with_covariance.hpp"
 #include "odometry.hpp"
+#include "point.hpp"
 #include "glider/utils/parameters.hpp"
 #include "glider/utils/time.hpp"
 
@@ -92,8 +93,19 @@ class FactorManager
          *  @param gyro: gyroscopre reading
          *  @param orient: orientation in quaternion (w,x,y,z) format */
         void addImuFactor(int64_t timestamp, const Eigen::Vector3d& accel, const Eigen::Vector3d& gyro, const Eigen::Vector4d& orient);
+        /*! @brief adds a landmark factor for an estimated utm point and covariance 
+         *  @param timestamp: time of the landmark measurements
+         *  @param landmark_id: a unique id for the landmark
+         *  @param utm: the estimated utm coordinate of the landmark
+         *  @param cov: the 3x3 covariance matrix for the estimated utm coordinate of the landmark */
+        void addLandmarkFactor(int64_t timestamp, size_t landmark_id, const Eigen::Vector3d& utm, const Eigen::Matrix3d& cov);
+
 
         // getters and checkers
+        /*! @brief gets the estimated landmark utm coordinate and covariance
+         *  @param landmark_id: the uinque id for the landmark 
+         *  @return the estimated utm point and covariance */
+        PointWithCovariance getLandmarkPoint(size_t landmark_id) const;
         /*! @brief gets the complete factor graph */
         gtsam::ExpressionFactorGraph getGraph();
         /*! @brief checks if the imu has been initialized 
@@ -210,5 +222,9 @@ class FactorManager
         bool imu_initialized_;
         // @param tracks if a gps measurement has been received
         bool gps_initialized_;
+
+        // landmark variables
+        std::unordered_map<size_t, Eigen::Matrix3d> landmark_info_;
+        std::unordered_map<size_t, Eigen::Vector3d> landmark_info_vec_;
 };
 }

glider/include/glider/core/glider.hpp

@@ -43,6 +43,9 @@ class Glider
          *  @param gyro: gyroscope measurement in the imu's frame
          *  @param quat: the orientation measurement in the imu's frame */
         void addImu(int64_t timestamp, Eigen::Vector3d& accel, Eigen::Vector3d& gyro, Eigen::Vector4d& quat);
+        void addLandmark(int64_t timestamp, size_t lid, const Eigen::Vector3d& utm, const Eigen::Matrix3d& cov);
+        PointWithCovariance getLandmark(size_t lid);
+        
 
         /*! @brief calls the factor manager to interpolate between GPS 
          *  measurements using the pim

glider/include/glider/core/point.hpp

@@ -0,0 +1,61 @@
+/*!
+* Jason Hughes
+* January 2026 
+*
+*
+* Struct to track landmark points
+*/
+
+#pragma once
+
+#include <Eigen/Dense>
+#include <gtsam/geometry/Pose3.h>
+
+namespace Glider
+{
+class Point
+{
+    public:
+        Point() = default;
+        Point(const Eigen::Vector3d& p);
+        Point(double x, double y, double z);    
+
+        double x() const { return x_; }
+        double y() const { return y_; }
+        double z() const { return z_; }
+    
+        void x(double x) { x_ = x; }
+        void y(double y) { y_ = y; }
+        void z(double z) { z_ = z; }
+
+        Eigen::Vector3d vector() const;
+
+        bool isInitialized() const { return initialized_; }
+
+    private:
+        bool initialized_{false};
+        double x_{0.0}, y_{0.0}, z_{0.0};
+};
+
+class PointWithCovariance : public Point
+{
+    public:
+        using Point::Point;
+        PointWithCovariance() = default;
+        PointWithCovariance(const Eigen::Vector3d& p, const Eigen::Matrix3d& c);
+
+        Eigen::Matrix3d cov() const;
+        double cov(int r, int c) const;
+
+        Eigen::Vector3d var() const;
+        double var(int i) const;
+
+        Eigen::Vector3d std() const;
+        double std(int i) const;
+
+        double trace() const;
+
+    private:
+        Eigen::Matrix3d cov_{Eigen::Matrix3d::Identity()}; 
+};
+} // namespace Glider

glider/src/factor_manager.cpp

@@ -252,6 +252,29 @@ void FactorManager::addImuFactor(int64_t timestamp, const Eigen::Vector3d& accel
     last_imu_time_ = current_time;
 }
 
+void FactorManager::addLandmarkFactor(int64_t timestamp, size_t landmark_id, const Eigen::Vector3d& utm, const Eigen::Matrix3d& cov)
+{
+    Eigen::Matrix3d obs_info = cov.inverse();
+    auto it = landmark_info_.find(landmark_id);
+    if (it == landmark_info_.end()) {
+        landmark_info_[landmark_id] = obs_info;
+        landmark_info_vec_[landmark_id] = obs_info * utm;
+    } else {
+        it->second += obs_info;
+        landmark_info_vec_[landmark_id] += obs_info * utm;
+    }
+}
+
+PointWithCovariance FactorManager::getLandmarkPoint(size_t landmark_id) const
+{
+    auto it = landmark_info_.find(landmark_id);
+    if (it == landmark_info_.end()) return PointWithCovariance();
+    
+    Eigen::Matrix3d cov = it->second.inverse();
+    Eigen::Vector3d point = cov * landmark_info_vec_.at(landmark_id);
+    return PointWithCovariance(point, cov);
+}
+
 Odometry FactorManager::predict(int64_t timestamp)
 {
     // TODO update this.

glider/src/glider.cpp

@@ -121,6 +121,16 @@ void Glider::addImu(int64_t timestamp, Eigen::Vector3d& accel, Eigen::Vector3d&
     }
 }  
 
+void Glider::addLandmark(int64_t timestamp, size_t lid, const Eigen::Vector3d& utm, const Eigen::Matrix3d& cov)
+{
+    factor_manager_.addLandmarkFactor(timestamp, lid, utm, cov);
+}
+
+PointWithCovariance Glider::getLandmark(size_t lid)
+{
+    return factor_manager_.getLandmarkPoint(lid);
+}
+
 Odometry Glider::interpolate(int64_t timestamp)
 {
     try

glider/src/point.cpp

@@ -0,0 +1,73 @@
+/*!
+* Jason Hughes
+* January 2026
+*
+* Point and PointWithCovariance obejcts
+*/
+
+#include "glider/core/point.hpp"
+
+using namespace Glider;
+
+Point::Point(const Eigen::Vector3d& p)
+{
+    x_ = p.x();
+    y_ = p.y();
+    z_ = p.z();
+    initialized_ = true;
+}
+
+Point::Point(double x, double y, double z)
+{
+    x_ = x;
+    y_ = y;
+    z_ = z;
+    initialized_ = true;
+}
+
+Eigen::Vector3d Point::vector() const
+{
+    return Eigen::Vector3d(x_, y_, z_);
+}
+
+PointWithCovariance::PointWithCovariance(const Eigen::Vector3d& p, const Eigen::Matrix3d& c) : Point(p)
+{
+    cov_ = c;
+}
+
+Eigen::Matrix3d PointWithCovariance::cov() const
+{
+    return cov_;
+}
+
+double PointWithCovariance::cov(int r, int c) const
+{
+    return cov_(r, c);
+}
+
+Eigen::Vector3d PointWithCovariance::var() const
+{
+    return cov_.diagonal();
+}
+
+double PointWithCovariance::var(int i) const
+{
+    Eigen::Vector3d var = cov_.diagonal();
+    return var(i);
+}
+
+Eigen::Vector3d PointWithCovariance::std() const
+{
+    return cov_.diagonal().cwiseSqrt();
+}
+
+double PointWithCovariance::std(int i) const
+{
+    Eigen::Vector3d std = cov_.diagonal().cwiseSqrt();
+    return std(i);
+}
+
+double PointWithCovariance::trace() const
+{
+    return cov_.trace();
+}