//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);
        }
Esempio n. 2
0
        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;
        }