1/**
2 * @file testDummyFactor.cpp
3 *
4 * @brief A simple dummy nonlinear factor that can be used to enforce connectivity
5 * without adding any real information
6 *
7 * @date Sep 10, 2012
8 * @author Alex Cunningham
9 */
10
11#include <CppUnitLite/TestHarness.h>
12
13#include <gtsam_unstable/slam/DummyFactor.h>
14
15#include <gtsam/geometry/Point3.h>
16#include <gtsam/base/TestableAssertions.h>
17
18using namespace gtsam;
19
20const double tol = 1e-9;
21
22/* ************************************************************************* */
23TEST( testDummyFactor, basics ) {
24
25 gtsam::Key key1 = 7, key2 = 9;
26 size_t dim1 = 3, dim2 = 3;
27 DummyFactor dummyfactor(key1, dim1, key2, dim2);
28
29 // verify contents
30 LONGS_EQUAL(2, dummyfactor.size());
31 EXPECT_LONGS_EQUAL(key1, dummyfactor.keys()[0]);
32 EXPECT_LONGS_EQUAL(key2, dummyfactor.keys()[1]);
33
34 LONGS_EQUAL(2, dummyfactor.dims().size());
35 EXPECT_LONGS_EQUAL(dim1, dummyfactor.dims()[0]);
36 EXPECT_LONGS_EQUAL(dim2, dummyfactor.dims()[1]);
37
38 Values c;
39 c.insert(j: key1, val: Point3(1.0, 2.0, 3.0));
40 c.insert(j: key2, val: Point3(4.0, 5.0, 6.0));
41
42 // errors - all zeros
43 DOUBLES_EQUAL(0.0, dummyfactor.error(c), tol);
44
45 // linearization: all zeros
46 GaussianFactor::shared_ptr actLinearization = dummyfactor.linearize(c);
47 CHECK(actLinearization);
48 noiseModel::Diagonal::shared_ptr model3 = noiseModel::Unit::Create(dim: 3);
49 GaussianFactor::shared_ptr expLinearization(new JacobianFactor(
50 key1, Matrix::Zero(rows: 3,cols: 3), key2, Matrix::Zero(rows: 3,cols: 3), Z_3x1, model3));
51 EXPECT(assert_equal(*expLinearization, *actLinearization, tol));
52}
53
54/* ************************************************************************* */
55int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr); }
56/* ************************************************************************* */
57