Example coded, throws ILS

release/4.3a0
Frank Dellaert 2019-05-15 20:55:16 -04:00
parent 912c091dc9
commit ed721a152a
1 changed files with 57 additions and 10 deletions

View File

@ -17,16 +17,6 @@
* @author Christian Potthast
*/
/*STL/C++*/
#include <iostream>
using namespace std;
#include <boost/assign/std/list.hpp>
#include <boost/assign/std/set.hpp>
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Matrix.h>
#include <tests/smallExample.h>
@ -34,7 +24,21 @@ using namespace boost::assign;
#include <gtsam/inference/Symbol.h>
#include <gtsam/symbolic/SymbolicFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/sam/RangeFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/list.hpp>
#include <boost/assign/std/set.hpp>
using namespace boost::assign;
/*STL/C++*/
#include <iostream>
using namespace std;
using namespace gtsam;
using namespace example;
@ -197,6 +201,49 @@ TEST(NonlinearFactorGraph, UpdateCholesky) {
EXPECT(assert_equal(initial, fg.updateCholesky(initial, boost::none, dampen), 1e-6));
}
/* ************************************************************************* */
// Example from issue #452 which threw an ILS error
TEST(testNonlinearFactorGraph, eliminate) {
// Linearization point
Pose2 T11(0, 0, 0);
Pose2 T12(1, 0, 0);
Pose2 T21(0, 1, 0);
Pose2 T22(1, 1, 0);
// Factor graph
auto graph = NonlinearFactorGraph();
// Priors
auto prior = noiseModel::Isotropic::Sigma(3, 1);
graph.add(PriorFactor<Pose2>(11, T11, prior));
graph.add(PriorFactor<Pose2>(21, T21, prior));
// Odometry
auto model = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 1e6));
graph.add(BetweenFactor<Pose2>(11, 12, T11.between(T12), model));
graph.add(BetweenFactor<Pose2>(21, 22, T21.between(T22), model));
// Range
auto model_rho = noiseModel::Isotropic::Sigma(1, 0.01);
graph.add(RangeFactor<Pose2>(12, 22, 1.0, model_rho));
// Print graph
GTSAM_PRINT(graph);
double sigma = 0.1;
Values values;
values.insert(11, T11.retract(Vector3(0.1,0.2,0.3)));
values.insert(12, T12);
values.insert(21, T21);
values.insert(22, T22);
auto linearized = graph.linearize(values);
// Eliminate
Ordering ordering;
ordering += 11, 21, 12, 22;
auto bn = linearized->eliminateSequential(ordering);
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */