Skip to content

Commit

Permalink
Merge pull request #3 from rock-drivers/cleanup_relationship_between_…
Browse files Browse the repository at this point in the history 
…utm_and_nwu

Cleanup relationship between UTM and NWU
  • Loading branch information
@doudou
doudou authored Jan 12, 2017
2 parents 8c9d16b + 26065d0 commit 8483210
Show file tree
Hide file tree
Showing 3 changed files with 184 additions and 68 deletions.
89 changes: 54 additions & 35 deletions src/UTMConverter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,10 +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();
createCoTransform();
}

Expand All @@ -22,67 +23,85 @@ 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)
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() 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
{
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;
position.position.y() = 1000000 - easting;
position.position -= origin;
std::swap(position.cov_position(0, 0), position.cov_position(1, 1));
return position;
}
48 changes: 40 additions & 8 deletions src/UTMConverter.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,14 +21,46 @@ namespace gps_base
public:
UTMConverter();

void setUtmZone(int zone);
void setUtmNorth(bool north);
int getUtmZone();
bool getUtmNorth();
void setOrigin(base::Position origin);
base::Position getOrigin();

bool convertSolutionToRBS(const gps_base::Solution &solution, base::samples::RigidBodyState &position);
/** Sets the UTM zone
*/
void setUTMZone(int zone);

/** Sets the UTM zone
*/
void setUTMNorth(bool north);

/** Get the UTM zone */
int getUTMZone() const;

/** Get whether we're north or south of the equator */
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;

/** 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
Expand Down
115 changes: 90 additions & 25 deletions test/test_UTMConverter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,61 +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;
base::samples::RigidBodyState pos;
solution.positionType = gps_base::AUTONOMOUS;
solution.latitude = -13.057361;
solution.longitude = -38.649902;
solution.altitude = 2.0;

converter.setUtmZone(24);
converter.setUtmNorth(false);
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<float>();
solution.deviationLongitude = base::unset<float>();
solution.deviationAltitude = base::unset<float>();

BOOST_REQUIRE_EQUAL(true, converter.convertSolutionToRBS(solution, pos));
BOOST_REQUIRE_EQUAL(true, base::isUnset<float>(pos.cov_position(0, 0)));
BOOST_REQUIRE_EQUAL(true, base::isUnset<float>(pos.cov_position(1, 1)));
BOOST_REQUIRE_EQUAL(true, base::isUnset<float>(pos.cov_position(2, 2)));
gps_base::UTMConverter converter;

base::samples::RigidBodyState pos;
pos = converter.convertToUTM(solution);
BOOST_REQUIRE(base::isUnset<float>(pos.cov_position(0, 0)));
BOOST_REQUIRE(base::isUnset<float>(pos.cov_position(1, 1)));
BOOST_REQUIRE(base::isUnset<float>(pos.cov_position(2, 2)));

pos = converter.convertToNWU(solution);
BOOST_REQUIRE(base::isUnset<float>(pos.cov_position(0, 0)));
BOOST_REQUIRE(base::isUnset<float>(pos.cov_position(1, 1)));
BOOST_REQUIRE(base::isUnset<float>(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());
}

0 comments on commit 8483210

Please sign in to comment.