pcd_reader/
lib.rs

1use byteorder::{ByteOrder, LittleEndian};
2use std::fmt;
3use std::fs::File;
4use std::io::prelude::*;
5use std::io::BufReader;
6
7/// PointCloud struct
8/// 
9/// Usage:
10/// 
11/// ```
12/// use pcd_reader::PointCloud;
13/// let filename = "sample/sample_binary_compressed.pcd";
14/// let pcd = PointCloud::from_filename(filename);
15/// let x_data = pcd.get_data_f32("x");
16/// let y_data = pcd.get_data_f32("y");
17/// let z_data = pcd.get_data_f32("z");
18/// let intensity_data = pcd.get_data_u8("intensity");
19/// let ring_data = pcd.get_data_u8("ring");
20/// assert_eq!(pcd.header.data_format, "binary_compressed");
21/// assert_eq!(pcd.header.num_points, 159602);
22/// assert_eq!(pcd.header.field_names, ["x", "y", "z", "intensity", "ring"]);
23/// assert_eq!(pcd.header.size_list, [4, 4, 4, 1, 1]);
24/// assert_eq!(pcd.header.type_list, ["F", "F", "F", "U", "U"]);
25/// assert_eq!(pcd.decompressed_buffer.len(), 2234428);
26/// assert_eq!(x_data.len(), 159602);
27/// assert_eq!(y_data.len(), 159602);
28/// assert_eq!(z_data.len(), 159602);
29/// assert_eq!(intensity_data.len(), 159602);
30/// assert_eq!(ring_data.len(), 159602);
31/// ```
32pub struct PointCloud {
33    pub header: PointCloudHeader,
34    pub decompressed_buffer: Vec<u8>,
35}
36
37impl fmt::Debug for PointCloud {
38    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
39        f.debug_struct("PointCloud")
40            .field("header", &self.header)
41            .field(
42                "decompressed_buffer",
43                &format!("[{} bytes buffer]", self.decompressed_buffer.len()),
44            )
45            .finish()
46    }
47}
48
49#[derive(Debug)]
50pub struct PointCloudHeader {
51    pub data_format: String,
52    pub num_points: usize,
53    pub field_names: Vec<String>,
54    pub size_list: Vec<usize>,
55    pub type_list: Vec<String>,
56}
57
58impl PointCloud {
59    pub fn from_filename(filename: &str) -> PointCloud {
60        let f = File::open(filename).expect("error reading pcd file.");
61        let mut reader = BufReader::new(&f);
62        let data_format: String;
63        let mut num_points: usize = 0;
64        let mut field_names = Vec::<String>::new();
65        let mut size_list = Vec::<usize>::new();
66        let mut type_list = Vec::<String>::new();
67
68        loop {
69            let mut line = String::new();
70            let _ = reader
71                .read_line(&mut line)
72                .expect("failed to read a line from pcd header.");
73            line = line[..line.len() - 1].to_string();
74
75            if line.starts_with("#") {
76            } else if line.starts_with("VERSION") {
77            } else if line.starts_with("FIELDS") {
78                let words: Vec<String> = line.split(" ").map(|s| s.to_string()).collect();
79                let words = &words[1..];
80                field_names = words.to_vec();
81            } else if line.starts_with("SIZE") {
82                let words: Vec<String> = line.split(" ").map(|s| s.to_string()).collect();
83                let words = &words[1..];
84                size_list = words.iter().map(|s|s.parse::<usize>().expect("entry POINTS in header has wrong format: its second term is not integer.")).collect();
85            } else if line.starts_with("TYPE") {
86                let words: Vec<String> = line.split(" ").map(|s| s.to_string()).collect();
87                let words = &words[1..];
88                type_list = words.to_vec();
89            } else if line.starts_with("COUNT") {
90            } else if line.starts_with("WIDTH") {
91            } else if line.starts_with("HEIGHT") {
92            } else if line.starts_with("VIEWPOINT") {
93            } else if line.starts_with("POINTS") {
94                let words: Vec<&str> = line.split(" ").collect();
95                if words.len() != 2 {
96                    panic!("entry POINTS in header has wrong format: it consists of other than 2 words")
97                }
98                num_points = words[1].parse::<usize>().expect(
99                    "entry POINTS in header has wrong format: its second term is not integer.",
100                );
101            } else if line.starts_with("DATA") {
102                let words: Vec<&str> = line.split(" ").collect();
103                if words.len() != 2 {
104                    panic!(
105                        "entry DATA in header has wrong format: it consists of other than 2 words"
106                    )
107                }
108                if "binary_compressed" == words[1] {
109                    data_format = words[1].to_string();
110                } else {
111                    panic!("currently only supporting binary_compressed format.");
112                }
113                break;
114            } else {
115                panic!("unknown header entry");
116            }
117        }
118
119        let mut u32_size_buffer = vec![0u8; 4];
120        let _ = reader.read_exact(&mut u32_size_buffer);
121        let compressed_size = LittleEndian::read_u32(&u32_size_buffer) as usize;
122        let _ = reader.read_exact(&mut u32_size_buffer);
123        let uncompressed_size = LittleEndian::read_u32(&u32_size_buffer) as usize;
124
125        let mut compressed_size_buffer = vec![0u8; compressed_size];
126        let _ = reader.read_exact(&mut compressed_size_buffer);
127        let decompressed_buffer = lzf::decompress(&compressed_size_buffer, uncompressed_size)
128            .expect("error decompressing binary_comprressed pcd data.");
129        PointCloud {
130            decompressed_buffer,
131            header: PointCloudHeader {
132                data_format,
133                num_points,
134                field_names,
135                size_list,
136                type_list,
137            },
138        }
139    }
140
141    fn get_data_offset(&self, fieldname: &str, type_string: &str, item_size: usize) -> usize {
142        let mut data_offset: usize = 0;
143        for (i, fname) in self.header.field_names.iter().enumerate() {
144            if fname == fieldname {
145                if self.header.type_list[i] != type_string || self.header.size_list[i] != item_size
146                {
147                    panic!(
148                        "required fieldname is not a type of {}{}",
149                        type_string, item_size
150                    )
151                }
152                break;
153            }
154            data_offset += self.header.size_list[i];
155        }
156        data_offset
157    }
158
159    fn read_data<T>(
160        &self,
161        fieldname: &str,
162        type_string: &str,
163        item_size: usize,
164        read_buffer_fn: fn(&[u8], &mut [T]),
165        data_buffer: &mut Vec<T>,
166    ) {
167        if !self.header.field_names.contains(&fieldname.to_string()) {
168            panic!("pointcloud does not contain required fieldname");
169        }
170        let data_offset = self.get_data_offset(fieldname, type_string, item_size);
171        read_buffer_fn(
172            &self.decompressed_buffer[data_offset * self.header.num_points
173                ..(data_offset + item_size) * self.header.num_points],
174            data_buffer,
175        );
176    }
177
178    pub fn get_data_f32(&self, fieldname: &str) -> Vec<f32> {
179        let mut data_buffer = vec![0.0; self.header.num_points];
180        self.read_data::<f32>(
181            fieldname,
182            "F",
183            4,
184            LittleEndian::read_f32_into,
185            &mut data_buffer,
186        );
187        data_buffer
188    }
189
190    pub fn get_data_f64(&self, fieldname: &str) -> Vec<f64> {
191        let mut data_buffer = vec![0.0; self.header.num_points];
192        self.read_data::<f64>(
193            fieldname,
194            "F",
195            8,
196            LittleEndian::read_f64_into,
197            &mut data_buffer,
198        );
199        data_buffer
200    }
201
202    pub fn get_data_u8(&self, fieldname: &str) -> Vec<u8> {
203        let mut data_buffer = vec![0; self.header.num_points];
204        fn copy_u8_into(source: &[u8], target: &mut [u8]) {
205            target[..].clone_from_slice(source);
206        }
207        self.read_data::<u8>(fieldname, "U", 1, copy_u8_into, &mut data_buffer);
208        data_buffer
209    }
210
211    pub fn get_data_u16(&self, fieldname: &str) -> Vec<u16> {
212        let mut data_buffer = vec![0; self.header.num_points];
213        self.read_data::<u16>(
214            fieldname,
215            "U",
216            2,
217            LittleEndian::read_u16_into,
218            &mut data_buffer,
219        );
220        data_buffer
221    }
222
223    pub fn get_data_u32(&self, fieldname: &str) -> Vec<u32> {
224        let mut data_buffer = vec![0; self.header.num_points];
225        self.read_data::<u32>(
226            fieldname,
227            "U",
228            4,
229            LittleEndian::read_u32_into,
230            &mut data_buffer,
231        );
232        data_buffer
233    }
234
235    pub fn get_data_u64(&self, fieldname: &str) -> Vec<u64> {
236        let mut data_buffer = vec![0; self.header.num_points];
237        self.read_data::<u64>(
238            fieldname,
239            "U",
240            8,
241            LittleEndian::read_u64_into,
242            &mut data_buffer,
243        );
244        data_buffer
245    }
246
247    // pub fn get_data_i8(&self, fieldname: &str) -> Vec<i8> {
248    //     let mut data_buffer = vec![0; self.header.num_points];
249    //     self.read_data::<i8>(
250    //         fieldname,
251    //         "I",
252    //         1,
253    //         LittleEndian::read_i8_into,
254    //         &mut data_buffer,
255    //     );
256    //     data_buffer
257    // }
258
259    pub fn get_data_i16(&self, fieldname: &str) -> Vec<i16> {
260        let mut data_buffer = vec![0; self.header.num_points];
261        self.read_data::<i16>(
262            fieldname,
263            "I",
264            2,
265            LittleEndian::read_i16_into,
266            &mut data_buffer,
267        );
268        data_buffer
269    }
270
271    pub fn get_data_i32(&self, fieldname: &str) -> Vec<i32> {
272        let mut data_buffer = vec![0; self.header.num_points];
273        self.read_data::<i32>(
274            fieldname,
275            "I",
276            4,
277            LittleEndian::read_i32_into,
278            &mut data_buffer,
279        );
280        data_buffer
281    }
282
283    pub fn get_data_i64(&self, fieldname: &str) -> Vec<i64> {
284        let mut data_buffer = vec![0; self.header.num_points];
285        self.read_data::<i64>(
286            fieldname,
287            "I",
288            8,
289            LittleEndian::read_i64_into,
290            &mut data_buffer,
291        );
292        data_buffer
293    }
294}
295
pcd_reader/lib.rs

pcd_reader/
lib.rs
