egg_corgidude.py

import sensor
import image
import lcd
import time

import math

lcd.init(freq=20000000)
lcd.rotation(2)
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.run(1)
egg_threshold   = (50, 127, -128, 127, -128, 127)
while True:
    try:
        img=sensor.snapshot()
        blobs = img.find_blobs([egg_threshold],invert=False, x_stride=1, y_stride=1, pixels_threshold=15000, merge=True)

        axis1 = [0,0,0,0]
        axis3 = [0,0,0,0]

        if blobs:
            for b in blobs:
                Gimg = img.copy(b[0:4])
                Gimg = Gimg.binary([egg_threshold])
                cx = b.w()/2
                cy = b.h()/2

                rmax1 = 0;
                rmin1 = 999;

                max_thata = 0
                for i in range(90):

                    r = 0.0
                    degree = i*2*math.pi/180.0

                    fail = 0;
                    while True:

                        r = r + 1.0
                        dx = r*math.cos(degree)
                        dy = r*math.sin(degree)

                        c = Gimg.get_pixel(int(cx+dx), int(cy+dy))

                        if c is not None:
                            if c[0] == 0:
                                fail = fail + 1
                                if fail == 2:
                                    break

                                if rmax1 < r:
                                    rmax1 = r
                                    axis1[0]=b.x()+int(cx+dx)
                                    axis1[1]=b.y()+int(cy+dy)
                                    max_thata = i*2

                            else:
                                fail = 0
                        else:

                            if rmax1 < r:
                                rmax1 = r
                                axis1[0]=b.x()+int(cx+dx)
                                axis1[1]=b.y()+int(cy+dy)
                                max_thata = i*2
                            break

                rmax2 = 0;
                rmin2 = 999;
                rmax3 = 0;
                if max_thata <  90:
                    for i in range(45):

                        r = 0.0
                        degree = (180 + i*2)*math.pi/180.0
                        fail = 0;
                        while True:

                            r = r + 1.0
                            dx = r*math.cos(degree)
                            dy = r*math.sin(degree)

                            c = Gimg.get_pixel(int(cx+dx), int(cy+dy))

                            if c is not None:
                                if c[0] == 0:

                                    fail = fail + 1
                                    if fail ==2:
                                        break

                                    if rmax2 < r:
                                        rmax2 = r
                                        axis1[2]=b.x()+int(cx+dx)
                                        axis1[3]=b.y()+int(cy+dy)


                                else:
                                    fail = 0
                            else:
                                if rmax2 < r:
                                    rmax2 = r
                                    axis1[2]=b.x()+int(cx+dx)
                                    axis1[3]=b.y()+int(cy+dy)
                                break
                else:
                    for i in range(45):

                        r = 0.0
                        degree = (180 + 90 + i*2)*math.pi/180.0
                        fail = 0;
                        while True:

                            r = r + 1.0
                            dx = r*math.cos(degree)
                            dy = r*math.sin(degree)

                            c = Gimg.get_pixel(int(cx+dx), int(cy+dy))

                            if c is not None:
                                if c[0] == 0:

                                    fail = fail + 1
                                    if fail ==2:
                                        break

                                    if rmax2 < r:
                                        rmax2 = r
                                        axis1[2]=b.x()+int(cx+dx)
                                        axis1[3]=b.y()+int(cy+dy)

                                else:
                                    fail = 0
                            else:
                                if rmax2 < r:
                                    rmax2 = r
                                    axis1[2]=b.x()+int(cx+dx)
                                    axis1[3]=b.y()+int(cy+dy)
                                break




                angle = math.atan2(axis1[3]-axis1[1],axis1[2]-axis1[0])

                x = axis1[0] -b.x()
                y = axis1[1] -b.y()

                for i in range((rmax1+rmax2)/2):


                    dx = x + i*2*math.cos(angle)
                    dy = y + i*2*math.sin(angle)

                    r1 = 0.0
                    r2 = 0.0
                    fail1 = 0;
                    fail2 = 0;
                    is_r1_end = True
                    is_r2_end = True
                    drx1 = 0.0
                    dry1 = 0.0
                    drx2 = 0.0
                    dry2 = 0.0
                    while is_r1_end or is_r2_end:

                        if is_r1_end:
                            r1 = r1 + 1.0
                            drx1 = r1*math.cos(angle + math.pi/2)
                            dry1 = r1*math.sin(angle + math.pi/2)

                            c = Gimg.get_pixel(int(dx+drx1), int(dy+dry1))

                            if c is not None:
                                if c[0] == 0:

                                    fail1 = fail1 + 1
                                    if fail1 == 2:
                                        is_r1_end = False

                                else:
                                    fail1 = 0
                            else:
                                is_r1_end = False

                        if is_r2_end:
                            r2 = r2 + 1.0
                            drx2 = r2*math.cos(angle - math.pi/2)
                            dry2 = r2*math.sin(angle - math.pi/2)

                            c = Gimg.get_pixel(int(dx+drx2), int(dy+dry2))

                            if c is not None:
                                if c[0] == 0:

                                    fail2 = fail2 + 1
                                    if fail2 == 2:
                                        is_r2_end = False

                                else:
                                    fail2 = 0
                            else:
                                is_r2_end = False

                    if rmax3 < (r1 + r2):
                        rmax3 = (r1 + r2)
                        axis3[0]=b.x()+int(dx+drx1)
                        axis3[1]=b.y()+int(dy+dry1)
                        axis3[2]=b.x()+int(dx+drx2)
                        axis3[3]=b.y()+int(dy+dry2)


                #tmp=img.draw_rectangle(b[0:4])
                tmp=img.draw_cross(b[5], b[6],(0,0,255))

                tmp=img.draw_line(axis3[0], axis3[1],axis3[2], axis3[3],(0,255,0),thickness = 3)
                tmp=img.draw_line(axis1[0], axis1[1],axis1[2], axis1[3],(255,0,0),thickness = 3)


                img.draw_string(45, 5, "H= "+str(int(rmax1+rmax2)), color=(255,0,0), scale=2)
                img.draw_string(45, 30, "W= "+str(int(rmax3)), color=(0,255,0), scale=2)
                img.draw_string(45, 55, "A= "+str(int(b.pixels())), color=(0,0,255), scale=2)

                H_mm = 0.2745*(rmax1+rmax2) +0.2238
                W_mm = 0.2745*(rmax3) +0.2238

                img.draw_string(45, 185, "H(mm)= "+str(int(H_mm)), color=(255,0,0), scale=2)
                img.draw_string(45, 210, "W(mm)= "+str(int(W_mm)), color=(0,255,0), scale=2)
                #tmp=img.get_pixel(blobs[5], blobs[6])

                print(b)
                print("rmax",rmax1+rmax2,"rmin",rmax3)

                del Gimg

        lcd.display(img)
        print()
        #lcd.display(img)
    except Exception as e:
        print(e)

    time.sleep(0.01)