testAttitudeFactor.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 
20 #include <gtsam/base/Testable.h>
25 
26 using namespace std;
27 using namespace gtsam;
28 
29 // *************************************************************************
30 TEST( Rot3AttitudeFactor, Constructor ) {
31 
32  // Example: pitch and roll of aircraft in an ENU Cartesian frame.
33  // If pitch and roll are zero for an aerospace frame,
34  // that means Z is pointing down, i.e., direction of Z = (0,0,-1)
35  Unit3 bZ(0, 0, 1); // reference direction is body Z axis
36  Unit3 nDown(0, 0, -1); // down, in ENU navigation frame, is "measurement"
37 
38  // Factor
39  Key key(1);
40  SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
41  Rot3AttitudeFactor factor0(key, nDown, model);
42  Rot3AttitudeFactor factor(key, nDown, model, bZ);
43  EXPECT(assert_equal(factor0,factor,1e-5));
44 
45  // Create a linearization point at the zero-error point
46  Rot3 nRb;
47  EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(nRb),1e-5));
48 
49  // Calculate numerical derivatives
50  Matrix expectedH = numericalDerivative11<Vector,Rot3>(
51  boost::bind(&Rot3AttitudeFactor::evaluateError, &factor, _1, boost::none),
52  nRb);
53 
54  // Use the factor to calculate the derivative
55  Matrix actualH;
56  factor.evaluateError(nRb, actualH);
57 
58  // Verify we get the expected error
59  EXPECT(assert_equal(expectedH, actualH, 1e-8));
60 }
61 
62 /* ************************************************************************* */
63 // Export Noisemodels
64 // See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
66 
67 /* ************************************************************************* */
68 TEST(Rot3AttitudeFactor, Serialization) {
69  Unit3 nDown(0, 0, -1);
70  SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
71  Rot3AttitudeFactor factor(0, nDown, model);
72 
76 }
77 
78 /* ************************************************************************* */
79 TEST(Rot3AttitudeFactor, CopyAndMove) {
80  Unit3 nDown(0, 0, -1);
81  SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
82  Rot3AttitudeFactor factor(0, nDown, model);
83 
84  // Copy assignable.
86  Rot3AttitudeFactor factor_copied = factor;
87  EXPECT(assert_equal(factor, factor_copied));
88 
89  // Move assignable.
91  Rot3AttitudeFactor factor_moved = std::move(factor_copied);
92  EXPECT(assert_equal(factor, factor_moved));
93 }
94 
95 // *************************************************************************
96 TEST( Pose3AttitudeFactor, Constructor ) {
97 
98  // Example: pitch and roll of aircraft in an ENU Cartesian frame.
99  // If pitch and roll are zero for an aerospace frame,
100  // that means Z is pointing down, i.e., direction of Z = (0,0,-1)
101  Unit3 bZ(0, 0, 1); // reference direction is body Z axis
102  Unit3 nDown(0, 0, -1); // down, in ENU navigation frame, is "measurement"
103 
104  // Factor
105  Key key(1);
106  SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
107  Pose3AttitudeFactor factor0(key, nDown, model);
108  Pose3AttitudeFactor factor(key, nDown, model, bZ);
109  EXPECT(assert_equal(factor0,factor,1e-5));
110 
111  // Create a linearization point at the zero-error point
112  Pose3 T(Rot3(), Point3(-5.0, 8.0, -11.0));
113  EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(T),1e-5));
114 
115  // Calculate numerical derivatives
116  Matrix expectedH = numericalDerivative11<Vector,Pose3>(
117  boost::bind(&Pose3AttitudeFactor::evaluateError, &factor, _1,
118  boost::none), T);
119 
120  // Use the factor to calculate the derivative
121  Matrix actualH;
122  factor.evaluateError(T, actualH);
123 
124  // Verify we get the expected error
125  EXPECT(assert_equal(expectedH, actualH, 1e-8));
126 }
127 
128 /* ************************************************************************* */
129 TEST(Pose3AttitudeFactor, Serialization) {
130  Unit3 nDown(0, 0, -1);
131  SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
132  Pose3AttitudeFactor factor(0, nDown, model);
133 
137 }
138 
139 /* ************************************************************************* */
140 TEST(Pose3AttitudeFactor, CopyAndMove) {
141  Unit3 nDown(0, 0, -1);
142  SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
143  Pose3AttitudeFactor factor(0, nDown, model);
144 
145  // Copy assignable.
147  Pose3AttitudeFactor factor_copied = factor;
148  EXPECT(assert_equal(factor, factor_copied));
149 
150  // Move assignable.
152  Pose3AttitudeFactor factor_moved = std::move(factor_copied);
153  EXPECT(assert_equal(factor, factor_moved));
154 }
155 
156 // *************************************************************************
157 int main() {
158  TestResult tr;
159  return TestRegistry::runAllTests(tr);
160 }
161 // *************************************************************************
int main()
Concept check for values that can be used in unit tests.
static int runAllTests(TestResult &result)
noiseModel::Diagonal::shared_ptr model
Vector evaluateError(const Rot3 &nRb, boost::optional< Matrix & > H=boost::none) const override
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
Definition: Half.h:150
Some functions to compute numerical derivatives.
BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic)
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
Eigen::VectorXd Vector
Definition: Vector.h:38
#define EXPECT(condition)
Definition: Test.h:151
Eigen::Triplet< double > T
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::Point3 bZ(0, 0, 1)
Vector evaluateError(const Pose3 &nTb, boost::optional< Matrix & > H=boost::none) const override
traits
Definition: chartTesting.h:28
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
Vector3 Point3
Definition: Point3.h:35
TEST(Rot3AttitudeFactor, Constructor)
Rot3 nRb
static const Eigen::MatrixBase< Vector2 >::ConstantReturnType Z_2x1
Definition: Vector.h:45
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:46:12