downsample_camera.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # A simple script to downsample camera images and camera_info messages
00004 #
00005 # Copyright 2012 by Daniel Maier, University of Freiburg
00006 #
00007 # This program is free software: you can redistribute it and/or modify
00008 # it under the terms of the GNU General Public License as published by
00009 # the Free Software Foundation, version 3.
00010 #
00011 # This program is distributed in the hope that it will be useful,
00012 # but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014 # GNU General Public License for more details.
00015 #
00016 # You should have received a copy of the GNU General Public License
00017 # along with this program.  If not, see <http://www.gnu.org/licenses/>.
00018 #
00019 
00020 import roslib
00021 roslib.load_manifest('fr_tools')
00022 import sys,  message_filters, rospy
00023 from sensor_msgs.msg import Image, CameraInfo
00024 import cv
00025 from cv_bridge import CvBridge, CvBridgeError
00026 
00027 def create_camera_info( info_in, width, height):
00028     info_out = info_in # copy header
00029     x_fac = float(width)/info_in.width
00030     y_fac = float(height)/info_in.height
00031     info_out.width = width
00032     info_out.height = height
00033     K = list(info_in.K)
00034     P = list(info_in.P)
00035     K[0:3] = [i*x_fac for i in K[0:3]]
00036     K[3:6] = [i*y_fac for i in K[3:6]]
00037     P[0:4] = [i*x_fac for i in P[0:4]]
00038     P[4:8] = [i*y_fac for i in P[4:8]]
00039     info_out.K = tuple(K)
00040     info_out.P = tuple(P)
00041 
00042     return info_out
00043 
00044 class DownSampler:
00045     def __init__(self, out_prefix, x_fac, y_fac):
00046         self.bridge = CvBridge()
00047         self.image_sub = message_filters.Subscriber( 'image', Image)
00048         self.info_sub = message_filters.Subscriber ( 'camera_info', CameraInfo)
00049         self.image_pub = rospy.Publisher(out_prefix + '/image_color', Image)
00050         self.info_pub = rospy.Publisher(out_prefix + '/camera_info', CameraInfo)
00051         self.ts = message_filters.TimeSynchronizer([self.image_sub, self.info_sub], 10)
00052         self.ts.registerCallback(self.callback)
00053         self.x_fac = x_fac
00054         self.y_fac = y_fac
00055 
00056 
00057     def callback(self, img, info):
00058         #print "image received"
00059         try:
00060             cv_image_in = self.bridge.imgmsg_to_cv(img, desired_encoding=img.encoding)
00061         except CvBridgeError, e:
00062             print e
00063             return
00064         x_fac = self.x_fac
00065         y_fac = self.y_fac
00066         cv_image_out = cv.CreateMat( int( round( cv_image_in.rows * y_fac)), int( round(cv_image_in.cols * x_fac)), cv_image_in.type)
00067         cv.Resize(cv_image_in, cv_image_out)
00068         try:
00069             img_out = self.bridge.cv_to_imgmsg(cv_image_out, img.encoding)
00070             img_out.header = img.header
00071             info_out = create_camera_info(info, img_out.width, img_out.height)
00072             self.image_pub.publish(img_out)
00073             #self.image_pub.publish(img)
00074             self.info_pub.publish(info_out)
00075         except CvBridgeError, e:
00076             print e
00077         return
00078 
00079 if __name__ == '__main__':
00080     rospy.init_node('camera_downsampler', anonymous=True)
00081 
00082     if len(sys.argv) < 4:
00083         print "Usage is: %s camera_prefix x_factor y_factor" % (sys.argv[0])
00084         sys.exit(0)
00085     camera_prefix = sys.argv[1]
00086     xfac = float(sys.argv[2])
00087     yfac = float(sys.argv[3])
00088     ds = DownSampler(camera_prefix, xfac, yfac)
00089     rospy.spin()
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


fr_tools
Author(s): Armin Hornung, Daniel Maier
autogenerated on Wed Dec 26 2012 16:19:58