Implementation : Vision Chapter 1

Vision

การทำงานนส่วนของ Vision จะทำหน้าที่เป็น Input รับข้อมูลสะภาพแวดล้อมทางกายภาพแล้วแปลงข้อมูลเป็นภาพสองมิติและสามมิติ โดยใช้อุปกรณ์ที่เรียกว่า Kinect แล้วทำการประมวลภาพผลเพื่อให้ได้ตำแหน่งของวัตถุที่สนใจเพื่อส่งให้ Task Planning ทำงานในขั้นต่อไป

Process

การทำงานของ Vision ส่วนที่ 1 จะรับหน้าที่ในการหารูปร่างของวัตถุชิ้นงานและตำแหน่งของชิ้นงานนั้น จากนนั้นจะส่งข้อมูลตำแหน่งและรูปร่างกับไปให้ Task Planning ดังนี้

1 ) นำข้อมูล DEPTH ทำการเปลี่ยนจุดอ้างอิงของระยะทางจาก "ตัว Kinect" เป็นพื้นของพื้นที่การทำงานด้วย

img_depth = (img_depth - CALIBRATE_FLOOR) * -1

เมื่อทำการแสดงค่าตำแหน่งสามมิติ x, y, z (มีความคลาดเคลื่อน) ของเครื่องหมาย "+" ที่อ้างอิงจากพื้นจะได้ได้ดังรูปที่ 1

รูปที่ 1 ภาพ RGB ที่ผ่านการกรองด้วย HSV space แล้วแสดงตำแหน่งของเครื่องหมาย "+"

2) ทำการเปลี่ยน color space ของภาพ RGB จาก RGB เป็น HSV จากนั้นทำการ threshold ภาพ HSV ที่มีค่า hue : 0 - 179, satuate : 0 - 255, value : 100 - 255 เพื่อทำการตัดพื้นหลังและแสงรบกวน  ทำการแปลงภาพ HSV กลับเป็น RGB ดังรูปที่ 1
3) ทำการหาขอบ (edge) ดังรูปที่ X ภาพโดยใช้ Canny Method เพื่อที่จะนำไปใช้ในการหารูปร่างของวัตถุในขั้นถัดไป
4) ทำการหา Contour จากภาพ edge ด้วยคำสั่ง findContours

cv2.findContours(img_edge, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

แล้วนำ Contour ทั้งหมดที่ได้ไปคำนวณว่า Contour เป็นรูปร่างอะไรด้วยคำสั่ง approxPolyDP จะได้ผลดังรูปที่ 2

approx = cv2.approxPolyDP(contour, epsilon, True)

รูปที่ 2 Contour ทั้งหมดที่ตรวจพบ

5) กรองรูปร่างจาก Contour ที่ต้องการคือรูป สามเปลี่ยม และรูปสีเหลี่ยมเท่านั้น   จากนั้นนำรูปที่ได้มาหามุมของรูปร่าง (Corner) เพื่อคำนวณหามุม (Rotation) ของวัตถุดังนี้
5.1) รูปสามเหลี่ยม : หา vector จากจุดที่เป็นมุมฉากของสามเหลี่ยม (คู่ของจุดที่สั้นที่สุด) แล้วนำ vector ทั้งสองมารวมกัน  นำผลลัพธ์ของ vector ไปหามุมเทียบกับแกน X แล้วนำมุมที่ได้มาลบ 45 องศา (เปลี่ยนแกนอ้างอิงให้ตรงกับ ABB Yumi) จะได้ขนาดมุมของรูปสามเหลี่ยมมุมฉากดังรูปที่ 3

                vec1 = P1 - ORG
                vec2 = P2 - ORG
                dir = vec1 + vec2
                _ROTATION2 = arctan(dir[1] / dir[0])
                _ROTATION2 = -(_ROTATION2 * (180 / PI))
                _ROTATION2 = _ROTATION2 - 45

รูปที่ 3 การหามุมของสามเหลี่ยม

5.2) รูปสี่เหลี่ยม : หา vector ที่มีระยะสั้นที่สุด (คู่ของจุดที่สั้นที่สุด) แล้วนำ vector ไปหามุมเทียบกับแกน X แล้วนำมุมที่ได้มาบวก 90 องศา (เปลี่ยนแกนอ้างอิงให้ตรงกับ ABB Yumi) จะได้ขนาดมุมของรูปสี่เหลี่ยมดังรูปที่ 4

                dir = P1 - P2
                _ROTATION2 = arctan(dir[1] / dir[0])
                _ROTATION2 = -(_ROTATION2 * (180 / PI))
                _ROTATION2 = _ROTATION2 - 90

รูปที่ 4 การหามุมของสี่เหลี่ยม

6) นำตำแหน่ง pixel ของรูปร่างชิ้นงานไปเทียบ (Map) กับ DEPTH เพื่อคำนวณหาคำเหน่งที่เป็น Cartesian (x, y, z) แล้วส่งให้กับ Task Planning ดังรูปที่ 5  โดยหลักการคำนวณหาระยะทางจะใช้หลักการของสามเหลี่ยมคล้าย โดยในการทำงานของ Vision ใช้จุดกึ่งกลางของภาพ (320, 240) เป็นจุด 0, 0

        Position_Z = DEPTH[py, px]
        Position_X = (px - 320) * CALIBRATE_FACTOR_H * Position_Z
        Position_Y = (py - 240) * CALIBRATE_FACTOR_V * Position_Z

รูปที่ 5 รูปร่างและตำแหน่งของวัตถุที่สนใจ

7) การทำงานทั้งหมดที่กล่าวมาจะเกิดขึ้นต่อกันเป็นลำดับขึ้นตอนตลอดเวลาวนเป็น Loop ทำงานโดยรอคำสั่งจาก Task Planning เพื่อให้ส่งข้อมูลรูปร่างและตำแหน่งของวัตถุที่สนใจกลับไปดังรูปที่ 6

รูปที่ 6 ภาพขณะโปรแกรมส่วน Vision 1 ทำงาน