depth_calibrator.py
Go to the documentation of this file.
00001 #! /usr/bin/python
00002 #***********************************************************
00003 #* Software License Agreement (BSD License)
00004 #*
00005 #*  Copyright (c) 2009, Willow Garage, Inc.
00006 #*  All rights reserved.
00007 #*
00008 #*  Redistribution and use in source and binary forms, with or without
00009 #*  modification, are permitted provided that the following conditions
00010 #*  are met:
00011 #*
00012 #*   * Redistributions of source code must retain the above copyright
00013 #*     notice, this list of conditions and the following disclaimer.
00014 #*   * Redistributions in binary form must reproduce the above
00015 #*     copyright notice, this list of conditions and the following
00016 #*     disclaimer in the documentation and/or other materials provided
00017 #*     with the distribution.
00018 #*   * Neither the name of Willow Garage, Inc. nor the names of its
00019 #*     contributors may be used to endorse or promote products derived
00020 #*     from this software without specific prior written permission.
00021 #*
00022 #*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 #*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 #*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 #*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 #*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 #*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 #*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 #*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 #*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 #*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 #*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 #*  POSSIBILITY OF SUCH DAMAGE.
00034 #* 
00035 #* Author: Eitan Marder-Eppstein
00036 #***********************************************************
00037 PKG = 'kinect_depth_calibration'
00038 NAME = 'depth_calibrator'
00039 
00040 import roslib; roslib.load_manifest(PKG)
00041 
00042 import rospy
00043 import tf
00044 import numpy
00045 import math
00046 from geometry_msgs.msg import PoseStamped
00047 from kinect_depth_calibration.srv import GetCheckerboardPose, GetCheckerboardPoseResponse, GetCheckerboardPoseRequest
00048 from kinect_depth_calibration.srv import GetCheckerboardCenter, GetCheckerboardCenterResponse, GetCheckerboardCenterRequest
00049 
00050 from geometry_msgs.msg import Quaternion, Pose
00051 
00052 from sensor_msgs.msg import CameraInfo
00053 
00054 def msg_to_quaternion(msg):
00055   return [msg.x, msg.y, msg.z, msg.w]
00056 
00057 def quaternion_to_msg(q):
00058   msg = Quaternion()
00059   msg.x = q[0]
00060   msg.y = q[1]
00061   msg.z = q[2]
00062   msg.w = q[3]
00063   return msg
00064 
00065 def msg_to_pose(msg):
00066   trans = tf.transformations.quaternion_matrix(msg_to_quaternion(msg.orientation))
00067   trans[0, 3] = msg.position.x
00068   trans[1, 3] = msg.position.y
00069   trans[2, 3] = msg.position.z
00070   trans[3, 3] = 1.0
00071   return trans
00072 
00073 def pose_to_msg(pose):
00074   msg = Pose()
00075   msg.position.x = pose[0, 3]
00076   msg.position.y = pose[1, 3]
00077   msg.position.z = pose[2, 3]
00078   q = tf.transformations.quaternion_from_matrix(pose)
00079   msg.orientation.x = q[0]
00080   msg.orientation.y = q[1]
00081   msg.orientation.z = q[2]
00082   msg.orientation.w = q[3]
00083   return msg
00084 
00085 def find_board_min(board_pose, corners_x, corners_y, spacing):
00086     trans = msg_to_pose(board_pose.pose)
00087     origin = tf.transformations.translation_matrix([-spacing, -spacing, 0.0])
00088     pose_mat = numpy.dot(trans, origin)
00089     return pose_to_msg(pose_mat)
00090 
00091 def find_board_max(board_pose, corners_x, corners_y, spacing):
00092     trans = msg_to_pose(board_pose.pose)
00093     origin = tf.transformations.translation_matrix([((corners_x) * spacing), ((corners_y) * spacing), 0.0])
00094     pose_mat = numpy.dot(trans, origin)
00095     return pose_to_msg(pose_mat)
00096 
00097 def find_board_center(board_pose, corners_x, corners_y, spacing):
00098     trans = msg_to_pose(board_pose.pose)
00099     origin = tf.transformations.translation_matrix([((corners_x - 1) * spacing) / 2.0, ((corners_y - 1) * spacing) / 2.0, 0.0])
00100     pose_mat = numpy.dot(trans, origin)
00101     return pose_to_msg(pose_mat)
00102 
00103 def project_pose(proj, pose):
00104     position = pose.pose.position
00105     point = numpy.array([[position.x], [position.y], [position.z]])
00106     mult = numpy.dot(proj, point)
00107     return mult / mult[2]
00108 
00109 def depth_calibrator_main(argv=None):
00110     rospy.init_node(NAME, anonymous=False)
00111     pose_pub = rospy.Publisher("new_pose", PoseStamped, latch=True)
00112 
00113     corners_x = rospy.get_param('~corners_x', 5)
00114     corners_y = rospy.get_param('~corners_y', 4)
00115     spacing = rospy.get_param('~spacing', 0.0245)
00116 
00117     listener = tf.TransformListener()
00118 
00119     print "Waiting to receive camera info for the depth camera..."
00120     depth_info = rospy.wait_for_message('depth_camera_info', CameraInfo)
00121     depth_proj = numpy.reshape(numpy.array(depth_info.P), (3,4))[:3, :3]
00122     depth_frame_id = depth_info.header.frame_id
00123     print "Got camera info, the depth camera operates in frame %s" % depth_frame_id
00124 
00125     run = True
00126     while run:
00127 
00128         while(raw_input("Is a checkerboard visible for your image producing camera? (y/n): ") != 'y'):
00129             print "For the Kinect, you may need to cover the IR projector for the CB to be detected."
00130 
00131         print "OK, thanks! Checking for the 'get_checkerboard_pose' service."
00132 
00133         rospy.wait_for_service('get_checkerboard_pose')
00134 
00135         print "Found the checkerboard service."
00136 
00137         print "Looking for a %d x %d checkerboard, with %.4f spacing...." % (corners_x, corners_y, spacing)
00138 
00139         cb_detector = rospy.ServiceProxy('get_checkerboard_pose', GetCheckerboardPose)
00140         cb = cb_detector.call(GetCheckerboardPoseRequest(corners_x, corners_y, spacing, spacing))
00141         img_center = find_board_center(cb.board_pose, corners_x, corners_y, spacing)
00142         img_msg = PoseStamped()
00143         img_msg.header = cb.board_pose.header
00144         img_msg.pose = img_center
00145         pose_pub.publish(img_msg)
00146 
00147         depth_center = None
00148         #now we want to transform the pose into the depth frame if the image frame is different
00149         if depth_frame_id is not None and img_msg.header.frame_id != depth_frame_id:
00150             img_min = PoseStamped(cb.board_pose.header, find_board_min(cb.board_pose, corners_x, corners_y, spacing))
00151             img_max = PoseStamped(cb.board_pose.header, find_board_max(cb.board_pose, corners_x, corners_y, spacing))
00152 
00153             print "Waiting for the transform between %s and %s" % (img_msg.header.frame_id, depth_frame_id)
00154             listener.waitForTransform(depth_frame_id, img_msg.header.frame_id, img_msg.header.stamp, rospy.Duration(2.0))
00155             depth_pose = listener.transformPose(depth_frame_id, img_msg)
00156             depth_min = listener.transformPose(depth_frame_id, img_min)
00157             depth_max = listener.transformPose(depth_frame_id, img_max)
00158 
00159             depth_min_pt = project_pose(depth_proj, depth_min)
00160             depth_max_pt = project_pose(depth_proj, depth_max)
00161 
00162             cb.min_x = depth_min_pt[0]
00163             cb.min_y = depth_min_pt[1]
00164             cb.max_x = depth_max_pt[0]
00165             cb.max_y = depth_max_pt[1]
00166 
00167             depth_center = depth_pose.pose
00168         else:
00169             depth_center = img_center
00170 
00171         print "Successfully found a checkerboard, with depth %.4f, noise_vel: %f, noise_rot: %f in frame %s" % (depth_center.position.z, 
00172                                                                                                                     cb.noise_vel, cb.noise_rot, 
00173                                                                                                                     depth_frame_id)
00174 
00175 
00176         print "Now we want to look for the center of the checkerboard in the depth image."
00177         while(raw_input("Have you uncovered the IR projector and turned off any other IR light source? (y/n): ") != 'y'):
00178             print "Don't be difficult, just do it."
00179 
00180         print "OK, thanks! Waiting for the 'get_checkerboard_center' service."
00181 
00182         rospy.wait_for_service('get_checkerboard_center')
00183 
00184         print "Found the service, looking for the center of the checkerboard bounded by the rectangle (%.2f, %.2f), (%.2f, %.2f)." % (
00185             cb.min_x, cb.min_y, cb.max_x, cb.max_y)
00186 
00187         center_detector = rospy.ServiceProxy('get_checkerboard_center', GetCheckerboardCenter)
00188         center_depth = center_detector.call(GetCheckerboardCenterRequest(cb.min_x, cb.max_x, cb.min_y, cb.max_y, depth_center.position.z))
00189 
00190         print "Found the center of the board at depth %.4f in the pointcloud" % (center_depth.depth)
00191 
00192         print "The offset between the img and the point cloud is: %.4f" % (depth_center.position.z - center_depth.depth)
00193 
00194         run = raw_input("Would you like to run again? (y/n): ") == 'y'
00195 
00196 if __name__ == '__main__':
00197     depth_calibrator_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