InconsistentEliminationRequested error with SmartFactor

[varunagrawal]

Issue discovered by @ToniRV.

A minimal test for which we get the InconsistentEliminationRequested when calling gtsam::Marginals.
It seems to happen when a smart factor is added with no measurements.

The example is in here: MIT-SPARK#1
where the example ISAM2_SmartFactorStereo_IMU is modified to showcase the problem.

In particular, the data is simplified so that there is only a single landmark, and:

1. Removed the part of the code that adds measurements to the stereo factors:

   Line 261 in examples/testISAM2_SmartFactorStereo_IMU.cpp

   //      smartFactors[landmark]->add(StereoPoint2(xl, xr, y), X(frame), K);
   

2. Used an Incremental Fixed Lag Smoother class instead of an ISAM2 class:

     auto smoother = gtsam::IncrementalFixedLagSmoother(horizon, parameters);
   

   To run this, you can just compile gtsam as always, and run:

   ./build/examples/testISAM2_SmartFactorStereo_IMU ./examples/Data/testISAM2_SmartFactorStereo_IMU.txt
   

The output is not too big, where the graph is printed before gtsam::Marginals is called:

./build/examples/testISAM2_SmartFactorStereo_IMU examples/Data/testISAM2_SmartFactorStereo_IMU.txt 
Running iSAM for frame: 1
Compute state covariance: keyframe # 0
New Graph Prior to Calculating Marginals!
size: 4

Factor 0: PriorFactor on x0
  prior mean: R:
[
 	1 0 0;
    	0 1 0;
    	0 0 1
  ]
[0, 0, 0]';isotropic dim=6 sigma=0.1

Factor 1: PriorFactor on b0
  prior mean: acc = [0, -0.0942015, 0]' gyro = [-0.00527483, -0.00757152, -0.00469968]'
  noise model: huber (1.345)
  noise model: diagonal sigmas[0.05; 0.05; 0.05; 0.005; 0.005; 0.005];

Factor 2: PriorFactor on v0
  prior mean: [
 	0;
    	0;
    	0
  ]
  noise model: huber (1.345)
isotropic dim=3 sigma=0.01

Factor 3: CombinedImuFactor(x0,v0,x1,v1,b0,b1)
  preintegrated measurements:    deltaTij 0.035
    deltaRij.ypr = ( 0.000918150444 -0.000242884465 -0.000291945117)
    deltaPij [-4.43459204e-05, -0.00608066539, 0.000525517612]'
    deltaVij [-0.00290516769, -0.345401368, 0.0289373307]'
    gyrobias [-0.00527483, -0.00757152, -0.00469968]'
    acc_bias [0, -0.0942015, 0]'

  preintMeasCov [ ... ]
  noise model: Gaussian[
 ...
  ]

Running iSAM for frame: 2
Compute state covariance: keyframe # 1
New Graph Prior to Calculating Marginals!
size: 2

Factor 0: SmartStereoProjectionPoseFactor, z = 
 SmartStereoProjectionFactor
linearizationMode:
0
triangulationParameters:
rankTolerance = 1
enableEPI = 0
landmarkDistanceThreshold = -1
dynamicOutlierRejectionThreshold = -1

result:
no point, status = 1

SmartFactorBase, z = 
  keys = { }

Factor 1: CombinedImuFactor(x1,v1,x2,v2,b1,b2)
  preintegrated measurements:    deltaTij 0.03375
    deltaRij.ypr = (  0.00132853246 -0.000366771874 -0.000408794198)
    deltaPij [-8.34607473e-05, -0.00552417096, 0.000465198658]'
    deltaVij [-0.00533959756, -0.326706335, 0.0287011522]'
    gyrobias [-0.00527483, -0.00757152, -0.00469968]'
    acc_bias [9.21565377e-18, -0.0942015, -1.96548395e-18]'

  preintMeasCov [  ... ]
  noise model: Gaussian[ ...]

terminate called after throwing an instance of 'gtsam::InconsistentEliminationRequested'
  what():  An inference algorithm was called with inconsistent arguments.  The
factor graph, ordering, or variable index were inconsistent with each
other, or a full elimination routine was called with an ordering that
does not include all of the variables.
Aborted (core dumped)

[ProfFan]

@varunagrawal Do you think this should be in 4.1 or not?

[varunagrawal]

Yes it should. I've not had the cycles to look into this deeply but Toni has done a great job making this reproducible so we should address this.

[varunagrawal]

@ToniRV I took a look at this issue again (apologies for the delay) and I can't reproduce it. I am wondering if the fixes in the interim have resolved this issue.

Can you please try this example with the latest develop?

[varunagrawal]

I'm closing this due to lack of a response. Please re-open if this issue still persists. 🙂