diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 79c3977cd..8784d0eb8 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -155,7 +155,13 @@ private: template void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(p_); - ar & BOOST_SERIALIZATION_NVP(B_); + // homebrew serialize Eigen Matrix + ar & boost::serialization::make_nvp("B11", (*B_)(0,0)); + ar & boost::serialization::make_nvp("B12", (*B_)(0,1)); + ar & boost::serialization::make_nvp("B21", (*B_)(1,0)); + ar & boost::serialization::make_nvp("B22", (*B_)(1,1)); + ar & boost::serialization::make_nvp("B31", (*B_)(2,0)); + ar & boost::serialization::make_nvp("B32", (*B_)(2,1)); } /// @} diff --git a/gtsam/geometry/tests/testSerializationGeometry.cpp b/gtsam/geometry/tests/testSerializationGeometry.cpp index a7e792cb8..bbc8be1ad 100644 --- a/gtsam/geometry/tests/testSerializationGeometry.cpp +++ b/gtsam/geometry/tests/testSerializationGeometry.cpp @@ -19,6 +19,8 @@ #include #include #include +#include +#include #include #include #include @@ -41,6 +43,9 @@ static Point3 pt3(1.0, 2.0, 3.0); static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); static Pose3 pose3(rt3, pt3); +static Unit3 unit3(1.0, 2.1, 3.4); +static EssentialMatrix ematrix(rt3, unit3); + static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); static Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); static Cal3Bundler cal3(1.0, 2.0, 3.0); @@ -59,6 +64,9 @@ TEST (Serialization, text_geometry) { EXPECT(equalsObj(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsObj(Rot2::fromDegrees(30.0))); + EXPECT(equalsObj(Unit3(1.0, 2.1, 3.4))); + EXPECT(equalsObj(EssentialMatrix(rt3, unit3))); + EXPECT(equalsObj(pt3)); EXPECT(equalsObj(rt3)); EXPECT(equalsObj(Pose3(rt3, pt3))); @@ -81,6 +89,9 @@ TEST (Serialization, xml_geometry) { EXPECT(equalsXML(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsXML(Rot2::fromDegrees(30.0))); + EXPECT(equalsXML(Unit3(1.0, 2.1, 3.4))); + EXPECT(equalsXML(EssentialMatrix(rt3, unit3))); + EXPECT(equalsXML(pt3)); EXPECT(equalsXML(rt3)); EXPECT(equalsXML(Pose3(rt3, pt3))); @@ -102,6 +113,9 @@ TEST (Serialization, binary_geometry) { EXPECT(equalsBinary(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsBinary(Rot2::fromDegrees(30.0))); + EXPECT(equalsBinary(Unit3(1.0, 2.1, 3.4))); + EXPECT(equalsBinary(EssentialMatrix(rt3, unit3))); + EXPECT(equalsBinary(pt3)); EXPECT(equalsBinary(rt3)); EXPECT(equalsBinary(Pose3(rt3, pt3)));