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 std::placeholders;
15 using namespace gtsam;
16 
17 SharedNoiseModel model1 = noiseModel::Unit::Create(1);
18 
19 const double tol = 1e-5;
20 
21 const Pose3 pose1(Rot3(), Point3(2.0, 3.0, 4.0));
22 const Pose3 pose2(Rot3::Pitch(-M_PI_2), Point3(2.0, 3.0, 4.0));
23 const Pose3 pose3(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(2.0, 3.0, 4.0));
24 const Point3 point1(3.0, 4.0, 2.0);
25 const gtsam::Key poseKey = 1, pointKey = 2;
26 
27 /* ************************************************************************* */
28 Vector evalFactorError(const RelativeElevationFactor& factor, const Pose3& x, const Point3& p) {
29  return factor.evaluateError(x, p);
30 }
31 
32 /* ************************************************************************* */
33 TEST( testRelativeElevationFactor, level_zero_error ) {
34  // Zero error
35  double measured = 2.0;
36  RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
37  Matrix actH1, actH2;
38  EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose1, point1, actH1, actH2)));
39  Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
40  std::bind(evalFactorError, factor, std::placeholders::_1,
41  std::placeholders::_2),
42  pose1, point1, 1e-5);
43  Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
44  std::bind(evalFactorError, factor, std::placeholders::_1,
45  std::placeholders::_2),
46  pose1, point1, 1e-5);
47  EXPECT(assert_equal(expH1, actH1, tol));
48  EXPECT(assert_equal(expH2, actH2, tol));
49 }
50 
51 /* ************************************************************************* */
52 TEST( testRelativeElevationFactor, level_positive ) {
53  // Positive meas
54  double measured = 0.0;
55  RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
56  Matrix actH1, actH2;
57  EXPECT(assert_equal((Vector(1) << 2.0).finished(), factor.evaluateError(pose1, point1, actH1, actH2)));
58  Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
59  std::bind(evalFactorError, factor, std::placeholders::_1,
60  std::placeholders::_2),
61  pose1, point1, 1e-5);
62  Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
63  std::bind(evalFactorError, factor, std::placeholders::_1,
64  std::placeholders::_2),
65  pose1, point1, 1e-5);
66  EXPECT(assert_equal(expH1, actH1, tol));
67  EXPECT(assert_equal(expH2, actH2, tol));
68 }
69 
70 /* ************************************************************************* */
71 TEST( testRelativeElevationFactor, level_negative ) {
72  // Negative meas
73  double measured = -1.0;
74  RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
75  Matrix actH1, actH2;
76  EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose1, point1, actH1, actH2)));
77  Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
78  std::bind(evalFactorError, factor, std::placeholders::_1,
79  std::placeholders::_2),
80  pose1, point1, 1e-5);
81  Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
82  std::bind(evalFactorError, factor, std::placeholders::_1,
83  std::placeholders::_2),
84  pose1, point1, 1e-5);
85  EXPECT(assert_equal(expH1, actH1, tol));
86  EXPECT(assert_equal(expH2, actH2, tol));
87 }
88 
89 /* ************************************************************************* */
90 TEST( testRelativeElevationFactor, rotated_zero_error ) {
91  // Zero error
92  double measured = 2.0;
93  RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
94  Matrix actH1, actH2;
95  EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose2, point1, actH1, actH2)));
96  Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
97  std::bind(evalFactorError, factor, std::placeholders::_1,
98  std::placeholders::_2),
99  pose2, point1, 1e-5);
100  Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
101  std::bind(evalFactorError, factor, std::placeholders::_1,
102  std::placeholders::_2),
103  pose2, point1, 1e-5);
104  EXPECT(assert_equal(expH1, actH1, tol));
105  EXPECT(assert_equal(expH2, actH2, tol));
106 }
107 
108 /* ************************************************************************* */
109 TEST( testRelativeElevationFactor, rotated_positive ) {
110  // Positive meas
111  double measured = 0.0;
112  RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
113  Matrix actH1, actH2;
114  EXPECT(assert_equal((Vector(1) << 2.0).finished(), factor.evaluateError(pose2, point1, actH1, actH2)));
115  Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
116  std::bind(evalFactorError, factor, std::placeholders::_1,
117  std::placeholders::_2),
118  pose2, point1, 1e-5);
119  Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
120  std::bind(evalFactorError, factor, std::placeholders::_1,
121  std::placeholders::_2),
122  pose2, point1, 1e-5);
123  EXPECT(assert_equal(expH1, actH1, tol));
124  EXPECT(assert_equal(expH2, actH2, tol));
125 }
126 
127 /* ************************************************************************* */
128 TEST( testRelativeElevationFactor, rotated_negative1 ) {
129  // Negative meas
130  double measured = -1.0;
131  RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
132  Matrix actH1, actH2;
133  EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose2, point1, actH1, actH2)));
134  Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
135  std::bind(evalFactorError, factor, std::placeholders::_1,
136  std::placeholders::_2),
137  pose2, point1, 1e-5);
138  Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
139  std::bind(evalFactorError, factor, std::placeholders::_1,
140  std::placeholders::_2),
141  pose2, point1, 1e-5);
142  EXPECT(assert_equal(expH1, actH1, tol));
143  EXPECT(assert_equal(expH2, actH2, tol));
144 }
145 
146 /* ************************************************************************* */
147 TEST( testRelativeElevationFactor, rotated_negative2 ) {
148  // Negative meas
149  double measured = -1.0;
150  RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
151  Matrix actH1, actH2;
152  EXPECT(assert_equal((Vector(1) << 3.0).finished(), factor.evaluateError(pose3, point1, actH1, actH2)));
153  Matrix expH1 = numericalDerivative21<Vector, Pose3, Point3>(
154  std::bind(evalFactorError, factor, std::placeholders::_1,
155  std::placeholders::_2),
156  pose3, point1, 1e-5);
157  Matrix expH2 = numericalDerivative22<Vector, Pose3, Point3>(
158  std::bind(evalFactorError, factor, std::placeholders::_1,
159  std::placeholders::_2),
160  pose3, point1, 1e-5);
161  EXPECT(assert_equal(expH1, actH1, tol));
162  EXPECT(assert_equal(expH2, actH2, tol));
163 }
164 
165 /* ************************************************************************* */
166 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
167 /* ************************************************************************* */
model1
SharedNoiseModel model1
Definition: testRelativeElevationFactor.cpp:17
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))
pointKey
const gtsam::Key pointKey
Definition: testRelativeElevationFactor.cpp:25
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
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
measured
Point2 measured(-17, 30)
main
int main()
Definition: testRelativeElevationFactor.cpp:166
tol
const double tol
Definition: testRelativeElevationFactor.cpp:19
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
pose2
const Pose3 pose2(Rot3::Pitch(-M_PI_2), Point3(2.0, 3.0, 4.0))
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::RelativeElevationFactor
Definition: RelativeElevationFactor.h:28
gtsam::RelativeElevationFactor::evaluateError
Vector evaluateError(const Pose3 &pose, const Point3 &point, OptionalMatrixType H1, OptionalMatrixType H2) const override
Definition: RelativeElevationFactor.cpp:20
numericalDerivative.h
Some functions to compute numerical derivatives.
evalFactorError
Vector evalFactorError(const RelativeElevationFactor &factor, const Pose3 &x, const Point3 &p)
Definition: testRelativeElevationFactor.cpp:28
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Pose3
Definition: Pose3.h:37
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
TestResult
Definition: TestResult.h:26
M_PI_2
#define M_PI_2
Definition: mconf.h:118
gtsam
traits
Definition: SFMdata.h:40
p
float * p
Definition: Tutorial_Map_using.cpp:9
TEST
TEST(testRelativeElevationFactor, level_zero_error)
Definition: testRelativeElevationFactor.cpp:33
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
point1
const Point3 point1(3.0, 4.0, 2.0)
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
RelativeElevationFactor.h
Factor representing a known relative altitude in global frame.
poseKey
const gtsam::Key poseKey
Definition: testRelativeElevationFactor.cpp:25
pose1
const Pose3 pose1(Rot3(), Point3(2.0, 3.0, 4.0))

gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:08:23