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