testBearingRangeFactor.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 
35 static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(2, 0.5));
36 BearingRangeFactor2D factor2D(poseKey, pointKey, 1, 2, model2D);
37 
39 static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(3, 0.5));
41  Pose3().bearing(Point3(1, 0, 0)), 1, model3D);
42 }
43 
44 /* ************************************************************************* */
46  // Set the linearization point
47  Values values;
48  values.insert(poseKey, Pose2(1.0, 2.0, 0.57));
49  values.insert(pointKey, Point2(-4.0, 11.0));
50 
51  EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor2D.expression({poseKey, pointKey}),
52  values, 1e-7, 1e-5);
53  EXPECT_CORRECT_FACTOR_JACOBIANS(factor2D, values, 1e-7, 1e-5);
54 }
55 
56 /* ************************************************************************* */
57 // TODO(frank): this test is disabled (for now) because the macros below are
58 // incompatible with the Unit3 localCoordinates. See testBearingFactor...
59 //TEST(BearingRangeFactor, 3D) {
60 // // Serialize the factor
61 // std::string serialized = serializeXML(factor3D);
62 //
63 // // And de-serialize it
64 // BearingRangeFactor3D factor;
65 // deserializeXML(serialized, factor);
66 //
67 // // Set the linearization point
68 // Values values;
69 // values.insert(poseKey, Pose3());
70 // values.insert(pointKey, Point3(1, 0, 0));
71 //
72 // EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression({poseKey, pointKey}),
73 // values, 1e-7, 1e-5);
74 // EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
75 //}
76 
77 /* ************************************************************************* */
78 int main() {
79  TestResult tr;
80  return TestRegistry::runAllTests(tr);
81 }
82 /* ************************************************************************* */
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
pointKey
const gtsam::Key pointKey
Definition: testRelativeElevationFactor.cpp:25
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
TestHarness.h
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
poseKey
const gtsam::Key poseKey
Definition: testPoseRotationPrior.cpp:29
gtsam::BearingRangeFactor
Definition: sam/BearingRangeFactor.h:34
gtsam::Pose3
Definition: Pose3.h:37
TEST
TEST(BearingRangeFactor, 2D)
Definition: testBearingRangeFactor.cpp:45
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
expressionTesting.h
Test harness methods for expressions.
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
BearingRangeFactor2D
BearingRangeFactor< Pose2, Point2 > BearingRangeFactor2D
Definition: serialization.cpp:74
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
BearingRangeFactor3D
BearingRangeFactor< Pose3, Point3 > BearingRangeFactor3D
Definition: serialization.cpp:75
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
main
int main()
Definition: testBearingRangeFactor.cpp:78
Pose3.h
3D Pose
gtsam::Pose2
Definition: Pose2.h:39
BearingRangeFactor.h
a single factor contains both the bearing and the range to prevent handle to pair bearing and range f...


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:07:16