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

made Between Factor more flexible to work with measurement that's not of its own Key::Value type. default behavior unchanged

release/4.3a0
Chris Beall 2011-03-17 04:36:27 +00:00
parent ad8b87c3c2
commit 3e143c1f24
2 changed files with 11 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
gtsam/slam/BetweenFactor.h
View File

@ -26,20 +26,16 @@ namespace gtsam {
/** /**
* A class for a measurement predicted by "between(config[key1],config[key2])" * A class for a measurement predicted by "between(config[key1],config[key2])"
* T is the Lie group type, Values where the T's are gotten from * KEY1::Value is the Lie Group type
* * T is the measurement type, by default the same
* FIXME: This should only need one key type, as we can't have different types
*/ */
template<class VALUES, class KEY1, class KEY2 = KEY1> template<class VALUES, class KEY1, class T = typename KEY1::Value>
class BetweenFactor: public NonlinearFactor2<VALUES, KEY1, KEY2> { class BetweenFactor: public NonlinearFactor2<VALUES, KEY1, KEY1> {
public:
typedef typename KEY1::Value T;
private: private:
typedef BetweenFactor<VALUES, KEY1, KEY2> This; typedef BetweenFactor<VALUES, KEY1, T> This;
typedef NonlinearFactor2<VALUES, KEY1, KEY2> Base; typedef NonlinearFactor2<VALUES, KEY1, KEY1> Base;
T measured_; /** The measurement */ T measured_; /** The measurement */
@ -52,7 +48,7 @@ namespace gtsam {
BetweenFactor() {} BetweenFactor() {}
/** Constructor */ /** Constructor */
BetweenFactor(const KEY1& key1, const KEY2& key2, const T& measured, BetweenFactor(const KEY1& key1, const KEY1& key2, const T& measured,
const SharedGaussian& model) : const SharedGaussian& model) :
Base(model, key1, key2), measured_(measured) { Base(model, key1, key2), measured_(measured) {
} }
@ -76,8 +72,9 @@ namespace gtsam {
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */
/** vector of errors */ /** vector of errors */
Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 = Vector evaluateError(const typename KEY1::Value& p1, const typename KEY1::Value& p2,
boost::none, boost::optional<Matrix&> H2 = boost::none) const { boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const {
T hx = p1.between(p2, H1, H2); // h(x) T hx = p1.between(p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x)) // manifold equivalent of h(x)-z -> log(z,h(x))
return measured_.logmap(hx); return measured_.logmap(hx);

2
gtsam/slam/StereoFactor.h
View File

@ -86,7 +86,7 @@ public:
} }
/** h(x)-z */ /** h(x)-z */
Vector evaluateError(const CamPose& pose, const Point3& point, Vector evaluateError(const Pose3& pose, const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
StereoCamera stereoCam(pose, *K_); StereoCamera stereoCam(pose, *K_);
return (stereoCam.project(point, H1, H2) - z_).vector(); return (stereoCam.project(point, H1, H2) - z_).vector();