search

borglab / gtsam

GTSAM is a library of C++ classes that implement smoothing and mapping (SAM) in robotics and vision, using factor graphs and Bayes networks as the underlying computing paradigm rather than sparse matrices.
http://gtsam.org
Other
2.63k stars 767 forks source link

Requested to eliminate a key that is not in the factors #1752

Open GPrathap opened 6 months ago

GPrathap commented 6 months ago

I am using develop branch and I have encountered this issue when I want to extend this example with IncrementalFixedLagSmoother. It ran a few iterations and then gave this error. Not sure how to solve it. Here I attached the response when it fails. 

        Factor Graph:
        size: 14

        Factor 0:   keys = { 14 }
          noise model: unit (2) 

        Factor 1: BetweenFactor(12,14)
          measured:  (-0.20871, -1.8819e-05, 3.2187e-05)
        isotropic dim=3 sigma=0.01

        Factor 2: BearingRangeFactor
        Factor 2:   keys = { 14 179 }
          noise model: unit (2) 
        ExpressionFactor with measurement: bearing : 0.1352
        range  4.9657

        Factor 3: BearingRangeFactor
        Factor 3:   keys = { 14 195 }
          noise model: unit (2) 
        ExpressionFactor with measurement: bearing : -0.27905
        range  2.3177

        Factor 4: BearingRangeFactor
        Factor 4:   keys = { 14 197 }
          noise model: unit (2) 
        ExpressionFactor with measurement: bearing : -0.13816
        range  4.9287

        Factor 5: BearingRangeFactor
        Factor 5:   keys = { 14 161 }
          noise model: unit (2) 
        ExpressionFactor with measurement: bearing : 0.38201
        range  5.2839

        Factor 6: BearingRangeFactor
        Factor 6:   keys = { 14 215 }
          noise model: unit (2) 
        ExpressionFactor with measurement: bearing : -0.38549
        range  5.2926

        Factor 7: BearingRangeFactor
        Factor 7:   keys = { 14 171 }
          noise model: unit (2) 
        ExpressionFactor with measurement: bearing : 3.0486
        range  6.4876

        Factor 8: BearingRangeFactor
        Factor 8:   keys = { 14 173 }
          noise model: unit (2) 
        ExpressionFactor with measurement: bearing : 2.9632
        range  3.5342

        Factor 9: BearingRangeFactor
        Factor 9:   keys = { 14 191 }
          noise model: unit (2) 
        ExpressionFactor with measurement: bearing : -2.9422
        range  3.5853

        Factor 10: BearingRangeFactor
        Factor 10:   keys = { 14 189 }
          noise model: unit (2) 
        ExpressionFactor with measurement: bearing : -3.0251
        range  6.4933

        Factor 11: BearingRangeFactor
        Factor 11:   keys = { 14 153 }
          noise model: unit (2) 
        ExpressionFactor with measurement: bearing : 2.8574
        range  6.8032

        Factor 12: BearingRangeFactor
        Factor 12:   keys = { 14 207 }
          noise model: unit (2) 
        ExpressionFactor with measurement: bearing : -2.8337
        range  6.8001

        Factor 13: BearingRangeFactor
        Factor 13:   keys = { 14 135 }
          noise model: unit (2) 
        ExpressionFactor with measurement: bearing : 2.697
        range  7.4595

        Initial Estimate:

        Values with 2 values:
        Value 14: (gtsam::Pose2)
        (27.89, -29.051, -1.4319)

        Value 195: (Eigen::Matrix<double, 2, 1, 0, 2, 1>)
        [
                27.55;
                -31.4
        ]
        terminate called after throwing an instance of 'std::runtime_error'
          what():  Requested to eliminate a key that is not in the factors
GPrathap commented 6 months ago

I believe this is a bug, when I use BatchFixedLagSmoother with the same setup it works fine.