#!/usr/bin/python
PKG = 'camera_calibration_parsers_python'
import roslib;

roslib.load_manifest(PKG)
import yaml
import sensor_msgs.msg


def parse_yaml(filename):
    stream = file(filename, 'r')
    calib_data = yaml.load(stream)
    cam_info = sensor_msgs.msg.CameraInfo()
    cam_info.width = calib_data['image_width']
    cam_info.height = calib_data['image_height']
    cam_info.K = calib_data['camera_matrix']['data']
    cam_info.D = calib_data['distortion_coefficients']['data']
    cam_info.R = calib_data['rectification_matrix']['data']
    cam_info.P = calib_data['projection_matrix']['data']
    cam_info.distortion_model = calib_data['distortion_model']
    return cam_info


if __name__ == "__main__":
    import argparse

    parser = argparse.ArgumentParser(
        description='Parses camera info yaml files and returns them as sensor_msgs.msg.CameraInfo.')
    parser.add_argument('filename', help='input yaml file')
    args = parser.parse_args()
    try:
        info = parse_yaml(args.filename)
        print 'Read the following info from', args.filename, '\n', info
    except Exception, e:
        import traceback

        traceback.print_exc()
