Camera instrinsic matrix for D435i

[upasana27]

I wanted to get the camera instrinsic matrix of the d-435i camera as a json file. Rosbag files provide it in sensor_msgs format and the package contains yaml files. How can I get the json file ?

[cedrusx]

Hi @upasana27 , I do not know any exisiting tools to do the conversion. But guessing that it should not take too much time to re-format the intrinsics manually (or to make a script) into your required format. There are only two sets of intrinsics in OpenLORIS-Scene: one for office/cafe/home/corridor, and another for market data. Let me know if there be any values missing.

[cedrusx]

The following script reads intrinsics from a yaml and publish it into a CameraInfo topic. Might be a reference for you to do similar conversions.

There are two alternative methods cv_yaml_to_CameraInfo and yaml_to_CameraInfo to take care of different format. Both are different from the yaml provided by the OpenLORIS datasets.

#!/usr/bin/env python3

import rospy
import yaml
from sensor_msgs.msg import CameraInfo
import cv2

def cv_yaml_to_CameraInfo(yaml_fname):
    cvf = cv2.FileStorage()
    cvf.open(yaml_fname, cv2.FileStorage_READ)
    camera_info_msg = CameraInfo()
    camera_info_msg.width = int(cvf.getNode('ImageSize').at(0).real())
    camera_info_msg.height = int(cvf.getNode('ImageSize').at(1).real())
    camera_info_msg.K = cvf.getNode('CameraMat').mat().flatten()
    camera_info_msg.D = cvf.getNode('DistCoeff').mat().flatten()
    #camera_info_msg.R = calib_data["rectification_matrix"]["data"]
    #camera_info_msg.P = calib_data["projection_matrix"]["data"]
    #camera_info_msg.distortion_model = calib_data["distortion_model"]
    print(camera_info_msg)
    return camera_info_msg

def yaml_to_CameraInfo(yaml_fname):
    """
    Parse a yaml file containing camera calibration data (as produced by 
    rosrun camera_calibration cameracalibrator.py) into a 
    sensor_msgs/CameraInfo msg.

    Parameters
    ----------
    yaml_fname : str
        Path to yaml file containing camera calibration data
    Returns
    -------
    camera_info_msg : sensor_msgs.msg.CameraInfo
        A sensor_msgs.msg.CameraInfo message containing the camera calibration
        data
    """
    # Load data from file
    with open(yaml_fname, "r") as file_handle:
        calib_data = yaml.load(file_handle)
    # Parse
    camera_info_msg = CameraInfo()
    camera_info_msg.width = calib_data["image_width"]
    camera_info_msg.height = calib_data["image_height"]
    camera_info_msg.K = calib_data["camera_matrix"]["data"]
    camera_info_msg.D = calib_data["distortion_coefficients"]["data"]
    camera_info_msg.R = calib_data["rectification_matrix"]["data"]
    camera_info_msg.P = calib_data["projection_matrix"]["data"]
    camera_info_msg.distortion_model = calib_data["distortion_model"]
    return camera_info_msg

if __name__ == "__main__":
    # Get fname from command line (cmd line input required)
    import argparse
    arg_parser = argparse.ArgumentParser()
    arg_parser.add_argument('-f', "--filename", help="Path to yaml file containing " +\
                                             "camera calibration data")
    arg_parser.add_argument('-t', "--topic", help="Topic to publish camera_info")
    args = arg_parser.parse_args()
    filename = args.filename

    # Parse yaml file
    camera_info_msg = cv_yaml_to_CameraInfo(filename)

    # Initialize publisher node
    rospy.init_node("camera_info_publisher", anonymous=True)
    publisher = rospy.Publisher(args.topic, CameraInfo, queue_size=10)
    rate = rospy.Rate(10)

    # Run publisher
    while not rospy.is_shutdown():
        publisher.publish(camera_info_msg)
        rate.sleep()

[cedrusx]

closing as no update for weeks - feel free to re-open