1use super::IoError;
38use nalgebra::Vector3;
39use std::path::Path;
40
41#[derive(Debug, Clone)]
49pub struct BalCamera {
50 pub rotation: Vector3<f64>,
52 pub translation: Vector3<f64>,
54 pub focal_length: f64,
56 pub k1: f64,
58 pub k2: f64,
60}
61
62#[derive(Debug, Clone)]
64pub struct BalPoint {
65 pub position: Vector3<f64>,
67}
68
69#[derive(Debug, Clone)]
73pub struct BalObservation {
74 pub camera_index: usize,
76 pub point_index: usize,
78 pub x: f64,
80 pub y: f64,
82}
83
84#[derive(Debug, Clone)]
86pub struct BalDataset {
87 pub cameras: Vec<BalCamera>,
89 pub points: Vec<BalPoint>,
91 pub observations: Vec<BalObservation>,
93}
94
95pub struct BalLoader;
97
98pub const DEFAULT_FOCAL_LENGTH: f64 = 500.0;
101
102impl BalCamera {
103 fn normalize_focal_length(focal_length: f64) -> f64 {
108 if focal_length > 0.0 && focal_length.is_finite() {
109 focal_length
110 } else {
111 DEFAULT_FOCAL_LENGTH
112 }
113 }
114}
115
116impl BalLoader {
117 pub fn load(path: impl AsRef<Path>) -> Result<BalDataset, IoError> {
139 let path_ref = path.as_ref();
140 let content = std::fs::read_to_string(path_ref).map_err(|e| {
141 IoError::Io(e).log_with_source(format!("Failed to read BAL file: {:?}", path_ref))
142 })?;
143
144 let mut lines = content
146 .lines()
147 .enumerate()
148 .map(|(idx, line)| (idx + 1, line.trim()))
149 .filter(|(_, line)| !line.is_empty());
150
151 let (num_cameras, num_points, num_observations) = Self::parse_header(&mut lines)?;
153
154 let observations = Self::parse_observations(&mut lines, num_observations)?;
156
157 let cameras = Self::parse_cameras(&mut lines, num_cameras)?;
159
160 let points = Self::parse_points(&mut lines, num_points)?;
162
163 if cameras.len() != num_cameras {
165 return Err(IoError::Parse {
166 line: 0,
167 message: format!(
168 "Camera count mismatch: header says {}, got {}",
169 num_cameras,
170 cameras.len()
171 ),
172 });
173 }
174
175 if points.len() != num_points {
176 return Err(IoError::Parse {
177 line: 0,
178 message: format!(
179 "Point count mismatch: header says {}, got {}",
180 num_points,
181 points.len()
182 ),
183 });
184 }
185
186 if observations.len() != num_observations {
187 return Err(IoError::Parse {
188 line: 0,
189 message: format!(
190 "Observation count mismatch: header says {}, got {}",
191 num_observations,
192 observations.len()
193 ),
194 });
195 }
196
197 Ok(BalDataset {
198 cameras,
199 points,
200 observations,
201 })
202 }
203
204 fn parse_header<'a>(
206 lines: &mut impl Iterator<Item = (usize, &'a str)>,
207 ) -> Result<(usize, usize, usize), IoError> {
208 let (line_num, header_line) = lines.next().ok_or(IoError::Parse {
209 line: 1,
210 message: "Missing header line".to_string(),
211 })?;
212
213 let parts: Vec<&str> = header_line.split_whitespace().collect();
214 if parts.len() != 3 {
215 return Err(IoError::MissingFields { line: line_num });
216 }
217
218 let num_cameras = parts[0]
219 .parse::<usize>()
220 .map_err(|_| IoError::InvalidNumber {
221 line: line_num,
222 value: parts[0].to_string(),
223 })?;
224
225 let num_points = parts[1]
226 .parse::<usize>()
227 .map_err(|_| IoError::InvalidNumber {
228 line: line_num,
229 value: parts[1].to_string(),
230 })?;
231
232 let num_observations = parts[2]
233 .parse::<usize>()
234 .map_err(|_| IoError::InvalidNumber {
235 line: line_num,
236 value: parts[2].to_string(),
237 })?;
238
239 Ok((num_cameras, num_points, num_observations))
240 }
241
242 fn parse_observations<'a>(
244 lines: &mut impl Iterator<Item = (usize, &'a str)>,
245 num_observations: usize,
246 ) -> Result<Vec<BalObservation>, IoError> {
247 let mut observations = Vec::with_capacity(num_observations);
248
249 for _ in 0..num_observations {
250 let (line_num, line) = lines.next().ok_or(IoError::Parse {
251 line: 0,
252 message: "Unexpected end of file in observations section".to_string(),
253 })?;
254
255 let parts: Vec<&str> = line.split_whitespace().collect();
256 if parts.len() != 4 {
257 return Err(IoError::MissingFields { line: line_num });
258 }
259
260 let camera_index = parts[0]
261 .parse::<usize>()
262 .map_err(|_| IoError::InvalidNumber {
263 line: line_num,
264 value: parts[0].to_string(),
265 })?;
266
267 let point_index = parts[1]
268 .parse::<usize>()
269 .map_err(|_| IoError::InvalidNumber {
270 line: line_num,
271 value: parts[1].to_string(),
272 })?;
273
274 let x = parts[2]
275 .parse::<f64>()
276 .map_err(|_| IoError::InvalidNumber {
277 line: line_num,
278 value: parts[2].to_string(),
279 })?;
280
281 let y = parts[3]
282 .parse::<f64>()
283 .map_err(|_| IoError::InvalidNumber {
284 line: line_num,
285 value: parts[3].to_string(),
286 })?;
287
288 observations.push(BalObservation {
289 camera_index,
290 point_index,
291 x,
292 y,
293 });
294 }
295
296 Ok(observations)
297 }
298
299 fn parse_cameras<'a>(
307 lines: &mut impl Iterator<Item = (usize, &'a str)>,
308 num_cameras: usize,
309 ) -> Result<Vec<BalCamera>, IoError> {
310 let mut cameras = Vec::with_capacity(num_cameras);
311
312 for camera_idx in 0..num_cameras {
313 let mut params = Vec::with_capacity(9);
314
315 for param_idx in 0..9 {
317 let (line_num, line) = lines.next().ok_or(IoError::Parse {
318 line: 0,
319 message: format!(
320 "Unexpected end of file in camera {} parameter {}",
321 camera_idx, param_idx
322 ),
323 })?;
324
325 let value = line
326 .trim()
327 .parse::<f64>()
328 .map_err(|_| IoError::InvalidNumber {
329 line: line_num,
330 value: line.to_string(),
331 })?;
332
333 params.push(value);
334 }
335
336 cameras.push(BalCamera {
337 rotation: Vector3::new(params[0], params[1], params[2]),
338 translation: Vector3::new(params[3], params[4], params[5]),
339 focal_length: BalCamera::normalize_focal_length(params[6]),
340 k1: params[7],
341 k2: params[8],
342 });
343 }
344
345 Ok(cameras)
346 }
347
348 fn parse_points<'a>(
352 lines: &mut impl Iterator<Item = (usize, &'a str)>,
353 num_points: usize,
354 ) -> Result<Vec<BalPoint>, IoError> {
355 let mut points = Vec::with_capacity(num_points);
356
357 for point_idx in 0..num_points {
358 let mut coords = Vec::with_capacity(3);
359
360 for coord_idx in 0..3 {
362 let (line_num, line) = lines.next().ok_or(IoError::Parse {
363 line: 0,
364 message: format!(
365 "Unexpected end of file in point {} coordinate {}",
366 point_idx, coord_idx
367 ),
368 })?;
369
370 let value = line
371 .trim()
372 .parse::<f64>()
373 .map_err(|_| IoError::InvalidNumber {
374 line: line_num,
375 value: line.to_string(),
376 })?;
377
378 coords.push(value);
379 }
380
381 points.push(BalPoint {
382 position: Vector3::new(coords[0], coords[1], coords[2]),
383 });
384 }
385
386 Ok(points)
387 }
388}
389
390#[cfg(test)]
391mod tests {
392 use super::*;
393 use std::io::Write;
394 use tempfile::NamedTempFile;
395
396 type TestResult = Result<(), Box<dyn std::error::Error>>;
397
398 fn write_minimal_bal() -> Result<NamedTempFile, Box<dyn std::error::Error>> {
400 let mut f = NamedTempFile::new()?;
401 writeln!(f, "1 1 1")?; writeln!(f, "0 0 -123.456 456.789")?; for v in [0.1f64, 0.2, 0.3, 0.4, 0.5, 0.6, 500.0, -0.1, 0.05] {
405 writeln!(f, "{v}")?;
406 }
407 for v in [1.0f64, 2.0, 3.0] {
409 writeln!(f, "{v}")?;
410 }
411 f.flush()?;
412 Ok(f)
413 }
414
415 fn write_bal_with_focal(focal: f64) -> Result<NamedTempFile, Box<dyn std::error::Error>> {
417 let mut f = NamedTempFile::new()?;
418 writeln!(f, "1 1 1")?;
419 writeln!(f, "0 0 0.0 0.0")?; for v in [0.0f64, 0.0, 0.0, 0.0, 0.0, 0.0, focal, 0.0, 0.0] {
422 writeln!(f, "{v}")?;
423 }
424 for v in [0.0f64, 0.0, 0.0] {
426 writeln!(f, "{v}")?;
427 }
428 f.flush()?;
429 Ok(f)
430 }
431
432 #[test]
433 fn test_load_minimal_dataset() -> TestResult {
434 let f = write_minimal_bal()?;
435 let ds = BalLoader::load(f.path())?;
436 assert_eq!(ds.cameras.len(), 1);
437 assert_eq!(ds.points.len(), 1);
438 assert_eq!(ds.observations.len(), 1);
439 Ok(())
440 }
441
442 #[test]
443 fn test_load_camera_values() -> TestResult {
444 let f = write_minimal_bal()?;
445 let ds = BalLoader::load(f.path())?;
446 let cam = &ds.cameras[0];
447 assert!((cam.rotation.x - 0.1).abs() < 1e-12);
448 assert!((cam.rotation.y - 0.2).abs() < 1e-12);
449 assert!((cam.rotation.z - 0.3).abs() < 1e-12);
450 assert!((cam.translation.x - 0.4).abs() < 1e-12);
451 assert!((cam.translation.y - 0.5).abs() < 1e-12);
452 assert!((cam.translation.z - 0.6).abs() < 1e-12);
453 assert!((cam.focal_length - 500.0).abs() < 1e-12);
454 assert!((cam.k1 - (-0.1)).abs() < 1e-12);
455 assert!((cam.k2 - 0.05).abs() < 1e-12);
456 Ok(())
457 }
458
459 #[test]
460 fn test_load_observation_values() -> TestResult {
461 let f = write_minimal_bal()?;
462 let ds = BalLoader::load(f.path())?;
463 let obs = &ds.observations[0];
464 assert_eq!(obs.camera_index, 0);
465 assert_eq!(obs.point_index, 0);
466 assert!((obs.x - (-123.456)).abs() < 1e-10);
467 assert!((obs.y - 456.789).abs() < 1e-10);
468 Ok(())
469 }
470
471 #[test]
472 fn test_load_point_values() -> TestResult {
473 let f = write_minimal_bal()?;
474 let ds = BalLoader::load(f.path())?;
475 let pt = &ds.points[0];
476 assert!((pt.position.x - 1.0).abs() < 1e-12);
477 assert!((pt.position.y - 2.0).abs() < 1e-12);
478 assert!((pt.position.z - 3.0).abs() < 1e-12);
479 Ok(())
480 }
481
482 #[test]
483 fn test_normalize_focal_length_negative_uses_default() -> TestResult {
484 let f = write_bal_with_focal(-100.0)?;
485 let ds = BalLoader::load(f.path())?;
486 assert!(
487 (ds.cameras[0].focal_length - DEFAULT_FOCAL_LENGTH).abs() < 1e-12,
488 "negative focal length should be replaced with DEFAULT_FOCAL_LENGTH"
489 );
490 Ok(())
491 }
492
493 #[test]
494 fn test_normalize_focal_length_zero_uses_default() -> TestResult {
495 let f = write_bal_with_focal(0.0)?;
496 let ds = BalLoader::load(f.path())?;
497 assert!(
498 (ds.cameras[0].focal_length - DEFAULT_FOCAL_LENGTH).abs() < 1e-12,
499 "zero focal length should be replaced with DEFAULT_FOCAL_LENGTH"
500 );
501 Ok(())
502 }
503
504 #[test]
505 fn test_normalize_focal_length_positive_preserved() -> TestResult {
506 let f = write_bal_with_focal(300.0)?;
507 let ds = BalLoader::load(f.path())?;
508 assert!(
509 (ds.cameras[0].focal_length - 300.0).abs() < 1e-12,
510 "positive focal length should be preserved"
511 );
512 Ok(())
513 }
514
515 #[test]
516 fn test_load_nonexistent_file() {
517 let result = BalLoader::load("/nonexistent/path/file.bal");
518 assert!(result.is_err(), "loading a missing file should return Err");
519 }
520
521 #[test]
522 fn test_load_empty_file() -> TestResult {
523 let f = NamedTempFile::new()?;
524 let result = BalLoader::load(f.path());
525 assert!(result.is_err(), "empty file should fail (missing header)");
526 Ok(())
527 }
528
529 #[test]
530 fn test_load_header_wrong_field_count() -> TestResult {
531 let mut f = NamedTempFile::new()?;
532 writeln!(f, "1 1")?; f.flush()?;
534 let result = BalLoader::load(f.path());
535 assert!(result.is_err(), "header with 2 fields should fail");
536 Ok(())
537 }
538
539 #[test]
540 fn test_load_header_invalid_number() -> TestResult {
541 let mut f = NamedTempFile::new()?;
542 writeln!(f, "1 abc 1")?;
543 f.flush()?;
544 let result = BalLoader::load(f.path());
545 assert!(result.is_err(), "non-numeric header field should fail");
546 Ok(())
547 }
548
549 #[test]
550 fn test_load_truncated_observations() -> TestResult {
551 let mut f = NamedTempFile::new()?;
552 writeln!(f, "1 1 2")?; writeln!(f, "0 0 1.0 1.0")?; f.flush()?;
555 let result = BalLoader::load(f.path());
556 assert!(result.is_err(), "truncated observation block should fail");
557 Ok(())
558 }
559
560 #[test]
561 fn test_load_truncated_cameras() -> TestResult {
562 let mut f = NamedTempFile::new()?;
563 writeln!(f, "1 1 1")?;
564 writeln!(f, "0 0 1.0 1.0")?; for v in [0.0f64, 0.0, 0.0, 0.0, 0.0] {
567 writeln!(f, "{v}")?;
568 }
569 f.flush()?;
570 let result = BalLoader::load(f.path());
571 assert!(result.is_err(), "truncated camera block should fail");
572 Ok(())
573 }
574
575 #[test]
576 fn test_load_multiple_cameras_and_points() -> TestResult {
577 let mut f = NamedTempFile::new()?;
578 writeln!(f, "2 2 3")?; writeln!(f, "0 0 1.0 1.0")?;
580 writeln!(f, "0 1 2.0 2.0")?;
581 writeln!(f, "1 0 3.0 3.0")?;
582 for v in [0.0f64; 9] {
584 writeln!(f, "{v}")?;
585 }
586 for _ in 0..8 {
588 writeln!(f, "0.0")?;
589 }
590 writeln!(f, "200.0")?; for v in [1.0f64, 2.0, 3.0] {
593 writeln!(f, "{v}")?;
594 }
595 for v in [4.0f64, 5.0, 6.0] {
597 writeln!(f, "{v}")?;
598 }
599 f.flush()?;
600 let ds = BalLoader::load(f.path())?;
601 assert_eq!(ds.cameras.len(), 2);
602 assert_eq!(ds.points.len(), 2);
603 assert_eq!(ds.observations.len(), 3);
604 Ok(())
605 }
606
607 #[test]
608 fn test_load_observation_invalid_number() -> TestResult {
609 let mut f = NamedTempFile::new()?;
610 writeln!(f, "1 1 1")?;
611 writeln!(f, "0 0 bad_x 1.0")?; f.flush()?;
613 let result = BalLoader::load(f.path());
614 assert!(
615 result.is_err(),
616 "invalid observation coordinate should fail"
617 );
618 Ok(())
619 }
620
621 #[test]
626 fn test_load_header_invalid_num_cameras() -> TestResult {
627 let mut f = NamedTempFile::new()?;
628 writeln!(f, "bad 1 1")?;
629 f.flush()?;
630 let result = BalLoader::load(f.path());
631 assert!(
632 matches!(result, Err(IoError::InvalidNumber { .. })),
633 "invalid num_cameras should return InvalidNumber"
634 );
635 Ok(())
636 }
637
638 #[test]
639 fn test_load_header_invalid_num_observations() -> TestResult {
640 let mut f = NamedTempFile::new()?;
641 writeln!(f, "1 1 bad")?;
642 f.flush()?;
643 let result = BalLoader::load(f.path());
644 assert!(
645 matches!(result, Err(IoError::InvalidNumber { .. })),
646 "invalid num_observations should return InvalidNumber"
647 );
648 Ok(())
649 }
650
651 #[test]
656 fn test_load_observation_missing_fields() -> TestResult {
657 let mut f = NamedTempFile::new()?;
658 writeln!(f, "1 1 1")?;
659 writeln!(f, "0 1.0")?; f.flush()?;
661 let result = BalLoader::load(f.path());
662 assert!(
663 matches!(result, Err(IoError::MissingFields { .. })),
664 "observation with too few fields should return MissingFields"
665 );
666 Ok(())
667 }
668
669 #[test]
670 fn test_load_observation_invalid_camera_index() -> TestResult {
671 let mut f = NamedTempFile::new()?;
672 writeln!(f, "1 1 1")?;
673 writeln!(f, "bad 0 1.0 2.0")?;
674 f.flush()?;
675 let result = BalLoader::load(f.path());
676 assert!(
677 matches!(result, Err(IoError::InvalidNumber { .. })),
678 "invalid camera_index in observation should return InvalidNumber"
679 );
680 Ok(())
681 }
682
683 #[test]
684 fn test_load_observation_invalid_point_index() -> TestResult {
685 let mut f = NamedTempFile::new()?;
686 writeln!(f, "1 1 1")?;
687 writeln!(f, "0 bad 1.0 2.0")?;
688 f.flush()?;
689 let result = BalLoader::load(f.path());
690 assert!(
691 matches!(result, Err(IoError::InvalidNumber { .. })),
692 "invalid point_index in observation should return InvalidNumber"
693 );
694 Ok(())
695 }
696
697 #[test]
698 fn test_load_observation_invalid_y() -> TestResult {
699 let mut f = NamedTempFile::new()?;
700 writeln!(f, "1 1 1")?;
701 writeln!(f, "0 0 1.0 bad")?;
702 f.flush()?;
703 let result = BalLoader::load(f.path());
704 assert!(
705 matches!(result, Err(IoError::InvalidNumber { .. })),
706 "invalid y in observation should return InvalidNumber"
707 );
708 Ok(())
709 }
710
711 #[test]
716 fn test_load_camera_invalid_parameter() -> TestResult {
717 let mut f = NamedTempFile::new()?;
718 writeln!(f, "1 1 1")?;
719 writeln!(f, "0 0 1.0 1.0")?; writeln!(f, "bad")?; f.flush()?;
722 let result = BalLoader::load(f.path());
723 assert!(
724 matches!(result, Err(IoError::InvalidNumber { .. })),
725 "invalid camera parameter should return InvalidNumber"
726 );
727 Ok(())
728 }
729
730 #[test]
731 fn test_load_truncated_points() -> TestResult {
732 let mut f = NamedTempFile::new()?;
733 writeln!(f, "1 1 1")?;
734 writeln!(f, "0 0 1.0 1.0")?; for v in [0.0f64, 0.0, 0.0, 0.0, 0.0, 0.0, 500.0, 0.0, 0.0] {
737 writeln!(f, "{v}")?;
738 }
739 writeln!(f, "1.0")?;
741 writeln!(f, "2.0")?;
742 f.flush()?;
744 let result = BalLoader::load(f.path());
745 assert!(result.is_err(), "truncated point block should fail");
746 Ok(())
747 }
748
749 #[test]
750 fn test_load_point_invalid_coordinate() -> TestResult {
751 let mut f = NamedTempFile::new()?;
752 writeln!(f, "1 1 1")?;
753 writeln!(f, "0 0 1.0 1.0")?; for v in [0.0f64, 0.0, 0.0, 0.0, 0.0, 0.0, 500.0, 0.0, 0.0] {
756 writeln!(f, "{v}")?;
757 }
758 writeln!(f, "bad")?; f.flush()?;
760 let result = BalLoader::load(f.path());
761 assert!(
762 matches!(result, Err(IoError::InvalidNumber { .. })),
763 "invalid point coordinate should return InvalidNumber"
764 );
765 Ok(())
766 }
767}