IncrementalFixedLagSmoother issue of IndeterminantLinearSystemException

[navid km]

Hello all,

I face the following issue when using IncrementalFixedLagSmoother to add landmarks using BearingRangeFactor.

this happens as soon as we add a BearingRangeFactor and when there is a call to the update() function. 

Here is a simple example similar to PlanarSLAMExample.cpp to reproduce the problem 

remarks: 

* I tried to add another observation to the landmark (l1) and then call update in isam2 but the result is the same.

* Moreover adding a unary factor (i.e. map landmark) to l1 does not solve the issue.

* This is also tested with real data and as soon as we add a landmark with BearingRangeFactor, the same issue happens.

* I use gtsam v4.3.0

TEST(tGraphFactor, IndeterminantLinearSystemException)

{

    // The incremental optimizer uses iSAM2 to perform the nonlinear optimization

    ISAM2Params parameters;

    parameters.relinearizeThreshold = 0.1;

    parameters.relinearizeSkip      = 1;

    IncrementalFixedLagSmoother smootherISAM2(0.0, parameters);

    Values               initialEstimate;

    NonlinearFactorGraph graph;

    // Create the keys X for poses and L for landmarks

    Symbol x1('x', 1);

    Symbol x2('x', 2);

    Symbol l1('l', 1);

    // Add prior node

    auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); // // 30cm std on x,y, 0.1 rad on theta

    graph.add(PriorFactor<Pose2>(x1, Pose2(0.0, 0.0, 0.0), priorNoise));

    // Add odometry factors

    auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta

    graph.emplace_shared<BetweenFactor<Pose2>>(x1, x2, Pose2(2.0, 0.0, 0.0), odometryNoise);

    initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));

    initialEstimate.insert(x2, Pose2(2.3, 0.1, -0.2));

    // Update Isam

    smootherISAM2.update(graph, initialEstimate);

    // clear the buffers

    initialEstimate.clear();

    graph.resize(0);

    // Add Range-Bearing measurements

    auto landmarkNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range

    tF64 range21{2};

    Rot2 bearing21{Rot2::fromDegrees(90)};

    // Add landmark

    graph.emplace_shared<BearingRangeFactor<Pose2, Pose2>>(x2, l1, bearing21, range21, landmarkNoise);

    initialEstimate.insert(l1, Pose2(2.1, 2.2, 0));

    // Update Isam, fixme: issue occurs here!

    smootherISAM2.update(graph, initialEstimate);

    Values results = smootherISAM2.calculateEstimate();

    results.print();

}

I appreciate if you could help.

Best regards

Navid

[Dellaert, Frank]

Probably a global gauge freedom? Adding a prior on x1 should resolve that.

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of navid km <n.mah...@gmail.com>
Sent: Friday, June 18, 2021 07:25
To: gtsam users <gtsam...@googlegroups.com>
Subject: [GTSAM] IncrementalFixedLagSmoother issue of IndeterminantLinearSystemException

 

--
You received this message because you are subscribed to the Google Groups "gtsam users" group.
To unsubscribe from this group and stop receiving emails from it, send an email to gtsam-users...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/2bed4183-c2e5-4b38-a739-ed578e8028a7n%40googlegroups.com.

[navid km]

there is already a prior on x1     

graph.add(PriorFactor<Pose2>(x1, Pose2(0.0, 0.0, 0.0), priorNoise));

and as mentioned adding a prior on l1 did not solve the issue.

/Navid

[dav...@oxfordrobotics.institute]

I'm not sure how the IncrementalFixedLagSmoother behaves when the lag time is set to 0.0 (which you have done in your example). Also you haven't given a timestamp to each node. i.e. the timestamps parameter is left empty when you call smootherISAM2.update(). 

You could try to set these time parameters appropriately to see if it helps.

David

[Dellaert, Frank]

Right! Very weird. Unfortunately I’m abroad now and can’t debug this. Can you check whether it also occurs when you just use the Gauss Newton optimizer? If it does not, there might be an issue with ISam and it would definitely be worth looking into it more closely.

But also see David’s comment, who looked at your code more closely than I did :-)

Best!

Frank

---

From: navid km <n.mah...@gmail.com>
Sent: Friday, June 18, 2021 1:57:05 PM
To: Dellaert, Frank <frank.d...@cc.gatech.edu>
Cc: gtsam users <gtsam...@googlegroups.com>
Subject: Re: [GTSAM] IncrementalFixedLagSmoother issue of IndeterminantLinearSystemException

 

[navid km]

thanks for the replies!

@Dellaert, Frank it seems that the issue is only with ISam, as the test with Levenberg Marquardt optimizer is fine. Shall I test with Gauss Newton?

@dav...@oxfordrobotics.institute

I think setting lag time to zero means batch optimization, but I updated the test case as follows but still the same issue:

TEST(tGraphFactor, IndeterminantLinearSystemException)
{
    // The incremental optimizer uses iSAM2 to perform the nonlinear optimization

    tF64        windowLag_F64 = 10.0;

    ISAM2Params parameters;
    parameters.relinearizeThreshold = 0.1;
    parameters.relinearizeSkip      = 1;

    IncrementalFixedLagSmoother smootherISAM2(windowLag_F64, parameters);

    Values                            initialEstimate;
    NonlinearFactorGraph              graph;
    FixedLagSmoother::KeyTimestampMap newTimestamps;

    // Create the keys X for poses and L for landmarks
    Symbol x1('x', 1);
    Symbol x2('x', 2);
    Symbol l1('l', 1);

    // Add prior node
    auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); // // 30cm std on x,y, 0.1 rad on theta
    graph.add(PriorFactor<Pose2>(x1, Pose2(0.0, 0.0, 0.0), priorNoise));

    newTimestamps[x1] = 0.0;

    // Add odometry factors
    auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
    graph.emplace_shared<BetweenFactor<Pose2>>(x1, x2, Pose2(2.0, 0.0, 0.0), odometryNoise);

    newTimestamps[x2] = 1.0;

    initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
    initialEstimate.insert(x2, Pose2(2.3, 0.1, -0.2));

    // Update Isam

    smootherISAM2.update(graph, initialEstimate, newTimestamps);

    // clear the buffers
    initialEstimate.clear();

    newTimestamps.clear();

    graph.resize(0);

    // Add Range-Bearing measurements
    auto landmarkNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
    tF64 range21{2};
    Rot2 bearing21{Rot2::fromDegrees(90)};

    // Add landmark
    graph.emplace_shared<BearingRangeFactor<Pose2, Pose2>>(x2, l1, bearing21, range21, landmarkNoise);
    initialEstimate.insert(l1, Pose2(2.1, 2.2, 0));

    newTimestamps[l1] = 2.0;

    // Update Isam, fixme: issue occurs here!

    smootherISAM2.update(graph, initialEstimate, newTimestamps);

    Values results = smootherISAM2.calculateEstimate();
    results.print();
}

[navid km]

I think I found where the issue is coming from. when adding the range and bearing factor I was using  Pose2 instead of  Point2 due to the data structure of my unary factor.

Using Point2 type solved the issue. 

graph.emplace_shared<BearingRangeFactor<Pose2, Point2>>(x2, l1, bearing21, range21, landmarkNoise);
initialEstimate.insert(l1, Point2(2.1, 2.2));

I am not sure if this is a bug, but I will test with real data and let you know. 

Thanks for the support!

Navid