testTSAMFactors.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 
22 
23 using namespace std;
24 using namespace gtsam;
25 
26 Key i(1), j(2); // Key for pose and point
27 
28 //*************************************************************************
29 TEST( DeltaFactor, all ) {
30  // Create a factor
31  Point2 measurement(1, 1);
32  static SharedNoiseModel model(noiseModel::Unit::Create(2));
33  DeltaFactor factor(i, j, measurement, model);
34 
35  // Set the linearization point
36  Pose2 pose(1, 2, 0);
37  Point2 point(4, 11);
38  Vector2 expected(4 - 1 - 1, 11 - 2 - 1);
39 
40  // Use the factor to calculate the Jacobians
41  Matrix H1Actual, H2Actual;
42  Vector actual = factor.evaluateError(pose, point, H1Actual, H2Actual);
43  EXPECT(assert_equal(expected, actual, 1e-9));
44 
45  // Use numerical derivatives to calculate the Jacobians
46  Matrix H1Expected, H2Expected;
47  H1Expected = numericalDerivative11<Vector2, Pose2>(
48  boost::bind(&DeltaFactor::evaluateError, &factor, _1, point, boost::none,
49  boost::none), pose);
50  H2Expected = numericalDerivative11<Vector2, Point2>(
51  boost::bind(&DeltaFactor::evaluateError, &factor, pose, _1, boost::none,
52  boost::none), point);
53 
54  // Verify the Jacobians are correct
55  EXPECT(assert_equal(H1Expected, H1Actual, 1e-9));
56  EXPECT(assert_equal(H2Expected, H2Actual, 1e-9));
57 }
58 
59 //*************************************************************************
61  // Create a factor
62  Key b1(10), b2(20);
63  Point2 measurement(1, 1);
64  static SharedNoiseModel model(noiseModel::Unit::Create(2));
65  DeltaFactorBase factor(b1, i, b2, j, measurement, model);
66 
67  // Set the linearization point
68  Pose2 base1, base2(1, 0, 0);
69  Pose2 pose(1, 2, 0);
70  Point2 point(4, 11);
71  Vector2 expected(4 + 1 - 1 - 1, 11 - 2 - 1);
72 
73  // Use the factor to calculate the Jacobians
74  Matrix H1Actual, H2Actual, H3Actual, H4Actual;
75  Vector actual = factor.evaluateError(base1, pose, base2, point, H1Actual,
76  H2Actual, H3Actual, H4Actual);
77  EXPECT(assert_equal(expected, actual, 1e-9));
78 
79  // Use numerical derivatives to calculate the Jacobians
80  Matrix H1Expected, H2Expected, H3Expected, H4Expected;
81  H1Expected = numericalDerivative11<Vector2, Pose2>(
82  boost::bind(&DeltaFactorBase::evaluateError, &factor, _1, pose, base2,
83  point, boost::none, boost::none, boost::none, boost::none), base1);
84  H2Expected = numericalDerivative11<Vector2, Pose2>(
85  boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, _1, base2,
86  point, boost::none, boost::none, boost::none, boost::none), pose);
87  H3Expected = numericalDerivative11<Vector2, Pose2>(
88  boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, _1,
89  point, boost::none, boost::none, boost::none, boost::none), base2);
90  H4Expected = numericalDerivative11<Vector2, Point2>(
91  boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, base2,
92  _1, boost::none, boost::none, boost::none, boost::none), point);
93 
94  // Verify the Jacobians are correct
95  EXPECT(assert_equal(H1Expected, H1Actual, 1e-9));
96  EXPECT(assert_equal(H2Expected, H2Actual, 1e-9));
97  EXPECT(assert_equal(H3Expected, H3Actual, 1e-9));
98  EXPECT(assert_equal(H4Expected, H4Actual, 1e-9));
99 }
100 
101 //*************************************************************************
103  // Create a factor
104  Key b1(10), b2(20);
105  Pose2 measurement(1, 1, 0);
106  static SharedNoiseModel model(noiseModel::Unit::Create(2));
107  OdometryFactorBase factor(b1, i, b2, j, measurement, model);
108 
109  // Set the linearization pose2
110  Pose2 base1, base2(1, 0, 0);
111  Pose2 pose1(1, 2, 0), pose2(4, 11, 0);
112  Vector3 expected(4 + 1 - 1 - 1, 11 - 2 - 1, 0);
113 
114  // Use the factor to calculate the Jacobians
115  Matrix H1Actual, H2Actual, H3Actual, H4Actual;
116  Vector actual = factor.evaluateError(base1, pose1, base2, pose2, H1Actual,
117  H2Actual, H3Actual, H4Actual);
118  EXPECT(assert_equal(expected, actual, 1e-9));
119 
120  // Use numerical derivatives to calculate the Jacobians
121  Matrix H1Expected, H2Expected, H3Expected, H4Expected;
122  H1Expected = numericalDerivative11<Vector3, Pose2>(
123  boost::bind(&OdometryFactorBase::evaluateError, &factor, _1, pose1, base2,
124  pose2, boost::none, boost::none, boost::none, boost::none), base1);
125  H2Expected = numericalDerivative11<Vector3, Pose2>(
126  boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, _1, base2,
127  pose2, boost::none, boost::none, boost::none, boost::none), pose1);
128  H3Expected = numericalDerivative11<Vector3, Pose2>(
129  boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, _1,
130  pose2, boost::none, boost::none, boost::none, boost::none), base2);
131  H4Expected = numericalDerivative11<Vector3, Pose2>(
132  boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1,
133  base2, _1, boost::none, boost::none, boost::none, boost::none),
134  pose2);
135 
136  // Verify the Jacobians are correct
137  EXPECT(assert_equal(H1Expected, H1Actual, 1e-9));
138  EXPECT(assert_equal(H2Expected, H2Actual, 1e-9));
139  EXPECT(assert_equal(H3Expected, H3Actual, 1e-9));
140  EXPECT(assert_equal(H4Expected, H4Actual, 1e-9));
141 }
142 
143 //*************************************************************************
144 int main() {
145  TestResult tr;
146  return TestRegistry::runAllTests(tr);
147 }
148 //*************************************************************************
149 
Vector evaluateError(const Pose2 &base1, const Pose2 &pose, const Pose2 &base2, const Point2 &point, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none, boost::optional< Matrix & > H4=boost::none) const override
Evaluate measurement error h(x)-z.
Definition: TSAMFactors.h:77
Key i(1)
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
noiseModel::Diagonal::shared_ptr model
Matrix expected
Definition: testMatrix.cpp:974
Vector2 Point2
Definition: Point2.h:27
Key j(2)
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
double measurement(10.0)
Definition: Half.h:150
Some functions to compute numerical derivatives.
TSAM 1 Factors, simpler than the hierarchical TSAM 2.
Point3 point(10, 0,-5)
Eigen::VectorXd Vector
Definition: Vector.h:38
static const Pose3 pose(Rot3(Vector3(1,-1,-1).asDiagonal()), Point3(0, 0, 0.5))
Vector evaluateError(const Pose2 &pose, const Point2 &point, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Evaluate measurement error h(x)-z.
Definition: TSAMFactors.h:48
#define EXPECT(condition)
Definition: Test.h:151
Vector2 b2(4,-5)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Vector evaluateError(const Pose2 &base1, const Pose2 &pose1, const Pose2 &base2, const Pose2 &pose2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none, boost::optional< Matrix & > H4=boost::none) const override
Evaluate measurement error h(x)-z.
Definition: TSAMFactors.h:132
TEST(DeltaFactor, all)
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
traits
Definition: chartTesting.h:28
int main()
Vector2 b1(2,-1)
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
static const Pose3 pose2
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:734


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:50:20