1234567891011121314151617181920212223242526272829303132333435363738394041424344454647 |
- import yaml
- from sensor_msgs.msg import CameraInfo
-
-
- def yaml_to_CameraInfo(yaml_fname):
- # Load data from file
- with open(yaml_fname, "r") as file_handle:
- calib_data = yaml.load(file_handle)
-
- # try to load the parameters
- camera_info_msg = CameraInfo()
- try:
- camera_info_msg.width = calib_data["image_width"]
- except:
- pass
-
- try:
- camera_info_msg.height = calib_data["image_height"]
- except:
- pass
-
- try:
- camera_info_msg.k = calib_data["camera_matrix"]["data"]
- except:
- pass
-
- try:
- camera_info_msg.d = calib_data["distortion_coefficients"]["data"]
- except:
- pass
-
- try:
- camera_info_msg.r = calib_data["rectification_matrix"]["data"]
- except:
- pass
-
- try:
- camera_info_msg.p = calib_data["projection_matrix"]["data"]
- except:
- pass
-
- try:
- camera_info_msg.distortion_model = calib_data["distortion_model"]
- except:
- pass
-
- return camera_info_msg
|