IncrementalFixedLagSmoother with Smart Factors: Marginalization Causes VariableIndex Assertion Failure

[JD-Florez]

Description

When using IncrementalFixedLagSmoother with SmartProjectionPoseFactor<Cal3_S2>, marginalizing out a pose included in a smart factor causes the following assertion failure:

gtsam/gtsam/inference/VariableIndex.h:188: gtsam::FactorIndices& gtsam::VariableIndex::internalAt(gtsam::Key): Assertion item != index_.end()' failed.

Steps to reproduce

I created a unit test for a center-pointing agent, observing a cluster of 5 points:

/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    testIncrementalFixedLagSmootherSmart.cpp
 * @brief   Unit test for the Incremental Fixed-Lag Smoother with Smart Factors
 * @author  Juan-Diego Florez ([email protected])
 * @date    Jan 12, 2025
 */

#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/debug.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/SmartProjectionPoseFactor.h>
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>

using namespace std;
using namespace gtsam;

typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;

// Create symbols for variables
Key PoseKey(size_t i) { return Symbol('x', i); }
Key PointKey(size_t j) { return Symbol('l', j); }

/**
 * Returns a Pose3 that places the camera on a center-pointing circular orbit
 * around `center`. `i` is the step index, and angleDeg is how many
 * degrees per step.
 */
Pose3 inspectionPose(size_t i, double angleDeg, double radius,
                     const Point3& center) {
  double angle = (angleDeg * M_PI / 180.0) * i;

  // Compute camera translation on a circle in X-Y around 'center.z'
  double camX = center.x() + radius * std::cos(angle);
  double camY = center.y() + radius * std::sin(angle);
  double camZ = 0.0;  // orbit in the plane z=0
  Point3 cameraPosition(camX, camY, camZ);

  Vector3 forward = (Vector3(center - cameraPosition)).normalized();
  Vector3 worldUp(0.0, 1.0, 0.0);
  Vector3 right = forward.cross(worldUp).normalized();
  Vector3 up = right.cross(forward).normalized();

  // Build the rotation matrix so that camera's +X=right, +Y=up, +Z=forward
  Matrix3 R;
  R.col(0) = right;
  R.col(1) = up;
  R.col(2) = forward;

  return Pose3(Rot3(R), cameraPosition);
}

/* ************************************************************************* */
bool check_smoother(const NonlinearFactorGraph& fullgraph,
                    const Values& fullinit,
                    const IncrementalFixedLagSmoother& smoother,
                    const Key& key) {
  GaussianFactorGraph linearized = *fullgraph.linearize(fullinit);
  VectorValues delta = linearized.optimize();
  Values fullfinal = fullinit.retract(delta);

  Pose3 expected = fullfinal.at<Pose3>(key);
  Pose3 actual = smoother.calculateEstimate<Pose3>(key);

  return assert_equal(expected, actual);
}

