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

Fully serializable expression factors

release/4.3a0
Frank Dellaert 2015-07-11 15:55:01 -07:00
parent 07bb930dbb
commit ed0d66cf62
3 changed files with 121 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

97
gtsam/nonlinear/ExpressionFactor.h
View File

@ -46,19 +46,10 @@ public:
typedef boost::shared_ptr<ExpressionFactor<T> > shared_ptr;
/// Constructor
ExpressionFactor(const SharedNoiseModel& noiseModel, //
const T& measurement, const Expression<T>& expression) :
measurement_(measurement), expression_(expression) {
if (!noiseModel)
throw std::invalid_argument("ExpressionFactor: no NoiseModel.");
if (noiseModel->dim() != Dim)
throw std::invalid_argument(
"ExpressionFactor was created with a NoiseModel of incorrect dimension.");
noiseModel_ = noiseModel;
// Get keys and dimensions for Jacobian matrices
// An Expression is assumed unmutable, so we do this now
boost::tie(keys_, dims_) = expression_.keysAndDims();
ExpressionFactor(const SharedNoiseModel& noiseModel, //
const T& measurement, const Expression<T>& expression)
: NoiseModelFactor(noiseModel), measurement_(measurement) {
initialize(expression);
}
/// print relies on Testable traits being defined for T
@ -71,7 +62,8 @@ public:
bool equals(const NonlinearFactor& f, double tol) const {
const ExpressionFactor* p = dynamic_cast<const ExpressionFactor*>(&f);
return p && NoiseModelFactor::equals(f, tol) &&
traits<T>::Equals(measurement_, p->measurement_, tol);
traits<T>::Equals(measurement_, p->measurement_, tol) &&
dims_ == p->dims_;
}
/**
@ -132,13 +124,84 @@ public:
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
protected:
/// Default constructor, for serialization
ExpressionFactor() {}
protected:
/// Default constructor, for serialization
ExpressionFactor() {}
/// Constructor for use by SerializableExpressionFactor
ExpressionFactor(const SharedNoiseModel& noiseModel, const T& measurement)
: NoiseModelFactor(noiseModel), measurement_(measurement) {
// Not properly initialized yet, need to call initialize
}
/// Initialize with constructor arguments
void initialize(const Expression<T>& expression) {
if (!noiseModel_)
throw std::invalid_argument("ExpressionFactor: no NoiseModel.");
if (noiseModel_->dim() != Dim)
throw std::invalid_argument(
"ExpressionFactor was created with a NoiseModel of incorrect dimension.");
expression_ = expression;
// Get keys and dimensions for Jacobian matrices
// An Expression is assumed unmutable, so we do this now
boost::tie(keys_, dims_) = expression_.keysAndDims();
}
private:
/// Serialization function
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor);
ar& boost::serialization::make_nvp("measurement_", this->measurement_);
}
friend class boost::serialization::access;
};
// ExpressionFactor
/**
* ExpressionFactor variant that supports serialization
* Simply overload the pure virtual method [expression] to construct an expression from keys_
*/
template <typename T>
class SerializableExpressionFactor : public ExpressionFactor<T> {
public:
/// Constructor takes only two arguments, still need to call initialize
SerializableExpressionFactor(const SharedNoiseModel& noiseModel, const T& measurement)
: ExpressionFactor<T>(noiseModel, measurement) {
}
protected:
/// Return an expression that predicts the measurement given Values
virtual Expression<T> expression() const = 0;
/// Default constructor, for serialization
SerializableExpressionFactor() {}
/// Save to an archive: just saves the base class
template <class Archive>
void save(Archive& ar, const unsigned int /*version*/) const {
ar << boost::serialization::make_nvp(
"ExpressionFactor", boost::serialization::base_object<ExpressionFactor<T> >(*this));
}
/// Load from an archive, creating a valid expression using the overloaded [expression] method
template <class Archive>
void load(Archive& ar, const unsigned int /*version*/) {
ar >> boost::serialization::make_nvp(
"ExpressionFactor", boost::serialization::base_object<ExpressionFactor<T> >(*this));
this->initialize(expression());
}
// Indicate that we implement save/load separately, and be friendly to boost
BOOST_SERIALIZATION_SPLIT_MEMBER()
friend class boost::serialization::access;
};
// SerializableExpressionFactor
/// traits
template <typename T>
struct traits<ExpressionFactor<T> > : public Testable<ExpressionFactor<T> > {};

47
gtsam/sam/BearingFactor.h
View File

@ -27,16 +27,6 @@ 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
@ -47,10 +37,10 @@ namespace gtsam {
* @addtogroup SAM
*/
template <class Pose, class Point, class Measured = typename Pose::Rotation>
class BearingFactor : public ExpressionFactor<Measured> {
class BearingFactor : public SerializableExpressionFactor<Measured> {
private:
typedef BearingFactor<Pose, Point> This;
typedef ExpressionFactor<Measured> Base;
typedef SerializableExpressionFactor<Measured> Base;
/** concept check by type */
GTSAM_CONCEPT_TESTABLE_TYPE(Measured)
@ -61,37 +51,28 @@ class BearingFactor : public ExpressionFactor<Measured> {
BearingFactor() {}
/// primary constructor
BearingFactor(Key poseKey, Key pointKey, const Measured& measured,
const SharedNoiseModel& model)
: Base(model, measured,
Expression<Measured>(Expression<Pose>(poseKey), &Pose::bearing,
Expression<Point>(pointKey))) {}
BearingFactor(Key poseKey, Key pointKey, const Measured& measured, const SharedNoiseModel& model)
: Base(model, measured) {
this->keys_.push_back(poseKey);
this->keys_.push_back(pointKey);
this->initialize(expression());
}
virtual ~BearingFactor() {}
/** print contents */
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "BearingFactor, bearing = ";
this->measurement_.print();
Base::print("", keyFormatter);
std::cout << s << "BearingFactor" << std::endl;
Base::print(s, 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(
"NoiseModelFactor",
boost::serialization::base_object<NoiseModelFactor>(*this));
ar& boost::serialization::make_nvp("measurement_", this->measurement_);
// Return an expression
virtual Expression<Measured> expression() const {
return Expression<Measured>(Expression<Pose>(this->keys_[0]), &Pose::bearing,
Expression<Point>(this->keys_[1]));
}
template <class Archive>
friend void boost::serialization::serialize(Archive& ar, Base& factor,
const unsigned int version);
}; // BearingFactor
/// traits

41
gtsam/sam/tests/testBearingFactor.cpp
View File

@ -29,30 +29,41 @@ using namespace std;
using namespace gtsam;
// Create a noise model for the pixel error
static SharedNoiseModel model(noiseModel::Unit::Create(1));
typedef BearingFactor<Pose2, Point2> BearingFactor2D;
typedef BearingFactor<Pose3, Point3, Unit3> BearingFactor3D;
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5));
Key poseKey(1);
Key pointKey(2);
typedef BearingFactor<Pose2, Point2> BearingFactor2D;
double measurement2D(10.0);
BearingFactor2D factor2D(poseKey, pointKey, measurement2D, model);
GTSAM_CONCEPT_TESTABLE_INST(BearingFactor2D)
typedef BearingFactor<Pose3, Point3, Unit3> BearingFactor3D;
/* ************************************************************************* */
// Export Noisemodels
// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic);
//BOOST_CLASS_EXPORT(BearingFactor2D);
//BOOST_CLASS_EXPORT(ExpressionFactor<Rot2>);
//BOOST_CLASS_EXPORT_GUID(gtsam::ExpressionFactor<Rot2>, "ExpressionFactorRot2")
/* ************************************************************************* */
TEST(BearingFactor, Serialization2D) {
EXPECT(serializationTestHelpers::equalsObj<BearingFactor2D>(factor2D));
EXPECT(serializationTestHelpers::equalsXML<BearingFactor2D>(factor2D));
EXPECT(serializationTestHelpers::equalsBinary<BearingFactor2D>(factor2D));
}
/* ************************************************************************* */
TEST(BearingFactor, 2D) {
// Create a factor
double measurement(10.0);
BearingFactor2D factor(poseKey, pointKey, measurement, model);
std::string serialized = serializeXML(factor);
cout << serialized << endl;
// Serialize the factor
std::string serialized = serializeXML(factor2D);
EXPECT(serializationTestHelpers::equalsObj<BearingFactor2D>(factor));
// And de-serialize it
BearingFactor2D factor;
deserializeXML(serialized, factor);
// Set the linearization point
Values values;
@ -63,15 +74,17 @@ TEST(BearingFactor, 2D) {
}
/* ************************************************************************* */
// TODO(frank): this is broken: (non-existing) Pose3::bearing should yield a Unit3
//TEST(BearingFactor, 3D) {
// TODO(frank): this is broken: (non-existing) Pose3::bearing should yield a
// Unit3
// TEST(BearingFactor, 3D) {
// // Create a factor
// Rot3 measurement;
// BearingFactor<Pose3, Point3> factor(poseKey, pointKey, measurement, model);
//
// // Set the linearization point
// Values values;
// values.insert(poseKey, Pose3(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, -3.0)));
// values.insert(poseKey, Pose3(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0,
// -3.0)));
// values.insert(pointKey, Point3(-2.0, 11.0, 1.0));
//
// EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5);