cylinder_test.py

#!/usr/bin/env python
 
# cylinder test
 
import math
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from matplotlib.patches import Circle, PathPatch
import random
# rectangle
 
# first, full rectangls [1, 1] - [-1, 1] - [-1, -1] - [1, -1]
LINES = np.array([[1, 1], [-1, 1], [-1, -1], [1, -1], [-2, 1], [-1, -2]])
SAMPLE_NUM = 400
ITER_NUM = 300
TRIAL_NUM = 400
def estimateCircle(points):
    min_ev = 1000000000000.0
    min_param = (0, 0, 1)
    for i in range(ITER_NUM):
        ai = random.randint(0, len(points) - 1)
        bi = random.randint(0, len(points) - 1)
        ci = random.randint(0, len(points) - 1)
        if ai == bi or bi == ci or ci == ai:
            continue
        try:
            (cx, cy, r) = _estimateCircle(points[ai], points[bi], points[ci])
            ev = 0
            for p in points:
                d = math.sqrt((cx - p[0]) * (cx - p[0]) + (cy - p[1]) * (cy - p[1])) - r
                dd = abs(d) * abs(d)
                ev = ev + dd
            if min_ev > ev:
                min_param = (cx, cy, r)
        except np.linalg.LinAlgError:
            print "singular equal"
        except ValueError:
            print "singular equal"
    return min_param
def _estimateCircle(A, B, C):
    # x^2 + y^2 + ax + bx + c = 0
    # a A[0] + bA[1] = XX
    # a B[0] + bB[1] = YY
    
    # A[0] A[1]  a    XX
    #               
    # B[0] B[1]  b =  YY
    # 
    # C[0] C[1] c =  ZZ
    mat = np.matrix([[A[0], A[1], 1],[B[0], B[1], 1], [C[0], C[1], 1]])
    XX = -(A[0] * A[0] + A[1] * A[1])
    YY = -(B[0] * B[0] + B[1] * B[1])
    ZZ = -(C[0] * C[0] + C[1] * C[1])
    result = np.dot(mat.I, np.array([XX, YY, ZZ]))
    a = result[0, 0]
    b = result[0, 1]
    c = result[0, 2]
    cx = - a / 2
    cy = - b / 2
    r = math.sqrt(- c + cx * cx + cy * cy)
    return (cx, cy, r)
 
def genPointSet0():
    points = []
    for i in range(SAMPLE_NUM/4):
        r = i / float(SAMPLE_NUM/4)
        points.append(LINES[0] * (1 - r) + LINES[1] * r)
    for i in range(SAMPLE_NUM/4):
        r = i / float(SAMPLE_NUM/4)
        points.append(LINES[1] * (1 - r) + LINES[2] * r)
    return points
def genPointSet1():
    points = []
    for i in range(SAMPLE_NUM/4):
        r = i / float(SAMPLE_NUM/4)
        points.append(LINES[0] * (1 - r) + LINES[1] * r)
    for i in range(SAMPLE_NUM/4):
        r = i / float(SAMPLE_NUM/4)
        points.append(LINES[1] * (1 - r) + LINES[2] * r)
    for i in range(SAMPLE_NUM/4):
        r = i / float(SAMPLE_NUM/4)
        points.append(LINES[2] * (1 - r) + LINES[3] * r)
    return points
def genPointSet2():
    points = []
    for i in range(SAMPLE_NUM/4):
        r = i / float(SAMPLE_NUM/4)
        points.append(LINES[0] * (1 - r) + LINES[1] * r)
    for i in range(SAMPLE_NUM/4):
        r = i / float(SAMPLE_NUM/4)
        points.append(LINES[1] * (1 - r) + LINES[2] * r)
    for i in range(SAMPLE_NUM/4):
        r = i / float(SAMPLE_NUM/4)
        points.append(LINES[2] * (1 - r) + LINES[3] * r)
    for i in range(SAMPLE_NUM/4):
        r = i / float(SAMPLE_NUM/4)
        points.append(LINES[3] * (1 - r) + LINES[0] * r)
    return points
def genPointSet3():
    points = []
    mu, sigma = 0, 0.1
    for i in range(SAMPLE_NUM/4):
        r = i / float(SAMPLE_NUM/4)
        points.append(LINES[0] * (1 - r) + LINES[1] * r + np.random.normal(mu, sigma, 2))
    for i in range(SAMPLE_NUM/4):
        r = i / float(SAMPLE_NUM/4)
        points.append(LINES[1] * (1 - r) + LINES[2] * r + np.random.normal(mu, sigma, 2))
    return points
def genPointSet4():
    points = []
    mu, sigma = 0, 0.1
    for i in range(SAMPLE_NUM/4):
        r = i / float(SAMPLE_NUM/4)
        points.append(LINES[0] * (1 - r) + LINES[4] * r + np.random.normal(mu, sigma, 2))
    for i in range(SAMPLE_NUM/4):
        r = i / float(SAMPLE_NUM/4)
        points.append(LINES[4] * (1 - r) + LINES[2] * r + np.random.normal(mu, sigma, 2))
    return points
def genPointSet5():
    points = []
    mu, sigma = 0, 0.1
    for i in range(SAMPLE_NUM/4):
        r = i / float(SAMPLE_NUM/4)
        points.append(LINES[0] * (1 - r) + LINES[1] * r + np.random.normal(mu, sigma, 2))
    for i in range(SAMPLE_NUM/4):
        r = i / float(SAMPLE_NUM/4)
        points.append(LINES[1] * (1 - r) + LINES[5] * r + np.random.normal(mu, sigma, 2))
    return points
 
 
fig1, axis1 = plt.subplots()
 
plt.axis([-5, 5, -5, 5])
def ani(i):
    # if i < 100:
    #     points = genPointSet0()
    # elif i < 200:
    #     points = genPointSet1()
    # elif i < 300:
    #     points = genPointSet2()
    # elif i < 400:
    if i < 100:
        points = genPointSet3()
    elif i < 200:
        points = genPointSet4()
    elif i < 400:
        points = genPointSet5()
    
    try:
        xs = [p[0] for p in points]
        ys = [p[1] for p in points]
        (cx, cy, r) = estimateCircle(points)
        if abs(cx) > 100000 or abs(cy) > 10000 or abs(r) > 10000:                   #too large
            return
        circle = Circle((cx, cy), r, facecolor="none")
        axis1.cla()
        axis1.add_patch(circle)
        #plt.plot([ai[0], ci[0], ci[0]], [ai[1], ci[1], ci[1]], 'ro')
        plt.axis([-5, 5, -5, 5])
        plt.plot(xs, ys, 'o')
        plt.plot([cx], [cy], 'ro')
        ax = sum(xs) / len(xs)
        ay = sum(ys) / len(ys)
        plt.plot([ax], [ay], 'go')
    except np.linalg.LinAlgError:
        print "singular equal"
    except ValueError:
        print "singular equal"
    
line_ani = animation.FuncAnimation(fig1, ani, np.arange(TRIAL_NUM), interval=25)
 
    
plt.show()