factor out key from state object

Author: rohan-bansal

gtsam_unstable/navigation/EqVIOFilter.cpp

@@ -17,7 +17,11 @@
 
 #include <gtsam_unstable/navigation/EqVIOFilter.h>
 
+#include <algorithm>
 #include <cassert>
+#include <map>
+#include <numeric>
+#include <set>
 #include <stdexcept>
 #include <tuple>
 
@@ -41,6 +45,7 @@ EqVIOFilter::EqVIOFilter(const EqVIOFilterParams& params)
   xi0.sensor.velocity.setZero();
   xi0.sensor.cameraOffset = Pose3::Identity();
   resetReferenceAndGroup(xi0, defaultCovariance(0), makeVioGroupIdentity());
+  landmarkKeys_.clear();
 }
 
 /**
@@ -53,6 +58,8 @@ EqVIOFilter::EqVIOFilter(const State& xi_ref, const Matrix& Sigma,
     : Base(State(), defaultCovariance(0), makeVioGroupIdentity()),
       params_(params) {
   resetReferenceAndGroup(xi_ref, Sigma, makeVioGroupIdentity(xi_ref.n()));
+  landmarkKeys_.resize(xi_ref.n());
+  std::iota(landmarkKeys_.begin(), landmarkKeys_.end(), 0);
   initialized_ = true;
 }
 
@@ -192,9 +199,9 @@ void EqVIOFilter::innovationUpdate(
   if (measurement.empty()) return;
 
   const VisionMeasurement estimatedMeasurement =
-      measureSystemState(state(), camera);
-  const Matrix Ct = EqFoutputMatrixC(referenceState(), groupEstimate(),
-                                     measurement, camera, true);
+      measureSystemState(state(), landmarkKeys_, camera);
+  const Matrix Ct = EqFoutputMatrixC(referenceState(), landmarkKeys_,
+                                     groupEstimate(), measurement, camera, true);
   const Matrix Rused = (outputGainMatrix.rows() == Ct.rows() &&
                         outputGainMatrix.cols() == Ct.rows())
                            ? outputGainMatrix
@@ -222,13 +229,15 @@ void EqVIOFilter::addNewLandmarks(
   if (!camera) throw std::invalid_argument("EqVIOFilter::addNewLandmarks: camera is null");
 
   std::vector<Landmark> newLandmarks;
-  const std::vector<Key> existingIds = referenceState().ids();
+  std::vector<Key> newKeys;
+  const std::vector<Key> existingIds = landmarkKeys_;
   for (const auto& [id, coord] : measurement) {
     if (std::find(existingIds.begin(), existingIds.end(), id) != existingIds.end()) {
       continue;
     }
     const Vector3 bearing = undistortPoint(*camera, coord);
-    newLandmarks.push_back(Landmark{bearing, id});
+    newLandmarks.push_back(Landmark{bearing});
+    newKeys.push_back(id);
   }
 
   if (newLandmarks.empty()) return;
@@ -244,6 +253,7 @@ void EqVIOFilter::addNewLandmarks(
   State xi_ref = referenceState();
   xi_ref.cameraLandmarks.insert(xi_ref.cameraLandmarks.end(), newLandmarks.begin(),
                              newLandmarks.end());
+  landmarkKeys_.insert(landmarkKeys_.end(), newKeys.begin(), newKeys.end());
 
   const auto& [A, Beta, B, Q] = decompose(groupEstimate());
   std::vector<SOT3> q;
@@ -270,7 +280,7 @@ void EqVIOFilter::addNewLandmarks(
 
 /// Remove any landmarks that are absent from the current measurement id list.
 void EqVIOFilter::removeOldLandmarks(const std::vector<Key>& measurementIds) {
-  const std::vector<Key> existingIds = referenceState().ids();
+  const std::vector<Key> existingIds = landmarkKeys_;
   std::vector<int> lostIndices(existingIds.size());
   std::iota(lostIndices.begin(), lostIndices.end(), 0);
   if (lostIndices.empty()) return;
@@ -295,6 +305,7 @@ void EqVIOFilter::removeOldLandmarks(const std::vector<Key>& measurementIds) {
 void EqVIOFilter::removeLandmarkByIndex(int idx) {
   State xi_ref = referenceState();
   xi_ref.cameraLandmarks.erase(xi_ref.cameraLandmarks.begin() + idx);
+  landmarkKeys_.erase(landmarkKeys_.begin() + idx);
 
   const auto& [A, Beta, B, Q] = decompose(groupEstimate());
 
@@ -316,22 +327,16 @@ void EqVIOFilter::removeLandmarkByIndex(int idx) {
 
 /// Remove landmark with matching key id.
 void EqVIOFilter::removeLandmarkById(Key id) {
-  const auto it = std::find_if(
-      referenceState().cameraLandmarks.begin(), referenceState().cameraLandmarks.end(),
-      [&id](const Landmark& lm) { return lm.id == id; });
-  assert(it != referenceState().cameraLandmarks.end());
-  removeLandmarkByIndex(
-      static_cast<int>(std::distance(referenceState().cameraLandmarks.begin(), it)));
+  const auto it = std::find(landmarkKeys_.begin(), landmarkKeys_.end(), id);
+  assert(it != landmarkKeys_.end());
+  removeLandmarkByIndex(static_cast<int>(std::distance(landmarkKeys_.begin(), it)));
 }
 
 /// Return 3x3 landmark-state covariance block for the specified id.
 Matrix3 EqVIOFilter::getLandmarkCovById(Key id) const {
-  const auto it = std::find_if(
-      referenceState().cameraLandmarks.begin(), referenceState().cameraLandmarks.end(),
-      [&id](const Landmark& lm) { return lm.id == id; });
-  assert(it != referenceState().cameraLandmarks.end());
-  const int i =
-      static_cast<int>(std::distance(referenceState().cameraLandmarks.begin(), it));
+  const auto it = std::find(landmarkKeys_.begin(), landmarkKeys_.end(), id);
+  assert(it != landmarkKeys_.end());
+  const int i = static_cast<int>(std::distance(landmarkKeys_.begin(), it));
   return errorCovariance().block<3, 3>(SensorState::CompDim + 3 * i,
                                        SensorState::CompDim + 3 * i);
 }
@@ -340,17 +345,15 @@ Matrix3 EqVIOFilter::getLandmarkCovById(Key id) const {
 Matrix2 EqVIOFilter::outputCovarianceById(
     Key id, const std::shared_ptr<const CameraModel>& camera) const {
   const Matrix3 lmCov = getLandmarkCovById(id);
-  const auto it = std::find_if(
-      referenceState().cameraLandmarks.begin(), referenceState().cameraLandmarks.end(),
-      [&id](const Landmark& lm) { return lm.id == id; });
-  assert(it != referenceState().cameraLandmarks.end());
+  const auto it = std::find(landmarkKeys_.begin(), landmarkKeys_.end(), id);
+  assert(it != landmarkKeys_.end());
 
-  const size_t i =
-      static_cast<size_t>(std::distance(referenceState().cameraLandmarks.begin(), it));
+  const size_t i = static_cast<size_t>(std::distance(landmarkKeys_.begin(), it));
   const LandmarkGroup& Q = std::get<3>(decompose(groupEstimate()));
   const SOT3& Q_i = Q[i];
+  const Point3& q0 = referenceState().cameraLandmarks[i].p;
 
-  const Matrix23 C0i = EqFoutputMatrixCi(it->p, Q_i, camera);
+  const Matrix23 C0i = EqFoutputMatrixCi(q0, Q_i, camera);
   return C0i * lmCov * C0i.transpose();
 }
 
@@ -361,7 +364,7 @@ void EqVIOFilter::removeInvalidLandmarks() {
   for (size_t i = 0; i < Q.size(); ++i) {
     const double a = SOT3Scale(Q[i]);
     if (!std::isfinite(a) || a <= 1e-8 || a > 1e8) {
-      invalidIds.insert(referenceState().cameraLandmarks[i].id);
+      invalidIds.insert(landmarkKeys_[i]);
     }
   }
   for (const Key id : invalidIds) {
@@ -382,7 +385,7 @@ void EqVIOFilter::removeOutliers(
       (1.0 - params_.featureRetention) * measurement.size());
   if (!camera) return;
 
-  const VisionMeasurement yHat = measureSystemState(state(), camera);
+  const VisionMeasurement yHat = measureSystemState(state(), landmarkKeys_, camera);
 
   std::vector<Key> proposedOutliers;
   std::map<Key, double> absoluteOutliers;

gtsam_unstable/navigation/EqVIOFilter.h

@@ -64,9 +64,8 @@ struct EqVIOFilterParams {
 /**
  * @brief Standalone EqVIO filter built on top of `EquivariantFilter`.
  *
- * Prediction is split into covariance propagation (single averaged IMU segment)
- * and state propagation (piecewise IMU holds), preserving the original EqVIO
- * replay semantics while still using the base equivariant filter machinery.
+ * Prediction uses the base equivariant `predict(...)` path per IMU hold,
+ * while dynamic landmark add/remove bookkeeping stays in this runtime filter.
  */
 class GTSAM_UNSTABLE_EXPORT EqVIOFilter
     : public EquivariantFilter<State, Symmetry> {
@@ -76,6 +75,8 @@ class GTSAM_UNSTABLE_EXPORT EqVIOFilter
  private:
   EqVIOFilterParams params_;
   bool initialized_ = false;
+  // Runtime key ordering aligned with `cameraLandmarks` and covariance blocks.
+  std::vector<Key> landmarkKeys_;
 
  public:
   EqVIOFilter();

gtsam_unstable/navigation/EqVIOState.cpp

@@ -22,12 +22,12 @@ namespace eqvio {
 
 void Landmark::print(const std::string& s) const {
   if (!s.empty()) std::cout << s << " ";
-  std::cout << "Landmark{id=" << id << ", p=" << p.transpose() << "}"
+  std::cout << "Landmark{p=" << p.transpose() << "}"
             << std::endl;
 }
 
 bool Landmark::equals(const Landmark& other, double tol) const {
-  return id == other.id && equal_with_abs_tol(p, other.p, tol);
+  return equal_with_abs_tol(p, other.p, tol);
 }
 
 /// Gravity direction in body frame derived from current body pose.
@@ -59,13 +59,6 @@ int State::dim() const {
   return SensorState::CompDim + Landmark::CompDim * static_cast<int>(n());
 }
 
-std::vector<Key> State::ids() const {
-  std::vector<Key> out;
-  out.reserve(cameraLandmarks.size());
-  for (const Landmark& lm : cameraLandmarks) out.push_back(lm.id);
-  return out;
-}
-
 State State::retract(const TangentVector& v, ChartJacobian H1,
                      ChartJacobian H2) const {
   const int d = dim();

gtsam_unstable/navigation/EqVIOState.h

@@ -34,16 +34,13 @@ namespace eqvio {
 /**
  * @brief One visual landmark state block.
  *
- * Stores a 3D landmark position plus its key id. The Lie/chart dimension
- * contribution is 3.
+ * Stores a 3D landmark position. The Lie/chart dimension contribution is 3.
  */
 struct GTSAM_UNSTABLE_EXPORT Landmark {
   static constexpr int CompDim = 3;
 
   /// Landmark position in world coordinates.
   Point3 p = Z_3x1;
-  /// Stable landmark identifier used for ordering/alignment.
-  Key id = 0;
 
   void print(const std::string& s = "") const;
   bool equals(const Landmark& other, double tol = 1e-9) const;
@@ -93,7 +90,6 @@ class GTSAM_UNSTABLE_EXPORT State {
   /// Number of landmarks.
   size_t n() const;
   int dim() const;
-  std::vector<Key> ids() const;
 
   /// Retract in the state chart.
   State retract(const TangentVector& v, ChartJacobian H1 = {},

gtsam_unstable/navigation/EqVIOSymmetry.cpp

@@ -18,7 +18,9 @@
 
 #include <gtsam_unstable/navigation/EqVIOSymmetry.h>
 
+#include <algorithm>
 #include <functional>
+#include <numeric>
 #include <stdexcept>
 
 namespace gtsam {
@@ -330,20 +332,25 @@ Matrix23 EqFoutputMatrixCi(
 
 /// Assemble full stacked output matrix for currently observed landmarks.
 Matrix EqFoutputMatrixC(
-    const State& xi0, const VioGroup& X, const VisionMeasurement& y,
+    const State& xi0, const std::vector<Key>& landmarkIds, const VioGroup& X,
+    const VisionMeasurement& y,
     const std::shared_ptr<const CameraModel>& camera, bool useEquivariance) {
   if (!camera) {
     throw std::invalid_argument("EqFoutputMatrixC: null camera");
   }
   const int M = static_cast<int>(xi0.n());
+  if (landmarkIds.size() != xi0.n()) {
+    throw std::invalid_argument(
+        "EqFoutputMatrixC: landmark id count mismatch");
+  }
   const std::vector<Key> yIds = measurementIds(y);
   const int N = static_cast<int>(yIds.size());
   const LandmarkGroup& Q = std::get<3>(decompose(X));
 
   Matrix C = Matrix::Zero(2 * N, SensorState::CompDim + Landmark::CompDim * M);
 
   for (int i = 0; i < M; ++i) {
-    const Key idNum = xi0.cameraLandmarks[static_cast<size_t>(i)].id;
+    const Key idNum = landmarkIds[static_cast<size_t>(i)];
     const auto itY = std::find(yIds.begin(), yIds.end(), idNum);
     if (itY == yIds.end()) continue;
 
@@ -407,7 +414,6 @@ State stateGroupAction(const VioGroup& X, const State& state) {
   for (size_t i = 0; i < Q.size(); ++i) {
     out.cameraLandmarks[i].p =
         SOT3ApplyInverse(Q[i], state.cameraLandmarks[i].p);
-    out.cameraLandmarks[i].id = state.cameraLandmarks[i].id;
   }
 
   return out;
@@ -441,13 +447,11 @@ VioGroup liftVelocityDiscrete(const State& state, const IMUInput& velocity,
   std::vector<SOT3> q;
   q.resize(state.n());
   for (size_t i = 0; i < state.n(); ++i) {
-    const Landmark& lm0 = state.cameraLandmarks[i];
-    Landmark lm1;
-    lm1.id = lm0.id;
-    lm1.p = c1Tc0.transformFrom(lm0.p);
+    const Point3 p0 = state.cameraLandmarks[i].p;
+    const Point3 p1 = c1Tc0.transformFrom(p0);
 
-    const Rot3 R = RotationFromTwoVectors(lm1.p, lm0.p);
-    const double a = lm0.p.norm() / lm1.p.norm();
+    const Rot3 R = RotationFromTwoVectors(p1, p0);
+    const double a = p0.norm() / p1.norm();
     q[i] = MakeSOT3(SO3(R.matrix()), a);
   }
 
@@ -459,22 +463,34 @@ VioGroup liftVelocityDiscrete(const State& state, const IMUInput& velocity,
 
 /**
  * @brief Predict ideal normalized image measurements from current state.
- * @throws std::invalid_argument if `camera` is null.
+ * @throws std::invalid_argument if `camera` is null or id count mismatches landmarks.
  */
 VisionMeasurement measureSystemState(
-    const State& state, const std::shared_ptr<const CameraModel>& camera) {
+    const State& state, const std::vector<Key>& landmarkIds,
+    const std::shared_ptr<const CameraModel>& camera) {
   if (!camera) {
     throw std::invalid_argument("measureSystemState: camera model is null");
   }
+  if (landmarkIds.size() != state.n()) {
+    throw std::invalid_argument(
+        "measureSystemState: landmark id count mismatch");
+  }
 
-  // Project each landmark through the camera model
   VisionMeasurement out;
-  for (const Landmark& lm : state.cameraLandmarks) {
-    out[lm.id] = camera->project2(lm.p);
+  for (size_t i = 0; i < state.n(); ++i) {
+    out[landmarkIds[i]] = camera->project2(state.cameraLandmarks[i].p);
   }
   return out;
 }
 
+/// Predict ideal measurements with sequential keys `0..n-1`.
+VisionMeasurement measureSystemState(
+    const State& state, const std::shared_ptr<const CameraModel>& camera) {
+  std::vector<Key> ids(state.n());
+  std::iota(ids.begin(), ids.end(), 0);
+  return measureSystemState(state, ids, camera);
+}
+
 /// Evaluate symmetry action and optionally compute Jacobians w.r.t. state and group.
 State Symmetry::operator()(
     const State& xi, const VioGroup& X,

gtsam_unstable/navigation/EqVIOSymmetry.h

@@ -55,6 +55,17 @@ GTSAM_UNSTABLE_EXPORT VioGroup liftVelocityDiscrete(const State& state,
 
 /**
  * @brief Generate ideal image measurements by projecting all landmarks.
+ *
+ * `landmarkIds[i]` is the key assigned to `state.cameraLandmarks[i]`.
+ *
+ * @throws std::invalid_argument if `camera` is null or id count mismatches landmarks.
+ */
+GTSAM_UNSTABLE_EXPORT VisionMeasurement measureSystemState(
+    const State& state, const std::vector<Key>& landmarkIds,
+    const std::shared_ptr<const CameraModel>& camera);
+
+/**
+ * @brief Generate ideal image measurements with sequential keys `0..n-1`.
  * @throws std::invalid_argument if `camera` is null.
  */
 GTSAM_UNSTABLE_EXPORT VisionMeasurement measureSystemState(
@@ -86,7 +97,8 @@ GTSAM_UNSTABLE_EXPORT Matrix23 EqFoutputMatrixCi(
     const std::shared_ptr<const CameraModel>& camera);
 /// Stacked output matrix for all currently observed landmarks.
 GTSAM_UNSTABLE_EXPORT Matrix EqFoutputMatrixC(
-    const State& xi0, const VioGroup& X, const VisionMeasurement& y,
+    const State& xi0, const std::vector<Key>& landmarkIds, const VioGroup& X,
+    const VisionMeasurement& y,
     const std::shared_ptr<const CameraModel>& camera,
     bool useEquivariance = true);