/*!
* Converts Earth Centered Earth Fixed coordinates into Latitude Longitude
* Altitude coordinates.
*
*/
AREXPORT ArLLACoords
ArECEFCoords::ECEF2LLA(void)
{
// ECEF2LLA - convert earth-centered earth-fixed (ECEF)
// cartesian coordinates to latitude, longitude,
// and altitude
//
// USAGE:
// [lat,lon,alt] = ecef2lla(x,y,z)
//
// lat = geodetic latitude (radians)
// lon = longitude (radians)
// alt = height above WGS84 ellipsoid (m)
// x = ECEF X-coordinate (m)
// y = ECEF Y-coordinate (m)
// z = ECEF Z-coordinate (m)
double x = myX;//(*this)(0);
double y = myY;//(*this)(1);
double z = myZ;//(*this)(2);
//
// Notes: (1) This function assumes the WGS84 model.
// (2) Latitude is customary geodetic (not geocentric).
// (3) Inputs may be scalars, vectors, or matrices of the same
// size and shape. Outputs will have that same size and shape.
// (4) Tested but no warranty; use at your own risk.
// (5) Michael Kleder, April 2006
// function [lat,lon,alt] = ecef2lla(x,y,z)
// WGS84 ellipsoid constants:
const double a = ArWGS84::getA();
const double e = ArWGS84::getE();
const double b = ArWGS84::getB();
const double ep = ArWGS84::getEP();
// Calculations.
double p = sqrt(x*x + y*y);
double th = atan2(a*z, b*p);
double lon = atan2(y, x);
double lat = atan2((z + ep*ep*b*pow(sin(th), 3)),
(p - e*e*a*pow(cos(th), 3)));
double N = a / sqrt(1 - e*e*pow(sin(lat), 2));
double alt = p / cos(lat) - N;
// return lon in range [0,2*pi)
if(lon < -M_PI)
lon += 2*M_PI;
lat *= 180.0/M_PI;
lon *= 180.0/M_PI;
// correct for numerical instability in altitude near exact poles:
// (after this correction, error is about 2 millimeters, which isabout
// the same as the numerical precision of the overall function)
if(fabs(x) < 1 && fabs(y) < 1)
alt = -b;
return ArLLACoords(lat, lon, alt);
}
ArPose GPSMapTools::getCurrentPosFromGPS()
{
ArLLACoords gpsCoords(myGPS->getLatitude(), myGPS->getLongitude(), myGPS->getAltitude());
double x, y, z;
x = y = z = NAN;
// XXX need to have previously saved origin in myMapGPSCoords; either recreate
// here each time, or reset whenever a new map is either created or loaded
// elsewhere, and in ctor if there is a map in arnl.
if(!myHaveMapGPSCoords)
{
// get origin from ArMap and setup MAp->GPS coords translator
if(myMap->hasOriginLatLongAlt())
{
ArPose p = myMap->getOriginLatLong();
myOrigin = ArLLACoords(p.getX(), p.getY(), myMap->getOriginAltitude());
resetMapCoords(myOrigin);
}
}
myMapGPSCoords.convertLLA2MapCoords(gpsCoords, x, y, z);
return ArPose(x, y);
}
void GPSMapTools::setOrigin()
{
if(!checkGPS("setting map origin")) return;
if(!checkMap("setting map origin")) return;
if(myRobot)
{
ArLog::log(ArLog::Normal, "GPSMapTools: Resetting robot odometric position to 0,0,0.");
myRobot->moveTo(0,0,0);
myRobot->com(ArCommands::SIM_RESET);
}
if(myMap->hasOriginLatLongAlt())
{
ArLog::log(ArLog::Terse, "GPSMapTools: setting map origin: Warning: Map already has an origin point, it will be replaced by current position.");
}
myOrigin = ArLLACoords(myGPS->getLatitude(), myGPS->getLongitude(), myGPS->getAltitude());
resetMapCoords(myOrigin);
myMap->lock();
myMap->setOriginLatLongAlt(true, ArPose(myGPS->getLatitude(), myGPS->getLongitude()), myGPS->getAltitude());
myMap->writeFile(myMap->getFileName(), true);
myMap->unlock();
reloadMapFile();
}
