downsample_camera.py

Go to the documentation of this file.

#!/usr/bin/env python
#
# A simple script to downsample camera images and camera_info messages
#
# Copyright 2012 by Daniel Maier, University of Freiburg
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, version 3.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program.  If not, see <http://www.gnu.org/licenses/>.
#

import roslib
roslib.load_manifest('fr_tools')
import sys,  message_filters, rospy
from sensor_msgs.msg import Image, CameraInfo
import cv
from cv_bridge import CvBridge, CvBridgeError

def create_camera_info( info_in, width, height):
    info_out = info_in # copy header
    x_fac = float(width)/info_in.width
    y_fac = float(height)/info_in.height
    info_out.width = width
    info_out.height = height
    K = list(info_in.K)
    P = list(info_in.P)
    K[0:3] = [i*x_fac for i in K[0:3]]
    K[3:6] = [i*y_fac for i in K[3:6]]
    P[0:4] = [i*x_fac for i in P[0:4]]
    P[4:8] = [i*y_fac for i in P[4:8]]
    info_out.K = tuple(K)
    info_out.P = tuple(P)

    return info_out

class DownSampler:
    def __init__(self, out_prefix, x_fac, y_fac):
        self.bridge = CvBridge()
        self.image_sub = message_filters.Subscriber( 'image', Image)
        self.info_sub = message_filters.Subscriber ( 'camera_info', CameraInfo)
        self.image_pub = rospy.Publisher(out_prefix + '/image_color', Image)
        self.info_pub = rospy.Publisher(out_prefix + '/camera_info', CameraInfo)
        self.ts = message_filters.TimeSynchronizer([self.image_sub, self.info_sub], 10)
        self.ts.registerCallback(self.callback)
        self.x_fac = x_fac
        self.y_fac = y_fac


    def callback(self, img, info):
        #print "image received"
        try:
            cv_image_in = self.bridge.imgmsg_to_cv(img, desired_encoding=img.encoding)
        except CvBridgeError, e:
            print e
            return
        x_fac = self.x_fac
        y_fac = self.y_fac
        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)
        cv.Resize(cv_image_in, cv_image_out)
        try:
            img_out = self.bridge.cv_to_imgmsg(cv_image_out, img.encoding)
            img_out.header = img.header
            info_out = create_camera_info(info, img_out.width, img_out.height)
            self.image_pub.publish(img_out)
            #self.image_pub.publish(img)
            self.info_pub.publish(info_out)
        except CvBridgeError, e:
            print e
        return

if __name__ == '__main__':
    rospy.init_node('camera_downsampler', anonymous=True)

    if len(sys.argv) < 4:
        print "Usage is: %s camera_prefix x_factor y_factor" % (sys.argv[0])
        sys.exit(0)
    camera_prefix = sys.argv[1]
    xfac = float(sys.argv[2])
    yfac = float(sys.argv[3])
    ds = DownSampler(camera_prefix, xfac, yfac)
    rospy.spin()