HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

478
navigations/geographic_info/geodesy/tests/test_utm.cpp Executable file
View File

@@ -0,0 +1,478 @@
/* $Id: 47db7b5025f4ca800961335f30ef67b18cfe69ca $ */
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2011 Jack O'Quin
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the author nor other contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include <sstream>
#include <gtest/gtest.h>
#include <angles/angles.h>
#include "geodesy/utm.h"
///////////////////////////////////////////////////////////////
// Utility functions
///////////////////////////////////////////////////////////////
// check that two UTM points are near each other
void check_utm_near(const geodesy::UTMPoint &pt1,
const geodesy::UTMPoint &pt2,
double abs_err)
{
EXPECT_NEAR(pt1.easting, pt2.easting, abs_err);
EXPECT_NEAR(pt1.northing, pt2.northing, abs_err);
EXPECT_NEAR(pt1.altitude, pt2.altitude, abs_err);
EXPECT_EQ(pt1.zone, pt2.zone);
EXPECT_EQ(pt1.band, pt2.band);
}
///////////////////////////////////////////////////////////////
// Test cases
///////////////////////////////////////////////////////////////
// Test null constructor
TEST(UTMPoint, nullConstructor)
{
geodesy::UTMPoint pt;
EXPECT_EQ(pt.easting, 0.0);
EXPECT_EQ(pt.northing, 0.0);
EXPECT_TRUE(geodesy::is2D(pt));
EXPECT_EQ(pt.zone, 0);
EXPECT_EQ(pt.band, ' ');
EXPECT_FALSE(geodesy::isValid(pt));
}
// Test 2D constructor
TEST(UTMPoint, flatConstructor)
{
double e = 1000.0;
double n = 2400.0;
uint8_t z = 14;
char b = 'R';
geodesy::UTMPoint pt(e, n, z, b);
EXPECT_TRUE(geodesy::is2D(pt));
EXPECT_TRUE(geodesy::isValid(pt));
EXPECT_EQ(pt.easting, e);
EXPECT_EQ(pt.northing, n);
EXPECT_EQ(pt.zone, z);
EXPECT_EQ(pt.band, b);
}
// Test 3D constructor
TEST(UTMPoint, hasAltitude)
{
double e = 1000.0;
double n = 2400.0;
double a = 200.0;
uint8_t z = 14;
char b = 'R';
geodesy::UTMPoint pt(e, n, a, z, b);
EXPECT_FALSE(geodesy::is2D(pt));
EXPECT_TRUE(geodesy::isValid(pt));
EXPECT_EQ(pt.easting, e);
EXPECT_EQ(pt.northing, n);
EXPECT_EQ(pt.zone, z);
EXPECT_EQ(pt.band, b);
}
// Test copy constructor
TEST(UTMPoint, copyConstructor)
{
double e = 1000.0;
double n = 2400.0;
double a = 200.0;
uint8_t z = 14;
char b = 'R';
geodesy::UTMPoint pt1(e, n, a, z, b);
geodesy::UTMPoint pt2(pt1);
EXPECT_FALSE(geodesy::is2D(pt2));
EXPECT_TRUE(geodesy::isValid(pt2));
EXPECT_EQ(pt2.easting, e);
EXPECT_EQ(pt2.northing, n);
EXPECT_EQ(pt2.zone, z);
EXPECT_EQ(pt2.band, b);
}
// Test UTM point constructor from WGS 84
TEST(UTMPoint, fromLatLong)
{
// University of Texas, Austin, Pickle Research Campus
double lat = 30.385315;
double lon = -97.728524;
double alt = 209.0;
geographic_msgs::GeoPoint ll;
ll.latitude = lat;
ll.longitude = lon;
ll.altitude = alt;
// create UTM from point
geodesy::UTMPoint pt(ll);
double e = 622159.34;
double n = 3362168.30;
uint8_t z = 14;
char b = 'R';
double abs_err = 0.01;
EXPECT_FALSE(geodesy::is2D(pt));
EXPECT_TRUE(geodesy::isValid(pt));
EXPECT_NEAR(pt.easting, e, abs_err);
EXPECT_NEAR(pt.northing, n, abs_err);
EXPECT_NEAR(pt.altitude, alt, abs_err);
EXPECT_EQ(pt.zone, z);
EXPECT_EQ(pt.band, b);
}
// Test zone numbers
TEST(UTMPoint, testZones)
{
geodesy::UTMPoint pt;
pt.band = 'X'; // supply a valid band letter
pt.zone = 0;
EXPECT_FALSE(geodesy::isValid(pt));
pt.zone = 61;
EXPECT_FALSE(geodesy::isValid(pt));
pt.zone = 255;
EXPECT_FALSE(geodesy::isValid(pt));
// these should all work
for (uint8_t b = 1; b <= 60; ++b)
{
pt.zone = b;
EXPECT_TRUE(geodesy::isValid(pt));
}
}
// Test band letters
TEST(UTMPoint, testBands)
{
geodesy::UTMPoint pt;
pt.zone = 14; // supply a valid zone number
EXPECT_FALSE(geodesy::isValid(pt));
pt.band = '9';
EXPECT_FALSE(geodesy::isValid(pt));
pt.band = ';';
EXPECT_FALSE(geodesy::isValid(pt));
pt.band = 'I';
EXPECT_FALSE(geodesy::isValid(pt));
pt.band = 'O';
EXPECT_FALSE(geodesy::isValid(pt));
pt.band = 'Y';
EXPECT_FALSE(geodesy::isValid(pt));
pt.band = 'r';
EXPECT_FALSE(geodesy::isValid(pt));
// this should work
pt.band = 'X';
EXPECT_TRUE(geodesy::isValid(pt));
}
// Test null pose constructor
TEST(UTMPose, nullConstructor)
{
geodesy::UTMPose pose;
EXPECT_TRUE(geodesy::is2D(pose));
EXPECT_FALSE(geodesy::isValid(pose));
}
// Test pose constructor from point and quaternion
TEST(UTMPose, pointQuaternionConstructor)
{
double e = 1000.0;
double n = 2400.0;
double a = 200.0;
uint8_t z = 14;
char b = 'R';
geodesy::UTMPoint pt(e, n, a, z, b);
geometry_msgs::Quaternion q; // identity quaternion
q.x = 1.0;
q.y = 0.0;
q.z = 0.0;
q.w = 0.0;
geodesy::UTMPose pose(pt, q);
EXPECT_FALSE(geodesy::is2D(pose));
EXPECT_TRUE(geodesy::isValid(pose));
EXPECT_EQ(pose.position.easting, pt.easting);
EXPECT_EQ(pose.position.northing, pt.northing);
EXPECT_EQ(pose.position.altitude, pt.altitude);
EXPECT_EQ(pose.position.zone, pt.zone);
EXPECT_EQ(pose.position.band, pt.band);
EXPECT_EQ(pose.orientation.w, q.w);
EXPECT_EQ(pose.orientation.x, q.x);
EXPECT_EQ(pose.orientation.y, q.y);
EXPECT_EQ(pose.orientation.z, q.z);
}
// Test pose quaternion validation
TEST(UTMPose, quaternionValidation)
{
double e = 1000.0;
double n = 2400.0;
double a = 200.0;
uint8_t z = 14;
char b = 'R';
geodesy::UTMPoint pt(e, n, a, z, b);
// identity quaternion
geometry_msgs::Quaternion q;
q.x = 1.0;
q.y = 0.0;
q.z = 0.0;
q.w = 0.0;
geodesy::UTMPose pose1(pt, q);
EXPECT_TRUE(geodesy::isValid(pose1));
// valid quaternion
q.x = 0.7071;
q.y = 0.0;
q.z = 0.0;
q.w = 0.7071;
geodesy::UTMPose pose2(pt, q);
EXPECT_TRUE(geodesy::isValid(pose2));
// quaternion not normalized
q.x = 0.8071;
q.y = 0.0;
q.z = 0.0;
q.w = 0.8071;
geodesy::UTMPose pose3(pt, q);
EXPECT_FALSE(geodesy::isValid(pose3));
// zero quaternion (not normalized)
q.x = 0.0;
q.y = 0.0;
q.z = 0.0;
q.w = 0.0;
geodesy::UTMPose pose4(pt, q);
EXPECT_FALSE(geodesy::isValid(pose4));
}
// Test UTM pose conversion
TEST(UTMConvert, fromUtmPoseToLatLongAndBack)
{
double e = 500000.0; // central meridian of each zone
double n = 1000.0;
double alt = 100.0;
char b = 'N';
// try every possible zone of longitude
for (uint8_t z = 1; z <= 60; ++z)
{
for (unsigned int heading = 0; heading < 360; heading++)
{
geodesy::UTMPose ps1(geodesy::UTMPoint(e, n, alt, z, b),
tf::createQuaternionMsgFromYaw(angles::from_degrees(heading)));
geographic_msgs::GeoPose ll;
convert(ps1, ll);
geodesy::UTMPose ps2;
convert(ll, ps2);
EXPECT_TRUE(geodesy::isValid(ps1));
EXPECT_TRUE(geodesy::isValid(ps2));
check_utm_near(ps1.position, ps2.position, 0.000001);
EXPECT_DOUBLE_EQ(tf::getYaw(ps1.orientation), tf::getYaw(ps2.orientation));
}
}
}
// Test conversion from UTM to WGS 84 and back
TEST(UTMConvert, fromUtmToLatLongAndBack)
{
double e = 500000.0; // central meridian of each zone
double n = 1000.0;
double alt = 100.0;
char b = 'N';
// try every possible zone of longitude
for (uint8_t z = 1; z <= 60; ++z)
{
geodesy::UTMPoint pt1(e, n, alt, z, b);
geographic_msgs::GeoPoint ll;
convert(pt1, ll);
geodesy::UTMPoint pt2;
convert(ll, pt2);
EXPECT_TRUE(geodesy::isValid(pt1));
EXPECT_TRUE(geodesy::isValid(pt2));
check_utm_near(pt1, pt2, 0.000001);
}
}
// Test conversion from WGS 84 to UTM and back
TEST(UTMConvert, fromLatLongToUtmAndBack)
{
double alt = 100.0;
// Try every possible degree of latitude and longitude. Avoid the
// international date line. Even though the converted longitude is
// close, it may end up less than -180 and hence inValid().
for (double lon = -179.5; lon < 180.0; lon += 1.0)
{
for (double lat = -80.0; lat <= 84.0; lat += 1.0)
{
geographic_msgs::GeoPoint pt1(geodesy::toMsg(lat, lon, alt));
EXPECT_TRUE(geodesy::isValid(pt1));
geodesy::UTMPoint utm(pt1);
EXPECT_TRUE(geodesy::isValid(utm));
geographic_msgs::GeoPoint pt2(geodesy::toMsg(utm));
EXPECT_TRUE(geodesy::isValid(pt2));
EXPECT_NEAR(pt1.latitude, pt2.latitude, 0.0000001);
EXPECT_NEAR(pt1.longitude, pt2.longitude, 0.0000012);
EXPECT_NEAR(pt1.altitude, pt2.altitude, 0.000001);
}
}
}
// Test conversion from WGS 84 to UTM and back at international date line
TEST(UTMConvert, internationalDateLine)
{
double alt = 100.0;
double lon = -180.0;
for (double lat = -80.0; lat <= 84.0; lat += 1.0)
{
geographic_msgs::GeoPoint pt1(geodesy::toMsg(lat, lon, alt));
EXPECT_TRUE(geodesy::isValid(pt1));
geodesy::UTMPoint utm;
geodesy::fromMsg(pt1, utm);
EXPECT_TRUE(geodesy::isValid(utm));
geographic_msgs::GeoPoint pt2(geodesy::toMsg(utm));
EXPECT_TRUE(geodesy::isValid(pt2));
EXPECT_NEAR(pt1.latitude, pt2.latitude, 0.0000001);
EXPECT_NEAR(pt1.altitude, pt2.altitude, 0.000001);
if (pt2.longitude - pt1.longitude > 359.0)
{
// pt2 seems to be slightly across the international date
// line from pt2, so de-normalize it
pt2.longitude -= 360.0;
}
EXPECT_NEAR(pt1.longitude, pt2.longitude, 0.0000012);
}
}
// Test point output stream operator
TEST(OStream, point)
{
geodesy::UTMPoint pt1;
std::ostringstream out1;
out1 << pt1;
std::string expected("(0, 0, nan [0 ])");
EXPECT_EQ(out1.str(), expected);
geodesy::UTMPoint pt2(622159.338, 3362168.303, 209, 14, 'R');
std::ostringstream out2;
out2 << pt2;
expected = "(622159.338, 3362168.303, 209 [14R])";
EXPECT_EQ(out2.str(), expected);
}
// Test pose output stream operator
TEST(OStream, pose)
{
geodesy::UTMPoint pt(1000.0, 2400.0, 200.0, 14, 'R');
geometry_msgs::Quaternion q;
q.w = 1.0;
q.x = 0.0;
q.y = 0.0;
q.z = 0.0;
geodesy::UTMPose pose(pt, q);
std::ostringstream out;
out << pose;
std::string expected("(1000, 2400, 200 [14R]), ([0, 0, 0], 1)");
EXPECT_EQ(out.str(), expected);
}
TEST(ForceUTMZone, point)
{
geographic_msgs::GeoPoint zone2, zone3;
zone2.latitude=24.02;
zone2 = geodesy::toMsg(24.02, 5.999);
zone3 = geodesy::toMsg(24.02, 6.001);
geodesy::UTMPoint pt2, pt3, pt4;
geodesy::fromMsg(zone2, pt2);
geodesy::fromMsg(zone3, pt3);
EXPECT_FALSE(geodesy::sameGridZone(pt2, pt3) );
double diffx = pt2.easting - pt3.easting;
double diffy = pt2.northing - pt3.northing;
double distance = std::sqrt(diffx*diffx + diffy*diffy);
//Now force the pt3 into pt2's grid zone
geodesy::fromMsg(zone3, pt4, true, pt2.band, pt2.zone);
diffx = pt2.easting - pt4.easting;
diffy = pt2.northing - pt4.northing;
double distance2 = std::sqrt(diffx*diffx + diffy*diffy);
ROS_INFO("Prev Distance %f, Actual Distance %f", distance, distance2);
EXPECT_LT(distance2, distance);
}
// Run all the tests that were declared with TEST()
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
// run the tests in this thread
return RUN_ALL_TESTS();
}