def main(args):
  from optparse import OptionParser
  parser = OptionParser()
  parser.add_option("-s", "--size", default="8x6", help="specify chessboard size as nxm [default: %default]")
  parser.add_option("-q", "--square", default=".108", help="specify chessboard square size in meters [default: %default]")
  options, args = parser.parse_args()
  size = tuple([int(c) for c in options.size.split('x')])
  dim = float(options.square)

  images = []
  for fname in args:
    if os.path.isfile(fname):
      img = cv.LoadImage(fname)
      if img is None:
        print("[WARN] Couldn't open image " + fname + "!", file=sys.stderr)
        sys.exit(1)
      else:
        print("[INFO] Loaded " + fname + " (" + str(img.width) + "x" + str(img.height) + ")")

      images.append(img)

  cboard = ChessboardInfo()
  cboard.dim = dim
  cboard.n_cols = size[0]
  cboard.n_rows = size[1]
  
  mc = MonoCalibrator([cboard])
  mc.cal(images)
  print(mc.as_message())
Exemple #2
0
    def test_nochecker(self):
        # Run with same images, but looking for an incorrect chessboard size (8, 7).
        # Should raise an exception because of lack of input points.
        new_board = copy.deepcopy(board)
        new_board.n_cols = 8
        new_board.n_rows = 7

        sc = StereoCalibrator([new_board])
        self.assertRaises(CalibrationException, lambda: sc.cal(self.limages, self.rimages))
        mc = MonoCalibrator([new_board])
        self.assertRaises(CalibrationException, lambda: mc.cal(self.limages))
def cal_from_tarfile(boards, tarname, mono = False, upload = False):
    if mono:
        calibrator = MonoCalibrator(boards)
    else:
        calibrator = StereoCalibrator(boards)

    calibrator.do_tarfile_calibration(tarname)

    print calibrator.ost()

    if upload: 
        info = calibrator.as_message()
        if mono:
            set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"), sensor_msgs.srv.SetCameraInfo)

            response = set_camera_info_service(info)
            if not response.success:
                raise RuntimeError("connected to set_camera_info service, but failed setting camera_info")
        else:
            set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"), sensor_msgs.srv.SetCameraInfo)
            set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"), sensor_msgs.srv.SetCameraInfo)

            response1 = set_camera_info_service(info[0])
            response2 = set_camera_info_service(info[1])
            if not (response1.success and response2.success):
                raise RuntimeError("connected to set_camera_info service, but failed setting camera_info")
    def test_monocular(self):
        # Run the calibrator, produce a calibration, check it
        for i, setup in enumerate(self.setups):
            board = ChessboardInfo()
            board.n_cols = setup.cols
            board.n_rows = setup.rows
            board.dim = self.board_width_dim

            mc = MonoCalibrator([ board ], flags=cv2.CALIB_FIX_K3, pattern=setup.pattern)

            if 0:
                # display the patterns viewed by the camera
                for pattern_warped in self.limages[i]:
                    cv2.imshow("toto", pattern_warped)
                    cv2.waitKey(0)

            mc.cal(self.limages[i])
            self.assert_good_mono(mc, self.limages[i], setup.lin_err)

            # Make sure the intrinsics are similar
            err_intrinsics = numpy.linalg.norm(mc.intrinsics - self.K, ord=numpy.inf)
            self.assert_(err_intrinsics < setup.K_err, 'intrinsics error is %f' % err_intrinsics)
            print('intrinsics error is %f' % numpy.linalg.norm(mc.intrinsics - self.K, ord=numpy.inf))
def cal_from_tarfile(boards, tarname, mono = False):
    if mono:
        calibrator = MonoCalibrator(boards)
    else:
        calibrator = StereoCalibrator(boards)

    calibrator.do_tarfile_calibration(tarname)

    print calibrator.ost()
Exemple #6
0
    def test_monocular(self):
        # Run the calibrator, produce a calibration, check it
        mc = MonoCalibrator([ board ], cv2.CALIB_FIX_K3)
        for dim in self.sizes:
            mc.cal(self.l[dim])
            self.assert_good_mono(mc, dim)

            # Make another calibration, import previous calibration as a message,
            # and assert that the new one is good.

            mc2 = MonoCalibrator([board])
            mc2.from_message(mc.as_message())
            self.assert_good_mono(mc2, dim)
