diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index e2ef0cb32..b4d23f147 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/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&> fi list 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 diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 904d0be1c..42b464e74 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -126,6 +126,8 @@ namespace gtsam { typedef boost::shared_ptr 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 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 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 shared_ptr; /** @@ -462,6 +475,8 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; + virtual ~Unit() {} + /** * Create a unit covariance noise model */ diff --git a/gtsam/linear/SharedGaussian.h b/gtsam/linear/SharedGaussian.h index 2271749b0..d14a43068 100644 --- a/gtsam/linear/SharedGaussian.h +++ b/gtsam/linear/SharedGaussian.h @@ -70,12 +70,6 @@ namespace gtsam { // note, deliberately not in noiseModel namespace friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - // apparently requires that subclasses be registered? - ar.template register_type(); - ar.template register_type(); - ar.template register_type(); - ar.template register_type(); - ar.template register_type(); ar & boost::serialization::make_nvp("SharedGaussian", boost::serialization::base_object(*this)); } diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 5d1f5e381..3c4de36d4 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/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) : Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible), - allow_error_(false), error_gain_(std::numeric_limits::infinity()), + allow_error_(false), error_gain_(0.0), compare_(compare) { } diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index f5bb7e1c8..09587cf22 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/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"; diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 1ed7ff60a..27255430f 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -96,7 +96,15 @@ namespace gtsam { boost::shared_ptr > linearize(const VALUES& config, const Ordering& ordering) const; + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactorGraph", + boost::serialization::base_object(*this)); + } }; } // namespace diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h index 5d4c758af..d87659cfe 100644 --- a/gtsam/slam/BearingFactor.h +++ b/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 H1, boost::optional H2) const { diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h index ef13b7374..3003983b9 100644 --- a/gtsam/slam/BearingRangeFactor.h +++ b/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 H1, boost::optional H2) const { diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 91d7b0a55..efb9e4e3f 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -57,6 +57,8 @@ namespace gtsam { Base(model, key1, key2), measured_(measured) { } + virtual ~BetweenFactor() {} + /** implement functions needed for Testable */ /** print */ diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 468776c5b..a49641d78 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/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 diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 7174d91bb..48e512555 100644 --- a/gtsam/slam/PriorFactor.h +++ b/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) : diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index 6480f39f4..68af9c380 100644 --- a/gtsam/slam/RangeFactor.h +++ b/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 H1, boost::optional H2) const { diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index b12dbdf27..93fa897d0 100644 --- a/gtsam/slam/StereoFactor.h +++ b/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 diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index 8d1c44a1b..6e14dfa61 100644 --- a/gtsam/slam/planarSLAM.h +++ b/gtsam/slam/planarSLAM.h @@ -17,6 +17,9 @@ #pragma once +#include +#include + #include #include #include diff --git a/tests/testSerialization.cpp b/tests/testSerialization.cpp index 5c037831f..420f4b3f0 100644 --- a/tests/testSerialization.cpp +++ b/tests/testSerialization.cpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include @@ -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) { - SharedDiagonal diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); - SharedGaussian iso3 = noiseModel::Isotropic::Sigma(3, 0.3); - SharedGaussian gaussian3 = noiseModel::Gaussian::SqrtInformation(eye(3,3)); +// example noise models +noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); +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::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1)); +noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3); - EXPECT(equalsDereferenced(diag3)); - EXPECT(equalsDereferencedXML(diag3)); - - EXPECT(equalsDereferenced(iso3)); - EXPECT(equalsDereferencedXML(iso3)); - - EXPECT(equalsDereferenced(gaussian3)); - EXPECT(equalsDereferencedXML(gaussian3)); -} +// export GUIDs for noisemodels +BOOST_CLASS_EXPORT_GUID(noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); +BOOST_CLASS_EXPORT_GUID(noiseModel::Constrained, "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT_GUID(noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); /* ************************************************************************* */ TEST (Serialization, noiseModels) { - noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); - 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); - + // tests using pointers to the derived class EXPECT( equalsDereferenced(diag3)); EXPECT(equalsDereferencedXML(diag3)); @@ -227,17 +223,57 @@ TEST (Serialization, noiseModels) { EXPECT( equalsDereferenced(iso3)); EXPECT(equalsDereferencedXML(iso3)); - // FAIL: stream error -// EXPECT( equalsDereferenced(constrained3)); -// EXPECT(equalsDereferencedXML(constrained3)); + EXPECT( equalsDereferenced(constrained3)); + EXPECT(equalsDereferencedXML(constrained3)); EXPECT( equalsDereferenced(unit3)); EXPECT(equalsDereferencedXML(unit3)); } +/* ************************************************************************* */ +TEST (Serialization, SharedGaussian_noiseModels) { + EXPECT(equalsDereferenced(diag3)); + EXPECT(equalsDereferencedXML(diag3)); + + EXPECT(equalsDereferenced(iso3)); + EXPECT(equalsDereferencedXML(iso3)); + + EXPECT(equalsDereferenced(gaussian3)); + EXPECT(equalsDereferencedXML(gaussian3)); + + EXPECT(equalsDereferenced(unit3)); + EXPECT(equalsDereferencedXML(unit3)); + + EXPECT(equalsDereferenced(constrained3)); + EXPECT(equalsDereferencedXML(constrained3)); +} + +/* ************************************************************************* */ +TEST (Serialization, SharedDiagonal_noiseModels) { + EXPECT(equalsDereferenced(diag3)); + EXPECT(equalsDereferencedXML(diag3)); + + EXPECT(equalsDereferenced(iso3)); + EXPECT(equalsDereferencedXML(iso3)); + + EXPECT(equalsDereferenced(unit3)); + EXPECT(equalsDereferencedXML(unit3)); + + EXPECT(equalsDereferenced(constrained3)); + EXPECT(equalsDereferencedXML(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)); EXPECT(equalsObj(range)); EXPECT(equalsObj(odometry)); -// EXPECT(equalsObj(constraint)); // FAIL: stream error -// EXPECT(equalsObj(graph)); // FAIL: segfaults if there are factors + EXPECT(equalsObj(constraint)); + EXPECT(equalsObj(graph)); // xml EXPECT(equalsXML(PoseKey(2))); @@ -282,8 +319,8 @@ TEST (Serialization, planar_system) { EXPECT(equalsXML(bearingRange)); EXPECT(equalsXML(range)); EXPECT(equalsXML(odometry)); -// EXPECT(equalsXML(constraint)); // FAIL: stream error -// EXPECT(equalsXML(graph)); + EXPECT(equalsXML(constraint)); + EXPECT(equalsXML(graph)); } /* ************************************************************************* */