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
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
@ -28,18 +27,21 @@ namespace gtsam {
class BearingFactor: public NonlinearFactor2<VALUES, POSEKEY, POINTKEY> {
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 NonlinearFactor2<VALUES, POSEKEY, POINTKEY> Base;
Rot z_; /** measurement */
public:
/** default constructor for serialization/testing only */
BearingFactor() {}
/** 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) :
Base(model, i, j), z_(z) {
}
@ -47,10 +49,10 @@ namespace gtsam {
virtual ~BearingFactor() {}
/** 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 {
Rot2 hx = pose.bearing(point, H1, H2);
return Rot2::Logmap(z_.between(hx));
return Rot::Logmap(z_.between(hx));
}
/** return the measured */

25
gtsam/slam/BearingRangeFactor.h
View File

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

7
gtsam/slam/planarSLAM.h
View File

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