Skip to content

Commit

Permalink
Relies on GetClosestPoint for the GetClosestPointUsing2dProjection me…
Browse files Browse the repository at this point in the history 
…thod. (#53)

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
  • Loading branch information
@francocipollone
francocipollone committed Feb 14, 2023
1 parent ae4bda5 commit 471c19d
Show file tree
Hide file tree
Showing 3 changed files with 75 additions and 51 deletions.
22 changes: 19 additions & 3 deletions include/maliput_sparse/geometry/utility/geometry.h
View file
Expand Up @@ -45,6 +45,18 @@ struct BoundPointsResult {
double length;
};

/// Holds the result of #GetClosestPointToSegment method.
/// @tparam CoordinateT The coordinate type.
template <typename CoordinateT>
struct ClosestPointToSegmentResult {
/// P coordinate in the line string matching the closest point.
double p;
/// Closest point.
CoordinateT point;
/// Distance between the closest point and the given point.
double distance;
};

/// Holds the result of #GetClosestPoint method.
/// @tparam CoordinateT The coordinate type.
template <typename CoordinateT>
Expand All @@ -55,10 +67,14 @@ struct ClosestPointResult {
CoordinateT point;
/// Distance between the closest point and the given point.
double distance;
/// Linestring's segment that contains the closest point.
typename LineString<CoordinateT>::Segment segment;
};

using ClosestPointResult3d = ClosestPointResult<maliput::math::Vector3>;
using ClosestPointResult2d = ClosestPointResult<maliput::math::Vector2>;
using ClosestPointToSegmentResult3d = ClosestPointToSegmentResult<maliput::math::Vector3>;
using ClosestPointToSegmentResult2d = ClosestPointToSegmentResult<maliput::math::Vector2>;

/// Computes a 3-dimensional centerline out of the @p left and @p right line string.
///
Expand Down Expand Up @@ -129,9 +145,9 @@ maliput::math::Vector3 GetTangentAtP(const LineString3d& line_string, double p,
/// @return A ClosestPointResult struct containing the closest point, the distance between the closest point and @p xyz
/// and the p coordinate in the segment matching the closest point.
template <typename CoordinateT = maliput::math::Vector3>
ClosestPointResult<CoordinateT> GetClosestPointToSegment(const CoordinateT& start_segment_point,
const CoordinateT& end_segment_point,
const CoordinateT& coordinate, double tolerance);
ClosestPointToSegmentResult<CoordinateT> GetClosestPointToSegment(const CoordinateT& start_segment_point,
const CoordinateT& end_segment_point,
const CoordinateT& coordinate, double tolerance);

/// Gets the closest point in the @p line_string to the given @p xyz point.
/// @param line_string LineString3d to be computed the closest point from.
Expand Down
70 changes: 32 additions & 38 deletions src/geometry/utility/geometry.cc
View file
Expand Up @@ -328,9 +328,9 @@ Vector3 GetTangentAtP(const LineString3d& line_string, double p, double toleranc
}

template <typename CoordinateT>
ClosestPointResult<CoordinateT> GetClosestPointToSegment(const CoordinateT& start_segment_point,
const CoordinateT& end_segment_point,
const CoordinateT& coordinate, double tolerance) {
ClosestPointToSegmentResult<CoordinateT> GetClosestPointToSegment(const CoordinateT& start_segment_point,
const CoordinateT& end_segment_point,
const CoordinateT& coordinate, double tolerance) {
if (start_segment_point == end_segment_point) {
return {0., start_segment_point, (start_segment_point - coordinate).norm()};
}
Expand All @@ -350,7 +350,7 @@ ClosestPointResult<CoordinateT> GetClosestPointToSegment(const CoordinateT& star
ClosestPointResult3d GetClosestPoint(const LineString3d& line_string, const maliput::math::Vector3& xyz,
double tolerance) {
std::optional<LineString3d::Segment> closest_segment{std::nullopt};
ClosestPointResult3d segment_closest_point_result;
ClosestPointToSegmentResult3d segment_closest_point_result;
segment_closest_point_result.distance = std::numeric_limits<double>::max();

const std::vector<LineString3d::Point>& line_string_points = line_string.points();
Expand Down Expand Up @@ -395,43 +395,35 @@ ClosestPointResult3d GetClosestPoint(const LineString3d& line_string, const mali
nearest_point.p().value(), line_string_points.at(nearest_idx + 1).p().value()}};
}
return {segment_closest_point_result.p + closest_segment->p_interval.min, segment_closest_point_result.point,
segment_closest_point_result.distance};
segment_closest_point_result.distance, closest_segment.value()};
}

ClosestPointResult3d GetClosestPointUsing2dProjection(const LineString3d& line_string,
const maliput::math::Vector3& xyz, double tolerance) {
// Get the closest segment in 3d.
const LineString3d::Segment closest_segment_3d = GetClosestPoint(line_string, xyz, tolerance).segment;
const auto& start = line_string[closest_segment_3d.idx_start];
const auto& end = line_string[closest_segment_3d.idx_end];

// Once we have the segment let's project the point to the segment in 2d.
const auto start_2d = To2D(start);
const auto end_2d = To2D(end);
const maliput::math::Vector2 xy = To2D(xyz);

// Find the closest segment in 2D
std::optional<LineString3d::Segment> closest_segment{std::nullopt};
ClosestPointResult2d segment_closest_point_result;
segment_closest_point_result.distance = std::numeric_limits<double>::max();

const auto& segments = line_string.segments();
std::for_each(segments.begin(), segments.end(), [&](const auto& segment) {
const auto& start = line_string[segment.second.idx_start];
const auto& end = line_string[segment.second.idx_end];
const auto current_closest_point_res = GetClosestPointToSegment(To2D(start), To2D(end), xy, tolerance);
if (current_closest_point_res.distance < segment_closest_point_result.distance) {
segment_closest_point_result = current_closest_point_res;
closest_segment = {segment.second.idx_start, segment.second.idx_end, segment.second.p_interval};
}
});

// Analyze closest segment and compute the closest point in 3D.
const auto& start_point = line_string[closest_segment->idx_start];
const auto& end_point = line_string[closest_segment->idx_end];
const double segment_3d_length = (end_point - start_point).norm();
const Segment2d segment_2d{To2D(start_point), To2D(end_point)};

const double scale_p = segment_3d_length / (segment_2d.first - segment_2d.second).norm();
const double p_3d = segment_closest_point_result.p * scale_p;
const double z_coordinate = start_point.z() + ((end_point - start_point).normalized() * p_3d).z();
// Get the closest point in 2d.
const ClosestPointToSegmentResult2d closest_point_to_segment_2d =
GetClosestPointToSegment(start_2d, end_2d, xy, tolerance);

// Scale the p coordinate in 2d to the 3d segment length to obtain the correct p coordinate in 3d.
const double segment_3d_length = (start - end).norm();
const double scale_p = segment_3d_length / (start_2d - end_2d).norm();
const double p_3d = closest_point_to_segment_2d.p * scale_p;
const double z_coordinate = start.z() + ((end - start).normalized() * p_3d).z();
// Compound closest point in 3d.
const maliput::math::Vector3 closest_point_3d{segment_closest_point_result.point.x(),
segment_closest_point_result.point.y(), z_coordinate};
const maliput::math::Vector3 closest_point_3d{closest_point_to_segment_2d.point.x(),
closest_point_to_segment_2d.point.y(), z_coordinate};

return {p_3d + closest_segment->p_interval.min, closest_point_3d, (xyz - closest_point_3d).norm()};
return {p_3d + closest_segment_3d.p_interval.min, closest_point_3d, (xyz - closest_point_3d).norm(),
closest_segment_3d};
}

double ComputeDistance(const LineString3d& lhs, const LineString3d& rhs, double tolerance) {
Expand All @@ -450,10 +442,12 @@ double ComputeDistance(const LineString3d& lhs, const LineString3d& rhs, double
template maliput::math::Vector3 InterpolatedPointAtP(const LineString3d&, double, double);
template maliput::math::Vector2 InterpolatedPointAtP(const LineString2d&, double, double);

template ClosestPointResult3d GetClosestPointToSegment(const maliput::math::Vector3&, const maliput::math::Vector3&,
const maliput::math::Vector3&, double);
template ClosestPointResult2d GetClosestPointToSegment(const maliput::math::Vector2&, const maliput::math::Vector2&,
const maliput::math::Vector2&, double);
template ClosestPointToSegmentResult3d GetClosestPointToSegment(const maliput::math::Vector3&,
const maliput::math::Vector3&,
const maliput::math::Vector3&, double);
template ClosestPointToSegmentResult2d GetClosestPointToSegment(const maliput::math::Vector2&,
const maliput::math::Vector2&,
const maliput::math::Vector2&, double);

template class ClosestPointResult<maliput::math::Vector3>;
template class ClosestPointResult<maliput::math::Vector2>;
Expand Down
34 changes: 24 additions & 10 deletions test/geometry/utility/geometry_test.cc
View file
Expand Up @@ -391,7 +391,7 @@ INSTANTIATE_TEST_CASE_P(Get2DHeadingAtPTestGroup, Get2DHeadingAtPTest, ::testing
struct GetClosestPointToSegmentTestCase {
std::pair<maliput::math::Vector3, maliput::math::Vector3> segment;
std::vector<maliput::math::Vector3> eval_points;
std::vector<ClosestPointResult3d> expected_closest{};
std::vector<ClosestPointToSegmentResult3d> expected_closest{};
};

std::vector<GetClosestPointToSegmentTestCase> GetClosestPointToSegmentTestCases() {
Expand Down Expand Up @@ -461,15 +461,15 @@ std::vector<GetClosestPointLineStringTestCase> GetClosestPointLineStringTestCase
{LineString3d{{0., 0., 0.}, {10., 0., 0.}} /* line_string */,
{{5., 5., 0.}, {-5., 5., 0.}, {10., 5., 0.}, {15., 5., 0.}} /* eval points */,
{
{5., {5., 0., 0.}, 5.}, /* expected: p, point, distance */
{0., {0., 0., 0.}, 5. * std::sqrt(2.)},
{10., {10., 0., 0.}, 5.},
{10., {10., 0., 0.}, 5. * std::sqrt(2.)},
{5., {5., 0., 0.}, 5., {0, 1, {0., 10.}}}, /* expected: p, point, distance, segment */
{0., {0., 0., 0.}, 5. * std::sqrt(2.), {0, 1, {0., 10.}}},
{10., {10., 0., 0.}, 5., {0, 1, {0., 10.}}},
{10., {10., 0., 0.}, 5. * std::sqrt(2.), {0, 1, {0., 10.}}},
}},
{LineString3d{{0., 0., 0.}, {10., 0., 0.}, {15., 0., 0.}} /* line_string */,
{{12.5, 0., 0.}} /* eval points */,
{
{12.5, {12.5, 0., 0.}, 0.}, /* expected: p, point, distance */
{12.5, {12.5, 0., 0.}, 0., {1, 2, {10., 15.}}}, /* expected: p, point, distance, segment */
}},
{LineString3d{{-2., -4., -6.},
{0., -2., -4.},
Expand All @@ -479,10 +479,12 @@ std::vector<GetClosestPointLineStringTestCase> GetClosestPointLineStringTestCase
{40., 20., 10.}} /* line_string */,
{{-1., -3., -5.}, {5., 5., 5.}} /* eval points */,
{
{std::sqrt(3.), {-1., -3., -5.}, 0.}, /* expected: p, point, distance */
{std::sqrt(3.), {-1., -3., -5.}, 0., {0, 1, {0., 3.4641016151377544}}}, /* expected: p, point, distance,
segment */
{std::sqrt(2. * 2. + 2. * 2. + 2. * 2.) + std::sqrt(2. * 2. + 4. * 4.) + 5.,
{5., 0., 0.},
5. * std::sqrt(2.)},
5. * std::sqrt(2.),
{2, 3, {7.936237570137334, 17.936237570137333}}},
}},
};
}
Expand All @@ -500,6 +502,10 @@ TEST_P(GetClosestPointLineStringTest, Test) {
EXPECT_NEAR(case_.expected_closest[i].p, dut.p, kTolerance);
EXPECT_TRUE(maliput::math::test::CompareVectors(case_.expected_closest[i].point, dut.point, kTolerance));
EXPECT_NEAR(case_.expected_closest[i].distance, dut.distance, kTolerance);
EXPECT_EQ(case_.expected_closest[i].segment.idx_start, dut.segment.idx_start);
EXPECT_EQ(case_.expected_closest[i].segment.idx_end, dut.segment.idx_end);
EXPECT_NEAR(case_.expected_closest[i].segment.p_interval.min, dut.segment.p_interval.min, kTolerance);
EXPECT_NEAR(case_.expected_closest[i].segment.p_interval.max, dut.segment.p_interval.max, kTolerance);
}
}

Expand All @@ -524,8 +530,12 @@ std::vector<GetClosestPointLineStringTestCase> GetClosestPointIn2dLineStringTest
{LineString3d{{0., 0., 0.}, {10., 0., 10.}} /* line_string */,
{{5., 0., 0.}, {5., -2., 0.}} /* eval points */,
{
{10 * std::sqrt(2.) / 2., {5., 0., 5.}, 5.}, /* expected: p, point, distance */
{10 * std::sqrt(2.) / 2., {5., 0., 5.}, 5.385164807134504}, /* expected: p, point, distance */
{10 * std::sqrt(2.) / 2., {5., 0., 5.}, 5., {0, 1, {0., 10. * std::sqrt(2.)}}}, /* expected: p, point,
distance, segment */
{10 * std::sqrt(2.) / 2.,
{5., 0., 5.},
5.385164807134504,
{0, 1, {0., 10. * std::sqrt(2.)}}}, /* expected: p, point, distance, segment */
}},
};
}
Expand All @@ -543,6 +553,10 @@ TEST_P(GetClosestPointIn2dLineStringTest, Test) {
EXPECT_NEAR(case_.expected_closest[i].p, dut.p, kTolerance);
EXPECT_TRUE(maliput::math::test::CompareVectors(case_.expected_closest[i].point, dut.point, kTolerance));
EXPECT_NEAR(case_.expected_closest[i].distance, dut.distance, kTolerance);
EXPECT_EQ(case_.expected_closest[i].segment.idx_start, dut.segment.idx_start);
EXPECT_EQ(case_.expected_closest[i].segment.idx_end, dut.segment.idx_end);
EXPECT_NEAR(case_.expected_closest[i].segment.p_interval.min, dut.segment.p_interval.min, kTolerance);
EXPECT_NEAR(case_.expected_closest[i].segment.p_interval.max, dut.segment.p_interval.max, kTolerance);
}
}

Expand Down

0 comments on commit 471c19d

Please sign in to comment.