//double GD_semiMajorAxis = 6378137.000000; //double GD_TranMercB = 6356752.314245; //double GD_geocentF = 0.003352810664; //void geodeticOffsetInv(double refLat, double refLon, // double lat, double lon, // out double xOffset, out double yOffset ) //{ // double a = GD_semiMajorAxis; // double b = GD_TranMercB; // double f = GD_geocentF; // double L = lon - refLon; // double U1 = Math.Atan((1 - f) * Math.Tan(refLat)); // double U2 = Math.Atan((1 - f) * Math.Tan(lat)); // double sinU1 = Math.Sin(U1); // double cosU1 = Math.Cos(U1); // double sinU2 = Math.Sin(U2); // double cosU2 = Math.Cos(U2); // double lambda = L; // double lambdaP; // double sinSigma; // double sigma; // double cosSigma; // double cosSqAlpha; // double cos2SigmaM; // double sinLambda; // double cosLambda; // double sinAlpha; // int iterLimit = 100; // do // { // sinLambda = Math.Sin(lambda); // cosLambda = Math.Cos(lambda); // sinSigma = Math.Sqrt((cosU2 * sinLambda) * (cosU2 * sinLambda) + // (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) * // (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda)); // if (sinSigma == 0) // { // xOffset = 0.0; // yOffset = 0.0; // return; // co-incident points // } // cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda; // sigma = Math.Atan2(sinSigma, cosSigma); // sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma; // cosSqAlpha = 1 - sinAlpha * sinAlpha; // if(cosSqAlpha == 0) // cos2SigmaM = 0; // else // cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha; // double C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha)); // lambdaP = lambda; // lambda = L + (1 - C) * f * sinAlpha * (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM))); // } while (Math.Abs(lambda - lambdaP) > 1e-12 && --iterLimit > 0); // if (iterLimit == 0) // { // xOffset = 0.0; // yOffset = 0.0; // return; // formula failed to converge // } // double uSq = cosSqAlpha * (a * a - b * b) / (b * b); // double A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq))); // double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq))); // double deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) - // B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM))); // double s = b * A * (sigma - deltaSigma); // double bearing = Math.Atan2(cosU2 * sinLambda, cosU1 * sinU2 - sinU1 * cosU2 * cosLambda); // xOffset = Math.Sin(bearing) * s; // yOffset = Math.Cos(bearing) * s; // return; //} public void BuildKMLFile() { KMLFile kmlFile = new KMLFile(); kmlFile.LoadXML("SamplePath.kml"); kmlFile.SetName(videoName); string coordinatesStr = ""; bool isFirst = true; int count = 0; int gpsCount = 0; foreach (var node in m_images_gps5_list) { gpsCount++; GPS5DataFile.GPS5Data gpsData = node.Value; if (isFirst) { coordinatesStr += Convert.ToString(gpsData.longitude); coordinatesStr += ","; coordinatesStr += Convert.ToString(gpsData.latitude); coordinatesStr += ","; coordinatesStr += Convert.ToString(gpsData.altitude); coordinatesStr += "\n "; isFirst = false; count++; continue; } if (gpsData.speed_2d < 1.0) { continue; } if (gpsCount % 30 == 0) { coordinatesStr += Convert.ToString(gpsData.longitude); coordinatesStr += ","; coordinatesStr += Convert.ToString(gpsData.latitude); coordinatesStr += ","; coordinatesStr += Convert.ToString(gpsData.altitude); coordinatesStr += "\n "; count++; continue; } if (gpsData.frame == max_count_image_idx) { coordinatesStr += Convert.ToString(gpsData.longitude); coordinatesStr += ","; coordinatesStr += Convert.ToString(gpsData.latitude); coordinatesStr += ","; coordinatesStr += Convert.ToString(gpsData.altitude); coordinatesStr += "\n "; count++; continue; } //string filename = node.Key; } kmlFile.SetCoordinates(coordinatesStr); kmlFile.SaveXML(kmlFilename); }
public void BuildKMLFile() { string kmlFilename = Path.Combine(destFolder, sceneName + ".kml"); KMLFile kmlFile = new KMLFile(); kmlFile.LoadXML("SamplePath.kml"); kmlFile.SetName(sceneName); string coordinatesStr = ""; bool isFirst = true; int count = 0; int gpsCount = 0; foreach (var node in m_total_gps_data_list) { gpsCount++; GPS5DataFile.GPS5Data gpsData = node.Value; if (isFirst) { coordinatesStr += Convert.ToString(gpsData.longitude); coordinatesStr += ","; coordinatesStr += Convert.ToString(gpsData.latitude); coordinatesStr += ","; coordinatesStr += Convert.ToString(gpsData.altitude); coordinatesStr += "\n "; isFirst = false; count++; continue; } if (gpsData.speed_2d < 1.0) { continue; } if (gpsCount % 30 == 0) { coordinatesStr += Convert.ToString(gpsData.longitude); coordinatesStr += ","; coordinatesStr += Convert.ToString(gpsData.latitude); coordinatesStr += ","; coordinatesStr += Convert.ToString(gpsData.altitude); coordinatesStr += "\n "; count++; continue; } //if (gpsData.frame == max_count_image_idx) //{ // coordinatesStr += Convert.ToString(gpsData.longitude); // coordinatesStr += ","; // coordinatesStr += Convert.ToString(gpsData.latitude); // coordinatesStr += ","; // coordinatesStr += Convert.ToString(gpsData.altitude); // coordinatesStr += "\n "; // count++; // continue; //} //string filename = node.Key; } kmlFile.SetCoordinates(coordinatesStr); kmlFile.SaveXML(kmlFilename); Program.AddLog("Save KML: " + kmlFilename); return; }