Пример #1
0
    void InitializeRobotState(double x, double y, double theta)
    {
        X_rob_predict = Vector <double> .Build.DenseOfArray(new double[] { 0, 0, 0 });

        X_rob_actual = X_rob_predict.Clone();

        observedLandmarks = ObserveLandmarks(X_rob_actual, rangeNoiseFactor, bearingNoiseFactor);

        BuildGraph(observedLandmarks, landmarkTrace, ref graph);
        robotTrace_predict.Add(X_rob_predict.Clone());
        robotTrace_actual.Add(X_rob_actual.Clone());
        landmarkTrace.Add(observedLandmarks);
        graphVisualizer.VisualizerRobotRegistration((float)x, (float)y, (float)theta);
    }
Пример #2
0
    void Graph_Update(double deltaDis, double deltaAng)
    {
        allowOptimization = false;

        //Shall not happen
        if (robotTrace_predict.Count != robotTrace_actual.Count)
        {
            return;
        }

        //Debug.Log("Graph update");

        ////Avoid Overflow
        //if (robotTrace_predict.Count >= historyLength)
        //{
        //    //Needs Debugging!
        //    robotTrace_predict.RemoveAt(0);
        //    robotTrace_actual.RemoveAt(0);
        //}

        double ang_noise = deltaDis / 1 * Deg2Rad * angNoiseFactor * normal.Sample(),
               dis_noise = deltaDis / 1 * transNoiseFactor * normal.Sample();

        StatesUpdate(ref X_rob_actual, deltaDis + dis_noise, deltaAng + ang_noise);
        StatesUpdate(ref X_rob_predict, deltaDis, deltaAng);
        observedLandmarks = ObserveLandmarks(X_rob_actual, rangeNoiseFactor, bearingNoiseFactor);

        graphVisualizer.Visualize(X_rob_predict, X_rob_actual, false);

        if (deltaDis != 0)
        {
            if (count == recordInterval)
            {
                graphVisualizer.Visualize(X_rob_predict, X_rob_actual, true);

                BuildGraph(observedLandmarks, landmarkTrace, ref graph);

                landmarkTrace.Add(observedLandmarks);
                robotTrace_predict.Add(X_rob_predict.Clone());
                robotTrace_actual.Add(X_rob_actual.Clone());

                List <List <int> > cycles = new List <List <int> >();
                //cycles = FindCycle(graph, landmarkTrace.Count - 1, landmarkTrace.Count - 1, new List<int>(), new List<int>(), cycles);
                cycles.Add(FindCycleSimplified(graph));

                if (cycles.Count != 0 && cycles[0].Count != 0)
                {
                    //Debug.Log("Debug graph:");
                    //for (int h = 0; h < graph.Count; h++)
                    //{
                    //    string str = h + "-";
                    //    for (int i = 0; i < graph[h].Count; i++)
                    //    {
                    //        str += graph[h][i] + "-";
                    //    }

                    //    Debug.Log("Entry: " + str);
                    //}

                    //Debug.Log("Print all cycle: ");
                    //for (int h = 0; h < cycles.Count; h++)
                    //{
                    //    string str = "";

                    //    for (int i = 0; i < cycles[h].Count; i++)
                    //    {
                    //        str += cycles[h][i] + "-";
                    //    }

                    //    Debug.Log("Cycle: " + str);
                    //}


                    allowOptimization = true;
                    Optimize(cycles);
                }

                count = 0;
            }

            count++;
        }
    }