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