Serialization of Factor base class works
parent
8f511078c4
commit
71231abf1b
|
|
@ -20,6 +20,26 @@
|
||||||
#include <gtsam/geometry/concepts.h>
|
#include <gtsam/geometry/concepts.h>
|
||||||
#include <gtsam/base/Testable.h>
|
#include <gtsam/base/Testable.h>
|
||||||
|
|
||||||
|
namespace boost {
|
||||||
|
namespace serialization {
|
||||||
|
|
||||||
|
template <class T, class Archive>
|
||||||
|
void serialize(Archive& ar, gtsam::NonlinearFactor& factor,
|
||||||
|
const unsigned int version) {}
|
||||||
|
|
||||||
|
// template <class T, class Archive>
|
||||||
|
// void serialize(Archive& ar, gtsam::ExpressionFactor<T>& factor,
|
||||||
|
// const unsigned int version) {
|
||||||
|
// ar& BOOST_SERIALIZATION_NVP(factor.measurement_);
|
||||||
|
//}
|
||||||
|
|
||||||
|
// template <class Factor, class Archive>
|
||||||
|
// void serialize(Archive& ar, Factor& factor, const unsigned int version) {
|
||||||
|
//}
|
||||||
|
|
||||||
|
} // namespace serialization
|
||||||
|
} // namespace boost
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
@ -36,8 +56,11 @@ class BearingFactor : public ExpressionFactor<Measured> {
|
||||||
GTSAM_CONCEPT_TESTABLE_TYPE(Measured)
|
GTSAM_CONCEPT_TESTABLE_TYPE(Measured)
|
||||||
GTSAM_CONCEPT_POSE_TYPE(Pose)
|
GTSAM_CONCEPT_POSE_TYPE(Pose)
|
||||||
|
|
||||||
|
/// Default constructor
|
||||||
|
BearingFactor() {}
|
||||||
|
|
||||||
public:
|
public:
|
||||||
/** primary constructor */
|
/// primary constructor
|
||||||
BearingFactor(Key poseKey, Key pointKey, const Measured& measured,
|
BearingFactor(Key poseKey, Key pointKey, const Measured& measured,
|
||||||
const SharedNoiseModel& model)
|
const SharedNoiseModel& model)
|
||||||
: Base(model, measured,
|
: Base(model, measured,
|
||||||
|
|
@ -53,6 +76,22 @@ class BearingFactor : public ExpressionFactor<Measured> {
|
||||||
this->measurement_.print();
|
this->measurement_.print();
|
||||||
Base::print("", keyFormatter);
|
Base::print("", keyFormatter);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
/** Serialization function */
|
||||||
|
friend class boost::serialization::access;
|
||||||
|
template <class ARCHIVE>
|
||||||
|
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||||
|
ar& boost::serialization::make_nvp(
|
||||||
|
"Factor", boost::serialization::base_object<Factor>(*this));
|
||||||
|
// ar& BOOST_SERIALIZATION_NVP(this->noiseModel_);
|
||||||
|
// ar& BOOST_SERIALIZATION_NVP(measurement_);
|
||||||
|
}
|
||||||
|
|
||||||
|
template <class Archive>
|
||||||
|
friend void boost::serialization::serialize(Archive& ar, Base& factor,
|
||||||
|
const unsigned int version);
|
||||||
|
|
||||||
}; // BearingFactor
|
}; // BearingFactor
|
||||||
|
|
||||||
} // namespace gtsam
|
} // namespace gtsam
|
||||||
|
|
|
||||||
|
|
@ -20,6 +20,7 @@
|
||||||
#include <gtsam/geometry/Pose2.h>
|
#include <gtsam/geometry/Pose2.h>
|
||||||
#include <gtsam/geometry/Pose3.h>
|
#include <gtsam/geometry/Pose3.h>
|
||||||
#include <gtsam/nonlinear/factorTesting.h>
|
#include <gtsam/nonlinear/factorTesting.h>
|
||||||
|
#include <gtsam/base/serialization.h>
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
|
|
@ -40,6 +41,8 @@ TEST(BearingFactor, 2D) {
|
||||||
// Create a factor
|
// Create a factor
|
||||||
double measurement(10.0);
|
double measurement(10.0);
|
||||||
BearingFactor<Pose2, Point2> factor(poseKey, pointKey, measurement, model);
|
BearingFactor<Pose2, Point2> factor(poseKey, pointKey, measurement, model);
|
||||||
|
std::string serialized = serializeXML(factor);
|
||||||
|
cout << serialized << endl;
|
||||||
|
|
||||||
// Set the linearization point
|
// Set the linearization point
|
||||||
Values values;
|
Values values;
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue