From 493e1e72309061856af680d26a2cb2d6db358743 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 24 May 2024 16:06:56 -0700 Subject: [PATCH] camera_info pub: Fixed yaml reading error, added "scale" parameter ofr convenience --- rtabmap_util/scripts/yaml_to_camera_info.py | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/rtabmap_util/scripts/yaml_to_camera_info.py b/rtabmap_util/scripts/yaml_to_camera_info.py index 5ead3a91..ab8a8b25 100755 --- a/rtabmap_util/scripts/yaml_to_camera_info.py +++ b/rtabmap_util/scripts/yaml_to_camera_info.py @@ -7,8 +7,8 @@ from sensor_msgs.msg import Image def yaml_to_CameraInfo(yaml_fname): with open(yaml_fname, "r") as file_handle: - calib_data = yaml.load(file_handle) - + calib_data = yaml.load(file_handle, Loader=yaml.FullLoader) + camera_info_msg = CameraInfo() camera_info_msg.width = calib_data["image_width"] camera_info_msg.height = calib_data["image_height"] @@ -33,13 +33,27 @@ if __name__ == "__main__": rospy.init_node("yaml_to_camera_info", anonymous=True) yaml_path = rospy.get_param('~yaml_path', '') + scale = rospy.get_param('~scale', 1.0) if not yaml_path: print('yaml_path parameter should be set to path of the calibration file!') sys.exit(1) frameId = rospy.get_param('~frame_id', '') camera_info_msg = yaml_to_CameraInfo(yaml_path) - + + if scale!=1.0: + camera_info_msg.K[0] = camera_info_msg.K[0]*scale + camera_info_msg.K[2] = camera_info_msg.K[2]*scale + camera_info_msg.K[4] = camera_info_msg.K[4]*scale + camera_info_msg.K[5] = camera_info_msg.K[5]*scale + camera_info_msg.P[0] = camera_info_msg.P[0]*scale + camera_info_msg.P[2] = camera_info_msg.P[2]*scale + camera_info_msg.P[3] = camera_info_msg.P[3]*scale + camera_info_msg.P[5] = camera_info_msg.P[5]*scale + camera_info_msg.P[6] = camera_info_msg.P[6]*scale + camera_info_msg.width = int(camera_info_msg.width*scale) + camera_info_msg.height = int(camera_info_msg.height*scale) + publisher = rospy.Publisher("camera_info", CameraInfo, queue_size=1) rospy.Subscriber("image", Image, callback, queue_size=1) rospy.spin()