drifty/comaps
1
0
Fork 0
mirror of https://codeberg.org/comaps/comaps synced 2025-12-20 13:23:59 +00:00
Code Issues Packages Projects Releases Wiki Activity

[routing] In decoder mode, snap to segment endpoints (no partial segments)

Browse Source 
Signed-off-by: mvglasow <michael -at- vonglasow.com>
This commit is contained in:
mvglasow
2025-11-11 00:56:48 +02:00
parent 5c2cedb19a
commit 8fcf00b0ca
12 changed files with 80 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
libs/geometry/parametrized_segment.hpp
View File

@@ -1,5 +1,6 @@
#pragma once
#include "geometry/mercator.hpp"
#include "geometry/point2d.hpp"
#include "base/math.hpp"
@@ -59,9 +60,20 @@ public:
/**
* @brief Returns the point of the segment that is closest to `p`.
*
* @param p The checkpoint
* @param snapToEnds If true, the result is the endpoint of the segment which is closest to `p`
*/
m2::PointD ClosestPointTo(Point const & p) const
m2::PointD ClosestPointTo(Point const & p, bool snapToEnds = false) const
{
if (snapToEnds)
{
if (mercator::DistanceOnEarth(p, m_p0) < mercator::DistanceOnEarth(p, m_p1))
return m_p0;
else
return m_p1;
}
m2::PointD const diff(p - m_p0);
double const t = DotProduct(m_d, diff);

14
libs/routing/fake_ending.cpp
View File

@@ -18,12 +18,12 @@ using namespace routing;
using namespace std;
LatLonWithAltitude CalcProjectionToSegment(LatLonWithAltitude const & begin, LatLonWithAltitude const & end,
m2::PointD const & point)
m2::PointD const & point, bool snapToEnds)
{
m2::ParametrizedSegment<m2::PointD> segment(mercator::FromLatLon(begin.GetLatLon()),
mercator::FromLatLon(end.GetLatLon()));
auto const projectedPoint = segment.ClosestPointTo(point);
auto const projectedPoint = segment.ClosestPointTo(point, snapToEnds);
auto const distBeginToEnd = ms::DistanceOnEarth(begin.GetLatLon(), end.GetLatLon());
auto const projectedLatLon = mercator::ToLatLon(projectedPoint);
@@ -45,7 +45,8 @@ bool Projection::operator==(Projection const & other) const
tie(other.m_segment, other.m_isOneWay, other.m_segmentFront, other.m_segmentBack, other.m_junction);
}
FakeEnding MakeFakeEnding(vector<Segment> const & segments, m2::PointD const & point, WorldGraph & graph)
FakeEnding MakeFakeEnding(vector<Segment> const & segments, m2::PointD const & point,
WorldGraph & graph, bool snapToEnds)
{
FakeEnding ending;
double averageAltitude = 0.0;
@@ -57,7 +58,7 @@ FakeEnding MakeFakeEnding(vector<Segment> const & segments, m2::PointD const & p
bool const oneWay = graph.IsOneWay(segment.GetMwmId(), segment.GetFeatureId());
auto const & frontJunction = graph.GetJunction(segment, true /* front */);
auto const & backJunction = graph.GetJunction(segment, false /* front */);
auto const & projectedJunction = CalcProjectionToSegment(backJunction, frontJunction, point);
auto const & projectedJunction = CalcProjectionToSegment(backJunction, frontJunction, point, snapToEnds);
ending.m_projections.emplace_back(segment, oneWay, frontJunction, backJunction, projectedJunction);
@@ -69,13 +70,14 @@ FakeEnding MakeFakeEnding(vector<Segment> const & segments, m2::PointD const & p
return ending;
}
FakeEnding MakeFakeEnding(Segment const & segment, m2::PointD const & point, IndexGraph & graph)
FakeEnding MakeFakeEnding(Segment const & segment, m2::PointD const & point, IndexGraph & graph,
bool snapToEnds)
{
auto const & road = graph.GetRoadGeometry(segment.GetFeatureId());
bool const oneWay = road.IsOneWay();
auto const & frontJunction = road.GetJunction(segment.GetPointId(true /* front */));
auto const & backJunction = road.GetJunction(segment.GetPointId(false /* front */));
auto const & projectedJunction = CalcProjectionToSegment(backJunction, frontJunction, point);
auto const & projectedJunction = CalcProjectionToSegment(backJunction, frontJunction, point, snapToEnds);
FakeEnding ending;
ending.m_originJunction = LatLonWithAltitude(mercator::ToLatLon(point), projectedJunction.GetAltitude());

8
libs/routing/fake_ending.hpp
View File

@@ -40,9 +40,11 @@ struct FakeEnding final
std::vector<Projection> m_projections;
};
FakeEnding MakeFakeEnding(std::vector<Segment> const & segments, m2::PointD const & point, WorldGraph & graph);
FakeEnding MakeFakeEnding(Segment const & segment, m2::PointD const & point, IndexGraph & graph);
FakeEnding MakeFakeEnding(std::vector<Segment> const & segments, m2::PointD const & point,
WorldGraph & graph, bool snapToEnds = false);
FakeEnding MakeFakeEnding(Segment const & segment, m2::PointD const & point, IndexGraph & graph,
bool snapToEnds = false);
LatLonWithAltitude CalcProjectionToSegment(LatLonWithAltitude const & begin, LatLonWithAltitude const & end,
m2::PointD const & point);
m2::PointD const & point, bool snapToEnds = false);
} // namespace routing

