private void Awake()
{
m_lastLineId = -1;
m_linesContainer = new SlamLinesContainer(GetComponentInChildren <FastLinesCloud>());
m_poseHistory = new Stack <Pose>();
m_lineIdsHistory = new Stack <int>();
}
public AddCommand(
ICloudObjectsContainer <SlamPoint> pointsContainer,
ICloudObjectsContainer <SlamLine> linesContainer,
ICloudObjectsContainer <SlamObservation> observationsContainer,
SlamPackage slamEvent)
{
m_pointsContainer = pointsContainer;
m_linesContainer = linesContainer;
m_observationsContainer = observationsContainer;
if (slamEvent.Points != null)
{
m_addedPoints = slamEvent.Points.Where(p => p.isNew).ToArray();
}
if (slamEvent.Lines != null)
{
m_addedLines = slamEvent.Lines.Where(l => !m_linesContainer.Exists(l)).ToArray();
for (int i = 0; i < m_addedLines.Length; ++i)
{
m_addedLines[i].isRemoved = true;
m_addedLines[i].vert1 = m_pointsContainer[m_addedLines[i].pointId1].position;
m_addedLines[i].vert2 = m_pointsContainer[m_addedLines[i].pointId2].position;
}
}
if (slamEvent.Observations != null)
{
m_addedObservations = slamEvent.Observations.Where(o => o.Point.isNew && o.Point.id != -1).ToArray();
}
}
public UpdateCommand(
ICloudObjectsContainer <SlamPoint> pointsContainer,
ICloudObjectsContainer <SlamObservation> observationsContainer,
IEnumerable <SlamPoint> points,
IEnumerable <SlamObservation> observations)
{
m_pointsContainer = pointsContainer;
m_observationsContainer = observationsContainer;
if (points != null)
{
m_points2Restore = points.Where(p => p.id != -1).Select(p => pointsContainer[p]).ToArray();
m_points2Update = points.Where(p => p.id != -1).ToArray();
}
if (observations != null)
{
m_observations2Restore = observations
.Where(o => o.Point.id != -1)
.Where(o => !o.Point.isRemoved)
.Select(o => observationsContainer[o])
.ToArray();
m_observations2Update = observations
.Where(o => o.Point.id != -1)
.Where(o => !o.Point.isRemoved)
.ToArray();
}
}
public void UpdateInfo(
SlamPackage package,
ICloudObjectsContainer <SlamPoint> pointsMap,
ICloudObjectsContainer <SlamObservation> graph)
{
listBoxWithSpecializedObjects.Clear();
if (package == null)
{
commonIformation.text = "";
return;
}
commonIformation.text = package.ToString();
SlamPoint[] specialPts = package.Points.Where(p => p.id != -1).Where(p => p.message != null).ToArray();
Vector3[] ptsPositionsFromMap = specialPts.Select(p => pointsMap[p].position).ToArray();
SlamObservation[] specialObs = package.Observations
.Where(o => o.Point.id != -1)
.Where(o => o.Point.message != null)
.ToArray();
Vector3[] obsPositionsFromMap = specialObs.Select(o => graph[o].Point.position).ToArray();
for (int i = 0; i < specialPts.Length; ++i)
{
var item = listBoxWithSpecializedObjects.Add() as SpecialInfoListBoxItem;
item.SetObject(specialPts[i].id, specialPts[i].ToString(), ptsPositionsFromMap[i], specialPts[i].message);
}
for (int i = 0; i < specialObs.Length; ++i)
{
var item = listBoxWithSpecializedObjects.Add() as SpecialInfoListBoxItem;
item.SetObject(specialObs[i].Point.id, specialObs[i].ToString(), obsPositionsFromMap[i], specialObs[i].Point.message);
}
}
public RemoveCommand(
ICloudObjectsContainer <SlamPoint> pointsContainer,
ICloudObjectsContainer <SlamLine> linesContainer,
ICloudObjectsContainer <SlamObservation> observationsContainer,
SlamPackage slamEvent)
{
m_pointsContainer = pointsContainer;
m_linesContainer = linesContainer;
m_observationsContainer = observationsContainer;
if (slamEvent.Points != null)
{
m_points2Remove = slamEvent.Points
.Where(p => p.id != -1)
.Where(p => p.isRemoved)
.Select(p => m_pointsContainer[p])
.ToArray();
}
if (slamEvent.Lines != null)
{
m_lines2Remove = slamEvent.Lines
.Where(l => l.isRemoved)
.Select(l => m_linesContainer[l])
.ToArray();
}
if (slamEvent.Observations != null)
{
m_observations2Remove = slamEvent.Observations
.Where(o => o.Point.id != -1)
.Where(o => o.Point.isRemoved)
.Select(o => m_observationsContainer[o])
.ToArray();
}
}
private void UpdateMap <T>(
ICollection <T> source,
Func <T, bool> isNewSelector,
Func <T, bool> isRemovedSelector,
Func <T, bool> justColoredSelector,
Func <T, bool> isValidSelector,
ICloudObjectsContainer <T> map)
{
if (source != null)
{
foreach (var element in source)
{
if (isValidSelector(element))
{
if (isNewSelector(element))
{
map.Add(element);
}
else if (isRemovedSelector(element))
{
map.Remove(element);
}
else if (justColoredSelector(element))
{
map.ChangeColor(element);
}
else
{
map.Update(element);
}
}
}
}
}
/// <param name="prefab">Desired prefab of observation</param>
/// <param name="lines">Lines cloud objects for connections drawing</param>
public SlamObservationsContainer(GameObject prefab, ICloudObjectsContainer <SlamLine> lines)
{
m_nodes = new List <SlamObservation>();
m_gameObjects = new Dictionary <int, GameObject>();
ObservationsPool = new ObjectPool(prefab);
m_lines = lines;
m_connections = new List <Connection>();
}
public ClearCommand(
ICloudObjectsContainer <SlamPoint> pointsContainer,
ICloudObjectsContainer <SlamLine> linesContainer,
ICloudObjectsContainer <SlamObservation> observationsContainer)
{
m_pointsContainer = pointsContainer;
m_linesContainer = linesContainer;
m_observationsContainer = observationsContainer;
m_undoLines = m_linesContainer.GetAll();
m_undoPoints = m_pointsContainer.GetAll();
m_undoObservations = m_observationsContainer.GetAll();
}
public static void TestExistent(
this SlamPackage package,
Func <SlamPoint, bool> filter,
ICloudObjectsContainer <SlamPoint> pointsContainer,
ICloudObjectsContainer <SlamObservation> graph)
{
if (package.Points != null)
{
var existentPoints = SlamObjectQuery(package.Points.Where(p => filter(p)), id => pointsContainer.Exists(id));
Test(package, existentPoints, "Point id {0} already exists. New: '{1}'. Removed: '{2}'");
}
if (package.Observations != null)
{
var existentObs = SlamObjectQuery(package.Observations.Select(o => (SlamPoint)o).Where(o => filter(o)), id => graph.Exists(id));
Test(package, existentObs, "Observation id {0} already exists. New: '{1}'. Removed: '{2}'");
}
}
public PostProcessingCommand(
ICloudObjectsContainer <SlamPoint> pointsContainer,
ICloudObjectsContainer <SlamLine> linesContainer,
ICloudObjectsContainer <SlamObservation> observationsContainer,
SlamPackage slamEvent)
{
IEnumerable <SlamPoint> points = null;
if (slamEvent.Points != null)
{
points = slamEvent.Points
.Where(p => p.id != -1)
.Select(p => pointsContainer[p])
.Select(p => { p.color = p.defaultColor; return(p); });
m_commands.Add(new UpdateCommand(pointsContainer, observationsContainer, points, null));
m_commands.Add(new RemoveCommand(pointsContainer, linesContainer, observationsContainer, slamEvent));
}
}
public static void TestNonExistent(
this SlamPackage package,
Func <SlamPoint, bool> filter,
ICloudObjectsContainer <SlamPoint> pointsContainer,
ICloudObjectsContainer <SlamObservation> graph)
{
#if DEBUG
if (package.Points != null)
{
var existentPoints = SlamObjectQuery(package.Points.Where(p => filter(p)), id => !pointsContainer.Exists(id));
ElektronikLogger.WrapDebug(() => Test(package, existentPoints, "Point id {0} doesn't exists. New: '{1}'. Removed: '{2}'"));
}
if (package.Observations != null)
{
var existentObs = SlamObjectQuery(package.Observations.Select(o => (SlamPoint)o).Where(o => filter(o)), id => !graph.Exists(id));
ElektronikLogger.WrapDebug(() => Test(package, existentObs, "Observation id {0} doesn't exists. New: '{1}'. Removed: '{2}'"));
}
#endif
}
public UpdateCommand(
ICloudObjectsContainer <SlamPoint> pointsContainer,
ICloudObjectsContainer <SlamObservation> graph,
SlamPackage slamEvent) : this(pointsContainer, graph, slamEvent.Points, slamEvent.Observations)
{
}