31 using namespace gtsam;
36 constexpr
double rangeMmeasurement(10.0);
46 TEST(SerializationSam, BearingFactor2D) {
48 double measurement2D(10.0);
51 EXPECT(serializationTestHelpers::equalsObj(factor2D));
52 EXPECT(serializationTestHelpers::equalsXML(factor2D));
53 EXPECT(serializationTestHelpers::equalsBinary(factor2D));
57 TEST(SerializationSam, BearingFactor3D) {
63 EXPECT(serializationTestHelpers::equalsObj(factor3D));
64 EXPECT(serializationTestHelpers::equalsXML(factor3D));
65 EXPECT(serializationTestHelpers::equalsBinary(factor3D));
76 EXPECT(serializationTestHelpers::equalsObj(factor2D));
77 EXPECT(serializationTestHelpers::equalsXML(factor2D));
78 EXPECT(serializationTestHelpers::equalsBinary(factor2D));
85 EXPECT(serializationTestHelpers::equalsObj(factor3D));
86 EXPECT(serializationTestHelpers::equalsXML(factor3D));
87 EXPECT(serializationTestHelpers::equalsBinary(factor3D));
96 poseKey,
pointKey, rangeMmeasurement, rangeNoiseModel, body_P_sensor_3D),
100 gtsam::serializationTestHelpers::roundtripXML(factor3D_1, factor3D_2);
103 const Pose3 pose(Rot3::RzRyRx(0.2, -0.3, 1.75),
Point3(1.0, 2.0, -3.0));
108 const Vector error_1 = factor3D_1.unwhitenedError(
values);
119 EXPECT(serializationTestHelpers::equalsObj(factor2D));
120 EXPECT(serializationTestHelpers::equalsXML(factor2D));
121 EXPECT(serializationTestHelpers::equalsBinary(factor2D));
130 EXPECT(serializationTestHelpers::equalsObj(factor3D));
131 EXPECT(serializationTestHelpers::equalsXML(factor3D));
132 EXPECT(serializationTestHelpers::equalsBinary(factor3D));