public override RoadNetwork.Trajectory match(RoadNetwork.Trajectory trajectory)
{
this.trj = trajectory;
double cellSize = 50;
GridTrj trjIndex = new GridTrj(trj, cellSize);
SortedSet <TrjTreeNode> openTable = new SortedSet <TrjTreeNode>(new TrjTreeNodeComparer());
List <TrjTreeNode> closeTable = new List <TrjTreeNode>();
int trjCount = trj.Count;
int currentIdx = 0;
while (currentIdx < trjCount - 1)
{
//1. get initial candidates
if (openTable.Count == 0)
{
currentIdx = getInitialCands(openTable, currentIdx);
}
//2. Create candidate paths
TrjTreeNode currentNode = null;
while (openTable.Count > 0)
{
Debug.Assert(currentIdx < trjCount);
//2.1.get the first node
currentNode = openTable.Min;
openTable.Remove(currentNode);
closeTable.Add(currentNode);
//2.2. create candidate paths
List <TrjTreeNode> cands = createCands(trjIndex, currentNode);
foreach (var cand in cands)
{
//2.3. check parallel paths
if (openTable.Contains(cand))
{
var tempCand = openTable.Where((n) => n == cand);
Trace.Assert(tempCand.Count() == 1, "More than two parallel paths were found!");
TrjTreeNode bestCand = getBestPath(cand, tempCand.First());
openTable.Add(bestCand);
}
else
{
openTable.Add(cand);
}
}
//2.3 prune and confirm
if (openTable.Count > 3)
{
openTable = pruneConfirm(openTable);
}
currentIdx = currentNode.idx + 1; //for breaking the loop
}
}
//3. Get the matched result
Trajectory newTrj = getMatchedResult(closeTable);
return(newTrj);
}
/// <summary>
/// Create candidate paths
/// </summary>
/// <param name="trjIndex"></param>
/// <param name="currentNode"></param>
/// <returns></returns>
private List <TrjTreeNode> createCands(GridTrj trjIndex, TrjTreeNode currentNode)
{
//1.get candidate paths for the current node
List <EdgePath> paths = buildPath(currentNode);
List <TrjTreeNode> cands = new List <TrjTreeNode>();
List <EdgePath> newPaths = new List <EdgePath>();
double baseDist = GPS_ERROR_DISTANCE;
int currentIdx = currentNode.idx;
//2.find the trajectory point near the end vertex of the path
foreach (EdgePath path in paths)
{
GeoPoint endPoint = path.End.Point;
HashSet <int> idxs = trjIndex.RangeQuery(endPoint, baseDist);
if (idxs.Count > 0)
{
double minDist = baseDist;
int bestIdx = -1;
foreach (int idx in idxs)
{
if (idx >= currentIdx)
{
//Filter by real distance
double distance = Polyline.DistFrom(trj[idx].point, trj[idx + 1].point, endPoint);
if (distance <= minDist)
{
minDist = distance;
bestIdx = idx;
}
}
}
if (bestIdx >= 0)
{
//Create new node
TrjTreeNode node = new TrjTreeNode(currentNode);
node.idx = bestIdx;
node.path = path;
node.v = path.End;
cands.Add(node);
}
}
}
return(cands);
}