Exemple #7
0
    def test_monocular(self):
        # Run the calibrator, produce a calibration, check it
        mc = MonoCalibrator([ board ], cv2.CALIB_FIX_K3)
        max_errs = [0.1, 0.2, 0.4, 0.7]
        for i, dim in enumerate(self.sizes):
            mc.cal(self.l[dim])
            self.assert_good_mono(mc, dim, max_errs[i])

            # Make another calibration, import previous calibration as a message,
            # and assert that the new one is good.

            mc2 = MonoCalibrator([board])
            mc2.from_message(mc.as_message())
            self.assert_good_mono(mc2, dim, max_errs[i])
Exemple #8
0
    def __init__(self, chess_size, dim):
        self.board = ChessboardInfo()
        self.board.n_cols = chess_size[0]
        self.board.n_rows = chess_size[1]
        self.board.dim = dim

        image_topic = rospy.resolve_name("monocular") + "/image_rect"
        camera_topic = rospy.resolve_name("monocular") + "/camera_info"

        tosync_mono = [
            (image_topic, sensor_msgs.msg.Image),
            (camera_topic, sensor_msgs.msg.CameraInfo),
        ]

        tsm = message_filters.TimeSynchronizer([message_filters.Subscriber(topic, type) for (topic, type) in tosync_mono], 10)
        tsm.registerCallback(self.queue_monocular)

        left_topic = rospy.resolve_name("stereo") + "/left/image_rect"
        left_camera_topic = rospy.resolve_name("stereo") + "/left/camera_info"
        right_topic = rospy.resolve_name("stereo") + "/right/image_rect"
        right_camera_topic = rospy.resolve_name("stereo") + "/right/camera_info"

        tosync_stereo = [
            (left_topic, sensor_msgs.msg.Image),
            (left_camera_topic, sensor_msgs.msg.CameraInfo),
            (right_topic, sensor_msgs.msg.Image),
            (right_camera_topic, sensor_msgs.msg.CameraInfo)
        ]

        tss = message_filters.TimeSynchronizer([message_filters.Subscriber(topic, type) for (topic, type) in tosync_stereo], 10)
        tss.registerCallback(self.queue_stereo)

        self.br = cv_bridge.CvBridge()

        self.q_mono = Queue()
        self.q_stereo = Queue()

        mth = ConsumerThread(self.q_mono, self.handle_monocular)
        mth.setDaemon(True)
        mth.start()

        sth = ConsumerThread(self.q_stereo, self.handle_stereo)
        sth.setDaemon(True)
        sth.start()

        self.mc = MonoCalibrator([self.board])
        self.sc = StereoCalibrator([self.board])
