1
1
use super :: super :: definitions:: sensor_msgs;
2
- use arrow:: {
3
- array:: { BooleanBuilder , StructBuilder } ,
4
- datatypes:: Field ,
5
- } ;
6
- use re_chunk:: {
7
- Chunk , ChunkId ,
8
- external:: arrow:: array:: { FixedSizeListBuilder , Float64Builder , StringBuilder , UInt32Builder } ,
9
- } ;
10
- use re_types:: { ComponentDescriptor , archetypes:: Pinhole , reflection:: ComponentDescriptorExt as _} ;
2
+ use re_chunk:: { Chunk , ChunkId } ;
3
+ use re_types:: archetypes:: Pinhole ;
11
4
12
5
use super :: super :: Ros2MessageParser ;
13
6
use crate :: {
14
7
Error ,
15
8
parsers:: {
16
9
cdr,
17
10
decode:: { MessageParser , ParserContext } ,
18
- util:: fixed_size_list_builder,
19
11
} ,
20
12
} ;
21
13
@@ -24,58 +16,13 @@ use crate::{
24
16
pub struct CameraInfoSchemaPlugin ;
25
17
26
18
pub struct CameraInfoMessageParser {
27
- distortion_models : FixedSizeListBuilder < StringBuilder > ,
28
- k_matrices : FixedSizeListBuilder < Float64Builder > ,
29
- d_coefficients : Vec < Vec < f64 > > ,
30
- r_matrices : FixedSizeListBuilder < Float64Builder > ,
31
- p_matrices : FixedSizeListBuilder < Float64Builder > ,
32
- widths : FixedSizeListBuilder < UInt32Builder > ,
33
- heights : FixedSizeListBuilder < UInt32Builder > ,
34
- binning_x : FixedSizeListBuilder < UInt32Builder > ,
35
- binning_y : FixedSizeListBuilder < UInt32Builder > ,
36
- rois : FixedSizeListBuilder < StructBuilder > ,
37
- frame_ids : FixedSizeListBuilder < StringBuilder > ,
38
19
image_from_cameras : Vec < [ f32 ; 9 ] > ,
39
20
resolutions : Vec < ( f32 , f32 ) > ,
40
21
}
41
22
42
- impl CameraInfoMessageParser {
43
- const ARCHETYPE_NAME : & str = "sensor_msgs.msg.CameraInfo" ;
44
- }
45
-
46
23
impl Ros2MessageParser for CameraInfoMessageParser {
47
24
fn new ( num_rows : usize ) -> Self {
48
25
Self {
49
- distortion_models : fixed_size_list_builder ( 1 , num_rows) ,
50
- k_matrices : fixed_size_list_builder ( 9 , num_rows) ,
51
- d_coefficients : Vec :: with_capacity ( num_rows) ,
52
- r_matrices : fixed_size_list_builder ( 9 , num_rows) ,
53
- p_matrices : fixed_size_list_builder ( 12 , num_rows) ,
54
- widths : fixed_size_list_builder ( 1 , num_rows) ,
55
- heights : fixed_size_list_builder ( 1 , num_rows) ,
56
- binning_x : fixed_size_list_builder ( 1 , num_rows) ,
57
- binning_y : fixed_size_list_builder ( 1 , num_rows) ,
58
- rois : FixedSizeListBuilder :: with_capacity (
59
- StructBuilder :: new (
60
- vec ! [
61
- Field :: new( "x_offset" , arrow:: datatypes:: DataType :: UInt32 , false ) ,
62
- Field :: new( "y_offset" , arrow:: datatypes:: DataType :: UInt32 , false ) ,
63
- Field :: new( "width" , arrow:: datatypes:: DataType :: UInt32 , false ) ,
64
- Field :: new( "height" , arrow:: datatypes:: DataType :: UInt32 , false ) ,
65
- Field :: new( "do_rectify" , arrow:: datatypes:: DataType :: Boolean , false ) ,
66
- ] ,
67
- vec ! [
68
- Box :: new( UInt32Builder :: new( ) ) ,
69
- Box :: new( UInt32Builder :: new( ) ) ,
70
- Box :: new( UInt32Builder :: new( ) ) ,
71
- Box :: new( UInt32Builder :: new( ) ) ,
72
- Box :: new( BooleanBuilder :: new( ) ) ,
73
- ] ,
74
- ) ,
75
- 1 ,
76
- num_rows,
77
- ) ,
78
- frame_ids : fixed_size_list_builder ( 1 , num_rows) ,
79
26
image_from_cameras : Vec :: with_capacity ( num_rows) ,
80
27
resolutions : Vec :: with_capacity ( num_rows) ,
81
28
}
@@ -88,81 +35,15 @@ impl MessageParser for CameraInfoMessageParser {
88
35
header,
89
36
width,
90
37
height,
91
- distortion_model,
92
- d,
93
38
k,
94
- r,
95
- p,
96
- binning_x,
97
- binning_y,
98
- roi,
39
+ ..
99
40
} = cdr:: try_decode_message :: < sensor_msgs:: CameraInfo > ( & msg. data ) ?;
100
41
101
42
// add the sensor timestamp to the context, `log_time` and `publish_time` are added automatically
102
43
ctx. add_timestamp_cell ( crate :: util:: TimestampCell :: guess_from_nanos_ros2 (
103
44
header. stamp . as_nanos ( ) as u64 ,
104
45
) ) ;
105
46
106
- self . distortion_models
107
- . values ( )
108
- . append_value ( & distortion_model) ;
109
- self . distortion_models . append ( true ) ;
110
- self . k_matrices . values ( ) . append_slice ( & k) ;
111
- self . k_matrices . append ( true ) ;
112
-
113
- self . r_matrices . values ( ) . append_slice ( & r) ;
114
- self . r_matrices . append ( true ) ;
115
-
116
- self . p_matrices . values ( ) . append_slice ( & p) ;
117
- self . p_matrices . append ( true ) ;
118
-
119
- self . d_coefficients . push ( d) ;
120
-
121
- self . widths . values ( ) . append_value ( width) ;
122
- self . widths . append ( true ) ;
123
-
124
- self . heights . values ( ) . append_value ( height) ;
125
- self . heights . append ( true ) ;
126
-
127
- self . binning_x . values ( ) . append_value ( binning_x) ;
128
- self . binning_x . append ( true ) ;
129
-
130
- self . binning_y . values ( ) . append_value ( binning_y) ;
131
- self . binning_y . append ( true ) ;
132
-
133
- self . frame_ids . values ( ) . append_value ( & header. frame_id ) ;
134
- self . frame_ids . append ( true ) ;
135
-
136
- let struct_builder = self . rois . values ( ) ;
137
-
138
- struct_builder
139
- . field_builder :: < UInt32Builder > ( 0 )
140
- . expect ( "has to exist" )
141
- . append_value ( roi. x_offset ) ;
142
-
143
- struct_builder
144
- . field_builder :: < UInt32Builder > ( 1 )
145
- . expect ( "has to exist" )
146
- . append_value ( roi. y_offset ) ;
147
-
148
- struct_builder
149
- . field_builder :: < UInt32Builder > ( 2 )
150
- . expect ( "has to exist" )
151
- . append_value ( roi. width ) ;
152
-
153
- struct_builder
154
- . field_builder :: < UInt32Builder > ( 3 )
155
- . expect ( "has to exist" )
156
- . append_value ( roi. height ) ;
157
-
158
- struct_builder
159
- . field_builder :: < BooleanBuilder > ( 4 )
160
- . expect ( "has to exist" )
161
- . append_value ( roi. do_rectify ) ;
162
-
163
- struct_builder. append ( true ) ;
164
- self . rois . append ( true ) ;
165
-
166
47
// ROS2 stores the intrinsic matrix K as a row-major 9-element array:
167
48
// [fx, 0, cx, 0, fy, cy, 0, 0, 1]
168
49
// this corresponds to the matrix:
@@ -188,94 +69,13 @@ impl MessageParser for CameraInfoMessageParser {
188
69
189
70
fn finalize ( self : Box < Self > , ctx : ParserContext ) -> anyhow:: Result < Vec < Chunk > > {
190
71
let Self {
191
- mut distortion_models,
192
- mut k_matrices,
193
- mut r_matrices,
194
- mut p_matrices,
195
- d_coefficients,
196
- mut widths,
197
- mut heights,
198
- mut binning_x,
199
- mut binning_y,
200
- mut frame_ids,
201
- mut rois,
202
72
image_from_cameras,
203
73
resolutions,
204
74
} = * self ;
205
75
206
76
let entity_path = ctx. entity_path ( ) . clone ( ) ;
207
77
let timelines = ctx. build_timelines ( ) ;
208
78
209
- let d_array = {
210
- let mut list_builder = arrow:: array:: ListBuilder :: new ( Float64Builder :: new ( ) ) ;
211
- for d_vec in d_coefficients {
212
- list_builder. values ( ) . append_slice ( & d_vec) ;
213
- list_builder. append ( true ) ;
214
- }
215
- list_builder. finish ( )
216
- } ;
217
-
218
- let chunk = Chunk :: from_auto_row_ids (
219
- ChunkId :: new ( ) ,
220
- entity_path. clone ( ) ,
221
- timelines. clone ( ) ,
222
- [
223
- (
224
- ComponentDescriptor :: partial ( "distortion_model" )
225
- . with_builtin_archetype ( Self :: ARCHETYPE_NAME ) ,
226
- distortion_models. finish ( ) . into ( ) ,
227
- ) ,
228
- (
229
- ComponentDescriptor :: partial ( "k" ) . with_builtin_archetype ( Self :: ARCHETYPE_NAME ) ,
230
- k_matrices. finish ( ) . into ( ) ,
231
- ) ,
232
- (
233
- ComponentDescriptor :: partial ( "width" )
234
- . with_builtin_archetype ( Self :: ARCHETYPE_NAME ) ,
235
- widths. finish ( ) . into ( ) ,
236
- ) ,
237
- (
238
- ComponentDescriptor :: partial ( "height" )
239
- . with_builtin_archetype ( Self :: ARCHETYPE_NAME ) ,
240
- heights. finish ( ) . into ( ) ,
241
- ) ,
242
- (
243
- ComponentDescriptor :: partial ( "d" ) . with_builtin_archetype ( Self :: ARCHETYPE_NAME ) ,
244
- d_array,
245
- ) ,
246
- (
247
- ComponentDescriptor :: partial ( "r" ) . with_builtin_archetype ( Self :: ARCHETYPE_NAME ) ,
248
- r_matrices. finish ( ) . into ( ) ,
249
- ) ,
250
- (
251
- ComponentDescriptor :: partial ( "p" ) . with_builtin_archetype ( Self :: ARCHETYPE_NAME ) ,
252
- p_matrices. finish ( ) . into ( ) ,
253
- ) ,
254
- (
255
- ComponentDescriptor :: partial ( "binning_x" )
256
- . with_builtin_archetype ( Self :: ARCHETYPE_NAME ) ,
257
- binning_x. finish ( ) . into ( ) ,
258
- ) ,
259
- (
260
- ComponentDescriptor :: partial ( "binning_y" )
261
- . with_builtin_archetype ( Self :: ARCHETYPE_NAME ) ,
262
- binning_y. finish ( ) . into ( ) ,
263
- ) ,
264
- (
265
- ComponentDescriptor :: partial ( "roi" )
266
- . with_builtin_archetype ( Self :: ARCHETYPE_NAME ) ,
267
- rois. finish ( ) . into ( ) ,
268
- ) ,
269
- (
270
- ComponentDescriptor :: partial ( "frame_id" )
271
- . with_builtin_archetype ( Self :: ARCHETYPE_NAME ) ,
272
- frame_ids. finish ( ) . into ( ) ,
273
- ) ,
274
- ]
275
- . into_iter ( )
276
- . collect ( ) ,
277
- ) ?;
278
-
279
79
let pinhole_chunk = Chunk :: from_auto_row_ids (
280
80
ChunkId :: new ( ) ,
281
81
entity_path. clone ( ) ,
@@ -288,6 +88,6 @@ impl MessageParser for CameraInfoMessageParser {
288
88
. collect ( ) ,
289
89
) ?;
290
90
291
- Ok ( vec ! [ chunk , pinhole_chunk] )
91
+ Ok ( vec ! [ pinhole_chunk] )
292
92
}
293
93
}
0 commit comments