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);
}
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++;
}
}
