Issue with iSAM2: IndeterminantLinearSystemException

[catproof]

I made a very simple factor graph that can be solved with a batch optimization via Levenberg-Marquardt. However, when I try solving it using iSAM2, I get the Indeterminant Linear System Exception. Here is my code:

#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <iterator>
#include <random>

using namespace std;
using namespace gtsam;

//Unary Factor code taken from the examples/LocalizationExample.cpp file in the GTSAM repo
class UnaryFactor: public NoiseModelFactor1<Pose3> {
  // The factor will hold a measurement consisting of an (X,Y) location
  // We could this with a Point2 but here we just use two doubles
  double mx_, my_, mz_;

 public:
  /// shorthand for a smart pointer to a factor
  typedef boost::shared_ptr<UnaryFactor> shared_ptr;

  // The constructor requires the variable key, the (X, Y) measurement value, and the noise model
  UnaryFactor(Key j, double x, double y, double z, const SharedNoiseModel& model):
    NoiseModelFactor1<Pose3>(model, j), mx_(x), my_(y), mz_(z) {}

  ~UnaryFactor() override {}

  // Using the NoiseModelFactor1 base class there are two functions that must be overridden.
  // The first is the 'evaluateError' function. This function implements the desired measurement
  // function, returning a vector of errors when evaluated at the provided variable value. It
  // must also calculate the Jacobians for this measurement function, if requested.
  Vector evaluateError(const Pose3& q, boost::optional<Matrix&> H = boost::none) const override {
    // The measurement function for a GPS-like measurement h(q) which predicts the measurement (m) is h(q) = q, q = [qx qy qtheta]
    // The error is then simply calculated as E(q) = h(q) - m:
    // error_x = q.x - mx
    // error_y = q.y - my
    // Node's orientation reflects in the Jacobian, in tangent space this is equal to the right-hand rule rotation matrix
    // H =  [ cos(q.theta)  -sin(q.theta) 0 ]
    //      [ sin(q.theta)   cos(q.theta) 0 ]
    //const Rot2& R = q.rotation();
    //if (H) (*H) = (gtsam::Matrix(2, 3) << R.c(), -R.s(), 0.0, R.s(), R.c(), 0.0).finished();
    if (H){
      H->resize(3,6); // jacobian wrt pose
      (*H) << Matrix3::Zero(),  q.rotation().matrix();
    }
    return (Vector(3) << q.x() - mx_, q.y() - my_, q.z() - mz_).finished();
  }

  // The second is a 'clone' function that allows the factor to be copied. Under most
  // circumstances, the following code that employs the default copy constructor should
  // work fine.
  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new UnaryFactor(*this))); }

  // Additionally, we encourage you the use of unit testing your custom factors,
  // (as all GTSAM factors are), in which you would need an equals and print, to satisfy the
  // GTSAM_CONCEPT_TESTABLE_INST(T) defined in Testable.h, but these are not needed below.
};  // UnaryFactor

int main(int argc, char** argv) {
  bool use_isam = true;
  ISAM2* isam2 = 0;
  if(use_isam){
    ISAM2Params parameters;
    parameters.relinearizeThreshold = 0.01;
    parameters.relinearizeSkip = 1;
    //suggested here: https://github.com/borglab/gtsam/issues/939
    //parameters.factorization = gtsam::ISAM2Params::QR;
    //suggested here:
    //parameters.setOptimizationParams(ISAM2DoglegParams());
    isam2 = new ISAM2(parameters);

  } else {
    printf("Using Levenberg Marquardt Optimizer\n");
  }

  NonlinearFactorGraph* graph = new NonlinearFactorGraph();
  Values initial;

  //noise models
  auto between_factor_noise = noiseModel::Diagonal::Sigmas((Vector(6)<<0.03, 0.03, 0.03, 0.1, 0.1, 0.1).finished());
  auto unary_factor_std_dev = 0.1;
  auto unary_factor_noise = noiseModel::Diagonal::Sigmas(Vector3(unary_factor_std_dev, unary_factor_std_dev, unary_factor_std_dev));

  Values result;

  graph->emplace_shared<BetweenFactor<Pose3> >(0, 1, Pose3(Rot3(Matrix3::Identity()), Point3(0.1,0.1,0.1)), between_factor_noise);
  //GPSFactor gps_factor(0, Point3(0,0,0),unary_factor_noise);
  //graph->add(gps_factor);
  graph->add(boost::make_shared<UnaryFactor>(0, 0, 0, 0, unary_factor_noise));

  initial.insert(0, Pose3(Rot3(Matrix3::Identity()), Point3(0.0,0.0,0.0)));
  initial.insert(1, Pose3(Rot3(Matrix3::Identity()), Point3(0.1,0.1,0.1)));

  if (use_isam) {
    isam2->update(*graph, initial);
    result = isam2->calculateEstimate();

    graph->resize(0);
    initial.clear();
  } else {
    // batch optimization using Levenberg-Marquardt
    result = LevenbergMarquardtOptimizer(*graph, initial).optimize();
  }
  auto pose = result.at<Pose3>(0);
  printf("first pose: %f,%f,%f\n",pose.z(), pose.x(), pose.y());
  pose = result.at<Pose3>(1);
  printf("second pose: %f,%f,%f\n",pose.z(), pose.x(), pose.y());

  return 0;
}

