camera_info_converter.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 import copy
00004 import roslib; roslib.load_manifest('camera_pose_calibration')
00005 import rospy
00006 import threading
00007 from sensor_msgs.msg import CameraInfo
00008 
00009 
00010 def unbin(msg_in):
00011     '''
00012     Modify projection matrix such that it corresponds to the binned image
00013     '''
00014     msg_out = copy.deepcopy(msg_in)
00015     b_x = msg_in.binning_x
00016     b_y = msg_in.binning_y
00017     off_x = msg_in.roi.x_offset
00018     off_y = msg_in.roi.y_offset
00019 
00020     P = list(msg_in.P)
00021 
00022     if b_x == 0:
00023         b_x = 1
00024     if b_y == 0:
00025         b_y = 1
00026 
00027     P[0] /= b_x
00028     P[1] /= b_x
00029     P[2] = (P[2] - off_x)/b_x
00030     P[3] /= b_x
00031 
00032     P[4] /= b_y
00033     P[5] /= b_y
00034     P[6] = (P[6] - off_y)/b_y
00035     P[7] /= b_y
00036 
00037     msg_out.P = P
00038     msg_out.binning_x = 1
00039     msg_out.binning_y = 1
00040 
00041     msg_out.height /= b_x
00042     msg_out.width /= b_y
00043 
00044     msg_out.roi.x_offset = 0
00045     msg_out.roi.y_offset = 0
00046     msg_out.roi.height = msg_out.height
00047     msg_out.roi.width = msg_out.width
00048 
00049     msg_out.D = msg_in.D
00050     msg_out.R = msg_in.R
00051     msg_out.K = (P[0], P[1], P[2],
00052                  P[4], P[5], P[6],
00053                  P[8], P[9], P[10])
00054     return msg_out;
00055 
00056 
00057 class CameraInfoConverter:
00058     def __init__(self):
00059         self.lock = threading.Lock()
00060         self.pub_interval = rospy.Duration(rospy.get_param('~publish_interval', 0.0))
00061         self.last_pub = rospy.Time()
00062         self.pub = rospy.Publisher('camera_info_out', CameraInfo)
00063         self.sub = rospy.Subscriber('camera_info_in', CameraInfo, self.cam_info_cb)
00064 
00065     def cam_info_cb(self, msg):
00066         with self.lock:
00067             time_now = rospy.Time.now()
00068             if self.last_pub + self.pub_interval < time_now:
00069                 self.pub.publish(unbin(msg))
00070                 self.last_pub = time_now
00071 
00072 def main():
00073     rospy.init_node('camera_info_converter')
00074     camera_converter = CameraInfoConverter()
00075     rospy.spin()
00076 
00077 
00078 
00079 if __name__ == '__main__':
00080     main()
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


camera_pose_calibration
Author(s): Vijay Pradeep, Wim Meeussen
autogenerated on Thu Aug 15 2013 10:18:08