testPoseRotationPrior.cpp
Go to the documentation of this file.
1 
11 
13 
14 #include <gtsam/geometry/Pose2.h>
15 #include <gtsam/geometry/Pose3.h>
16 
18 
19 using namespace gtsam;
20 
23 
26 
27 const double tol = 1e-5;
28 
29 const gtsam::Key poseKey = 1;
30 
31 // Pose3 examples
32 const Point3 point3A(1.0, 2.0, 3.0), point3B(4.0, 6.0, 8.0);
33 const Rot3 rot3A, rot3B = Rot3::Pitch(-M_PI_2), rot3C = Rot3::Expmap(Vector3(0.1, 0.2, 0.3));
34 
35 // Pose2 examples
36 const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0);
37 const Rot2 rot2A, rot2B = Rot2::fromAngle(M_PI_2);
39 
40 /* ************************************************************************* */
41 Vector evalFactorError3(const Pose3RotationPrior& factor, const Pose3& x) {
42  return factor.evaluateError(x);
43 }
44 
45 /* ************************************************************************* */
46 Vector evalFactorError2(const Pose2RotationPrior& factor, const Pose2& x) {
47  return factor.evaluateError(x);
48 }
49 
50 /* ************************************************************************* */
51 TEST( testPoseRotationFactor, level3_zero_error ) {
52  Pose3 pose1(rot3A, point3A);
53  Pose3RotationPrior factor(poseKey, rot3A, model3);
54  Matrix actH1;
55  EXPECT(assert_equal(Z_3x1, factor.evaluateError(pose1, actH1)));
56  Matrix expH1 = numericalDerivative22<Vector3,Pose3RotationPrior,Pose3>(evalFactorError3, factor, pose1, 1e-5);
57  EXPECT(assert_equal(expH1, actH1, tol));
58 }
59 
60 /* ************************************************************************* */
61 TEST( testPoseRotationFactor, level3_error ) {
62  Pose3 pose1(rot3A, point3A);
63  Pose3RotationPrior factor(poseKey, rot3C, model3);
64  Matrix actH1;
65 #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
66  EXPECT(assert_equal(Vector3(-0.1, -0.2,-0.3), factor.evaluateError(pose1, actH1)));
67 #else
68  EXPECT(assert_equal(Vector3(-0.1, -0.2, -0.3), factor.evaluateError(pose1, actH1),1e-2));
69 #endif
70  Matrix expH1 = numericalDerivative22<Vector3,Pose3RotationPrior,Pose3>(evalFactorError3, factor, pose1, 1e-5);
71  // the derivative is more complex, but is close to the identity for Rot3 around the origin
72  // If not using true expmap will be close, but not exact around the origin
73  // EXPECT(assert_equal(expH1, actH1, tol));
74 }
75 
76 /* ************************************************************************* */
77 TEST( testPoseRotationFactor, level2_zero_error ) {
78  Pose2 pose1(rot2A, point2A);
79  Pose2RotationPrior factor(poseKey, rot2A, model1);
80  Matrix actH1;
81  EXPECT(assert_equal(Z_1x1, factor.evaluateError(pose1, actH1)));
82  Matrix expH1 = numericalDerivative22<Vector1,Pose2RotationPrior,Pose2>(evalFactorError2, factor, pose1, 1e-5);
83  EXPECT(assert_equal(expH1, actH1, tol));
84 }
85 
86 /* ************************************************************************* */
87 TEST( testPoseRotationFactor, level2_error ) {
88  Pose2 pose1(rot2A, point2A);
89  Pose2RotationPrior factor(poseKey, rot2B, model1);
90  Matrix actH1;
91  EXPECT(assert_equal((Vector(1) << -M_PI_2).finished(), factor.evaluateError(pose1, actH1)));
92  Matrix expH1 = numericalDerivative22<Vector1,Pose2RotationPrior,Pose2>(evalFactorError2, factor, pose1, 1e-5);
93  EXPECT(assert_equal(expH1, actH1, tol));
94 }
95 
96 /* ************************************************************************* */
97 TEST( testPoseRotationFactor, level2_error_wrap ) {
98  Pose2 pose1(rot2C, point2A);
99  Pose2RotationPrior factor(poseKey, rot2D, model1);
100  Matrix actH1;
101  EXPECT(assert_equal((Vector(1) << -0.02).finished(), factor.evaluateError(pose1, actH1)));
102  Matrix expH1 = numericalDerivative22<Vector1,Pose2RotationPrior,Pose2>(evalFactorError2, factor, pose1, 1e-5);
103  EXPECT(assert_equal(expH1, actH1, tol));
104 }
105 
106 /* ************************************************************************* */
107 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
108 /* ************************************************************************* */
const gtsam::Key poseKey
const Rot2 rot2C
const Rot3 rot3B
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:46
const Point3 point3B(4.0, 6.0, 8.0)
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
const Rot3 rot3C
static Rot3 Pitch(double t)
Positive pitch is up (increasing aircraft altitude).See ypr.
Definition: Rot3.h:177
Vector2 Point2
Definition: Point2.h:32
Implements a prior on the rotation component of a pose.
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
#define M_PI
Definition: main.h:106
const Point2 point2B(4.0, 6.0)
const Rot2 rot2A
Some functions to compute numerical derivatives.
Vector evalFactorError3(const Pose3RotationPrior &factor, const Pose3 &x)
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
static Rot3 Expmap(const Vector3 &v, OptionalJacobian< 3, 3 > H={})
Definition: Rot3.h:374
Eigen::VectorXd Vector
Definition: Vector.h:38
const Rot2 rot2D
#define EXPECT(condition)
Definition: Test.h:150
const Rot3 rot3A
PoseRotationPrior< Pose2 > Pose2RotationPrior
Array< double, 1, 3 > e(1./3., 0.5, 2.)
const SharedNoiseModel model3
Vector evaluateError(const Pose &pose, OptionalMatrixType H) const override
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
traits
Definition: chartTesting.h:28
PoseRotationPrior< Pose3 > Pose3RotationPrior
const Rot2 rot2B
Vector evalFactorError2(const Pose2RotationPrior &factor, const Pose2 &x)
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:270
int main()
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
Definition: Rot2.h:61
const G double tol
Definition: Group.h:86
Vector3 Point3
Definition: Point3.h:38
TEST(SmartFactorBase, Pinhole)
2D Pose
const SharedNoiseModel model1
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
3D Pose
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
const Point3 point3A(1.0, 2.0, 3.0)
const Point2 point2A(1.0, 2.0)
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741


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