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

197
gtsam/slam/BearingRangeFactor.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 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -13,111 +13,126 @@
* @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
* Binary factor for a bearing measurement template <typename A1, typename A2>
* @addtogroup SLAM struct Bearing;
*/
template<class POSE, class POINT, class ROTATION = typename POSE::Rotation>
class BearingRangeFactor: public NoiseModelFactor2<POSE, POINT>
{
public:
typedef BearingRangeFactor<POSE, POINT> This;
typedef NoiseModelFactor2<POSE, POINT> Base;
typedef boost::shared_ptr<This> shared_ptr;
private: // forward declaration of Range functor, assumed partially specified
typedef POSE Pose; template <typename A1, typename A2>
typedef ROTATION Rot; struct Range;
typedef POINT Point;
// the measurement /**
Rot measuredBearing_; * Binary factor for a bearing/range measurement
double measuredRange_; * @addtogroup SLAM
*/
template <typename A1, typename A2,
typename BEARING = typename Bearing<A1, A2>::result_type,
typename RANGE = typename Range<A1, A2>::result_type>
class BearingRangeFactor : public NoiseModelFactor2<A1, A2> {
public:
typedef BearingRangeFactor<A1, A2> This;
typedef NoiseModelFactor2<A1, A2> Base;
typedef boost::shared_ptr<This> shared_ptr;
/** concept check by type */ private:
GTSAM_CONCEPT_TESTABLE_TYPE(Rot) // the measurement
GTSAM_CONCEPT_POSE_TYPE(Pose) BEARING measuredBearing_;
RANGE measuredRange_;
public: /** concept check by type */
BOOST_CONCEPT_ASSERT((IsTestable<BEARING>));
BOOST_CONCEPT_ASSERT((IsTestable<RANGE>));
BearingRangeFactor() {} /* Default constructor */ public:
BearingRangeFactor(Key poseKey, Key pointKey, const Rot& measuredBearing, const double measuredRange, BearingRangeFactor() {} /* Default constructor */
const SharedNoiseModel& model) : BearingRangeFactor(Key poseKey, Key pointKey, const BEARING& measuredBearing,
Base(model, poseKey, pointKey), measuredBearing_(measuredBearing), measuredRange_(measuredRange) { const RANGE measuredRange, const SharedNoiseModel& model)
: Base(model, poseKey, pointKey),
measuredBearing_(measuredBearing),
measuredRange_(measuredRange) {}
virtual ~BearingRangeFactor() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/** Print */
virtual void print(
const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "BearingRangeFactor(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n";
measuredBearing_.print("measured bearing: ");
std::cout << "measured range: " << measuredRange_ << std::endl;
this->noiseModel_->print("noise model:\n");
}
/** equals */
virtual bool equals(const NonlinearFactor& expected, RANGE tol = 1e-9) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) &&
fabs(this->measuredRange_ - e->measuredRange_) < tol &&
this->measuredBearing_.equals(e->measuredBearing_, tol);
}
/** h(x)-z -> between(z,h(x)) for BEARING manifold */
Vector evaluateError(const A1& pose, const A2& point,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
typename MakeJacobian<BEARING, A1>::type HB1;
typename MakeJacobian<BEARING, A2>::type HB2;
typename MakeJacobian<RANGE, A1>::type HR1;
typename MakeJacobian<RANGE, A2>::type HR2;
BEARING y1 = Bearing<A1, A2>()(pose, point, H1 ? &HB1 : 0, H2 ? &HB2 : 0);
Vector e1 = traits<BEARING>::Logmap(traits<BEARING>::Between(measuredBearing_,y1));
RANGE y2 = Range<A1, A2>()(pose, point, H1 ? &HR1 : 0, H2 ? &HR2 : 0);
Vector e2 = traits<RANGE>::Logmap(traits<RANGE>::Between(measuredRange_, y2));
if (H1) {
H1->resize(HB1.RowsAtCompileTime + HR1.RowsAtCompileTime, HB1.ColsAtCompileTime);
*H1 << HB1, HR1;
} }
if (H2) {
virtual ~BearingRangeFactor() {} H2->resize(HB2.RowsAtCompileTime + HR2.RowsAtCompileTime, HB2.ColsAtCompileTime);
*H2 << HB2, HR2;
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** Print */
virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "BearingRangeFactor("
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n";
measuredBearing_.print("measured bearing: ");
std::cout << "measured range: " << measuredRange_ << std::endl;
this->noiseModel_->print("noise model:\n");
} }
return concatVectors(2, &e1, &e2);
}
/** equals */ /** return the measured */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const std::pair<BEARING, RANGE> measured() const {
const This *e = dynamic_cast<const This*> (&expected); return std::make_pair(measuredBearing_, measuredRange_);
return e != NULL && Base::equals(*e, tol) && }
fabs(this->measuredRange_ - e->measuredRange_) < tol &&
this->measuredBearing_.equals(e->measuredBearing_, tol);
}
/** h(x)-z -> between(z,h(x)) for Rot manifold */ private:
Vector evaluateError(const Pose& pose, const Point& point, /** Serialization function */
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { friend typename boost::serialization::access;
Matrix H11, H21, H12, H22; template <typename ARCHIVE>
boost::optional<Matrix&> H11_ = H1 ? boost::optional<Matrix&>(H11) : boost::optional<Matrix&>(); void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
boost::optional<Matrix&> H21_ = H1 ? boost::optional<Matrix&>(H21) : boost::optional<Matrix&>(); ar& boost::serialization::make_nvp(
boost::optional<Matrix&> H12_ = H2 ? boost::optional<Matrix&>(H12) : boost::optional<Matrix&>(); "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
boost::optional<Matrix&> H22_ = H2 ? boost::optional<Matrix&>(H22) : boost::optional<Matrix&>(); ar& BOOST_SERIALIZATION_NVP(measuredBearing_);
ar& BOOST_SERIALIZATION_NVP(measuredRange_);
}
}; // BearingRangeFactor
Rot y1 = pose.bearing(point, H11_, H12_); } // namespace gtsam
Vector e1 = Rot::Logmap(measuredBearing_.between(y1));
double y2 = pose.range(point, H21_, H22_);
Vector e2 = (Vector(1) << y2 - measuredRange_).finished();
if (H1) *H1 = gtsam::stack(2, &H11, &H21);
if (H2) *H2 = gtsam::stack(2, &H12, &H22);
return concatVectors(2, &e1, &e2);
}
/** return the measured */
const std::pair<Rot, double> measured() const {
return std::make_pair(measuredBearing_, measuredRange_);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measuredBearing_);
ar & BOOST_SERIALIZATION_NVP(measuredRange_);
}
}; // BearingRangeFactor
} // namespace gtsam