public Vector2 ECEFtoXY(Vector3 ecef)
{
// convert to zero altitude lla
LLACoord lla = WGS84.ECEFtoLLA(ecef);
lla.alt = 0;
ecef = WGS84.LLAtoECEF(lla);
Vector3 enu = R_ecef_to_enu * (ecef - originECEF);
return(new Vector2(enu.X, enu.Y));
}
public static Vector3 LLAtoECEF(LLACoord lla)
{
double sin_lat = Math.Sin(lla.lat);
double cos_lat = Math.Cos(lla.lat);
double N = ae / Math.Sqrt(1 - e * e * sin_lat * sin_lat);
return new Vector3(
(N + lla.alt) * cos_lat * Math.Cos(lla.lon),
(N + lla.alt) * cos_lat * Math.Sin(lla.lon),
(be * be / (ae * ae) * N + lla.alt) * sin_lat);
}
public static Vector3 LLAtoECEF(LLACoord lla)
{
double sin_lat = Math.Sin(lla.lat);
double cos_lat = Math.Cos(lla.lat);
double N = ae / Math.Sqrt(1 - e * e * sin_lat * sin_lat);
return(new Vector3(
(N + lla.alt) * cos_lat * Math.Cos(lla.lon),
(N + lla.alt) * cos_lat * Math.Sin(lla.lon),
(be * be / (ae * ae) * N + lla.alt) * sin_lat));
}
/// <summary>
/// Constructs a planar projection based upon an origin latitude and longitude
/// </summary>
/// <param name="latitude"></param>
/// <param name="longitude"></param>
/// <returns></returns>
public static PlanarProjection PlanarProjection(GpsCoordinate origin, bool isDegrees)
{
if (isDegrees)
{
// transform
LLACoord llaOrigin = DegreesToLLA(origin);
// returns a new planar projection based on origin
return(new PlanarProjection(llaOrigin.lat, llaOrigin.lon));
}
// returns a new planar projection based on origin
return(new PlanarProjection(origin.latitude, origin.longitude));
}
/// <summary>
/// Saves the temp points to a file of degrees
/// </summary>
/// <param name="fileName"></param>
public void SaveToFileAsDegrees(string fileName, PlanarProjection pp)
{
FileStream fs = new FileStream(fileName, FileMode.Create);
StreamWriter sw = new StreamWriter(fs);
foreach (KeyValuePair <string, Coordinates> c in this.Points)
{
LLACoord lla = GpsTools.XyToLlaDegrees(c.Value, pp);
//LLACoord lla = new LLACoord(c.Value.X, c.Value.Y, 0);
sw.WriteLine(c.Key + "\t" + lla.lat.ToString("F6") + "\t" + lla.lon.ToString("F6"));
}
sw.Close();
fs.Dispose();
}
/// <summary>
/// Returns a string representing the lla coord in arc minutes and seconds
/// </summary>
/// <param name="lla"></param>
/// <returns></returns>
public static string LlaDegreesToArcMinSecs(LLACoord lla)
{
double latDegFrac = lla.lat % 1;
double latDeg = lla.lat - latDegFrac;
double latMinFull = latDegFrac * 60.0;
double latSecondsInMins = latMinFull % 1;
double latMin = latMinFull - latSecondsInMins;
double latSeconds = latSecondsInMins * 60.0;
string nString = latDeg.ToString() + " " + latMin.ToString() + "' " + latSeconds.ToString("F6") + "\" N";
double lonDegFrac = -lla.lon % 1;
double lonDeg = -lla.lon - lonDegFrac;
double lonMinFull = lonDegFrac * 60.0;
double lonSecondsInMins = lonMinFull % 1;
double lonMin = lonMinFull - lonSecondsInMins;
double lonSeconds = lonSecondsInMins * 60.0;
string eString = lonDeg.ToString() + " " + lonMin.ToString() + "' " + lonSeconds.ToString("F6") + "\" W";
return(nString + "\n" + eString);
}
public PlanarProjection(double originLat, double originLon)
{
LLACoord originLLA = new LLACoord(originLat, originLon, 0);
originECEF = WGS84.LLAtoECEF(originLLA);
double slon = Math.Sin(originLon);
double clon = Math.Cos(originLon);
double slat = Math.Sin(originLat);
double clat = Math.Cos(originLat);
R_enu_to_ecef = new Matrix(3, 3);
R_enu_to_ecef[0, 0] = -slon;
R_enu_to_ecef[0, 1] = -clon * slat;
R_enu_to_ecef[0, 2] = clon * clat;
R_enu_to_ecef[1, 0] = clon;
R_enu_to_ecef[1, 1] = -slon * slat;
R_enu_to_ecef[1, 2] = slon * clat;
R_enu_to_ecef[2, 0] = 0;
R_enu_to_ecef[2, 1] = clat;
R_enu_to_ecef[2, 2] = slat;
R_ecef_to_enu = R_enu_to_ecef.Transpose();
}
public PlanarProjection(double originLat, double originLon)
{
LLACoord originLLA = new LLACoord(originLat, originLon, 0);
originECEF = WGS84.LLAtoECEF(originLLA);
double slon = Math.Sin(originLon);
double clon = Math.Cos(originLon);
double slat = Math.Sin(originLat);
double clat = Math.Cos(originLat);
R_enu_to_ecef = new Matrix(3, 3);
R_enu_to_ecef[0, 0] = -slon;
R_enu_to_ecef[0, 1] = -clon * slat;
R_enu_to_ecef[0, 2] = clon * clat;
R_enu_to_ecef[1, 0] = clon;
R_enu_to_ecef[1, 1] = -slon * slat;
R_enu_to_ecef[1, 2] = slon * clat;
R_enu_to_ecef[2, 0] = 0;
R_enu_to_ecef[2, 1] = clat;
R_enu_to_ecef[2, 2] = slat;
R_ecef_to_enu = R_enu_to_ecef.Transpose();
}
void client_PoseAbsReceived(object sender, PoseAbsReceivedEventArgs e)
{
packetCount++;
Services.Dataset.MarkOperation("pose rate", LocalCarTimeProvider.LocalNow);
//OperationalTrace.WriteVerbose("got absolute pose for time {0}", e.PoseAbsData.timestamp);
if (lastPacketTime.ts > e.PoseAbsData.timestamp.ts)
{
lastPacketTime = new CarTimestamp(-10000);
}
if (e.PoseAbsData.timestamp.ts - lastPacketTime.ts >= 0.02)
{
DatasetSource ds = Services.Dataset;
PoseAbsData d = e.PoseAbsData;
CarTimestamp now = e.PoseAbsData.timestamp;
if (Services.Projection != null)
{
Coordinates xy = Services.Projection.ECEFtoXY(new Vector3(d.ecef_px, d.ecef_py, d.ecef_pz));
ds.ItemAs <Coordinates>("xy").Add(xy, now);
AbsolutePose absPose = new AbsolutePose(xy, d.yaw, now);
Services.AbsolutePose.PushAbsolutePose(absPose);
}
if (!Settings.UseWheelSpeed)
{
ds.ItemAs <double>("speed").Add(d.veh_vx, now);
}
ds.ItemAs <double>("vel - y").Add(d.veh_vy, now);
ds.ItemAs <double>("heading").Add(BiasEstimator.CorrectHeading(d.yaw), now);
ds.ItemAs <double>("pitch").Add(d.pitch + 1.25 * Math.PI / 180.0, now);
ds.ItemAs <double>("roll").Add(d.roll, now);
ds.ItemAs <double>("ba - x").Add(d.bax, now);
ds.ItemAs <double>("ba - y").Add(d.bay, now);
ds.ItemAs <double>("ba - z").Add(d.baz, now);
ds.ItemAs <double>("bw - x").Add(d.bwx, now);
ds.ItemAs <double>("bw - y").Add(d.bwy, now);
ds.ItemAs <double>("bw - z").Add(d.bwz, now);
ds.ItemAs <PoseCorrectionMode>("correction mode").Add(d.correction_mode, now);
LLACoord lla = WGS84.ECEFtoLLA(new Vector3(d.ecef_px, d.ecef_py, d.ecef_pz));
ds.ItemAs <double>("altitude").Add(lla.alt, now);
if (Services.Projection != null)
{
ds.ItemAs <Coordinates>("gps xy").Add(Services.Projection.ECEFtoXY(new Vector3(d.gps_px, d.gps_py, d.gps_pz)), now);
ds.ItemAs <Coordinates>("hp xy").Add(Services.Projection.ECEFtoXY(new Vector3(d.hp_px, d.hp_py, d.hp_pz)), now);
}
ds.ItemAs <double>("sep heading").Add(d.sep_heading, now);
ds.ItemAs <double>("sep pitch").Add(d.sep_pitch, now);
ds.ItemAs <double>("sep roll").Add(d.sep_roll, now);
if (!double.IsNaN(lastYawRate) && lastPacketTime.ts > 0)
{
// get the enu velocity
Vector3 pECEF = new Vector3(d.ecef_px, d.ecef_py, d.ecef_pz);
Vector3 vECEF = new Vector3(d.ecef_vx, d.ecef_vy, d.ecef_vz);
Matrix3 Recef2enu = Geocentric.Recef2enu(pECEF);
Vector3 vENU = Recef2enu * vECEF;
BiasEstimator.Update(lastYawRate, d.yaw, vENU, now.ts - lastPacketTime.ts, now);
Services.Dataset.ItemAs <double>("heading bias").Add(BiasEstimator.HeadingBias, now);
}
lastPacketTime = now;
}
}
public void Render(System.Drawing.Graphics g, WorldTransform t)
{
if (!this.snapToWaypoints || this.roadNetwork == null)
{
if (this.Current != null)
{
LLACoord lla = GpsTools.XyToLlaDegrees(Current, projection);
string locString = Current.X.ToString("F6") + ", " + Current.Y.ToString("F6") + "\n" +
lla.lat.ToString("F6") + ", " + lla.lon.ToString("F6") + "\n" + GpsTools.LlaDegreesToArcMinSecs(lla);
DrawingUtility.DrawControlPoint(this.Current, DrawingUtility.ColorToolPointAnalysis, locString,
ContentAlignment.BottomCenter, ControlPointStyle.SmallCircle, g, t);
}
if (Save != null && Save.Count > 0)
{
foreach (Coordinates tmp in Save)
{
LLACoord lla = GpsTools.XyToLlaDegrees(tmp, projection);
string locString = tmp.X.ToString("F6") + ", " + tmp.Y.ToString("F6") + "\n" +
lla.lat.ToString("F6") + ", " + lla.lon.ToString("F6") + "\n" + GpsTools.LlaDegreesToArcMinSecs(lla);
DrawingUtility.DrawControlPoint(tmp, DrawingUtility.ColorToolPointAnalysis, locString,
ContentAlignment.BottomCenter, ControlPointStyle.SmallCircle, g, t);
}
}
}
else
{
if (this.Current != null)
{
Coordinates c = this.Current;
double minDist = Double.MaxValue;
Coordinates?closest = null;
foreach (IArbiterWaypoint iaw in this.roadNetwork.ArbiterWaypoints.Values)
{
double d = iaw.Position.DistanceTo(c);
if (d < minDist && ((IDisplayObject)iaw).HitTest(c, (float)0.2, wt, DrawingUtility.DefaultFilter).Hit)
{
minDist = d;
closest = iaw.Position;
}
}
if (closest != null)
{
c = closest.Value;
}
LLACoord lla = GpsTools.XyToLlaDegrees(c, projection);
string locString = c.X.ToString("F6") + ", " + c.Y.ToString("F6") + "\n" +
lla.lat.ToString("F6") + ", " + lla.lon.ToString("F6") + "\n" + GpsTools.LlaDegreesToArcMinSecs(lla);
DrawingUtility.DrawControlPoint(c, DrawingUtility.ColorToolPointAnalysis, locString,
ContentAlignment.BottomCenter, ControlPointStyle.SmallCircle, g, t);
}
}
}
