testBetweenFactor.cpp
Go to the documentation of this file.
1 
11 #include <gtsam/geometry/Pose3.h>
12 #include <gtsam/geometry/Rot3.h>
13 #include <gtsam/inference/Symbol.h>
16 
17 using namespace std::placeholders;
18 using namespace gtsam;
19 using namespace gtsam::symbol_shorthand;
20 using namespace gtsam::noiseModel;
21 
26  Rot3 R1 = Rot3::Rodrigues(0.1, 0.2, 0.3);
27  Rot3 R2 = Rot3::Rodrigues(0.4, 0.5, 0.6);
28  Rot3 noise = Rot3(); // Rot3::Rodrigues(0.01, 0.01, 0.01); // Uncomment to make unit test fail
29  Rot3 measured = R1.between(R2)*noise ;
30 
31  BetweenFactor<Rot3> factor(R(1), R(2), measured, Isotropic::Sigma(3, 0.05));
32  Matrix actualH1, actualH2;
33  Vector actual = factor.evaluateError(R1, R2, actualH1, actualH2);
34 
35  Vector expected = Rot3::Logmap(measured.inverse() * R1.between(R2));
36  EXPECT(assert_equal(expected,actual/*, 1e-100*/)); // Uncomment to make unit test fail
37 
38  Matrix numericalH1 = numericalDerivative21<Vector3, Rot3, Rot3>(
39  [&factor](const Rot3& r1, const Rot3& r2) {return factor.evaluateError(r1, r2);},
40  R1, R2, 1e-5);
41  EXPECT(assert_equal(numericalH1,actualH1, 1E-5));
42 
43  Matrix numericalH2 = numericalDerivative22<Vector3,Rot3,Rot3>(
44  [&factor](const Rot3& r1, const Rot3& r2) {return factor.evaluateError(r1, r2);},
45  R1, R2, 1e-5);
46  EXPECT(assert_equal(numericalH2,actualH2, 1E-5));
47 }
48 
49 /* ************************************************************************* */
50 // Constructor scalar
51 TEST(BetweenFactor, ConstructorScalar) {
53  double measured = 0.0;
54  BetweenFactor<double> factor(1, 2, measured, model);
55 }
56 
57 /* ************************************************************************* */
58 // Constructor vector3
59 TEST(BetweenFactor, ConstructorVector3) {
60  SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
61  Vector3 measured(1, 2, 3);
62  BetweenFactor<Vector3> factor(1, 2, measured, model);
63 }
64 
65 /* ************************************************************************* */
66 // Constructor dynamic sized vector
67 TEST(BetweenFactor, ConstructorDynamicSizeVector) {
68  SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0);
69  Vector measured(5); measured << 1, 2, 3, 4, 5;
70  BetweenFactor<Vector> factor(1, 2, measured, model);
71 }
72 
73 /* ************************************************************************* */
74 TEST(BetweenFactor, Point3Jacobians) {
75  SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
76  Point3 measured(1, 2, 3);
77  BetweenFactor<Point3> factor(1, 2, measured, model);
78 
79  Values values;
80  values.insert(1, Point3(0, 0, 0));
81  values.insert(2, Point3(1, 2, 3));
82  Vector3 error = factor.evaluateError(Point3(0, 0, 0), Point3(1, 2, 3));
83  EXPECT(assert_equal<Vector3>(Vector3::Zero(), error, 1e-9));
84  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
85 }
86 
87 /* ************************************************************************* */
88 TEST(BetweenFactor, Rot3Jacobians) {
89  SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
90  Rot3 measured = Rot3::Ry(M_PI_2);
91  BetweenFactor<Rot3> factor(1, 2, measured, model);
92 
93  Values values;
94  values.insert(1, Rot3());
95  values.insert(2, Rot3::Ry(M_PI_2));
96  Vector3 error = factor.evaluateError(Rot3(), Rot3::Ry(M_PI_2));
97  EXPECT(assert_equal<Vector3>(Vector3::Zero(), error, 1e-9));
98  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
99 }
100 
101 /* ************************************************************************* */
102 TEST(BetweenFactor, Pose3Jacobians) {
103  SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 1.0);
104  Pose3 measured(Rot3(), Point3(1, 2, 3));
105  BetweenFactor<Pose3> factor(1, 2, measured, model);
106 
107  Pose3 pose1, pose2(Rot3(), Point3(1, 2, 3));
108  Values values;
109  values.insert(1, pose1);
110  values.insert(2, pose2);
111  Vector6 error = factor.evaluateError(pose1, pose2);
112  EXPECT(assert_equal<Vector6>(Vector6::Zero(), error, 1e-9));
113  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
114 }
115 
116 /* ************************************************************************* */
117 int main() {
118  TestResult tr;
119  return TestRegistry::runAllTests(tr);
120 }
121 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::LieGroup::between
Class between(const Class &g) const
Definition: Lie.h:52
TEST
TEST(BetweenFactor, Rot3)
Definition: testBetweenFactor.cpp:25
EXPECT_CORRECT_FACTOR_JACOBIANS
#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance)
Check the Jacobians produced by a factor against finite differences.
Definition: factorTesting.h:114
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
r2
static const double r2
Definition: testSmartRangeFactor.cpp:32
measured
Point2 measured(-17, 30)
gtsam::symbol_shorthand
Definition: inference/Symbol.h:147
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
simple::pose2
static Pose3 pose2
Definition: testInitializePose3.cpp:58
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
r1
static const double r1
Definition: testSmartRangeFactor.cpp:32
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
TestableAssertions.h
Provides additional testing facilities for common data structures.
Rot3.h
3D rotation represented as a rotation matrix or quaternion
so3::R1
SO3 R1
Definition: testShonanFactor.cpp:41
numericalDerivative.h
Some functions to compute numerical derivatives.
BetweenFactor.h
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
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
factorTesting.h
Evaluate derivatives of a nonlinear factor numerically.
Symbol.h
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
TestResult
Definition: TestResult.h:26
M_PI_2
#define M_PI_2
Definition: mconf.h:118
E
DiscreteKey E(5, 2)
so3::R2
SO3 R2
Definition: testShonanFactor.cpp:43
gtsam
traits
Definition: SFMdata.h:40
error
static double error
Definition: testRot3.cpp:37
pose1
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
gtsam::BetweenFactor::evaluateError
Vector evaluateError(const T &p1, const T &p2, OptionalMatrixType H1, OptionalMatrixType H2) const override
evaluate error, returns vector of errors size of tangent space
Definition: BetweenFactor.h:111
gtsam::Values
Definition: Values.h:65
gtsam::noiseModel
All noise models live in the noiseModel namespace.
Definition: LossFunctions.cpp:28
main
int main()
Definition: testBetweenFactor.cpp:117
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
R
Rot2 R(Rot2::fromAngle(0.1))
Pose3.h
3D Pose
gtsam::BetweenFactor
Definition: BetweenFactor.h:40


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:39:44