Simple regressive case using Pose2 Prior/Between factors; fails to converge with LM

[Terry]

Hi gtsam folks,

I found a scenario where optimization failed to converge (or even find a better solution than the initial)

I'd love some insight into what could be causing it, and if I can find a robust solution. It seems to happen when the initial estimate has heading~=pi/2. Beyond that I'm a bit stumped. Increasing the sigma for the BetweenFactor resolves this specific case.

Is there a way to at least detect this is happening to at least avoid a failure?

Many thanks,

Terry

Here's the code that reproduce the result:

Values initial;

initial.insert(0, Pose2(5.73879859, -0.460481998, -0.112793985));

initial.insert(1, Pose2(-0.372136134, 1.33611664, -1.56068983));

auto nm1 = noiseModel::Isotropic::Sigma(3, 0.316228);

auto f1 = boost::make_shared<BetweenFactor<Pose2> >(0, 1, Pose2(0.375387937, 0.097459428, -0.0294623178), nm1);

auto nm2 = noiseModel::Isotropic::Sigma(3, 0.05);

auto f2 = boost::make_shared<PriorFactor<Pose2> >(0, Pose2(5.73879859, -0.460481998, -0.112793985), nm2);

NonlinearFactorGraph graph;

graph.add<BetweenFactor<Pose2> >(f1);

graph.add<PriorFactor<Pose2> >(f2);

LevenbergMarquardtParams params;

params.setVerbosityLM("TRYDELTA");

gtsam::LevenbergMarquardtOptimizer optimizer(graph, initial, params);

auto optimized = optimizer.optimize();

initial.print("Initial\n");

graph.print("Graph\n");

optimized.print("Optimized\n");

std::cout << "Error: " << optimizer.error() << std::endl;

optimizer.params().print("Parameters:\n");

std::cout << "\n\n\n";

return optimized;

[voltron]

I tried parameter settings to make the solvers more brute force.

I couldn't get the GN or LM solvers to work.

I could get Isam2 to work.

const double init_cost { graph.error(initial) };

ISAM2Params isam2_params;
isam2_params.relinearizeSkip = 1;
isam2_params.enableRelinearization = true;
isam2_params.relinearizeThreshold = 0;
isam2_params.factorization = ISAM2Params::QR;
ISAM2 solver(isam2_params);
solver.update(graph, initial);
for (size_t i = 0; i < 100; ++i)
solver.update();
const Values optimized { solver.calculateBestEstimate() };

// GaussNewtonParams params;
// params.relativeErrorTol = 0;
// params.absoluteErrorTol = 0;
// params.errorTol = 0;
// params.maxIterations = 1000;
// params.linearSolverType = LevenbergMarquardtParams::SEQUENTIAL_QR;
// gtsam::GaussNewtonOptimizer optimizer(graph, initial, params);
// Values optimized = optimizer.optimize();
// initial.print("Initial\n");
// graph.print("Graph\n");
// optimized.print("Optimized\n");

// LevenbergMarquardtParams params;
// //params.setVerbosityLM("TRYDELTA");
// params.relativeErrorTol = 0;
// params.absoluteErrorTol = 0;
// params.errorTol = 0;
// params.maxIterations = 200;
// params.linearSolverType = LevenbergMarquardtParams::SEQUENTIAL_QR;
// params.lambdaUpperBound = 1e100;
// gtsam::LevenbergMarquardtOptimizer optimizer(graph, initial, params);
// Values optimized = optimizer.optimize();
// initial.print("Initial\n");
// //graph.print("Graph\n");
// optimized.print("Optimized\n");

        const double final_cost { graph.error(optimized) };

     std::cout << "initial: " << init_cost << std::endl;
std::cout << "final: " << final_cost << std::endl;

// initial: 236.151
// final: 1.40593e-31

[voltron]

https://github.com/borglab/gtsam/blob/develop/gtsam/slam/BetweenFactor.h#L115

someone has already been here.

define SLOW_BUT_CORRECT_BETWEENFACTOR above your BetweenFactor include and everything works.

#include "gtsam/geometry/Pose2.h"
#define SLOW_BUT_CORRECT_BETWEENFACTOR
#include "gtsam/slam/BetweenFactor.h"
#include "gtsam/nonlinear/LevenbergMarquardtOptimizer.h"

[Terry]

That's done it thank you! Do you know of other implications to running this flag other than it being "slow"? (it appears only marginally slower)