12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added more serialization

release/4.3a0
Alex Cunningham 2011-05-20 18:43:42 +00:00
parent 2e942f08ac
commit 040493474f
4 changed files with 80 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/linear/HessianFactor.h
View File

@ -152,6 +152,15 @@ namespace gtsam {
/** assert invariants */
void assertInvariants() const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(info_);
ar & BOOST_SERIALIZATION_NVP(matrix_);
}
};
}

11
gtsam/linear/JacobianFactor.h
View File

@ -255,6 +255,17 @@ namespace gtsam {
/** Assert invariants after elimination */
void assertInvariants() const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
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 */

21
gtsam/linear/VectorValues.h
View File

@ -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<class ARCHIVE>
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<size_t> {

68
tests/testSerialization.cpp
View File

@ -145,10 +145,6 @@ bool equalsDereferencedXML(const T& input = T()) {
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Cal3_S2.h>
//#include <gtsam/linear/VectorValues.h>
//#include <gtsam/linear/GaussianConditional.h>
//#include <gtsam/inference/SymbolicConditional.h>
#include <gtsam/slam/planarSLAM.h>
#include <gtsam/slam/BearingFactor.h>
@ -193,20 +189,6 @@ TEST (Serialization, xml_geometry) {
EXPECT(equalsXML<gtsam::Pose3>(Pose3(rt3, pt3)));
}
/* ************************************************************************* */
TEST (Serialization, text_linear) {
// NOT WORKING:
// EXPECT(equalsObj<VectorValues>());
// EXPECT(equalsObj<GaussianConditional>());
}
/* ************************************************************************* */
TEST (Serialization, xml_linear) {
// NOT WORKING:
// EXPECT(equalsXML<VectorValues>());
// EXPECT(equalsXML<GaussianConditional>());
}
/* ************************************************************************* */
// Export Noisemodels
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
@ -282,6 +264,56 @@ TEST (Serialization, SharedDiagonal_noiseModels) {
EXPECT(equalsDereferencedXML<SharedDiagonal>(constrained3));
}
/* ************************************************************************* */
// Linear components
/* ************************************************************************* */
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h>
/* ************************************************************************* */
TEST (Serialization, text_linear) {
vector<size_t> 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<VectorValues>(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<VectorValues>(jacobianfactor));
HessianFactor hessianfactor(jacobianfactor);
EXPECT(equalsObj<VectorValues>(hessianfactor));
}
/* ************************************************************************* */
TEST (Serialization, xml_linear) {
vector<size_t> 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<VectorValues>(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<VectorValues>(jacobianfactor));
HessianFactor hessianfactor(jacobianfactor);
EXPECT(equalsObj<VectorValues>(hessianfactor));
}
/* ************************************************************************* */
/* Create GUIDs for factors */
BOOST_CLASS_EXPORT_GUID(gtsam::planarSLAM::Prior, "gtsam::planarSLAM::Prior");