cam_geom/
ray_intersection.rs

1#![deny(unsafe_code, missing_docs)]
2
3use nalgebra as na;
4
5use na::{
6    allocator::Allocator, base::storage::Owned, DefaultAllocator, Dim, Dyn, Matrix3, OMatrix,
7    RealField, SMatrix, Vector3, U1, U3,
8};
9
10use itertools::izip;
11
12use crate::{CoordinateSystem, Error, Points, Ray};
13
14/// Iter::Sum is not implemented for R.
15macro_rules! sum_as_f64 {
16    ($iter:expr,$R:ty) => {{
17        $iter
18            .map(|x| na::try_convert::<$R, f64>(x.clone()).unwrap())
19            .sum()
20    }};
21}
22// just like above but without * dereference.
23macro_rules! refsum_as_f64 {
24    ($iter:expr,$R:ty) => {{
25        $iter.map(|x| na::try_convert::<$R, f64>(x).unwrap()).sum()
26    }};
27}
28
29/// convert from specialized type (e.g. f64) into generic type $R
30macro_rules! despecialize {
31    ($a:expr, $R:ty, $rows:expr, $cols:expr) => {{
32        SMatrix::<$R, $rows, $cols>::from_iterator(
33            $a.as_slice().into_iter().map(|x| na::convert(*x)),
34        )
35    }};
36}
37
38/// A single point. (This is a `Points` instance defined to have only one point).
39pub type SinglePoint<Coords, R> = Points<Coords, R, U1, Owned<R, U1, U3>>;
40
41/// Return the 3D point which is the best intersection of rays.
42#[allow(non_snake_case)]
43pub fn best_intersection_of_rays<Coords, R>(
44    rays: &[Ray<Coords, R>],
45) -> Result<SinglePoint<Coords, R>, Error>
46where
47    Coords: CoordinateSystem,
48    R: RealField,
49    DefaultAllocator: Allocator<Dyn, U3>,
50    DefaultAllocator: Allocator<U1, Dyn>,
51    DefaultAllocator: Allocator<U1, U3>,
52{
53    if rays.len() < 2 {
54        return Err(Error::MinimumTwoRaysNeeded);
55    }
56
57    let npts = Dyn(rays.len());
58    let u1 = U1::from_usize(1);
59    let u3 = U3::from_usize(3);
60
61    let mut line_dirs = OMatrix::<R, Dyn, U3>::zeros_generic(npts, u3);
62    let mut line_points = OMatrix::<R, Dyn, U3>::zeros_generic(npts, u3);
63
64    for i in 0..rays.len() {
65        let ray_wc = rays.get(i).unwrap();
66        let d = &ray_wc.direction;
67
68        // Normalize the vector length.
69        let dir = nalgebra::base::Unit::new_normalize(Vector3::new(
70            d.data.0[0][0].clone(),
71            d.data.0[1][0].clone(),
72            d.data.0[2][0].clone(),
73        ));
74
75        line_dirs[(i, 0)] = dir[0].clone();
76        line_dirs[(i, 1)] = dir[1].clone();
77        line_dirs[(i, 2)] = dir[2].clone();
78
79        line_points[(i, 0)] = ray_wc.center.data.0[0][0].clone();
80        line_points[(i, 1)] = ray_wc.center.data.0[1][0].clone();
81        line_points[(i, 2)] = ray_wc.center.data.0[2][0].clone();
82    }
83
84    let mut xxm1 = OMatrix::<R, U1, Dyn>::zeros_generic(u1, npts);
85    let mut yym1 = OMatrix::<R, U1, Dyn>::zeros_generic(u1, npts);
86    let mut zzm1 = OMatrix::<R, U1, Dyn>::zeros_generic(u1, npts);
87    let mut xy = OMatrix::<R, U1, Dyn>::zeros_generic(u1, npts);
88    let mut xz = OMatrix::<R, U1, Dyn>::zeros_generic(u1, npts);
89    let mut yz = OMatrix::<R, U1, Dyn>::zeros_generic(u1, npts);
90
91    // TODO element-wise add, mul with nalgebra matrices
92
93    let nx = line_dirs.column(0);
94    let ny = line_dirs.column(1);
95    let nz = line_dirs.column(2);
96
97    let minusone: R = na::convert(-1.0);
98
99    for (x, xxm1) in nx.iter().zip(xxm1.iter_mut()) {
100        *xxm1 = x.clone().powi(2).add(minusone.clone());
101    }
102
103    for (y, yym1) in ny.iter().zip(yym1.iter_mut()) {
104        *yym1 = y.clone().powi(2).add(minusone.clone());
105    }
106
107    for (z, zzm1) in nz.iter().zip(zzm1.iter_mut()) {
108        *zzm1 = z.clone().powi(2).add(minusone.clone());
109    }
110
111    for (x, y, xy) in izip!(nx.iter(), ny.iter(), xy.iter_mut()) {
112        *xy = x.clone() * y.clone();
113    }
114
115    for (x, z, xz) in izip!(nx.iter(), nz.iter(), xz.iter_mut()) {
116        *xz = x.clone() * z.clone();
117    }
118
119    for (y, z, yz) in izip!(ny.iter(), nz.iter(), yz.iter_mut()) {
120        *yz = y.clone() * z.clone();
121    }
122
123    let SXX: f64 = sum_as_f64!(xxm1.iter(), R);
124    let SYY: f64 = sum_as_f64!(yym1.iter(), R);
125    let SZZ: f64 = sum_as_f64!(zzm1.iter(), R);
126    let SXY: f64 = sum_as_f64!(xy.iter(), R);
127    let SXZ: f64 = sum_as_f64!(xz.iter(), R);
128    let SYZ: f64 = sum_as_f64!(yz.iter(), R);
129
130    let S = Matrix3::new(SXX, SXY, SXZ, SXY, SYY, SYZ, SXZ, SYZ, SZZ);
131
132    let px = line_points.column(0);
133    let py = line_points.column(1);
134    let pz = line_points.column(2);
135
136    let xt1 = px
137        .iter()
138        .zip(xxm1.iter())
139        .map(|(a, b)| a.clone() * b.clone());
140    let xt2 = py.iter().zip(xy.iter()).map(|(a, b)| a.clone() * b.clone());
141    let xt3 = pz.iter().zip(xz.iter()).map(|(a, b)| a.clone() * b.clone());
142    let CX: f64 = refsum_as_f64!(izip!(xt1, xt2, xt3).map(|(t1, t2, t3)| t1 + t2 + t3), R);
143    let CX: R = na::convert(CX);
144
145    let yt1 = px.iter().zip(xy.iter()).map(|(a, b)| a.clone() * b.clone());
146    let yt2 = py
147        .iter()
148        .zip(yym1.iter())
149        .map(|(a, b)| a.clone() * b.clone());
150    let yt3 = pz.iter().zip(yz.iter()).map(|(a, b)| a.clone() * b.clone());
151    let CY: f64 = refsum_as_f64!(izip!(yt1, yt2, yt3).map(|(t1, t2, t3)| t1 + t2 + t3), R);
152    let CY: R = na::convert(CY);
153
154    let zt1 = px.iter().zip(xz.iter()).map(|(a, b)| a.clone() * b.clone());
155    let zt2 = py.iter().zip(yz.iter()).map(|(a, b)| a.clone() * b.clone());
156    let zt3 = pz
157        .iter()
158        .zip(zzm1.iter())
159        .map(|(a, b)| a.clone() * b.clone());
160    let CZ: f64 = refsum_as_f64!(izip!(zt1, zt2, zt3).map(|(t1, t2, t3)| t1 + t2 + t3), R);
161    let CZ: R = na::convert(CZ);
162
163    let C = Vector3::new(CX, CY, CZ);
164
165    let s_f64_pinv = my_pinv(&S)?;
166    let s_pinv = despecialize!(s_f64_pinv, R, 3, 3);
167    let r: Vector3<R> = s_pinv * C;
168
169    let mut result = crate::Points::new(nalgebra::OMatrix::<R, U1, U3>::zeros_generic(u1, u3));
170    for j in 0..3 {
171        result.data[(0, j)] = r[j].clone();
172    }
173    Ok(result)
174}
175
176fn my_pinv<R: RealField>(m: &SMatrix<R, 3, 3>) -> Result<SMatrix<R, 3, 3>, Error> {
177    Ok(
178        na::linalg::SVD::try_new(m.clone(), true, true, na::convert(1e-7), 100)
179            .ok_or(Error::SvdFailed)?
180            .pseudo_inverse(na::convert(1.0e-7))
181            .unwrap(),
182    )
183    // The unwrap() above is safe because, as of nalgebra 0.19, this could only error if the SVD has not been computed.
184    // But this is not possible here, so the unwrap() should never result in a panic.
185}
186
187#[cfg(test)]
188mod tests {
189    use nalgebra::Vector3;
190
191    use crate::{Camera, ExtrinsicParameters, IntrinsicParametersPerspective, PerspectiveParams};
192
193    use super::*;
194
195    #[test]
196    fn roundtrip() {
197        let i1: IntrinsicParametersPerspective<_> = PerspectiveParams {
198            fx: 100.0,
199            fy: 102.0,
200            skew: 0.1,
201            cx: 321.0,
202            cy: 239.9,
203        }
204        .into();
205        let e1 = ExtrinsicParameters::from_view(
206            &Vector3::new(1.2, 3.4, 5.6),                                // camcenter
207            &Vector3::new(2.2, 3.4, 5.6),                                // lookat
208            &nalgebra::Unit::new_normalize(Vector3::new(0.0, 0.0, 1.0)), // up
209        );
210        let cam1 = Camera::new(i1, e1);
211
212        let i2: IntrinsicParametersPerspective<_> = PerspectiveParams {
213            fx: 200.0,
214            fy: 202.0,
215            skew: 0.01,
216            cx: 321.0,
217            cy: 239.9,
218        }
219        .into();
220        let e2 = ExtrinsicParameters::from_view(
221            &Vector3::new(3.4, 1.2, 5.6),                                // camcenter
222            &Vector3::new(2.2, 3.4, 5.6),                                // lookat
223            &nalgebra::Unit::new_normalize(Vector3::new(0.0, 0.0, 1.0)), // up
224        );
225        let cam2 = Camera::new(i2, e2);
226
227        let expected_point = Points::new(SMatrix::<_, 1, 3>::new(2.21, 3.41, 5.61));
228
229        let image1 = cam1.world_to_pixel(&expected_point);
230
231        let image2 = cam2.world_to_pixel(&expected_point);
232
233        let ray1 = cam1.pixel_to_world(&image1).to_single_ray();
234        let ray2 = cam2.pixel_to_world(&image2).to_single_ray();
235        let rays = [ray1, ray2];
236
237        let point3d = best_intersection_of_rays(&rays).unwrap();
238
239        // check reprojection for cam1
240        let image1_actual = cam1.world_to_pixel(&point3d);
241        approx::assert_abs_diff_eq!(
242            image1_actual.data,
243            image1.data,
244            epsilon = nalgebra::convert(1e-10)
245        );
246
247        // check reprojection for cam2
248        let image2_actual = cam2.world_to_pixel(&point3d);
249        approx::assert_abs_diff_eq!(
250            image2_actual.data,
251            image2.data,
252            epsilon = nalgebra::convert(1e-10)
253        );
254
255        // check 3d reconstruction
256        approx::assert_abs_diff_eq!(
257            point3d.data,
258            expected_point.data,
259            epsilon = nalgebra::convert(1e-10)
260        );
261    }
262}
cam_geom/ray_intersection.rs

cam_geom/
ray_intersection.rs
