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

Generalized bearing and bearingrange factors to allow for different variable types

release/4.3a0
Alex Cunningham 2011-10-07 14:46:43 +00:00
parent c6e2547dad
commit 39658269d4
3 changed files with 26 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/slam/BearingFactor.h
View File

@ -16,7 +16,6 @@
#pragma once #pragma once
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam { namespace gtsam {
@ -28,18 +27,21 @@ namespace gtsam {
class BearingFactor: public NonlinearFactor2<VALUES, POSEKEY, POINTKEY> { class BearingFactor: public NonlinearFactor2<VALUES, POSEKEY, POINTKEY> {
private: private:
Rot2 z_; /** measurement */ typedef typename POSEKEY::Value Pose;
typedef typename POSEKEY::Value::Rotation Rot;
typedef typename POINTKEY::Value Point;
typedef BearingFactor<VALUES, POSEKEY, POINTKEY> This; typedef BearingFactor<VALUES, POSEKEY, POINTKEY> This;
typedef NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base; typedef NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base;
Rot z_; /** measurement */
public: public:
/** default constructor for serialization/testing only */ /** default constructor for serialization/testing only */
BearingFactor() {} BearingFactor() {}
/** primary constructor */ /** primary constructor */
BearingFactor(const POSEKEY& i, const POINTKEY& j, const Rot2& z, BearingFactor(const POSEKEY& i, const POINTKEY& j, const Rot& z,
const SharedNoiseModel& model) : const SharedNoiseModel& model) :
Base(model, i, j), z_(z) { Base(model, i, j), z_(z) {
} }
@ -47,10 +49,10 @@ namespace gtsam {
virtual ~BearingFactor() {} virtual ~BearingFactor() {}
/** h(x)-z -> between(z,h(x)) for Rot2 manifold */ /** h(x)-z -> between(z,h(x)) for Rot2 manifold */
Vector evaluateError(const Pose2& pose, const Point2& point, Vector evaluateError(const Pose& pose, const Point& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Rot2 hx = pose.bearing(point, H1, H2); Rot2 hx = pose.bearing(point, H1, H2);
return Rot2::Logmap(z_.between(hx)); return Rot::Logmap(z_.between(hx));
} }
/** return the measured */ /** return the measured */

25
gtsam/slam/BearingRangeFactor.h
View File

@ -19,8 +19,7 @@
#pragma once #pragma once
#include <gtsam/slam/BearingFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/slam/RangeFactor.h>
namespace gtsam { namespace gtsam {
@ -31,25 +30,29 @@ namespace gtsam {
class BearingRangeFactor: public NonlinearFactor2<VALUES, POSEKEY, POINTKEY> { class BearingRangeFactor: public NonlinearFactor2<VALUES, POSEKEY, POINTKEY> {
private: private:
// the measurement typedef typename POSEKEY::Value Pose;
Rot2 bearing_; typedef typename POSEKEY::Value::Rotation Rot;
double range_; typedef typename POINTKEY::Value Point;
typedef BearingRangeFactor<VALUES, POSEKEY, POINTKEY> This; typedef BearingRangeFactor<VALUES, POSEKEY, POINTKEY> This;
typedef NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base; typedef NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base;
// the measurement
Rot bearing_;
double range_;
public: public:
BearingRangeFactor() {} /* Default constructor */ BearingRangeFactor() {} /* Default constructor */
BearingRangeFactor(const POSEKEY& i, const POINTKEY& j, const Rot2& bearing, const double range, BearingRangeFactor(const POSEKEY& i, const POINTKEY& j, const Rot& bearing, const double range,
const SharedNoiseModel& model) : const SharedNoiseModel& model) :
Base(model, i, j), bearing_(bearing), range_(range) { Base(model, i, j), bearing_(bearing), range_(range) {
} }
virtual ~BearingRangeFactor() {} virtual ~BearingRangeFactor() {}
/** h(x)-z -> between(z,h(x)) for Rot2 manifold */ /** h(x)-z -> between(z,h(x)) for Rot manifold */
Vector evaluateError(const Pose2& pose, const Point2& point, Vector evaluateError(const Pose& pose, const Point& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Matrix H11, H21, H12, H22; Matrix H11, H21, H12, H22;
boost::optional<Matrix&> H11_ = H1 ? boost::optional<Matrix&>(H11) : boost::optional<Matrix&>(); boost::optional<Matrix&> H11_ = H1 ? boost::optional<Matrix&>(H11) : boost::optional<Matrix&>();
@ -57,8 +60,8 @@ namespace gtsam {
boost::optional<Matrix&> H12_ = H2 ? boost::optional<Matrix&>(H12) : boost::optional<Matrix&>(); boost::optional<Matrix&> H12_ = H2 ? boost::optional<Matrix&>(H12) : boost::optional<Matrix&>();
boost::optional<Matrix&> H22_ = H2 ? boost::optional<Matrix&>(H22) : boost::optional<Matrix&>(); boost::optional<Matrix&> H22_ = H2 ? boost::optional<Matrix&>(H22) : boost::optional<Matrix&>();
Rot2 y1 = pose.bearing(point, H11_, H12_); Rot y1 = pose.bearing(point, H11_, H12_);
Vector e1 = Rot2::Logmap(bearing_.between(y1)); Vector e1 = Rot::Logmap(bearing_.between(y1));
double y2 = pose.range(point, H21_, H22_); double y2 = pose.range(point, H21_, H22_);
Vector e2 = Vector_(1, y2 - range_); Vector e2 = Vector_(1, y2 - range_);
@ -69,7 +72,7 @@ namespace gtsam {
} }
/** return the measured */ /** return the measured */
inline const std::pair<Rot2, double> measured() const { inline const std::pair<Rot, double> measured() const {
return std::make_pair(bearing_, range_); return std::make_pair(bearing_, range_);
} }

7
gtsam/slam/planarSLAM.h
View File

@ -13,15 +13,18 @@
* @file planarSLAM.h * @file planarSLAM.h
* @brief: bearing/range measurements in 2D plane * @brief: bearing/range measurements in 2D plane
* @authors Frank Dellaert * @authors Frank Dellaert
**/ */
#pragma once #pragma once
#include <gtsam/slam/BearingRangeFactor.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/TupleValues.h> #include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearEquality.h> #include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/slam/BearingFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h> #include <gtsam/nonlinear/NonlinearOptimizer.h>