Exemple #9
0
class CameraCheckerNode:

    def __init__(self, chess_size, dim):
        self.board = ChessboardInfo()
        self.board.n_cols = chess_size[0]
        self.board.n_rows = chess_size[1]
        self.board.dim = dim

        image_topic = rospy.resolve_name("monocular") + "/image_rect"
        camera_topic = rospy.resolve_name("monocular") + "/camera_info"

        tosync_mono = [
            (image_topic, sensor_msgs.msg.Image),
            (camera_topic, sensor_msgs.msg.CameraInfo),
        ]

        tsm = message_filters.TimeSynchronizer([message_filters.Subscriber(topic, type) for (topic, type) in tosync_mono], 10)
        tsm.registerCallback(self.queue_monocular)

        left_topic = rospy.resolve_name("stereo") + "/left/image_rect"
        left_camera_topic = rospy.resolve_name("stereo") + "/left/camera_info"
        right_topic = rospy.resolve_name("stereo") + "/right/image_rect"
        right_camera_topic = rospy.resolve_name("stereo") + "/right/camera_info"

        tosync_stereo = [
            (left_topic, sensor_msgs.msg.Image),
            (left_camera_topic, sensor_msgs.msg.CameraInfo),
            (right_topic, sensor_msgs.msg.Image),
            (right_camera_topic, sensor_msgs.msg.CameraInfo)
        ]

        tss = message_filters.TimeSynchronizer([message_filters.Subscriber(topic, type) for (topic, type) in tosync_stereo], 10)
        tss.registerCallback(self.queue_stereo)

        self.br = cv_bridge.CvBridge()

        self.q_mono = Queue()
        self.q_stereo = Queue()

        mth = ConsumerThread(self.q_mono, self.handle_monocular)
        mth.setDaemon(True)
        mth.start()

        sth = ConsumerThread(self.q_stereo, self.handle_stereo)
        sth.setDaemon(True)
        sth.start()

        self.mc = MonoCalibrator([self.board])
        self.sc = StereoCalibrator([self.board])

    def queue_monocular(self, msg, cmsg):
        self.q_mono.put((msg, cmsg))

    def queue_stereo(self, lmsg, lcmsg, rmsg, rcmsg):
        self.q_stereo.put((lmsg, lcmsg, rmsg, rcmsg))

    def mkgray(self, msg):
        return self.mc.mkgray(msg)

    def image_corners(self, im):
        (ok, corners, b) = self.mc.get_corners(im)
        if ok:
            return corners
        else:
            return None

    def handle_monocular(self, msg):

        (image, camera) = msg
        gray = self.mkgray(image)
        C = self.image_corners(gray)
        if C:
            linearity_rms = self.mc.linear_error(C, self.board)

            # Add in reprojection check
            image_points = C
            object_points = self.mc.mk_object_points([self.board], use_board_size=True)[0]
            dist_coeffs = numpy.zeros((4, 1))
            camera_matrix = numpy.array( [ [ camera.P[0], camera.P[1], camera.P[2]  ],
                                           [ camera.P[4], camera.P[5], camera.P[6]  ],
                                           [ camera.P[8], camera.P[9], camera.P[10] ] ] )
            ok, rot, trans = cv2.solvePnP(object_points, image_points, camera_matrix, dist_coeffs)
            # Convert rotation into a 3x3 Rotation Matrix
            rot3x3, _ = cv2.Rodrigues(rot)
            # Reproject model points into image
            object_points_world = numpy.asmatrix(rot3x3) * (numpy.asmatrix(object_points.squeeze().T) + numpy.asmatrix(trans))
            reprojected_h = camera_matrix * object_points_world
            reprojected   = (reprojected_h[0:2, :] / reprojected_h[2, :])
            reprojection_errors = image_points.squeeze().T - reprojected.T
            reprojection_rms = numpy.sqrt(numpy.sum(numpy.array(reprojection_errors) ** 2) / numpy.product(reprojection_errors.shape))

            # Print the results
            print("Linearity RMS Error: %.3f Pixels      Reprojection RMS Error: %.3f Pixels" % (linearity_rms, reprojection_rms))
        else:
            print('no chessboard')

    def handle_stereo(self, msg):

        (lmsg, lcmsg, rmsg, rcmsg) = msg
        lgray = self.mkgray(lmsg)
        rgray = self.mkgray(rmsg)

        L = self.image_corners(lgray)
        R = self.image_corners(rgray)
        if L and R:
            epipolar = self.sc.epipolar_error(L, R)

            dimension = self.sc.chessboard_size(L, R, [self.board], msg=(lcmsg, rcmsg))

            print("epipolar error: %f pixels   dimension: %f m" % (epipolar, dimension))
        else:
            print("no chessboard")
