From b51079f3399b7411a7f27783d536ced46ca78424 Mon Sep 17 00:00:00 2001 From: Sylvain Joyeux Date: Thu, 12 Jan 2017 08:09:04 -0200 Subject: [PATCH 1/5] fix leaking coTransform when the zone/north flag is modified --- src/UTMConverter.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/UTMConverter.cpp b/src/UTMConverter.cpp index f88e515..f9ecd5f 100644 --- a/src/UTMConverter.cpp +++ b/src/UTMConverter.cpp @@ -10,6 +10,7 @@ UTMConverter::UTMConverter() this->utm_zone = 32; this->utm_north = true; this->origin = base::Position::Zero(); + this->coTransform = NULL; createCoTransform(); } @@ -22,12 +23,13 @@ void UTMConverter::createCoTransform() oTargetSRS.SetWellKnownGeogCS("WGS84"); oTargetSRS.SetUTM(this->utm_zone, this->utm_north); - coTransform = OGRCreateCoordinateTransformation(&oSourceSRS, &oTargetSRS); - - if (coTransform == NULL) - { + OGRCoordinateTransformation* newTransform = + OGRCreateCoordinateTransformation(&oSourceSRS, &oTargetSRS); + if (newTransform == NULL) throw runtime_error("Failed to initialize CoordinateTransform"); - } + + delete coTransform; + coTransform = newTransform; } void UTMConverter::setUtmZone(int zone) From 12cb6bdfae77356d683c933f1e7236ec72a2f642 Mon Sep 17 00:00:00 2001 From: Sylvain Joyeux Date: Thu, 12 Jan 2017 08:11:02 -0200 Subject: [PATCH 2/5] use proper capitalization for UTM --- src/UTMConverter.cpp | 8 ++++---- src/UTMConverter.hpp | 16 ++++++++++++---- test/test_UTMConverter.cpp | 5 ++--- 3 files changed, 18 insertions(+), 11 deletions(-) diff --git a/src/UTMConverter.cpp b/src/UTMConverter.cpp index f9ecd5f..a7eb505 100644 --- a/src/UTMConverter.cpp +++ b/src/UTMConverter.cpp @@ -32,24 +32,24 @@ void UTMConverter::createCoTransform() coTransform = newTransform; } -void UTMConverter::setUtmZone(int zone) +void UTMConverter::setUTMZone(int zone) { this->utm_zone = zone; createCoTransform(); } -void UTMConverter::setUtmNorth(bool north) +void UTMConverter::setUTMNorth(bool north) { this->utm_north = north; createCoTransform(); } -int UTMConverter::getUtmZone() +int UTMConverter::getUTMZone() { return this->utm_zone; } -bool UTMConverter::getUtmNorth() +bool UTMConverter::getUTMNorth() { return this->utm_north; } diff --git a/src/UTMConverter.hpp b/src/UTMConverter.hpp index 0d801c2..ea408d9 100644 --- a/src/UTMConverter.hpp +++ b/src/UTMConverter.hpp @@ -21,13 +21,21 @@ namespace gps_base public: UTMConverter(); - void setUtmZone(int zone); - void setUtmNorth(bool north); - int getUtmZone(); - bool getUtmNorth(); + /** Sets the UTM zone + */ + void setUTMZone(int zone); void setOrigin(base::Position origin); base::Position getOrigin(); + /** Sets the UTM zone + */ + void setUTMNorth(bool north); + + /** Get the UTM zone */ + int getUTMZone(); + + /** Get whether we're north or south of the equator */ + bool getUTMNorth(); bool convertSolutionToRBS(const gps_base::Solution &solution, base::samples::RigidBodyState &position); }; diff --git a/test/test_UTMConverter.cpp b/test/test_UTMConverter.cpp index 619148c..ede7c09 100644 --- a/test/test_UTMConverter.cpp +++ b/test/test_UTMConverter.cpp @@ -14,12 +14,11 @@ BOOST_AUTO_TEST_CASE(it_should_output_utm_position) gps_base::Solution solution; base::samples::RigidBodyState pos; - converter.setUtmZone(24); - converter.setUtmNorth(false); - solution.latitude = -13.057361; solution.longitude = -38.649902; solution.altitude = 0.0; + converter.setUTMZone(24); + converter.setUTMNorth(false); solution.positionType = gps_base::AUTONOMOUS; solution.deviationLatitude = 0.2; solution.deviationLongitude = 0.33; From 606c5eb46524f4f5155db59ab63b5acf75d0305e Mon Sep 17 00:00:00 2001 From: Sylvain Joyeux Date: Thu, 12 Jan 2017 08:19:30 -0200 Subject: [PATCH 3/5] use initializers in the constructor --- src/UTMConverter.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/UTMConverter.cpp b/src/UTMConverter.cpp index a7eb505..2703471 100644 --- a/src/UTMConverter.cpp +++ b/src/UTMConverter.cpp @@ -6,11 +6,11 @@ using namespace std; using namespace gps_base; UTMConverter::UTMConverter() + : utm_zone(32) + , utm_north(true) + , origin(base::Position::Zero()) + , coTransform(NULL) { - this->utm_zone = 32; - this->utm_north = true; - this->origin = base::Position::Zero(); - this->coTransform = NULL; createCoTransform(); } From a735b402db7c8644c6332a98909ad7d74c2111db Mon Sep 17 00:00:00 2001 From: Sylvain Joyeux Date: Thu, 12 Jan 2017 08:30:49 -0200 Subject: [PATCH 4/5] cleanly separate the UTM coordinates from the Rock convention's NWU coordinates This changes method names, and also changes the behaviour of the convert methods in case the solution is a NO_SOLUTION. RBS can cleanly represent NO_SOLUTION, so simply return a RBS in this case. The caller can still manually check for NO_SOLUTION if he wants a different behaviour. --- src/UTMConverter.cpp | 62 ++++++++++++--------- src/UTMConverter.hpp | 30 ++++++++-- test/test_UTMConverter.cpp | 110 +++++++++++++++++++++++++++++-------- 3 files changed, 150 insertions(+), 52 deletions(-) diff --git a/src/UTMConverter.cpp b/src/UTMConverter.cpp index 2703471..d762fce 100644 --- a/src/UTMConverter.cpp +++ b/src/UTMConverter.cpp @@ -44,47 +44,59 @@ void UTMConverter::setUTMNorth(bool north) createCoTransform(); } -int UTMConverter::getUTMZone() +int UTMConverter::getUTMZone() const { return this->utm_zone; } -bool UTMConverter::getUTMNorth() +bool UTMConverter::getUTMNorth() const { return this->utm_north; } -base::Position UTMConverter::getOrigin() +base::Position UTMConverter::getNWUOrigin() const { return this->origin; } -void UTMConverter::setOrigin(base::Position origin) +void UTMConverter::setNWUOrigin(base::Position origin) { this->origin = origin; } -bool UTMConverter::convertSolutionToRBS(const gps_base::Solution &solution, base::samples::RigidBodyState &position) +base::samples::RigidBodyState UTMConverter::convertToUTM(const gps_base::Solution &solution) const { + base::samples::RigidBodyState position; + position.time = solution.time; + + if (solution.positionType == gps_base::NO_SOLUTION) + return position; + // if there is a valid reading, then write it to position readings port - if (solution.positionType != gps_base::NO_SOLUTION) - { - double la = solution.latitude; - double lo = solution.longitude; - double alt = solution.altitude; - - coTransform->Transform(1, &lo, &la, &alt); - - position.time = solution.time; - position.position.x() = lo - origin.x(); - position.position.y() = la - origin.y(); - position.position.z() = alt - origin.z(); - position.cov_position(0, 0) = solution.deviationLongitude * solution.deviationLongitude; - position.cov_position(1, 1) = solution.deviationLatitude * solution.deviationLatitude; - position.cov_position(2, 2) = solution.deviationAltitude * solution.deviationAltitude; - - return true; - } - - return false; + double northing = solution.latitude; + double easting = solution.longitude; + double altitude = solution.altitude; + + coTransform->Transform(1, &easting, &northing, &altitude); + + position.time = solution.time; + position.position.x() = easting; + position.position.y() = northing; + position.position.z() = altitude; + position.cov_position(0, 0) = solution.deviationLongitude * solution.deviationLongitude; + position.cov_position(1, 1) = solution.deviationLatitude * solution.deviationLatitude; + position.cov_position(2, 2) = solution.deviationAltitude * solution.deviationAltitude; + return position; +} + +base::samples::RigidBodyState UTMConverter::convertToNWU(const gps_base::Solution &solution) const +{ + base::samples::RigidBodyState position = convertToUTM(solution); + double easting = position.position.x(); + double northing = position.position.y(); + position.position.x() = northing; + position.position.y() = 1000000 - easting; + position.position -= origin; + std::swap(position.cov_position(0, 0), position.cov_position(1, 1)); + return position; } diff --git a/src/UTMConverter.hpp b/src/UTMConverter.hpp index ea408d9..5b9ced6 100644 --- a/src/UTMConverter.hpp +++ b/src/UTMConverter.hpp @@ -24,19 +24,39 @@ namespace gps_base /** Sets the UTM zone */ void setUTMZone(int zone); - void setOrigin(base::Position origin); - base::Position getOrigin(); /** Sets the UTM zone */ void setUTMNorth(bool north); /** Get the UTM zone */ - int getUTMZone(); + int getUTMZone() const; /** Get whether we're north or south of the equator */ - bool getUTMNorth(); - bool convertSolutionToRBS(const gps_base::Solution &solution, base::samples::RigidBodyState &position); + bool getUTMNorth() const; + + /** Set a position that will be removed from the computed UTM + * solution + */ + void setNWUOrigin(base::Position origin); + + /** Returns the current origin position in UTM coordinates + */ + base::Position getNWUOrigin() const; + + /** Convert a GPS solution into UTM coordinates + * + * The returned RBS will has all its fields invalidated (only the + * timestamp updated) if there is no solution + */ + base::samples::RigidBodyState convertToUTM(const gps_base::Solution &solution) const; + + /** Convert a GPS solution into NWU coordinates (Rock's convention) + * + * The returned RBS will has all its fields invalidated (only the + * timestamp updated) if there is no solution + */ + base::samples::RigidBodyState convertToNWU(const gps_base::Solution &solution) const; }; } // end namespace gps_base diff --git a/test/test_UTMConverter.cpp b/test/test_UTMConverter.cpp index ede7c09..c64c7b8 100644 --- a/test/test_UTMConverter.cpp +++ b/test/test_UTMConverter.cpp @@ -8,60 +8,126 @@ BOOST_AUTO_TEST_CASE(it_should_not_crash_when_instantiated) gps_base::UTMConverter converter; } -BOOST_AUTO_TEST_CASE(it_should_output_utm_position) +BOOST_AUTO_TEST_CASE(it_converts_the_position) { - gps_base::UTMConverter converter; gps_base::Solution solution; + solution.positionType = gps_base::AUTONOMOUS; + solution.latitude = -13.057361; + solution.longitude = -38.649902; + solution.altitude = 2.0; + + gps_base::UTMConverter converter; + converter.setUTMZone(24); + converter.setUTMNorth(false); + base::samples::RigidBodyState pos; + pos = converter.convertToUTM(solution); + BOOST_REQUIRE_CLOSE(pos.position.x(), 537956.57943, 0.0001); + BOOST_REQUIRE_CLOSE(pos.position.y(), 8556494.7274, 0.0001); + BOOST_REQUIRE_CLOSE(pos.position.z(), 2, 0.0001); + pos = converter.convertToNWU(solution); + BOOST_REQUIRE_CLOSE(pos.position.x(), 8556494.7274, 0.0001); + BOOST_REQUIRE_CLOSE(pos.position.y(), 1000000 - 537956.57943, 0.0001); + BOOST_REQUIRE_CLOSE(pos.position.z(), 2, 0.0001); +} +BOOST_AUTO_TEST_CASE(it_applies_the_origin_only_in_NWU_coordinates) +{ + gps_base::Solution solution; + solution.positionType = gps_base::AUTONOMOUS; solution.latitude = -13.057361; solution.longitude = -38.649902; - solution.altitude = 0.0; + solution.altitude = 2.0; + + gps_base::UTMConverter converter; converter.setUTMZone(24); converter.setUTMNorth(false); + converter.setNWUOrigin(base::Position(8550000, 400000, 0)); + + base::samples::RigidBodyState pos; + pos = converter.convertToUTM(solution); + BOOST_REQUIRE_CLOSE(pos.position.x(), 537956.57943, 0.0001); + BOOST_REQUIRE_CLOSE(pos.position.y(), 8556494.7274, 0.0001); + BOOST_REQUIRE_CLOSE(pos.position.z(), 2, 0.0001); + pos = converter.convertToNWU(solution); + BOOST_REQUIRE_CLOSE(pos.position.x(), 6494.7274, 0.0001); + BOOST_REQUIRE_CLOSE(pos.position.y(), 62043.420570012648, 0.0001); + BOOST_REQUIRE_CLOSE(pos.position.z(), 2, 0.0001); +} + +BOOST_AUTO_TEST_CASE(it_propagates_the_deviations) +{ + gps_base::Solution solution; solution.positionType = gps_base::AUTONOMOUS; solution.deviationLatitude = 0.2; solution.deviationLongitude = 0.33; solution.deviationAltitude = 0.27; - BOOST_REQUIRE_EQUAL(true, converter.convertSolutionToRBS(solution, pos)); + gps_base::UTMConverter converter; - BOOST_REQUIRE_CLOSE(pos.position.x(), 537956.57943, 0.0001); - BOOST_REQUIRE_CLOSE(pos.position.y(), 8556494.7274, 0.0001); - BOOST_REQUIRE_CLOSE(pos.position.z(), 0, 0.0001); + base::samples::RigidBodyState pos; + pos = converter.convertToUTM(solution); BOOST_REQUIRE_CLOSE(0.33*0.33, pos.cov_position(0, 0), 0.0001); - BOOST_REQUIRE_CLOSE(0.2*0.2, pos.cov_position(1, 1), 0.0001); + BOOST_REQUIRE_CLOSE(0.2*0.2, pos.cov_position(1, 1), 0.0001); + BOOST_REQUIRE_CLOSE(0.27*0.27, pos.cov_position(2, 2), 0.0001); + pos = converter.convertToNWU(solution); + BOOST_REQUIRE_CLOSE(0.2*0.2, pos.cov_position(0, 0), 0.0001); + BOOST_REQUIRE_CLOSE(0.33*0.33, pos.cov_position(1, 1), 0.0001); BOOST_REQUIRE_CLOSE(0.27*0.27, pos.cov_position(2, 2), 0.0001); - - return; } -BOOST_AUTO_TEST_CASE(covariance_should_be_unset_if_deviation_is_unset) +BOOST_AUTO_TEST_CASE(it_propagates_the_timestamp) { - gps_base::UTMConverter converter; gps_base::Solution solution; + solution.time = base::Time::fromSeconds(100); + solution.positionType = gps_base::AUTONOMOUS; + + gps_base::UTMConverter converter; + base::samples::RigidBodyState pos; + pos = converter.convertToUTM(solution); + BOOST_REQUIRE_EQUAL(base::Time::fromSeconds(100), pos.time); + pos = converter.convertToNWU(solution); + BOOST_REQUIRE_EQUAL(base::Time::fromSeconds(100), pos.time); +} +BOOST_AUTO_TEST_CASE(covariance_should_be_unset_if_deviation_is_unset) +{ + gps_base::Solution solution; solution.positionType = gps_base::AUTONOMOUS; solution.deviationLatitude = base::unset(); solution.deviationLongitude = base::unset(); solution.deviationAltitude = base::unset(); - BOOST_REQUIRE_EQUAL(true, converter.convertSolutionToRBS(solution, pos)); - BOOST_REQUIRE_EQUAL(true, base::isUnset(pos.cov_position(0, 0))); - BOOST_REQUIRE_EQUAL(true, base::isUnset(pos.cov_position(1, 1))); - BOOST_REQUIRE_EQUAL(true, base::isUnset(pos.cov_position(2, 2))); + gps_base::UTMConverter converter; + + base::samples::RigidBodyState pos; + pos = converter.convertToUTM(solution); + BOOST_REQUIRE(base::isUnset(pos.cov_position(0, 0))); + BOOST_REQUIRE(base::isUnset(pos.cov_position(1, 1))); + BOOST_REQUIRE(base::isUnset(pos.cov_position(2, 2))); + + pos = converter.convertToNWU(solution); + BOOST_REQUIRE(base::isUnset(pos.cov_position(0, 0))); + BOOST_REQUIRE(base::isUnset(pos.cov_position(1, 1))); + BOOST_REQUIRE(base::isUnset(pos.cov_position(2, 2))); } -BOOST_AUTO_TEST_CASE(it_should_return_false_if_solution_is_unset) +BOOST_AUTO_TEST_CASE(the_convertion_methods_return_an_invalid_RBS_with_timestamp_set_if_there_is_no_solution) { - gps_base::UTMConverter converter; gps_base::Solution solution; - base::samples::RigidBodyState pos; - + solution.time = base::Time::now(); solution.positionType = gps_base::NO_SOLUTION; - BOOST_REQUIRE_EQUAL(false, converter.convertSolutionToRBS(solution, pos)); - return; + gps_base::UTMConverter converter; + + base::samples::RigidBodyState pos; + pos = converter.convertToUTM(solution); + BOOST_REQUIRE_EQUAL(solution.time, pos.time); + BOOST_REQUIRE(!pos.hasValidPosition()); + + pos = converter.convertToNWU(solution); + BOOST_REQUIRE_EQUAL(solution.time, pos.time); + BOOST_REQUIRE(!pos.hasValidPosition()); } From 26065d072a7db8a76cd3c4ff7c3b2748817a3dad Mon Sep 17 00:00:00 2001 From: Sylvain Joyeux Date: Thu, 12 Jan 2017 09:44:25 -0200 Subject: [PATCH 5/5] overload convertToNWU to allow for computing the UTM once and then convert it to NWU --- src/UTMConverter.cpp | 7 ++++++- src/UTMConverter.hpp | 4 ++++ 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/src/UTMConverter.cpp b/src/UTMConverter.cpp index d762fce..58f94c7 100644 --- a/src/UTMConverter.cpp +++ b/src/UTMConverter.cpp @@ -91,7 +91,12 @@ base::samples::RigidBodyState UTMConverter::convertToUTM(const gps_base::Solutio base::samples::RigidBodyState UTMConverter::convertToNWU(const gps_base::Solution &solution) const { - base::samples::RigidBodyState position = convertToUTM(solution); + return convertToNWU(convertToUTM(solution)); +} + +base::samples::RigidBodyState UTMConverter::convertToNWU(const base::samples::RigidBodyState &utm) const +{ + base::samples::RigidBodyState position = utm; double easting = position.position.x(); double northing = position.position.y(); position.position.x() = northing; diff --git a/src/UTMConverter.hpp b/src/UTMConverter.hpp index 5b9ced6..ac41484 100644 --- a/src/UTMConverter.hpp +++ b/src/UTMConverter.hpp @@ -57,6 +57,10 @@ namespace gps_base * timestamp updated) if there is no solution */ base::samples::RigidBodyState convertToNWU(const gps_base::Solution &solution) const; + + /** Convert a UTM-converted GPS solution into NWU coordinates (Rock's convention) + */ + base::samples::RigidBodyState convertToNWU(const base::samples::RigidBodyState &solution) const; }; } // end namespace gps_base