Example coded, throws ILS
parent
912c091dc9
commit
ed721a152a
|
@ -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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
Loading…
Reference in New Issue