It looks like this is an issue with how iSAM2 solves small graphs... other people have had this issue

In my code you can toggle the "use_isam" bool to see for yourself that the batch optimization works, but iSAM2 doesn't. Also, I tried toggling QR factorization as well as the dogleg vs gauss newton optimizers as suggested by others (here and here). This seemed to have no effect.

[dellaert]

OK, please help us solve this? What I would do to track down is is (a) break a debugger on the exception to see the actual reason, (b) look at the call-stack to set a breakpoint before it happens, examine what's going on.

[catproof]

Hi @dellaert do you have steps for setting up a debug environment? I am a bit of a C++ noob. See this google conversation. I'm trying to figure out how to debug one of the example files I modified.

[catproof]

I know how to setup debugging for very simple C++ programs, but am struggling to do it with the example files in GTSAM as they are a part of a larger build process.

[dellaert]

I do this in VS Code, with lldb.

[mmattamala]

@NickPerezCarletonUniversity I think that the issue is that your problem is ill-defined, and that breaks iSAM. A simple check that I usually do is thinking of the problem like a linear system:

• You have 2 Pose3 unknowns, each one is 6DoF -> so we can think of it as if you have 12 unknowns
• You are adding 1 BetweenFactor (Pose3) between the 2 poses -> that's like having 6 equations
• Additionally you have your Unary factor. It receives a Pose3 (6DoF) but the output is 3 Dof (Vector3). -> 3 equations

In summary, you have 12 unknowns but 9 equations, so the system is ill-defined, and that's a potential issue with iSAM. However, it should also break a batch optimization with Gauss Newton or DogLeg instead of Levenberg Marquardt. The reason why LM works anyways is because the damping factor you add when you are solving the normal equations improves the conditioning of the system (you can imagine it as setting the value of the unconstrained unknowns as the same initial value as you gave, so you get "3 extra equations" to match your 12 unknowns) but that doesn't solve the core issue in your formulation.

I also noticed that you added a GPS factor (the commented part), but that probably doesn't help either. The reason is that both the GPS factor and the Unary factor are a function of the position variables, not the orientation ones. So you get redundancy in equations to constrain the position unknowns, but you are still missing equations to solve for the orientation.

If you want to try again with iSAM, you can try adding a Pose3 prior to the second pose, with a large covariance, so it doesn't affect the contribution of the other factors but still ensures that all the variables are constrained. I hope it helps.

[catproof]

hi @mmattamala thank you for your insight. You are correct, it seems that I don't specify the orientation of the robot at any point.

I tried doing batch optimization using Gauss Newton and Dogleg optimizers. Contrary to what you said, it still works (despite in fact being underconstrained).

[catproof]

I am still able to get iSAM2 to work without specifying the orientation of the robot. See this google conversation. I needed to solve the first 100 poses with batch optimization, then if I switch to iSAM2 after that point onwards, I no longer get the error.

I also tried adding a prior (like you mentioned) to get iSAM2 to work, and indeed that also resolves the issue without needing to do any batch optimization.

[varunagrawal]

@NickPerezCarletonUniversity if this is resolved, can you please close this issue?