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

found there are two implementations of bearing and range in gtsam. removed the redundant one.

release/4.3a0
Kai Ni 2010-03-31 23:32:39 +00:00
parent c1baca1b29
commit db533c565b
4 changed files with 23 additions and 133 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
cpp/BearingFactor.h
View File

@ -13,37 +13,11 @@
namespace gtsam {
/**
* Calculate bearing to a landmark
* @param pose 2D pose of robot
* @param point 2D location of landmark
* @return 2D rotation \in SO(2)
*/
Rot2 bearing(const Pose2& pose, const Point2& point) {
Point2 d = transform_to(pose, point);
return relativeBearing(d);
}
/**
* Calculate bearing and optional derivative(s)
*/
Rot2 bearing(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (!H1 && !H2) return bearing(pose, point);
Point2 d = transform_to(pose, point);
Matrix D_result_d;
Rot2 result = relativeBearing(d, D_result_d);
if (H1) *H1 = D_result_d * Dtransform_to1(pose, point);
if (H2) *H2 = D_result_d * Dtransform_to2(pose, point);
return result;
}
/**
* Non-linear factor for a constraint derived from a 2D measurement,
* i.e. the main building block for visual SLAM.
* Binary factor for a bearing measurement
*/
template<class Config, class PoseKey, class PointKey>
class BearingFactor: public NonlinearFactor2<Config, PoseKey, Pose2,
PointKey, Point2> {
PointKey, Point2> {
private:
Rot2 z_; /** measurement */
@ -53,9 +27,9 @@ namespace gtsam {
public:
BearingFactor(); /* Default constructor */
BearingFactor(const Rot2& z, double sigma, const PoseKey& i,
const PointKey& j) :
Base(sigma, i, j), z_(z) {
BearingFactor(const PoseKey& i, const PointKey& j, const Rot2& z,
const SharedGaussian& model) :
Base(model, i, j), z_(z) {
}
/** h(x)-z -> between(z,h(x)) for Rot2 manifold */
@ -64,6 +38,11 @@ namespace gtsam {
Rot2 hx = bearing(pose, point, H1, H2);
return logmap(between(z_, hx));
}
/** return the measured */
inline const Rot2 measured() const {
return z_;
}
}; // BearingFactor
} // namespace gtsam

2
cpp/Makefile.am
View File

@ -212,7 +212,7 @@ testPose2SLAM_SOURCES = testPose2SLAM.cpp
testPose2SLAM_LDADD = libgtsam.la
# 2D SLAM using Bearing and Range
headers +=
headers += BearingFactor.h RangeFactor.h
sources += planarSLAM.cpp
check_PROGRAMS += testPlanarSLAM
testPlanarSLAM_SOURCES = testPlanarSLAM.cpp

40
cpp/RangeFactor.h
View File

@ -12,37 +12,11 @@
namespace gtsam {
/**
* Calculate range to a landmark
* @param pose 2D pose of robot
* @param point 2D location of landmark
* @return range (double)
*/
double range(const Pose2& pose, const Point2& point) {
Point2 d = transform_to(pose, point);
return d.norm();
}
/**
* Calculate range and optional derivative(s)
*/
double range(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
if (!H1 && !H2) return range(pose, point);
Point2 d = transform_to(pose, point);
double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
Matrix D_result_d = Matrix_(1, 2, x / n, y / n);
if (H1) *H1 = D_result_d * Dtransform_to1(pose, point);
if (H2) *H2 = D_result_d * Dtransform_to2(pose, point);
return n;
}
/**
* Non-linear factor for a constraint derived from a 2D measurement,
* i.e. the main building block for visual SLAM.
* Binary factor for a range measurement
*/
template<class Config, class PoseKey, class PointKey>
class RangeFactor: public NonlinearFactor2<Config, PoseKey, Pose2, PointKey,
Point2> {
Point2> {
private:
double z_; /** measurement */
@ -53,8 +27,9 @@ namespace gtsam {
RangeFactor(); /* Default constructor */
RangeFactor(double z, double sigma, const PoseKey& i, const PointKey& j) :
Base(sigma, i, j), z_(z) {
RangeFactor(const PoseKey& i, const PointKey& j, double z,
const SharedGaussian& model) :
Base(model, i, j), z_(z) {
}
/** h(x)-z */
@ -63,6 +38,11 @@ namespace gtsam {
double hx = gtsam::range(pose, point, H1, H2);
return Vector_(1, hx - z_);
}
/** return the measured */
inline const double measured() const {
return z_;
}
}; // RangeFactor
} // namespace gtsam

73
cpp/planarSLAM.h
View File

@ -6,10 +6,8 @@
#pragma once
#include "Key.h"
#include "Pose2.h"
#include "Point2.h"
#include "NonlinearFactor.h"
#include "BearingFactor.h"
#include "RangeFactor.h"
#include "TupleConfig.h"
#include "NonlinearEquality.h"
#include "PriorFactor.h"
@ -20,73 +18,6 @@
// We use gtsam namespace for generally useful factors
namespace gtsam {
/**
* Binary factor for a bearing measurement
*/
template<class Config, class PoseKey, class PointKey>
class BearingFactor: public NonlinearFactor2<Config, PoseKey, Pose2,
PointKey, Point2> {
private:
Rot2 z_; /** measurement */
typedef NonlinearFactor2<Config, PoseKey, Pose2, PointKey, Point2> Base;
public:
BearingFactor(); /* Default constructor */
BearingFactor(const PoseKey& i, const PointKey& j, const Rot2& z,
const SharedGaussian& model) :
Base(model, i, j), z_(z) {
}
/** h(x)-z -> between(z,h(x)) for Rot2 manifold */
Vector evaluateError(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Rot2 hx = bearing(pose, point, H1, H2);
return logmap(between(z_, hx));
}
/** return the measured */
inline const Rot2 measured() const {
return z_;
}
}; // BearingFactor
/**
* Binary factor for a range measurement
*/
template<class Config, class PoseKey, class PointKey>
class RangeFactor: public NonlinearFactor2<Config, PoseKey, Pose2, PointKey,
Point2> {
private:
double z_; /** measurement */
typedef NonlinearFactor2<Config, PoseKey, Pose2, PointKey, Point2> Base;
public:
RangeFactor(); /* Default constructor */
RangeFactor(const PoseKey& i, const PointKey& j, double z,
const SharedGaussian& model) :
Base(model, i, j), z_(z) {
}
/** h(x)-z */
Vector evaluateError(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
double hx = gtsam::range(pose, point, H1, H2);
return Vector_(1, hx - z_);
}
/** return the measured */
inline const double measured() const {
return z_;
}
}; // RangeFactor
// Use planarSLAM namespace for specific SLAM instance
namespace planarSLAM {