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

Made use of Bearing/Range functors now

release/4.3a0
Frank Dellaert 2015-07-12 16:34:23 -07:00
parent 02b703c5c8
commit 05ce9b1e0e
1 changed files with 106 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

109
gtsam/slam/BearingRangeFactor.h
View File

@ -13,61 +13,71 @@
* @file BearingRangeFactor.h * @file BearingRangeFactor.h
* @date Apr 1, 2010 * @date Apr 1, 2010
* @author Kai Ni * @author Kai Ni
* @brief a single factor contains both the bearing and the range to prevent handle to pair bearing and range factors * @brief a single factor contains both the bearing and the range to prevent
* handle to pair bearing and range factors
*/ */
#pragma once #pragma once
#include <gtsam/geometry/concepts.h>
#include <gtsam/base/Testable.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/Testable.h>
#include <boost/concept/assert.hpp>
namespace gtsam { 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;
/** /**
* Binary factor for a bearing measurement * Binary factor for a bearing/range measurement
* @addtogroup SLAM * @addtogroup SLAM
*/ */
template<class POSE, class POINT, class ROTATION = typename POSE::Rotation> template <typename A1, typename A2,
class BearingRangeFactor: public NoiseModelFactor2<POSE, POINT> typename BEARING = typename Bearing<A1, A2>::result_type,
{ typename RANGE = typename Range<A1, A2>::result_type>
class BearingRangeFactor : public NoiseModelFactor2<A1, A2> {
public: public:
typedef BearingRangeFactor<POSE, POINT> This; typedef BearingRangeFactor<A1, A2> This;
typedef NoiseModelFactor2<POSE, POINT> Base; typedef NoiseModelFactor2<A1, A2> Base;
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
private: private:
typedef POSE Pose;
typedef ROTATION Rot;
typedef POINT Point;
// the measurement // the measurement
Rot measuredBearing_; BEARING measuredBearing_;
double measuredRange_; RANGE measuredRange_;
/** concept check by type */ /** concept check by type */
GTSAM_CONCEPT_TESTABLE_TYPE(Rot) BOOST_CONCEPT_ASSERT((IsTestable<BEARING>));
GTSAM_CONCEPT_POSE_TYPE(Pose) BOOST_CONCEPT_ASSERT((IsTestable<RANGE>));
public: public:
BearingRangeFactor() {} /* Default constructor */ BearingRangeFactor() {} /* Default constructor */
BearingRangeFactor(Key poseKey, Key pointKey, const Rot& measuredBearing, const double measuredRange, BearingRangeFactor(Key poseKey, Key pointKey, const BEARING& measuredBearing,
const SharedNoiseModel& model) : const RANGE measuredRange, const SharedNoiseModel& model)
Base(model, poseKey, pointKey), measuredBearing_(measuredBearing), measuredRange_(measuredRange) { : Base(model, poseKey, pointKey),
} measuredBearing_(measuredBearing),
measuredRange_(measuredRange) {}
virtual ~BearingRangeFactor() {} virtual ~BearingRangeFactor() {}
/// @return a deep copy of this factor /// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const { virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); } gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/** Print */ /** Print */
virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { virtual void print(
std::cout << s << "BearingRangeFactor(" const std::string& s = "",
<< keyFormatter(this->key1()) << "," const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "BearingRangeFactor(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n"; << keyFormatter(this->key2()) << ")\n";
measuredBearing_.print("measured bearing: "); measuredBearing_.print("measured bearing: ");
std::cout << "measured range: " << measuredRange_ << std::endl; std::cout << "measured range: " << measuredRange_ << std::endl;
@ -75,46 +85,51 @@ namespace gtsam {
} }
/** equals */ /** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { virtual bool equals(const NonlinearFactor& expected, RANGE tol = 1e-9) const {
const This* e = dynamic_cast<const This*>(&expected); const This* e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) && return e != NULL && Base::equals(*e, tol) &&
fabs(this->measuredRange_ - e->measuredRange_) < tol && fabs(this->measuredRange_ - e->measuredRange_) < tol &&
this->measuredBearing_.equals(e->measuredBearing_, tol); this->measuredBearing_.equals(e->measuredBearing_, tol);
} }
/** h(x)-z -> between(z,h(x)) for Rot manifold */ /** h(x)-z -> between(z,h(x)) for BEARING manifold */
Vector evaluateError(const Pose& pose, const Point& point, Vector evaluateError(const A1& pose, const A2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1,
Matrix H11, H21, H12, H22; boost::optional<Matrix&> H2) const {
boost::optional<Matrix&> H11_ = H1 ? boost::optional<Matrix&>(H11) : boost::optional<Matrix&>(); typename MakeJacobian<BEARING, A1>::type HB1;
boost::optional<Matrix&> H21_ = H1 ? boost::optional<Matrix&>(H21) : boost::optional<Matrix&>(); typename MakeJacobian<BEARING, A2>::type HB2;
boost::optional<Matrix&> H12_ = H2 ? boost::optional<Matrix&>(H12) : boost::optional<Matrix&>(); typename MakeJacobian<RANGE, A1>::type HR1;
boost::optional<Matrix&> H22_ = H2 ? boost::optional<Matrix&>(H22) : boost::optional<Matrix&>(); typename MakeJacobian<RANGE, A2>::type HR2;
Rot y1 = pose.bearing(point, H11_, H12_); BEARING y1 = Bearing<A1, A2>()(pose, point, H1 ? &HB1 : 0, H2 ? &HB2 : 0);
Vector e1 = Rot::Logmap(measuredBearing_.between(y1)); Vector e1 = traits<BEARING>::Logmap(traits<BEARING>::Between(measuredBearing_,y1));
double y2 = pose.range(point, H21_, H22_); RANGE y2 = Range<A1, A2>()(pose, point, H1 ? &HR1 : 0, H2 ? &HR2 : 0);
Vector e2 = (Vector(1) << y2 - measuredRange_).finished(); Vector e2 = traits<RANGE>::Logmap(traits<RANGE>::Between(measuredRange_, y2));
if (H1) *H1 = gtsam::stack(2, &H11, &H21); if (H1) {
if (H2) *H2 = gtsam::stack(2, &H12, &H22); H1->resize(HB1.RowsAtCompileTime + HR1.RowsAtCompileTime, HB1.ColsAtCompileTime);
*H1 << HB1, HR1;
}
if (H2) {
H2->resize(HB2.RowsAtCompileTime + HR2.RowsAtCompileTime, HB2.ColsAtCompileTime);
*H2 << HB2, HR2;
}
return concatVectors(2, &e1, &e2); return concatVectors(2, &e1, &e2);
} }
/** return the measured */ /** return the measured */
const std::pair<Rot, double> measured() const { const std::pair<BEARING, RANGE> measured() const {
return std::make_pair(measuredBearing_, measuredRange_); return std::make_pair(measuredBearing_, measuredRange_);
} }
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend typename boost::serialization::access;
template<class ARCHIVE> template <typename ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) { void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NoiseModelFactor2", ar& boost::serialization::make_nvp(
boost::serialization::base_object<Base>(*this)); "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
ar& BOOST_SERIALIZATION_NVP(measuredBearing_); ar& BOOST_SERIALIZATION_NVP(measuredBearing_);
ar& BOOST_SERIALIZATION_NVP(measuredRange_); ar& BOOST_SERIALIZATION_NVP(measuredRange_);
} }