testBinaryMeasurement.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010-2020, 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 
21 #include <gtsam/geometry/Rot3.h>
22 #include <gtsam/geometry/Unit3.h>
23 #include <gtsam/geometry/Pose3.h>
24 
26 
27 using namespace std;
28 using namespace gtsam;
29 
30 static const Key kKey1(2), kKey2(1);
31 
32 // Create noise models for unit3 and rot3
33 static SharedNoiseModel unit3_model(noiseModel::Isotropic::Sigma(2, 0.05));
34 static SharedNoiseModel rot3_model(noiseModel::Isotropic::Sigma(3, 0.05));
35 
36 const Unit3 unit3Measured(Vector3(1, 1, 1));
38 
39 /* ************************************************************************* */
42  unit3_model);
43  EXPECT_LONGS_EQUAL(unit3Measurement.key1(), kKey1);
44  EXPECT_LONGS_EQUAL(unit3Measurement.key2(), kKey2);
45  EXPECT(unit3Measurement.measured().equals(unit3Measured));
46 
47  BinaryMeasurement<Unit3> unit3MeasurementCopy(kKey1, kKey2, unit3Measured,
48  unit3_model);
49  EXPECT(unit3Measurement.equals(unit3MeasurementCopy));
50 }
51 
52 /* ************************************************************************* */
54  // testing the accessors
55  BinaryMeasurement<Rot3> rot3Measurement(kKey1, kKey2, rot3Measured,
56  rot3_model);
57  EXPECT_LONGS_EQUAL(rot3Measurement.key1(), kKey1);
58  EXPECT_LONGS_EQUAL(rot3Measurement.key2(), kKey2);
59  EXPECT(rot3Measurement.measured().equals(rot3Measured));
60 
61  // testing the equals function
62  BinaryMeasurement<Rot3> rot3MeasurementCopy(kKey1, kKey2, rot3Measured,
63  rot3_model);
64  EXPECT(rot3Measurement.equals(rot3MeasurementCopy));
65 }
66 
67 /* ************************************************************************* */
68 TEST(BinaryMeasurement, Rot3MakeRobust) {
69  auto huber_model = noiseModel::Robust::Create(
70  noiseModel::mEstimator::Huber::Create(1.345), rot3_model);
71  BinaryMeasurement<Rot3> rot3Measurement(kKey1, kKey2, rot3Measured,
72  huber_model);
73 
74  EXPECT_LONGS_EQUAL(rot3Measurement.key1(), kKey1);
75  EXPECT_LONGS_EQUAL(rot3Measurement.key2(), kKey2);
76  EXPECT(rot3Measurement.measured().equals(rot3Measured));
77  const auto &robust = std::dynamic_pointer_cast<noiseModel::Robust>(
78  rot3Measurement.noiseModel());
79  EXPECT(robust);
80 }
81 
82 /* ************************************************************************* */
83 int main() {
84  TestResult tr;
85  return TestRegistry::runAllTests(tr);
86 }
87 /* ************************************************************************* */
const SharedNoiseModel & noiseModel() const
static const Key kKey2(1)
const Rot3 rot3Measured
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
bool equals(const Rot3 &p, double tol=1e-9) const
Definition: Rot3.cpp:99
TEST(BinaryMeasurement, Unit3)
bool equals(const Unit3 &s, double tol=1e-9) const
The equals function with tolerance.
Definition: Unit3.h:114
Definition: BFloat16.h:88
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
static SharedNoiseModel unit3_model(noiseModel::Isotropic::Sigma(2, 0.05))
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
int main()
#define EXPECT(condition)
Definition: Test.h:150
static const Key kKey1(2)
const Unit3 unit3Measured(Vector3(1, 1, 1))
traits
Definition: chartTesting.h:28
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
const T & measured() const
Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
3D Pose
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
bool equals(const BinaryMeasurement &expected, double tol=1e-9) const
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741
static SharedNoiseModel rot3_model(noiseModel::Isotropic::Sigma(3, 0.05))
3D rotation represented as a rotation matrix or quaternion


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