- std::log and M_PI are not constexpr's by the standard. replace by a constant - reformat
619 lines
25 KiB
C++
619 lines
25 KiB
C++
/*
|
|
|
|
Copyright (c) 2015, Project OSRM contributors
|
|
All rights reserved.
|
|
|
|
Redistribution and use in source and binary forms, with or without modification,
|
|
are permitted provided that the following conditions are met:
|
|
|
|
Redistributions of source code must retain the above copyright notice, this list
|
|
of conditions and the following disclaimer.
|
|
Redistributions in binary form must reproduce the above copyright notice, this
|
|
list of conditions and the following disclaimer in the documentation and/or
|
|
other materials provided with the distribution.
|
|
|
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
|
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
|
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
|
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
|
|
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
|
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
|
|
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
|
|
*/
|
|
|
|
#ifndef MAP_MATCHING_HPP
|
|
#define MAP_MATCHING_HPP
|
|
|
|
#include "routing_base.hpp"
|
|
|
|
#include "../data_structures/coordinate_calculation.hpp"
|
|
#include "../util/simple_logger.hpp"
|
|
#include "../util/json_util.hpp"
|
|
#include "../util/json_logger.hpp"
|
|
|
|
#include <osrm/json_container.hpp>
|
|
#include <variant/variant.hpp>
|
|
|
|
#include <fstream>
|
|
|
|
#include <algorithm>
|
|
#include <iomanip>
|
|
#include <numeric>
|
|
|
|
namespace Matching
|
|
{
|
|
|
|
struct SubMatching
|
|
{
|
|
std::vector<PhantomNode> nodes;
|
|
std::vector<unsigned> indices;
|
|
double length;
|
|
double confidence;
|
|
};
|
|
|
|
using CandidateList = std::vector<std::pair<PhantomNode, double>>;
|
|
using CandidateLists = std::vector<CandidateList>;
|
|
using SubMatchingList = std::vector<SubMatching>;
|
|
constexpr static const double IMPOSSIBLE_LOG_PROB = -std::numeric_limits<double>::infinity();
|
|
constexpr static const double MINIMAL_LOG_PROB = -std::numeric_limits<double>::max();
|
|
constexpr static const unsigned INVALID_STATE = std::numeric_limits<unsigned>::max();
|
|
constexpr static const unsigned MAX_BROKEN_STATES = 6;
|
|
constexpr static const unsigned MAX_BROKEN_TIME = 30;
|
|
}
|
|
|
|
// implements a hidden markov model map matching algorithm
|
|
template <class DataFacadeT>
|
|
class MapMatching final : public BasicRoutingInterface<DataFacadeT, MapMatching<DataFacadeT>>
|
|
{
|
|
using super = BasicRoutingInterface<DataFacadeT, MapMatching<DataFacadeT>>;
|
|
using QueryHeap = SearchEngineData::QueryHeap;
|
|
SearchEngineData &engine_working_data;
|
|
|
|
// FIXME this value should be a table based on samples/meter (or samples/min)
|
|
constexpr static const double default_beta = 10.0;
|
|
constexpr static const double default_sigma_z = 4.07;
|
|
constexpr static const double log_2_pi = 1.837877066409346; // std::log(2. * M_PI);
|
|
|
|
// closures to precompute log -> only simple floating point operations
|
|
struct EmissionLogProbability
|
|
{
|
|
double sigma_z;
|
|
double log_sigma_z;
|
|
|
|
EmissionLogProbability(const double sigma_z)
|
|
: sigma_z(sigma_z), log_sigma_z(std::log(sigma_z))
|
|
{
|
|
}
|
|
|
|
double operator()(const double distance) const
|
|
{
|
|
return -0.5 * (log_2_pi + (distance / sigma_z) * (distance / sigma_z)) - log_sigma_z;
|
|
}
|
|
};
|
|
struct TransitionLogProbability
|
|
{
|
|
double beta;
|
|
double log_beta;
|
|
TransitionLogProbability(const double beta) : beta(beta), log_beta(std::log(beta)) {}
|
|
|
|
double operator()(const double d_t) const { return -log_beta - d_t / beta; }
|
|
};
|
|
|
|
double get_network_distance(const PhantomNode &source_phantom,
|
|
const PhantomNode &target_phantom) const
|
|
{
|
|
EdgeWeight upper_bound = INVALID_EDGE_WEIGHT;
|
|
NodeID middle_node = SPECIAL_NODEID;
|
|
EdgeWeight edge_offset = std::min(0, -source_phantom.GetForwardWeightPlusOffset());
|
|
edge_offset = std::min(edge_offset, -source_phantom.GetReverseWeightPlusOffset());
|
|
|
|
engine_working_data.InitializeOrClearFirstThreadLocalStorage(
|
|
super::facade->GetNumberOfNodes());
|
|
engine_working_data.InitializeOrClearSecondThreadLocalStorage(
|
|
super::facade->GetNumberOfNodes());
|
|
|
|
QueryHeap &forward_heap = *(engine_working_data.forward_heap_1);
|
|
QueryHeap &reverse_heap = *(engine_working_data.reverse_heap_1);
|
|
|
|
if (source_phantom.forward_node_id != SPECIAL_NODEID)
|
|
{
|
|
forward_heap.Insert(source_phantom.forward_node_id,
|
|
-source_phantom.GetForwardWeightPlusOffset(),
|
|
source_phantom.forward_node_id);
|
|
}
|
|
if (source_phantom.reverse_node_id != SPECIAL_NODEID)
|
|
{
|
|
forward_heap.Insert(source_phantom.reverse_node_id,
|
|
-source_phantom.GetReverseWeightPlusOffset(),
|
|
source_phantom.reverse_node_id);
|
|
}
|
|
|
|
if (target_phantom.forward_node_id != SPECIAL_NODEID)
|
|
{
|
|
reverse_heap.Insert(target_phantom.forward_node_id,
|
|
target_phantom.GetForwardWeightPlusOffset(),
|
|
target_phantom.forward_node_id);
|
|
}
|
|
if (target_phantom.reverse_node_id != SPECIAL_NODEID)
|
|
{
|
|
reverse_heap.Insert(target_phantom.reverse_node_id,
|
|
target_phantom.GetReverseWeightPlusOffset(),
|
|
target_phantom.reverse_node_id);
|
|
}
|
|
|
|
// search from s and t till new_min/(1+epsilon) > length_of_shortest_path
|
|
while (0 < (forward_heap.Size() + reverse_heap.Size()))
|
|
{
|
|
if (0 < forward_heap.Size())
|
|
{
|
|
super::RoutingStep(forward_heap, reverse_heap, &middle_node, &upper_bound,
|
|
edge_offset, true);
|
|
}
|
|
if (0 < reverse_heap.Size())
|
|
{
|
|
super::RoutingStep(reverse_heap, forward_heap, &middle_node, &upper_bound,
|
|
edge_offset, false);
|
|
}
|
|
}
|
|
|
|
double distance = std::numeric_limits<double>::max();
|
|
if (upper_bound != INVALID_EDGE_WEIGHT)
|
|
{
|
|
std::vector<NodeID> packed_leg;
|
|
super::RetrievePackedPathFromHeap(forward_heap, reverse_heap, middle_node, packed_leg);
|
|
std::vector<PathData> unpacked_path;
|
|
PhantomNodes nodes;
|
|
nodes.source_phantom = source_phantom;
|
|
nodes.target_phantom = target_phantom;
|
|
super::UnpackPath(packed_leg, nodes, unpacked_path);
|
|
|
|
FixedPointCoordinate previous_coordinate = source_phantom.location;
|
|
FixedPointCoordinate current_coordinate;
|
|
distance = 0;
|
|
for (const auto &p : unpacked_path)
|
|
{
|
|
current_coordinate = super::facade->GetCoordinateOfNode(p.node);
|
|
distance += coordinate_calculation::great_circle_distance(previous_coordinate,
|
|
current_coordinate);
|
|
previous_coordinate = current_coordinate;
|
|
}
|
|
distance += coordinate_calculation::great_circle_distance(previous_coordinate,
|
|
target_phantom.location);
|
|
}
|
|
|
|
return distance;
|
|
}
|
|
|
|
struct HiddenMarkovModel
|
|
{
|
|
std::vector<std::vector<double>> viterbi;
|
|
std::vector<std::vector<std::pair<unsigned, unsigned>>> parents;
|
|
std::vector<std::vector<float>> path_lengths;
|
|
std::vector<std::vector<bool>> pruned;
|
|
std::vector<bool> breakage;
|
|
|
|
const Matching::CandidateLists &candidates_list;
|
|
const EmissionLogProbability &emission_log_probability;
|
|
|
|
HiddenMarkovModel(const Matching::CandidateLists &candidates_list,
|
|
const EmissionLogProbability &emission_log_probability)
|
|
: breakage(candidates_list.size()), candidates_list(candidates_list),
|
|
emission_log_probability(emission_log_probability)
|
|
{
|
|
for (const auto &l : candidates_list)
|
|
{
|
|
viterbi.emplace_back(l.size());
|
|
parents.emplace_back(l.size());
|
|
path_lengths.emplace_back(l.size());
|
|
pruned.emplace_back(l.size());
|
|
}
|
|
|
|
clear(0);
|
|
}
|
|
|
|
void clear(unsigned initial_timestamp)
|
|
{
|
|
BOOST_ASSERT(viterbi.size() == parents.size() &&
|
|
parents.size() == path_lengths.size() &&
|
|
path_lengths.size() == pruned.size() && pruned.size() == breakage.size());
|
|
|
|
for (unsigned t = initial_timestamp; t < viterbi.size(); t++)
|
|
{
|
|
std::fill(viterbi[t].begin(), viterbi[t].end(), Matching::IMPOSSIBLE_LOG_PROB);
|
|
std::fill(parents[t].begin(), parents[t].end(), std::make_pair(0u, 0u));
|
|
std::fill(path_lengths[t].begin(), path_lengths[t].end(), 0);
|
|
std::fill(pruned[t].begin(), pruned[t].end(), true);
|
|
}
|
|
std::fill(breakage.begin() + initial_timestamp, breakage.end(), true);
|
|
}
|
|
|
|
unsigned initialize(unsigned initial_timestamp)
|
|
{
|
|
BOOST_ASSERT(initial_timestamp < candidates_list.size());
|
|
|
|
do
|
|
{
|
|
for (auto s = 0u; s < viterbi[initial_timestamp].size(); ++s)
|
|
{
|
|
viterbi[initial_timestamp][s] =
|
|
emission_log_probability(candidates_list[initial_timestamp][s].second);
|
|
parents[initial_timestamp][s] = std::make_pair(initial_timestamp, s);
|
|
pruned[initial_timestamp][s] =
|
|
viterbi[initial_timestamp][s] < Matching::MINIMAL_LOG_PROB;
|
|
|
|
breakage[initial_timestamp] =
|
|
breakage[initial_timestamp] && pruned[initial_timestamp][s];
|
|
}
|
|
|
|
++initial_timestamp;
|
|
} while (breakage[initial_timestamp - 1]);
|
|
|
|
if (initial_timestamp >= viterbi.size())
|
|
{
|
|
return Matching::INVALID_STATE;
|
|
}
|
|
|
|
BOOST_ASSERT(initial_timestamp > 0);
|
|
--initial_timestamp;
|
|
|
|
BOOST_ASSERT(breakage[initial_timestamp] == false);
|
|
|
|
return initial_timestamp;
|
|
}
|
|
};
|
|
|
|
// Provides the debug interface for introspection tools
|
|
struct DebugInfo
|
|
{
|
|
DebugInfo(const osrm::json::Logger *logger) : logger(logger)
|
|
{
|
|
if (logger)
|
|
{
|
|
object = &logger->map->at("matching");
|
|
}
|
|
}
|
|
|
|
void initialize(const Matching::CandidateLists &candidates_list)
|
|
{
|
|
// json logger not enabled
|
|
if (!logger)
|
|
return;
|
|
|
|
osrm::json::Array states;
|
|
for (unsigned t = 0; t < candidates_list.size(); t++)
|
|
{
|
|
osrm::json::Array timestamps;
|
|
for (unsigned s = 0; s < candidates_list[t].size(); s++)
|
|
{
|
|
osrm::json::Object state;
|
|
state.values["transitions"] = osrm::json::Array();
|
|
state.values["coordinate"] = osrm::json::make_array(
|
|
candidates_list[t][s].first.location.lat / COORDINATE_PRECISION,
|
|
candidates_list[t][s].first.location.lon / COORDINATE_PRECISION);
|
|
state.values["viterbi"] =
|
|
osrm::json::clamp_float(Matching::IMPOSSIBLE_LOG_PROB);
|
|
state.values["pruned"] = 0u;
|
|
timestamps.values.push_back(state);
|
|
}
|
|
states.values.push_back(timestamps);
|
|
}
|
|
osrm::json::get(*object, "states") = states;
|
|
}
|
|
|
|
void add_transition_info(const unsigned prev_t,
|
|
const unsigned current_t,
|
|
const unsigned prev_state,
|
|
const unsigned current_state,
|
|
const double prev_viterbi,
|
|
const double emission_pr,
|
|
const double transition_pr,
|
|
const double network_distance,
|
|
const double great_circle_distance)
|
|
{
|
|
// json logger not enabled
|
|
if (!logger)
|
|
return;
|
|
|
|
osrm::json::Object transistion;
|
|
transistion.values["to"] = osrm::json::make_array(current_t, current_state);
|
|
transistion.values["properties"] = osrm::json::make_array(
|
|
osrm::json::clamp_float(prev_viterbi), osrm::json::clamp_float(emission_pr),
|
|
osrm::json::clamp_float(transition_pr), network_distance, great_circle_distance);
|
|
|
|
osrm::json::get(*object, "states", prev_t, prev_state, "transitions")
|
|
.get<mapbox::util::recursive_wrapper<osrm::json::Array>>()
|
|
.get()
|
|
.values.push_back(transistion);
|
|
}
|
|
|
|
void set_viterbi(const std::vector<std::vector<double>> &viterbi,
|
|
const std::vector<std::vector<bool>> &pruned)
|
|
{
|
|
// json logger not enabled
|
|
if (!logger)
|
|
return;
|
|
|
|
for (auto t = 0u; t < viterbi.size(); t++)
|
|
{
|
|
for (auto s_prime = 0u; s_prime < viterbi[t].size(); ++s_prime)
|
|
{
|
|
osrm::json::get(*object, "states", t, s_prime, "viterbi") =
|
|
osrm::json::clamp_float(viterbi[t][s_prime]);
|
|
osrm::json::get(*object, "states", t, s_prime, "pruned") =
|
|
static_cast<unsigned>(pruned[t][s_prime]);
|
|
}
|
|
}
|
|
}
|
|
|
|
void add_chosen(const unsigned t, const unsigned s)
|
|
{
|
|
// json logger not enabled
|
|
if (!logger)
|
|
return;
|
|
|
|
osrm::json::get(*object, "states", t, s, "chosen") = true;
|
|
}
|
|
|
|
void add_breakage(const std::vector<bool> &breakage)
|
|
{
|
|
// json logger not enabled
|
|
if (!logger)
|
|
return;
|
|
|
|
osrm::json::get(*object, "breakage") = osrm::json::make_array(breakage);
|
|
}
|
|
|
|
const osrm::json::Logger *logger;
|
|
osrm::json::Value *object;
|
|
};
|
|
|
|
public:
|
|
MapMatching(DataFacadeT *facade, SearchEngineData &engine_working_data)
|
|
: super(facade), engine_working_data(engine_working_data)
|
|
{
|
|
}
|
|
|
|
void operator()(const Matching::CandidateLists &candidates_list,
|
|
const std::vector<FixedPointCoordinate> &trace_coordinates,
|
|
const std::vector<unsigned> &trace_timestamps,
|
|
const double matching_beta,
|
|
const double gps_precision,
|
|
Matching::SubMatchingList &sub_matchings) const
|
|
{
|
|
BOOST_ASSERT(candidates_list.size() > 0);
|
|
|
|
// TODO replace default values with table lookup based on sampling frequency
|
|
EmissionLogProbability emission_log_probability(gps_precision > 0 ? gps_precision
|
|
: default_sigma_z);
|
|
TransitionLogProbability transition_log_probability(matching_beta > 0 ? matching_beta
|
|
: default_beta);
|
|
|
|
HiddenMarkovModel model(candidates_list, emission_log_probability);
|
|
|
|
unsigned initial_timestamp = model.initialize(0);
|
|
if (initial_timestamp == Matching::INVALID_STATE)
|
|
{
|
|
return;
|
|
}
|
|
|
|
DebugInfo debug(osrm::json::Logger::get());
|
|
debug.initialize(candidates_list);
|
|
|
|
unsigned breakage_begin = std::numeric_limits<unsigned>::max();
|
|
std::vector<unsigned> split_points;
|
|
std::vector<unsigned> prev_unbroken_timestamps;
|
|
prev_unbroken_timestamps.reserve(candidates_list.size());
|
|
prev_unbroken_timestamps.push_back(initial_timestamp);
|
|
for (auto t = initial_timestamp + 1; t < candidates_list.size(); ++t)
|
|
{
|
|
// breakage recover has removed all previous good points
|
|
bool trace_split = prev_unbroken_timestamps.size() < 1;
|
|
|
|
// use temporal information if available to determine a split
|
|
if (trace_timestamps.size() > 0)
|
|
{
|
|
trace_split =
|
|
trace_split ||
|
|
(trace_timestamps[t] - trace_timestamps[prev_unbroken_timestamps.back()] >
|
|
Matching::MAX_BROKEN_TIME);
|
|
}
|
|
else
|
|
{
|
|
trace_split = trace_split ||
|
|
(t - prev_unbroken_timestamps.back() > Matching::MAX_BROKEN_STATES);
|
|
}
|
|
|
|
if (trace_split)
|
|
{
|
|
unsigned split_index = t;
|
|
if (breakage_begin != std::numeric_limits<unsigned>::max())
|
|
{
|
|
split_index = breakage_begin;
|
|
breakage_begin = std::numeric_limits<unsigned>::max();
|
|
}
|
|
split_points.push_back(split_index);
|
|
|
|
// note: this preserves everything before split_index
|
|
model.clear(split_index);
|
|
unsigned new_start = model.initialize(split_index);
|
|
// no new start was found -> stop viterbi calculation
|
|
if (new_start == Matching::INVALID_STATE)
|
|
{
|
|
break;
|
|
}
|
|
prev_unbroken_timestamps.clear();
|
|
prev_unbroken_timestamps.push_back(new_start);
|
|
// Important: We potentially go back here!
|
|
// However since t > new_start >= breakge_begin
|
|
// we can only reset trace_coordindates.size() times.
|
|
t = new_start + 1;
|
|
}
|
|
|
|
unsigned prev_unbroken_timestamp = prev_unbroken_timestamps.back();
|
|
|
|
const auto &prev_viterbi = model.viterbi[prev_unbroken_timestamp];
|
|
const auto &prev_pruned = model.pruned[prev_unbroken_timestamp];
|
|
const auto &prev_unbroken_timestamps_list = candidates_list[prev_unbroken_timestamp];
|
|
const auto &prev_coordinate = trace_coordinates[prev_unbroken_timestamp];
|
|
|
|
auto ¤t_viterbi = model.viterbi[t];
|
|
auto ¤t_pruned = model.pruned[t];
|
|
auto ¤t_parents = model.parents[t];
|
|
auto ¤t_lengths = model.path_lengths[t];
|
|
const auto ¤t_timestamps_list = candidates_list[t];
|
|
const auto ¤t_coordinate = trace_coordinates[t];
|
|
|
|
// compute d_t for this timestamp and the next one
|
|
for (auto s = 0u; s < prev_viterbi.size(); ++s)
|
|
{
|
|
if (prev_pruned[s])
|
|
continue;
|
|
|
|
for (auto s_prime = 0u; s_prime < current_viterbi.size(); ++s_prime)
|
|
{
|
|
// how likely is candidate s_prime at time t to be emitted?
|
|
const double emission_pr =
|
|
emission_log_probability(candidates_list[t][s_prime].second);
|
|
double new_value = prev_viterbi[s] + emission_pr;
|
|
if (current_viterbi[s_prime] > new_value)
|
|
continue;
|
|
|
|
// get distance diff between loc1/2 and locs/s_prime
|
|
const auto network_distance =
|
|
get_network_distance(prev_unbroken_timestamps_list[s].first,
|
|
current_timestamps_list[s_prime].first);
|
|
const auto great_circle_distance =
|
|
coordinate_calculation::great_circle_distance(prev_coordinate,
|
|
current_coordinate);
|
|
|
|
const auto d_t = std::abs(network_distance - great_circle_distance);
|
|
|
|
// very low probability transition -> prune
|
|
if (d_t > 500)
|
|
continue;
|
|
|
|
const double transition_pr = transition_log_probability(d_t);
|
|
new_value += transition_pr;
|
|
|
|
debug.add_transition_info(prev_unbroken_timestamp, t, s, s_prime,
|
|
prev_viterbi[s], emission_pr, transition_pr,
|
|
network_distance, great_circle_distance);
|
|
|
|
if (new_value > current_viterbi[s_prime])
|
|
{
|
|
current_viterbi[s_prime] = new_value;
|
|
current_parents[s_prime] = std::make_pair(prev_unbroken_timestamp, s);
|
|
current_lengths[s_prime] = network_distance;
|
|
current_pruned[s_prime] = false;
|
|
model.breakage[t] = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (model.breakage[t])
|
|
{
|
|
// save start of breakage -> we need this as split point
|
|
if (t < breakage_begin)
|
|
{
|
|
breakage_begin = t;
|
|
}
|
|
|
|
BOOST_ASSERT(prev_unbroken_timestamps.size() > 0);
|
|
|
|
// remove both ends of the breakage
|
|
prev_unbroken_timestamps.pop_back();
|
|
}
|
|
else
|
|
{
|
|
prev_unbroken_timestamps.push_back(t);
|
|
}
|
|
}
|
|
|
|
debug.set_viterbi(model.viterbi, model.pruned);
|
|
|
|
if (prev_unbroken_timestamps.size() > 0)
|
|
{
|
|
split_points.push_back(prev_unbroken_timestamps.back() + 1);
|
|
}
|
|
|
|
unsigned sub_matching_begin = initial_timestamp;
|
|
for (const unsigned sub_matching_end : split_points)
|
|
{
|
|
Matching::SubMatching matching;
|
|
|
|
// find real end of trace
|
|
// not sure if this is really needed
|
|
unsigned parent_timestamp_index = sub_matching_end - 1;
|
|
while (parent_timestamp_index >= sub_matching_begin &&
|
|
model.breakage[parent_timestamp_index])
|
|
{
|
|
parent_timestamp_index--;
|
|
}
|
|
|
|
// matchings that only consist of one candidate are invalid
|
|
if (parent_timestamp_index - sub_matching_begin + 1 < 2)
|
|
{
|
|
sub_matching_begin = sub_matching_end;
|
|
continue;
|
|
}
|
|
|
|
// loop through the columns, and only compare the last entry
|
|
auto max_element_iter = std::max_element(model.viterbi[parent_timestamp_index].begin(),
|
|
model.viterbi[parent_timestamp_index].end());
|
|
|
|
unsigned parent_candidate_index =
|
|
std::distance(model.viterbi[parent_timestamp_index].begin(), max_element_iter);
|
|
|
|
std::deque<std::pair<unsigned, unsigned>> reconstructed_indices;
|
|
while (parent_timestamp_index > sub_matching_begin)
|
|
{
|
|
if (model.breakage[parent_timestamp_index])
|
|
{
|
|
continue;
|
|
}
|
|
|
|
reconstructed_indices.emplace_front(parent_timestamp_index, parent_candidate_index);
|
|
const auto &next = model.parents[parent_timestamp_index][parent_candidate_index];
|
|
parent_timestamp_index = next.first;
|
|
parent_candidate_index = next.second;
|
|
}
|
|
reconstructed_indices.emplace_front(parent_timestamp_index, parent_candidate_index);
|
|
if (reconstructed_indices.size() < 2)
|
|
{
|
|
sub_matching_begin = sub_matching_end;
|
|
continue;
|
|
}
|
|
|
|
matching.length = 0.0f;
|
|
matching.nodes.resize(reconstructed_indices.size());
|
|
matching.indices.resize(reconstructed_indices.size());
|
|
for (auto i = 0u; i < reconstructed_indices.size(); ++i)
|
|
{
|
|
auto timestamp_index = reconstructed_indices[i].first;
|
|
auto location_index = reconstructed_indices[i].second;
|
|
|
|
matching.indices[i] = timestamp_index;
|
|
matching.nodes[i] = candidates_list[timestamp_index][location_index].first;
|
|
matching.length += model.path_lengths[timestamp_index][location_index];
|
|
|
|
debug.add_chosen(timestamp_index, location_index);
|
|
}
|
|
|
|
sub_matchings.push_back(matching);
|
|
|
|
sub_matching_begin = sub_matching_end;
|
|
}
|
|
|
|
debug.add_breakage(model.breakage);
|
|
}
|
|
};
|
|
|
|
//[1] "Hidden Markov Map Matching Through Noise and Sparseness"; P. Newson and J. Krumm; 2009; ACM
|
|
// GIS
|
|
|
|
#endif /* MAP_MATCHING_HPP */
|