├── .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 | ![Rviz](http://i.imgur.com/0C7KkZz.png) 8 | ![Setup](http://i.imgur.com/Wlda6IL.jpg) -------------------------------------------------------------------------------- /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 --------------------------------------------------------------------------------