mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-12 17:58:08 -05:00
5392fcff4d
* Refactor module links and improve code documentation. Updated documentation to rename "API" sections to "Code Link" for clarity and consistency. Enhanced docstrings for `circle_fitting` and `kmeans_clustering` functions, improving parameter descriptions and adding return value details. Fixed typos in function and file names in the ray casting grid map module. * Fix import typo in ray casting grid map test module. Corrected the import statement in the test file by updating the module's name to `ray_casting_grid_map` for consistency with the source file. This ensures proper functionality of the test suite.
163 lines
4.4 KiB
Python
163 lines
4.4 KiB
Python
"""
|
|
|
|
Object shape recognition with circle fitting
|
|
|
|
author: Atsushi Sakai (@Atsushi_twi)
|
|
|
|
"""
|
|
|
|
import matplotlib.pyplot as plt
|
|
import math
|
|
import random
|
|
import numpy as np
|
|
|
|
show_animation = True
|
|
|
|
|
|
def circle_fitting(x, y):
|
|
"""
|
|
Fits a circle to a given set of points using a least-squares approach.
|
|
|
|
This function calculates the center (x, y) and radius of a circle that best fits
|
|
the given set of points in a two-dimensional plane. It minimizes the squared
|
|
errors between the circle and the provided points and returns the calculated
|
|
center coordinates, radius, and the fitting error.
|
|
|
|
Raises
|
|
------
|
|
ValueError
|
|
If the input lists x and y do not contain the same number of elements.
|
|
|
|
Parameters
|
|
----------
|
|
x : list[float]
|
|
The x-coordinates of the points.
|
|
y : list[float]
|
|
The y-coordinates of the points.
|
|
|
|
Returns
|
|
-------
|
|
tuple[float, float, float, float]
|
|
A tuple containing:
|
|
- The x-coordinate of the center of the fitted circle (float).
|
|
- The y-coordinate of the center of the fitted circle (float).
|
|
- The radius of the fitted circle (float).
|
|
- The fitting error as a deviation metric (float).
|
|
"""
|
|
|
|
sumx = sum(x)
|
|
sumy = sum(y)
|
|
sumx2 = sum([ix ** 2 for ix in x])
|
|
sumy2 = sum([iy ** 2 for iy in y])
|
|
sumxy = sum([ix * iy for (ix, iy) in zip(x, y)])
|
|
|
|
F = np.array([[sumx2, sumxy, sumx],
|
|
[sumxy, sumy2, sumy],
|
|
[sumx, sumy, len(x)]])
|
|
|
|
G = np.array([[-sum([ix ** 3 + ix * iy ** 2 for (ix, iy) in zip(x, y)])],
|
|
[-sum([ix ** 2 * iy + iy ** 3 for (ix, iy) in zip(x, y)])],
|
|
[-sum([ix ** 2 + iy ** 2 for (ix, iy) in zip(x, y)])]])
|
|
|
|
T = np.linalg.inv(F).dot(G)
|
|
|
|
cxe = float(T[0, 0] / -2)
|
|
cye = float(T[1, 0] / -2)
|
|
re = math.sqrt(cxe**2 + cye**2 - T[2, 0])
|
|
|
|
error = sum([np.hypot(cxe - ix, cye - iy) - re for (ix, iy) in zip(x, y)])
|
|
|
|
return (cxe, cye, re, error)
|
|
|
|
|
|
def get_sample_points(cx, cy, cr, angle_reso):
|
|
x, y, angle, r = [], [], [], []
|
|
|
|
# points sampling
|
|
for theta in np.arange(0.0, 2.0 * math.pi, angle_reso):
|
|
nx = cx + cr * math.cos(theta)
|
|
ny = cy + cr * math.sin(theta)
|
|
nangle = math.atan2(ny, nx)
|
|
nr = math.hypot(nx, ny) * random.uniform(0.95, 1.05)
|
|
|
|
x.append(nx)
|
|
y.append(ny)
|
|
angle.append(nangle)
|
|
r.append(nr)
|
|
|
|
# ray casting filter
|
|
rx, ry = ray_casting_filter(x, y, angle, r, angle_reso)
|
|
|
|
return rx, ry
|
|
|
|
|
|
def ray_casting_filter(xl, yl, thetal, rangel, angle_reso):
|
|
rx, ry = [], []
|
|
rangedb = [float("inf") for _ in range(
|
|
int(math.floor((math.pi * 2.0) / angle_reso)) + 1)]
|
|
|
|
for i, _ in enumerate(thetal):
|
|
angleid = math.floor(thetal[i] / angle_reso)
|
|
|
|
if rangedb[angleid] > rangel[i]:
|
|
rangedb[angleid] = rangel[i]
|
|
|
|
for i, _ in enumerate(rangedb):
|
|
t = i * angle_reso
|
|
if rangedb[i] != float("inf"):
|
|
rx.append(rangedb[i] * math.cos(t))
|
|
ry.append(rangedb[i] * math.sin(t))
|
|
|
|
return rx, ry
|
|
|
|
|
|
def plot_circle(x, y, size, color="-b"): # pragma: no cover
|
|
deg = list(range(0, 360, 5))
|
|
deg.append(0)
|
|
xl = [x + size * math.cos(np.deg2rad(d)) for d in deg]
|
|
yl = [y + size * math.sin(np.deg2rad(d)) for d in deg]
|
|
plt.plot(xl, yl, color)
|
|
|
|
|
|
def main():
|
|
|
|
# simulation parameters
|
|
simtime = 15.0 # simulation time
|
|
dt = 1.0 # time tick
|
|
|
|
cx = -2.0 # initial x position of obstacle
|
|
cy = -8.0 # initial y position of obstacle
|
|
cr = 1.0 # obstacle radious
|
|
theta = np.deg2rad(30.0) # obstacle moving direction
|
|
angle_reso = np.deg2rad(3.0) # sensor angle resolution
|
|
|
|
time = 0.0
|
|
while time <= simtime:
|
|
time += dt
|
|
|
|
cx += math.cos(theta)
|
|
cy += math.cos(theta)
|
|
|
|
x, y = get_sample_points(cx, cy, cr, angle_reso)
|
|
|
|
ex, ey, er, error = circle_fitting(x, y)
|
|
print("Error:", error)
|
|
|
|
if show_animation: # pragma: no cover
|
|
plt.cla()
|
|
# for stopping simulation with the esc key.
|
|
plt.gcf().canvas.mpl_connect('key_release_event',
|
|
lambda event: [exit(0) if event.key == 'escape' else None])
|
|
plt.axis("equal")
|
|
plt.plot(0.0, 0.0, "*r")
|
|
plot_circle(cx, cy, cr)
|
|
plt.plot(x, y, "xr")
|
|
plot_circle(ex, ey, er, "-r")
|
|
plt.pause(dt)
|
|
|
|
print("Done")
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|