PythonRobotics机器人算法-映射算法之目标形状识别(Object shape recognition using circle fitting)


1.利用圆拟合进行目标形状识别(Object shape recognition using circle fitting)

下面展示一下基于python实现的利用圆拟合进行目标形状识别。

(1)定义初始参数

1
2
3
4
5
6
import matplotlib.pyplot as plt
import math
import random
import numpy as np

show_animation = True

(2)算法过程

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
def circle_fitting(x, y):
    """
    Circle Fitting with least squared
        input: point x-y positions
        output  cxe x center position
                cye y center position
                re  radius of circle
                error: prediction error
    """

    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] / -2)
    cye = float(T[1] / -2)
    re = math.sqrt(cxe* * 2 + cye* * 2 - T[2])
    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 range(len(thetal)):
        angleid = math.floor(thetal[i] / angle_reso)
        if rangedb[angleid] > rangel[i]:
            rangedb[angleid] = rangel[i]
    for i in range(len(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

(3)结果可视化

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
def plot_circle(x, y, size, color="-b"):
    deg = list(range(0, 360, 5))
    deg.append(0)
    xl = [x + size * math.cos(math.radians(d)) for d in deg]
    yl = [y + size * math.sin(math.radians(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 = math.radians(30.0)      # obstacle moving direction
    angle_reso = math.radians(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:
            plt.cla()
            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()

执行结果如下。下面为利用圆拟合进行目标形状识别的实例。蓝色圆圈是实际的目标形状;红色叉是通过距离传感器观测到的点;红色圆圈是利用圆拟合估计的目标形状。

参考文献

  1. https://pythonrobotics.readthedocs.io/en/latest/
  2. https://github.com/AtsushiSakai/PythonRobotics/
  3. PythonRobotics: a Python code collection of robotics algorithms

进化学习团队将会根据大家意见和建议持续修改、维护与更新。转载请注明出处(进化学习: https://www.evolutionarylearn.com/paper/ra-pythonrobotics-mapping-osr/)。

赞赏

微信赞赏支付宝赞赏

Have any Question or Comment?

发表评论

电子邮件地址不会被公开。 必填项已用*标注

热门主题 & 页面

分类目录

博客统计

无点击次数。