d96960f9cc
This PR improves routing results by adding support for snapping to multiple ways at input locations. This means all edges at the snapped location can act as source/target candidates for routing search, ensuring we always find the best route, and not the one dependent on the edge selected.
320 lines
13 KiB
C++
320 lines
13 KiB
C++
#include "engine/plugins/match.hpp"
|
|
#include "engine/plugins/plugin_base.hpp"
|
|
|
|
#include "engine/api/match_api.hpp"
|
|
#include "engine/api/match_parameters.hpp"
|
|
#include "engine/api/match_parameters_tidy.hpp"
|
|
#include "engine/map_matching/sub_matching.hpp"
|
|
#include "util/coordinate_calculation.hpp"
|
|
#include "util/integer_range.hpp"
|
|
|
|
#include <cstdlib>
|
|
|
|
#include <algorithm>
|
|
#include <functional>
|
|
#include <memory>
|
|
#include <set>
|
|
#include <vector>
|
|
|
|
namespace osrm
|
|
{
|
|
namespace engine
|
|
{
|
|
namespace plugins
|
|
{
|
|
|
|
// Filters PhantomNodes to obtain a set of viable candidates
|
|
void filterCandidates(const std::vector<util::Coordinate> &coordinates,
|
|
MatchPlugin::CandidateLists &candidates_lists)
|
|
{
|
|
for (const auto current_coordinate : util::irange<std::size_t>(0, coordinates.size()))
|
|
{
|
|
bool allow_uturn = false;
|
|
|
|
if (coordinates.size() - 1 > current_coordinate && 0 < current_coordinate)
|
|
{
|
|
double turn_angle =
|
|
util::coordinate_calculation::computeAngle(coordinates[current_coordinate - 1],
|
|
coordinates[current_coordinate],
|
|
coordinates[current_coordinate + 1]);
|
|
|
|
// sharp turns indicate a possible uturn
|
|
if (turn_angle <= 45.0 || turn_angle >= 315.0)
|
|
{
|
|
allow_uturn = true;
|
|
}
|
|
}
|
|
|
|
auto &candidates = candidates_lists[current_coordinate];
|
|
if (candidates.empty())
|
|
{
|
|
continue;
|
|
}
|
|
|
|
// sort by forward id, then by reverse id and then by distance
|
|
std::sort(candidates.begin(),
|
|
candidates.end(),
|
|
[](const PhantomNodeWithDistance &lhs, const PhantomNodeWithDistance &rhs) {
|
|
return lhs.phantom_node.forward_segment_id.id <
|
|
rhs.phantom_node.forward_segment_id.id ||
|
|
(lhs.phantom_node.forward_segment_id.id ==
|
|
rhs.phantom_node.forward_segment_id.id &&
|
|
(lhs.phantom_node.reverse_segment_id.id <
|
|
rhs.phantom_node.reverse_segment_id.id ||
|
|
(lhs.phantom_node.reverse_segment_id.id ==
|
|
rhs.phantom_node.reverse_segment_id.id &&
|
|
lhs.distance < rhs.distance)));
|
|
});
|
|
|
|
auto new_end =
|
|
std::unique(candidates.begin(),
|
|
candidates.end(),
|
|
[](const PhantomNodeWithDistance &lhs, const PhantomNodeWithDistance &rhs) {
|
|
return lhs.phantom_node.forward_segment_id.id ==
|
|
rhs.phantom_node.forward_segment_id.id &&
|
|
lhs.phantom_node.reverse_segment_id.id ==
|
|
rhs.phantom_node.reverse_segment_id.id;
|
|
});
|
|
candidates.resize(new_end - candidates.begin());
|
|
|
|
if (!allow_uturn)
|
|
{
|
|
const auto compact_size = candidates.size();
|
|
for (const auto i : util::irange<std::size_t>(0, compact_size))
|
|
{
|
|
// Split edge if it is bidirectional and append reverse direction to end of list
|
|
if (candidates[i].phantom_node.forward_segment_id.enabled &&
|
|
candidates[i].phantom_node.reverse_segment_id.enabled)
|
|
{
|
|
PhantomNode reverse_node(candidates[i].phantom_node);
|
|
reverse_node.forward_segment_id.enabled = false;
|
|
candidates.push_back(
|
|
PhantomNodeWithDistance{reverse_node, candidates[i].distance});
|
|
|
|
candidates[i].phantom_node.reverse_segment_id.enabled = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
// sort by distance to make pruning effective
|
|
std::sort(candidates.begin(),
|
|
candidates.end(),
|
|
[](const PhantomNodeWithDistance &lhs, const PhantomNodeWithDistance &rhs) {
|
|
return lhs.distance < rhs.distance;
|
|
});
|
|
}
|
|
}
|
|
|
|
Status MatchPlugin::HandleRequest(const RoutingAlgorithmsInterface &algorithms,
|
|
const api::MatchParameters ¶meters,
|
|
osrm::engine::api::ResultT &result) const
|
|
{
|
|
if (!algorithms.HasMapMatching())
|
|
{
|
|
return Error("NotImplemented",
|
|
"Map matching is not implemented for the chosen search algorithm.",
|
|
result);
|
|
}
|
|
|
|
if (!CheckAlgorithms(parameters, algorithms, result))
|
|
return Status::Error;
|
|
|
|
const auto &facade = algorithms.GetFacade();
|
|
|
|
BOOST_ASSERT(parameters.IsValid());
|
|
|
|
// enforce maximum number of locations for performance reasons
|
|
if (max_locations_map_matching > 0 &&
|
|
static_cast<int>(parameters.coordinates.size()) > max_locations_map_matching)
|
|
{
|
|
return Error("TooBig", "Too many trace coordinates", result);
|
|
}
|
|
|
|
if (!CheckAllCoordinates(parameters.coordinates))
|
|
{
|
|
return Error("InvalidValue", "Invalid coordinate value.", result);
|
|
}
|
|
|
|
if (max_radius_map_matching > 0 && std::any_of(parameters.radiuses.begin(),
|
|
parameters.radiuses.end(),
|
|
[&](const auto &radius) {
|
|
if (!radius)
|
|
return false;
|
|
return *radius > max_radius_map_matching;
|
|
}))
|
|
{
|
|
return Error("TooBig", "Radius search size is too large for map matching.", result);
|
|
}
|
|
|
|
// Check for same or increasing timestamps. Impl. note: Incontrast to `sort(first,
|
|
// last, less_equal)` checking `greater` in reverse meets irreflexive requirements.
|
|
const auto time_increases_monotonically = std::is_sorted(
|
|
parameters.timestamps.rbegin(), parameters.timestamps.rend(), std::greater<>{});
|
|
|
|
if (!time_increases_monotonically)
|
|
{
|
|
return Error("InvalidValue", "Timestamps need to be monotonically increasing.", result);
|
|
}
|
|
|
|
SubMatchingList sub_matchings;
|
|
api::tidy::Result tidied;
|
|
if (parameters.tidy)
|
|
{
|
|
// Transparently tidy match parameters, do map matching on tidied parameters.
|
|
// Then use the mapping to restore the original <-> tidied relationship.
|
|
tidied = api::tidy::tidy(parameters);
|
|
}
|
|
else
|
|
{
|
|
tidied = api::tidy::keep_all(parameters);
|
|
}
|
|
|
|
// Error: first and last points should be waypoints
|
|
if (!parameters.waypoints.empty() &&
|
|
(tidied.parameters.waypoints[0] != 0 ||
|
|
tidied.parameters.waypoints.back() != (tidied.parameters.coordinates.size() - 1)))
|
|
{
|
|
return Error(
|
|
"InvalidValue", "First and last coordinates must be specified as waypoints.", result);
|
|
}
|
|
|
|
// assuming radius is the standard deviation of a normal distribution
|
|
// that models GPS noise (in this model), x3 should give us the correct
|
|
// search radius with > 99% confidence
|
|
std::vector<double> search_radiuses;
|
|
if (tidied.parameters.radiuses.empty())
|
|
{
|
|
search_radiuses.resize(tidied.parameters.coordinates.size(),
|
|
routing_algorithms::DEFAULT_GPS_PRECISION * RADIUS_MULTIPLIER);
|
|
}
|
|
else
|
|
{
|
|
search_radiuses.resize(tidied.parameters.coordinates.size());
|
|
std::transform(tidied.parameters.radiuses.begin(),
|
|
tidied.parameters.radiuses.end(),
|
|
search_radiuses.begin(),
|
|
[](const boost::optional<double> &maybe_radius) {
|
|
if (maybe_radius)
|
|
{
|
|
return *maybe_radius * RADIUS_MULTIPLIER;
|
|
}
|
|
else
|
|
{
|
|
return routing_algorithms::DEFAULT_GPS_PRECISION * RADIUS_MULTIPLIER;
|
|
}
|
|
});
|
|
}
|
|
|
|
auto candidates_lists =
|
|
GetPhantomNodesInRange(facade, tidied.parameters, search_radiuses, true);
|
|
|
|
filterCandidates(tidied.parameters.coordinates, candidates_lists);
|
|
if (std::all_of(candidates_lists.begin(),
|
|
candidates_lists.end(),
|
|
[](const std::vector<PhantomNodeWithDistance> &candidates) {
|
|
return candidates.empty();
|
|
}))
|
|
{
|
|
return Error("NoSegment",
|
|
std::string("Could not find a matching segment for any coordinate."),
|
|
result);
|
|
}
|
|
|
|
// call the actual map matching
|
|
sub_matchings =
|
|
algorithms.MapMatching(candidates_lists,
|
|
tidied.parameters.coordinates,
|
|
tidied.parameters.timestamps,
|
|
tidied.parameters.radiuses,
|
|
parameters.gaps == api::MatchParameters::GapsType::Split);
|
|
|
|
if (sub_matchings.size() == 0)
|
|
{
|
|
return Error("NoMatch", "Could not match the trace.", result);
|
|
}
|
|
|
|
// trace was split, we don't support the waypoints parameter across multiple match objects
|
|
if (sub_matchings.size() > 1 && !parameters.waypoints.empty())
|
|
{
|
|
return Error("NoMatch", "Could not match the trace with the given waypoints.", result);
|
|
}
|
|
|
|
// Error: Check if user-supplied waypoints can be found in the resulting matches
|
|
if (!parameters.waypoints.empty())
|
|
{
|
|
std::set<std::size_t> tidied_waypoints(tidied.parameters.waypoints.begin(),
|
|
tidied.parameters.waypoints.end());
|
|
for (const auto &sm : sub_matchings)
|
|
{
|
|
std::for_each(sm.indices.begin(),
|
|
sm.indices.end(),
|
|
[&tidied_waypoints](const auto index) { tidied_waypoints.erase(index); });
|
|
}
|
|
if (!tidied_waypoints.empty())
|
|
{
|
|
return Error("NoMatch", "Requested waypoint parameter could not be matched.", result);
|
|
}
|
|
}
|
|
// we haven't errored yet, only allow leg collapsing if it was originally requested
|
|
BOOST_ASSERT(parameters.waypoints.empty() || sub_matchings.size() == 1);
|
|
const auto collapse_legs = !parameters.waypoints.empty();
|
|
|
|
// each sub_route will correspond to a MatchObject
|
|
std::vector<InternalRouteResult> sub_routes(sub_matchings.size());
|
|
for (auto index : util::irange<std::size_t>(0UL, sub_matchings.size()))
|
|
{
|
|
BOOST_ASSERT(sub_matchings[index].nodes.size() > 1);
|
|
|
|
// FIXME we only run this to obtain the geometry
|
|
// The clean way would be to get this directly from the map matching plugin
|
|
for (unsigned i = 0; i < sub_matchings[index].nodes.size() - 1; ++i)
|
|
{
|
|
PhantomEndpoints current_endpoints{sub_matchings[index].nodes[i],
|
|
sub_matchings[index].nodes[i + 1]};
|
|
BOOST_ASSERT(current_endpoints.source_phantom.IsValid());
|
|
BOOST_ASSERT(current_endpoints.target_phantom.IsValid());
|
|
sub_routes[index].leg_endpoints.push_back(current_endpoints);
|
|
}
|
|
|
|
std::vector<PhantomNodeCandidates> waypoint_candidates;
|
|
waypoint_candidates.reserve(sub_matchings[index].nodes.size());
|
|
std::transform(sub_matchings[index].nodes.begin(),
|
|
sub_matchings[index].nodes.end(),
|
|
std::back_inserter(waypoint_candidates),
|
|
[](const auto &phantom) { return PhantomNodeCandidates{phantom}; });
|
|
|
|
// force uturns to be on
|
|
// we split the phantom nodes anyway and only have bi-directional phantom nodes for
|
|
// possible uturns
|
|
sub_routes[index] = algorithms.ShortestPathSearch(waypoint_candidates, {false});
|
|
BOOST_ASSERT(sub_routes[index].shortest_path_weight != INVALID_EDGE_WEIGHT);
|
|
if (collapse_legs)
|
|
{
|
|
std::vector<bool> waypoint_legs;
|
|
waypoint_legs.reserve(sub_matchings[index].indices.size());
|
|
for (unsigned i = 0, j = 0; i < sub_matchings[index].indices.size(); ++i)
|
|
{
|
|
auto current_wp = tidied.parameters.waypoints[j];
|
|
if (current_wp == sub_matchings[index].indices[i])
|
|
{
|
|
waypoint_legs.push_back(true);
|
|
++j;
|
|
}
|
|
else
|
|
{
|
|
waypoint_legs.push_back(false);
|
|
}
|
|
}
|
|
sub_routes[index] = CollapseInternalRouteResult(sub_routes[index], waypoint_legs);
|
|
}
|
|
}
|
|
|
|
api::MatchAPI match_api{facade, parameters, tidied};
|
|
match_api.MakeResponse(sub_matchings, sub_routes, result);
|
|
|
|
return Status::Ok;
|
|
}
|
|
} // namespace plugins
|
|
} // namespace engine
|
|
} // namespace osrm
|