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 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
Pose2.h
2D Pose
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
D
MatrixXcd D
Definition: EigenSolver_EigenSolver_MatrixType.cpp:14
TEST
TEST(BearingFactor, 2D)
Definition: testBearingFactor.cpp:46
pointKey
const gtsam::Key pointKey
Definition: testRelativeElevationFactor.cpp:25
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
TestHarness.h
gtsam::BearingFactor
Definition: sam/BearingFactor.h:37
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
poseKey
const gtsam::Key poseKey
Definition: testPoseRotationPrior.cpp:29
gtsam::Pose3
Definition: Pose3.h:37
factorTesting.h
Evaluate derivatives of a nonlinear factor numerically.
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
TestResult
Definition: TestResult.h:26
main
int main()
Definition: testBearingFactor.cpp:87
expressionTesting.h
Test harness methods for expressions.
gtsam::Pose3::bearing
Unit3 bearing(const Point3 &point, OptionalJacobian< 2, 6 > Hself={}, OptionalJacobian< 2, 3 > Hpoint={}) const
Definition: Pose3.cpp:426
BearingFactor.h
Serializable factor induced by a bearing measurement.
gtsam
traits
Definition: SFMdata.h:40
gtsam::Values
Definition: Values.h:65
std
Definition: BFloat16.h:88
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
EXPECT_CORRECT_EXPRESSION_JACOBIANS
#define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, numerical_derivative_step, tolerance)
Check the Jacobians produced by an expression against finite differences.
Definition: expressionTesting.h:48
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
Pose3.h
3D Pose manifold SO(3) x R^3 and group SE(3)
gtsam::Pose2
Definition: Pose2.h:39


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:16:07