├── .gitignore
├── CMakeLists.txt
├── README.md
├── camera_info
├── left_camera.yaml
└── right_camera.yaml
├── launch
├── only_left.launch
├── tracker1.launch
└── tracker2.launch
├── package.xml
├── scripts
├── camera_threshold_helper.py
├── disparity_track.py
└── image_track.py
├── src
└── basic_shapes.cpp
└── test_camera_matrix.py
/.gitignore:
--------------------------------------------------------------------------------
1 | *.png
2 |
--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 2.8.3)
2 | project(stereo_color_tracker)
3 |
4 | ## Find catkin macros and libraries
5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
6 | ## is used, also find other catkin packages
7 | find_package(catkin REQUIRED COMPONENTS
8 | geometry_msgs
9 | roscpp
10 | rospy
11 | std_msgs
12 | )
13 |
14 | ## System dependencies are found with CMake's conventions
15 | # find_package(Boost REQUIRED COMPONENTS system)
16 |
17 |
18 | ## Uncomment this if the package has a setup.py. This macro ensures
19 | ## modules and global scripts declared therein get installed
20 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
21 | # catkin_python_setup()
22 |
23 | ################################################
24 | ## Declare ROS messages, services and actions ##
25 | ################################################
26 |
27 | ## To declare and build messages, services or actions from within this
28 | ## package, follow these steps:
29 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in
30 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
31 | ## * In the file package.xml:
32 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
33 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been
34 | ## pulled in transitively but can be declared for certainty nonetheless:
35 | ## * add a build_depend tag for "message_generation"
36 | ## * add a run_depend tag for "message_runtime"
37 | ## * In this file (CMakeLists.txt):
38 | ## * add "message_generation" and every package in MSG_DEP_SET to
39 | ## find_package(catkin REQUIRED COMPONENTS ...)
40 | ## * add "message_runtime" and every package in MSG_DEP_SET to
41 | ## catkin_package(CATKIN_DEPENDS ...)
42 | ## * uncomment the add_*_files sections below as needed
43 | ## and list every .msg/.srv/.action file to be processed
44 | ## * uncomment the generate_messages entry below
45 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
46 |
47 | ## Generate messages in the 'msg' folder
48 | # add_message_files(
49 | # FILES
50 | # Message1.msg
51 | # Message2.msg
52 | # )
53 |
54 | ## Generate services in the 'srv' folder
55 | # add_service_files(
56 | # FILES
57 | # Service1.srv
58 | # Service2.srv
59 | # )
60 |
61 | ## Generate actions in the 'action' folder
62 | # add_action_files(
63 | # FILES
64 | # Action1.action
65 | # Action2.action
66 | # )
67 |
68 | ## Generate added messages and services with any dependencies listed here
69 | # generate_messages(
70 | # DEPENDENCIES
71 | # geometry_msgs# std_msgs
72 | # )
73 |
74 | ###################################
75 | ## catkin specific configuration ##
76 | ###################################
77 | ## The catkin_package macro generates cmake config files for your package
78 | ## Declare things to be passed to dependent projects
79 | ## INCLUDE_DIRS: uncomment this if you package contains header files
80 | ## LIBRARIES: libraries you create in this project that dependent projects also need
81 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need
82 | ## DEPENDS: system dependencies of this project that dependent projects also need
83 | catkin_package(
84 | # INCLUDE_DIRS include
85 | # LIBRARIES stereo_color_tracker
86 | # CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs
87 | # DEPENDS system_lib
88 | )
89 |
90 | ###########
91 | ## Build ##
92 | ###########
93 |
94 | ## Specify additional locations of header files
95 | ## Your package locations should be listed before other locations
96 | # include_directories(include)
97 | include_directories(
98 | ${catkin_INCLUDE_DIRS}
99 | )
100 |
101 | ## Declare a cpp library
102 | # add_library(stereo_color_tracker
103 | # src/${PROJECT_NAME}/stereo_color_tracker.cpp
104 | # )
105 |
106 | ## Declare a cpp executable
107 | # add_executable(stereo_color_tracker_node src/stereo_color_tracker_node.cpp)
108 | add_executable(basic_shapes src/basic_shapes.cpp)
109 |
110 | ## Add cmake target dependencies of the executable/library
111 | ## as an example, message headers may need to be generated before nodes
112 | # add_dependencies(stereo_color_tracker_node stereo_color_tracker_generate_messages_cpp)
113 |
114 | ## Specify libraries to link a library or executable target against
115 | # target_link_libraries(stereo_color_tracker_node
116 | # ${catkin_LIBRARIES}
117 | # )
118 | target_link_libraries(basic_shapes ${catkin_LIBRARIES})
119 |
120 | #############
121 | ## Install ##
122 | #############
123 |
124 | # all install targets should use catkin DESTINATION variables
125 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
126 |
127 | ## Mark executable scripts (Python etc.) for installation
128 | ## in contrast to setup.py, you can choose the destination
129 | install(PROGRAMS
130 | scripts/image_track.py
131 | scripts/disparity_track.py
132 | scripts/camera_threshold_helper.py
133 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
134 | )
135 |
136 | ## Mark executables and/or libraries for installation
137 | # install(TARGETS stereo_color_tracker stereo_color_tracker_node
138 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
139 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
140 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
141 | # )
142 |
143 | ## Mark cpp header files for installation
144 | # install(DIRECTORY include/${PROJECT_NAME}/
145 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
146 | # FILES_MATCHING PATTERN "*.h"
147 | # PATTERN ".svn" EXCLUDE
148 | # )
149 |
150 | ## Mark other files for installation (e.g. launch and bag files, etc.)
151 | # install(FILES
152 | # # myfile1
153 | # # myfile2
154 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
155 | # )
156 |
157 | #############
158 | ## Testing ##
159 | #############
160 |
161 | ## Add gtest based cpp test target and link libraries
162 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_stereo_color_tracker.cpp)
163 | # if(TARGET ${PROJECT_NAME}-test)
164 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
165 | # endif()
166 |
167 | ## Add folders to be run by python nosetests
168 | # catkin_add_nosetests(test)
169 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | Stereo Tracking using ROS and OpenCV
2 | ---
3 | Stereo tracking a green android figurine using ROS and OpenCV. It's very simple right now, color centroid tracking on both cameras and a disparity map (both cameras parallel aligned).
4 |
5 | Video: http://youtu.be/0cQXU5X0Yyk
6 |
7 | 
8 | 
--------------------------------------------------------------------------------
/camera_info/left_camera.yaml:
--------------------------------------------------------------------------------
1 | image_width: 640
2 | image_height: 480
3 | camera_name: left_camera
4 | camera_matrix:
5 | rows: 3
6 | cols: 3
7 | data: [861.7849547322785, 0, 320.0, 0, 848.6931340450212, 240.0, 0, 0, 1]
8 | distortion_model: plumb_bob
9 | distortion_coefficients:
10 | rows: 1
11 | cols: 5
12 | data: [0.1321407125406009, -0.1414210683732252, -0.009010450393461093, 0.06420631003377338, 0]
13 | rectification_matrix:
14 | rows: 3
15 | cols: 3
16 | data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
17 | projection_matrix:
18 | rows: 3
19 | cols: 4
20 | data: [817.0499877929688, 0, 320.0, 0, 0, 876.6177978515625, 240.0, 0, 0, 0, 1, 0]
--------------------------------------------------------------------------------
/camera_info/right_camera.yaml:
--------------------------------------------------------------------------------
1 | image_width: 640
2 | image_height: 480
3 | camera_name: right_camera
4 | camera_matrix:
5 | rows: 3
6 | cols: 3
7 | data: [861.7849547322785, 0, 320.0, 0, 848.6931340450212, 240.0, 0, 0, 1]
8 | distortion_model: plumb_bob
9 | distortion_coefficients:
10 | rows: 1
11 | cols: 5
12 | data: [0.1321407125406009, -0.1414210683732252, -0.009010450393461093, 0.06420631003377338, 0]
13 | rectification_matrix:
14 | rows: 3
15 | cols: 3
16 | data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
17 | projection_matrix:
18 | rows: 3
19 | cols: 4
20 | data: [817.0499877929688, 0, 320.0, 0, 0, 876.6177978515625, 240.0, 0, 0, 0, 1, 0]
--------------------------------------------------------------------------------
/launch/only_left.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
9 |
10 |
11 |
12 |
--------------------------------------------------------------------------------
/launch/tracker1.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
7 |
9 |
11 |
12 |
14 |
15 |
16 |
17 |
18 |
19 |
20 |
21 |
23 |
24 |
25 |
26 |
27 |
28 |
29 |
30 |
32 |
33 |
34 |
35 |
36 |
37 |
38 |
39 |
--------------------------------------------------------------------------------
/launch/tracker2.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
7 |
8 |
9 |
10 |
12 |
14 |
15 |
17 |
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 |
--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 | stereo_color_tracker
4 | 0.0.0
5 | The stereo_color_tracker package
6 |
7 |
8 |
9 |
10 | Sam
11 |
12 |
13 |
14 |
15 |
16 | BSD
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 | catkin
43 | geometry_msgs
44 | roscpp
45 | rospy
46 | std_msgs
47 | geometry_msgs
48 | roscpp
49 | rospy
50 | std_msgs
51 |
52 |
53 |
54 |
55 |
56 |
57 |
58 |
59 |
60 |
61 |
62 |
--------------------------------------------------------------------------------
/scripts/camera_threshold_helper.py:
--------------------------------------------------------------------------------
1 | #!/usr/bin/env python
2 | import roslib
3 | roslib.load_manifest('stereo_color_tracker')
4 | import sys
5 | import rospy
6 | import cv2
7 | import numpy as np
8 | from std_msgs.msg import String, Header
9 | from sensor_msgs.msg import Image
10 | from geometry_msgs.msg import PointStamped, Point
11 | from cv_bridge import CvBridge, CvBridgeError
12 | # import IPython
13 |
14 | class camera_threshold_helper:
15 |
16 | def __init__(self):
17 | # self.image_pub = rospy.Publisher("left_tracker_image",Image, queue_size=5)
18 | self.left_point_pub = rospy.Publisher("left_point", PointStamped, queue_size=5)
19 |
20 | # cv2.namedWindow("Image window", 1)
21 | self.bridge = CvBridge()
22 | self.image_sub = rospy.Subscriber("/left_cam/image_raw",Image,self.callback)
23 |
24 | # Trackbar stuff
25 | self.lower_threshold = np.array([66, 97, 131])
26 | self.upper_threshold = np.array([96, 222, 255])
27 |
28 | # # Green Android
29 | # self.lower_threshold = np.array([39, 68, 163])
30 | # self.upper_threshold = np.array([79, 222, 255])
31 |
32 | self.f = 2684.0
33 | # 1280 x 960 image
34 | self.center_x = (1280.0/2.0)/0.6096
35 | self.center_y = (960.0/2.0)/0.6096
36 | self.invCameraMatrix = np.matrix([[self.f, 0, self.center_x],
37 | [0, self.f, self.center_y],
38 | [0, 0, 1.0]]).I
39 |
40 | cv2.namedWindow("Control", cv2.CV_WINDOW_AUTOSIZE); # Threshold Controller window
41 | cv2.namedWindow("Thresholded Image", cv2.CV_WINDOW_AUTOSIZE); # Threshold image window
42 |
43 | cv2.createTrackbar("LowH", "Control", self.lower_threshold[0], 255, self.updateLowH); # Hue (0 - 255)
44 | cv2.createTrackbar("HighH", "Control", self.upper_threshold[0], 255, self.updateHighH);
45 | cv2.createTrackbar("LowS", "Control", self.lower_threshold[1], 255, self.updateLowS); # Saturation (0 - 255)
46 | cv2.createTrackbar("HighS", "Control", self.upper_threshold[1], 255, self.updateHighS);
47 | cv2.createTrackbar("LowV", "Control", self.lower_threshold[2], 255, self.updateLowV); # Value (0 - 255)
48 | cv2.createTrackbar("HighV", "Control", self.upper_threshold[2], 255, self.updateHighV);
49 | cv2.createTrackbar("offset", "Control", 2684, 10000, self.updateF);
50 |
51 | def updateLowH(self, value):
52 | self.lower_threshold[0] = value
53 | def updateHighH(self, value):
54 | self.upper_threshold[0] = value
55 | def updateLowS(self, value):
56 | self.lower_threshold[1] = value
57 | def updateHighS(self, value):
58 | self.upper_threshold[1] = value
59 | def updateLowV(self, value):
60 | self.lower_threshold[2] = value
61 | def updateHighV(self, value):
62 | self.upper_threshold[2] = value
63 | def updateF(self, value):
64 | self.f = float(value)
65 | self.invCameraMatrix = np.matrix([[self.f, 0, self.center_x],
66 | [0, self.f, self.center_y],
67 | [0, 0, 1.0]]).I
68 | def callback(self,data):
69 | try:
70 | cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
71 | except CvBridgeError, e:
72 | print e
73 |
74 | # IPython.embed()
75 |
76 | # Get HSV image
77 | hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
78 |
79 | # Threshold image to range
80 | mask = cv2.inRange(hsv, self.lower_threshold, self.upper_threshold)
81 |
82 | # Erode/Dilate mask to remove noise
83 | # mask = cv2.erode(mask, cv2.getStructuringElement(cv2.MORPH_ERODE, (2,2) ))
84 | # mask = cv2.dilate(mask, cv2.getStructuringElement(cv2.MORPH_DILATE, (2,2) ))
85 |
86 | # Mask image
87 | cv_image = cv2.bitwise_and(cv_image, cv_image, mask=mask)
88 |
89 | # Use Mask to get blob information
90 | moments = cv2.moments(mask)
91 | area = moments['m00']
92 | if (area > 0):
93 | cx = int(moments['m10']/moments['m00']) # cx = M10/M00
94 | cy = int(moments['m01']/moments['m00']) # cy = M01/M00
95 | cv2.circle(cv_image, (cx,cy), 10, (0,0,255))
96 | self.postLeftPoint(cx,cy) # Publish it
97 | cv2.putText(cv_image,"Area: %10d, X: %3d, Y: %3d" % (area, cx, cy), (10,20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255,255,0)) # bgr
98 |
99 | # (rows,cols,channels) = cv_image.shape
100 |
101 | cv2.imshow("Thresholded Image", cv_image)
102 | k = cv2.waitKey(3) & 0xFF
103 | if k == 113 or k == 27: # Escape key = 27, 'q' = 113
104 | rospy.signal_shutdown("User Exit")
105 |
106 | # try:
107 | # self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
108 | # except CvBridgeError, e:
109 | # print e
110 |
111 | def postLeftPoint(self, x, y):
112 | worldPos = self.invCameraMatrix * np.matrix([[x],[y],[0.6096]])
113 | point = PointStamped(header=Header(stamp=rospy.Time.now(),
114 | frame_id='/left_camera'),
115 | point=Point(worldPos[0],worldPos[1],worldPos[2]))
116 | self.left_point_pub.publish(point)
117 |
118 | def main(args):
119 | ic = camera_threshold_helper()
120 | rospy.init_node('camera_threshold_helper', anonymous=True)
121 | try:
122 | rospy.spin()
123 | except KeyboardInterrupt:
124 | print "Shutting down"
125 | cv2.destroyAllWindows()
126 | print "Finished."
127 |
128 | if __name__ == '__main__':
129 | main(sys.argv)
--------------------------------------------------------------------------------
/scripts/disparity_track.py:
--------------------------------------------------------------------------------
1 | #!/usr/bin/env python
2 | import roslib
3 | roslib.load_manifest('stereo_color_tracker')
4 | import sys
5 | import rospy
6 | import cv2
7 | import numpy as np
8 | from std_msgs.msg import String, Header
9 | from sensor_msgs.msg import Image
10 | from geometry_msgs.msg import PointStamped, Point
11 | from cv_bridge import CvBridge, CvBridgeError
12 | # import IPython
13 |
14 | class disparity_track:
15 |
16 | def __init__(self):
17 | # self.tracked_point_pub = rospy.Publisher("tracked_point", PointStamped, queue_size=5)
18 | self.left_point_pub = rospy.Publisher("left_point", PointStamped, queue_size=5)
19 |
20 | # cv2.namedWindow("Image window", 1)
21 | self.bridge = CvBridge()
22 | self.image_sub = rospy.Subscriber("/left_cam/image_raw",Image,self.left_callback)
23 | self.image_sub = rospy.Subscriber("/right_cam/image_raw",Image,self.right_callback)
24 |
25 | # Trackbar stuff
26 | # Green Marker
27 | # self.lower_threshold = np.array([66, 97, 180])
28 | # self.upper_threshold = np.array([96, 222, 255])
29 |
30 | # Green cloth on a plastic stick
31 | # self.lower_threshold = np.array([37, 64, 73])
32 | # self.upper_threshold = np.array([63, 149, 233])
33 |
34 | # Green Marker 3d print table nov 26
35 | # self.lower_threshold = np.array([60, 96, 131])
36 | # self.upper_threshold = np.array([84, 221, 255])
37 |
38 | # Green Android
39 | self.lower_threshold = np.array([39, 68, 163])
40 | self.upper_threshold = np.array([79, 222, 255])
41 |
42 | self.disparity_ratio = 211.4
43 | # 1280 x 960 image
44 | # self.center_x = (1280.0/2.0) # half x pixels
45 | # self.center_y = (960.0/2.0) # half y pixels
46 | # 640 x 480
47 | self.center_x = (640.0/2.0) # half x pixels
48 | self.center_y = (480.0/2.0) # half y pixels
49 | self.Kinv = np.matrix([[861.7849547322785, 0, 320.0],
50 | [0, 848.6931340450212, 240.0],
51 | [0, 0, 1]]).I # K inverse
52 |
53 | cv2.namedWindow("Control", cv2.CV_WINDOW_AUTOSIZE); # Threshold Controller window
54 | # cv2.namedWindow("Thresholded Image Left", cv2.CV_WINDOW_AUTOSIZE); # Threshold image window
55 | # cv2.namedWindow("Thresholded Image Right", cv2.CV_WINDOW_AUTOSIZE); # Threshold image window
56 |
57 | # cv2.createTrackbar("LowH", "Control", self.lower_threshold[0], 255, self.updateLowH); # Hue (0 - 255)
58 | # cv2.createTrackbar("HighH", "Control", self.upper_threshold[0], 255, self.updateHighH);
59 | # cv2.createTrackbar("LowS", "Control", self.lower_threshold[1], 255, self.updateLowS); # Saturation (0 - 255)
60 | # cv2.createTrackbar("HighS", "Control", self.upper_threshold[1], 255, self.updateHighS);
61 | # cv2.createTrackbar("LowV", "Control", self.lower_threshold[2], 255, self.updateLowV); # Value (0 - 255)
62 | # cv2.createTrackbar("HighV", "Control", self.upper_threshold[2], 255, self.updateHighV);
63 | cv2.createTrackbar("Disparity ratio", "Control", int(self.disparity_ratio*10), 5000, self.updateDisparity);
64 |
65 | self.last_left_image_pos = np.array([0.0, 0.0])
66 |
67 | def updateLowH(self, value):
68 | self.lower_threshold[0] = value
69 | def updateHighH(self, value):
70 | self.upper_threshold[0] = value
71 | def updateLowS(self, value):
72 | self.lower_threshold[1] = value
73 | def updateHighS(self, value):
74 | self.upper_threshold[1] = value
75 | def updateLowV(self, value):
76 | self.lower_threshold[2] = value
77 | def updateHighV(self, value):
78 | self.upper_threshold[2] = value
79 | def updateDisparity(self, value):
80 | self.disparity_ratio = float(value) * 0.1
81 | def left_callback(self,data):
82 | try:
83 | cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
84 | except CvBridgeError, e:
85 | print e
86 |
87 | # Get HSV image
88 | hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
89 |
90 | # Threshold image to range
91 | mask = cv2.inRange(hsv, self.lower_threshold, self.upper_threshold)
92 |
93 | # Erode/Dilate mask to remove noise
94 | mask = cv2.erode(mask, cv2.getStructuringElement(cv2.MORPH_ERODE, (3,3) ))
95 | mask = cv2.dilate(mask, cv2.getStructuringElement(cv2.MORPH_DILATE, (3,3) ))
96 |
97 | # Use Mask to get blob information
98 | moments = cv2.moments(mask)
99 | area = moments['m00']
100 | if (area > 0):
101 | cx = moments['m10']/moments['m00'] # cx = M10/M00
102 | cy = moments['m01']/moments['m00'] # cy = M01/M00
103 | self.last_left_image_pos[0] = cx
104 | self.last_left_image_pos[1] = cy
105 | # self.postLeftPoint(cx,cy) # Publish it
106 |
107 | k = cv2.waitKey(3) & 0xFF
108 | if k == 113 or k == 27: # Escape key = 27, 'q' = 113
109 | rospy.signal_shutdown("User Exit")
110 |
111 | def right_callback(self,data):
112 | try:
113 | cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
114 | except CvBridgeError, e:
115 | print e
116 |
117 | # Get HSV image
118 | hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
119 |
120 | # Threshold image to range
121 | mask = cv2.inRange(hsv, self.lower_threshold, self.upper_threshold)
122 |
123 | # Erode/Dilate mask to remove noise
124 | # mask = cv2.erode(mask, cv2.getStructuringElement(cv2.MORPH_ERODE, (3,3) ))
125 | # mask = cv2.dilate(mask, cv2.getStructuringElement(cv2.MORPH_DILATE, (3,3) ))
126 |
127 | # Use Mask to get blob information
128 | moments = cv2.moments(mask)
129 | area = moments['m00']
130 | if (area > 0):
131 | cx = moments['m10']/moments['m00'] # cx = M10/M00
132 | cy = moments['m01']/moments['m00'] # cy = M01/M00
133 | dx = float(self.last_left_image_pos[0] - cx)
134 | # dx of 31px for 10cm at 2.7m depth
135 | # disparity is inversely proportional to distance from camera (bigger disparity = smaller distance)
136 | # ratio/disparity = distance
137 | # 2.7m = ratio / 31px
138 | depth = self.disparity_ratio/dx
139 | # print "dx: %3.2f, dy: %3.2f, guessed depth: %3.2f" % ((self.last_left_image_pos[0] - cx), (self.last_left_image_pos[1] - cy), depth)
140 | self.postLeftPoint(self.last_left_image_pos[0],self.last_left_image_pos[1],depth)
141 |
142 | def postLeftPoint(self, x, y, depth):
143 | worldPos = self.Kinv * np.matrix([x,y,1]).T * depth
144 | point = PointStamped(header=Header(stamp=rospy.Time.now(),
145 | frame_id='/left_camera'),
146 | point=Point(worldPos[0],worldPos[1], worldPos[2]))
147 | self.left_point_pub.publish(point)
148 |
149 |
150 | def main(args):
151 | rospy.init_node('disparity_track', anonymous=True)
152 | ic = disparity_track()
153 | try:
154 | rospy.spin()
155 | except KeyboardInterrupt:
156 | print "Shutting down"
157 | cv2.destroyAllWindows()
158 | print "Finished."
159 |
160 | if __name__ == '__main__':
161 | main(sys.argv)
--------------------------------------------------------------------------------
/scripts/image_track.py:
--------------------------------------------------------------------------------
1 | #!/usr/bin/env python
2 | import roslib
3 | roslib.load_manifest('stereo_color_tracker')
4 | import sys
5 | import rospy
6 | import cv2
7 | import numpy as np
8 | from std_msgs.msg import String, Header
9 | from sensor_msgs.msg import Image
10 | from geometry_msgs.msg import PointStamped, Point
11 | from cv_bridge import CvBridge, CvBridgeError
12 | # import IPython
13 |
14 | class image_track:
15 |
16 | def __init__(self):
17 | # self.image_pub = rospy.Publisher("left_tracker_image",Image, queue_size=5)
18 | self.left_point_pub = rospy.Publisher("left_point", PointStamped, queue_size=5)
19 | self.right_point_pub = rospy.Publisher("right_point", PointStamped, queue_size=5)
20 |
21 | # cv2.namedWindow("Image window", 1)
22 | self.bridge = CvBridge()
23 | self.image_sub = rospy.Subscriber("/left_cam/image_raw",Image,self.left_callback)
24 | self.image_sub = rospy.Subscriber("/right_cam/image_raw",Image,self.right_callback)
25 |
26 | # Trackbar stuff
27 | # Green Marker
28 | # self.lower_threshold = np.array([66, 97, 180])
29 | # self.upper_threshold = np.array([96, 222, 255])
30 |
31 | # Green cloth on a plastic stick
32 | # self.lower_threshold = np.array([37, 64, 73])
33 | # self.upper_threshold = np.array([63, 149, 233])
34 |
35 | # Green Marker 3d print table nov 26
36 | self.lower_threshold = np.array([60, 96, 131])
37 | self.upper_threshold = np.array([84, 221, 255])
38 |
39 | self.f = 640.0
40 | # 1280 x 960 image
41 | # self.center_x = (1280.0/2.0) # half x pixels
42 | # self.center_y = (960.0/2.0) # half y pixels
43 | # 640 x 480
44 | self.center_x = (640.0/2.0) # half x pixels
45 | self.center_y = (480.0/2.0) # half y pixels
46 | self.invCameraMatrix = np.matrix([[self.f, 0, self.center_x],
47 | [0, self.f, self.center_y],
48 | [0, 0, 1.0]]).I
49 |
50 | cv2.namedWindow("Control", cv2.CV_WINDOW_AUTOSIZE); # Threshold Controller window
51 | # cv2.namedWindow("Thresholded Image", cv2.CV_WINDOW_AUTOSIZE); # Threshold image window
52 |
53 | cv2.createTrackbar("LowH", "Control", self.lower_threshold[0], 255, self.updateLowH); # Hue (0 - 255)
54 | cv2.createTrackbar("HighH", "Control", self.upper_threshold[0], 255, self.updateHighH);
55 | cv2.createTrackbar("LowS", "Control", self.lower_threshold[1], 255, self.updateLowS); # Saturation (0 - 255)
56 | cv2.createTrackbar("HighS", "Control", self.upper_threshold[1], 255, self.updateHighS);
57 | cv2.createTrackbar("LowV", "Control", self.lower_threshold[2], 255, self.updateLowV); # Value (0 - 255)
58 | cv2.createTrackbar("HighV", "Control", self.upper_threshold[2], 255, self.updateHighV);
59 | cv2.createTrackbar("offset F", "Control", int(self.f), 3000, self.updateF);
60 |
61 | def updateLowH(self, value):
62 | self.lower_threshold[0] = value
63 | def updateHighH(self, value):
64 | self.upper_threshold[0] = value
65 | def updateLowS(self, value):
66 | self.lower_threshold[1] = value
67 | def updateHighS(self, value):
68 | self.upper_threshold[1] = value
69 | def updateLowV(self, value):
70 | self.lower_threshold[2] = value
71 | def updateHighV(self, value):
72 | self.upper_threshold[2] = value
73 | def updateF(self, value):
74 | self.f = float(value)
75 | self.invCameraMatrix = np.matrix([[self.f, 0, self.center_x],
76 | [0, self.f, self.center_y],
77 | [0, 0, 1.0]]).I
78 | def left_callback(self,data):
79 | try:
80 | cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
81 | except CvBridgeError, e:
82 | print e
83 |
84 | # IPython.embed()
85 |
86 | # Get HSV image
87 | hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
88 |
89 | # Threshold image to range
90 | mask = cv2.inRange(hsv, self.lower_threshold, self.upper_threshold)
91 |
92 | # Erode/Dilate mask to remove noise
93 | mask = cv2.erode(mask, cv2.getStructuringElement(cv2.MORPH_ERODE, (3,3) ))
94 | mask = cv2.dilate(mask, cv2.getStructuringElement(cv2.MORPH_DILATE, (3,3) ))
95 |
96 | # Mask image
97 | # cv_image = cv2.bitwise_and(cv_image, cv_image, mask=mask)
98 |
99 | # Use Mask to get blob information
100 | moments = cv2.moments(mask)
101 | area = moments['m00']
102 | if (area > 0):
103 | cx = int(moments['m10']/moments['m00']) # cx = M10/M00
104 | cy = int(moments['m01']/moments['m00']) # cy = M01/M00
105 | # cv2.circle(cv_image, (cx,cy), 10, (0,0,255))
106 | self.postLeftPoint(cx,cy) # Publish it
107 | # cv2.putText(cv_image,"Area: %10d, X: %3d, Y: %3d" % (area, cx, cy), (10,20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255,255,0)) # bgr
108 |
109 | # (rows,cols,channels) = cv_image.shape
110 |
111 | # cv2.imshow("Thresholded Image", cv_image)
112 | k = cv2.waitKey(3) & 0xFF
113 | if k == 113 or k == 27: # Escape key = 27, 'q' = 113
114 | rospy.signal_shutdown("User Exit")
115 |
116 | # try:
117 | # self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
118 | # except CvBridgeError, e:
119 | # print e
120 |
121 | def right_callback(self,data):
122 | try:
123 | cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
124 | except CvBridgeError, e:
125 | print e
126 |
127 | # Get HSV image
128 | hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
129 |
130 | # Threshold image to range
131 | mask = cv2.inRange(hsv, self.lower_threshold, self.upper_threshold)
132 |
133 | # Erode/Dilate mask to remove noise
134 | mask = cv2.erode(mask, cv2.getStructuringElement(cv2.MORPH_ERODE, (3,3) ))
135 | mask = cv2.dilate(mask, cv2.getStructuringElement(cv2.MORPH_DILATE, (3,3) ))
136 |
137 | # Use Mask to get blob information
138 | moments = cv2.moments(mask)
139 | area = moments['m00']
140 | if (area > 0):
141 | cx = int(moments['m10']/moments['m00']) # cx = M10/M00
142 | cy = int(moments['m01']/moments['m00']) # cy = M01/M00
143 | self.postRightPoint(cx,cy) # Publish it
144 |
145 | def postLeftPoint(self, x, y):
146 | worldPos = self.invCameraMatrix * np.matrix([x,y,1]).T
147 | point = PointStamped(header=Header(stamp=rospy.Time.now(),
148 | frame_id='/left_camera'),
149 | point=Point(x,y,worldPos[2]))
150 | self.left_point_pub.publish(point)
151 |
152 | def postRightPoint(self, x, y):
153 | worldPos = self.invCameraMatrix * np.matrix([x,y,1]).T
154 | point = PointStamped(header=Header(stamp=rospy.Time.now(),
155 | frame_id='/right_camera'),
156 | point=Point(x,y,worldPos[2]))
157 | self.right_point_pub.publish(point)
158 |
159 |
160 | def main(args):
161 | ic = image_track()
162 | rospy.init_node('image_track', anonymous=True)
163 | try:
164 | rospy.spin()
165 | except KeyboardInterrupt:
166 | print "Shutting down"
167 | cv2.destroyAllWindows()
168 | print "Finished."
169 |
170 | if __name__ == '__main__':
171 | main(sys.argv)
--------------------------------------------------------------------------------
/src/basic_shapes.cpp:
--------------------------------------------------------------------------------
1 | #include
2 | #include
3 |
4 | int main( int argc, char** argv )
5 | {
6 | ros::init(argc, argv, "basic_shapes");
7 | ros::NodeHandle n;
8 | ros::Rate r(1);
9 | ros::Publisher marker_pub = n.advertise("visualization_marker", 1);
10 |
11 | // Set our initial shape type to be a cube
12 | uint32_t shape = visualization_msgs::Marker::CUBE;
13 |
14 | while (ros::ok())
15 | {
16 | visualization_msgs::Marker marker;
17 | // Set the frame ID and timestamp. See the TF tutorials for information on these.
18 | marker.header.frame_id = "/marker_frame";
19 | marker.header.stamp = ros::Time::now();
20 |
21 | // Set the namespace and id for this marker. This serves to create a unique ID
22 | // Any marker sent with the same namespace and id will overwrite the old one
23 | marker.ns = "basic_shapes";
24 | marker.id = 0;
25 |
26 | // Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
27 | marker.type = shape;
28 |
29 | // Set the marker action. Options are ADD and DELETE
30 | marker.action = visualization_msgs::Marker::ADD;
31 |
32 | // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header
33 | marker.pose.position.x = 0;
34 | marker.pose.position.y = 0;
35 | marker.pose.position.z = 0;
36 | marker.pose.orientation.x = 0.0;
37 | marker.pose.orientation.y = 0.0;
38 | marker.pose.orientation.z = 0.0;
39 | marker.pose.orientation.w = 1.0;
40 |
41 | // Set the scale of the marker -- 1x1x1 here means 1m on a side
42 | marker.scale.x = 1.0;
43 | marker.scale.y = 1.0;
44 | marker.scale.z = 1.0;
45 |
46 | // Set the color -- be sure to set alpha to something non-zero!
47 | marker.color.r = 0.0f;
48 | marker.color.g = 1.0f;
49 | marker.color.b = 0.0f;
50 | marker.color.a = 1.0;
51 |
52 | marker.lifetime = ros::Duration();
53 |
54 | // Publish the marker
55 | marker_pub.publish(marker);
56 |
57 | // Cycle between different shapes
58 | switch (shape)
59 | {
60 | case visualization_msgs::Marker::CUBE:
61 | shape = visualization_msgs::Marker::SPHERE;
62 | break;
63 | case visualization_msgs::Marker::SPHERE:
64 | shape = visualization_msgs::Marker::ARROW;
65 | break;
66 | case visualization_msgs::Marker::ARROW:
67 | shape = visualization_msgs::Marker::CYLINDER;
68 | break;
69 | case visualization_msgs::Marker::CYLINDER:
70 | shape = visualization_msgs::Marker::CUBE;
71 | break;
72 | }
73 |
74 | r.sleep();
75 | }
76 | }
77 |
78 |
--------------------------------------------------------------------------------
/test_camera_matrix.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 | np.set_printoptions(suppress=True, precision=2)
3 |
4 | # Camera Matrix
5 | K = np.matrix([[861.7849547322785, 0, 320.0, 0],
6 | [0, 848.6931340450212, 240.0, 0],
7 | [0, 0, 1, 0]])
8 | # Distortion
9 | D = np.array([0.1321407125406009, -0.1414210683732252, -0.009010450393461093, 0.06420631003377338, 0])
10 |
11 | # uv pixel coords
12 | p1 = np.matrix([[320.0, 240.0, 1]]).T # origin
13 | p2 = np.matrix([[289.0, 240.0, 1]]).T # 10cm left, 2.7 m away
14 | # world X Y Z (Z = depth)
15 | P1 = np.matrix([[0.0, 0.0, 2.7, 1]]).T # origin world
16 | P2 = np.matrix([[-0.1, 0.0, 2.7, 1]]).T # origin world
17 |
18 | print K
19 | print "World P1", P1.A1
20 | print "World P2", P2.A1
21 |
22 |
23 | a = K*P1
24 | a /= a.A1[2]
25 | print "Guessed p1", a.A1, "vs actual p1", p1.A1
26 |
27 | b = K*P2
28 | b /= b.A1[2]
29 | print "Guessed p2", b.A1, "vs actual p2", p2.A1
30 |
31 | # p = K*P
32 | # P = K^-1 * p
33 | Kinv = K.I
34 | depth = 2.7 # found using disparity
35 | print Kinv * p1 * depth
36 | print Kinv * p2 * depth
--------------------------------------------------------------------------------