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);
39 
40 /* ************************************************************************* */
42  return factor.evaluateError(x);
43 }
44 
45 /* ************************************************************************* */
47  return factor.evaluateError(x);
48 }
49 
50 /* ************************************************************************* */
51 TEST( testPoseRotationFactor, level3_zero_error ) {
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 ) {
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 ) {
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 ) {
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 ) {
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 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
point2A
const Point2 point2A(1.0, 2.0)
Pose2RotationPrior
PoseRotationPrior< Pose2 > Pose2RotationPrior
Definition: testPoseRotationPrior.cpp:24
Pose2.h
2D Pose
rot3C
const Rot3 rot3C
Definition: testPoseRotationPrior.cpp:33
point2B
const Point2 point2B(4.0, 6.0)
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Z_3x1
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:46
TestHarness.h
evalFactorError3
Vector evalFactorError3(const Pose3RotationPrior &factor, const Pose3 &x)
Definition: testPoseRotationPrior.cpp:41
rot2A
const Rot2 rot2A
Definition: testPoseRotationPrior.cpp:37
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
rot2B
const Rot2 rot2B
Definition: testPoseRotationPrior.cpp:37
evalFactorError2
Vector evalFactorError2(const Pose2RotationPrior &factor, const Pose2 &x)
Definition: testPoseRotationPrior.cpp:46
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::noiseModel::Diagonal::Sigmas
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:291
point3A
const Point3 point3A(1.0, 2.0, 3.0)
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
poseKey
const gtsam::Key poseKey
Definition: testPoseRotationPrior.cpp:29
PoseRotationPrior.h
Implements a prior on the rotation component of a pose.
Pose3RotationPrior
PoseRotationPrior< Pose3 > Pose3RotationPrior
Definition: testPoseRotationPrior.cpp:25
gtsam::Rot2::fromAngle
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
Definition: Rot2.h:61
numericalDerivative.h
Some functions to compute numerical derivatives.
rot2D
const Rot2 rot2D
Definition: testPoseRotationPrior.cpp:38
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::Rot3::Pitch
static Rot3 Pitch(double t)
Positive pitch is up (increasing aircraft altitude).See ypr.
Definition: Rot3.h:177
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
rot3B
const Rot3 rot3B
Definition: testPoseRotationPrior.cpp:33
rot3A
const Rot3 rot3A
Definition: testPoseRotationPrior.cpp:33
TestResult
Definition: TestResult.h:26
gtsam::PoseRotationPrior::evaluateError
Vector evaluateError(const Pose &pose, OptionalMatrixType H) const override
Definition: PoseRotationPrior.h:81
M_PI_2
#define M_PI_2
Definition: mconf.h:118
gtsam::Rot2
Definition: Rot2.h:35
gtsam::PoseRotationPrior
Definition: PoseRotationPrior.h:19
main
int main()
Definition: testPoseRotationPrior.cpp:107
gtsam
traits
Definition: SFMdata.h:40
gtsam::TEST
TEST(SmartFactorBase, Pinhole)
Definition: testSmartFactorBase.cpp:38
pose1
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
gtsam::Rot3::Expmap
static Rot3 Expmap(const Vector3 &v, OptionalJacobian< 3, 3 > H={})
Definition: Rot3.h:374
point3B
const Point3 point3B(4.0, 6.0, 8.0)
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
M_PI
#define M_PI
Definition: mconf.h:117
model3
const SharedNoiseModel model3
Definition: testPoseRotationPrior.cpp:22
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
rot2C
const Rot2 rot2C
Definition: testPoseRotationPrior.cpp:38
Pose3.h
3D Pose
model1
const SharedNoiseModel model1
Definition: testPoseRotationPrior.cpp:21
gtsam::Pose2
Definition: Pose2.h:39


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:41:00