From 592fa95d660bc6add65bfd156479d33c365f603e Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Tue, 24 Jul 2012 14:50:01 +0000 Subject: [PATCH] Fixed serialization for LieVector and LieMatrix --- gtsam/base/LieMatrix.h | 14 ++++++++++++ gtsam/base/LieVector.h | 12 ++++++++++ tests/testSerializationSLAM.cpp | 40 ++++++++++++++++----------------- 3 files changed, 46 insertions(+), 20 deletions(-) diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index 2ab15a054..5844e8201 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -22,6 +22,7 @@ #include #include #include +#include namespace gtsam { @@ -135,5 +136,18 @@ struct LieMatrix : public Matrix, public DerivedValue { static inline Vector Logmap(const LieMatrix& p) { return Eigen::Map(&p(0,0), p.dim()); } +private: + + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("LieMatrix", + boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("Matrix", + boost::serialization::base_object(*this)); + + } + }; } // \namespace gtsam diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index ef31e93cc..916730fd3 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -119,5 +119,17 @@ struct LieVector : public Vector, public DerivedValue { /** Logmap around identity - just returns with default cast back */ static inline Vector Logmap(const LieVector& p) { return p; } +private: + + // Serialization function + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("LieVector", + boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("Vector", + boost::serialization::base_object(*this)); + } + }; } // \namespace gtsam diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index 27f74c222..c0cdb6a93 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -269,8 +269,8 @@ TEST (Serialization, factors) { Symbol b01('b',1), b02('b',2), b03('b',3), b04('b',4), b05('b',5), b06('b',6), b07('b',7), b08('b',8), b09('b',9), b10('b',10), b11('b',11), b12('b',12), b13('b',13); Values values; -// values.insert(a01, lieVector); -// values.insert(a02, lieMatrix); + values.insert(a01, lieVector); + values.insert(a02, lieMatrix); values.insert(a03, point2); values.insert(a04, stereoPoint2); values.insert(a05, point3); @@ -352,8 +352,8 @@ TEST (Serialization, factors) { NonlinearFactorGraph graph; -// graph.add(priorFactorLieVector); -// graph.add(priorFactorLieMatrix); + graph.add(priorFactorLieVector); + graph.add(priorFactorLieMatrix); graph.add(priorFactorPoint2); graph.add(priorFactorStereoPoint2); graph.add(priorFactorPoint3); @@ -366,8 +366,8 @@ TEST (Serialization, factors) { graph.add(priorFactorCalibratedCamera); graph.add(priorFactorSimpleCamera); -// graph.add(betweenFactorLieVector); -// graph.add(betweenFactorLieMatrix); + graph.add(betweenFactorLieVector); + graph.add(betweenFactorLieMatrix); graph.add(betweenFactorPoint2); graph.add(betweenFactorPoint3); graph.add(betweenFactorRot2); @@ -375,8 +375,8 @@ TEST (Serialization, factors) { graph.add(betweenFactorPose2); graph.add(betweenFactorPose3); -// graph.add(nonlinearEqualityLieVector); -// graph.add(nonlinearEqualityLieMatrix); + graph.add(nonlinearEqualityLieVector); + graph.add(nonlinearEqualityLieMatrix); graph.add(nonlinearEqualityPoint2); graph.add(nonlinearEqualityStereoPoint2); graph.add(nonlinearEqualityPoint3); @@ -412,8 +412,8 @@ TEST (Serialization, factors) { EXPECT(equalsObj(values)); EXPECT(equalsObj(graph)); -// EXPECT(equalsObj(priorFactorLieVector)); -// EXPECT(equalsObj(priorFactorLieMatrix)); + EXPECT(equalsObj(priorFactorLieVector)); + EXPECT(equalsObj(priorFactorLieMatrix)); EXPECT(equalsObj(priorFactorPoint2)); EXPECT(equalsObj(priorFactorStereoPoint2)); EXPECT(equalsObj(priorFactorPoint3)); @@ -426,8 +426,8 @@ TEST (Serialization, factors) { EXPECT(equalsObj(priorFactorCalibratedCamera)); EXPECT(equalsObj(priorFactorSimpleCamera)); -// EXPECT(equalsObj(betweenFactorLieVector)); -// EXPECT(equalsObj(betweenFactorLieMatrix)); + EXPECT(equalsObj(betweenFactorLieVector)); + EXPECT(equalsObj(betweenFactorLieMatrix)); EXPECT(equalsObj(betweenFactorPoint2)); EXPECT(equalsObj(betweenFactorPoint3)); EXPECT(equalsObj(betweenFactorRot2)); @@ -435,8 +435,8 @@ TEST (Serialization, factors) { EXPECT(equalsObj(betweenFactorPose2)); EXPECT(equalsObj(betweenFactorPose3)); -// EXPECT(equalsObj(nonlinearEqualityLieVector)); -// EXPECT(equalsObj(nonlinearEqualityLieMatrix)); + EXPECT(equalsObj(nonlinearEqualityLieVector)); + EXPECT(equalsObj(nonlinearEqualityLieMatrix)); EXPECT(equalsObj(nonlinearEqualityPoint2)); EXPECT(equalsObj(nonlinearEqualityStereoPoint2)); EXPECT(equalsObj(nonlinearEqualityPoint3)); @@ -472,8 +472,8 @@ TEST (Serialization, factors) { EXPECT(equalsXML(values)); EXPECT(equalsXML(graph)); -// EXPECT(equalsXML(priorFactorLieVector)); -// EXPECT(equalsXML(priorFactorLieMatrix)); + EXPECT(equalsXML(priorFactorLieVector)); + EXPECT(equalsXML(priorFactorLieMatrix)); EXPECT(equalsXML(priorFactorPoint2)); EXPECT(equalsXML(priorFactorStereoPoint2)); EXPECT(equalsXML(priorFactorPoint3)); @@ -486,8 +486,8 @@ TEST (Serialization, factors) { EXPECT(equalsXML(priorFactorCalibratedCamera)); EXPECT(equalsXML(priorFactorSimpleCamera)); -// EXPECT(equalsXML(betweenFactorLieVector)); -// EXPECT(equalsXML(betweenFactorLieMatrix)); + EXPECT(equalsXML(betweenFactorLieVector)); + EXPECT(equalsXML(betweenFactorLieMatrix)); EXPECT(equalsXML(betweenFactorPoint2)); EXPECT(equalsXML(betweenFactorPoint3)); EXPECT(equalsXML(betweenFactorRot2)); @@ -495,8 +495,8 @@ TEST (Serialization, factors) { EXPECT(equalsXML(betweenFactorPose2)); EXPECT(equalsXML(betweenFactorPose3)); -// EXPECT(equalsXML(nonlinearEqualityLieVector)); -// EXPECT(equalsXML(nonlinearEqualityLieMatrix)); + EXPECT(equalsXML(nonlinearEqualityLieVector)); + EXPECT(equalsXML(nonlinearEqualityLieMatrix)); EXPECT(equalsXML(nonlinearEqualityPoint2)); EXPECT(equalsXML(nonlinearEqualityStereoPoint2)); EXPECT(equalsXML(nonlinearEqualityPoint3));