// Algorithm specific implementation of the path planning protected override void DoPathPlanning() { // Take care of business without hierarchical search // Always use parallelization at this level no matter what if (!curRequest.UseEndPoint) { // For the no end point path planning requests // Always use CC PathPlanningRequest newRequest = curRequest.DeepClone(); newRequest.AlgToUse = AlgType.CC; AlgPathPlanning myAlg = new AlgCC(newRequest, mDist, mDiff, Efficiency_UB); lstThreads.Add(myAlg); // Also always use LHCGWCONV newRequest = curRequest.DeepClone(); newRequest.AlgToUse = AlgType.LHCGWCONV; RtwMatrix mDistCopy1 = mDist.Clone(); RtwMatrix mDiffCopy1 = mDiff.Clone(); myAlg = new AlgGlobalWarming(newRequest, ModeCount, mDistCopy1, mDiffCopy1, Efficiency_UB); lstThreads.Add(myAlg); // TopTwo works best when there are two modes (n>=2) if (ModeCount == 2) { newRequest = curRequest.DeepClone(); newRequest.AlgToUse = AlgType.TopTwo; RtwMatrix mDistCopy2 = mDist.Clone(); RtwMatrix mDiffCopy2 = mDiff.Clone(); myAlg = new AlgTopTwo(newRequest, ModeCount, mModes, mDistCopy2, mDiffCopy2, Efficiency_UB); lstThreads.Add(myAlg); } // Use TopN if there are more than two modes if (ModeCount >= 3) { newRequest = curRequest.DeepClone(); newRequest.AlgToUse = AlgType.TopN; if (ModeCount <= ProjectConstants.Max_N) { newRequest.TopN = ModeCount; } // This is really hierarchical search for (int i = 3; i < ProjectConstants.Max_N + 1; i++) { PathPlanningRequest newR = newRequest.DeepClone(); newR.TopN = i; RtwMatrix mDistCopy2 = mDist.Clone(); RtwMatrix mDiffCopy2 = mDiff.Clone(); myAlg = new AlgTopN(newR, ModeCount, mModes, mDistCopy2, mDiffCopy2, Efficiency_UB); lstThreads.Add(myAlg); } } if (ProjectConstants.DebugMode) { curRequest.SetLog("Running a total of " + lstThreads.Count + " path planning tasks.\n"); } SpawnThreads(); } else { } //if (HierarchicalSearch()) //{ // return; //} }
// Function to actually generate seeds of various algorithms private bool GenerateSeeds(List<EAPath> AllPaths, PathPlanningRequest newRequest, AlgPathPlanning myAlg, int count) { for (int i = 0; i < count; i++) { switch (newRequest.AlgToUse) { case AlgType.CC: myAlg = new AlgCC(newRequest, mDist, mDiff, Efficiency_UB); break; case AlgType.LHCGWCONV: myAlg = new AlgGlobalWarming(newRequest, ModeCount, mDist, mDiff, Efficiency_UB); break; case AlgType.LHCGWPF: myAlg = new AlgGlobalWarming(newRequest, ModeCount, mDist, mDiff, Efficiency_UB); break; case AlgType.LHCRandom: myAlg = new AlgLHCRandom(newRequest, mDist, mDiff, Efficiency_UB); break; case AlgType.Random: myAlg = new AlgRandom(newRequest, mDist, mDiff, Efficiency_UB); break; case AlgType.TopTwoH: myAlg = new AlgTopTwo(newRequest, ModeCount, mModes, mDist, mDiff, Efficiency_UB); break; case AlgType.TopNH: myAlg = new AlgTopTwo(newRequest, ModeCount, mModes, mDist, mDiff, Efficiency_UB); break; default: break; } DateTime startTime2 = DateTime.Now; myAlg.PlanPath(); DateTime stopTime2 = DateTime.Now; TimeSpan duration2 = stopTime2 - startTime2; double RunTime2 = duration2.TotalSeconds; if (ProjectConstants.DebugMode) { curRequest.SetLog("Algorithm " + newRequest.AlgToUse + " took " + RunTime2 + " seconds.\n"); } EAPath eap = new EAPath(); eap.CDF = myAlg.GetCDF(); eap.Path.AddRange(myAlg.GetPath()); myAlg = null; // Add EAPath to population AllPaths.Add(eap); // If we already have the best path, then no need to continue CDF = eap.CDF; Path = eap.Path; if (Math.Abs(CDF - Efficiency_UB) < 0.001) { return true; } } return false; }
// Algorithm specific implementation of the path planning protected override void DoPathPlanning() { AlgPathPlanning curAlg; AlgPathPlanning curAlgReversed; PathPlanningRequest curRequestReversed = curRequest.Clone(); curRequestReversed.pStart = curRequest.pEnd; curRequestReversed.pEnd = curRequest.pStart; // Use the right algorithm switch (curRequest.AlgToUse) { case AlgType.CC_E: curAlg = new AlgCC_E(curRequest, mDist, mDiff, Efficiency_UB); curAlgReversed = new AlgCC_E(curRequestReversed, mDist, mDiff, Efficiency_UB); break; case AlgType.LHCGWCONV_E: curAlg = new AlgGlobalWarming(curRequest, ModeCount, mDist, mDiff, Efficiency_UB); curAlgReversed = new AlgGlobalWarming(curRequestReversed, ModeCount, mDist, mDiff, Efficiency_UB); break; case AlgType.LHCGWPF_E: curAlg = new AlgGlobalWarming(curRequest, ModeCount, mDist, mDiff, Efficiency_UB); curAlgReversed = new AlgGlobalWarming(curRequestReversed, ModeCount, mDist, mDiff, Efficiency_UB); break; case AlgType.LHCRandom_E: curAlg = new AlgLHCRandom(curRequest, mDist, mDiff, Efficiency_UB); curAlgReversed = new AlgLHCRandom(curRequestReversed, mDist, mDiff, Efficiency_UB); break; case AlgType.Random_E: curAlg = new AlgRandom(curRequest, mDist, mDiff, Efficiency_UB); curAlgReversed = new AlgRandom(curRequestReversed, mDist, mDiff, Efficiency_UB); break; case AlgType.CONV_E: curAlg = new AlgGlobalWarming(curRequest, ModeCount, mDist, mDiff, Efficiency_UB); curAlgReversed = new AlgGlobalWarming(curRequestReversed, ModeCount, mDist, mDiff, Efficiency_UB); break; case AlgType.PF_E: curAlg = new AlgPFLooper(curRequest, mDist, mDiff, Efficiency_UB); curAlgReversed = new AlgPFLooper(curRequestReversed, mDist, mDiff, Efficiency_UB); break; case AlgType.EA_E: curAlg = new AlgEA_E(curRequest, ModeCount, mDist, mDiff, Efficiency_UB); curAlgReversed = new AlgEA_E(curRequestReversed, ModeCount, mDist, mDiff, Efficiency_UB); break; default: curAlg = null; curAlgReversed = null; break; } curAlg.PlanPath(); curAlgReversed.PlanPath(); if (curAlg.GetCDF() >= curAlgReversed.GetCDF()) { CDF = curAlg.GetCDF(); Path = curAlg.GetPath(); } else { CDF = curAlgReversed.GetCDF(); Path = curAlgReversed.GetPath(); Path.Reverse(); } }
// When the test button is pressed (Count how many modes) private void btnTest_Click(object sender, EventArgs e) { #region Test Mode Count //DateTime startTime = DateTime.Now; //CountDistModes myCount; //if (chkUseDiff.Checked) //{ // RtwMatrix mDistReachable = test.DistMap.Clone(); // RtwMatrix mDiffReachable = test.DiffMap.Clone(); // RtwMatrix mRealModes = new RtwMatrix(CurDiffMap.Rows, CurDiffMap.Columns); // for (int i = 0; i < mRealModes.Rows; i++) // { // for (int j = 0; j < mRealModes.Columns; j++) // { // mRealModes[i, j] = mDistReachable[i, j] * // (float)test.DiffRates[Convert.ToInt32(mDiffReachable[i, j])]; // } // } // myCount = new CountDistModes(mRealModes); // // Debug code: Showing the product of dist and diff // frmMap mapReal = new frmMap(); // Bitmap CurBMPReal = new Bitmap(mRealModes.Columns, mRealModes.Rows); // ImgLib.MatrixToImage(ref mRealModes, ref CurBMPReal); // mapReal.Text = "Real probability distribution with respect to difficulty map"; // mapReal.setImage(CurBMPReal); // mapReal.Show(); // mapReal.resetImage(); //} //else //{ // myCount = new CountDistModes(CurDistMap); //} //Log(myCount.GetCount().ToString() + "\n"); //DateTime stopTime = DateTime.Now; //TimeSpan duration = stopTime - startTime; //Log("Computation took " + duration.ToString() + " seconds.\n"); //// Show mode nodes //RtwMatrix myModes = myCount.GetModes().Clone(); //for (int i = 0; i < myModes.Rows; i++) //{ // for (int j = 0; j < myModes.Columns; j++) // { // if (myModes[i, j] > 0) // { // myModes[i, j] = 255; // } // } //} //// Convert matrix to image //Bitmap CurBMP = new Bitmap(myModes.Columns, myModes.Rows); //ImgLib.MatrixToImage(ref myModes, ref CurBMP); //// Showing map in map form //frmMap myModesForm = new frmMap(this); //myModesForm.Text = "Modes Map"; //myModesForm.setImage(CurBMP); //myModesForm.Show(); //myCount = null; #endregion #region Test permutation //int[] intInput = { 1, 2, 3, 4}; //Log(ShowPermutations<int>(intInput, 3)); //string[] stringInput = { "Hello", "World", "Foo" }; //Log(ShowPermutations<string>(stringInput, 2)); #endregion #region Test MATLAB ////////////////////// //// Input Parameters ////////////////////// //// create an array ar for the real part of "a" //System.Array ar = new double[2]; //ar.SetValue(11, 0); //ar.SetValue(12, 1); //// create an array ai for the imaginary part of "a" //System.Array ai = new double[2]; //ai.SetValue(1, 0); //ai.SetValue(2, 1); //// create an array br for the real part of "b" //System.Array br = new double[2]; //br.SetValue(21, 0); //br.SetValue(22, 1); //// create an array bi for the imaginary part of "b" //System.Array bi = new double[2]; //bi.SetValue(3, 0); //bi.SetValue(4, 1); /////////////////////// //// Output Parameters /////////////////////// //// initialize variables for return value from ML //System.Array cr = new double[2]; //System.Array ci = new double[2]; //System.Array dr = new double[2]; //System.Array di = new double[2]; ////////////////////////// //// Call MATLAB function ////////////////////////// //// call appropriate function/method based on Mode //// use MATLAB engine //UseEngine(ar, ai, br, bi, ref cr, ref ci, ref dr, ref di); //Log("ar = " + ar.GetValue(0).ToString() + " " + ar.GetValue(1).ToString() + "\n"); //Log("ai = " + ai.GetValue(0).ToString() + " " + ai.GetValue(1).ToString() + "\n"); //Log("br = " + br.GetValue(0).ToString() + " " + br.GetValue(1).ToString() + "\n"); //Log("bi = " + bi.GetValue(0).ToString() + " " + bi.GetValue(1).ToString() + "\n"); //Log("cr = " + cr.GetValue(0).ToString() + " " + cr.GetValue(1).ToString() + "\n"); //Log("ci = " + ci.GetValue(0).ToString() + " " + ci.GetValue(1).ToString() + "\n"); //Log("dr = " + dr.GetValue(0).ToString() + " " + dr.GetValue(1).ToString() + "\n"); //Log("di = " + di.GetValue(0).ToString() + " " + di.GetValue(1).ToString() + "\n"); #endregion #region Test MATH.NET EVD //DenseMatrix m = new DenseMatrix(new[,] { { 81.1887, -18.4630 }, { -18.4630, 115.9033 } }); //System.Numerics.Complex[] d = m.Evd().EigenValues().ToArray(); //double a = d[0].Real; //double b = d[1].Real; //Log(a + " " + b); #endregion #region Test Arrays //int n = 3; //Array arrModes = new double[n]; //Array arrMUs = new double[n, 2]; //Array arrSigmaXSigmaY = new double[n]; //for (int i = 0; i < n; i++) //{ // // Means // arrMUs.SetValue(21, i, 0); // arrMUs.SetValue(22, i, 1); //} #endregion #region Compute Efficiency with existing path #region Sanity Check // Make sure maps are loaded if (chkUseDist.Checked && CurDistMap == null) { System.Windows.Forms.MessageBox.Show("Please load a probability distribution map first!"); return; } if (chkUseDiff.Checked && CurDiffMap == null) { System.Windows.Forms.MessageBox.Show("Please load a task-difficulty map first!"); return; } // Make sure distribution map and task-difficulty map have same size if (chkUseDiff.Checked && chkUseDist.Checked) { if (CurDistMap.Rows != CurDiffMap.Rows || CurDistMap.Columns != CurDiffMap.Columns) { System.Windows.Forms.MessageBox.Show("Please make sure the distribution map and the " + "task-difficulty map must be the same size!"); return; } } // Use default distribution map or task-difficulty map if only one is checked if (chkUseDiff.Checked && !chkUseDist.Checked) { CurDistMap = new RtwMatrix(CurDiffMap.Rows, CurDiffMap.Columns); } if (!chkUseDiff.Checked && chkUseDist.Checked) { CurDiffMap = new RtwMatrix(CurDistMap.Rows, CurDistMap.Columns); } #endregion // Create request object PathPlanningRequest newRequest = new PathPlanningRequest(); #region Setting Request Object Properties newRequest.UseDistributionMap = chkUseDist.Checked; newRequest.UseTaskDifficultyMap = chkUseDiff.Checked; newRequest.UseHierarchy = chkHierarchy.Checked; newRequest.UseCoarseToFineSearch = chkCoaseToFine.Checked; newRequest.UseParallelProcessing = chkParallel.Checked; if (rbtnFixWing.Checked) { newRequest.VehicleType = UAVType.FixWing; } if (rbtnCopter.Checked) { newRequest.VehicleType = UAVType.Copter; } if (rbtnFixedAmount.Checked) { newRequest.DetectionType = DType.FixAmount; } if (rbtnFixedAmountPercent.Checked) { newRequest.DetectionType = DType.FixAmountInPercentage; } if (rbtnFixedPercent.Checked) { newRequest.DetectionType = DType.FixPercentage; } newRequest.DetectionRate = Convert.ToDouble(ntxtDetectionRate.Value); newRequest.DistMap = CurDistMap; newRequest.DiffMap = CurDiffMap; newRequest.UseEndPoint = chkUseEndPoint.Checked; newRequest.T = trbFlightTime.Value; newRequest.pStart.column = Convert.ToInt16(ntxtSX.Value); newRequest.pStart.row = Convert.ToInt16(ntxtSY.Value); newRequest.pEnd.column = Convert.ToInt16(ntxtEX.Value); newRequest.pEnd.row = Convert.ToInt16(ntxtEY.Value); newRequest.AlgToUse = AlgType.LHCGWCONV; newRequest.DrawPath = chkShowPath.Checked; #endregion #region Find max task-difficulty and compute diff rates only once if (chkUseDiff.Checked) { newRequest.MaxDifficulty = Convert.ToInt32(CurDiffMap.MinMaxValue()[1]); // Set task-difficulty rates double[] DiffRates = new double[newRequest.MaxDifficulty + 1]; double rate = 1.0 / (newRequest.MaxDifficulty + 1); for (int i = 0; i < newRequest.MaxDifficulty + 1; i++) { DiffRates[i] = 1 - i * rate; } newRequest.DiffRates = DiffRates; } if (!newRequest.SanityCheck()) { System.Windows.Forms.MessageBox.Show(newRequest.GetLog()); return; } #endregion // Read in existing path using hard-coded file path string BAPathFileName = @"H:\Research\20 New IPPAs for Partial Detection Conference Paper\Selected Cases Maps\BAPath.csv"; RtwMatrix BAPath = MISCLib.ReadInMap(BAPathFileName); #region First do reachable area (Dist and Diff) RtwMatrix mDistReachable; RtwMatrix mDiffReachable; if (newRequest.T < newRequest.DistMap.Rows + newRequest.DistMap.Columns) { mDistReachable = newRequest.DistMap.Clone(); mDiffReachable = newRequest.DiffMap.Clone(); if (!ComputeReachableArea(newRequest, mDistReachable, mDiffReachable)) { // Cannot plan path. return; } } else { mDistReachable = newRequest.DistMap.Clone(); mDiffReachable = newRequest.DiffMap.Clone(); } #endregion // Then do efficiency lower bound ComputeEfficiencyUB myELB = new ComputeEfficiencyUB(newRequest, mDistReachable, mDiffReachable); double Efficiency_UB = myELB.GetEfficiency_UB(); List<Point> TeleportPath = myELB.GetTeleportPath(); myELB = null; // Creating path planning object AlgPathPlanning curAlg = new AlgGlobalWarming(newRequest, 1, mDistReachable, mDiffReachable, Efficiency_UB); // Call method to compute existing path CDF List<Point> Path = new List<Point>(); for(int i=0; i<BAPath.Rows; i++) { Point p = new Point(Convert.ToInt16(BAPath[i,0]), Convert.ToInt16(BAPath[i,1])); Path.Add(p); } float CDF = curAlg.GetTrueCDF(Path); // Compute efficiency double Efficiency = 0; if (Efficiency_UB == 0) { Efficiency = 1; } else { Efficiency = CDF / Efficiency_UB; } // Compute CDF for graph Console.WriteLine("Print Existing Path CDF Graph:"); curAlg.PrintCDFGraph(TeleportPath, newRequest.DistMap); Console.WriteLine("Print BAPath CDF Graph:"); curAlg.PrintCDFGraph(Path, newRequest.DistMap); curAlg.SetPath(Path); // Show results Log("----------------------------------------------\n"); Log("Efficiency: " + Efficiency.ToString() + "\n"); Log("----------------------------------------------"); Log("----------------------------------------------\n"); #region Show path if (newRequest.DrawPath) { // Draw path with map remains Bitmap CurBMP3 = new Bitmap(mDistReachable.Columns, mDistReachable.Rows); ImgLib.MatrixToImage(ref mDistReachable, ref CurBMP3); frmMap map3 = new frmMap(); map3.Text = "UAV trajectory and coverage"; map3.setImage(CurBMP3); map3.Show(); map3.resetImage(); List<float> remains = curAlg.ShowCoverage(); Color c = Color.FromArgb(255, 0, 0); for (int i = 0; i < Path.Count; i++) { Point p = Path[i]; map3.setPointColor(p, c); map3.Refresh(); map3.setPointColor(p, remains[i]); map3.Refresh(); } // Drawing real path MISCLib.ShowImage(MISCLib.DrawPath(Path), "Real Path"); } #endregion #endregion }