/*! * 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(); }