Detect, correct and optimize route for a straight route

Question

There is a road which is mostly straight except for a few 90° turns. The road when converted to coordinate system gives the following values.

point 1 0.00 1.41
point 2 0.71 2.12
point 3 1.41 2.83
point 4 -1.41 5.66
point 5 -0.71 6.36
point 6 0.00 7.07
point 7 2.83 4.24
point 8 4.24 5.66
point 9 4.95 6.36
point 10 5.66 5.66
point 11 6.36 6.36
point 12 7.07 7.07
point 13 6.36 7.78
point 14 7.07 8.49
point 15 7.78 9.19
point 16 8.49 9.90
point 17 9.19 10.61
point 18 7.78 12.02
point 19 9.19 13.44
point 20 11.31 11.31
point 21 12.02 12.02
point 22 11.31 12.73
point 23 12.02 13.44

q1) Build a python code to detect the points that are out of the straight route. The code should automatically fix such points, so that a straight route is obtained. (Assume a straight route is available always).

q2) There is a straight route available from point1 to point14. However, if we follow the road as given above, it involves unnecessary turns. What points should be corrected, to obtain a straight route?

Below are the approaches I have tried

q1 answer)

import numpy as np

# Function to calculate the distance between two points
def distance(point1, point2):
    x1, y1 = point1
    x2, y2 = point2
    return np.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)

# Function to check if three points are collinear
def are_collinear(p1, p2, p3):
    return abs(distance(p1, p2) + distance(p2, p3) - distance(p1, p3)) !=0

# Iterate through the points to find the points deviating from the straight route
straight_route = [points[1]]
out_of_straight_points = []

for i in range(3, len(points) + 1):
    current_point = points[i]
    if not are_collinear(straight_route[-1], points[i - 1], current_point):
        out_of_straight_points.append(i)
    else:
        straight_route.append(current_point)

# Print the points that are out of the straight route
print("Points out of the straight route:", out_of_straight_points)

# Print the corrected straight route
print("Corrected Straight Route:")
for i, point in enumerate(straight_route):
    print(f"point {i + 1}: {point[0]:.2f} {point[1]:.2f}")

results=

Points out of the straight route: [4, 6, 8, 10, 12, 14, 16, 18, 20, 22]
Corrected Straight Route:
point 1: 0.00 1.41
point 2: 1.41 2.83
point 3: -0.71 6.36
point 4: 2.83 4.24
point 5: 4.95 6.36
point 6: 6.36 6.36
point 7: 6.36 7.78
point 8: 7.78 9.19
point 9: 9.19 10.61
point 10: 9.19 13.44
point 11: 12.02 12.02
point 12: 12.02 13.44

q2 answer)

def calculate_slope(point1, point2):
    x1, y1 = point1
    x2, y2 = point2
    if x2 - x1 == 0:
        return float('inf')  # Vertical line, infinite slope
    return round((y2 - y1) / (x2 - x1),1)
# Define the indices of point 1 and point 14
point1_index = 0
point14_index = 13

# Calculate the expected slope between point 1 and point 14
expected_slope = calculate_slope(points[point1_index], points[point14_index])
print("expected slope = ",expected_slope)

# Identify points that need correction
points_to_correct = []

for i in range(point1_index, point14_index):
    actual_slope = calculate_slope(points[i], points[i + 1])
    print(actual_slope)
    if expected_slope != actual_slope:
        points_to_correct.append(i + 1)

print(f"Points to correct for a straight route: {points_to_correct}")

result

expected slope =  1.0
1.0
1.0
-1.0
1.0
1.0
-1.0
1.0
1.0
-1.0
1.0
1.0
-1.0
1.0
Points to correct for a straight route: [3, 6, 9, 12]

Both approaches are wrong as there can be multiple points in space whose slopes are same but are not in a straight line.

Answer 1

The hint given by user19077881 is the one that actually do the trick.

Below find a revised version to answer q1.

import matplotlib.pyplot as plt
import math

# Function to calculate the distance between two points
def distance(point1, point2):
    x1, y1 = point1
    x2, y2 = point2
    return np.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)

# Function to calculate the slope and offset of the line through two points
def line_equation(p1, p2):
    # m = dy / dx
    m = (p1[1] - p2[1]) / (p1[0] - p2[0])
    # y = mx + q --> q = y - mx
    q = p1[1] - m * p1[0]
    return m, q


# Iterate through the points to find the points deviating from the straight route
is_on_route = [None for i in range(len(points))]
m_route, q_route = line_equation(points[0], points[1])  # Assumption. The first two points are
                                                        # on the straight route
is_on_route[0] = True
is_on_route[1] = True

print("Points out of the straight route:")
for i in range(1, len(points) - 1):
    p_curr = points[i]
    p_next = points[i + 1]
    m, q = line_equation(p_curr, p_next)
    if math.isclose(m_route, m, rel_tol=1e-1) and math.isclose(q_route, q, rel_tol=1e-1):
        is_on_route[i]   = True
        is_on_route[i+1] = True
    else:
        is_on_route[i+1] = False

straight_route = [p for p, on_route in zip(points, is_on_route) if on_route]

# Plot points
fig = plt.figure()
plt.plot(list(zip(*points))[0], 
         list(zip(*points))[1], 
         linestyle = '-',
         marker = 'o',
         color = 'black',
         label = 'original'
         )
plt.plot(list(zip(*straight_route))[0], 
         list(zip(*straight_route))[1], 
         linestyle = '--', 
         marker = 'o', 
         color = 'orange',
         label = 'straight route'
         )
plt.legend()
plt.show()

Which allows you to define the straight root as you can observe below:

Some general advice

• When you are working with arrays, use numpy as it is more intuitive and -generally speaking- more efficient than working with lists.

• The code you posted does not have any plot. Looking at your data in graphical form usually makes the problem waaaaaay more clear than looking at raw numbers!