depth_center_extractor.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 import roslib; roslib.load_manifest('kinect_depth_calibration')
00004 import rospy
00005 from kinect_depth_calibration.srv import GetCheckerboardCenter, GetCheckerboardCenterResponse
00006 from stereo_msgs.msg import DisparityImage
00007 import cv
00008 from cv_bridge import CvBridge, CvBridgeError
00009 import threading
00010 
00011 
00012 class DepthCenterExtractor:
00013     def __init__(self):
00014         self.bridge = CvBridge()
00015         self.lock = threading.Lock()
00016         self.depth = None
00017         
00018         self.sub = rospy.Subscriber('disparity', DisparityImage, self.depth_cb)
00019         self.srv = rospy.Service('get_checkerboard_center', GetCheckerboardCenter, self.get_center_cb)
00020 
00021 
00022     def get_depth(self, disp, disp_msg):
00023         return disp_msg.f * disp_msg.T / disp
00024 
00025 
00026     def depth_cb(self, msg):
00027         with self.lock:
00028             self.depth = msg
00029 
00030 
00031     def get_center_cb(self, req):
00032         # get new disp message
00033         now = rospy.Time.now()
00034         while not rospy.is_shutdown():
00035             rospy.sleep(0.2)
00036             with self.lock:
00037                 if self.depth and self.depth.header.stamp > now:
00038                     msg = self.depth
00039                     img = self.bridge.imgmsg_to_cv(self.depth.image, "passthrough")
00040                     break
00041 
00042 
00043         # get average disparity
00044         depth_sum = 0.0
00045         depth_nr = 0.0
00046         if req.min_x >= 0 and req.min_x < req.max_x and req.max_x < msg.image.width and req.min_y >= 0 and req.min_y < req.max_y and req.max_y < msg.image.height:
00047             for i in range(int(req.min_x), int(req.max_x)+1):
00048                 for j in range(int(req.min_y), int(req.max_y)+1):
00049                     if img[j, i] > 0:
00050                         depth = self.get_depth(img[j, i], msg)
00051                         if depth > req.depth_prior*0.8 and depth < req.depth_prior*1.2:
00052                             depth_sum += depth
00053                             depth_nr += 1
00054 
00055         # check if we got at least 75% of points
00056         res = GetCheckerboardCenterResponse()
00057         if (req.max_x - req.min_x)*(req.max_y - req.min_y)*0.75 < depth_nr:
00058             res.depth = depth_sum / depth_nr
00059         else:
00060             res.depth = 0.0
00061             print "Could not find enough depth points: expected %f points but only found %f"%((req.max_x - req.min_x)*(req.max_y - req.min_y)*0.75, depth_nr)
00062             
00063         return res
00064     
00065 
00066         
00067 
00068 
00069 def main():
00070     rospy.init_node('depth_center_extractor')
00071     rospy.sleep(0.1) # init sim time
00072     d = DepthCenterExtractor()
00073     rospy.spin()
00074 
00075 
00076 if __name__ == '__main__':
00077     main()
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


kinect_depth_calibration
Author(s): Eitan Marder-Eppstein
autogenerated on Thu Aug 15 2013 12:03:24