SVCAM_examples/detector_blob.py at main · AppliedRobotics/SVCAM_examples · GitHub

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
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
#!/usr/bin/env python3
from __future__ import print_function

import roslib
import sys
import rospy
import cv2
import numpy as np
import message_filters
from std_msgs.msg import String
from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge, CvBridgeError
from visualization_msgs.msg import MarkerArray,Marker
from geometry_msgs.msg import Pose
import math

class Get_distance_from_camera:

    def __init__(self):

        marker = Marker()

        self.bridge = CvBridge()

        self.camera_info_sub = message_filters.Subscriber('/trackingcam3d_client_ros/trackingcam3d0/left/camera_info', CameraInfo)

        self.image_sub = message_filters.Subscriber("/trackingcam3d_client_ros/trackingcam3d0/left/image_raw",Image)
        self.depth_sub = message_filters.Subscriber("/trackingcam3d_client_ros/trackingcam3d0/depth/image_rect",Image)

        self.ts = message_filters.ApproximateTimeSynchronizer([self.image_sub, self.camera_info_sub], queue_size=10, slop=0.5)
        self.ts.registerCallback(self.callback)

        self.pub = rospy.Publisher('/trackingcam3d_client_ros/recognition_object', Image, queue_size=1)


    def callback(self, rgb_data, camera_info):

        try:
            camera_info_K = np.array(camera_info.K)

      # Intrinsic camera matrix for the raw (distorted) images.
      #     [fx  0 cx]
      # K = [ 0 fy cy]
      #     [ 0  0  1]

    #   m_fx = camera_info.K[0]
    #   m_fy = camera_info.K[4]
    #   m_cx = camera_info.K[2]
    #   m_cy = camera_info.K[5]
    #   inv_fx = 1. / m_fx
    #   inv_fy = 1. / m_fy


            cv_rgb = self.bridge.imgmsg_to_cv2(rgb_data, "bgr8")
            cv_rgb = cv2.resize(cv_rgb, (640, 480))

            y_size = np.size(cv_rgb, 0)
            x_size = np.size(cv_rgb, 1)

            x_offset = x_size/2
            y_offset = y_size/2

            cv_rgb = cv2.GaussianBlur(cv_rgb, (5,5), 0)
            hsv_cv_rgb = cv2.cvtColor(cv_rgb, cv2.COLOR_BGR2HSV)
            bin_cv_rgb = cv2.inRange(hsv_cv_rgb, (0, 50, 50), (10, 255, 255))
            bin_cv_rgb = cv2.erode(bin_cv_rgb, None, iterations = 1)
            bin_cv_rgb = cv2.erode(bin_cv_rgb, None, iterations = 2)

            contours, h = cv2.findContours(bin_cv_rgb.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)  # Find contours
            contours = sorted(contours, key=cv2.contourArea, reverse=True)  # Sort by area, descending

            if contours:
                (x, y), radius = cv2.minEnclosingCircle(contours[0])
                if (x > x_size):
                    x = x_size
                if (x < 0):
                    x = 0
                if (y > y_size):
                    y = y_size
                if (y < 0):
                    y = 0

                x_centre = x - x_offset
                y_centre = y - y_offset
                area_circle = math.pi * (radius**2)
               # print(area_circle)
                if area_circle>16500:
                    k = 165000
                elif(area_circle<165000 and area_circle>6500):
                    k  = 42462
                elif (area_circle>4900 and area_circle<6500):
                    k = 24450
                elif (area_circle>3100 and area_circle<4900):
                    k = 12462
                elif (area_circle<3100):
                    k = 5800

                dist = area_circle/k

                area_str = "dist:" + str(format(dist, '.2f')) + "m"
                print(area_str)

        except CvBridgeError as e:
            print(e)


        rgb = np.concatenate((cv_rgb), axis=1)

def main(args):
    rospy.init_node('unibas_face_distance_calculator', anonymous=True)
    fd = Get_distance_from_camera()
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("Shutting down")

if __name__ == '__main__':
    main(sys.argv)