コード例 #1
0
        public string Proximas_Secuencias_de_movimiento(ListBox lb_distancias, ListBox lb_iteraciones)
        {
            if (this.optimal_path != null && this.optimal_path.Count != 0 && this.index_path != -1)
            {
                List <TRNode> temp_path = this.optimal_path[index_path++];
                temp_path.Insert(0, new TRNode(this.arm[this.arm.Count - 1]));
                List <TRNode> current_path = TREXT_TrajectoryGraph.GetThinPartition(temp_path, this.config.TR_Separacion_entre_nodos);
                Publica_lo_basico();
                this.progres_bar.Visible = true;
                this.progres_bar.Maximum = current_path.Count;

                ParticleFilter pf           = new ParticleFilter();
                List <TMPoint> chain_result = null;
                int            iteraciones  = 0;
                double         distancia    = 0;
                this.progres_bar.Minimum = 0;

                #region codigo para verificar en matlab
                //string s_matlab = "[" + current_path[0].Point.X.ToString() + "," + current_path[0].Point.Y.ToString();
                //for (int i = 1; i < current_path.Count; i++)
                //{
                //    string x_pe = Math.Floor(current_path[i].Point.X).ToString();
                //    string x_pf = (current_path[i].Point.X - Math.Floor(current_path[i].Point.X)).ToString();
                //    if(x_pf != "0")
                //        x_pf = x_pf.Substring(2);
                //    string sx = x_pe + "." + x_pf;

                //    string y_pe = Math.Floor(current_path[i].Point.Y).ToString();
                //    string y_pf = (current_path[i].Point.Y - Math.Floor(current_path[i].Point.Y)).ToString();
                //    if (y_pf != "0")
                //        y_pf = y_pf.Substring(2);
                //    string sy = y_pe + "." + y_pf;

                //    s_matlab += "; " + sx + "," + sy;
                //}
                //s_matlab += "]";
                #endregion

                List <TMPoint> temp_arm = this.arm;
                //for (int i = index_firts_node; i < current_path.Count; i++)
                for (int i = 0; i < current_path.Count; i++)
                {
                    this.progres_bar.Increment(1);
                    bool llego = pf.Filter(out chain_result, out iteraciones, out distancia, config.OPT_Maximo_iteraciones, config.PF_Epsilon, this.sistema_obstaculos, temp_arm, current_path[i].Point, config.PF_Numero_muestras, config.PF_Media_Gausiana);
                    Publicar_Brazo(chain_result);

                    lb_distancias.Items.Add(distancia);
                    string sllega = (llego) ? "SI" : "NO";
                    lb_iteraciones.Items.Add(iteraciones.ToString() + " - " + sllega);
                    temp_arm = chain_result;
                }
                if (index_path >= optimal_path.Count)
                {
                    this.index_path = -1;
                }
                this.progres_bar.Visible = false;
                return("OK");
            }
            else
            {
                return("Se han terminado los caminos posibles a seguir");
            }
        }
コード例 #2
0
        public string Proximas_Secuencias_de_movimiento(ListBox lb_distancias, ListBox lb_iteraciones)
        {
            if (this.optimal_path != null && this.optimal_path.Count != 0 && this.index_path != -1)
            {
                List <TRNode> temp_path = this.optimal_path[index_path++];
                temp_path.Insert(0, new TRNode(this.arm[this.arm.Count - 1]));
                List <TRNode> current_path = TREXT_TrajectoryGraph.GetThinPartition(temp_path, this.config.TR_Separacion_entre_nodos);
                Publica_lo_basico();
                this.progres_bar.Visible = true;
                this.progres_bar.Maximum = current_path.Count;

                pf = new ParticleFilter();
                this.progres_bar.Minimum = 0;

                #region codigo para verificar en matlab
                //string s_matlab = "[" + current_path[0].Point.X.ToString() + "," + current_path[0].Point.Y.ToString();
                //for (int i = 1; i < current_path.Count; i++)
                //{
                //    string x_pe = Math.Floor(current_path[i].Point.X).ToString();
                //    string x_pf = (current_path[i].Point.X - Math.Floor(current_path[i].Point.X)).ToString();
                //    if(x_pf != "0")
                //        x_pf = x_pf.Substring(2);
                //    string sx = x_pe + "." + x_pf;

                //    string y_pe = Math.Floor(current_path[i].Point.Y).ToString();
                //    string y_pf = (current_path[i].Point.Y - Math.Floor(current_path[i].Point.Y)).ToString();
                //    if (y_pf != "0")
                //        y_pf = y_pf.Substring(2);
                //    string sy = y_pe + "." + y_pf;

                //    s_matlab += "; " + sx + "," + sy;
                //}
                //s_matlab += "]";
                #endregion

                List <TMPoint> temp_arm = this.arm;
                //for (int i = index_firts_node; i < current_path.Count; i++)

                const int Top_MaxIter = 50;
                double    Top_esp     = Math.Pow(10, -8);

                int    cant_iteraciones = 0;
                double suma_distancia   = 0;
                double distancia        = 0;
                foreach (TRNode t in current_path)
                {
                    int indexOf_t = current_path.IndexOf(t);
                    this.progres_bar.Increment(1);
                    List <TMPoint> chain_result;
                    int            iteraciones;
                    double         w;
                    bool           llego =
                        pf.MATLAB_Filter(out chain_result,
                                         out iteraciones,
                                         out distancia,
                                         out w,
                                         indexOf_t == current_path.Count - 1 ? Top_MaxIter : config.PF_Maximo_iteraciones,
                                         indexOf_t == current_path.Count - 1 ? Top_esp : config.PF_Epsilon,
                                         this.sistema_obstaculos,
                                         temp_arm,
                                         t.Point,
                                         config.PF_Numero_muestras,
                                         config.PF_Media_Gausiana);
                    //pf.CS_Filter( out chain_result, out iteraciones, out distancia, out w, config.OPT_Maximo_iteraciones, config.PF_Epsilon, this.sistema_obstaculos, temp_arm, t.Point, config.PF_Numero_muestras, config.PF_Media_Gausiana );
                    Publicar_Brazo(chain_result);

                    if (w == 0)
                    {
                        MessageBox.Show(string.Format("Mejor partícula con peso 0"));
                    }

                    lb_distancias.Items.Add(distancia);
                    string sllega = (llego) ? "SI" : "NO";
                    lb_iteraciones.Items.Add(string.Format("{0} - {1}", iteraciones, sllega));
                    temp_arm        = chain_result;
                    suma_distancia += distancia;
                    cant_iteraciones++;
                }
                if (index_path >= optimal_path.Count)
                {
                    this.index_path = -1;
                }
                this.progres_bar.Visible = false;
                return(string.Format("Separación promedio de la trayectoria {0} px\nSeparación final del e.e. al objetivo {1}", suma_distancia / cant_iteraciones, distancia));
            }
            return("Se han terminado los caminos posibles a seguir");
        }