testSerializationSam.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 
22 #include <gtsam/geometry/Pose2.h>
23 #include <gtsam/geometry/Pose3.h>
28 #include <gtsam/sam/RangeFactor.h>
29 
30 using namespace std;
31 using namespace gtsam;
32 
33 namespace {
34 Key poseKey(1);
35 Key pointKey(2);
36 constexpr double rangeMmeasurement(10.0);
37 } // namespace
38 
39 /* ************************************************************************* */
40 // Export Noisemodels
41 // See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
44 
45 /* ************************************************************************* */
46 TEST(SerializationSam, BearingFactor2D) {
47  using BearingFactor2D = BearingFactor<Pose2, Point2>;
48  double measurement2D(10.0);
49  static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(1, 0.5));
50  BearingFactor2D factor2D(poseKey, pointKey, measurement2D, model2D);
51  EXPECT(serializationTestHelpers::equalsObj(factor2D));
52  EXPECT(serializationTestHelpers::equalsXML(factor2D));
53  EXPECT(serializationTestHelpers::equalsBinary(factor2D));
54 }
55 
56 /* ************************************************************************* */
57 TEST(SerializationSam, BearingFactor3D) {
58  using BearingFactor3D = BearingFactor<Pose3, Point3>;
59  Unit3 measurement3D =
60  Pose3().bearing(Point3(1, 0, 0)); // has to match values!
61  static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(2, 0.5));
62  BearingFactor3D factor3D(poseKey, pointKey, measurement3D, model3D);
63  EXPECT(serializationTestHelpers::equalsObj(factor3D));
64  EXPECT(serializationTestHelpers::equalsXML(factor3D));
65  EXPECT(serializationTestHelpers::equalsBinary(factor3D));
66 }
67 
68 /* ************************************************************************* */
69 namespace {
70 static SharedNoiseModel rangeNoiseModel(noiseModel::Unit::Create(1));
71 }
72 
73 TEST(SerializationSam, RangeFactor2D) {
75  RangeFactor2D factor2D(poseKey, pointKey, rangeMmeasurement, rangeNoiseModel);
76  EXPECT(serializationTestHelpers::equalsObj(factor2D));
77  EXPECT(serializationTestHelpers::equalsXML(factor2D));
78  EXPECT(serializationTestHelpers::equalsBinary(factor2D));
79 }
80 
81 /* ************************************************************************* */
82 TEST(SerializationSam, RangeFactor3D) {
84  RangeFactor3D factor3D(poseKey, pointKey, rangeMmeasurement, rangeNoiseModel);
85  EXPECT(serializationTestHelpers::equalsObj(factor3D));
86  EXPECT(serializationTestHelpers::equalsXML(factor3D));
87  EXPECT(serializationTestHelpers::equalsBinary(factor3D));
88 }
89 
90 /* ************************************************************************* */
91 TEST(RangeFactor, EqualsAfterDeserializing) {
92  // Check that the same results are obtained after deserializing:
93  Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
94  Point3(0.25, -0.10, 1.0));
96  poseKey, pointKey, rangeMmeasurement, rangeNoiseModel, body_P_sensor_3D),
97  factor3D_2;
98 
99  // check with Equal() trait:
100  gtsam::serializationTestHelpers::roundtripXML(factor3D_1, factor3D_2);
101  CHECK(assert_equal(factor3D_1, factor3D_2));
102 
103  const Pose3 pose(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, -3.0));
104  const Point3 point(-2.0, 11.0, 1.0);
105  const Values values = {{poseKey, genericValue(pose)},
107 
108  const Vector error_1 = factor3D_1.unwhitenedError(values);
109  const Vector error_2 = factor3D_2.unwhitenedError(values);
110  CHECK(assert_equal(error_1, error_2));
111 }
112 
113 /* ************************************************************************* */
114 TEST(BearingRangeFactor, Serialization2D) {
116  static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(2, 0.5));
117  BearingRangeFactor2D factor2D(poseKey, pointKey, 1, 2, model2D);
118 
119  EXPECT(serializationTestHelpers::equalsObj(factor2D));
120  EXPECT(serializationTestHelpers::equalsXML(factor2D));
121  EXPECT(serializationTestHelpers::equalsBinary(factor2D));
122 }
123 
124 /* ************************************************************************* */
125 TEST(BearingRangeFactor, Serialization3D) {
127  static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(3, 0.5));
129  Pose3().bearing(Point3(1, 0, 0)), 1, model3D);
130  EXPECT(serializationTestHelpers::equalsObj(factor3D));
131  EXPECT(serializationTestHelpers::equalsXML(factor3D));
132  EXPECT(serializationTestHelpers::equalsBinary(factor3D));
133 }
134 
135 /* ************************************************************************* */
136 int main() {
137  TestResult tr;
138  return TestRegistry::runAllTests(tr);
139 }
140 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
Pose2.h
2D Pose
BOOST_CLASS_EXPORT
BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic)
gtsam::RangeFactor
Definition: sam/RangeFactor.h:35
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
pointKey
const gtsam::Key pointKey
Definition: testRelativeElevationFactor.cpp:25
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
gtsam::BearingFactor
Definition: sam/BearingFactor.h:37
gtsam::noiseModel::Unit
Definition: NoiseModel.h:614
RangeFactor.h
Serializable factor induced by a range measurement.
TEST
TEST(SerializationSam, BearingFactor2D)
Definition: testSerializationSam.cpp:46
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::genericValue
GenericValue< T > genericValue(const T &v)
Definition: GenericValue.h:211
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
poseKey
const gtsam::Key poseKey
Definition: testPoseRotationPrior.cpp:29
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
gtsam::noiseModel::Isotropic
Definition: NoiseModel.h:541
gtsam::BearingRangeFactor
Definition: sam/BearingRangeFactor.h:34
gtsam::Pose3
Definition: Pose3.h:37
factorTesting.h
Evaluate derivatives of a nonlinear factor numerically.
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
serialization.h
Convenience functions for serializing data structures via boost.serialization.
serializationTestHelpers.h
TestResult
Definition: TestResult.h:26
expressionTesting.h
Test harness methods for expressions.
M_PI_2
#define M_PI_2
Definition: mconf.h:118
gtsam::Pose3::bearing
Unit3 bearing(const Point3 &point, OptionalJacobian< 2, 6 > Hself={}, OptionalJacobian< 2, 3 > Hpoint={}) const
Definition: Pose3.cpp:425
gtsam::RangeFactorWithTransform
Definition: sam/RangeFactor.h:104
gtsam::ExpressionFactor< typename Range< A1, A1 >::result_type >::unwhitenedError
Vector unwhitenedError(const Values &x, OptionalMatrixVecType H=nullptr) const override
Definition: ExpressionFactor.h:104
BearingFactor.h
Serializable factor induced by a bearing measurement.
gtsam
traits
Definition: SFMdata.h:40
gtsam::Values
Definition: Values.h:65
CHECK
#define CHECK(condition)
Definition: Test.h:108
std
Definition: BFloat16.h:88
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
main
int main()
Definition: testSerializationSam.cpp:136
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
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:08:32