Optimization fails when two different poses are initialized with the same value
Issue #266
new
When performing pose slam with GTSAM, and initializing two poses with the same value (because the vehicle is static in between, and the two poses correspond to observations of different sensors at different timestamps), optimization fails ("Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" at first iteration).
Slightly changing one of the pose is enough to have the optimization succeed.
Is this normal behavior?
Comments (3)
-
-
reporter
Interesting, thanks, but I'm not sure this is the case : the only factor that involves both of the poses is a BetweenFactor<Pose2> between them. In addition one is involved in a GpsFactor, and the other one in multiple BetweenFactor<Pose2> with other poses.
If you think this is not normal I can try to make a minimal example.
-
Sure, maybe just add a PR with a (failing) test in testNonlinearOptimizer.cpp and I can take a look.
- Log in to comment
Hard to say without more context, but my guess is that your measurement functions are undefined for this case, e.g., if you only have bearing measurements then two bearings from the same location do not intersect in a single point. Just guessing...