testRelativeElevationFactor.cpp
Go to the documentation of this file.
1 
8 #include <CppUnitLite/TestHarness.h>
9 
10 #include <gtsam_unstable/slam/RelativeElevationFactor.h>
11 
12 #include <gtsam/base/numericalDerivative.h>
13 
14 using namespace gtsam;
15 
16 SharedNoiseModel model1 = noiseModel::Unit::Create(1);
17 
18 const double tol = 1e-5;
19 
20 const Pose3 pose1(Rot3(), Point3(2.0, 3.0, 4.0));
21 const Pose3 pose2(Rot3::Pitch(-M_PI_2), Point3(2.0, 3.0, 4.0));
22 const Pose3 pose3(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(2.0, 3.0, 4.0));
23 const Point3 point1(3.0, 4.0, 2.0);
24 const gtsam::Key poseKey = 1, pointKey = 2;
25 
26 /* ************************************************************************* */
27 Vector evalFactorError(const RelativeElevationFactor& factor, const Pose3& x, const Point3& p) {
28  return factor.evaluateError(x, p);
29 }
30 
31 /* ************************************************************************* */
32 TEST( testRelativeElevationFactor, level_zero_error ) {
33  // Zero error
34  double measured = 2.0;
35  RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
36  Matrix actH1, actH2;
37  EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose1, point1, actH1, actH2)));
38  Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
39  boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
40  Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
41  boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
42  EXPECT(assert_equal(expH1, actH1, tol));
43  EXPECT(assert_equal(expH2, actH2, tol));
44 }
45 
46 /* ************************************************************************* */
47 TEST( testRelativeElevationFactor, level_positive ) {
48  // Positive meas
49  double measured = 0.0;
50  RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
51  Matrix actH1, actH2;
52  EXPECT(assert_equal((Vector(1) << 2.0).finished(), factor.evaluateError(pose1, point1, actH1, actH2)));
53  Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
54  boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
55  Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
56  boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
57  EXPECT(assert_equal(expH1, actH1, tol));
58  EXPECT(assert_equal(expH2, actH2, tol));
59 }
60 
61 /* ************************************************************************* */
62 TEST( testRelativeElevationFactor, level_negative ) {
63  // Negative meas
64  double measured = -1.0;
65  RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
66  Matrix actH1, actH2;
67  EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose1, point1, actH1, actH2)));
68  Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
69  boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
70  Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
71  boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
72  EXPECT(assert_equal(expH1, actH1, tol));
73  EXPECT(assert_equal(expH2, actH2, tol));
74 }
75 
76 /* ************************************************************************* */
77 TEST( testRelativeElevationFactor, rotated_zero_error ) {
78  // Zero error
79  double measured = 2.0;
80  RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
81  Matrix actH1, actH2;
82  EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose2, point1, actH1, actH2)));
83  Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
84  boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
85  Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
86  boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
87  EXPECT(assert_equal(expH1, actH1, tol));
88  EXPECT(assert_equal(expH2, actH2, tol));
89 }
90 
91 /* ************************************************************************* */
92 TEST( testRelativeElevationFactor, rotated_positive ) {
93  // Positive meas
94  double measured = 0.0;
95  RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
96  Matrix actH1, actH2;
97  EXPECT(assert_equal((Vector(1) << 2.0).finished(), factor.evaluateError(pose2, point1, actH1, actH2)));
98  Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
99  boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
100  Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
101  boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
102  EXPECT(assert_equal(expH1, actH1, tol));
103  EXPECT(assert_equal(expH2, actH2, tol));
104 }
105 
106 /* ************************************************************************* */
107 TEST( testRelativeElevationFactor, rotated_negative1 ) {
108  // Negative meas
109  double measured = -1.0;
110  RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
111  Matrix actH1, actH2;
112  EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose2, point1, actH1, actH2)));
113  Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
114  boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
115  Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
116  boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
117  EXPECT(assert_equal(expH1, actH1, tol));
118  EXPECT(assert_equal(expH2, actH2, tol));
119 }
120 
121 /* ************************************************************************* */
122 TEST( testRelativeElevationFactor, rotated_negative2 ) {
123  // Negative meas
124  double measured = -1.0;
125  RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
126  Matrix actH1, actH2;
127  EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose3, point1, actH1, actH2)));
128  Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
129  boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5);
130  Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
131  boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5);
132  EXPECT(assert_equal(expH1, actH1, tol));
133  EXPECT(assert_equal(expH2, actH2, tol));
134 }
135 
136 /* ************************************************************************* */
137 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
138 /* ************************************************************************* */
pose2
const Pose3 pose2(Rot3::Pitch(-M_PI_2), Point3(2.0, 3.0, 4.0))
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
pose3
const Pose3 pose3(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(2.0, 3.0, 4.0))
gtsam::Rot3::Pitch
static Rot3 Pitch(double t)
Positive pitch is up (increasing aircraft altitude).See ypr.
Definition: Rot3.h:180
poseKey
const gtsam::Key poseKey
Definition: testRelativeElevationFactor.cpp:24
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
pose1
const Pose3 pose1(Rot3(), Point3(2.0, 3.0, 4.0))
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::Rot3::RzRyRx
static Rot3 RzRyRx(double x, double y, double z, OptionalJacobian< 3, 1 > Hx=boost::none, OptionalJacobian< 3, 1 > Hy=boost::none, OptionalJacobian< 3, 1 > Hz=boost::none)
Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.
Definition: Rot3M.cpp:85
pointKey
const gtsam::Key pointKey
Definition: testRelativeElevationFactor.cpp:24
model1
SharedNoiseModel model1
Definition: testRelativeElevationFactor.cpp:16
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Definition: NoiseModel.h:608
gtsam::RelativeElevationFactor::evaluateError
Vector evaluateError(const Pose3 &pose, const Point3 &point, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Definition: RelativeElevationFactor.cpp:20
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
EXPECT
#define EXPECT(condition)
Definition: Test.h:151
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam
traits
Definition: chartTesting.h:28
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
p
float * p
Definition: Tutorial_Map_using.cpp:9
RelativeElevationFactor.h
Factor representing a known relative altitude in global frame.
gtsam::TEST
TEST(LPInitSolver, InfiniteLoopSingleVar)
Definition: testLPSolver.cpp:64
gtsam::tol
const G double tol
Definition: Group.h:83
point1
const Point3 point1(3.0, 4.0, 2.0)
gtsam::Point3
Vector3 Point3
Definition: Point3.h:35
measured
Point3 measured
Definition: testMagFactor.cpp:49
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
evalFactorError
Vector evalFactorError(const RelativeElevationFactor &factor, const Pose3 &x, const Point3 &p)
Definition: testRelativeElevationFactor.cpp:27
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:734

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:49:22