testBearingFactor.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
20 #include <gtsam/geometry/Pose2.h>
21 #include <gtsam/geometry/Pose3.h>
24 
26 
27 using namespace std;
28 using namespace gtsam;
29 
30 namespace {
31 Key poseKey(1);
32 Key pointKey(2);
33 
34 typedef BearingFactor<Pose2, Point2> BearingFactor2D;
35 double measurement2D(10.0);
36 static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(1, 0.5));
37 BearingFactor2D factor2D(poseKey, pointKey, measurement2D, model2D);
38 
39 typedef BearingFactor<Pose3, Point3> BearingFactor3D;
40 Unit3 measurement3D = Pose3().bearing(Point3(1, 0, 0)); // has to match values!
41 static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(2, 0.5));
42 BearingFactor3D factor3D(poseKey, pointKey, measurement3D, model3D);
43 }
44 
45 /* ************************************************************************* */
47  // Set the linearization point
48  Values values;
49  values.insert(poseKey, Pose2(1.0, 2.0, 0.57));
50  values.insert(pointKey, Point2(-4.0, 11.0));
51 
52  EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor2D.expression({poseKey, pointKey}),
53  values, 1e-7, 1e-5);
54  EXPECT_CORRECT_FACTOR_JACOBIANS(factor2D, values, 1e-7, 1e-5);
55 }
56 
57 /* ************************************************************************* */
58 // TODO(frank): this test is disabled (for now) because the macros below are
59 // incompatible with the Unit3 localCoordinates. The issue is the following:
60 // For factors, we want to use Local(value, measured), because we need the error
61 // to be expressed in the tangent space of value. This surfaced in the Unit3 case
62 // where the tangent space can be radically didfferent from one value to the next.
63 // For derivatives, we want Local(constant, varying), because we need a derivative
64 // in a constant tangent space. But since the macros below call whitenedError
65 // which calls Local(value,measured), we actually call the reverse. This does not
66 // matter for types with a commutative Local, but matters a lot for Unit3.
67 // More thinking needed about what the right thing is, here...
68 //TEST(BearingFactor, 3D) {
69 // // Serialize the factor
70 // std::string serialized = serializeXML(factor3D);
71 //
72 // // And de-serialize it
73 // BearingFactor3D factor;
74 // deserializeXML(serialized, factor);
75 //
76 // // Set the linearization point
77 // Values values;
78 // values.insert(poseKey, Pose3());
79 // values.insert(pointKey, Point3(1, 0, 0));
80 //
81 // EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression({poseKey, pointKey}),
82 // values, 1e-7, 1e-5);
83 // EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
84 //}
85 
86 /* ************************************************************************* */
87 int main() {
88  TestResult tr;
89  return TestRegistry::runAllTests(tr);
90 }
91 /* ************************************************************************* */
const gtsam::Key poseKey
static int runAllTests(TestResult &result)
Vector2 Point2
Definition: Point2.h:32
leaf::MyValues values
Definition: BFloat16.h:88
Evaluate derivatives of a nonlinear factor numerically.
const gtsam::Key pointKey
#define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, numerical_derivative_step, tolerance)
Check the Jacobians produced by an expression against finite differences.
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
Test harness methods for expressions.
Unit3 bearing(const Point3 &point, OptionalJacobian< 2, 6 > Hself={}, OptionalJacobian< 2, 3 > Hpoint={}) const
Definition: Pose3.cpp:425
Array< double, 1, 3 > e(1./3., 0.5, 2.)
int main()
traits
Definition: chartTesting.h:28
#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance)
Check the Jacobians produced by a factor against finite differences.
void insert(Key j, const Value &val)
Definition: Values.cpp:155
Vector3 Point3
Definition: Point3.h:38
2D Pose
3D Pose
TEST(BearingFactor, 2D)
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:37:53