testAntiFactor.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
19 
20 #include <gtsam/slam/AntiFactor.h>
24 #include <gtsam/nonlinear/Values.h>
29 #include <gtsam/geometry/Pose3.h>
30 
31 using namespace std;
32 using namespace gtsam;
33 
34 /* ************************************************************************* */
35 TEST( AntiFactor, NegativeHessian)
36 {
37  // The AntiFactor should produce a Hessian Factor with negative matrices
38 
39  // Create linearization points
40  Pose3 pose1(Rot3(), Point3(0, 0, 0));
41  Pose3 pose2(Rot3(), Point3(2, 1, 3));
42  Pose3 z(Rot3(), Point3(1, 1, 1));
43  SharedNoiseModel sigma(noiseModel::Unit::Create(6));
44 
45  // Create a configuration corresponding to the ground truth
46  Values values;
47  values.insert(1, pose1);
48  values.insert(2, pose2);
49 
50  // Create a "standard" factor
51  BetweenFactor<Pose3>::shared_ptr originalFactor(new BetweenFactor<Pose3>(1, 2, z, sigma));
52 
53  // Linearize it into a Jacobian Factor
54  GaussianFactor::shared_ptr originalJacobian = originalFactor->linearize(values);
55 
56  // Convert it to a Hessian Factor
57  HessianFactor::shared_ptr originalHessian = HessianFactor::shared_ptr(new HessianFactor(*originalJacobian));
58 
59  // Create the AntiFactor version of the original nonlinear factor
60  AntiFactor::shared_ptr antiFactor(new AntiFactor(originalFactor));
61 
62  // Linearize the AntiFactor into a Hessian Factor
63  GaussianFactor::shared_ptr antiGaussian = antiFactor->linearize(values);
64  HessianFactor::shared_ptr antiHessian = std::dynamic_pointer_cast<HessianFactor>(antiGaussian);
65 
66  Matrix expected_information = -originalHessian->information();
67  Matrix actual_information = antiHessian->information();
68  EXPECT(assert_equal(expected_information, actual_information));
69 
70  Vector expected_linear = -originalHessian->linearTerm();
71  Vector actual_linear = antiHessian->linearTerm();
72  EXPECT(assert_equal(expected_linear, actual_linear));
73 
74  double expected_f = -originalHessian->constantTerm();
75  double actual_f = antiHessian->constantTerm();
76  EXPECT_DOUBLES_EQUAL(expected_f, actual_f, 1e-5);
77 }
78 
79 /* ************************************************************************* */
80 TEST( AntiFactor, EquivalentBayesNet)
81 {
82  // Test the AntiFactor by creating a simple graph and eliminating into a BayesNet
83  // Then add an additional factor and the corresponding AntiFactor and eliminate
84  // The resulting BayesNet should be identical to the first
85 
86  Pose3 pose1(Rot3(), Point3(0, 0, 0));
87  Pose3 pose2(Rot3(), Point3(2, 1, 3));
88  Pose3 z(Rot3(), Point3(1, 1, 1));
89  SharedNoiseModel sigma(noiseModel::Unit::Create(6));
90 
92  graph.addPrior(1, pose1, sigma);
93  graph.emplace_shared<BetweenFactor<Pose3> >(1, 2, pose1.between(pose2), sigma);
94 
95  // Create a configuration corresponding to the ground truth
96  Values values;
97  values.insert(1, pose1);
98  values.insert(2, pose2);
99 
100  // Define an elimination ordering
101  Ordering ordering = graph.orderingCOLAMD();
102 
103  // Eliminate into a BayesNet
104  GaussianFactorGraph lin_graph = *graph.linearize(values);
105  GaussianBayesNet::shared_ptr expectedBayesNet = lin_graph.eliminateSequential(ordering);
106 
107  // Back-substitute to find the optimal deltas
108  VectorValues expectedDeltas = expectedBayesNet->optimize();
109 
110  // Add an additional factor between Pose1 and Pose2
112  graph.push_back(f1);
113 
114  // Add the corresponding AntiFactor between Pose1 and Pose2
116  graph.push_back(f2);
117 
118  // Again, Eliminate into a BayesNet
119  GaussianFactorGraph lin_graph1 = *graph.linearize(values);
120  GaussianBayesNet::shared_ptr actualBayesNet = lin_graph1.eliminateSequential(ordering);
121 
122  // Back-substitute to find the optimal deltas
123  VectorValues actualDeltas = actualBayesNet->optimize();
124 
125  // Verify the BayesNets are identical
126  CHECK(assert_equal(*expectedBayesNet, *actualBayesNet, 1e-5));
127  CHECK(assert_equal(expectedDeltas, actualDeltas, 1e-5));
128 }
129 
130 /* ************************************************************************* */
131  int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
132 /* ************************************************************************* */
#define CHECK(condition)
Definition: Test.h:108
TEST(AntiFactor, NegativeHessian)
int main()
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
static int runAllTests(TestResult &result)
A non-templated config holding any types of Manifold-group elements.
Factor Graph consisting of non-linear factors.
std::shared_ptr< This > shared_ptr
A shared_ptr to this class.
std::shared_ptr< AntiFactor > shared_ptr
Definition: AntiFactor.h:44
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
leaf::MyValues values
Definition: BFloat16.h:88
double f2(const Vector2 &x)
NonlinearFactorGraph graph
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
static enum @1107 ordering
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
std::shared_ptr< GaussianFactorGraph > linearize(const Values &linearizationPoint) const
Linearize a nonlinear factor graph.
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
std::shared_ptr< This > shared_ptr
std::shared_ptr< BetweenFactor > shared_ptr
Definition: BetweenFactor.h:63
Eigen::VectorXd Vector
Definition: Vector.h:38
#define EXPECT(condition)
Definition: Test.h:150
Matrix information() const override
Contains the HessianFactor class, a general quadratic factor.
std::shared_ptr< BayesNetType > eliminateSequential(OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Linear Factor Graph where all factors are Gaussians.
std::shared_ptr< GaussianFactor > linearize(const Values &x) const override
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
traits
Definition: chartTesting.h:28
Point2 f1(const Point3 &p, OptionalJacobian< 2, 3 > H)
Class between(const Class &g) const
Definition: Lie.h:52
static Pose3 pose2
A Gaussian factor using the canonical parameters (information form)
Chordal Bayes Net, the result of eliminating a factor graph.
static const double sigma
void insert(Key j, const Value &val)
Definition: Values.cpp:155
Vector3 Point3
Definition: Point3.h:38
Base class and parameters for nonlinear optimization algorithms.
3D Pose
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:37:49