diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 0418382bf..39f8a5822 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -152,6 +152,15 @@ namespace gtsam { /** assert invariants */ void assertInvariants() const; + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(info_); + ar & BOOST_SERIALIZATION_NVP(matrix_); + } }; } diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 44e6e04ff..368681a77 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -255,6 +255,17 @@ namespace gtsam { /** Assert invariants after elimination */ void assertInvariants() const; + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(firstNonzeroBlocks_); + ar & BOOST_SERIALIZATION_NVP(Ab_); + ar & BOOST_SERIALIZATION_NVP(model_); + ar & BOOST_SERIALIZATION_NVP(matrix_); + } + }; // JacobianFactor /** return A*x */ diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 5ca6c5f1e..48474f2f6 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -140,16 +140,12 @@ public: VectorValues result; result.varStarts_ = varStarts_; result.values_ = values_.head(varStarts_.back()) + c.values_.head(varStarts_.back()); -// result.values_ = boost::numeric::ublas::project(values_, boost::numeric::ublas::range(0, varStarts_.back())) + -// boost::numeric::ublas::project(c.values_, boost::numeric::ublas::range(0, c.varStarts_.back())); return result; } void operator+=(const VectorValues& c) { assert(varStarts_ == c.varStarts_); this->values_ += c.values_.head(varStarts_.back()); -// this->values_ = boost::numeric::ublas::project(this->values_, boost::numeric::ublas::range(0, varStarts_.back())) + -// boost::numeric::ublas::project(c.values_, boost::numeric::ublas::range(0, c.varStarts_.back())); } @@ -215,8 +211,6 @@ public: bool flag = false ; size_t i=0; for (const double *v = x.values_.data(); i< (size_t) x.values_.size(); ++v) { -// BOOST_FOREACH(const double &v, x.values_) { -// double v = x(i); if ( *v < tol && *v > -tol) { flag = true ; break; @@ -225,7 +219,16 @@ public: } return flag; } -}; + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(values_); + ar & BOOST_SERIALIZATION_NVP(varStarts_); + } +}; // \class VectorValues definition /// Implementations of functions @@ -281,16 +284,12 @@ inline VectorValues::mapped_type VectorValues::operator[](Index variable) { checkVariable(variable); const size_t start = varStarts_[variable], n = varStarts_[variable+1] - start; return values_.segment(start, n); -// return boost::numeric::ublas::project(values_, -// boost::numeric::ublas::range(varStarts_[variable], varStarts_[variable+1])); } inline VectorValues::const_mapped_type VectorValues::operator[](Index variable) const { checkVariable(variable); const size_t start = varStarts_[variable], n = varStarts_[variable+1] - start; return values_.segment(start, n); -// return boost::numeric::ublas::project(values_, -// boost::numeric::ublas::range(varStarts_[variable], varStarts_[variable+1])); } struct DimSpec: public std::vector { diff --git a/tests/testSerialization.cpp b/tests/testSerialization.cpp index b818b319a..d48c88c44 100644 --- a/tests/testSerialization.cpp +++ b/tests/testSerialization.cpp @@ -145,10 +145,6 @@ bool equalsDereferencedXML(const T& input = T()) { #include #include -//#include -//#include -//#include - #include #include @@ -193,20 +189,6 @@ TEST (Serialization, xml_geometry) { EXPECT(equalsXML(Pose3(rt3, pt3))); } -/* ************************************************************************* */ -TEST (Serialization, text_linear) { - // NOT WORKING: -// EXPECT(equalsObj()); -// EXPECT(equalsObj()); -} - -/* ************************************************************************* */ -TEST (Serialization, xml_linear) { - // NOT WORKING: -// EXPECT(equalsXML()); -// EXPECT(equalsXML()); -} - /* ************************************************************************* */ // Export Noisemodels BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); @@ -282,6 +264,56 @@ TEST (Serialization, SharedDiagonal_noiseModels) { EXPECT(equalsDereferencedXML(constrained3)); } +/* ************************************************************************* */ +// Linear components +/* ************************************************************************* */ + +#include +#include +#include + +/* ************************************************************************* */ +TEST (Serialization, text_linear) { + vector dims; + dims.push_back(1); + dims.push_back(2); + dims.push_back(2); + double v[] = {1., 2., 3., 4., 5.}; + VectorValues values(dims, v); + EXPECT(equalsObj(values)); + + Index i1 = 4, i2 = 7; + Matrix A1 = eye(3), A2 = -1.0 * eye(3); + Vector b = ones(3); + SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 1.0, 2.0, 3.0)); + JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model); + EXPECT(equalsObj(jacobianfactor)); + + HessianFactor hessianfactor(jacobianfactor); + EXPECT(equalsObj(hessianfactor)); +} + +/* ************************************************************************* */ +TEST (Serialization, xml_linear) { + vector dims; + dims.push_back(1); + dims.push_back(2); + dims.push_back(2); + double v[] = {1., 2., 3., 4., 5.}; + VectorValues values(dims, v); + EXPECT(equalsXML(values)); + + Index i1 = 4, i2 = 7; + Matrix A1 = eye(3), A2 = -1.0 * eye(3); + Vector b = ones(3); + SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 1.0, 2.0, 3.0)); + JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model); + EXPECT(equalsObj(jacobianfactor)); + + HessianFactor hessianfactor(jacobianfactor); + EXPECT(equalsObj(hessianfactor)); +} + /* ************************************************************************* */ /* Create GUIDs for factors */ BOOST_CLASS_EXPORT_GUID(gtsam::planarSLAM::Prior, "gtsam::planarSLAM::Prior");