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

Added binary specialization

release/4.3a0
Frank Dellaert 2015-07-12 12:06:13 -07:00
parent 752a6b5537
commit 3d4ea47ab9
3 changed files with 81 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
gtsam/nonlinear/SerializableExpressionFactor.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -24,21 +24,21 @@ namespace gtsam {
/**
* ExpressionFactor variant that supports serialization
* Simply overload the pure virtual method [expression] to construct an expression from keys_
* 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) {
}
SerializableExpressionFactor(const SharedNoiseModel& noiseModel,
const T& measurement)
: ExpressionFactor<T>(noiseModel, measurement) {}
/// Destructor
virtual ~SerializableExpressionFactor() {}
protected:
/// Return an expression that predicts the measurement given Values
virtual Expression<T> expression() const = 0;
@ -49,14 +49,17 @@ class SerializableExpressionFactor : public ExpressionFactor<T> {
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));
"ExpressionFactor",
boost::serialization::base_object<ExpressionFactor<T> >(*this));
}
/// Load from an archive, creating a valid expression using the overloaded [expression] method
/// 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));
"ExpressionFactor",
boost::serialization::base_object<ExpressionFactor<T> >(*this));
this->initialize(expression());
}
@ -66,10 +69,52 @@ class SerializableExpressionFactor : public ExpressionFactor<T> {
};
// SerializableExpressionFactor
/// traits
template <typename T>
struct traits<SerializableExpressionFactor<T> >
: public Testable<SerializableExpressionFactor<T> > {};
/**
* Binary specialization of SerializableExpressionFactor
* Enforces expression method with two keys, and provides evaluateError
*/
template <typename T, typename A1, typename A2>
class SerializableExpressionFactor2 : public SerializableExpressionFactor<T> {
public:
/// Constructor takes care of keys, but still need to call initialize
SerializableExpressionFactor2(Key key1, Key key2,
const SharedNoiseModel& noiseModel,
const T& measurement)
: SerializableExpressionFactor<T>(noiseModel, measurement) {
this->keys_.push_back(key1);
this->keys_.push_back(key2);
}
}// \ namespace gtsam
/// Destructor
virtual ~SerializableExpressionFactor2() {}
/// Backwards compatible evaluateError, to make existing tests compile
Vector evaluateError(const A1& a1, const A2& a2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
Values values;
values.insert(this->keys_[0], a1);
values.insert(this->keys_[1], a2);
std::vector<Matrix> H(2);
Vector error = this->unwhitenedError(values, H);
if (H1) (*H1) = H[0];
if (H2) (*H2) = H[1];
return error;
}
/// Return an expression that predicts the measurement given Values
virtual Expression<T> expression(Key key1, Key key2) const = 0;
protected:
/// Default constructor, for serialization
SerializableExpressionFactor2() {}
private:
/// Return an expression that predicts the measurement given Values
virtual Expression<T> expression() const {
return expression(this->keys_[0], this->keys_[1]);
}
};
// SerializableExpressionFactor2
} // \ namespace gtsam

29
gtsam/sam/BearingFactor.h
View File

@ -11,7 +11,8 @@
/**
* @file BearingFactor.h
* @brief Serializable factor induced by a bearing measurement of a point from a given pose
* @brief Serializable factor induced by a bearing measurement of a point from
*a given pose
* @date July 2015
* @author Frank Dellaert
**/
@ -28,34 +29,36 @@ namespace gtsam {
* @addtogroup SAM
*/
template <typename POSE, typename POINT, typename T>
struct BearingFactor : public SerializableExpressionFactor<T> {
struct BearingFactor : public SerializableExpressionFactor2<T, POSE, POINT> {
typedef SerializableExpressionFactor2<T, POSE, POINT> Base;
/// default constructor
BearingFactor() {}
/// primary constructor
BearingFactor(Key poseKey, Key pointKey, const T& measured, const SharedNoiseModel& model)
: SerializableExpressionFactor<T>(model, measured) {
this->keys_.push_back(poseKey);
this->keys_.push_back(pointKey);
this->initialize(expression());
BearingFactor(Key key1, Key key2, const T& measured,
const SharedNoiseModel& model)
: Base(key1, key2, model, measured) {
this->initialize(expression(key1, key2));
}
// Return measurement expression
virtual Expression<T> expression() const {
return Expression<T>(Expression<POSE>(this->keys_[0]), &POSE::bearing,
Expression<POINT>(this->keys_[1]));
virtual Expression<T> expression(Key key1, Key key2) const {
return Expression<T>(Expression<POSE>(key1), &POSE::bearing,
Expression<POINT>(key2));
}
/// print
void print(const std::string& s = "", const KeyFormatter& kf = DefaultKeyFormatter) const {
void print(const std::string& s = "",
const KeyFormatter& kf = DefaultKeyFormatter) const {
std::cout << s << "BearingFactor" << std::endl;
SerializableExpressionFactor<T>::print(s, kf);
Base::print(s, kf);
}
}; // BearingFactor
/// traits
template <class POSE, class POINT, class T>
struct traits<BearingFactor<POSE, POINT, T> > : public Testable<BearingFactor<POSE, POINT, T> > {};
struct traits<BearingFactor<POSE, POINT, T> >
: public Testable<BearingFactor<POSE, POINT, T> > {};
} // namespace gtsam

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

@ -38,7 +38,7 @@ static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(1, 0.5));
BearingFactor2D factor2D(poseKey, pointKey, measurement2D, model2D);
typedef BearingFactor<Pose3, Point3, Unit3> BearingFactor3D;
Unit3 measurement3D = Pose3().bearing(Point3(1,0,0)); // has to match values!
Unit3 measurement3D = Pose3().bearing(Point3(1, 0, 0)); // has to match values!
static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(2, 0.5));
BearingFactor3D factor3D(poseKey, pointKey, measurement3D, model3D);
@ -68,7 +68,8 @@ TEST(BearingFactor, 2D) {
values.insert(poseKey, Pose2(1.0, 2.0, 0.57));
values.insert(pointKey, Point2(-4.0, 11.0));
EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(), values, 1e-7,1e-5);
EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey),
values, 1e-7, 1e-5);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
@ -91,9 +92,10 @@ TEST(BearingFactor, 3D) {
// Set the linearization point
Values values;
values.insert(poseKey, Pose3());
values.insert(pointKey, Point3(1,0,0));
values.insert(pointKey, Point3(1, 0, 0));
EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(), values, 1e-7,1e-5);
EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey),
values, 1e-7, 1e-5);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}