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

put back Value in slam sub-namespaces for the wrapper to interface with MATLAB. Cannot solve the return shared_ptr problem in NonlinearOptimizationParameters::newDecreaseThresholds

release/4.3a0
Duy-Nguyen Ta 2012-02-05 22:34:35 +00:00
parent b849fbec16
commit f4515d7b30
14 changed files with 209 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/PlanarSLAMExample_easy.cpp
View File

@ -76,7 +76,7 @@ int main(int argc, char** argv) {
graph.print("full graph");
// initialize to noisy points
Values initialEstimate;
planarSLAM::Values initialEstimate;
initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
@ -86,7 +86,7 @@ int main(int argc, char** argv) {
initialEstimate.print("initial estimate");
// optimize using Levenberg-Marquardt optimization with an ordering from colamd
Values result = optimize<Graph>(graph, initialEstimate);
planarSLAM::Values result = optimize<Graph>(graph, initialEstimate);
result.print("final result");
return 0;

4
examples/Pose2SLAMExample_advanced.cpp
View File

@ -57,7 +57,7 @@ int main(int argc, char** argv) {
/* 3. Create the data structure to hold the initial estimate to the solution
* initialize to noisy points */
shared_ptr<Values> initial(new Values);
shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values);
initial->insert(x1, Pose2(0.5, 0.0, 0.2));
initial->insert(x2, Pose2(2.3, 0.1,-0.2));
initial->insert(x3, Pose2(4.1, 0.1, 0.1));
@ -72,7 +72,7 @@ int main(int argc, char** argv) {
Optimizer optimizer(graph, initial, ordering, params);
Optimizer optimizer_result = optimizer.levenbergMarquardt();
Values result = *optimizer_result.values();
pose2SLAM::Values result = *optimizer_result.values();
result.print("final result");
/* Get covariances */

4
examples/Pose2SLAMExample_easy.cpp
View File

@ -55,7 +55,7 @@ int main(int argc, char** argv) {
/* 3. Create the data structure to hold the initial estinmate to the solution
* initialize to noisy points */
Values initial;
pose2SLAM::Values initial;
initial.insert(x1, Pose2(0.5, 0.0, 0.2));
initial.insert(x2, Pose2(2.3, 0.1,-0.2));
initial.insert(x3, Pose2(4.1, 0.1, 0.1));
@ -63,7 +63,7 @@ int main(int argc, char** argv) {
/* 4 Single Step Optimization
* optimize using Levenberg-Marquardt optimization with an ordering from colamd */
Values result = optimize<Graph>(graph, initial);
pose2SLAM::Values result = optimize<Graph>(graph, initial);
result.print("final result");

6
gtsam.h
View File

@ -389,6 +389,8 @@ class NonlinearOptimizationParameters {
NonlinearOptimizationParameters(double absDecrease, double relDecrease,
double sumError, int iIters, double lambda, double lambdaFactor);
void print(string s) const;
static gtsam::NonlinearOptimizationParameters* newDrecreaseThresholds(double absDecrease,
double relDecrease);
};
@ -427,14 +429,14 @@ class Graph {
void addRange(int poseKey, int pointKey, double range, const gtsam::SharedNoiseModel& noiseModel);
void addBearingRange(int poseKey, int pointKey, const gtsam::Rot2& bearing, double range,
const gtsam::SharedNoiseModel& noiseModel);
Values optimize(const Values& initialEstimate);
planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate);
};
class Odometry {
Odometry(int key1, int key2, const gtsam::Pose2& measured,
const gtsam::SharedNoiseModel& model);
void print(string s) const;
gtsam::GaussianFactor* linearize(const Values& center, const Ordering& ordering) const;
gtsam::GaussianFactor* linearize(const planarSLAM::Values& center, const gtsam::Ordering& ordering) const;
};
class Optimizer {

127
gtsam/slam/planarSLAM.h
View File

@ -31,70 +31,97 @@
// Use planarSLAM namespace for specific SLAM instance
namespace planarSLAM {
using namespace gtsam;
using namespace gtsam;
/// Typedef for a PoseKey with Pose2 data and 'x' symbol
typedef TypedSymbol<Pose2, 'x'> PoseKey;
/// Typedef for a PoseKey with Pose2 data and 'x' symbol
typedef TypedSymbol<Pose2, 'x'> PoseKey;
/// Typedef for a PointKey with Point2 data and 'l' symbol
typedef TypedSymbol<Point2, 'l'> PointKey;
/**
* List of typedefs for factors
/// Typedef for a PointKey with Point2 data and 'l' symbol
typedef TypedSymbol<Point2, 'l'> PointKey;
/**
* List of typedefs for factors
*/
/// A hard constraint for PoseKeys to enforce particular values
typedef NonlinearEquality<PoseKey> Constraint;
/// A prior factor to bias the value of a PoseKey
typedef PriorFactor<PoseKey> Prior;
/// A factor between two PoseKeys set with a Pose2
typedef BetweenFactor<PoseKey> Odometry;
/// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2)
typedef BearingFactor<PoseKey, PointKey> Bearing;
/// A factor between a PoseKey and a PointKey to express distance between them (set with a double)
typedef RangeFactor<PoseKey, PointKey> Range;
/// A factor between a PoseKey and a PointKey to express difference in rotation and location
typedef BearingRangeFactor<PoseKey, PointKey> BearingRange;
/** Values class, using specific PoseKeys and PointKeys
* Mainly as a convenience for MATLAB wrapper, which does not allow for identically named methods
*/
/// A hard constraint for PoseKeys to enforce particular values
typedef NonlinearEquality<PoseKey> Constraint;
/// A prior factor to bias the value of a PoseKey
typedef PriorFactor<PoseKey> Prior;
/// A factor between two PoseKeys set with a Pose2
typedef BetweenFactor<PoseKey> Odometry;
/// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2)
typedef BearingFactor<PoseKey, PointKey> Bearing;
/// A factor between a PoseKey and a PointKey to express distance between them (set with a double)
typedef RangeFactor<PoseKey, PointKey> Range;
/// A factor between a PoseKey and a PointKey to express difference in rotation and location
typedef BearingRangeFactor<PoseKey, PointKey> BearingRange;
struct Values: public gtsam::Values {
/// Creates a NonlinearFactorGraph with the Values type
struct Graph: public NonlinearFactorGraph {
/// Default constructor
Values() {}
/// Default constructor for a NonlinearFactorGraph
Graph(){}
/// Copy constructor
Values(const gtsam::Values& values) :
gtsam::Values(values) {
}
/// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
Graph(const NonlinearFactorGraph& graph);
/// get a pose
Pose2 pose(int key) const { return (*this)[PoseKey(key)]; }
/// Biases the value of PoseKey key with Pose2 p given a noise model
void addPrior(const PoseKey& key, const Pose2& pose, const SharedNoiseModel& noiseModel);
/// get a point
Point2 point(int key) const { return (*this)[PointKey(key)]; }
/// Creates a hard constraint to enforce Pose2 p for PoseKey poseKey's value
void addPoseConstraint(const PoseKey& poseKey, const Pose2& pose);
/// insert a pose
void insertPose(int key, const Pose2& pose) {insert(PoseKey(key), pose); }
/// Creates a factor with a Pose2 between PoseKeys poseKey and pointKey (poseKey.e. an odometry measurement)
void addOdometry(const PoseKey& poseKey, const PoseKey& pointKey, const Pose2& odometry,
const SharedNoiseModel& model);
/// insert a point
void insertPoint(int key, const Point2& point) {insert(PointKey(key), point); }
};
/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation
void addBearing(const PoseKey& poseKey, const PointKey& pointKey, const Rot2& bearing,
const SharedNoiseModel& model);
/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in location
void addRange(const PoseKey& poseKey, const PointKey& pointKey, double range,
const SharedNoiseModel& model);
/// Creates a NonlinearFactorGraph with the Values type
struct Graph: public NonlinearFactorGraph {
/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation and location
void addBearingRange(const PoseKey& poseKey, const PointKey& pointKey,
const Rot2& bearing, double range, const SharedNoiseModel& model);
/// Default constructor for a NonlinearFactorGraph
Graph(){}
/// Optimize
Values optimize(const Values& initialEstimate) {
typedef NonlinearOptimizer<Graph> Optimizer;
return *Optimizer::optimizeLM(*this, initialEstimate,
NonlinearOptimizationParameters::LAMBDA);
}
};
/// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
Graph(const NonlinearFactorGraph& graph);
/// Optimizer
typedef NonlinearOptimizer<Graph> Optimizer;
/// Biases the value of PoseKey key with Pose2 p given a noise model
void addPrior(const PoseKey& key, const Pose2& pose, const SharedNoiseModel& noiseModel);
/// Creates a hard constraint to enforce Pose2 p for PoseKey poseKey's value
void addPoseConstraint(const PoseKey& poseKey, const Pose2& pose);
/// Creates a factor with a Pose2 between PoseKeys poseKey and pointKey (poseKey.e. an odometry measurement)
void addOdometry(const PoseKey& poseKey, const PoseKey& pointKey, const Pose2& odometry,
const SharedNoiseModel& model);
/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation
void addBearing(const PoseKey& poseKey, const PointKey& pointKey, const Rot2& bearing,
const SharedNoiseModel& model);
/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in location
void addRange(const PoseKey& poseKey, const PointKey& pointKey, double range,
const SharedNoiseModel& model);
/// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation and location
void addBearingRange(const PoseKey& poseKey, const PointKey& pointKey,
const Rot2& bearing, double range, const SharedNoiseModel& model);
/// Optimize
Values optimize(const Values& initialEstimate) {
typedef NonlinearOptimizer<Graph> Optimizer;
return *Optimizer::optimizeLM(*this, initialEstimate,
NonlinearOptimizationParameters::LAMBDA);
}
};
/// Optimizer
typedef NonlinearOptimizer<Graph> Optimizer;
} // planarSLAM

19
gtsam/slam/pose2SLAM.h
View File

@ -36,6 +36,25 @@ namespace pose2SLAM {
/// Keys with Pose2 and symbol 'x'
typedef TypedSymbol<Pose2, 'x'> PoseKey;
/// Values class, inherited from Values, using PoseKeys, mainly used as a convenience for MATLAB wrapper
struct Values: public gtsam::Values {
/// Default constructor
Values() {}
/// Copy constructor
Values(const gtsam::Values& values) :
gtsam::Values(values) {
}
// Convenience for MATLAB wrapper, which does not allow for identically named methods
/// get a pose
Pose2 pose(int key) const { return (*this)[PoseKey(key)]; }
/// insert a pose
void insertPose(int key, const Pose2& pose) { insert(PoseKey(key), pose); }
};
/**
* Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)

54
gtsam/slam/simulated2D.h
View File

@ -32,6 +32,60 @@ namespace simulated2D {
typedef TypedSymbol<Point2, 'x'> PoseKey;
typedef TypedSymbol<Point2, 'l'> PointKey;
/**
* Custom Values class that holds poses and points, mainly used as a convenience for MATLAB wrapper
*/
class Values: public gtsam::Values {
private:
int nrPoses_, nrPoints_;
public:
typedef gtsam::Values Base; ///< base class
typedef boost::shared_ptr<Point2> sharedPoint; ///< shortcut to shared Point type
/// Constructor
Values() : nrPoses_(0), nrPoints_(0) {
}
/// Copy constructor
Values(const Base& base) :
Base(base), nrPoses_(0), nrPoints_(0) {
}
/// Insert a pose
void insertPose(const simulated2D::PoseKey& i, const Point2& p) {
insert(i, p);
nrPoses_++;
}
/// Insert a point
void insertPoint(const simulated2D::PointKey& j, const Point2& p) {
insert(j, p);
nrPoints_++;
}
/// Number of poses
int nrPoses() const {
return nrPoses_;
}
/// Number of points
int nrPoints() const {
return nrPoints_;
}
/// Return pose i
Point2 pose(const simulated2D::PoseKey& i) const {
return (*this)[i];
}
/// Return point j
Point2 point(const simulated2D::PointKey& j) const {
return (*this)[j];
}
};
/// Prior on a single pose
inline Point2 prior(const Point2& x) {
return x;

24
gtsam/slam/simulated2DOriented.h
View File

@ -31,6 +31,30 @@ namespace simulated2DOriented {
typedef TypedSymbol<Point2, 'l'> PointKey;
typedef TypedSymbol<Pose2, 'x'> PoseKey;
/// Specialized Values structure with syntactic sugar for
/// compatibility with matlab
class Values: public gtsam::Values {
int nrPoses_, nrPoints_;
public:
Values() : nrPoses_(0), nrPoints_(0) {}
void insertPose(const PoseKey& i, const Pose2& p) {
insert(i, p);
nrPoses_++;
}
void insertPoint(const PointKey& j, const Point2& p) {
insert(j, p);
nrPoints_++;
}
int nrPoses() const { return nrPoses_; }
int nrPoints() const { return nrPoints_; }
Pose2 pose(const PoseKey& i) const { return (*this)[i]; }
Point2 point(const PointKey& j) const { return (*this)[j]; }
};
//TODO:: point prior is not implemented right now
/// Prior on a single pose

8
gtsam/slam/tests/testPlanarSLAM.cpp
View File

@ -51,7 +51,7 @@ TEST( planarSLAM, BearingFactor )
planarSLAM::Bearing factor(2, 3, z, sigma);
// create config
Values c;
planarSLAM::Values c;
c.insert(PoseKey(2), x2);
c.insert(PointKey(3), l3);
@ -79,7 +79,7 @@ TEST( planarSLAM, RangeFactor )
planarSLAM::Range factor(2, 3, z, sigma);
// create config
Values c;
planarSLAM::Values c;
c.insert(PoseKey(2), x2);
c.insert(PointKey(3), l3);
@ -106,7 +106,7 @@ TEST( planarSLAM, BearingRangeFactor )
planarSLAM::BearingRange factor(2, 3, r, b, sigma2);
// create config
Values c;
planarSLAM::Values c;
c.insert(PoseKey(2), x2);
c.insert(PointKey(3), l3);
@ -139,7 +139,7 @@ TEST( planarSLAM, PoseConstraint_equals )
TEST( planarSLAM, constructor )
{
// create config
Values c;
planarSLAM::Values c;
c.insert(PoseKey(2), x2);
c.insert(PoseKey(3), x3);
c.insert(PointKey(3), l3);

24
gtsam/slam/tests/testPose2SLAM.cpp
View File

@ -345,13 +345,13 @@ using namespace pose2SLAM;
TEST(Pose2Values, pose2Circle )
{
// expected is 4 poses tangent to circle with radius 1m
Values expected;
pose2SLAM::Values expected;
expected.insert(pose2SLAM::PoseKey(0), Pose2( 1, 0, M_PI_2));
expected.insert(pose2SLAM::PoseKey(1), Pose2( 0, 1, - M_PI ));
expected.insert(pose2SLAM::PoseKey(2), Pose2(-1, 0, - M_PI_2));
expected.insert(pose2SLAM::PoseKey(3), Pose2( 0, -1, 0 ));
Values actual = pose2SLAM::circle(4,1.0);
pose2SLAM::Values actual = pose2SLAM::circle(4,1.0);
CHECK(assert_equal(expected,actual));
}
@ -359,21 +359,21 @@ TEST(Pose2Values, pose2Circle )
TEST(Pose2SLAM, expmap )
{
// expected is circle shifted to right
Values expected;
pose2SLAM::Values expected;
expected.insert(pose2SLAM::PoseKey(0), Pose2( 1.1, 0, M_PI_2));
expected.insert(pose2SLAM::PoseKey(1), Pose2( 0.1, 1, - M_PI ));
expected.insert(pose2SLAM::PoseKey(2), Pose2(-0.9, 0, - M_PI_2));
expected.insert(pose2SLAM::PoseKey(3), Pose2( 0.1, -1, 0 ));
// Note expmap coordinates are in local coordinates, so shifting to right requires thought !!!
Values circle(pose2SLAM::circle(4,1.0));
pose2SLAM::Values circle(pose2SLAM::circle(4,1.0));
Ordering ordering(*circle.orderingArbitrary());
VectorValues delta(circle.dims(ordering));
delta[ordering[pose2SLAM::PoseKey(0)]] = Vector_(3, 0.0,-0.1,0.0);
delta[ordering[pose2SLAM::PoseKey(1)]] = Vector_(3, -0.1,0.0,0.0);
delta[ordering[pose2SLAM::PoseKey(2)]] = Vector_(3, 0.0,0.1,0.0);
delta[ordering[pose2SLAM::PoseKey(3)]] = Vector_(3, 0.1,0.0,0.0);
Values actual = circle.retract(delta, ordering);
pose2SLAM::Values actual = circle.retract(delta, ordering);
CHECK(assert_equal(expected,actual));
}
@ -386,7 +386,7 @@ TEST( Pose2Prior, error )
{
// Choose a linearization point
Pose2 p1(1, 0, 0); // robot at (1,0)
Values x0;
pose2SLAM::Values x0;
x0.insert(pose2SLAM::PoseKey(1), p1);
// Create factor
@ -408,7 +408,7 @@ TEST( Pose2Prior, error )
VectorValues addition(VectorValues::Zero(x0.dims(ordering)));
addition[ordering["x1"]] = Vector_(3, 0.1, 0.0, 0.0);
VectorValues plus = delta + addition;
Values x1 = x0.retract(plus, ordering);
pose2SLAM::Values x1 = x0.retract(plus, ordering);
Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 !
CHECK(assert_equal(error_at_plus,factor.whitenedError(x1)));
CHECK(assert_equal(error_at_plus,linear->error_vector(plus)));
@ -430,7 +430,7 @@ LieVector hprior(const Pose2& p1) {
TEST( Pose2Prior, linearize )
{
// Choose a linearization point at ground truth
Values x0;
pose2SLAM::Values x0;
x0.insert(pose2SLAM::PoseKey(1),priorVal);
// Actual linearization
@ -450,7 +450,7 @@ TEST( Pose2Factor, error )
// Choose a linearization point
Pose2 p1; // robot at origin
Pose2 p2(1, 0, 0); // robot at (1,0)
Values x0;
pose2SLAM::Values x0;
x0.insert(pose2SLAM::PoseKey(1), p1);
x0.insert(pose2SLAM::PoseKey(2), p2);
@ -474,7 +474,7 @@ TEST( Pose2Factor, error )
// Check error after increasing p2
VectorValues plus = delta;
plus[ordering["x2"]] = Vector_(3, 0.1, 0.0, 0.0);
Values x1 = x0.retract(plus, ordering);
pose2SLAM::Values x1 = x0.retract(plus, ordering);
Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 !
CHECK(assert_equal(error_at_plus,factor.whitenedError(x1)));
CHECK(assert_equal(error_at_plus,linear->error_vector(plus)));
@ -491,7 +491,7 @@ TEST( Pose2Factor, rhs )
// Choose a linearization point
Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2)
Pose2 p2(-1,4.1,M_PI); // robot at (-1,4.1) looking at negative (ground truth is at -1,4)
Values x0;
pose2SLAM::Values x0;
x0.insert(pose2SLAM::PoseKey(1),p1);
x0.insert(pose2SLAM::PoseKey(2),p2);
@ -521,7 +521,7 @@ TEST( Pose2Factor, linearize )
// Choose a linearization point at ground truth
Pose2 p1(1,2,M_PI_2);
Pose2 p2(-1,4,M_PI);
Values x0;
pose2SLAM::Values x0;
x0.insert(pose2SLAM::PoseKey(1),p1);
x0.insert(pose2SLAM::PoseKey(2),p2);

2
gtsam/slam/tests/testSimulated2D.cpp
View File

@ -30,7 +30,7 @@ using namespace simulated2D;
/* ************************************************************************* */
TEST( simulated2D, Simulated2DValues )
{
Values actual;
simulated2D::Values actual;
actual.insert(simulated2D::PoseKey(1),Point2(1,1));
actual.insert(simulated2D::PointKey(2),Point2(2,2));
EXPECT(assert_equal(actual,actual,1e-9));

2
gtsam/slam/tests/testSimulated2DOriented.cpp
View File

@ -57,7 +57,7 @@ TEST( simulated2DOriented, constructor )
SharedDiagonal model(Vector_(3, 1., 1., 1.));
simulated2DOriented::Odometry factor(measurement, model, simulated2DOriented::PoseKey(1), simulated2DOriented::PoseKey(2));
Values config;
simulated2DOriented::Values config;
config.insert(simulated2DOriented::PoseKey(1), Pose2(1., 0., 0.2));
config.insert(simulated2DOriented::PoseKey(2), Pose2(2., 0., 0.1));
boost::shared_ptr<GaussianFactor> lf = factor.linearize(config, *config.orderingArbitrary());

8
tests/testNonlinearISAM.cpp
View File

@ -30,8 +30,8 @@ TEST(testNonlinearISAM, markov_chain ) {
Graph start_factors;
start_factors.addPoseConstraint(key, cur_pose);
Values init;
Values expected;
planarSLAM::Values init;
planarSLAM::Values expected;
init.insert(key, cur_pose);
expected.insert(key, cur_pose);
isam.update(start_factors, init);
@ -43,7 +43,7 @@ TEST(testNonlinearISAM, markov_chain ) {
Graph new_factors;
PoseKey key1(i-1), key2(i);
new_factors.addOdometry(key1, key2, z, model);
Values new_init;
planarSLAM::Values new_init;
// perform a check on changing orderings
if (i == 5) {
@ -67,7 +67,7 @@ TEST(testNonlinearISAM, markov_chain ) {
}
// verify values - all but the last one should be very close
Values actual = isam.estimate();
planarSLAM::Values actual = isam.estimate();
for (size_t i=0; i<nrPoses; ++i) {
PoseKey cur_key(i);
EXPECT(assert_equal(expected[cur_key], actual[cur_key], tol));

6
tests/testSerialization.cpp
View File

@ -501,7 +501,7 @@ BOOST_CLASS_EXPORT_GUID(planarSLAM::Constraint, "gtsam::planarSLAM::Constraint"
/* ************************************************************************* */
TEST (Serialization, planar_system) {
using namespace planarSLAM;
Values values;
planarSLAM::Values values;
values.insert(PointKey(3), Point2(1.0, 2.0));
values.insert(PoseKey(4), Pose2(1.0, 2.0, 0.3));
@ -527,7 +527,7 @@ TEST (Serialization, planar_system) {
// text
EXPECT(equalsObj<PoseKey>(PoseKey(2)));
EXPECT(equalsObj<PointKey>(PointKey(3)));
EXPECT(equalsObj<Values>(values));
EXPECT(equalsObj<planarSLAM::Values>(values));
EXPECT(equalsObj<Prior>(prior));
EXPECT(equalsObj<Bearing>(bearing));
EXPECT(equalsObj<BearingRange>(bearingRange));
@ -539,7 +539,7 @@ TEST (Serialization, planar_system) {
// xml
EXPECT(equalsXML<PoseKey>(PoseKey(2)));
EXPECT(equalsXML<PointKey>(PointKey(3)));
EXPECT(equalsXML<Values>(values));
EXPECT(equalsXML<planarSLAM::Values>(values));
EXPECT(equalsXML<Prior>(prior));
EXPECT(equalsXML<Bearing>(bearing));
EXPECT(equalsXML<BearingRange>(bearingRange));