5
libs/routing/features_road_graph.cpp
View File

@@ -143,9 +143,10 @@ void FeaturesRoadGraphBase::ForEachFeatureClosestToCross(m2::PointD const & cros
}
void FeaturesRoadGraphBase::FindClosestEdges(m2::RectD const & rect, uint32_t count,
vector<pair<Edge, geometry::PointWithAltitude>> & vicinities) const
vector<pair<Edge, geometry::PointWithAltitude>> & vicinities,
bool snapToEnds) const
{
NearestEdgeFinder finder(rect.Center(), nullptr /* IsEdgeProjGood */);
NearestEdgeFinder finder(rect.Center(), nullptr /* IsEdgeProjGood */, snapToEnds);
m_dataSource.ForEachStreet([&](FeatureType & ft)
{

3
libs/routing/features_road_graph.hpp
View File

@@ -85,7 +85,8 @@ public:
/// @{
void ForEachFeatureClosestToCross(m2::PointD const & cross, ICrossEdgesLoader & edgesLoader) const override;
void FindClosestEdges(m2::RectD const & rect, uint32_t count,
std::vector<std::pair<Edge, geometry::PointWithAltitude>> & vicinities) const override;
std::vector<std::pair<Edge, geometry::PointWithAltitude>> & vicinities,
bool snapToEnds = false) const override;
std::vector<IRoadGraph::FullRoadInfo> FindRoads(m2::RectD const & rect,
IsGoodFeatureFn const & isGoodFeature) const override;
void GetFeatureTypes(FeatureID const & featureId, feature::TypesHolder & types) const override;

9
libs/routing/index_router.cpp
View File

@@ -314,7 +314,7 @@ bool IndexRouter::FindClosestProjectionToRoad(m2::PointD const & point, m2::Poin
std::vector<EdgeProjectionT> candidates;
uint32_t const count = direction.IsAlmostZero() ? 1 : 4;
m_roadGraph.FindClosestEdges(rect, count, candidates);
m_roadGraph.FindClosestEdges(rect, count, candidates, (GetMode() == Mode::Decoding));
if (candidates.empty())
return false;
@@ -1146,10 +1146,10 @@ int IndexRouter::PointsOnEdgesSnapping::Snap(m2::PointD const & start, m2::Point
// One of startEnding or finishEnding will be empty here.
if (startEnding.m_projections.empty())
startEnding = MakeFakeEnding(m_startSegments, start, m_graph);
startEnding = MakeFakeEnding(m_startSegments, start, m_graph, (m_router.GetMode() == Mode::Decoding));
if (finishEnding.m_projections.empty())
finishEnding = MakeFakeEnding(finishSegments, finish, m_graph);
finishEnding = MakeFakeEnding(finishSegments, finish, m_graph, (m_router.GetMode() == Mode::Decoding));
return 0;
}
@@ -1231,12 +1231,11 @@ bool IndexRouter::PointsOnEdgesSnapping::IsFencedOff(m2::PointD const & point, E
return false;
}
// static
void IndexRouter::PointsOnEdgesSnapping::RoadsToNearestEdges(m2::PointD const & point, vector<RoadInfoT> const & roads,
IsEdgeProjGood const & isGood,
vector<EdgeProjectionT> & edgeProj)
{
NearestEdgeFinder finder(point, isGood);
NearestEdgeFinder finder(point, isGood, (m_router.GetMode() == Mode::Decoding));
for (auto const & road : roads)
finder.AddInformationSource(road);

2
libs/routing/index_router.hpp
View File

@@ -247,7 +247,7 @@ private:
static bool IsFencedOff(m2::PointD const & point, EdgeProjectionT const & edgeProjection,
std::vector<RoadInfoT> const & fences);
static void RoadsToNearestEdges(m2::PointD const & point, std::vector<RoadInfoT> const & roads,
void RoadsToNearestEdges(m2::PointD const & point, std::vector<RoadInfoT> const & roads,
IsEdgeProjGood const & isGood, std::vector<EdgeProjectionT> & edgeProj);
Segment GetSegmentByEdge(Edge const & edge) const;

8
libs/routing/nearest_edge_finder.cpp
View File

@@ -9,9 +9,11 @@ namespace routing
{
using namespace std;
NearestEdgeFinder::NearestEdgeFinder(m2::PointD const & point, IsEdgeProjGood const & isEdgeProjGood)
NearestEdgeFinder::NearestEdgeFinder(m2::PointD const & point, IsEdgeProjGood const & isEdgeProjGood,
bool snapToEnds)
: m_point(point)
, m_isEdgeProjGood(isEdgeProjGood)
, m_snapToEnds(snapToEnds)
{}
void NearestEdgeFinder::AddInformationSource(IRoadGraph::FullRoadInfo const & roadInfo)
@@ -28,7 +30,7 @@ void NearestEdgeFinder::AddInformationSource(IRoadGraph::FullRoadInfo const & ro
{
m2::ParametrizedSegment<m2::PointD> segment(junctions[i - 1].GetPoint(), junctions[i].GetPoint());
m2::PointD const closestPoint = segment.ClosestPointTo(m_point);
m2::PointD const closestPoint = segment.ClosestPointTo(m_point, m_snapToEnds);
double const squaredDist = m_point.SquaredLength(closestPoint);
if (squaredDist < res.m_squaredDist)
@@ -48,7 +50,7 @@ void NearestEdgeFinder::AddInformationSource(IRoadGraph::FullRoadInfo const & ro
geometry::Altitude const startAlt = segStart.GetAltitude();
geometry::Altitude const endAlt = segEnd.GetAltitude();
m2::ParametrizedSegment<m2::PointD> segment(junctions[idx - 1].GetPoint(), junctions[idx].GetPoint());
m2::PointD const closestPoint = segment.ClosestPointTo(m_point);
m2::PointD const closestPoint = segment.ClosestPointTo(m_point, m_snapToEnds);
double const segLenM = mercator::DistanceOnEarth(segStart.GetPoint(), segEnd.GetPoint());
geometry::Altitude projPointAlt = geometry::kDefaultAltitudeMeters;

8
libs/routing/nearest_edge_finder.hpp
View File

@@ -27,7 +27,12 @@ using IsEdgeProjGood = std::function<bool(std::pair<Edge, geometry::PointWithAlt
class NearestEdgeFinder
{
public:
NearestEdgeFinder(m2::PointD const & point, IsEdgeProjGood const & isEdgeProjGood);
/**
* @param snapToEnds If false, projections of `point` can be anywhere on a segment;
* if true, `point` is projected onto the nearest segment endpoint.
*/
NearestEdgeFinder(m2::PointD const & point, IsEdgeProjGood const & isEdgeProjGood,
bool snapToEnds = false);
inline bool HasCandidates() const { return !m_candidates.empty(); }
@@ -56,6 +61,7 @@ private:
std::vector<EdgeProjectionT> & res) const;
m2::PointD const m_point;
bool m_snapToEnds;
std::vector<Candidate> m_candidates;
IsEdgeProjGood m_isEdgeProjGood;
};

2
libs/routing/road_graph.hpp
View File

@@ -263,7 +263,7 @@ public:
/// then returns empty array.
using EdgeProjectionT = std::pair<Edge, JunctionPointT>;
virtual void FindClosestEdges(m2::RectD const & /*rect*/, uint32_t /*count*/,
std::vector<EdgeProjectionT> & /*vicinities*/) const
std::vector<EdgeProjectionT> & /*vicinities*/, bool snapToEnds) const
{}
/// \returns Vector of pairs FeatureID and corresponding RoadInfo for road features

31
libs/traffxml/traff_decoder.cpp
View File

@@ -964,11 +964,15 @@ void RoutingTraffDecoder::AddDecodedSegment(traffxml::MultiMwmColoring & decoded
void RoutingTraffDecoder::TruncateRoute(std::vector<routing::RouteSegment> & rsegments,
routing::Checkpoints const & checkpoints, bool backwards)
{
double startWeight = 0;
double const endWeight = rsegments.back().GetTimeFromBeginningSec();
// erase leading and trailing fake segments
while(!rsegments.empty() && rsegments.front().GetSegment().GetMwmId() == routing::kFakeNumMwmId)
{
startWeight = rsegments.front().GetTimeFromBeginningSec();
rsegments.erase(rsegments.begin());
}
while(!rsegments.empty() && rsegments.back().GetSegment().GetMwmId() == routing::kFakeNumMwmId)
rsegments.pop_back();
@@ -985,7 +989,7 @@ void RoutingTraffDecoder::TruncateRoute(std::vector<routing::RouteSegment> & rse
// Cost saved by omitting the last `end` segments.
double endSaving = 0;
TruncateStart(rsegments, checkpoints, start, startSaving,
TruncateStart(rsegments, checkpoints, start, startSaving, startWeight,
backwards ? m_endJunctions : m_startJunctions);
TruncateEnd(rsegments, checkpoints, end, endSaving, endWeight,
backwards ? m_startJunctions : m_endJunctions);
@@ -1016,7 +1020,7 @@ void RoutingTraffDecoder::TruncateRoute(std::vector<routing::RouteSegment> & rse
rsegments.erase(rsegments.begin() + end + 1, rsegments.end());
start = 0;
startSaving = 0;
TruncateStart(rsegments, checkpoints, start, startSaving,
TruncateStart(rsegments, checkpoints, start, startSaving, startWeight,
backwards ? m_endJunctions : m_startJunctions);
rsegments.erase(rsegments.begin(), rsegments.begin() + start);
}
@@ -1497,7 +1501,7 @@ std::vector<std::string> ParseRef(std::string const & ref)
void TruncateStart(std::vector<routing::RouteSegment> & rsegments,
routing::Checkpoints const & checkpoints,
size_t & start, double & startSaving,
size_t & start, double & startSaving, double const startWeight,
std::map<m2::PointD, double> const & junctions)
{
if (rsegments.empty())
@@ -1535,6 +1539,16 @@ void TruncateStart(std::vector<routing::RouteSegment> & rsegments,
startSaving = newStartSaving;
}
}
/*
* The router may return a route that starts and ends with a partial segment. In decoder mode,
* we don’t allow starting or ending a segment at an intermediate point but use the nearest
* segment endpoint instead. However, this may leave us with a zero-length segment, which we
* need to remove so we don’t overshoot the reference point.
*/
// TODO not if we have an `at` point
if ((start == 0) && (rsegments.size() > 1)
&& (rsegments[0].GetTimeFromBeginningSec() == startWeight))
start = 1;
}
void TruncateEnd(std::vector<routing::RouteSegment> & rsegments,
@@ -1574,5 +1588,16 @@ void TruncateEnd(std::vector<routing::RouteSegment> & rsegments,
endSaving = newEndSaving;
}
}
/*
* The router may return a route that starts and ends with a partial segment. In decoder mode,
* we don’t allow starting or ending a segment at an intermediate point but use the nearest
* segment endpoint instead. However, this may leave us with a zero-length segment, which we
* need to remove so we don’t overshoot the reference point.
*/
// TODO not if we have an `at` point
if ((end == (rsegments.size() - 1)) && (rsegments.size() > 1)
&& (rsegments[rsegments.size() - 1].GetTimeFromBeginningSec()
== rsegments[rsegments.size() - 2].GetTimeFromBeginningSec()))
end--;
}
} // namespace traffxml

3
libs/traffxml/traff_decoder.hpp
View File

@@ -596,11 +596,12 @@ std::vector<std::string> ParseRef(std::string const & ref);
* @param checkpoints The reference points (at least two)
* @param start Index of the first segment to keep
* @param startSaving Cost saved by truncating
* @param startWeight Weight of the fake segments up to the first real segment
* @param junctions Junctions with the weight of their leap segment
*/
void TruncateStart(std::vector<routing::RouteSegment> & rsegments,
routing::Checkpoints const & checkpoints,
size_t & start, double & startSaving,
size_t & start, double & startSaving, double const startWeight,
std::map<m2::PointD, double> const & junctions);
/**