Serialization for (some) nonlinear factors now works, added virtual destructors to factor classes that needed them.

gtsam/linear/NoiseModel.cpp

@@ -300,6 +300,10 @@ Diagonal::Diagonal(const Vector& sigmas) :
 		Gaussian(sigmas.size()), sigmas_(sigmas), invsigmas_(reciprocal(sigmas)) {
 }
 
+Diagonal::Diagonal(const Vector& sigmas, const Vector& invsigmas):
+	Gaussian(sigmas.size()), sigmas_(sigmas), invsigmas_(invsigmas) {
+}
+
 /* ************************************************************************* */
 Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) {
 	if (smart) {
@@ -412,7 +416,8 @@ SharedDiagonal Constrained::QR(Matrix& Ab, boost::optional<std::vector<int>&> fi
 	list<Triple> Rd;
 
 	Vector pseudo(m); // allocate storage for pseudo-inverse
-	Vector weights = emul(invsigmas_,invsigmas_); // calculate weights once
+	Vector invsigmas = reciprocal(sigmas_);
+	Vector weights = emul(invsigmas,invsigmas); // calculate weights once
 
 	// We loop over all columns, because the columns that can be eliminated
 	// are not necessarily contiguous. For each one, estimate the corresponding

gtsam/linear/NoiseModel.h

@@ -126,6 +126,8 @@ namespace gtsam {
 
 			typedef boost::shared_ptr<Gaussian> shared_ptr;
 
+			virtual ~Gaussian() {}
+
 			/**
 			 * A Gaussian noise model created by specifying a square root information matrix.
 			 * @param smart, check if can be simplified to derived class
@@ -222,6 +224,9 @@ namespace gtsam {
 			/** sigmas and reciprocal */
 			Vector sigmas_, invsigmas_;
 
+			/** protected constructor for constructing constraints */
+			Diagonal(const Vector& sigmas, const Vector& invsigmas);
+
 			/** protected constructor takes sigmas */
 			Diagonal(const Vector& sigmas = ones(1));
 
@@ -229,6 +234,8 @@ namespace gtsam {
 
 			typedef boost::shared_ptr<Diagonal> shared_ptr;
 
+			virtual ~Diagonal() {}
+
 			/**
 			 * A diagonal noise model created by specifying a Vector of sigmas, i.e.
 			 * standard devations, the diagonal of the square root covariance matrix.
@@ -307,12 +314,16 @@ namespace gtsam {
 			// Instead (possibly zero) sigmas are stored in Diagonal Base class
 
 			/** protected constructor takes sigmas */
-			Constrained(const Vector& sigmas = zero(1)) :Diagonal(sigmas) {}
+			// Keeps only sigmas and calculates invsigmas when necessary
+			Constrained(const Vector& sigmas = zero(1)) :
+				Diagonal(sigmas, sigmas) {}
 
 		public:
 
 			typedef boost::shared_ptr<Constrained> shared_ptr;
 
+			virtual ~Constrained() {}
+
 			/**
 			 * A diagonal noise model created by specifying a Vector of
 			 * standard devations, some of which might be zero
@@ -393,11 +404,13 @@ namespace gtsam {
 			Isotropic(size_t dim, double sigma) :
 				Diagonal(repeat(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {}
 
-		public:
-
 			/* dummy constructor to allow for serialization */
 			Isotropic() : Diagonal(repeat(1, 1.0)),sigma_(1.0),invsigma_(1.0) {}
 
+		public:
+
+			virtual ~Isotropic() {}
+
 			typedef boost::shared_ptr<Isotropic> shared_ptr;
 
 			/**
@@ -462,6 +475,8 @@ namespace gtsam {
 
 			typedef boost::shared_ptr<Unit> shared_ptr;
 
+			virtual ~Unit() {}
+
 			/**
 			 * Create a unit covariance noise model
 			 */

gtsam/linear/SharedGaussian.h

@@ -70,12 +70,6 @@ namespace gtsam { // note, deliberately not in noiseModel namespace
 		friend class boost::serialization::access;
 		template<class ARCHIVE>
 		void serialize(ARCHIVE & ar, const unsigned int version) {
-			// apparently requires that subclasses be registered?
-			ar.template register_type<noiseModel::Gaussian>();
-			ar.template register_type<noiseModel::Diagonal>();
-			ar.template register_type<noiseModel::Constrained>();
-			ar.template register_type<noiseModel::Isotropic>();
-			ar.template register_type<noiseModel::Unit>();
 			ar & boost::serialization::make_nvp("SharedGaussian",
 			        boost::serialization::base_object<noiseModel::Gaussian::shared_ptr >(*this));
 		}

gtsam/nonlinear/NonlinearEquality.h

@@ -70,12 +70,14 @@ namespace gtsam {
 		/** default constructor - only for serialization */
 		NonlinearEquality() {}
 
+		virtual ~NonlinearEquality() {}
+
 		/**
 		 * Constructor - forces exact evaluation
 		 */
 		NonlinearEquality(const KEY& j, const T& feasible, bool (*compare)(const T&, const T&) = compare<T>) :
 			Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible),
-			allow_error_(false), error_gain_(std::numeric_limits<double>::infinity()),
+			allow_error_(false), error_gain_(0.0),
 			compare_(compare) {
 		}
 

gtsam/nonlinear/NonlinearFactor.h

@@ -71,6 +71,8 @@ namespace gtsam {
 		NonlinearFactor() {
 		}
 
+//		virtual ~NonlinearFactor() {}
+
 		/**
 		 *  Constructor
 		 *  @param noiseModel shared pointer to a noise model
@@ -185,6 +187,8 @@ namespace gtsam {
 		NonlinearFactor1() {
 		}
 
+		virtual ~NonlinearFactor1() {}
+
 		inline const KEY& key() const {
 			return key_;
 		}
@@ -310,6 +314,8 @@ namespace gtsam {
 			this->keys_.push_back(key2_);
 		}
 
+		virtual ~NonlinearFactor2() {}
+
 		/** Print */
 		virtual void print(const std::string& s = "") const {
 			std::cout << s << ": NonlinearFactor2\n";
@@ -449,6 +455,8 @@ namespace gtsam {
       this->keys_.push_back(key3_);
     }
 
+    virtual ~NonlinearFactor3() {}
+
     /** Print */
     virtual void print(const std::string& s = "") const {
       std::cout << s << ": NonlinearFactor3\n";

gtsam/nonlinear/NonlinearFactorGraph.h

@@ -96,7 +96,15 @@ namespace gtsam {
 		boost::shared_ptr<FactorGraph<JacobianFactor> >
 				linearize(const VALUES& config, const Ordering& ordering) const;
 
+	private:
 
+		/** Serialization function */
+		friend class boost::serialization::access;
+		template<class ARCHIVE>
+		void serialize(ARCHIVE & ar, const unsigned int version) {
+			ar & boost::serialization::make_nvp("NonlinearFactorGraph",
+								boost::serialization::base_object<Base>(*this));
+		}
 	};
 
 } // namespace

gtsam/slam/BearingFactor.h

@@ -46,6 +46,8 @@ namespace gtsam {
 					Base(model, i, j), z_(z) {
 		}
 
+		virtual ~BearingFactor() {}
+
 		/** h(x)-z -> between(z,h(x)) for Rot2 manifold */
 		Vector evaluateError(const Pose2& pose, const Point2& point,
 				boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {

gtsam/slam/BearingRangeFactor.h

@@ -46,6 +46,8 @@ namespace gtsam {
 					Base(model, i, j), bearing_(bearing), range_(range) {
 		}
 
+		virtual ~BearingRangeFactor() {}
+
 		/** h(x)-z -> between(z,h(x)) for Rot2 manifold */
 		Vector evaluateError(const Pose2& pose, const Point2& point,
 				boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {

gtsam/slam/BetweenFactor.h

@@ -57,6 +57,8 @@ namespace gtsam {
 			Base(model, key1, key2), measured_(measured) {
 		}
 
+		virtual ~BetweenFactor() {}
+
 		/** implement functions needed for Testable */
 
 		/** print */

gtsam/slam/GeneralSFMFactor.h

@@ -46,6 +46,8 @@ namespace gtsam {
 		GeneralSFMFactor(double x, double y):z_(x,y) {}
 		GeneralSFMFactor(const Point2& z, const SharedGaussian& model, const CamK& i, const LmK& j) : Base(model, i, j), z_(z) {}
 
+		virtual ~GeneralSFMFactor() {}
+
 		/**
 		 * print
 		 * @param s optional string naming the factor

gtsam/slam/PriorFactor.h

@@ -51,6 +51,8 @@ namespace gtsam {
 		/** default constructor - only use for serialization */
 		PriorFactor() {}
 
+		virtual ~PriorFactor() {}
+
 		/** Constructor */
 		PriorFactor(const KEY& key, const T& prior,
 				const SharedGaussian& model) :

gtsam/slam/RangeFactor.h

@@ -43,6 +43,8 @@ namespace gtsam {
 					Base(model, i, j), z_(z) {
 		}
 
+		virtual ~RangeFactor() {}
+
 		/** h(x)-z */
 		Vector evaluateError(const Pose2& pose, const Point2& point,
 				boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {

gtsam/slam/StereoFactor.h

@@ -62,6 +62,8 @@ public:
 		Base(model, j_pose, j_landmark), z_(z), K_(K), baseline_(baseline) {
 	}
 
+	virtual ~GenericStereoFactor() {}
+
 	/**
 	 * print
 	 * @param s optional string naming the factor

gtsam/slam/planarSLAM.h

@@ -17,6 +17,9 @@
 
 #pragma once
 
+#include <boost/serialization/serialization.hpp>
+#include <boost/serialization/export.hpp>
+
 #include <gtsam/slam/BearingRangeFactor.h>
 #include <gtsam/nonlinear/TupleValues.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>

tests/testSerialization.cpp

@@ -29,6 +29,7 @@
 #include <boost/serialization/vector.hpp>
 #include <boost/serialization/map.hpp>
 #include <boost/serialization/list.hpp>
+#include <boost/serialization/export.hpp>
 
 #include <boost/archive/text_oarchive.hpp>
 #include <boost/archive/text_iarchive.hpp>
@@ -153,6 +154,7 @@ bool equalsDereferencedXML(const T& input = T()) {
 
 using namespace std;
 using namespace gtsam;
+using namespace planarSLAM;
 
 /* ************************************************************************* */
 TEST (Serialization, text_geometry) {
@@ -195,29 +197,23 @@ TEST (Serialization, xml_linear) {
 }
 
 /* ************************************************************************* */
-TEST (Serialization, Shared_noiseModels) {
+// example noise models
-	SharedDiagonal diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3));
+noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3));
-	SharedGaussian iso3 = noiseModel::Isotropic::Sigma(3, 0.3);
+noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3));
-	SharedGaussian gaussian3 = noiseModel::Gaussian::SqrtInformation(eye(3,3));
+noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2);
+noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1));
+noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3);
 
-	EXPECT(equalsDereferenced<SharedDiagonal>(diag3));
+// export GUIDs for noisemodels
-	EXPECT(equalsDereferencedXML<SharedDiagonal>(diag3));
+BOOST_CLASS_EXPORT_GUID(noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
-
+BOOST_CLASS_EXPORT_GUID(noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
-	EXPECT(equalsDereferenced<SharedGaussian>(iso3));
+BOOST_CLASS_EXPORT_GUID(noiseModel::Constrained, "gtsam_noiseModel_Constrained");
-	EXPECT(equalsDereferencedXML<SharedGaussian>(iso3));
+BOOST_CLASS_EXPORT_GUID(noiseModel::Unit, "gtsam_noiseModel_Unit");
-
+BOOST_CLASS_EXPORT_GUID(noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
-	EXPECT(equalsDereferenced<SharedGaussian>(gaussian3));
-	EXPECT(equalsDereferencedXML<SharedGaussian>(gaussian3));
-}
 
 /* ************************************************************************* */
 TEST (Serialization, noiseModels) {
-	noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3));
+	// tests using pointers to the derived class
-	noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3));
-	noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2);
-	noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::All(3);
-	noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3);
-
 	EXPECT(   equalsDereferenced<noiseModel::Diagonal::shared_ptr>(diag3));
 	EXPECT(equalsDereferencedXML<noiseModel::Diagonal::shared_ptr>(diag3));
 
@@ -227,17 +223,57 @@ TEST (Serialization, noiseModels) {
 	EXPECT(   equalsDereferenced<noiseModel::Isotropic::shared_ptr>(iso3));
 	EXPECT(equalsDereferencedXML<noiseModel::Isotropic::shared_ptr>(iso3));
 
-	// FAIL: stream error
+	EXPECT(   equalsDereferenced<noiseModel::Constrained::shared_ptr>(constrained3));
-//	EXPECT(   equalsDereferenced<noiseModel::Constrained::shared_ptr>(constrained3));
+	EXPECT(equalsDereferencedXML<noiseModel::Constrained::shared_ptr>(constrained3));
-//	EXPECT(equalsDereferencedXML<noiseModel::Constrained::shared_ptr>(constrained3));
 
 	EXPECT(   equalsDereferenced<noiseModel::Unit::shared_ptr>(unit3));
 	EXPECT(equalsDereferencedXML<noiseModel::Unit::shared_ptr>(unit3));
 }
 
+/* ************************************************************************* */
+TEST (Serialization, SharedGaussian_noiseModels) {
+	EXPECT(equalsDereferenced<SharedGaussian>(diag3));
+	EXPECT(equalsDereferencedXML<SharedGaussian>(diag3));
+
+	EXPECT(equalsDereferenced<SharedGaussian>(iso3));
+	EXPECT(equalsDereferencedXML<SharedGaussian>(iso3));
+
+	EXPECT(equalsDereferenced<SharedGaussian>(gaussian3));
+	EXPECT(equalsDereferencedXML<SharedGaussian>(gaussian3));
+
+	EXPECT(equalsDereferenced<SharedGaussian>(unit3));
+	EXPECT(equalsDereferencedXML<SharedGaussian>(unit3));
+
+	EXPECT(equalsDereferenced<SharedGaussian>(constrained3));
+	EXPECT(equalsDereferencedXML<SharedGaussian>(constrained3));
+}
+
+/* ************************************************************************* */
+TEST (Serialization, SharedDiagonal_noiseModels) {
+	EXPECT(equalsDereferenced<SharedDiagonal>(diag3));
+	EXPECT(equalsDereferencedXML<SharedDiagonal>(diag3));
+
+	EXPECT(equalsDereferenced<SharedDiagonal>(iso3));
+	EXPECT(equalsDereferencedXML<SharedDiagonal>(iso3));
+
+	EXPECT(equalsDereferenced<SharedDiagonal>(unit3));
+	EXPECT(equalsDereferencedXML<SharedDiagonal>(unit3));
+
+	EXPECT(equalsDereferenced<SharedDiagonal>(constrained3));
+	EXPECT(equalsDereferencedXML<SharedDiagonal>(constrained3));
+}
+
+/* ************************************************************************* */
+// exporting factor classes
+BOOST_CLASS_EXPORT_GUID(Prior, "gtsam_planarSLAM_Prior");
+BOOST_CLASS_EXPORT_GUID(Bearing, "gtsam_planarSLAM_Bearing");
+BOOST_CLASS_EXPORT_GUID(Range, "gtsam_planarSLAM_Range");
+BOOST_CLASS_EXPORT_GUID(BearingRange, "gtsam_planarSLAM_BearingRange");
+BOOST_CLASS_EXPORT_GUID(Odometry, "gtsam_planarSLAM_Odometry");
+BOOST_CLASS_EXPORT_GUID(Constraint, "gtsam_planarSLAM_Constraint");
+
 /* ************************************************************************* */
 TEST (Serialization, planar_system) {
-	using namespace planarSLAM;
 
 	Values values;
 	values.insert(PointKey(3), Point2(1.0, 2.0));
@@ -247,18 +283,19 @@ TEST (Serialization, planar_system) {
 	SharedGaussian model2 = noiseModel::Isotropic::Sigma(2, 0.3);
 	SharedGaussian model3 = noiseModel::Isotropic::Sigma(3, 0.3);
 
-	Graph graph;
 	Prior prior(PoseKey(3), Pose2(0.1,-0.3, 0.2), model1);
-	graph.add(prior);
 	Bearing bearing(PoseKey(3), PointKey(5), Rot2::fromDegrees(0.5), model1);
-	graph.add(bearing);
 	Range range(PoseKey(2), PointKey(9), 7.0, model1);
-	graph.add(range);
 	BearingRange bearingRange(PoseKey(2), PointKey(3), Rot2::fromDegrees(0.6), 2.0, model2);
-	graph.add(bearingRange);
 	Odometry odometry(PoseKey(2), PoseKey(3), Pose2(1.0, 2.0, 0.3), model3);
-	graph.add(odometry);
 	Constraint constraint(PoseKey(9), Pose2(2.0,-1.0, 0.2));
+
+	Graph graph;
+	graph.add(prior);
+	graph.add(bearing);
+	graph.add(range);
+	graph.add(bearingRange);
+	graph.add(odometry);
 	graph.add(constraint);
 
 	// text
@@ -270,8 +307,8 @@ TEST (Serialization, planar_system) {
 	EXPECT(equalsObj<BearingRange>(bearingRange));
 	EXPECT(equalsObj<Range>(range));
 	EXPECT(equalsObj<Odometry>(odometry));
-//	EXPECT(equalsObj<Constraint>(constraint)); // FAIL: stream error
+	EXPECT(equalsObj<Constraint>(constraint));
-//	EXPECT(equalsObj<Graph>(graph)); // FAIL: segfaults if there are factors
+	EXPECT(equalsObj<Graph>(graph));
 
 	// xml
 	EXPECT(equalsXML<PoseKey>(PoseKey(2)));
@@ -282,8 +319,8 @@ TEST (Serialization, planar_system) {
 	EXPECT(equalsXML<BearingRange>(bearingRange));
 	EXPECT(equalsXML<Range>(range));
 	EXPECT(equalsXML<Odometry>(odometry));
-//	EXPECT(equalsXML<Constraint>(constraint)); // FAIL: stream error
+	EXPECT(equalsXML<Constraint>(constraint));
-//	EXPECT(equalsXML<Graph>(graph));
+	EXPECT(equalsXML<Graph>(graph));
 }
 
 /* ************************************************************************* */