00001
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()
