WaySublineMatchString MaximalNearestSublineMatcher::findMatch(const ConstOsmMapPtr &map,
const ConstWayPtr& way1, const ConstWayPtr &way2, double &score, Meters maxRelevantDistance) const
{
score = 0;
Meters mrd = maxRelevantDistance == -1 ? way1->getCircularError() + way2->getCircularError() :
maxRelevantDistance;
vector<long> wayIds;
wayIds.push_back(way1->getId());
wayIds.push_back(way2->getId());
OsmMapPtr mapCopy(map->copyWays(wayIds));
WayPtr way1NonConst = mapCopy->getWay(way1->getId());
WayPtr way2NonConst = mapCopy->getWay(way2->getId());
MaximalNearestSubline mns1(
mapCopy, way1NonConst, way2NonConst, _minSplitSize, mrd, _maxRelevantAngle, _headingDelta);
// use the maximal nearest subline code to find the best subline
std::vector<WayLocation> interval1 = mns1.getInterval();
if (!interval1[0].isValid() || !interval1[1].isValid() ||
interval1[0] == interval1[1])
{
// if the interval isn't valid then return an invalid result.
return WaySublineMatchString();
}
_snapToEnds(map, interval1);
WayPtr subline1 = WaySubline(interval1[0], interval1[1]).toWay(mapCopy);
MaximalNearestSubline mns2(mapCopy, way2NonConst, subline1, _minSplitSize, -1, -1, _headingDelta);
std::vector<WayLocation> interval2 = mns2.getInterval();
if (!interval2[0].isValid() || !interval2[1].isValid() ||
interval2[0] == interval2[1])
{
return WaySublineMatchString();
}
_snapToEnds(map, interval2);
WaySublineMatch match = WaySublineMatch(WaySubline(interval1[0], interval1[1]),
WaySubline(interval2[0], interval2[1]));
if (subline1->getNodeCount() > 1)
{
shared_ptr<LineString> ls = ElementConverter(mapCopy).convertToLineString(subline1);
if (ls->isValid())
{
score = ls->getLength();
}
}
vector<WaySublineMatch> v;
// switch the subline match to reference a different map.
v.push_back(WaySublineMatch(match, map));
return WaySublineMatchString(v);
}
WaySublineMatchString::WaySublineMatchString(const WaySublineMatchString& other,
const OsmMapPtr& newMap)
{
_matches.resize(other._matches.size());
for (size_t i = 0; i < other.getMatches().size(); i++)
{
_matches[i] = WaySublineMatch(other.getMatches()[i], newMap);
}
}
vector<WaySublineMatch> MaximalSubline::_extractAllMatches(const ConstOsmMapPtr &map,
const ConstWayPtr& w1, const ConstWayPtr& w2, Sparse2dMatrix& sublineMatrix)
{
vector<Sparse2dCellId> endMatches = _findEndMatches(sublineMatrix);
vector<Sparse2dCellId> startMatches(endMatches.size());
for (size_t i = 0; i < endMatches.size(); i++)
{
startMatches[i] = _findStartMatch(sublineMatrix, endMatches[i]);
}
vector<WaySublineMatch> result;
result.reserve(endMatches.size());
// calculate the way locations for each subline on each way.
for (size_t i = 0; i < endMatches.size(); i++)
{
WayLocation start1 = _calculateStartWayLocation(map, w1, w2, startMatches[i].row(),
startMatches[i].col());
WayLocation start2 = _calculateStartWayLocation(map, w2, w1, startMatches[i].col(),
startMatches[i].row());
_snapToStart(start1);
_snapToStart(start2);
WayLocation end1 = _calculateEndWayLocation(map, w1, w2, endMatches[i].row(),
endMatches[i].col());
WayLocation end2 = _calculateEndWayLocation(map, w2, w1, endMatches[i].col(),
endMatches[i].row());
_snapToEnd(end1);
_snapToEnd(end2);
WaySubline ws1(start1, end1);
WaySubline ws2(start2, end2);
if (ws1.isValid() && ws1.isZeroLength() == false &&
ws2.isValid() && ws2.isZeroLength() == false)
{
result.push_back(WaySublineMatch(ws1, ws2));
}
}
return result;
}
/// @todo this is in desperate need of a rewrite by someone with more rest than myself. -JRS
vector<WaySublineMatch> MaximalSubline::_snapIntersections(const ConstOsmMapPtr& map,
const ConstWayPtr& w1, const ConstWayPtr& w2, vector<WaySublineMatch>& rawSublineMatches)
{
// this only works if the rawSublineMatches are in order. We order by subline1
sort(rawSublineMatches.begin(), rawSublineMatches.end(), lessThan);
// make sure that ordering by subline1 results in sorted subline2s. If this isn't the case then
// there isn't much we can do.
for (size_t i = 2; i < rawSublineMatches.size(); i++)
{
if (rawSublineMatches[i].getSubline2().getStart() >
rawSublineMatches[i - 1].getSubline2().getStart())
{
LOG_WARN("Way matches sublines out of order. This is unusual and may give a sub-optimal "
"result.");
return rawSublineMatches;
}
}
for (size_t i = 0; i < rawSublineMatches.size(); i++)
{
// if any of the raw sublines are crazy small, then don't try to snap the intersections.
if (rawSublineMatches[i].getSubline1().getLength() < _spacing * 2 ||
rawSublineMatches[i].getSubline2().getLength() < _spacing * 2)
{
return rawSublineMatches;
}
}
////
// calculate a series of point pair matches along the lines.
////
// discretize the first line into a series of points.
vector< pair<WayLocation, WayLocation> > pairs;
pairs = _discretizePointPairs(map, w1, w2, rawSublineMatches);
assert(pairs.size() > 0);
//LOG_DEBUG_VAR(pairs);
// extract features on the point pairs and populate a matrix.
Meters acc = w1->getCircularError() + w2->getCircularError();
cv::Mat m(pairs.size(), 2, CV_64F);
size_t currentSubline = 0;
vector<int> starts(rawSublineMatches.size(), numeric_limits<int>::max());
vector<int> ends(rawSublineMatches.size(), 0);
for (size_t i = 0; i < pairs.size(); i++)
{
WayLocation& wl1 = pairs[i].first;
WayLocation& wl2 = pairs[i].second;
// If the rawSublineMatches is smaller than _spacing, then it may not directly overlap with
// one of the point pairs. To avoid this, we create a subline that surrounds the point pair
// and will guarantee that each rawSublineMatches will touch at least one point pair.
WaySubline ws1 = WaySubline(wl1.move(-_spacing / 2.0), wl1.move(_spacing / 2.0));
WaySubline ws2 = WaySubline(wl2.move(-_spacing / 2.0), wl2.move(_spacing / 2.0));
if (currentSubline < rawSublineMatches.size())
{
// figure out the first and last match for this subline.
if (rawSublineMatches[currentSubline].getSubline1().touches(ws1) ||
rawSublineMatches[currentSubline].getSubline2().touches(ws2))
{
starts[currentSubline] = min<int>(starts[currentSubline], i);
ends[currentSubline] = max<int>(ends[currentSubline], i);
}
else
{
// if this is past the current subline, advance to the right subline.
while (currentSubline < rawSublineMatches.size() &&
rawSublineMatches[currentSubline].getSubline1().getEnd() < ws1.getStart() &&
rawSublineMatches[currentSubline].getSubline2().getEnd() < ws2.getStart())
{
currentSubline++;
}
}
}
Meters distance = wl1.getCoordinate().distance(wl2.getCoordinate());
Radians angle1 = WayHeading::calculateHeading(wl1);
Radians angle2 = WayHeading::calculateHeading(wl2);
Radians angleDiff = WayHeading::deltaMagnitude(angle1, angle2);
m.at<double>(i, 0) = distance / acc;
m.at<double>(i, 1) = angleDiff;
}
//LOG_DEBUG("starts: " << starts);
//LOG_DEBUG("ends: " << ends);
// create the matrix of constraints.
vector<int> finalStarts;
vector<int> finalEnds;
if (starts[0] != 0)
{
finalStarts.push_back(0);
finalEnds.push_back(starts[0] + (ends[0] - starts[0]) / 3);
}
// this maps finalStarts indexes to the rawSublineMatches indexes. E.g.
// rawSublineMatches[i] maps to finalStarts[matchIndexes[i]]
vector<int> matchIndexes(starts.size());
for (size_t i = 0; i < starts.size(); i++)
{
if (starts[i] == numeric_limits<int>::max())
{
// Due to poor subline pair matching we cannot properly snap these intersections. Warn the
// user and move on. It is likely they aren't a good match anyways.
LOG_WARN("A solid set of point pair matches could not be found.");
return rawSublineMatches;
}
matchIndexes[i] = finalStarts.size();
finalStarts.push_back(starts[i]);
finalEnds.push_back(ends[i]);
if (i != starts.size() - 1)
{
finalStarts.push_back(ends[i] - (ends[i] - starts[i]) / 3);
finalEnds.push_back(starts[i + 1] + (ends[i + 1] - starts[i + 1]) / 3);
}
}
int last = ends.size() - 1;
if ((size_t)ends[last] != pairs.size() - 1)
{
finalStarts.push_back(ends[last] - (ends[last] - starts[last]) / 3);
finalEnds.push_back(pairs.size() - 1);
}
//LOG_DEBUG("finalStarts: " << finalStarts);
//LOG_DEBUG("finalEnds: " << finalEnds);
Mat ranges(finalStarts.size(), 2, CV_32S);
for (size_t i = 0; i < finalStarts.size(); i++)
{
ranges.at<int>(i, 0) = finalStarts[i];
ranges.at<int>(i, 1) = finalEnds[i];
}
// run ExpectationIntersection to determine new intersection points.
ExpectationIntersection ei;
vector<double> splits = ei.snapMatches(m, ranges);
//LOG_DEBUG_VAR(splits);
vector<WaySublineMatch> result = rawSublineMatches;
for (size_t i = 0; i < matchIndexes.size(); i++)
{
size_t mi = matchIndexes[i];
WayLocation w1Start;
WayLocation w2Start;
//LOG_DEBUG(rawSublineMatches[i]);
// if this is the first subline, then it starts at the beginning of the subline.
if (matchIndexes[i] == 0)
{
w1Start = WayLocation(rawSublineMatches[i].getSubline1().getStart());
w2Start = WayLocation(rawSublineMatches[i].getSubline2().getStart());
}
else
{
int wi = (int)splits[mi - 1];
//LOG_DEBUG("start split: " << wi);
double r = splits[mi - 1] - wi;
Meters offset1 = pairs[wi].first.calculateDistanceOnWay() * r +
pairs[wi + 1].first.calculateDistanceOnWay() * (1 - r);
Meters offset2 = pairs[wi].second.calculateDistanceOnWay() * r +
pairs[wi + 1].second.calculateDistanceOnWay() * (1 - r);
//LOG_DEBUG("offset1: " << offset1 << " r: " << r);
//LOG_DEBUG("offset2: " << offset2 << " r: " << r);
w1Start = WayLocation(map, w1, offset1);
w2Start = WayLocation(map, w2, offset2);
}
//LOG_DEBUG_VAR(w1Start);
//LOG_DEBUG_VAR(w2Start);
// if we are passed the point where we have a node pairing, then snap it back to a legit pair.
if (w1Start < pairs.front().first.move(-_spacing))
{
w1Start = pairs.front().first;
}
if (w2Start < pairs.front().second.move(-_spacing))
{
w2Start = pairs.front().second;
}
//LOG_DEBUG_VAR(w1Start);
//LOG_DEBUG_VAR(w2Start);
WayLocation w1End(rawSublineMatches[i].getSubline1().getEnd());
WayLocation w2End(rawSublineMatches[i].getSubline2().getEnd());
// convert the end split location into a WayLocation
if (matchIndexes[i] < (int)splits.size())
{
int wi = (int)splits[mi];
//LOG_DEBUG("end split: " << wi << " matchIndexes[" << i << "]: " << mi);
double r = splits[mi] - wi;
Meters offset1 = pairs[wi].first.calculateDistanceOnWay() * r +
pairs[wi + 1].first.calculateDistanceOnWay() * (1 - r);
Meters offset2 = pairs[wi].second.calculateDistanceOnWay() * r +
pairs[wi + 1].second.calculateDistanceOnWay() * (1 - r);
w1End = WayLocation(map, w1, offset1);
w2End = WayLocation(map, w2, offset2);
//LOG_DEBUG("offset1: " << offset1 << " r: " << r);
//LOG_DEBUG("offset2: " << offset2 << " r: " << r);
}
// if we are passed the point where we have a node pairing, then snap it back to a legit pair.
//LOG_DEBUG_VAR(w1End);
//LOG_DEBUG_VAR(pairs.back().first.move(_spacing));
if (w1End > pairs.back().first.move(_spacing))
{
w1End = pairs.back().first;
}
if (w2End > pairs.back().second.move(_spacing))
{
w2End = pairs.back().second;
}
//LOG_DEBUG("w1End: " << w1End.toString());
//LOG_DEBUG("w2End: " << w2End.toString());
WaySubline ws1Expanded = rawSublineMatches[i].getSubline1().expand(
max(_minSplitSize, _spacing));
WaySubline ws2Expanded = rawSublineMatches[i].getSubline2().expand(
max(_minSplitSize, _spacing));
if (ws1Expanded.contains(w1Start) == false ||
ws2Expanded.contains(w2Start) == false ||
ws1Expanded.contains(w1End) == false ||
ws2Expanded.contains(w2End) == false ||
w1Start > w1End ||
w2Start > w2End)
{
LOG_DEBUG("Point pair matching failed, skipping intersection snapping.");
return rawSublineMatches;
}
// snap to the start if we're within spacing distance.
_snapToStart(w1Start, max(_minSplitSize, _spacing));
_snapToStart(w2Start, max(_minSplitSize, _spacing));
// snap to the end if we're within spacing distance.
_snapToEnd(w1End, max(_minSplitSize, _spacing));
_snapToEnd(w2End, max(_minSplitSize, _spacing));
//LOG_DEBUG_VAR(w1End);
//LOG_DEBUG_VAR(w2End);
WaySubline ws1(w1Start, w1End);
WaySubline ws2(w2Start, w2End);
result[i] = WaySublineMatch(ws1, ws2);
}
// Put the updated intersection points into the result.
return result;
}
MaximalSublineStringMatcher::ScoredMatch MaximalSublineStringMatcher::_evaluateMatch(
const ConstOsmMapPtr &map, Meters maxDistance, const vector<ConstWayPtr>& ways1,
const vector<ConstWayPtr> &ways2, const vector<bool>& reversed1,
const vector<bool> &reversed2) const
{
vector<WaySublineMatch> matches;
// make a copy of the map and the ways we need so we can reverse the ways as needed.
set<ElementId> eids;
_insertElementIds(ways1, eids);
_insertElementIds(ways2, eids);
OsmMapPtr copiedMap(new OsmMap(map->getProjection()));
CopySubsetOp(map, eids).apply(copiedMap);
vector<WayPtr> prep1 = _changeMap(ways1, copiedMap);
vector<WayPtr> prep2 = _changeMap(ways2, copiedMap);
// reversed ways as appropriate
_reverseWays(prep1, reversed1);
_reverseWays(prep2, reversed2);
double scoreSum = 0;
// go through and match each way against every other way
for (size_t i = 0; i < prep1.size(); i++)
{
for (size_t j = 0; j < prep2.size(); j++)
{
double score;
WaySublineMatchString m = _sublineMatcher->findMatch(copiedMap, prep1[i], prep2[j], score,
maxDistance);
scoreSum += score;
matches.insert(matches.end(), m.getMatches().begin(), m.getMatches().end());
}
}
HashMap<long, bool> wayIdToReversed1, wayIdToReversed2;
// create a map from way id to reverse status
for (size_t i = 0; i < prep1.size(); i++)
{
wayIdToReversed1[prep1[i]->getId()] = reversed1[i];
}
for (size_t i = 0; i < prep2.size(); i++)
{
wayIdToReversed2[prep2[i]->getId()] = reversed2[i];
}
// go through all the matches
for (size_t i = 0; i < matches.size(); i++)
{
WaySubline ws1, ws2;
// if the direction is reversed on one but not both ways then mark the match as reversed.
long m1Id = matches[i].getSubline1().getStart().getWay()->getId();
long m2Id = matches[i].getSubline2().getStart().getWay()->getId();
if (wayIdToReversed1[m1Id])
{
// make sure the way subline is pointed to the right way (not reversed)
ConstWayPtr w = map->getWay(matches[i].getSubline1().getElementId());
ws1 = matches[i].getSubline1().reverse(w);
}
else
{
ws1 = WaySubline(matches[i].getSubline1(), map);
}
if (wayIdToReversed2[m2Id])
{
// make sure the way subline is pointed to the right way (not reversed)
ConstWayPtr w = map->getWay(matches[i].getSubline2().getElementId());
ws2 = matches[i].getSubline2().reverse(w);
}
else
{
ws2 = WaySubline(matches[i].getSubline2(), map);
}
if (wayIdToReversed1[m1Id] != wayIdToReversed2[m2Id])
{
matches[i] = WaySublineMatch(ws1, ws2, true);
}
else
{
matches[i] = WaySublineMatch(ws1, ws2, false);
}
}
return ScoredMatch(scoreSum, matches);
}
