diff --git a/launch/calibration/euroc_left.yaml b/launch/calibration/euroc_left.yaml new file mode 100644 index 00000000..92b7974e --- /dev/null +++ b/launch/calibration/euroc_left.yaml @@ -0,0 +1,30 @@ +#%YAML:1.0 +--- +camera_name: MH_01_easy_left +image_width: 752 +image_height: 480 +camera_matrix: + rows: 3 + cols: 3 + data: [ 4.5865400000000000e+02, 0., 3.6721499999999997e+02, 0., + 4.5729599999999999e+02, 2.4837500000000000e+02, 0., 0., 1. ] +distortion_coefficients: + rows: 1 + cols: 4 + data: [ -2.8340810999999999e-01, 7.3959070000000002e-02, + 1.9358999999999999e-04, 1.7618711400000001e-05 ] +distortion_model: plumb_bob +rectification_matrix: + rows: 3 + cols: 3 + data: [ 9.9996634750298619e-01, -1.4227432298321767e-03, + 8.0795831104762770e-03, 1.3657459036274155e-03, + 9.9997417608074302e-01, 7.0556296505566432e-03, + -8.0894128132919258e-03, -7.0443575534647465e-03, + 9.9994246755850658e-01 ] +projection_matrix: + rows: 3 + cols: 4 + data: [ 4.3520469597145990e+02, 0., 3.6745172119140625e+02, 0., 0., + 4.3520469597145990e+02, 2.5220085144042969e+02, 0., 0., 0., 1., + 0. ] diff --git a/launch/calibration/euroc_right.yaml b/launch/calibration/euroc_right.yaml new file mode 100644 index 00000000..f5ebe0d3 --- /dev/null +++ b/launch/calibration/euroc_right.yaml @@ -0,0 +1,30 @@ +#%YAML:1.0 +--- +camera_name: MH_01_easy_right +image_width: 752 +image_height: 480 +camera_matrix: + rows: 3 + cols: 3 + data: [ 4.5758699999999999e+02, 0., 3.7999900000000002e+02, 0., + 4.5613400000000001e+02, 2.5523800000000000e+02, 0., 0., 1. ] +distortion_coefficients: + rows: 1 + cols: 4 + data: [ -2.8368365000000001e-01, 7.4512839999999997e-02, + -1.0473000000000000e-04, -3.5559070000000001e-05 ] +distortion_model: plumb_bob +rectification_matrix: + rows: 3 + cols: 3 + data: [ 9.9996335257946345e-01, -3.6258159819210472e-03, + 7.7554468926514068e-03, 3.6804026836554193e-03, + 9.9996847525904631e-01, -7.0358456623254633e-03, + -7.7296917225483383e-03, 7.0641309842873097e-03, + 9.9994517345668066e-01 ] +projection_matrix: + rows: 3 + cols: 4 + data: [ 4.3520469597145990e+02, 0., 3.6745172119140625e+02, + -4.7906395664848930e+01, 0., 4.3520469597145990e+02, + 2.5220085144042969e+02, 0., 0., 0., 1., 0. ] diff --git a/launch/tests/euroc_datasets.launch b/launch/tests/euroc_datasets.launch new file mode 100644 index 00000000..e90eb277 --- /dev/null +++ b/launch/tests/euroc_datasets.launch @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/scripts/point_to_tf.py b/scripts/point_to_tf.py new file mode 100755 index 00000000..ea3498f5 --- /dev/null +++ b/scripts/point_to_tf.py @@ -0,0 +1,28 @@ +#!/usr/bin/env python +import rospy +import tf +from geometry_msgs.msg import PointStamped + +def callback(point): + global br + global frame_id + local_frame_id = point.header.frame_id + if not local_frame_id: + local_frame_id = frame_id + br.sendTransform( + (point.point.x, point.point.y, point.point.z), + tf.transformations.quaternion_from_euler(0,0,0), + point.header.stamp, + local_frame_id, + fixed_frame_id) + +if __name__ == "__main__": + + rospy.init_node("yaml_to_camera_info", anonymous=True) + + frame_id = rospy.get_param('~frame_id', 'point') + fixed_frame_id = rospy.get_param('~fixed_frame_id', 'world') + + br = tf.TransformBroadcaster() + rospy.Subscriber("point", PointStamped, callback, queue_size=1) + rospy.spin() diff --git a/scripts/yaml_to_camera_info.py b/scripts/yaml_to_camera_info.py new file mode 100755 index 00000000..ee955ffb --- /dev/null +++ b/scripts/yaml_to_camera_info.py @@ -0,0 +1,40 @@ +#!/usr/bin/env python +import rospy +import yaml +from sensor_msgs.msg import CameraInfo +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) + + 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 + +def callback(image): + global publisher + global camera_info_msg + camera_info_msg.header = image.header + publisher.publish(camera_info_msg) + +if __name__ == "__main__": + + rospy.init_node("yaml_to_camera_info", anonymous=True) + + yaml_path = rospy.get_param('~yaml_path', '') + if not yaml_path: + print 'yaml_path parameter should be set to path of the calibration file!' + sys.exit(1) + + camera_info_msg = yaml_to_CameraInfo(yaml_path) + + publisher = rospy.Publisher("camera_info", CameraInfo, queue_size=1) + rospy.Subscriber("image", Image, callback, queue_size=1) + rospy.spin()