12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed ILS error in example

release/4.3a0
Frank Dellaert 2019-05-15 22:33:22 -04:00
parent 3f487c24f7
commit 70d3c9044d
1 changed files with 5 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
tests/testNonlinearFactorGraph.cpp
View File

@ -202,7 +202,8 @@ TEST(NonlinearFactorGraph, UpdateCholesky) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Example from issue #452 which threw an ILS error // Example from issue #452 which threw an ILS error. The reason was a very
// weak prior on heading, which was tightened, and the ILS disappeared.
TEST(testNonlinearFactorGraph, eliminate) { TEST(testNonlinearFactorGraph, eliminate) {
// Linearization point // Linearization point
Pose2 T11(0, 0, 0); Pose2 T11(0, 0, 0);
@ -219,18 +220,14 @@ TEST(testNonlinearFactorGraph, eliminate) {
graph.add(PriorFactor<Pose2>(21, T21, prior)); graph.add(PriorFactor<Pose2>(21, T21, prior));
// Odometry // Odometry
auto model = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 1e6)); auto model = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.3));
graph.add(BetweenFactor<Pose2>(11, 12, T11.between(T12), model)); graph.add(BetweenFactor<Pose2>(11, 12, T11.between(T12), model));
graph.add(BetweenFactor<Pose2>(21, 22, T21.between(T22), model)); graph.add(BetweenFactor<Pose2>(21, 22, T21.between(T22), model));
// Range // Range factor
auto model_rho = noiseModel::Isotropic::Sigma(1, 0.01); auto model_rho = noiseModel::Isotropic::Sigma(1, 0.01);
graph.add(RangeFactor<Pose2>(12, 22, 1.0, model_rho)); graph.add(RangeFactor<Pose2>(12, 22, 1.0, model_rho));
// Print graph
GTSAM_PRINT(graph);
double sigma = 0.1;
Values values; Values values;
values.insert(11, T11.retract(Vector3(0.1,0.2,0.3))); values.insert(11, T11.retract(Vector3(0.1,0.2,0.3)));
values.insert(12, T12); values.insert(12, T12);
@ -242,6 +239,7 @@ TEST(testNonlinearFactorGraph, eliminate) {
Ordering ordering; Ordering ordering;
ordering += 11, 21, 12, 22; ordering += 11, 21, 12, 22;
auto bn = linearized->eliminateSequential(ordering); auto bn = linearized->eliminateSequential(ordering);
EXPECT_LONGS_EQUAL(4, bn->size());
} }
/* ************************************************************************* */ /* ************************************************************************* */