testDummyFactor.cpp
Go to the documentation of this file.
1 
12 
14 
15 #include <gtsam/geometry/Point3.h>
17 
18 using namespace gtsam;
19 
20 const double tol = 1e-9;
21 
22 /* ************************************************************************* */
23 TEST( 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(key1, Point3(1.0, 2.0, 3.0));
40  c.insert(key2, 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);
49  GaussianFactor::shared_ptr expLinearization(new JacobianFactor(
50  key1, Matrix::Zero(3,3), key2, Matrix::Zero(3,3), Z_3x1, model3));
51  EXPECT(assert_equal(*expLinearization, *actLinearization, tol));
52 }
53 
54 /* ************************************************************************* */
55 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
56 /* ************************************************************************* */
Provides additional testing facilities for common data structures.
#define CHECK(condition)
Definition: Test.h:108
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:46
static int runAllTests(TestResult &result)
size_t size() const
Definition: Factor.h:159
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:141
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
double error(const Values &c) const override
static shared_ptr Create(size_t dim)
Definition: NoiseModel.h:610
const Symbol key1('v', 1)
#define EXPECT(condition)
Definition: Test.h:150
Array< double, 1, 3 > e(1./3., 0.5, 2.)
const SharedNoiseModel model3
const std::vector< size_t > & dims() const
std::shared_ptr< This > shared_ptr
shared_ptr to this class
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:134
traits
Definition: chartTesting.h:28
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
3D Point
const KeyVector & keys() const
Access the factor&#39;s involved variable keys.
Definition: Factor.h:142
const G double tol
Definition: Group.h:86
Vector3 Point3
Definition: Point3.h:38
TEST(SmartFactorBase, Pinhole)
std::shared_ptr< GaussianFactor > linearize(const Values &c) const override
Definition: DummyFactor.cpp:39
std::shared_ptr< Diagonal > shared_ptr
Definition: NoiseModel.h:307
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
int main()
const Symbol key2('v', 2)


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