/// <summary>
/// Reads in a file with arc mins to coords
/// </summary>
/// <param name="fileName"></param>
public void ReadArcMinutesFromFile(string fileName, PlanarProjection pp)
{
Points = new Dictionary <string, Coordinates>();
FileStream fs = new FileStream(fileName, FileMode.Open);
StreamReader sr = new StreamReader(fs);
while (!sr.EndOfStream)
{
string lineStream = sr.ReadLine();
string[] del1 = new string[] { "\t" };
string[] nwSplit = lineStream.Split(del1, StringSplitOptions.RemoveEmptyEntries);
/*string name = nwSplit[0];
*
* string[] del2 = new string[] { " " };
* string[] n = nwSplit[1].Split(del2, StringSplitOptions.RemoveEmptyEntries);
* string[] w = nwSplit[2].Split(del2, StringSplitOptions.RemoveEmptyEntries);
*/
string name = nwSplit[0];
double degN = double.Parse(nwSplit[1]);
double degE = double.Parse(nwSplit[2]);
Coordinates c = pp.ECEFtoXY(WGS84.LLAtoECEF((new LLACoord(degN * Math.PI / 180.0, degE * Math.PI / 180.0, 0.0))));
Points.Add(name, c);
}
sr.Close();
fs.Dispose();
}
static void Main(string[] args)
{
// 1.使用Microsoft.SqlServer.Types计算WGS84坐标的两点距离
var c = SqlGeography.Point(31.837964, 117.203123, 4326); //公司的位置
var d = SqlGeography.Point(31.822426, 117.221662, 4326); // 合肥市政务中心,4236代表WGS84这种坐标参照系统。
Console.WriteLine("公司和合肥市政务中心的距离是：" + c.STDistance(d) + "米"); //距离
// 2.使用把坐标当做平面坐标计算两点距离
double width = 31.837964 - 31.822426;
double height = 117.203123 - 117.221662;
double result = (width * width) + (height * height);
result = Math.Sqrt(result); //根号
Console.WriteLine("想象成平面时公司和合肥市政务中心的距离是：" + result + "米"); //距离
// 3.用haversine公式计算WGS84坐标的两点距离
WGS84 wgs84 = new WGS84();
var coordinateA = new Coordinate()
{
Latitude = 31.837964, Longitude = 117.203123
};
var coordinateB = new Coordinate()
{
Latitude = 31.822426, Longitude = 117.221662
};
var xxx = wgs84.Distance(coordinateA, coordinateB);
Console.WriteLine("用haversine公式时公司和合肥市政务中心的距离是：" + xxx * 1000 + "米"); //距离
Console.ReadKey();
}
public static void LoadGeotiffData(string path, out WGS84 coordinates, out Texture2D heightTexture)
{
Band band;
LoadGeotiffImage(path, out band, out coordinates);
heightTexture = GrayScaleTexture2DFromGDALBand(band);
}
private void DoSimpleReceiving(object sender, DoWorkEventArgs e)
{
if (!(e.Argument is CancellationToken))
{
throw new ArgumentException("e.Argument is CancellationToken");
}
var cancelToken = (CancellationToken)e.Argument;
//receiving loop as long as the subscrition is active or a chancelation is requested
while (_subscription.isActive())
{
var msg = _subscription.receiveMessage();
if (msg != null)
{
//Receiving Pedestrian GPS msg using Player service
var Stream = msg.getStreamID();
var datatype = (msg.getDataTypeID());
if (datatype == 16002349372918614042) //Datatype with GNSS Datatype
{
var gnss = (DioneData.Protobuf.GNSS.GlobalPositionData)ProtobufMessageConverter.MessageToProtobuf <DioneData.Protobuf.GNSS.GlobalPositionData>(msg);
// var gnss = new DioneData.Protobuf.GNSS.GlobalPositionData();
var lat_gps = gnss.Wgs84Latitude;
var lo_gps = gnss.Wgs84Longitude;
var height_gps = gnss.Wgs84Height;
var wg = new WGS84();
wg.Latitude = lat_gps;
wg.Longitude = lo_gps;
wg.Height = height_gps;
//Covert WGS-84 to UTM coordinates
var utm = UTM.FromWGS84(wg, 33);
var reader = new HDMapReader(utm, gpsCoordinate_previous);
//For storing previous coordinate HISTORIZATION
PointQ History_gps = new PointQ();
History_gps.X = utm.X;
History_gps.Y = utm.Y;
gpsCoordinate_previous = History_gps;
//gpsCoordinate_previous.Y = utm.Y;
//Console.WriteLine("time stamp: " + time.ToString() + "\t" + "vehicle velocity: " + vel);
// Console.WriteLine("WGS-Stream: " + Stream.ToString() + "\t" + "Pedestrian Latitude: " + lat);
// Console.WriteLine("WGS-Stream: " + Stream.ToString() + "\t" + "Pedestrian Longitude: " + lo);
// Console.WriteLine("WGS-Stream: " + Stream.ToString() + "\t" + "Pedestrian Altitude: " + al);
Console.WriteLine("UTM Coordinates of GPS: " + utm);
}
}
if (cancelToken.IsCancellationRequested)
{
break;
}
}
}
/// <summary>
/// LLA Coord in rad
/// </summary>
/// <param name="coordinate"></param>
/// <param name="proj"></param>
/// <returns></returns>
public static LLACoord XyToLlaRadians(Coordinates coordinate, PlanarProjection proj)
{
// lla coord
LLACoord llaRad = WGS84.ECEFtoLLA(proj.XYtoECEF(coordinate));
// return
return(llaRad);
}
private static void LoadGeotiffImage(string path, out Band band, out WGS84 coordinates)
{
try
{
Gdal.AllRegister();
Dataset rasterDataset = Gdal.Open(path, Access.GA_ReadOnly);
if (rasterDataset == null)
{
Debug.Log("Unable to read input raster..");
}
//raster bands
int bandCount = rasterDataset.RasterCount;
if (bandCount > 1)
{
Debug.Log("Input error, please provide single band raster image only..");
}
//raster size
int rasterCols = rasterDataset.RasterXSize;
int rasterRows = rasterDataset.RasterYSize;
//Extract geotransform
double[] geotransform = new double[6];
rasterDataset.GetGeoTransform(geotransform);
//Get raster bounding box
double originX = geotransform[0];
double originY = geotransform[3];
double pixelWidth = geotransform[1];
double pixelHeight = geotransform[5];
//Calculate box
double finalX = originX + (rasterCols * pixelWidth);
double finalY = originY + (rasterRows * pixelHeight);
coordinates.west = originX;
coordinates.south = finalY;
coordinates.east = finalX;
coordinates.north = originY;
//Read 1st band from raster
band = rasterDataset.GetRasterBand(1);
int rastWidth = rasterCols;
int rastHeight = rasterRows;
}
catch (Exception e)
{
//TODO: Deal with this errors
Debug.Log("error to read geotif. Error: " + e.Message);
band = null;
coordinates = new WGS84();
}
}
/// <summary>
/// Transoforms list of gps coordinates to xy coordinates
/// </summary>
/// <param name="gpsCoordinates"></param>
/// <param name="projection"></param>
/// <returns></returns>
public static List <Coordinates> TransformToXy(List <GpsCoordinate> gpsCoordinates, PlanarProjection projection)
{
// final list of referenced coordinates
List <Coordinates> xyCoordinates = new List <Coordinates>();
// loop through coordinates
foreach (GpsCoordinate gpsCooridnate in gpsCoordinates)
{
// generate xy coordinate from projection
xyCoordinates.Add(projection.ECEFtoXY(WGS84.LLAtoECEF(new LLACoord(gpsCooridnate.latitude, gpsCooridnate.longitude, 0))));
}
// return final list
return(xyCoordinates);
}
public SharpDX.Vector3 GetDirection(DateTime currentTime, double latitude, double longitude)
{
Vector3D position = WGS84.ToWorld(latitude, longitude);
Vector3 up = Vector3.Normalize(position.ToVector3());
Vector3 left = Vector3.Normalize(Vector3.Cross(up, Vector3.UnitZ)); // up x world_up
Vector3 north = Vector3.Normalize(Vector3.Cross(left, up));
double azimuth, altitude;
GetAziumuthAltitude(currentTime, latitude, longitude, out azimuth, out altitude);
Matrix azimuthRot = Matrix.RotationAxis(up, (float)azimuth);
Matrix altitudeRot = Matrix.RotationAxis(left, (float)altitude);
Matrix rot = azimuthRot * altitudeRot;
var sunDir = Vector3.Transform(north, rot);
return(new Vector3(sunDir.X, sunDir.Y, sunDir.Z));
}
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;
}
}
/// <summary>
/// Transoforms list of gps coordinates to xy coordinates
/// </summary>
/// <param name="gpsCoordinates"></param>
/// <param name="projection"></param>
/// <returns></returns>
public static Coordinates TransformToXy(GpsCoordinate gpsCoordinate, PlanarProjection projection)
{
// generate xy coordinate from projection
return(projection.ECEFtoXY(WGS84.LLAtoECEF(new LLACoord(gpsCoordinate.latitude, gpsCoordinate.longitude, 0))));
}
/// <summary>
/// Opens an rndf file and transforms the points to xy around a central projection
/// </summary>
/// <param name="fileName"></param>
/// <param name="rndf"></param>
/// <param name="projection"></param>
public static void GenerateXyRndfAndProjection(string fileName, out IRndf rndf, out PlanarProjection projection)
{
// generate the gps rndf
IRndf gpsRndf = GenerateGpsRndf(fileName);
// get the planar projection
projection = GetProjection(gpsRndf);
// create an xy rndf
IRndf xyRndf = gpsRndf;
#region Apply transform to all point
// Get coordinates from segments
foreach (SimpleSegment segment in xyRndf.Segments)
{
// Get coordinates from lanes
List <SimpleLane> lanes = (List <SimpleLane>)segment.Lanes;
foreach (SimpleLane lane in lanes)
{
// Get coordinates from waypoints
List <SimpleWaypoint> waypoints = (List <SimpleWaypoint>)lane.Waypoints;
foreach (SimpleWaypoint waypoint in waypoints)
{
// Transform the gps coordinate into an xy xoordinate
GpsCoordinate position = new GpsCoordinate(waypoint.Position.X, waypoint.Position.Y);
Coordinates tmpXY = projection.ECEFtoXY(WGS84.LLAtoECEF(GpsTools.DegreesToLLA(position)));
// Add position to the coordinate list
waypoint.Position = tmpXY;
}
}
}
foreach (SimpleZone zone in xyRndf.Zones)
{
// Get coordinates from perimeter
ZonePerimeter perimeter = zone.Perimeter;
List <PerimeterPoint> perimeterPoints = (List <PerimeterPoint>)perimeter.PerimeterPoints;
foreach (PerimeterPoint perimeterPoint in perimeterPoints)
{
// Transform the gps coordinate into an xy xoordinate
GpsCoordinate position = new GpsCoordinate(perimeterPoint.position.X, perimeterPoint.position.Y);
Coordinates tmpXY = projection.ECEFtoXY(WGS84.LLAtoECEF(GpsTools.DegreesToLLA(perimeterPoint.position)));
// Add position to the coordinate list
perimeterPoint.position = tmpXY;
}
// Get coordiantes from parking spots
List <ParkingSpot> parkingSpots = (List <ParkingSpot>)zone.ParkingSpots;
foreach (ParkingSpot parkingSpot in parkingSpots)
{
// Transform the gps coordinate into an xy xoordinate
GpsCoordinate position = new GpsCoordinate(parkingSpot.Waypoint1.Position.X, parkingSpot.Waypoint1.Position.Y);
Coordinates tmpXY = projection.ECEFtoXY(WGS84.LLAtoECEF(GpsTools.DegreesToLLA(parkingSpot.Waypoint1.Position)));
// wp1 position set
parkingSpot.Waypoint1.Position = tmpXY;
// Transform the gps coordinate into an xy xoordinate
position = new GpsCoordinate(parkingSpot.Waypoint2.Position.X, parkingSpot.Waypoint2.Position.Y);
Coordinates tmpXY2 = projection.ECEFtoXY(WGS84.LLAtoECEF(GpsTools.DegreesToLLA(parkingSpot.Waypoint2.Position)));
// wp1 position set
parkingSpot.Waypoint2.Position = tmpXY2;
}
}
# endregion
public void ConvertToGeodetic(double east, double north, double up, out double latitude, out double longitude, out double altitude)
{
ConvertToECEF(east, north, up, out double x, out double y, out double z);
WGS84.ConvertFromECEF(x, y, z, out latitude, out longitude, out altitude);
}
public void ConvertFromGeodetic(double latitude, double longitude, double altitude, out double east, out double north, out double up)
{
WGS84.ConvertToECEF(latitude, longitude, altitude, out double x, out double y, out double z);
ConvertFromECEF(x, y, z, out east, out north, out up);
}
public static List <Vector3> RouteDataFromKML(string kmlPath, TerrainManager terrainManager, WGS84 limits)
{
double horizontalSize = limits.east - limits.west;
double verticalSize = limits.north - limits.south;
List <Vector3> positions = new List <Vector3>();
int count = 0;
List <KMLPlacemark> placemarks = ReadKMLFile(kmlPath);
foreach (KMLPlacemark item in placemarks)
{
if (item.Type == KMLPlacemark.PlacemarkType.ROUTE)
{
foreach (Vector2 value in item.RouteValues)
{
double auxLong = value.x - limits.west;
double auxLat = value.y - limits.south;
double calculatedX = terrainManager.width * auxLong / horizontalSize;
double calculatedY = terrainManager.height * auxLat / verticalSize;
float auxHeight = terrainManager.terrain.GetTexture().GetPixel((int)calculatedX, (int)calculatedY).r;
//Add 10% of maxHeight to unblock from model
float finalHeight = auxHeight * terrainManager.maxHeight * 1.1f;
positions.Add(new Vector3((float)calculatedX, finalHeight, (float)calculatedY));
count++;
}
}
}
return(positions);
}
