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

Fixed BearingRangeFactor

release/4.3a0
Frank Dellaert 2015-07-12 20:51:01 -07:00
parent d9fe27a131
commit 850c8a7921
2 changed files with 13 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

92
gtsam/sam/BearingRangeFactor.h
View File

@ -20,50 +20,11 @@
#pragma once
#include <gtsam/nonlinear/SerializableExpressionFactor.h>
#include <gtsam/base/Testable.h>
#include <gtsam/geometry/BearingRange.h>
#include <boost/concept/assert.hpp>
namespace gtsam {
// forward declaration of Bearing functor, assumed partially specified
template <typename A1, typename A2>
struct Bearing;
// forward declaration of Range functor, assumed partially specified
template <typename A1, typename A2>
struct Range;
template <typename B, typename R>
struct BearingRange : public ProductManifold<B, R> {
BearingRange() {}
BearingRange(const ProductManifold<B, R>& br) : ProductManifold<B, R>(br) {}
BearingRange(const B& b, const R& r) : ProductManifold<B, R>(b, r) {}
private:
/// Serialization function
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp("bearing", this->first);
ar& boost::serialization::make_nvp("range", this->second);
}
friend class boost::serialization::access;
};
template <typename B, typename R>
struct traits<BearingRange<B, R> >
: internal::ManifoldTraits<BearingRange<B, R> > {
static void Print(const BearingRange<B, R>& m, const std::string& str = "") {
traits<B>::Print(m.first, str);
traits<R>::Print(m.second, str);
}
static bool Equals(const BearingRange<B, R>& m1, const BearingRange<B, R>& m2,
double tol = 1e-8) {
return traits<B>::Equals(m1.first, m2.first, tol) &&
traits<R>::Equals(m1.second, m2.second, tol);
}
};
/**
* Binary factor for a bearing/range measurement
* @addtogroup SLAM
@ -72,25 +33,21 @@ template <typename A1, typename A2,
typename B = typename Bearing<A1, A2>::result_type,
typename R = typename Range<A1, A2>::result_type>
class BearingRangeFactor
: public SerializableExpressionFactor2<BearingRange<B, R>, A1, A2> {
public:
typedef BearingRange<B, R> T;
: public SerializableExpressionFactor2<BearingRange<A1, A2>, A1, A2> {
private:
typedef BearingRange<A1, A2> T;
typedef SerializableExpressionFactor2<T, A1, A2> Base;
typedef BearingRangeFactor<A1, A2> This;
typedef boost::shared_ptr<This> shared_ptr;
private:
/** concept check by type */
BOOST_CONCEPT_ASSERT((IsTestable<B>));
BOOST_CONCEPT_ASSERT((IsTestable<R>));
public:
typedef boost::shared_ptr<This> shared_ptr;
/// default constructor
BearingRangeFactor() {}
/// primary constructor
BearingRangeFactor(Key key1, Key key2, const B& measuredBearing,
const R measuredRange, const SharedNoiseModel& model)
const R& measuredRange, const SharedNoiseModel& model)
: Base(key1, key2, model, T(measuredBearing, measuredRange)) {
this->initialize(expression(key1, key2));
}
@ -105,37 +62,14 @@ class BearingRangeFactor
// Return measurement expression
virtual Expression<T> expression(Key key1, Key key2) const {
Expression<A1> a1_(key1);
Expression<A2> a2_(key2);
return Expression<T>(This::Predict, a1_, a2_);
return Expression<T>(T::Measure, Expression<A1>(key1), Expression<A2>(key2));
}
/** Print */
virtual void print(
const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "BearingRangeFactor(" << keyFormatter(this->keys_[0])
<< "," << keyFormatter(this->keys_[1]) << ")\n";
traits<B>::Print(this->measured_.first, "measured bearing: ");
traits<R>::Print(this->measured_.second, "measured range: ");
this->noiseModel_->print("noise model:\n");
}
/// Prediction function that stacks measurements
static T Predict(const A1& pose, const A2& point,
typename MakeOptionalJacobian<T, A1>::type H1,
typename MakeOptionalJacobian<T, A2>::type H2) {
typename MakeJacobian<B, A1>::type HB1;
typename MakeJacobian<B, A2>::type HB2;
typename MakeJacobian<R, A1>::type HR1;
typename MakeJacobian<R, A2>::type HR2;
B b = Bearing<A1, A2>()(pose, point, H1 ? &HB1 : 0, H2 ? &HB2 : 0);
R r = Range<A1, A2>()(pose, point, H1 ? &HR1 : 0, H2 ? &HR2 : 0);
if (H1) *H1 << HB1, HR1;
if (H2) *H2 << HB2, HR2;
return T(b, r);
/// print
virtual void print(const std::string& s = "",
const KeyFormatter& kf = DefaultKeyFormatter) const {
std::cout << s << "BearingRangeFactor" << std::endl;
Base::print(s, kf);
}
}; // BearingRangeFactor

2
gtsam/sam/tests/testBearingRangeFactor.cpp
View File

@ -34,12 +34,10 @@ Key poseKey(1);
Key pointKey(2);
typedef BearingRangeFactor<Pose2, Point2> BearingRangeFactor2D;
BearingRangeFactor2D::T measurement2D(1, 2);
static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(2, 0.5));
BearingRangeFactor2D factor2D(poseKey, pointKey, 1, 2, model2D);
typedef BearingRangeFactor<Pose3, Point3> BearingRangeFactor3D;
BearingRangeFactor3D::T measurement3D(Pose3().bearing(Point3(1, 0, 0)), 0);
static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(3, 0.5));
BearingRangeFactor3D factor3D(poseKey, pointKey,
Pose3().bearing(Point3(1, 0, 0)), 1, model3D);