Open-Harbor/osrm-backend
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 3 Wiki Activity

First compiling version of map_match plugin

Browse Source
This commit is contained in:
Patrick Niklaus
2016-02-21 04:27:26 +01:00
parent e05a45b080
commit 35b098e656
19 changed files with 587 additions and 457 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine.cpp
+3 -4
View File
@@ -9,8 +9,8 @@
//#include "engine/plugins/timestamp.hpp"
//#include "engine/plugins/trip.hpp"
#include "engine/plugins/viaroute.hpp"
//#include "engine/plugins/match.hpp"
//#include "engine/plugins/tile.hpp"
#include "engine/plugins/match.hpp"
#include "engine/datafacade/datafacade_base.hpp"
#include "engine/datafacade/internal_datafacade.hpp"
@@ -147,7 +147,7 @@ Engine::Engine(EngineConfig &config)
table_plugin = create<TablePlugin>(*query_data_facade, config.max_locations_distance_table);
nearest_plugin = create<NearestPlugin>(*query_data_facade);
// trip_plugin = ceate<TripPlugin>(*query_data_facade, config.max_locations_trip);
// match_plugin = create<MatchPlugin>(*query_data_facade, config.max_locations_map_matching);
match_plugin = create<MatchPlugin>(*query_data_facade, config.max_locations_map_matching);
}
// make sure we deallocate the unique ptr at a position where we know the size of the plugins
@@ -178,8 +178,7 @@ Status Engine::Trip(const api::TripParameters &params, util::json::Object &resul
Status Engine::Match(const api::MatchParameters &params, util::json::Object &result)
{
// return RunQuery(lock, *query_data_facade, params, *match_plugin, result);
return Status::Error;
return RunQuery(lock, *query_data_facade, params, *match_plugin, result);
}
} // engine ns
src/engine/plugins/match.cpp
+142 -336
View File
@@ -1,14 +1,10 @@
/* TODO(daniel-j-h): fix for new server api, uncomment when this compiles
#include "engine/plugins/plugin_base.hpp"
#include "engine/plugins/match.hpp"
#include "engine/map_matching/bayes_classifier.hpp"
#include "engine/api/match_parameters.hpp"
#include "engine/api/match_api.hpp"
#include "engine/object_encoder.hpp"
#include "engine/search_engine.hpp"
#include "engine/guidance/textual_route_annotation.hpp"
#include "engine/guidance/segment_list.hpp"
#include "engine/api_response_generator.hpp"
#include "engine/routing_algorithms/map_matching.hpp"
#include "util/coordinate_calculation.hpp"
#include "util/integer_range.hpp"
#include "util/json_logger.hpp"
@@ -29,359 +25,169 @@ namespace engine
namespace plugins
{
template <class DataFacadeT> class MapMatchingPlugin : public BasePlugin
// Filters PhantomNodes to obtain a set of viable candiates
void filterCandidates(const std::vector<util::FixedPointCoordinate> &coordinates,
MatchPlugin::CandidateLists &candidates_lists)
{
std::shared_ptr<SearchEngine<DataFacadeT>> search_engine_ptr;
using SubMatching = routing_algorithms::SubMatching;
using SubMatchingList = routing_algorithms::SubMatchingList;
using CandidateLists = routing_algorithms::CandidateLists;
using ClassifierT = map_matching::BayesClassifier<map_matching::LaplaceDistribution,
map_matching::LaplaceDistribution,
double>;
using TraceClassification = ClassifierT::ClassificationT;
public:
MapMatchingPlugin(DataFacadeT *facade, const int max_locations_map_matching)
: descriptor_string("match"), facade(facade),
max_locations_map_matching(max_locations_map_matching),
// the values where derived from fitting a laplace distribution
// to the values of manually classified traces
classifier(map_matching::LaplaceDistribution(0.005986, 0.016646),
map_matching::LaplaceDistribution(0.054385, 0.458432),
0.696774) // valid apriori probability
for (const auto current_coordinate : util::irange<std::size_t>(0, coordinates.size()))
{
search_engine_ptr = std::make_shared<SearchEngine<DataFacadeT>>(facade);
}
bool allow_uturn = false;
virtual ~MapMatchingPlugin() {}
const std::string GetDescriptor() const final override { return descriptor_string; }
TraceClassification
classify(const float trace_length, const float matched_length, const int removed_points) const
{
(void)removed_points; // unused
const double distance_feature = -std::log(trace_length) + std::log(matched_length);
// matched to the same point
if (!std::isfinite(distance_feature))
if (coordinates.size() - 1 > current_coordinate && 0 < current_coordinate)
{
return std::make_pair(ClassifierT::ClassLabel::NEGATIVE, 1.0);
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 <= 90.0 || turn_angle >= 270.0)
{
allow_uturn = true;
}
}
const auto label_with_confidence = classifier.classify(distance_feature);
auto &candidates = candidates_lists[current_coordinate];
return label_with_confidence;
}
// 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_node_id < rhs.phantom_node.forward_node_id ||
(lhs.phantom_node.forward_node_id == rhs.phantom_node.forward_node_id &&
(lhs.phantom_node.reverse_node_id < rhs.phantom_node.reverse_node_id ||
(lhs.phantom_node.reverse_node_id == rhs.phantom_node.reverse_node_id &&
lhs.distance < rhs.distance)));
});
CandidateLists getCandidates(
const std::vector<util::FixedPointCoordinate> &input_coords,
const std::vector<std::pair<const int, const boost::optional<int>>> &input_bearings,
const double gps_precision,
std::vector<double> &sub_trace_lengths)
{
CandidateLists candidates_lists;
auto new_end = std::unique(
candidates.begin(), candidates.end(),
[](const PhantomNodeWithDistance &lhs, const PhantomNodeWithDistance &rhs)
{
return lhs.phantom_node.forward_node_id == rhs.phantom_node.forward_node_id &&
lhs.phantom_node.reverse_node_id == rhs.phantom_node.reverse_node_id;
});
candidates.resize(new_end - candidates.begin());
// assuming gps_precision is the standard deviation of a normal distribution that
// models GPS noise (in this model), this should give us the correct search radius
// with > 99% confidence
double query_radius = 3 * gps_precision;
double last_distance =
util::coordinate_calculation::haversineDistance(input_coords[0], input_coords[1]);
sub_trace_lengths.resize(input_coords.size());
sub_trace_lengths[0] = 0;
for (const auto current_coordinate : util::irange<std::size_t>(0, input_coords.size()))
if (!allow_uturn)
{
bool allow_uturn = false;
if (0 < current_coordinate)
const auto compact_size = candidates.size();
for (const auto i : util::irange<std::size_t>(0, compact_size))
{
last_distance = util::coordinate_calculation::haversineDistance(
input_coords[current_coordinate - 1], input_coords[current_coordinate]);
sub_trace_lengths[current_coordinate] +=
sub_trace_lengths[current_coordinate - 1] + last_distance;
}
if (input_coords.size() - 1 > current_coordinate && 0 < current_coordinate)
{
double turn_angle = util::coordinate_calculation::computeAngle(
input_coords[current_coordinate - 1], input_coords[current_coordinate],
input_coords[current_coordinate + 1]);
// sharp turns indicate a possible uturn
if (turn_angle <= 90.0 || turn_angle >= 270.0)
// Split edge if it is bidirectional and append reverse direction to end of list
if (candidates[i].phantom_node.forward_node_id != SPECIAL_NODEID &&
candidates[i].phantom_node.reverse_node_id != SPECIAL_NODEID)
{
allow_uturn = true;
PhantomNode reverse_node(candidates[i].phantom_node);
reverse_node.forward_node_id = SPECIAL_NODEID;
candidates.push_back(
PhantomNodeWithDistance{reverse_node, candidates[i].distance});
candidates[i].phantom_node.reverse_node_id = SPECIAL_NODEID;
}
}
// Use bearing values if supplied, otherwise fallback to 0,180 defaults
auto bearing = input_bearings.size() > 0 ? input_bearings[current_coordinate].first : 0;
auto range = input_bearings.size() > 0
? (input_bearings[current_coordinate].second
? *input_bearings[current_coordinate].second
: 10)
: 180;
auto candidates = facade->NearestPhantomNodesInRange(input_coords[current_coordinate],
query_radius, bearing, range);
if (candidates.size() == 0)
{
break;
}
// 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_node_id < rhs.phantom_node.forward_node_id ||
(lhs.phantom_node.forward_node_id == rhs.phantom_node.forward_node_id &&
(lhs.phantom_node.reverse_node_id < rhs.phantom_node.reverse_node_id ||
(lhs.phantom_node.reverse_node_id ==
rhs.phantom_node.reverse_node_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_node_id == rhs.phantom_node.forward_node_id &&
lhs.phantom_node.reverse_node_id == rhs.phantom_node.reverse_node_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_node_id != SPECIAL_NODEID &&
candidates[i].phantom_node.reverse_node_id != SPECIAL_NODEID)
{
PhantomNode reverse_node(candidates[i].phantom_node);
reverse_node.forward_node_id = SPECIAL_NODEID;
candidates.push_back(
PhantomNodeWithDistance{reverse_node, candidates[i].distance});
candidates[i].phantom_node.reverse_node_id = SPECIAL_NODEID;
}
}
}
// sort by distance to make pruning effective
std::sort(candidates.begin(), candidates.end(),
[](const PhantomNodeWithDistance &lhs, const PhantomNodeWithDistance &rhs)
{
return lhs.distance < rhs.distance;
});
candidates_lists.push_back(std::move(candidates));
}
return candidates_lists;
// sort by distance to make pruning effective
std::sort(candidates.begin(), candidates.end(),
[](const PhantomNodeWithDistance &lhs, const PhantomNodeWithDistance &rhs)
{
return lhs.distance < rhs.distance;
});
}
}
util::json::Object submatchingToJSON(const SubMatching &sub,
const RouteParameters &route_parameters,
const InternalRouteResult &raw_route)
Status MatchPlugin::HandleRequest(const api::MatchParameters &parameters,
util::json::Object &json_result)
{
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)
{
util::json::Object subtrace;
if (route_parameters.classify)
{
subtrace.values["confidence"] = sub.confidence;
}
auto response_generator = MakeApiResponseGenerator(facade);
subtrace.values["hint_data"] = response_generator.BuildHintData(raw_route);
if (route_parameters.geometry || route_parameters.print_instructions)
{
using SegmentList = guidance::SegmentList<DataFacadeT>;
// Passing false to extract_alternative extracts the route.
const constexpr bool EXTRACT_ROUTE = false;
// by passing false to segment_list, we skip the douglas peucker simplification
// and mark all segments as necessary within the generation process
const constexpr bool NO_ROUTE_SIMPLIFICATION = false;
SegmentList segment_list(raw_route, EXTRACT_ROUTE, route_parameters.zoom_level,
NO_ROUTE_SIMPLIFICATION, facade);
if (route_parameters.geometry)
{
subtrace.values["geometry"] =
response_generator.GetGeometry(route_parameters.compression, segment_list);
}
if (route_parameters.print_instructions)
{
subtrace.values["instructions"] =
guidance::AnnotateRoute<DataFacadeT>(segment_list.Get(), facade);
}
util::json::Object json_route_summary;
json_route_summary.values["total_distance"] = segment_list.GetDistance();
json_route_summary.values["total_time"] = segment_list.GetDuration();
subtrace.values["route_summary"] = json_route_summary;
}
subtrace.values["indices"] = util::json::make_array(sub.indices);
util::json::Array points;
for (const auto &node : sub.nodes)
{
points.values.emplace_back(
util::json::make_array(node.location.lat / COORDINATE_PRECISION,
node.location.lon / COORDINATE_PRECISION));
}
subtrace.values["matched_points"] = points;
util::json::Array names;
for (const auto &node : sub.nodes)
{
names.values.emplace_back(facade->get_name_for_id(node.name_id));
}
subtrace.values["matched_names"] = names;
return subtrace;
return Error("TooBig", "Too many trace coordinates", json_result);
}
Status HandleRequest(const RouteParameters &route_parameters,
util::json::Object &json_result) final override
if (!CheckAllCoordinates(parameters.coordinates))
{
// enforce maximum number of locations for performance reasons
if (max_locations_map_matching > 0 &&
static_cast<int>(route_parameters.coordinates.size()) > max_locations_map_matching)
{
json_result.values["status_message"] = "Too many coodindates";
return Status::Error;
}
// check number of parameters
if (!check_all_coordinates(route_parameters.coordinates))
{
json_result.values["status_message"] = "Invalid coordinates";
return Status::Error;
}
std::vector<double> sub_trace_lengths;
const auto &input_coords = route_parameters.coordinates;
const auto &input_timestamps = route_parameters.timestamps;
const auto &input_bearings = route_parameters.bearings;
if (input_timestamps.size() > 0 && input_coords.size() != input_timestamps.size())
{
json_result.values["status_message"] =
"Number of timestamps does not match number of coordinates";
return Status::Error;
}
if (input_bearings.size() > 0 && input_coords.size() != input_bearings.size())
{
json_result.values["status_message"] =
"Number of bearings does not match number of coordinates";
return Status::Error;
}
// at least two coordinates are needed for map matching
if (static_cast<int>(input_coords.size()) < 2)
{
json_result.values["status_message"] = "At least two coordinates needed";
return Status::Error;
}
const auto candidates_lists = getCandidates(
input_coords, input_bearings, route_parameters.gps_precision, sub_trace_lengths);
if (candidates_lists.size() != input_coords.size())
{
BOOST_ASSERT(candidates_lists.size() < input_coords.size());
json_result.values["status_message"] =
std::string("Could not find a matching segment for coordinate ") +
std::to_string(candidates_lists.size());
return Status::NoSegment;
}
// setup logging if enabled
if (util::json::Logger::get())
util::json::Logger::get()->initialize("matching");
// call the actual map matching
SubMatchingList sub_matchings;
search_engine_ptr->map_matching(candidates_lists, input_coords, input_timestamps,
route_parameters.matching_beta,
route_parameters.gps_precision, sub_matchings);
util::json::Array matchings;
for (auto &sub : sub_matchings)
{
// classify result
if (route_parameters.classify)
{
double trace_length =
sub_trace_lengths[sub.indices.back()] - sub_trace_lengths[sub.indices.front()];
TraceClassification classification =
classify(trace_length, sub.length,
(sub.indices.back() - sub.indices.front() + 1) - sub.nodes.size());
if (classification.first == ClassifierT::ClassLabel::POSITIVE)
{
sub.confidence = classification.second;
}
else
{
sub.confidence = 1 - classification.second;
}
}
BOOST_ASSERT(sub.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
InternalRouteResult raw_route;
PhantomNodes current_phantom_node_pair;
for (unsigned i = 0; i < sub.nodes.size() - 1; ++i)
{
current_phantom_node_pair.source_phantom = sub.nodes[i];
current_phantom_node_pair.target_phantom = sub.nodes[i + 1];
BOOST_ASSERT(current_phantom_node_pair.source_phantom.IsValid());
BOOST_ASSERT(current_phantom_node_pair.target_phantom.IsValid());
raw_route.segment_end_coordinates.emplace_back(current_phantom_node_pair);
}
search_engine_ptr->shortest_path(
raw_route.segment_end_coordinates,
std::vector<bool>(raw_route.segment_end_coordinates.size() + 1, true), raw_route);
BOOST_ASSERT(raw_route.shortest_path_length != INVALID_EDGE_WEIGHT);
matchings.values.emplace_back(submatchingToJSON(sub, route_parameters, raw_route));
}
if (util::json::Logger::get())
util::json::Logger::get()->render("matching", json_result);
json_result.values["matchings"] = matchings;
if (sub_matchings.empty())
{
json_result.values["status_message"] = "Cannot find matchings";
return Status::EmptyResult;
}
json_result.values["status_message"] = "Found matchings";
return Status::Ok;
return Error("InvalidValue", "Invalid coordinate value.", json_result);
}
private:
std::string descriptor_string;
DataFacadeT *facade;
int max_locations_map_matching;
ClassifierT classifier;
};
// 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 (parameters.radiuses.empty())
{
search_radiuses.resize(parameters.coordinates.size(),
DEFAULT_GPS_PRECISION * RADIUS_MULTIPLIER);
}
else
{
search_radiuses.resize(parameters.coordinates.size());
std::transform(parameters.radiuses.begin(), parameters.radiuses.end(),
search_radiuses.begin(), [](const boost::optional<double> &maybe_radius)
{
if (maybe_radius)
{
return *maybe_radius * RADIUS_MULTIPLIER;
}
else
{
return DEFAULT_GPS_PRECISION * RADIUS_MULTIPLIER;
}
});
}
auto candidates_lists = GetPhantomNodesInRange(parameters, search_radiuses);
filterCandidates(parameters.coordinates, candidates_lists);
if (candidates_lists.size() != parameters.coordinates.size())
{
BOOST_ASSERT(candidates_lists.size() < parameters.coordinates.size());
return Error("NoSegment", std::string("Could not find a matching segment for coordinate ") +
std::to_string(candidates_lists.size()),
json_result);
}
// setup logging if enabled
if (util::json::Logger::get())
util::json::Logger::get()->initialize("matching");
// call the actual map matching
SubMatchingList sub_matchings = map_matching(candidates_lists, parameters.coordinates,
parameters.timestamps, parameters.radiuses);
std::vector<InternalRouteResult> sub_routes(sub_matchings.size());
for (auto index : util::irange(0UL, sub_matchings.size()))
{
BOOST_ASSERT(sub.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
PhantomNodes current_phantom_node_pair;
for (unsigned i = 0; i < sub_matchings[index].nodes.size() - 1; ++i)
{
current_phantom_node_pair.source_phantom = sub_matchings[index].nodes[i];
current_phantom_node_pair.target_phantom = sub_matchings[index].nodes[i + 1];
BOOST_ASSERT(current_phantom_node_pair.source_phantom.IsValid());
BOOST_ASSERT(current_phantom_node_pair.target_phantom.IsValid());
sub_routes[index].segment_end_coordinates.emplace_back(current_phantom_node_pair);
}
shortest_path(sub_routes[index].segment_end_coordinates, {}, sub_routes[index]);
BOOST_ASSERT(raw_route.shortest_path_length != INVALID_EDGE_WEIGHT);
}
api::MatchAPI match_api{BasePlugin::facade, parameters};
match_api.MakeResponse(sub_matchings, sub_routes, json_result);
if (util::json::Logger::get())
util::json::Logger::get()->render("matching", json_result);
return Status::Ok;
}
}
}
}
*/