testPoseTranslationPrior.cpp
Go to the documentation of this file.
1 
9 
11 #include <gtsam/geometry/Pose2.h>
12 #include <gtsam/geometry/Pose3.h>
13 
15 
16 using namespace gtsam;
17 
20 
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 ) {
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 ) {
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 /* ************************************************************************* */
static Rot3 RzRyRx(double x, double y, double z, OptionalJacobian< 3, 1 > Hx={}, OptionalJacobian< 3, 1 > Hy={}, OptionalJacobian< 3, 1 > Hz={})
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:84
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:46
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
static Rot3 Pitch(double t)
Positive pitch is up (increasing aircraft altitude).See ypr.
Definition: Rot3.h:177
Vector2 Point2
Definition: Point2.h:32
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
PoseTranslationPrior< Pose2 > Pose2TranslationPrior
Some functions to compute numerical derivatives.
Vector evalFactorError2(const Pose2TranslationPrior &factor, const Pose2 &x)
const Rot3 rot3A
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
Implements a prior on the translation component of a pose.
Vector evalFactorError3(const Pose3TranslationPrior &factor, const Pose3 &x)
Eigen::VectorXd Vector
Definition: Vector.h:38
#define EXPECT(condition)
Definition: Test.h:150
const gtsam::Key poseKey
Array< double, 1, 3 > e(1./3., 0.5, 2.)
const Point2 point2A(1.0, 2.0)
const Rot2 rot2B
const Rot2 rot2A
const Rot3 rot3B
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
traits
Definition: chartTesting.h:28
Eigen::Vector2d Vector2
Definition: Vector.h:42
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:270
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
Definition: Rot2.h:61
Vector evaluateError(const Pose &pose, OptionalMatrixType H) const override
const Point3 point3A(1.0, 2.0, 3.0)
PoseTranslationPrior< Pose3 > Pose3TranslationPrior
const G double tol
Definition: Group.h:86
Vector3 Point3
Definition: Point3.h:38
TEST(SmartFactorBase, Pinhole)
2D Pose
const SharedNoiseModel model2
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
const Point2 point2B(4.0, 6.0)
3D Pose
const Point3 point3B(4.0, 6.0, 8.0)
static const Eigen::MatrixBase< Vector2 >::ConstantReturnType Z_2x1
Definition: Vector.h:45
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
const Rot3 rot3C
const SharedNoiseModel model3
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:39:03