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