class CameraCheckerNode:
    def __init__(self, chess_size, dim):
        self.board = ChessboardInfo()
        self.board.n_cols = chess_size[0]
        self.board.n_rows = chess_size[1]
        self.board.dim = dim

        image_topic = rospy.resolve_name("monocular") + "/image_rect"
        camera_topic = rospy.resolve_name("monocular") + "/camera_info"

        tosync_mono = [(image_topic, sensor_msgs.msg.Image), (camera_topic, sensor_msgs.msg.CameraInfo)]

        tsm = message_filters.TimeSynchronizer(
            [message_filters.Subscriber(topic, type) for (topic, type) in tosync_mono], 10
        )
        tsm.registerCallback(self.queue_monocular)

        left_topic = rospy.resolve_name("stereo") + "/left/image_rect"
        left_camera_topic = rospy.resolve_name("stereo") + "/left/camera_info"
        right_topic = rospy.resolve_name("stereo") + "/right/image_rect"
        right_camera_topic = rospy.resolve_name("stereo") + "/right/camera_info"

        tosync_stereo = [
            (left_topic, sensor_msgs.msg.Image),
            (left_camera_topic, sensor_msgs.msg.CameraInfo),
            (right_topic, sensor_msgs.msg.Image),
            (right_camera_topic, sensor_msgs.msg.CameraInfo),
        ]

        tss = message_filters.TimeSynchronizer(
            [message_filters.Subscriber(topic, type) for (topic, type) in tosync_stereo], 10
        )
        tss.registerCallback(self.queue_stereo)

        self.br = cv_bridge.CvBridge()

        self.q_mono = Queue.Queue()
        self.q_stereo = Queue.Queue()

        mth = ConsumerThread(self.q_mono, self.handle_monocular)
        mth.setDaemon(True)
        mth.start()

        sth = ConsumerThread(self.q_stereo, self.handle_stereo)
        sth.setDaemon(True)
        sth.start()

        self.mc = MonoCalibrator([self.board])

    def queue_monocular(self, msg, cmsg):
        self.q_mono.put((msg, cmsg))

    def queue_stereo(self, lmsg, lcmsg, rmsg, rcmsg):
        self.q_stereo.put((lmsg, lcmsg, rmsg, rcmsg))

    def mkgray(self, msg):
        return self.br.imgmsg_to_cv(msg, "bgr8")

    def image_corners(self, im):
        (ok, corners, b) = self.mc.get_corners(im)
        if ok:
            return list(cvmat_iterator(cv.Reshape(self.mc.mk_image_points([(corners, b)]), 2)))
        else:
            return None

    def handle_monocular(self, msg):
        def pt2line(x0, y0, x1, y1, x2, y2):
            """ point is (x0, y0), line is (x1, y1, x2, y2) """
            return abs((x2 - x1) * (y1 - y0) - (x1 - x0) * (y2 - y1)) / math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)

        (image, camera) = msg
        rgb = self.mkgray(image)
        C = self.image_corners(rgb)
        if C:
            cc = self.board.n_cols
            cr = self.board.n_rows
            errors = []
            for r in range(cr):
                (x1, y1) = C[(cc * r) + 0]
                (x2, y2) = C[(cc * r) + cc - 1]
                for i in range(1, cc - 1):
                    (x0, y0) = C[(cc * r) + i]
                    errors.append(pt2line(x0, y0, x1, y1, x2, y2))
            linearity_rms = math.sqrt(sum([e ** 2 for e in errors]) / len(errors))

            # Add in reprojection check
            A = cv.CreateMat(3, 3, 0)
            image_points = cv.fromarray(numpy.array(C))
            object_points = cv.fromarray(numpy.zeros([cc * cr, 3]))
            for i in range(cr):
                for j in range(cc):
                    object_points[i * cc + j, 0] = j * self.board.dim
                    object_points[i * cc + j, 1] = i * self.board.dim
                    object_points[i * cc + j, 2] = 0.0
            dist_coeffs = cv.fromarray(numpy.array([[0.0, 0.0, 0.0, 0.0]]))
            camera_matrix = numpy.array(
                [
                    [camera.P[0], camera.P[1], camera.P[2]],
                    [camera.P[4], camera.P[5], camera.P[6]],
                    [camera.P[8], camera.P[9], camera.P[10]],
                ]
            )
            rot = cv.CreateMat(3, 1, cv.CV_32FC1)
            trans = cv.CreateMat(3, 1, cv.CV_32FC1)
            cv.FindExtrinsicCameraParams2(
                object_points, image_points, cv.fromarray(camera_matrix), dist_coeffs, rot, trans
            )
            # Convert rotation into a 3x3 Rotation Matrix
            rot3x3 = cv.CreateMat(3, 3, cv.CV_32FC1)
            cv.Rodrigues2(rot, rot3x3)
            # Reproject model points into image
            object_points_world = numpy.asmatrix(rot3x3) * (numpy.asmatrix(object_points).T) + numpy.asmatrix(trans)
            reprojected_h = camera_matrix * object_points_world
            reprojected = reprojected_h[0:2, :] / reprojected_h[2, :]
            reprojection_errors = image_points - reprojected.T
            reprojection_rms = numpy.sqrt(
                numpy.sum(numpy.array(reprojection_errors) ** 2) / numpy.product(reprojection_errors.shape)
            )

            # Print the results
            print "Linearity RMS Error: %.3f Pixels      Reprojection RMS Error: %.3f Pixels" % (
                linearity_rms,
                reprojection_rms,
            )
        else:
            print "no chessboard"

    def handle_stereo(self, msg):

        (lmsg, lcmsg, rmsg, rcmsg) = msg
        lrgb = self.mkgray(lmsg)
        rrgb = self.mkgray(rmsg)

        sc = StereoCalibrator([self.board])

        L = self.image_corners(lrgb)
        R = self.image_corners(rrgb)
        scm = image_geometry.StereoCameraModel()
        scm.fromCameraInfo(lcmsg, rcmsg)
        if L and R:
            d = [(y0 - y1) for ((_, y0), (_, y1)) in zip(L, R)]
            epipolar = math.sqrt(sum([i ** 2 for i in d]) / len(d))

            disparities = [(x0 - x1) for ((x0, y0), (x1, y1)) in zip(L, R)]
            pt3d = [scm.projectPixelTo3d((x, y), d) for ((x, y), d) in zip(L, disparities)]

            def l2(p0, p1):
                return math.sqrt(sum([(c0 - c1) ** 2 for (c0, c1) in zip(p0, p1)]))

            # Compute the length from each horizontal and vertical lines, and return the mean
            cc = self.board.n_cols
            cr = self.board.n_rows
            lengths = [l2(pt3d[cc * r + 0], pt3d[cc * r + (cc - 1)]) / (cc - 1) for r in range(cr)] + [
                l2(pt3d[c + 0], pt3d[c + (cc * (cr - 1))]) / (cr - 1) for c in range(cc)
            ]
            dimension = sum(lengths) / len(lengths)

            print "epipolar error: %f pixels   dimension: %f m" % (epipolar, dimension)
        else:
            print "no chessboard"
