-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathrules.bmr
368 lines (323 loc) · 14.9 KB
/
rules.bmr
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
class update_sensor_msgs_CameraInfo_1b5cf7f984c229b6141ceb3a955aa18f(MessageUpdateRule):
old_type = "sensor_msgs/CameraInfo"
old_full_text = """
# This message defines meta information for a camera. It should be in a
# camera namespace and accompanied by up to 5 image topics named:
#
# image_raw, image, image_color, image_rect, and image_rect_color
#
# The meaning of the camera parameters are described in detail at
# http://pr.willowgarage.com/wiki/Camera_Calibration.
##########################
# Image acquisition info #
##########################
# Time of image acquisition, camera coordinate frame ID
Header header # Header timestamp should be acquisition time of image
# Header frame_id should be optical frame of camera
# origin of frame should be optical center of camera
# +x should point to the right in the image
# +y should point down in the image
# +z should point into to plane of the image
# Camera resolution in pixels
uint32 height
uint32 width
# Region of interest (subwindow of full camera resolution), if applicable
RegionOfInterest roi
##########################################
# Internal parameters #
# Used for warping to: #
# 1. An undistorted image (just D and K)#
# 2. A rectified image (D, K, R) #
# Users should not normally need these #
##########################################
# Distortion parameters: k1, k2, t1, t2, k3
# These model radial and tangential distortion of the camera.
float64[5] D # 5x1 vector
# Intrinsic camera matrix for the raw (distorted) images
# Projects 3D points in the camera coordinate frame to 2D pixel
# coordinates using the focal lengths (fx, fy) and principal point
# (cx, cy):
# [fx 0 cx]
# K = [ 0 fy cy]
# [ 0 0 1]
float64[9] K # 3x3 row-major matrix
# Rectification matrix (stereo cameras only)
# A homography which takes an image to the ideal stereo image plane
# so that epipolar lines in both stereo images are parallel.
float64[9] R # 3x3 row-major matrix
######################################
# Projection/camera matrix #
# This is a 3x4 projection matrix #
# for going from 3D to 2D coords #
######################################
# Projection/camera matrix
# By convention, this matrix specifies the intrinsic (camera)
# matrix of the processed (rectified) image.
# Upper 3x3 portion is the normal camera intrinsic matrix for
# the rectified image
# Projects 3D points in the camera coordinate frame to 2D pixel
# coordinates using the focal lengths (fx, fy) and principal point
# (cx, cy):
# [fx 0 cx]
# P[1:3,1:3] = [ 0 fy cy]
# [ 0 0 1]
# For the right camera of a stereo pair, P[4,1] is the position
# of the right camera center in the left camera's frame, times
# the focal length fx.
#
# Given a 3D point q=[XYZ]^T in the left camera frame, the projection
# of the point onto the image is given by [uvw]^T = Pq, with x=u/w and y=v/w.
# This holds for both left and right images of a stereo pair.
# For monocular cameras and the left image of a stereo pair, P[4,1] is always 0.
float64[12] P # 3x4 row-major matrix
================================================================================
MSG: std_msgs/Header
# Standard metadata for higher-level stamped data types.
# This is generally used to communicate timestamped data
# in a particular coordinate frame.
#
# sequence ID: consecutively increasing ID
uint32 seq
#Two-integer timestamp that is expressed as:
# * stamp.secs: seconds (stamp_secs) since epoch
# * stamp.nsecs: nanoseconds since stamp_secs
# time-handling sugar is provided by the client library
time stamp
#Frame this data is associated with
# 0: no frame
# 1: global frame
string frame_id
================================================================================
MSG: sensor_msgs/RegionOfInterest
# This message is used to specify a region of interest within an image
#
# When used to specify the ROI setting of the camera when the image was taken,
# the height and width fields should either match the height and width
# fields for the associated image or be zeroes to indicate that the full image
# was captured
uint32 x_offset #Leftmost pixel of the ROI (0 if the left edge of the image is included in the ROI)
uint32 y_offset #Topmost pixel of the ROI (0 if the top edge of the image is included in the ROI)
uint32 height #Height of ROI
uint32 width #Width of ROI
"""
new_type = "sensor_msgs/CameraInfo"
new_full_text = """
# This message defines meta information for a camera. It should be in a
# camera namespace on topic "camera_info" and accompanied by up to five
# image topics named:
#
# image_raw - raw data from the camera driver, possibly Bayer encoded
# image - monochrome, distorted
# image_color - color, distorted
# image_rect - monochrome, rectified
# image_rect_color - color, rectified
#
# The image_pipeline contains packages (image_proc, stereo_image_proc)
# for producing the four processed image topics from image_raw and
# camera_info. The meaning of the camera parameters are described in
# detail at http://www.ros.org/wiki/image_pipeline/CameraInfo.
#
# The image_geometry package provides a user-friendly interface to
# common operations using this meta information. If you want to, e.g.,
# project a 3d point into image coordinates, we strongly recommend
# using image_geometry.
#
# If the camera is uncalibrated, the matrices D, K, R, P should be left
# zeroed out. In particular, clients may assume that K[0] == 0.0
# indicates an uncalibrated camera.
#######################################################################
# Image acquisition info #
#######################################################################
# Time of image acquisition, camera coordinate frame ID
Header header # Header timestamp should be acquisition time of image
# Header frame_id should be optical frame of camera
# origin of frame should be optical center of camera
# +x should point to the right in the image
# +y should point down in the image
# +z should point into the plane of the image
#######################################################################
# Calibration Parameters #
#######################################################################
# These are fixed during camera calibration. Their values will be the #
# same in all messages until the camera is recalibrated. Note that #
# self-calibrating systems may "recalibrate" frequently. #
# #
# The internal parameters can be used to warp a raw (distorted) image #
# to: #
# 1. An undistorted image (requires D and K) #
# 2. A rectified image (requires D, K, R) #
# The projection matrix P projects 3D points into the rectified image.#
#######################################################################
# The image dimensions with which the camera was calibrated. Normally
# this will be the full camera resolution in pixels.
uint32 height
uint32 width
# The distortion model used. Supported models are listed in
# sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a
# simple model of radial and tangential distortion - is sufficient.
string distortion_model
# The distortion parameters, size depending on the distortion model.
# For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3).
float64[] D
# Intrinsic camera matrix for the raw (distorted) images.
# [fx 0 cx]
# K = [ 0 fy cy]
# [ 0 0 1]
# Projects 3D points in the camera coordinate frame to 2D pixel
# coordinates using the focal lengths (fx, fy) and principal point
# (cx, cy).
float64[9] K # 3x3 row-major matrix
# Rectification matrix (stereo cameras only)
# A rotation matrix aligning the camera coordinate system to the ideal
# stereo image plane so that epipolar lines in both stereo images are
# parallel.
float64[9] R # 3x3 row-major matrix
# Projection/camera matrix
# [fx' 0 cx' Tx]
# P = [ 0 fy' cy' Ty]
# [ 0 0 1 0]
# By convention, this matrix specifies the intrinsic (camera) matrix
# of the processed (rectified) image. That is, the left 3x3 portion
# is the normal camera intrinsic matrix for the rectified image.
# It projects 3D points in the camera coordinate frame to 2D pixel
# coordinates using the focal lengths (fx', fy') and principal point
# (cx', cy') - these may differ from the values in K.
# For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will
# also have R = the identity and P[1:3,1:3] = K.
# For a stereo pair, the fourth column [Tx Ty 0]' is related to the
# position of the optical center of the second camera in the first
# camera's frame. We assume Tz = 0 so both cameras are in the same
# stereo image plane. The first camera always has Tx = Ty = 0. For
# the right (second) camera of a horizontal stereo pair, Ty = 0 and
# Tx = -fx' * B, where B is the baseline between the cameras.
# Given a 3D point [X Y Z]', the projection (x, y) of the point onto
# the rectified image is given by:
# [u v w]' = P * [X Y Z 1]'
# x = u / w
# y = v / w
# This holds for both images of a stereo pair.
float64[12] P # 3x4 row-major matrix
#######################################################################
# Operational Parameters #
#######################################################################
# These define the image region actually captured by the camera #
# driver. Although they affect the geometry of the output image, they #
# may be changed freely without recalibrating the camera. #
#######################################################################
# Binning refers here to any camera setting which combines rectangular
# neighborhoods of pixels into larger "super-pixels." It reduces the
# resolution of the output image to
# (width / binning_x) x (height / binning_y).
# The default values binning_x = binning_y = 0 is considered the same
# as binning_x = binning_y = 1 (no subsampling).
uint32 binning_x
uint32 binning_y
# Region of interest (subwindow of full camera resolution), given in
# full resolution (unbinned) image coordinates. A particular ROI
# always denotes the same window of pixels on the camera sensor,
# regardless of binning settings.
# The default setting of roi (all values 0) is considered the same as
# full resolution (roi.width = width, roi.height = height).
RegionOfInterest roi
================================================================================
MSG: std_msgs/Header
# Standard metadata for higher-level stamped data types.
# This is generally used to communicate timestamped data
# in a particular coordinate frame.
#
# sequence ID: consecutively increasing ID
uint32 seq
#Two-integer timestamp that is expressed as:
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
# time-handling sugar is provided by the client library
time stamp
#Frame this data is associated with
# 0: no frame
# 1: global frame
string frame_id
================================================================================
MSG: sensor_msgs/RegionOfInterest
# This message is used to specify a region of interest within an image.
#
# When used to specify the ROI setting of the camera when the image was
# taken, the height and width fields should either match the height and
# width fields for the associated image; or height = width = 0
# indicates that the full resolution image was captured.
uint32 x_offset # Leftmost pixel of the ROI
# (0 if the ROI includes the left edge of the image)
uint32 y_offset # Topmost pixel of the ROI
# (0 if the ROI includes the top edge of the image)
uint32 height # Height of ROI
uint32 width # Width of ROI
# True if a distinct rectified ROI should be calculated from the "raw"
# ROI in this message. Typically this should be False if the full image
# is captured (ROI not used), and True if a subwindow is captured (ROI
# used).
bool do_rectify
"""
order = 0
migrated_types = [
("Header","Header"),
("RegionOfInterest","RegionOfInterest"),]
valid = False
def update(self, old_msg, new_msg):
self.migrate(old_msg.header, new_msg.header)
new_msg.height = old_msg.height
new_msg.width = old_msg.width
#No matching field name in old message
new_msg.distortion_model = ''
#Converted from fixed array of length 5 to variable length
new_msg.D = []
new_msg.K = old_msg.K
new_msg.R = old_msg.R
new_msg.P = old_msg.P
#No matching field name in old message
new_msg.binning_x = 0
#No matching field name in old message
new_msg.binning_y = 0
self.migrate(old_msg.roi, new_msg.roi)
class update_sensor_msgs_RegionOfInterest_878e60591f2679769082130f7aafa371(MessageUpdateRule):
old_type = "sensor_msgs/RegionOfInterest"
old_full_text = """
# This message is used to specify a region of interest within an image
#
# When used to specify the ROI setting of the camera when the image was taken,
# the height and width fields should either match the height and width
# fields for the associated image or be zeroes to indicate that the full image
# was captured
uint32 x_offset #Leftmost pixel of the ROI (0 if the left edge of the image is included in the ROI)
uint32 y_offset #Topmost pixel of the ROI (0 if the top edge of the image is included in the ROI)
uint32 height #Height of ROI
uint32 width #Width of ROI
"""
new_type = "sensor_msgs/RegionOfInterest"
new_full_text = """
# This message is used to specify a region of interest within an image.
#
# When used to specify the ROI setting of the camera when the image was
# taken, the height and width fields should either match the height and
# width fields for the associated image; or height = width = 0
# indicates that the full resolution image was captured.
uint32 x_offset # Leftmost pixel of the ROI
# (0 if the ROI includes the left edge of the image)
uint32 y_offset # Topmost pixel of the ROI
# (0 if the ROI includes the top edge of the image)
uint32 height # Height of ROI
uint32 width # Width of ROI
# True if a distinct rectified ROI should be calculated from the "raw"
# ROI in this message. Typically this should be False if the full image
# is captured (ROI not used), and True if a subwindow is captured (ROI
# used).
bool do_rectify
"""
order = 0
migrated_types = []
valid = False
def update(self, old_msg, new_msg):
new_msg.x_offset = old_msg.x_offset
new_msg.y_offset = old_msg.y_offset
new_msg.height = old_msg.height
new_msg.width = old_msg.width
#No matching field name in old message
new_msg.do_rectify = 0