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++.