def cal_from_tarfile(boards, tarname, mono = False, upload = False, calib_flags = 0, visualize = False, alpha=1.0):
    if mono:
        calibrator = MonoCalibrator(boards, calib_flags)
    else:
        calibrator = StereoCalibrator(boards, calib_flags)

    calibrator.do_tarfile_calibration(tarname)

    print(calibrator.ost())

    if upload: 
        info = calibrator.as_message()
        if mono:
            set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"), sensor_msgs.srv.SetCameraInfo)

            response = set_camera_info_service(info)
            if not response.success:
                raise RuntimeError("connected to set_camera_info service, but failed setting camera_info")
        else:
            set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"), sensor_msgs.srv.SetCameraInfo)
            set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"), sensor_msgs.srv.SetCameraInfo)

            response1 = set_left_camera_info_service(info[0])
            response2 = set_right_camera_info_service(info[1])
            if not (response1.success and response2.success):
                raise RuntimeError("connected to set_camera_info service, but failed setting camera_info")

    if visualize:

        #Show rectified images
        calibrator.set_alpha(alpha)

        archive = tarfile.open(tarname, 'r')
        if mono:
            for f in archive.getnames():
                if f.startswith('left') and (f.endswith('.pgm') or f.endswith('png')):
                    filedata = archive.extractfile(f).read()
                    file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8)
                    im=cv2.imdecode(file_bytes,cv2.CV_LOAD_IMAGE_COLOR)

                    bridge = cv_bridge.CvBridge()
                    try:
                        msg=bridge.cv2_to_imgmsg(im, "bgr8")
                    except cv_bridge.CvBridgeError as e:
                        print(e)

                    #handle msg returns the recitifed image with corner detection once camera is calibrated.
                    drawable=calibrator.handle_msg(msg)
                    vis=numpy.asarray( drawable.scrib[:,:])
                    #Display. Name of window:f
                    display(f, vis)
        else:
            limages = [ f for f in archive.getnames() if (f.startswith('left') and (f.endswith('pgm') or f.endswith('png'))) ]
            limages.sort()
            rimages = [ f for f in archive.getnames() if (f.startswith('right') and (f.endswith('pgm') or f.endswith('png'))) ]
            rimages.sort()

            if not len(limages) == len(rimages):
                raise RuntimeError("Left, right images don't match. %d left images, %d right" % (len(limages), len(rimages)))
            
            for i in range(len(limages)):
                l=limages[i]
                r=rimages[i]

                if l.startswith('left') and (l.endswith('.pgm') or l.endswith('png')) and r.startswith('right') and (r.endswith('.pgm') or r.endswith('png')):
                    # LEFT IMAGE
                    filedata = archive.extractfile(l).read()
                    file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8)
                    im_left=cv2.imdecode(file_bytes,cv2.CV_LOAD_IMAGE_COLOR)
       
                    bridge = cv_bridge.CvBridge()
                    try:
                        msg_left=bridge.cv2_to_imgmsg(im_left, "bgr8")
                    except cv_bridge.CvBridgeError as e:
                        print(e)

                    #RIGHT IMAGE
                    filedata = archive.extractfile(r).read()
                    file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8)
                    im_right=cv2.imdecode(file_bytes,cv2.CV_LOAD_IMAGE_COLOR)
                    try:
                        msg_right=bridge.cv2_to_imgmsg(im_right, "bgr8")
                    except cv_bridge.CvBridgeError as e:
                        print(e)

                    drawable=calibrator.handle_msg([ msg_left,msg_right] )

                    h, w = numpy.asarray(drawable.lscrib[:,:]).shape[:2]
                    vis = numpy.zeros((h, w*2, 3), numpy.uint8)
                    vis[:h, :w ,:] = numpy.asarray(drawable.lscrib[:,:])
                    vis[:h, w:w*2, :] = numpy.asarray(drawable.rscrib[:,:])
                    
                    display(l+" "+r,vis)    
