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

merged master->ros2

Browse Source
This commit is contained in:
matlabbe
2024-05-27 12:37:39 -07:00
parent 655ab10fc8 86f91f3cec
commit 0dc233c840
14 changed files with 89 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_util/scripts/yaml_to_camera_info.py
+16 -1
View File
@@ -8,7 +8,7 @@ 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)
msg = CameraInfo()
msg.width = calib_data["image_width"]
@@ -26,7 +26,9 @@ class YamlToCameraInfo(Node):
super().__init__('yaml_to_camera_info')
self.declare_parameter('yaml_path', '')
self.declare_parameter('scale', 1.0)
yaml_path = self.get_parameter('yaml_path').get_parameter_value().string_value
scale = self.get_parameter('scale').get_parameter_value().double_value
if not yaml_path:
print('yaml_path parameter should be set to path of the calibration file!')
@@ -36,6 +38,19 @@ class YamlToCameraInfo(Node):
self.frame_id = self.get_parameter('frame_id').get_parameter_value().string_value
self.camera_info_msg = yaml_to_CameraInfo(yaml_path)
if scale!=1.0:
self.camera_info_msg.k[0] = self.camera_info_msg.k[0]*scale
self.camera_info_msg.k[2] = self.camera_info_msg.k[2]*scale
self.camera_info_msg.k[4] = self.camera_info_msg.k[4]*scale
self.camera_info_msg.k[5] = self.camera_info_msg.k[5]*scale
self.camera_info_msg.p[0] = self.camera_info_msg.p[0]*scale
self.camera_info_msg.p[2] = self.camera_info_msg.p[2]*scale
self.camera_info_msg.p[3] = self.camera_info_msg.p[3]*scale
self.camera_info_msg.p[5] = self.camera_info_msg.p[5]*scale
self.camera_info_msg.p[6] = self.camera_info_msg.p[6]*scale
self.camera_info_msg.width = int(self.camera_info_msg.width*scale)
self.camera_info_msg.height = int(self.camera_info_msg.height*scale)
self.publisher_ = self.create_publisher(CameraInfo, 'camera_info', 1)
self.subscription = self.create_subscription(