@@ -57,6 +57,8 @@ Base.getindex(pc::PointCloud, i) = pc.points[i]
5757Base. setindex! (pc:: PointCloud , pt:: PointT , idx) = (pc. points[idx] = pt)
5858Base. resize! (pc:: PointCloud , s:: Integer ) = resize! (pc. points, s)
5959
60+ Base. length (pc:: PointCloud ) = length (pc. points)
61+
6062# # not very Julian translations from C++ above
6163# # ==============================================================================================
6264
@@ -146,7 +148,7 @@ function createMapping(T,msg_fields::AbstractVector{<:PointField}, field_map::Ms
146148
147149 # Coalesce adjacent fields into single copy where possible
148150 if 1 < length (field_map)
149- # TODO check accending vs descending order
151+ # TODO check accending vs descending order>
150152 sort! (field_map, by = x-> x. serialized_offset)
151153
152154 # something strange with how C++ does and skips the while loop, disabling the coalescing for now
@@ -167,7 +169,7 @@ function createMapping(T,msg_fields::AbstractVector{<:PointField}, field_map::Ms
167169 # else
168170 # i += 1
169171 # j += 1
170-
172+
171173 # _jmap = field_map[j]
172174 # _jend = field_map[end]
173175 # end
@@ -177,6 +179,14 @@ function createMapping(T,msg_fields::AbstractVector{<:PointField}, field_map::Ms
177179
178180 return field_map
179181end
182+ # pc2.fields
183+ # 6-element Vector{Caesar._PCL.PointField}:
184+ # Caesar._PCL.PointField("x", 0x00000000, Caesar._PCL._PCL_FLOAT32, 0x00000001)
185+ # Caesar._PCL.PointField("y", 0x00000004, Caesar._PCL._PCL_FLOAT32, 0x00000001)
186+ # Caesar._PCL.PointField("z", 0x00000008, Caesar._PCL._PCL_FLOAT32, 0x00000001)
187+ # Caesar._PCL.PointField("intensity", 0x00000010, Caesar._PCL._PCL_FLOAT32, 0x00000001)
188+ # Caesar._PCL.PointField("timestamp", 0x00000018, Caesar._PCL._PCL_FLOAT64, 0x00000001)
189+ # Caesar._PCL.PointField("ring", 0x00000020, Caesar._PCL._PCL_UINT16, 0x00000001)
180190
181191
182192# https://pointclouds.org/documentation/conversions_8h_source.html#l00166
@@ -193,13 +203,14 @@ function PointCloud(
193203 cloudsize = msg. width* msg. height
194204 # cloud_data = Vector{UInt8}(undef, cloudsize)
195205
196- # NOTE assume all fields use the same data type
206+ # FIXME cannot assume all fields use the same data type
197207 # off script conversion for XYZ_ data only
198208 datatype = asType {msg.fields[1].datatype} ()
199209 len = trunc (Int, length (msg. data)/ field_map[1 ]. size)
200210 data_ = Vector {datatype} (undef, len)
201211 read! (IOBuffer (msg. data), data_)
202212 mat = reshape (data_, :, cloudsize)
213+ # see createMapping above
203214
204215
205216 # Check if we can copy adjacent points in a single memcpy. We can do so if there
@@ -214,7 +225,7 @@ function PointCloud(
214225 error (" copy of just one field_map not implemented yet"
215226 else
216227 # If not, memcpy each group of contiguous fields separately
217- @assert msg. height == 1 " only decoding msg.height=1 messages, update converter here."
228+ @assert msg. height == 1 " only decoding msg::PCLPointCloud2 .height=1 messages, update converter here."
218229 for row in 1 : msg. height
219230 # TODO check might have an off by one error here
220231 # row_data = row * msg.row_step + 1 # msg.data[(row-1) * msg.row_step]
@@ -223,12 +234,11 @@ function PointCloud(
223234 # the slow way of building the point.data entry
224235 ptdata = zeros (datatype, 4 )
225236 for (i,mapping) in enumerate (field_map)
237+ # @info "test" i mapping maxlog=3
226238 midx = trunc (Int,mapping. serialized_offset/ mapping. size) + 1
227- # TODO , why the weird index reversal?
239+ # Copy individual points as columns from mat -- i.e. memcpy
240+ # TODO copy only works if all elements have the same type -- suggestion, ptdata::ArrayPartition instead
228241 ptdata[i] = mat[midx, col]
229- # @info "DO COPY" mapping
230- # memcpy (cloud_data + mapping.struct_offset, msg_data + mapping.serialized_offset, mapping.size);
231- # @info "copy" mapping.struct_offset mapping.serialized_offset mapping.size
232242 end
233243 pt = T (;data= SVector (ptdata... ))
234244 push! (cloud. points, pt)
@@ -241,22 +251,89 @@ function PointCloud(
241251end
242252
243253
254+ """
255+ $SIGNATURES
256+
257+ Convert a pcl::PointCloud<T> object to a PCLPointCloud2 binary data blob.
258+ In: cloud the input pcl::PointCloud<T>
259+ Out: msg the resultant PCLPointCloud2 binary blob
260+
261+ DevNotes:
262+ - TODO, remove hacks, e.g. fields [x,y,z,intensity,]-only.
263+ """
264+ function PCLPointCloud2 (cloud:: PointCloud{T,P,R} ; datatype = _PCL_FLOAT32) where {T,P,R}
265+ # Ease the user's burden on specifying width/height for unorganized datasets
266+ if (cloud. width == 0 && cloud. height == 0 )
267+ height = 1 ;
268+ width = length (cloud)
269+ else
270+ @assert (length (cloud) == cloud. width * cloud. height) " Input `cloud::PointCloud`'s width*height is not equal to length(cloud)"
271+ height = cloud. height;
272+ width = cloud. width;
273+ end
274+
275+ # // Fill point cloud binary data (padding and all)
276+ # std::size_t data_size = sizeof (PointT) * cloud.size ();
277+ # msg.data.resize (data_size);
278+ # if (data_size)
279+ # {
280+ # memcpy(&msg.data[0], &cloud[0], data_size);
281+ # }
282+
283+ fields = Vector {PointField} (undef, 4 )
284+ # TODO assuming all(fields[_].count==1)
285+ fields[1 ] = PointField (" x" UInt32 (0 ), datatype, UInt32 (1 ))
286+ fields[2 ] = PointField (" y" UInt32 (4 ), datatype, UInt32 (1 ))
287+ fields[3 ] = PointField (" z" UInt32 (8 ), datatype, UInt32 (1 ))
288+ fields[4 ] = PointField (" intensity" UInt32 (12 ), datatype, UInt32 (1 ))
289+ _nflds = length (fields)
290+ # # Fill fields metadata
291+ # msg.fields.clear ();
292+ # for_each_type<typename traits::fieldList<PointT>::type> (detail::FieldAdder<PointT>(msg.fields));
293+
294+ # NOTE get the last possible byte location of all fields, assume no padding after last used byte
295+ _fend = fields[end ]
296+ point_step = _fend. offset + sizeof (asType {_fend.datatype} ()) # = sizeof (PointT);
297+ # msg.row_step = (sizeof (PointT) * msg.width);
298+ # # todo msg.is_bigendian = ?;
299+
300+ # flatten all point fields as binary
301+ data = Vector {UInt8} (undef, point_step* width)
302+ for (i,pt) in enumerate (cloud. points)
303+ offset = (point_step* (i- 1 )+ 1 )
304+ # NOTE assume continuous data block in struct (all of same datatype, FIXME )
305+ data[offset: (offset- 1 + point_step)] .= reinterpret (UInt8, view (pt. data, 1 : _nflds))
306+ end
307+
308+ # @error("noise")
309+ return PCLPointCloud2 (;
310+ header = cloud. header,
311+ height,
312+ width,
313+ fields,
314+ data,
315+ # is_bigendian = ,
316+ point_step,
317+ row_step = sizeof (T)* width,
318+ is_dense = cloud. is_dense,
319+ )
320+ end
321+
322+
244323# # =========================================================================================================
245324# # Coordinate transformations using Manifolds.jl
246325# # =========================================================================================================
247326
248327
249328# 2D, do similar or better for 3D
250329# FIXME , to optimize, this function will likely be slow
251- function apply ( M_:: typeof (SpecialEuclidean (2 )),
252- rPp:: Union{<:ProductRepr,<:Manifolds.ArrayPartition} ,
253- pc:: PointCloud{T} ) where T
330+ # TODO , consolidate with transformPointcloud(::ScatterAlign,..) function
331+ function apply ( M_:: Union{<:typeof(SpecialEuclidean(2)),<:typeof(SpecialEuclidean(3))} ,
332+ rPp:: Manifolds.ArrayPartition ,
333+ pc:: PointCloud{T} ) where T
254334 #
255335
256- rTp = affine_matrix (M_, rPp)
257- pV = MVector (0.0 ,0.0 ,1.0 )
258- _data = MVector (0.0 ,0.0 ,0.0 ,0.0 )
259-
336+ # allocate destination
260337 _pc = PointCloud (;header= pc. header,
261338 points = Vector {T} (),
262339 width= pc. width,
@@ -266,13 +343,18 @@ function apply( M_::typeof(SpecialEuclidean(2)),
266343 sensor_orientation_= pc. sensor_orientation_ )
267344 #
268345
346+ ft3 = _FastTransform3D (M_, rPp, 0f0 )
347+ nc = M_ isa typeof (SpecialEuclidean (3 )) ? 3 : 2
348+ _data = MVector (0f0 ,0f0 ,0f0 ,0f0 )
349+
269350 # rotate the elements from the old point cloud into new static memory locations
270351 # NOTE these types must match the types use for PointCloud and PointXYZ
352+ # TODO not the world's fastest implementation
271353 for pt in pc. points
272- pV[ 1 ] = pt. x
273- pV[ 2 ] = pt. y
274- _data[ 1 : 3 ] . = rTp * pV
275- push! (_pc. points, PointXYZ (;color = pt . color, data = SVector {4,eltype(pt.data)} (_data[ 1 ], _data[ 2 ], pt . data[ 3 : 4 ] . .. )) )
354+ _data[ 1 : nc ] = ft3 ( pt. data[ 1 : nc])
355+ _data[ 4 ] = pt. data[ 4 ]
356+ npt = PointXYZ (;color = pt . color, data = SVector {4,eltype(pt.data)} (_data ... ))
357+ push! (_pc. points, npt )
276358 end
277359
278360 # return the new point cloud
0 commit comments