i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

camera_info pub: Fixed yaml reading error, added "scale" parameter ofr convenience

Browse Source
This commit is contained in:
matlabbe
2024-05-24 16:06:56 -07:00
parent f07e4f0a71
commit 493e1e7230
1 changed files with 17 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_util/scripts/yaml_to_camera_info.py
+17 -3
View File
@@ -7,8 +7,8 @@ from sensor_msgs.msg import Image
def yaml_to_CameraInfo(yaml_fname): def yaml_to_CameraInfo(yaml_fname):
with open(yaml_fname, "r") as file_handle: 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 = CameraInfo()
camera_info_msg.width = calib_data["image_width"] camera_info_msg.width = calib_data["image_width"]
camera_info_msg.height = calib_data["image_height"] camera_info_msg.height = calib_data["image_height"]
@@ -33,13 +33,27 @@ if __name__ == "__main__":
rospy.init_node("yaml_to_camera_info", anonymous=True) rospy.init_node("yaml_to_camera_info", anonymous=True)
yaml_path = rospy.get_param('~yaml_path', '') yaml_path = rospy.get_param('~yaml_path', '')
scale = rospy.get_param('~scale', 1.0)
if not yaml_path: if not yaml_path:
print('yaml_path parameter should be set to path of the calibration file!') print('yaml_path parameter should be set to path of the calibration file!')
sys.exit(1) sys.exit(1)
frameId = rospy.get_param('~frame_id', '') frameId = rospy.get_param('~frame_id', '')
camera_info_msg = yaml_to_CameraInfo(yaml_path) 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) publisher = rospy.Publisher("camera_info", CameraInfo, queue_size=1)
rospy.Subscriber("image", Image, callback, queue_size=1) rospy.Subscriber("image", Image, callback, queue_size=1)
rospy.spin() rospy.spin()