testLinearizedFactor.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 
21 #include <gtsam/geometry/Point3.h>
22 #include <gtsam/inference/Key.h>
24 #include <gtsam/nonlinear/Values.h>
27 
28 using namespace gtsam;
29 
30 /* ************************************************************************* */
31 TEST( LinearizedFactor, equals_jacobian )
32 {
33  // Create a Between Factor from a Point3. This is actually a linear factor.
34  Key x1(1);
35  Key x2(2);
36  Values values;
37  values.insert(x1, Point3(-22.4, +8.5, +2.4));
38  values.insert(x2, Point3(-21.0, +5.0, +21.0));
39 
40  Point3 measured(1.0, -2.5, 17.8);
42  BetweenFactor<Point3> betweenFactor(x1, x2, measured, model);
43 
44 
45  // Create two identical factors and make sure they're equal
46  JacobianFactor::shared_ptr jf = std::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values));
47  LinearizedJacobianFactor jacobian1(jf, values);
48  LinearizedJacobianFactor jacobian2(jf, values);
49 
50  CHECK(assert_equal(jacobian1, jacobian2));
51 }
52 
53 /* ************************************************************************* */
54 TEST( LinearizedFactor, clone_jacobian )
55 {
56  // Create a Between Factor from a Point3. This is actually a linear factor.
57  Key x1(1);
58  Key x2(2);
59  Values values;
60  values.insert(x1, Point3(-22.4, +8.5, +2.4));
61  values.insert(x2, Point3(-21.0, +5.0, +21.0));
62 
63  Point3 measured(1.0, -2.5, 17.8);
65  BetweenFactor<Point3> betweenFactor(x1, x2, measured, model);
66 
67  // Create one factor that is a clone of the other and make sure they're equal
68  JacobianFactor::shared_ptr jf = std::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values));
69  LinearizedJacobianFactor jacobian1(jf, values);
70  LinearizedJacobianFactor::shared_ptr jacobian2 = std::static_pointer_cast<LinearizedJacobianFactor>(jacobian1.clone());
71  CHECK(assert_equal(jacobian1, *jacobian2));
72 
73  JacobianFactor::shared_ptr jf1 = std::static_pointer_cast<JacobianFactor>(jacobian1.linearize(values));
74  JacobianFactor::shared_ptr jf2 = std::static_pointer_cast<JacobianFactor>(jacobian2->linearize(values));
75  CHECK(assert_equal(*jf1, *jf2));
76 
77  Matrix information1 = jf1->augmentedInformation();
78  Matrix information2 = jf2->augmentedInformation();
79  CHECK(assert_equal(information1, information2));
80 }
81 
82 /* ************************************************************************* */
83 TEST( LinearizedFactor, add_jacobian )
84 {
85  // Create a Between Factor from a Point3. This is actually a linear factor.
86  Key x1(1);
87  Key x2(2);
88  Values values;
89  values.insert(x1, Point3(-22.4, +8.5, +2.4));
90  values.insert(x2, Point3(-21.0, +5.0, +21.0));
91 
92  Point3 measured(1.0, -2.5, 17.8);
94  BetweenFactor<Point3> betweenFactor(x1, x2, measured, model);
95 
96  // Create two factor graphs, one using 'push_back' and one using 'add' and make sure they're equal
97  JacobianFactor::shared_ptr jf = std::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values));
99  NonlinearFactorGraph graph1; graph1.push_back(jacobian);
100  NonlinearFactorGraph graph2; graph2.push_back(*jacobian);
101 
102  // TODO: When creating a Jacobian from a cached factor, I experienced a problem in the 'add' version
103  // However, I am currently unable to reproduce the error in this unit test.
104  // I don't know if this affects the Hessian version as well.
105  CHECK(assert_equal(graph1, graph2));
106 }
107 
109 //TEST( LinearizedFactor, error_jacobian )
110 //{
111 // // Create a Between Factor from a Point3. This is actually a linear factor.
112 // Key key1(1);
113 // Key key2(2);
114 // Ordering ordering;
115 // ordering.push_back(key1);
116 // ordering.push_back(key2);
117 // Values values;
118 // values.insert(key1, Point3(-22.4, +8.5, +2.4));
119 // values.insert(key2, Point3(-21.0, +5.0, +21.0));
120 //
121 // Point3 measured(1.0, -2.5, 17.8);
122 // SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.1);
123 // BetweenFactor<Point3> betweenFactor(key1, key2, measured, model);
124 //
125 //
126 // // Create a linearized jacobian factors
127 // JacobianFactor::shared_ptr jf = std::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values));
128 // LinearizedJacobianFactor jacobian(jf, values);
129 //
130 //
131 // for(double x1 = -10; x1 < 10; x1 += 2.0) {
132 // for(double y1 = -10; y1 < 10; y1 += 2.0) {
133 // for(double z1 = -10; z1 < 10; z1 += 2.0) {
134 //
135 // for(double x2 = -10; x2 < 10; x2 += 2.0) {
136 // for(double y2 = -10; y2 < 10; y2 += 2.0) {
137 // for(double z2 = -10; z2 < 10; z2 += 2.0) {
138 //
139 // Values linpoint;
140 // linpoint.insert(key1, Point3(x1, y1, z1));
141 // linpoint.insert(key2, Point3(x2, y2, z2));
142 //
143 // // Check that the error of the Linearized Jacobian and the original factor match
144 // // This only works because a BetweenFactor on a Point3 is actually a linear system
145 // double expected_error = betweenFactor.error(linpoint);
146 // double actual_error = jacobian.error(linpoint);
147 // EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 );
148 //
149 // // Check that the linearized factors are identical
150 // GaussianFactor::shared_ptr expected_factor = betweenFactor.linearize(linpoint);
151 // GaussianFactor::shared_ptr actual_factor = jacobian.linearize(linpoint);
152 // CHECK(assert_equal(*expected_factor, *actual_factor));
153 // }
154 // }
155 // }
156 //
157 // }
158 // }
159 // }
160 //
161 //}
162 
163 /* ************************************************************************* */
164 TEST( LinearizedFactor, equals_hessian )
165 {
166  // Create a Between Factor from a Point3. This is actually a linear factor.
167  Key x1(1);
168  Key x2(2);
169  Values values;
170  values.insert(x1, Point3(-22.4, +8.5, +2.4));
171  values.insert(x2, Point3(-21.0, +5.0, +21.0));
172 
173  Point3 measured(1.0, -2.5, 17.8);
175  BetweenFactor<Point3> betweenFactor(x1, x2, measured, model);
176 
177 
178  // Create two identical factors and make sure they're equal
179  JacobianFactor::shared_ptr jf = std::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values));
181  LinearizedHessianFactor hessian1(hf, values);
182  LinearizedHessianFactor hessian2(hf, values);
183 
184  CHECK(assert_equal(hessian1, hessian2));
185 }
186 
187 /* ************************************************************************* */
188 TEST( LinearizedFactor, clone_hessian )
189 {
190  // Create a Between Factor from a Point3. This is actually a linear factor.
191  Key x1(1);
192  Key x2(2);
193  Values values;
194  values.insert(x1, Point3(-22.4, +8.5, +2.4));
195  values.insert(x2, Point3(-21.0, +5.0, +21.0));
196 
197  Point3 measured(1.0, -2.5, 17.8);
199  BetweenFactor<Point3> betweenFactor(x1, x2, measured, model);
200 
201 
202  // Create two identical factors and make sure they're equal
203  JacobianFactor::shared_ptr jf = std::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values));
205  LinearizedHessianFactor hessian1(hf, values);
206  LinearizedHessianFactor::shared_ptr hessian2 = std::static_pointer_cast<LinearizedHessianFactor>(hessian1.clone());
207 
208  CHECK(assert_equal(hessian1, *hessian2));
209 }
210 
211 /* ************************************************************************* */
212 TEST( LinearizedFactor, add_hessian )
213 {
214  // Create a Between Factor from a Point3. This is actually a linear factor.
215  Key x1(1);
216  Key x2(2);
217  Values values;
218  values.insert(x1, Point3(-22.4, +8.5, +2.4));
219  values.insert(x2, Point3(-21.0, +5.0, +21.0));
220 
221  Point3 measured(1.0, -2.5, 17.8);
223  BetweenFactor<Point3> betweenFactor(x1, x2, measured, model);
224 
225 
226  // Create two identical factors and make sure they're equal
227  JacobianFactor::shared_ptr jf = std::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values));
230  NonlinearFactorGraph graph1; graph1.push_back(hessian);
231  NonlinearFactorGraph graph2; graph2.push_back(*hessian);
232 
233  CHECK(assert_equal(graph1, graph2));
234 }
235 
237 //TEST( LinearizedFactor, error_hessian )
238 //{
239 // // Create a Between Factor from a Point3. This is actually a linear factor.
240 // Key key1(1);
241 // Key key2(2);
242 // Ordering ordering;
243 // ordering.push_back(key1);
244 // ordering.push_back(key2);
245 // Values values;
246 // values.insert(key1, Point3(-22.4, +8.5, +2.4));
247 // values.insert(key2, Point3(-21.0, +5.0, +21.0));
248 //
249 // Point3 measured(1.0, -2.5, 17.8);
250 // SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.1);
251 // BetweenFactor<Point3> betweenFactor(key1, key2, measured, model);
252 //
253 //
254 // // Create a linearized hessian factor
255 // JacobianFactor::shared_ptr jf = std::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values));
256 // HessianFactor::shared_ptr hf(new HessianFactor(*jf));
257 // LinearizedHessianFactor hessian(hf, values);
258 //
259 //
260 // for(double x1 = -10; x1 < 10; x1 += 2.0) {
261 // for(double y1 = -10; y1 < 10; y1 += 2.0) {
262 // for(double z1 = -10; z1 < 10; z1 += 2.0) {
263 //
264 // for(double x2 = -10; x2 < 10; x2 += 2.0) {
265 // for(double y2 = -10; y2 < 10; y2 += 2.0) {
266 // for(double z2 = -10; z2 < 10; z2 += 2.0) {
267 //
268 // Values linpoint;
269 // linpoint.insert(key1, Point3(x1, y1, z1));
270 // linpoint.insert(key2, Point3(x2, y2, z2));
271 //
272 // // Check that the error of the Linearized Hessian and the original factor match
273 // // This only works because a BetweenFactor on a Point3 is actually a linear system
274 // double expected_error = betweenFactor.error(linpoint);
275 // double actual_error = hessian.error(linpoint);
276 // EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 );
277 //
278 // // Check that the linearized factors are identical
279 // GaussianFactor::shared_ptr expected_factor = HessianFactor::shared_ptr(new HessianFactor(*betweenFactor.linearize(linpoint)));
280 // GaussianFactor::shared_ptr actual_factor = hessian.linearize(linpoint);
281 // CHECK(assert_equal(*expected_factor, *actual_factor));
282 // }
283 // }
284 // }
285 //
286 // }
287 // }
288 // }
289 //
290 //}
291 
292 /* ************************************************************************* */
293 int main()
294 {
295  TestResult tr; return TestRegistry::runAllTests(tr);
296 }
297 /* ************************************************************************* */
298 
Point2 measured(-17, 30)
#define CHECK(condition)
Definition: Test.h:108
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.
noiseModel::Diagonal::shared_ptr model
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
Some functions to compute numerical derivatives.
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
std::shared_ptr< LinearizedJacobianFactor > shared_ptr
gtsam::NonlinearFactor::shared_ptr clone() const override
A dummy factor that allows a linear factor to act as a nonlinear factor.
std::shared_ptr< This > shared_ptr
shared_ptr to this class
std::shared_ptr< GaussianFactor > linearize(const Values &x) const override
NonlinearFactorGraph graph2()
traits
Definition: chartTesting.h:28
int main()
Pose3 x1
Definition: testPose3.cpp:663
3D Point
A Gaussian factor using the canonical parameters (information form)
void insert(Key j, const Value &val)
Definition: Values.cpp:155
Vector3 Point3
Definition: Point3.h:38
TEST(SmartFactorBase, Pinhole)
std::shared_ptr< LinearizedHessianFactor > shared_ptr
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:594


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:40