private void MakeConnectionsAndConnectionPlaceholdersFor(SlamObservation observation)
{
foreach (var covisible in observation.CovisibleInfos)
{
int connectionIdx = m_connections.FindIndex(c =>
c.obsId1 == observation.Point.id && c.obsId2 == covisible.id ||
c.obsId2 == observation.Point.id && c.obsId1 == covisible.id);
if (connectionIdx == -1)
{
connectionIdx = m_connections.Count;
m_connections.Add(new Connection(observation.Point.id, covisible.id, observation));
}
var connection = m_connections[connectionIdx];
SlamObservation covisibleObs = m_nodes.FirstOrDefault(node => node.Point.id == covisible.id);
if (
/*already in map*/ covisibleObs != null &&
/*is placeholder*/ covisibleObs.Point.id == connection.obsId2 && connection.lineId == -1)
{
SlamLine line = new SlamLine()
{
vert1 = observation.Point.position,
vert2 = covisibleObs.Point.position,
pointId1 = observation.Point.id,
pointId2 = covisibleObs.Point.id,
color1 = observation.Point.color,
color2 = covisibleObs.Point.color,
isRemoved = false
};
m_connections[connectionIdx] = new Connection(
connection.obsId1, connection.obsId2,
observation, covisibleObs,
m_lines.Add(line));
}
//Debug.Assert(m_connections[connectionIdx].first != null || m_connections[connectionIdx].second != null,
// $"[SlamObservationsContainer.UpdateConnectionsOf] connection.first == connection.second == null for " +
// $"id1 = {m_connections[connectionIdx].obsId1}, " +
// $"id2 = {m_connections[connectionIdx].obsId2}");
if (!(m_connections[connectionIdx].first != null || m_connections[connectionIdx].second != null))
{
throw new InvalidSlamContainerOperationException(
$"[SlamObservationsContainer.UpdateConnectionsOf] connection.first == connection.second == null for " +
$"id1 = {m_connections[connectionIdx].obsId1}, " +
$"id2 = {m_connections[connectionIdx].obsId2}");
}
}
}
private void UpdateConnectionVerticesFor(SlamObservation observation)
{
var allConnectionsOfArg = m_connections.Where(con => (con.first == observation || con.second == observation) && con.lineId != -1);
foreach (var connection in allConnectionsOfArg)
{
SlamLine line = m_lines[connection.lineId];
if (connection.first == observation)
{
line.vert1 = connection.first.Point.position;
line.vert2 = connection.second.Point.position;
}
else
{
line.vert2 = connection.second.Point.position;
line.vert1 = connection.first.Point.position;
}
m_lines.Update(line);
}
}
private void ContinueTrack(Vector3 vert1, Vector3 vert2)
{
if (vert1 == vert2)
{
m_lineIdsHistory.Push(-1);
return;
}
SlamLine line = new SlamLine()
{
color1 = color,
isRemoved = false,
pointId1 = m_lineSegmentIdx,
pointId2 = ++m_lineSegmentIdx,
vert1 = vert1,
vert2 = vert2,
};
m_lastLineId = m_linesContainer.Add(line);
m_lineIdsHistory.Push(m_lastLineId);
m_linesContainer.Repaint();
}
private void ReplaceConnectionPlaceholdersFor(SlamObservation observation)
{
for (int i = 0; i < m_connections.Count; ++i)
{
if (m_connections[i].obsId2 == observation.Point.id && m_connections[i].second == null)
{
SlamLine line = new SlamLine()
{
vert1 = m_connections[i].first.Point.position,
vert2 = observation.Point.position,
pointId1 = m_connections[i].first.Point.id,
pointId2 = observation.Point.id,
color1 = m_connections[i].first.Point.color,
color2 = observation.Point.color,
isRemoved = false
};
m_connections[i] = new Connection(
m_connections[i].obsId1, m_connections[i].obsId2,
m_connections[i].first, observation,
m_lines.Add(line));
}
}
}
private void AddWithHistory()
{
var messages = this.FindAllPreviousMessages()
.Select(m => MessageParser.Parse(m.Data, Topic.Type, true) !.GetPose() !.ToUnity())
.ToList();
if (messages.Count == 0)
{
return;
}
var obj = new SlamTrackedObject(0, messages.Last().Item1, messages.Last().Item2);
var history = new SlamLine[messages.Count - 1];
for (int i = 0; i < messages.Count - 1; i++)
{
history[i] = new SlamLine(new SlamPoint(i, messages[i].Item1, Color.black),
new SlamPoint(i + 1, messages[i + 1].Item1, Color.black),
i);
}
AddWithHistory(obj, history);
}
public static LinePb ToProtobuf(this SlamLine l, ICSConverter converter)
=> new LinePb
{
Pt1 = l.Point1.ToProtobuf(converter), Pt2 = l.Point2.ToProtobuf(converter)
};
