diff --git a/examples/PlanarSLAMExample_easy.cpp b/examples/PlanarSLAMExample_easy.cpp index b71b80876..d3aecb4b6 100644 --- a/examples/PlanarSLAMExample_easy.cpp +++ b/examples/PlanarSLAMExample_easy.cpp @@ -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, initialEstimate); + planarSLAM::Values result = optimize(graph, initialEstimate); result.print("final result"); return 0; diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp index 398fec627..bb7f60516 100644 --- a/examples/Pose2SLAMExample_advanced.cpp +++ b/examples/Pose2SLAMExample_advanced.cpp @@ -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 initial(new Values); + shared_ptr 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 */ diff --git a/examples/Pose2SLAMExample_easy.cpp b/examples/Pose2SLAMExample_easy.cpp index 402a3d30f..43a648398 100644 --- a/examples/Pose2SLAMExample_easy.cpp +++ b/examples/Pose2SLAMExample_easy.cpp @@ -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, initial); + pose2SLAM::Values result = optimize(graph, initial); result.print("final result"); diff --git a/gtsam.h b/gtsam.h index 97dea0b31..5f11d88ad 100644 --- a/gtsam.h +++ b/gtsam.h @@ -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 { diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index 5740d893f..50fae36a5 100644 --- a/gtsam/slam/planarSLAM.h +++ b/gtsam/slam/planarSLAM.h @@ -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 PoseKey; + /// Typedef for a PoseKey with Pose2 data and 'x' symbol + typedef TypedSymbol PoseKey; - /// Typedef for a PointKey with Point2 data and 'l' symbol - typedef TypedSymbol PointKey; - /** - * List of typedefs for factors + /// Typedef for a PointKey with Point2 data and 'l' symbol + typedef TypedSymbol PointKey; + /** + * List of typedefs for factors + */ + /// A hard constraint for PoseKeys to enforce particular values + typedef NonlinearEquality Constraint; + /// A prior factor to bias the value of a PoseKey + typedef PriorFactor Prior; + /// A factor between two PoseKeys set with a Pose2 + typedef BetweenFactor Odometry; + /// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2) + typedef BearingFactor Bearing; + /// A factor between a PoseKey and a PointKey to express distance between them (set with a double) + typedef RangeFactor Range; + /// A factor between a PoseKey and a PointKey to express difference in rotation and location + typedef BearingRangeFactor 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 Constraint; - /// A prior factor to bias the value of a PoseKey - typedef PriorFactor Prior; - /// A factor between two PoseKeys set with a Pose2 - typedef BetweenFactor Odometry; - /// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2) - typedef BearingFactor Bearing; - /// A factor between a PoseKey and a PointKey to express distance between them (set with a double) - typedef RangeFactor Range; - /// A factor between a PoseKey and a PointKey to express difference in rotation and location - typedef BearingRangeFactor 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 Optimizer; - return *Optimizer::optimizeLM(*this, initialEstimate, - NonlinearOptimizationParameters::LAMBDA); - } - }; + /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph + Graph(const NonlinearFactorGraph& graph); - /// Optimizer - typedef NonlinearOptimizer 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 Optimizer; + return *Optimizer::optimizeLM(*this, initialEstimate, + NonlinearOptimizationParameters::LAMBDA); + } + }; + +/// Optimizer +typedef NonlinearOptimizer Optimizer; } // planarSLAM diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index 89a3b7ec6..23e087be6 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -36,6 +36,25 @@ namespace pose2SLAM { /// Keys with Pose2 and symbol 'x' typedef TypedSymbol 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) diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h index 4798bae84..a9ab58d3c 100644 --- a/gtsam/slam/simulated2D.h +++ b/gtsam/slam/simulated2D.h @@ -32,6 +32,60 @@ namespace simulated2D { typedef TypedSymbol PoseKey; typedef TypedSymbol 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 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; diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h index 51a2903fe..0c5533291 100644 --- a/gtsam/slam/simulated2DOriented.h +++ b/gtsam/slam/simulated2DOriented.h @@ -31,6 +31,30 @@ namespace simulated2DOriented { typedef TypedSymbol PointKey; typedef TypedSymbol 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 diff --git a/gtsam/slam/tests/testPlanarSLAM.cpp b/gtsam/slam/tests/testPlanarSLAM.cpp index a74f79d49..56ac71923 100644 --- a/gtsam/slam/tests/testPlanarSLAM.cpp +++ b/gtsam/slam/tests/testPlanarSLAM.cpp @@ -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); diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp index 9151dd6af..a8828a83a 100644 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ b/gtsam/slam/tests/testPose2SLAM.cpp @@ -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); diff --git a/gtsam/slam/tests/testSimulated2D.cpp b/gtsam/slam/tests/testSimulated2D.cpp index 30c5561ce..3513927a7 100644 --- a/gtsam/slam/tests/testSimulated2D.cpp +++ b/gtsam/slam/tests/testSimulated2D.cpp @@ -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)); diff --git a/gtsam/slam/tests/testSimulated2DOriented.cpp b/gtsam/slam/tests/testSimulated2DOriented.cpp index f5ebfa177..64844cb2a 100644 --- a/gtsam/slam/tests/testSimulated2DOriented.cpp +++ b/gtsam/slam/tests/testSimulated2DOriented.cpp @@ -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 lf = factor.linearize(config, *config.orderingArbitrary()); diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 0ad4e03ae..2316c5652 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -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(PoseKey(2))); EXPECT(equalsObj(PointKey(3))); - EXPECT(equalsObj(values)); + EXPECT(equalsObj(values)); EXPECT(equalsObj(prior)); EXPECT(equalsObj(bearing)); EXPECT(equalsObj(bearingRange)); @@ -539,7 +539,7 @@ TEST (Serialization, planar_system) { // xml EXPECT(equalsXML(PoseKey(2))); EXPECT(equalsXML(PointKey(3))); - EXPECT(equalsXML(values)); + EXPECT(equalsXML(values)); EXPECT(equalsXML(prior)); EXPECT(equalsXML(bearing)); EXPECT(equalsXML(bearingRange));