testPoseTranslationPrior.cpp
Go to the documentation of this file.
1 
8 #include <CppUnitLite/TestHarness.h>
9 
10 #include <gtsam/base/numericalDerivative.h>
11 #include <gtsam/geometry/Pose2.h>
12 #include <gtsam/geometry/Pose3.h>
13 
14 #include <gtsam/slam/PoseTranslationPrior.h>
15 
16 using namespace gtsam;
17 
18 const SharedNoiseModel model2 = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2));
19 const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3));
20 
21 typedef PoseTranslationPrior<Pose2> Pose2TranslationPrior;
22 typedef PoseTranslationPrior<Pose3> Pose3TranslationPrior;
23 
24 const double tol = 1e-5;
25 
26 const gtsam::Key poseKey = 1;
27 
28 // Pose3 examples
29 const Point3 point3A(1.0, 2.0, 3.0), point3B(4.0, 6.0, 8.0);
30 const Rot3 rot3A, rot3B = Rot3::Pitch(-M_PI_2), rot3C = Rot3::RzRyRx(0.1, 0.2, 0.3);
31 
32 // Pose2 examples
33 const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0);
34 const Rot2 rot2A, rot2B = Rot2::fromAngle(M_PI_2);
35 
36 /* ************************************************************************* */
37 Vector evalFactorError3(const Pose3TranslationPrior& factor, const Pose3& x) {
38  return factor.evaluateError(x);
39 }
40 
41 /* ************************************************************************* */
42 Vector evalFactorError2(const Pose2TranslationPrior& factor, const Pose2& x) {
43  return factor.evaluateError(x);
44 }
45 
46 /* ************************************************************************* */
47 TEST( testPoseTranslationFactor, level3_zero_error ) {
48  Pose3 pose1(rot3A, point3A);
49  Pose3TranslationPrior factor(poseKey, point3A, model3);
50  Matrix actH1;
51  EXPECT(assert_equal(Z_3x1, factor.evaluateError(pose1, actH1)));
52  Matrix expH1 = numericalDerivative22<Vector3,Pose3TranslationPrior,Pose3>(evalFactorError3, factor, pose1, 1e-5);
53  EXPECT(assert_equal(expH1, actH1, tol));
54 }
55 
56 /* ************************************************************************* */
57 TEST( testPoseTranslationFactor, level3_error ) {
58  Pose3 pose1(rot3A, point3A);
59  Pose3TranslationPrior factor(poseKey, point3B, model3);
60  Matrix actH1;
61  EXPECT(assert_equal(Vector3(-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
62  Matrix expH1 = numericalDerivative22<Vector3,Pose3TranslationPrior,Pose3>(evalFactorError3, factor, pose1, 1e-5);
63  EXPECT(assert_equal(expH1, actH1, tol));
64 }
65 
66 /* ************************************************************************* */
67 TEST( testPoseTranslationFactor, pitched3_zero_error ) {
68  Pose3 pose1(rot3B, point3A);
69  Pose3TranslationPrior factor(poseKey, point3A, model3);
70  Matrix actH1;
71  EXPECT(assert_equal(Z_3x1, factor.evaluateError(pose1, actH1)));
72  Matrix expH1 = numericalDerivative22<Vector3,Pose3TranslationPrior,Pose3>(evalFactorError3, factor, pose1, 1e-5);
73  EXPECT(assert_equal(expH1, actH1, tol));
74 }
75 
76 /* ************************************************************************* */
77 TEST( testPoseTranslationFactor, pitched3_error ) {
78  Pose3 pose1(rot3B, point3A);
79  Pose3TranslationPrior factor(poseKey, point3B, model3);
80  Matrix actH1;
81  EXPECT(assert_equal(Vector3(-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
82  Matrix expH1 = numericalDerivative22<Vector3,Pose3TranslationPrior,Pose3>(evalFactorError3, factor, pose1, 1e-5);
83  EXPECT(assert_equal(expH1, actH1, tol));
84 }
85 
86 /* ************************************************************************* */
87 TEST( testPoseTranslationFactor, smallrot3_zero_error ) {
88  Pose3 pose1(rot3C, point3A);
89  Pose3TranslationPrior factor(poseKey, point3A, model3);
90  Matrix actH1;
91  EXPECT(assert_equal(Z_3x1, factor.evaluateError(pose1, actH1)));
92  Matrix expH1 = numericalDerivative22<Vector3,Pose3TranslationPrior,Pose3>(evalFactorError3, factor, pose1, 1e-5);
93  EXPECT(assert_equal(expH1, actH1, tol));
94 }
95 
96 /* ************************************************************************* */
97 TEST( testPoseTranslationFactor, smallrot3_error ) {
98  Pose3 pose1(rot3C, point3A);
99  Pose3TranslationPrior factor(poseKey, point3B, model3);
100  Matrix actH1;
101  EXPECT(assert_equal(Vector3(-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
102  Matrix expH1 = numericalDerivative22<Vector3,Pose3TranslationPrior,Pose3>(evalFactorError3, factor, pose1, 1e-5);
103  EXPECT(assert_equal(expH1, actH1, tol));
104 }
105 
106 /* ************************************************************************* */
107 TEST( testPoseTranslationFactor, level2_zero_error ) {
108  Pose2 pose1(rot2A, point2A);
109  Pose2TranslationPrior factor(poseKey, point2A, model2);
110  Matrix actH1;
111  EXPECT(assert_equal(Z_2x1, factor.evaluateError(pose1, actH1)));
112  Matrix expH1 = numericalDerivative22<Vector2,Pose2TranslationPrior,Pose2>(evalFactorError2, factor, pose1, 1e-5);
113  EXPECT(assert_equal(expH1, actH1, tol));
114 }
115 
116 /* ************************************************************************* */
117 TEST( testPoseTranslationFactor, level2_error ) {
118  Pose2 pose1(rot2A, point2A);
119  Pose2TranslationPrior factor(poseKey, point2B, model2);
120  Matrix actH1;
121  EXPECT(assert_equal(Vector2(-3.0,-4.0), factor.evaluateError(pose1, actH1)));
122  Matrix expH1 = numericalDerivative22<Vector2,Pose2TranslationPrior,Pose2>(evalFactorError2, factor, pose1, 1e-5);
123  EXPECT(assert_equal(expH1, actH1, tol));
124 }
125 
126 /* ************************************************************************* */
127 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
128 /* ************************************************************************* */
gtsam::Z_3x1
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:46
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::Rot3::Pitch
static Rot3 Pitch(double t)
Positive pitch is up (increasing aircraft altitude).See ypr.
Definition: Rot3.h:180
gtsam::Point2
Vector2 Point2
Definition: Point2.h:27
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
Pose2TranslationPrior
PoseTranslationPrior< Pose2 > Pose2TranslationPrior
Definition: testPoseTranslationPrior.cpp:21
numericalDerivative.h
Some functions to compute numerical derivatives.
evalFactorError2
Vector evalFactorError2(const Pose2TranslationPrior &factor, const Pose2 &x)
Definition: testPoseTranslationPrior.cpp:42
rot3A
const Rot3 rot3A
Definition: testPoseTranslationPrior.cpp:30
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
PoseTranslationPrior.h
Implements a prior on the translation component of a pose.
evalFactorError3
Vector evalFactorError3(const Pose3TranslationPrior &factor, const Pose3 &x)
Definition: testPoseTranslationPrior.cpp:37
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::PoseTranslationPrior::evaluateError
Vector evaluateError(const Pose &pose, boost::optional< Matrix & > H=boost::none) const override
Definition: PoseTranslationPrior.h:62
EXPECT
#define EXPECT(condition)
Definition: Test.h:151
poseKey
const gtsam::Key poseKey
Definition: testPoseTranslationPrior.cpp:26
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
point2A
const Point2 point2A(1.0, 2.0)
rot2B
const Rot2 rot2B
Definition: testPoseTranslationPrior.cpp:34
rot2A
const Rot2 rot2A
Definition: testPoseTranslationPrior.cpp:34
rot3B
const Rot3 rot3B
Definition: testPoseTranslationPrior.cpp:30
pose1
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
gtsam
traits
Definition: chartTesting.h:28
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
gtsam::noiseModel::Diagonal::Sigmas
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:270
gtsam::TEST
TEST(LPInitSolver, InfiniteLoopSingleVar)
Definition: testLPSolver.cpp:64
gtsam::Rot2::fromAngle
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
Definition: Rot2.h:58
point3A
const Point3 point3A(1.0, 2.0, 3.0)
Pose3TranslationPrior
PoseTranslationPrior< Pose3 > Pose3TranslationPrior
Definition: testPoseTranslationPrior.cpp:22
gtsam::tol
const G double tol
Definition: Group.h:83
gtsam::Point3
Vector3 Point3
Definition: Point3.h:35
Pose2.h
2D Pose
model2
const SharedNoiseModel model2
Definition: testPoseTranslationPrior.cpp:18
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
point2B
const Point2 point2B(4.0, 6.0)
Pose3.h
3D Pose
point3B
const Point3 point3B(4.0, 6.0, 8.0)
gtsam::Z_2x1
static const Eigen::MatrixBase< Vector2 >::ConstantReturnType Z_2x1
Definition: Vector.h:45
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
rot3C
const Rot3 rot3C
Definition: testPoseTranslationPrior.cpp:30
model3
const SharedNoiseModel model3
Definition: testPoseTranslationPrior.cpp:19
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:734

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:48:52