/* ************************************************************************* */
TEST(IncrementalFixedLagSmoother, SmartFactors) {
  SETDEBUG("IncrementalFixedLagSmoother update", true);

  // Create a Fixed-Lag Smoother
  typedef IncrementalFixedLagSmoother::KeyTimestampMap Timestamps;
  IncrementalFixedLagSmoother smoother(5.0, ISAM2Params());

  // Camera calibration
  Cal3_S2::shared_ptr K(new Cal3_S2(3824.462729,  // fx
                                    3824.462729,  // fy
                                    0.0,          // skew
                                    500.0,        // cx
                                    500.0         // cy
                                    ));

  // Noise models
  auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
  auto poseNoise = noiseModel::Diagonal::Sigmas(
      (Vector(6) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1).finished());

  // Create containers to keep the full graph
  Values fullinit;
  NonlinearFactorGraph fullgraph;

  // Create landmarks
  vector<Point3> landmarks;
  landmarks.push_back(Point3(1.0, 1.0, 15.0));
  landmarks.push_back(Point3(-2.0, 2.0, 16.0));
  landmarks.push_back(Point3(2.0, -1.0, 17.0));
  landmarks.push_back(Point3(3.0, -1.5, 18.0));
  landmarks.push_back(Point3(-1.5, 1.5, 19.0));

  // Initialize smart factors
  std::vector<SmartFactor::shared_ptr> smartFactors;
  for (size_t j = 0; j < landmarks.size(); ++j) {
    SmartFactor::shared_ptr smartFactor(new SmartFactor(measurementNoise, K));
    smartFactors.push_back(smartFactor);
  }

  // Add poses and smart factors, run for 10 frames to test marginalization
  for (size_t i = 0; i < 10; ++i) {
    std::cout << "_________________________\nCamera at pose " << i << std::endl;

    Key poseKey = PoseKey(i);

    NonlinearFactorGraph newFactors;
    Values newValues;
    Timestamps newTimestamps;
    Pose3 pose;

    if (i == 0) {
      // Identity for x0
      pose = Pose3::Identity();
    } else {
      pose = inspectionPose(i, 10.0, 30.0, Point3(0.0, 0.0, 20.0));
    }

    newValues.insert(poseKey, pose);
    newTimestamps[poseKey] = double(i);

    // Add prior on first and second poses
    if (i == 0 || i == 1) {
      newFactors.addPrior(poseKey, pose, poseNoise);
    }

    // Add measurements to smart factors
    for (size_t j = 0; j < landmarks.size(); ++j) {
      // Create camera at the current pose:
      PinholeCamera<Cal3_S2> camera(pose, *K);

      // Project the 3D landmark into the camera:
      Point2 measurement = camera.project(landmarks[j]);
      smartFactors[j]->add(measurement, poseKey);
      auto camPoint = pose.transformTo(landmarks[j]);

      std::cout << "Landmark " << j << " in image coords: " << measurement
                << ",\nin cam coords: " << camPoint << std::endl;

      // Add smart factor to graph when it has at least 3 measurements
      if (smartFactors[j]->measured().size() == 3) {
        newFactors.push_back(smartFactors[j]);
      }
    }

    // Update full graph and values
    fullgraph.push_back(newFactors);
    fullinit.insert(newValues);

    // Update the smoother
    smoother.update(newFactors, newValues, newTimestamps);

    // Verify triangulation
    Values currentEstimate = smoother.calculateEstimate();
    for (size_t j = 0; j < smartFactors.size(); ++j) {
      auto tripoint = smartFactors[j]->point();

      if (!tripoint) {
        std::cout << "SmartFactor " << j << " not yet triangulated"
                  << std::endl;
        continue;
      }
      std::cout << "SmartFactor " << j << " triangulated at "
                << (*tripoint).transpose() << std::endl;
    }

    // Check if the estimate is correct
    CHECK(check_smoother(fullgraph, fullinit, smoother, poseKey));

    if (i <= 5) {
      // Check nominal operation
      size_t activeFactors = 0;
      for (const auto& factor : smoother.getFactors()) {
        if (factor) activeFactors++;
      }
      CHECK(activeFactors == fullgraph.size());
    } else {
      // Test marginalization
      size_t activeKeys = 0;
      const auto& smootherFactors = smoother.getFactors();
      for (const auto& factor : smootherFactors) {
        if (factor) {
          activeKeys++;
          // Verify no keys outside the lag window are involved
          for (Key key : factor->keys()) {
            double keyTime = smoother.timestamps().at(key);
            CHECK(double(i) - keyTime <= 5.0);  // Check within 5-frame horizon
          }
        }
      }
    }
  }
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

This is the full output:

_________________________
Camera at pose 0
Landmark 0 in image coords: 754.964
754.964,
in cam coords:  1
 1
15
Landmark 1 in image coords: 21.9422
978.058,
in cam coords: -2
 2
16
Landmark 2 in image coords: 949.937
275.032,
in cam coords:  2
-1
17
Landmark 3 in image coords: 1137.41
181.295,
in cam coords:    3
-1.5
  18
Landmark 4 in image coords: 198.069
801.931,
in cam coords: -1.5
 1.5
  19
IncrementalFixedLagSmoother::update() Start
Bayes Tree Before Update:
{Empty Tree}
END
Current Timestamp: 0
Marginalizable Keys: 
Constrained Keys: 
Bayes Tree After Update, Before Marginalization:
P( x0 )
END
Final Bayes Tree:
P( x0 )
END
IncrementalFixedLagSmoother::update() Finish
SmartFactor 0 not yet triangulated
SmartFactor 1 not yet triangulated
SmartFactor 2 not yet triangulated
SmartFactor 3 not yet triangulated
SmartFactor 4 not yet triangulated
_________________________
Camera at pose 1
Landmark 0 in image coords:  923.64
555.013,
in cam coords:  3.57991
0.464884
 32.3181
Landmark 1 in image coords: 981.888
705.896,
in cam coords: 4.43356
1.89433
35.1866
Landmark 2 in image coords: 658.471
328.897,
in cam coords:  1.36313
-1.47179
 32.8971
Landmark 3 in image coords: 497.012
265.514,
in cam coords: -0.0255495
  -2.00519
   32.7046
Landmark 4 in image coords: 674.811
665.779,
in cam coords: 1.66897
1.58274
36.5132
IncrementalFixedLagSmoother::update() Start
Bayes Tree Before Update:
P( x0 )
END
Current Timestamp: 1
Marginalizable Keys: 
Constrained Keys: 
Bayes Tree After Update, Before Marginalization:
P( x0 )
P( x1 )
END
Final Bayes Tree:
P( x0 )
P( x1 )
END
IncrementalFixedLagSmoother::update() Finish
SmartFactor 0 not yet triangulated
SmartFactor 1 not yet triangulated
SmartFactor 2 not yet triangulated
SmartFactor 3 not yet triangulated
SmartFactor 4 not yet triangulated
_________________________
Camera at pose 2
Landmark 0 in image coords: 915.424
488.512,
in cam coords:    3.49935
-0.0967663
   32.2156
Landmark 1 in image coords: 985.273
689.168,
in cam coords: 4.41963
1.72285
34.8313
Landmark 2 in image coords: 648.941
278.605,
in cam coords:  1.28953
-1.91684
 33.1122
Landmark 3 in image coords: 487.878
214.722,
in cam coords: -0.104686
 -2.46361
  33.0274
Landmark 4 in image coords: 677.632
671.084,
in cam coords: 1.68353
1.62147
36.2468
IncrementalFixedLagSmoother::update() Start
Bayes Tree Before Update:
P( x0 )
P( x1 )
END
Current Timestamp: 2
Marginalizable Keys: 
Constrained Keys: 
Bayes Tree After Update, Before Marginalization:
P( x0 x1 x2 )
END
Final Bayes Tree:
P( x0 x1 x2 )
END
IncrementalFixedLagSmoother::update() Finish
SmartFactor 0 triangulated at  1  1 15
SmartFactor 1 triangulated at -2  2 16
SmartFactor 2 triangulated at  2 -1 17
SmartFactor 3 triangulated at    3 -1.5   18
SmartFactor 4 triangulated at -1.5  1.5   19
_________________________
Camera at pose 3
Landmark 0 in image coords: 898.804
418.007,
in cam coords:   3.35203
-0.689172
  32.1454
Landmark 1 in image coords: 987.371
662.427,
in cam coords: 4.38961
1.46294
34.4458
Landmark 2 in image coords: 632.642
232.935,
in cam coords:  1.15723
-2.32999
 33.3663
Landmark 3 in image coords: 471.934
172.535,
in cam coords: -0.245171
 -2.86056
  33.4084
Landmark 4 in image coords: 681.599
670.681,
in cam coords:  1.7074
1.60475
35.9576
IncrementalFixedLagSmoother::update() Start
Bayes Tree Before Update:
P( x0 x1 x2 )
END
Current Timestamp: 3
Marginalizable Keys: 
Constrained Keys: 
Bayes Tree After Update, Before Marginalization:
P( x0 x1 x2 )
END
Final Bayes Tree:
P( x0 x1 x2 )
END
IncrementalFixedLagSmoother::update() Finish
SmartFactor 0 triangulated at  1  1 15
SmartFactor 1 triangulated at -2  2 16
SmartFactor 2 triangulated at  2 -1 17
SmartFactor 3 triangulated at    3 -1.5   18
SmartFactor 4 triangulated at -1.5  1.5   19
_________________________
Camera at pose 4
Landmark 0 in image coords: 871.042
343.492,
in cam coords:  3.11523
-1.31402
 32.1098
Landmark 1 in image coords: 986.495
622.724,
in cam coords: 4.33033
1.09238
34.0418
Landmark 2 in image coords: 607.974
192.559,
in cam coords: 0.950061
-2.70517
 33.6515
Landmark 3 in image coords: 447.921
 140.57,
in cam coords: -0.460763
 -3.17999
  33.8362
Landmark 4 in image coords: 686.539
663.202,
in cam coords: 1.73906
 1.5215
35.6546
IncrementalFixedLagSmoother::update() Start
Bayes Tree Before Update:
P( x0 x1 x2 )
END
Current Timestamp: 4
Marginalizable Keys: 
Constrained Keys: 
Bayes Tree After Update, Before Marginalization:
P( x0 x1 x2 )
END
Final Bayes Tree:
P( x0 x1 x2 )
END
IncrementalFixedLagSmoother::update() Finish
SmartFactor 0 triangulated at  1  1 15
SmartFactor 1 triangulated at -2  2 16
SmartFactor 2 triangulated at  2 -1 17
SmartFactor 3 triangulated at    3 -1.5   18
SmartFactor 4 triangulated at -1.5  1.5   19
_________________________
Camera at pose 5
Landmark 0 in image coords: 827.613
265.828,
in cam coords:   2.7506
-1.96608
 32.1098
Landmark 1 in image coords: 979.445
567.153,
in cam coords:  4.21615
0.590529
 33.6316
Landmark 2 in image coords: 572.361
 158.55,
in cam coords: 0.642526
-3.03189
 33.9591
Landmark 3 in image coords: 413.977
120.792,
in cam coords: -0.771452
 -3.40073
  34.2977
Landmark 4 in image coords: 691.934
647.212,
in cam coords: 1.77392
1.36058
 35.347
IncrementalFixedLagSmoother::update() Start
Bayes Tree Before Update:
P( x0 x1 x2 )
END
Current Timestamp: 5
Marginalizable Keys: 
Constrained Keys: 
Bayes Tree After Update, Before Marginalization:
P( x0 x1 x2 )
END
Final Bayes Tree:
P( x0 x1 x2 )
END
IncrementalFixedLagSmoother::update() Finish
SmartFactor 0 triangulated at  1  1 15
SmartFactor 1 triangulated at -2  2 16
SmartFactor 2 triangulated at  2 -1 17
SmartFactor 3 triangulated at    3 -1.5   18
SmartFactor 4 triangulated at -1.5  1.5   19
_________________________
Camera at pose 6
Landmark 0 in image coords: 761.742
188.137,
in cam coords:      2.2
-2.62128
 32.1454
Landmark 1 in image coords: 960.396
493.738,
in cam coords:          4
-0.0544029
   33.2276
Landmark 2 in image coords: 522.313
133.233,
in cam coords:      0.2
-3.28745
 34.2799
Landmark 3 in image coords: 368.042
116.219,
in cam coords:     -1.2
-3.49002
 34.7789
Landmark 4 in image coords:  696.44
621.369,
in cam coords:     1.8
1.11212
 35.044
IncrementalFixedLagSmoother::update() Start
Bayes Tree Before Update:
P( x0 x1 x2 )
END
Current Timestamp: 6
Marginalizable Keys: x0 
Constrained Keys: x0(0)  x1(1)  x2(1)  x3(1)  x4(1)  x5(1)  x6(1)  
Bayes Tree After Update, Before Marginalization:
P( x0 x1 x2 )
END
testIncrementalFixedLagSmootherSmart: /home/jdflo/packages/gtsam/gtsam/inference/VariableIndex.h:188: gtsam::FactorIndices& gtsam::VariableIndex::internalAt(gtsam::Key): Assertion `item != index_.end()' failed.

Expected behavior

I expect that smart factors should be able to handle references to marginalized poses. If not, then the smart factor would need to be recreated without the marginalized pose.

Environment

I am currently on gtsam commit d48b1fc, on Ubuntu 22.04, and am using C++.

[dellaert]

Thanks for assigning me, @varunagrawal ;-)
Unfortunately, I have not worked with smart factors in a while. @JD-Florez want to create a draft PR with what you think the solution could be? Or we could discuss it here before implementing.

[JD-Florez]

@dellaert what I am currently doing (and what I also see implemented in Kimera) is deleting smart factors that reference marginalized poses and rebuilding them without those poses. In scenarios where an environment is being traversed, it may be possible that a landmark stops being visible after some time and that that time just so happens to be within the optimization window, but it is just as likely that a trajectory allows a landmark to be observed during a significant portion of the trajectory. What I expect should happen is that the smart factor logic has a way to deal with partial marginalization, such that a pose can be removed from it to return a partially marginalized smart factor.
Naively, I think the correct approach would be to

1. linearize the smart factor about the current estimates,
2. eliminate the to-be-marginalized pose to produce a factor constrained by the eliminated pose but not dependent on it
3. insert the updated factor in place of the old factor
4. marginalize the pose from the graph

[dellaert]

Yes, I think your intuition is right. Except, there is a possible issue with 1, using the “current estimate”. We might want to use the Jacobian when the factor was first linearized, so called FEJ. See https://pgeneva.com/downloads/papers/Chen2023IROS.pdf
That’s probably gravy though.

Step 2 is really two steps:
A. marginalization, already done in the fixed lag smoother
B. making a “1 pose smaller” factor, which has a baked in pose along with its Jacobian.

B involves some work (and unit tests) to even enable the smart factor to allow for that. That is not too hard, I think, but should be done in isolation and tested in a separate PR.

Another PR then, however, needs to somehow allow the incremental smoother to call this “fixPose” method on smart factors.

It might be worth assuming that

newFactor <- oldFactor.fixPose(pose)

Is already written and see how PR 2 could be done.

[JD-Florez]

Alternatively, what if smart factors were made to operate with isam2's wildfire threshold, such that they can internally manage what measurements to consider for triangulation. Perhaps if a landmark is already settled/not changing by much, the measurements linked to old pose variables don't need to be considered. Since the fixed-lag smoother is based on isam2, this could be a more far-reaching functionality?

[dellaert]

Hmmm. I think that enabling smart factors to fix poses with their marginalization values is more general, as that could be used with any scheme; fixed lag smoother, iSAM, batch with graph simplification etc.

[JD-Florez]

So perhaps some implementation that first allows smart factors to internally marginalize poses, and then allows for selection? In an extended isam2 case, a smart factor could virtually cause a batch optimization over its pose history, no?

[dellaert]

A smart factor by itself cannot marginalize a pose, because that pose might be connected to many smart factors. So I think what you need is a method 1st fixPose` at this value: it will then calculate the Jacobian and redefine the error function to add that additional cost.