Exemple #12
0
def find_intrinsics(visualize=False):
    fatal = False
    good = []
    xys = []

    if 1:
        with open('setup.json',mode='r') as fd:
            setup = json.loads(fd.read())

    globber = '*-corners*.data'
    files = glob.glob(globber)
    files.sort()
    print 'all data files matching "%s"'%(globber,)
    print files
    for f in files:
        this_fatal = False
        parts = f.split('-')
        board = None
        for p in parts:
            if p.startswith('board'):
                boardname = p
                with open(boardname+'.json',mode='r') as fd:
                    buf = fd.read()
                boardd = json.loads(buf)
                board = ChessboardInfo()
                board.n_cols = boardd['n_cols']
                board.n_rows = boardd['n_rows']
                board.dim = boardd['dim']

        assert board is not None

        xy = np.loadtxt(f)
        xys.append(xy)
        if len(xy) != board.n_cols * board.n_rows:
            rospy.logfatal('Error: %d points in %s. Expected %d.'%(
                len(xy), f, board.n_cols * board.n_rows))
            this_fatal = True
            fatal = True
        if visualize:
            if 0:
                plt.figure()
            fmt = 'o-'
            label = f
            if this_fatal:
                fmt = 'kx:'
                label = 'BAD: '+ f
            plt.plot( xy[:,0], xy[:,1], fmt, mfc='none', label=label )
            if this_fatal:
                for i in range(len(xy)):
                    plt.text( xy[i,0],
                              xy[i,1],
                              str(i) )
        corners = [ (x,y) for (x,y) in xy ]
        good.append( (corners, board) )
    if visualize:
        plt.legend()
        plt.show()
    if fatal:
        sys.exit(1)

    mc = MonoCalibrator([ board ])#, cv2.CALIB_FIX_K1 | cv2.CALIB_FIX_K2 | cv2.CALIB_FIX_K3 | cv2.CALIB_ZERO_TANGENT_DIST )
    mc.size = tuple(setup['size'])

    mc.cal_fromcorners(good)
    msg = mc.as_message()

    cam = pymvg.CameraModel.from_ros_like( intrinsics=msg )
    cam_mirror = cam.get_mirror_camera()

    if 1:

        # The intrinsics are valid for the whole physical display and
        # each virtual display.
        names = setup['names']

        for name in names:
            name = str(name) # remove unicode -- ROS bag fails on unicode topic name
            if 'mirror' in name:
                c = cam_mirror
            else:
                c = cam
            msg = c.get_intrinsics_as_msg()
            fname = 'display-intrinsic-cal-%s.bag'%( name.replace('/','-'), )
            bagout = rosbag.Bag(fname, 'w')
            topic = '/%s/camera_info'%name
            bagout.write(topic, msg)
            bagout.close()
            print 'saved to',fname

            print 'You can play this calibration with:\n'
            print 'rosbag play %s -l'%(fname,)
            print