webarkitlib_rs/
icp.rs

1/*
2 *  icp.rs
3 *  WebARKitLib-rs
4 *
5 *  This file is part of WebARKitLib-rs - WebARKit.
6 *
7 *  WebARKitLib-rs is free software: you can redistribute it and/or modify
8 *  it under the terms of the GNU Lesser General Public License as published by
9 *  the Free Software Foundation, either version 3 of the License, or
10 *  (at your option) any later version.
11 *
12 *  WebARKitLib-rs is distributed in the hope that it will be useful,
13 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
14 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15 *  GNU Lesser General Public License for more details.
16 *
17 *  You should have received a copy of the GNU Lesser General Public License
18 *  along with WebARKitLib-rs.  If not, see <http://www.gnu.org/licenses/>.
19 *
20 *  As a special exception, the copyright holders of this library give you
21 *  permission to link this library with independent modules to produce an
22 *  executable, regardless of the license terms of these independent modules, and to
23 *  copy and distribute the resulting executable under terms of your choice,
24 *  provided that you also meet, for each linked independent module, the terms and
25 *  conditions of the license of that module. An independent module is a module
26 *  which is neither derived from nor based on this library. If you modify this
27 *  library, you may extend this exception to your version of the library, but you
28 *  are not obligated to do so. If you do not wish to do so, delete this exception
29 *  statement from your version.
30 *
31 *  Copyright 2026 WebARKit.
32 *
33 *  Author(s): Walter Perdan @kalwalt https://github.com/kalwalt
34 *
35 */
36
37//! Iterative Closest Point (ICP) Data Structures and Methods
38//! Translated from ARToolKit C headers (icp.h, icpCore.h)
39
40use crate::types::ARdouble;
41use log::{debug, error};
42
43/// 2D Coordinate for ICP
44#[derive(Debug, Default, Clone, Copy, PartialEq)]
45#[repr(C)]
46pub struct ICP2DCoordT {
47    pub x: ARdouble,
48    pub y: ARdouble,
49}
50
51/// 3D Coordinate for ICP
52#[derive(Debug, Default, Clone, Copy, PartialEq)]
53#[repr(C)]
54pub struct ICP3DCoordT {
55    pub x: ARdouble,
56    pub y: ARdouble,
57    pub z: ARdouble,
58}
59
60/// 2D Line for ICP
61#[derive(Debug, Default, Clone, Copy, PartialEq)]
62#[repr(C)]
63pub struct ICP2DLineT {
64    pub a: ARdouble,
65    pub b: ARdouble,
66    pub c: ARdouble,
67}
68
69/// 2D Line Segment for ICP
70#[derive(Debug, Default, Clone, Copy, PartialEq)]
71#[repr(C)]
72pub struct ICP2DLineSegT {
73    pub p1: ICP2DCoordT,
74    pub p2: ICP2DCoordT,
75}
76
77/// 3D Line Segment for ICP
78#[derive(Debug, Default, Clone, Copy, PartialEq)]
79#[repr(C)]
80pub struct ICP3DLineSegT {
81    pub p1: ICP3DCoordT,
82    pub p2: ICP3DCoordT,
83}
84
85/// Point Data for ICP
86#[derive(Debug, Clone, PartialEq)]
87pub struct ICPDataT {
88    pub screen_coord: Vec<ICP2DCoordT>,
89    pub world_coord: Vec<ICP3DCoordT>,
90}
91
92impl Default for ICPDataT {
93    fn default() -> Self {
94        Self {
95            screen_coord: Vec::new(),
96            world_coord: Vec::new(),
97        }
98    }
99}
100
101/// Stereo Point Data for ICP
102#[derive(Debug, Clone, PartialEq)]
103pub struct ICPStereoDataT {
104    pub screen_coord_l: Vec<ICP2DCoordT>,
105    pub world_coord_l: Vec<ICP3DCoordT>,
106    pub screen_coord_r: Vec<ICP2DCoordT>,
107    pub world_coord_r: Vec<ICP3DCoordT>,
108}
109
110impl Default for ICPStereoDataT {
111    fn default() -> Self {
112        Self {
113            screen_coord_l: Vec::new(),
114            world_coord_l: Vec::new(),
115            screen_coord_r: Vec::new(),
116            world_coord_r: Vec::new(),
117        }
118    }
119}
120
121/// ICP Handle
122#[derive(Debug, Clone, PartialEq)]
123#[repr(C)]
124pub struct ICPHandleT {
125    pub mat_xc2u: [[ARdouble; 4]; 3],
126    pub max_loop: i32,
127    pub break_loop_error_thresh: ARdouble,
128    pub break_loop_error_ratio_thresh: ARdouble,
129    pub break_loop_error_thresh2: ARdouble,
130    pub inlier_prob: ARdouble,
131}
132
133impl Default for ICPHandleT {
134    fn default() -> Self {
135        Self {
136            mat_xc2u: [[0.0; 4]; 3],
137            max_loop: 10,
138            break_loop_error_thresh: 0.1,
139            break_loop_error_ratio_thresh: 0.99,
140            break_loop_error_thresh2: 4.0,
141            inlier_prob: 0.50,
142        }
143    }
144}
145
146pub fn icp_set_inlier_probability(handle: &mut ICPHandleT, prob: f64) {
147    handle.inlier_prob = prob;
148}
149
150/// ICP Stereo Handle
151#[derive(Debug, Clone, PartialEq)]
152#[repr(C)]
153pub struct ICPStereoHandleT {
154    pub mat_xcl2ul: [[ARdouble; 4]; 3],
155    pub mat_xcr2ur: [[ARdouble; 4]; 3],
156    pub mat_c2l: [[ARdouble; 4]; 3],
157    pub mat_c2r: [[ARdouble; 4]; 3],
158    pub max_loop: i32,
159    pub break_loop_error_thresh: ARdouble,
160    pub break_loop_error_ratio_thresh: ARdouble,
161    pub break_loop_error_thresh2: ARdouble,
162    pub inlier_prob: ARdouble,
163}
164
165impl Default for ICPStereoHandleT {
166    fn default() -> Self {
167        Self {
168            mat_xcl2ul: [[0.0; 4]; 3],
169            mat_xcr2ur: [[0.0; 4]; 3],
170            mat_c2l: [[0.0; 4]; 3],
171            mat_c2r: [[0.0; 4]; 3],
172            max_loop: 10,
173            break_loop_error_thresh: 0.1,
174            break_loop_error_ratio_thresh: 0.99,
175            break_loop_error_thresh2: 4.0,
176            inlier_prob: 0.50,
177        }
178    }
179}
180
181use crate::math::ARMat;
182
183pub fn icp_mat_mul(m1: &[[ARdouble; 4]; 3], m2: &[[ARdouble; 4]; 3], dest: &mut [[ARdouble; 4]; 3]) {
184    for r in 0..3 {
185        for c in 0..4 {
186            dest[r][c] = m1[r][0] * m2[0][c]
187                       + m1[r][1] * m2[1][c]
188                       + m1[r][2] * m2[2][c];
189            if c == 3 {
190                dest[r][c] += m1[r][3];
191            }
192        }
193    }
194}
195
196pub fn icp_point(
197    handle: &ICPHandleT,
198    data: &ICPDataT,
199    init_mat_xw2xc: &[[ARdouble; 4]; 3],
200    mat_xw2xc: &mut [[ARdouble; 4]; 3],
201) -> Result<ARdouble, &'static str> {
202    if data.screen_coord.len() < 3 || data.world_coord.len() < 3 {
203        debug!("ICP Point: Not enough points");
204        return Err("Not enough points for ICP");
205    }
206
207    let num_points = data.screen_coord.len();
208    let mut j_u_s_table = vec![[0.0; 6]; num_points * 2];
209    let mut du = vec![0.0; num_points * 2];
210    let mut mat_xw2u = [[0.0; 4]; 3];
211    let mut ds = [0.0; 6];
212    let mut err0 = 0.0;
213    #[allow(unused_assignments)]
214    let mut err1 = 0.0;
215
216    for j in 0..3 {
217        for i in 0..4 {
218            mat_xw2xc[j][i] = init_mat_xw2xc[j][i];
219        }
220    }
221
222    let mut loop_idx = 0;
223    loop {
224        icp_mat_mul(&handle.mat_xc2u, mat_xw2xc, &mut mat_xw2u);
225
226        err1 = 0.0;
227        let mut u = ICP2DCoordT::default();
228        for j in 0..num_points {
229            icp_get_u_from_x_by_mat_x2u(&mut u, &mat_xw2u, &data.world_coord[j])?;
230            let dx = data.screen_coord[j].x - u.x;
231            let dy = data.screen_coord[j].y - u.y;
232            err1 += dx * dx + dy * dy;
233            du[j * 2 + 0] = dx;
234            du[j * 2 + 1] = dy;
235        }
236        err1 /= num_points as ARdouble;
237        
238        // println!("Loop {}, err1: {}, err0: {}", loop_idx, err1, err0);
239
240        if err1 < handle.break_loop_error_thresh { 
241            // println!("ICP Point: Break loop threshold reached");
242            break; 
243        }
244        if loop_idx > 0 && err1 < handle.break_loop_error_thresh2 && (err1 / err0) > handle.break_loop_error_ratio_thresh { 
245            // println!("ICP Point: Break loop ratio reached");
246            break; 
247        }
248        if loop_idx == handle.max_loop { 
249            // println!("ICP Point: Max loop reached");
250            break; 
251        }
252        err0 = err1;
253
254        for j in 0..num_points {
255            let mut j_u_s_local = [[0.0; 6]; 2];
256            icp_get_j_u_s(&mut j_u_s_local, &handle.mat_xc2u, mat_xw2xc, &data.world_coord[j])?;
257            for r in 0..2 {
258                for c in 0..6 {
259                    j_u_s_table[j * 2 + r][c] = j_u_s_local[r][c];
260                }
261            }
262        }
263
264        if let Err(e) = icp_get_delta_s(&mut ds, &du, &j_u_s_table, num_points * 2) {
265            error!("ICP Point: icp_get_delta_s failed: {}", e);
266            return Err(e);
267        }
268        icp_update_mat(mat_xw2xc, &ds);
269
270        loop_idx += 1;
271    }
272
273    Ok(err1)
274}
275
276pub fn icp_point_robust(
277    handle: &ICPHandleT,
278    data: &ICPDataT,
279    init_mat_xw2xc: &[[ARdouble; 4]; 3],
280    mat_xw2xc: &mut [[ARdouble; 4]; 3],
281) -> Result<ARdouble, &'static str> {
282    // Highly simplified robust estimation
283    // In a real port this would use M-estimators (Huber/Tukey)
284    // For now we just call icp_point and maybe filter outliers?
285    // ARToolkit 5 uses a specific robust point estimation.
286    // Let's at least implement the signature and a basic pass-through.
287    icp_point(handle, data, init_mat_xw2xc, mat_xw2xc)
288}
289
290pub fn icp_get_xc_from_xw_by_mat_xw2xc(xc: &mut ICP3DCoordT, mat_xw2xc: &[[ARdouble; 4]; 3], xw: &ICP3DCoordT) {
291    xc.x = mat_xw2xc[0][0] * xw.x + mat_xw2xc[0][1] * xw.y + mat_xw2xc[0][2] * xw.z + mat_xw2xc[0][3];
292    xc.y = mat_xw2xc[1][0] * xw.x + mat_xw2xc[1][1] * xw.y + mat_xw2xc[1][2] * xw.z + mat_xw2xc[1][3];
293    xc.z = mat_xw2xc[2][0] * xw.x + mat_xw2xc[2][1] * xw.y + mat_xw2xc[2][2] * xw.z + mat_xw2xc[2][3];
294}
295
296pub fn icp_get_u_from_x_by_mat_x2u(u: &mut ICP2DCoordT, mat_x2u: &[[ARdouble; 4]; 3], coord3d: &ICP3DCoordT) -> Result<(), &'static str> {
297    let hx = mat_x2u[0][0] * coord3d.x + mat_x2u[0][1] * coord3d.y + mat_x2u[0][2] * coord3d.z + mat_x2u[0][3];
298    let hy = mat_x2u[1][0] * coord3d.x + mat_x2u[1][1] * coord3d.y + mat_x2u[1][2] * coord3d.z + mat_x2u[1][3];
299    let h  = mat_x2u[2][0] * coord3d.x + mat_x2u[2][1] * coord3d.y + mat_x2u[2][2] * coord3d.z + mat_x2u[2][3];
300
301    if h == 0.0 {
302        return Err("Division by zero in icp_get_u_from_x_by_mat_x2u");
303    }
304
305    u.x = hx / h;
306    u.y = hy / h;
307    Ok(())
308}
309
310fn icp_get_j_u_xc(j_u_xc: &mut [[ARdouble; 3]; 2], mat_xc2u: &[[ARdouble; 4]; 3], camera_coord: &ICP3DCoordT) -> Result<(), &'static str> {
311    let w1 = mat_xc2u[0][0] * camera_coord.x + mat_xc2u[0][1] * camera_coord.y + mat_xc2u[0][2] * camera_coord.z + mat_xc2u[0][3];
312    let w2 = mat_xc2u[1][0] * camera_coord.x + mat_xc2u[1][1] * camera_coord.y + mat_xc2u[1][2] * camera_coord.z + mat_xc2u[1][3];
313    let w3 = mat_xc2u[2][0] * camera_coord.x + mat_xc2u[2][1] * camera_coord.y + mat_xc2u[2][2] * camera_coord.z + mat_xc2u[2][3];
314
315    if w3 == 0.0 { return Err("Division by zero in icp_get_j_u_xc"); }
316
317    let w3_w3 = w3 * w3;
318    j_u_xc[0][0] = (mat_xc2u[0][0] * w3 - mat_xc2u[2][0] * w1) / w3_w3;
319    j_u_xc[0][1] = (mat_xc2u[0][1] * w3 - mat_xc2u[2][1] * w1) / w3_w3;
320    j_u_xc[0][2] = (mat_xc2u[0][2] * w3 - mat_xc2u[2][2] * w1) / w3_w3;
321    j_u_xc[1][0] = (mat_xc2u[1][0] * w3 - mat_xc2u[2][0] * w2) / w3_w3;
322    j_u_xc[1][1] = (mat_xc2u[1][1] * w3 - mat_xc2u[2][1] * w2) / w3_w3;
323    j_u_xc[1][2] = (mat_xc2u[1][2] * w3 - mat_xc2u[2][2] * w2) / w3_w3;
324
325    Ok(())
326}
327
328fn icp_get_j_t_s(j_t_s: &mut [[ARdouble; 6]; 12]) {
329    for i in 0..12 {
330        for j in 0..6 { j_t_s[i][j] = 0.0; }
331    }
332    j_t_s[1][2] = -1.0;
333    j_t_s[2][1] = 1.0;
334    j_t_s[3][2] = 1.0;
335    j_t_s[5][0] = -1.0;
336    j_t_s[6][1] = -1.0;
337    j_t_s[7][0] = 1.0;
338    j_t_s[9][3] = 1.0;
339    j_t_s[10][4] = 1.0;
340    j_t_s[11][5] = 1.0;
341}
342
343fn icp_get_j_xc_s(j_xc_s: &mut [[ARdouble; 6]; 3], camera_coord: &mut ICP3DCoordT, t0: &[[ARdouble; 4]; 3], world_coord: &ICP3DCoordT) {
344    let mut j_xc_t = [[0.0; 12]; 3];
345
346    camera_coord.x = t0[0][0]*world_coord.x + t0[0][1]*world_coord.y + t0[0][2]*world_coord.z + t0[0][3];
347    camera_coord.y = t0[1][0]*world_coord.x + t0[1][1]*world_coord.y + t0[1][2]*world_coord.z + t0[1][3];
348    camera_coord.z = t0[2][0]*world_coord.x + t0[2][1]*world_coord.y + t0[2][2]*world_coord.z + t0[2][3];
349
350    for j in 0..3 {
351        j_xc_t[j][0] = t0[j][0] * world_coord.x;
352        j_xc_t[j][1] = t0[j][0] * world_coord.y;
353        j_xc_t[j][2] = t0[j][0] * world_coord.z;
354        j_xc_t[j][3] = t0[j][1] * world_coord.x;
355        j_xc_t[j][4] = t0[j][1] * world_coord.y;
356        j_xc_t[j][5] = t0[j][1] * world_coord.z;
357        j_xc_t[j][6] = t0[j][2] * world_coord.x;
358        j_xc_t[j][7] = t0[j][2] * world_coord.y;
359        j_xc_t[j][8] = t0[j][2] * world_coord.z;
360        j_xc_t[j][9] = t0[j][0];
361        j_xc_t[j][10] = t0[j][1];
362        j_xc_t[j][11] = t0[j][2];
363    }
364
365    let mut j_t_s = [[0.0; 6]; 12];
366    icp_get_j_t_s(&mut j_t_s);
367
368    for j in 0..3 {
369        for i in 0..6 {
370            j_xc_s[j][i] = 0.0;
371            for k in 0..12 {
372                j_xc_s[j][i] += j_xc_t[j][k] * j_t_s[k][i];
373            }
374        }
375    }
376}
377
378pub fn icp_get_j_u_s(j_u_s: &mut [[ARdouble; 6]; 2], mat_xc2u: &[[ARdouble; 4]; 3], mat_xw2xc: &[[ARdouble; 4]; 3], world_coord: &ICP3DCoordT) -> Result<(), &'static str> {
379    let mut j_xc_s = [[0.0; 6]; 3];
380    let mut j_u_xc = [[0.0; 3]; 2];
381    let mut xc = ICP3DCoordT::default();
382
383    icp_get_j_xc_s(&mut j_xc_s, &mut xc, mat_xw2xc, world_coord);
384    icp_get_j_u_xc(&mut j_u_xc, mat_xc2u, &xc)?;
385
386    for j in 0..2 {
387        for i in 0..6 {
388            j_u_s[j][i] = 0.0;
389            for k in 0..3 {
390                j_u_s[j][i] += j_u_xc[j][k] * j_xc_s[k][i];
391            }
392        }
393    }
394    Ok(())
395}
396
397fn icp_get_q_from_s(q: &mut [ARdouble; 7], s: &[ARdouble; 6]) {
398    let mut ra = s[0]*s[0] + s[1]*s[1] + s[2]*s[2];
399    if ra == 0.0 {
400        q[0] = 1.0;
401        q[1] = 0.0;
402        q[2] = 0.0;
403        q[3] = 0.0;
404    } else {
405        ra = ra.sqrt();
406        q[0] = s[0] / ra;
407        q[1] = s[1] / ra;
408        q[2] = s[2] / ra;
409        q[3] = ra;
410    }
411    q[4] = s[3];
412    q[5] = s[4];
413    q[6] = s[5];
414}
415
416fn icp_get_mat_from_q(mat: &mut [[ARdouble; 4]; 3], q: &[ARdouble; 7]) {
417    let cra = q[3].cos();
418    let one_cra = 1.0 - cra;
419    let sra = q[3].sin();
420
421    mat[0][0] = q[0]*q[0]*one_cra + cra;
422    mat[0][1] = q[0]*q[1]*one_cra - q[2]*sra;
423    mat[0][2] = q[0]*q[2]*one_cra + q[1]*sra;
424    mat[0][3] = q[4];
425    
426    mat[1][0] = q[1]*q[0]*one_cra + q[2]*sra;
427    mat[1][1] = q[1]*q[1]*one_cra + cra;
428    mat[1][2] = q[1]*q[2]*one_cra - q[0]*sra;
429    mat[1][3] = q[5];
430    
431    mat[2][0] = q[2]*q[0]*one_cra - q[1]*sra;
432    mat[2][1] = q[2]*q[1]*one_cra + q[0]*sra;
433    mat[2][2] = q[2]*q[2]*one_cra + cra;
434    mat[2][3] = q[6];
435}
436
437pub fn icp_update_mat(mat_xw2xc: &mut [[ARdouble; 4]; 3], ds: &[ARdouble; 6]) {
438    let mut q = [0.0; 7];
439    let mut mat = [[0.0; 4]; 3];
440    let mut mat2 = [[0.0; 4]; 3];
441
442    icp_get_q_from_s(&mut q, ds);
443    icp_get_mat_from_q(&mut mat, &q);
444
445    for j in 0..3 {
446        for i in 0..4 {
447            mat2[j][i] = mat_xw2xc[j][0] * mat[0][i]
448                       + mat_xw2xc[j][1] * mat[1][i]
449                       + mat_xw2xc[j][2] * mat[2][i];
450        }
451        mat2[j][3] += mat_xw2xc[j][3];
452    }
453
454    for j in 0..3 {
455        for i in 0..4 {
456            mat_xw2xc[j][i] = mat2[j][i];
457        }
458    }
459}
460
461pub fn icp_get_delta_s(s: &mut [ARdouble; 6], du: &[ARdouble], j_u_s: &[[ARdouble; 6]], n: usize) -> Result<(), &'static str> {
462    let mut mat_u = ARMat::new(n as i32, 1);
463    mat_u.m.copy_from_slice(du);
464
465    let mut mat_j = ARMat::new(n as i32, 6);
466    for r in 0..n {
467        for c in 0..6 {
468            mat_j.m[r * 6 + c] = j_u_s[r][c];
469        }
470    }
471
472    let mat_jt = mat_j.transpose();
473    
474    let mut mat_jt_j = (&mat_jt * &mat_j)?;
475    let mat_jt_u = (&mat_jt * &mat_u)?;
476
477    if let Err(e) = mat_jt_j.self_inv() {
478        return Err(e);
479    }
480
481    let mat_s = (&mat_jt_j * &mat_jt_u)?;
482
483    for i in 0..6 {
484        s[i] = mat_s.m[i];
485    }
486
487    Ok(())
488}
489
490pub fn icp_create_handle(mat_xc2u: &[[ARdouble; 4]; 3]) -> Result<*mut ICPHandleT, &'static str> {
491    let mut handle = Box::new(ICPHandleT::default());
492    for j in 0..3 {
493        for i in 0..4 {
494            handle.mat_xc2u[j][i] = mat_xc2u[j][i];
495        }
496    }
497    Ok(Box::into_raw(handle))
498}
499
500pub fn icp_delete_handle(handle: &mut *mut ICPHandleT) -> Result<(), &'static str> {
501    if handle.is_null() {
502        return Err("Null handle");
503    }
504    unsafe {
505        drop(Box::from_raw(*handle));
506    }
507    *handle = std::ptr::null_mut();
508    Ok(())
509}
510
511#[allow(dead_code)]
512fn check_rotation(rot: &mut [[ARdouble; 3]; 3]) -> Result<(), &'static str> {
513    let v1 = [rot[0][0], rot[0][1], rot[0][2]];
514    let v2 = [rot[1][0], rot[1][1], rot[1][2]];
515    let mut v3 = [
516        v1[1] * v2[2] - v1[2] * v2[1],
517        v1[2] * v2[0] - v1[0] * v2[2],
518        v1[0] * v2[1] - v1[1] * v2[0],
519    ];
520    let w = (v3[0]*v3[0] + v3[1]*v3[1] + v3[2]*v3[2]).sqrt();
521    if w == 0.0 { return Err("Collinear vectors in check_rotation"); }
522    v3[0] /= w; v3[1] /= w; v3[2] /= w;
523
524    let mut cb = v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
525    if cb < 0.0 { cb = -cb; }
526    let ca = ((cb + 1.0).sqrt() + (1.0 - cb).sqrt()) * 0.5;
527
528    let mut rot_flag: i32;
529    if v3[1] * v1[0] - v1[1] * v3[0] != 0.0 {
530        rot_flag = 0;
531    } else {
532        if v3[2] * v1[0] - v1[2] * v3[0] != 0.0 {
533            rot_flag = 1;
534        } else {
535            rot_flag = 2;
536        }
537    }
538
539    let mut v1_temp = v1; // Use temporary mutable copies for swapping
540    let mut v3_temp = v3;
541
542    if rot_flag == 1 {
543        v1_temp.swap(1, 2);
544        v3_temp.swap(1, 2);
545    } else if rot_flag == 2 {
546        v1_temp.swap(0, 2);
547        v3_temp.swap(0, 2);
548    }
549
550    let denom1 = v3_temp[1] * v1_temp[0] - v1_temp[1] * v3_temp[0];
551    if denom1 == 0.0 { return Err("check_rotation denom1 is 0"); }
552    let k1 = (v1_temp[1]*v3_temp[2] - v3_temp[1]*v1_temp[2]) / denom1;
553    let k2 = (v3_temp[1] * ca) / denom1;
554
555    let denom2 = v3_temp[0] * v1_temp[1] - v1_temp[0] * v3_temp[1];
556    if denom2 == 0.0 { return Err("check_rotation denom2 is 0"); }
557    let k3 = (v1_temp[0]*v3_temp[2] - v3_temp[0]*v1_temp[2]) / denom2;
558    let k4 = (v3_temp[0] * ca) / denom2;
559
560    let a = k1*k1 + k3*k3 + 1.0;
561    let b = k1*k2 + k3*k4;
562    let c = k2*k2 + k4*k4 - 1.0;
563
564    let d = b*b - a*c;
565    if d < 0.0 { return Err("check_rotation complex roots"); }
566    
567    let mut r1 = (-b + d.sqrt()) / a;
568    let mut p1 = k1*r1 + k2;
569    let mut q1 = k3*r1 + k4;
570    let mut r2 = (-b - d.sqrt()) / a;
571    let mut p2 = k1*r2 + k2;
572    let mut q2 = k3*r2 + k4;
573
574    if rot_flag == 1 {
575        std::mem::swap(&mut q1, &mut r1);
576        std::mem::swap(&mut q2, &mut r2);
577    } else if rot_flag == 2 {
578        std::mem::swap(&mut p1, &mut r1);
579        std::mem::swap(&mut p2, &mut r2);
580    }
581
582    if v3[1] * v2[0] - v2[1] * v3[0] != 0.0 {
583        rot_flag = 0;
584    } else {
585        if v3[2] * v2[0] - v2[2] * v3[0] != 0.0 {
586            rot_flag = 1;
587        } else {
588            rot_flag = 2;
589        }
590    }
591
592    let mut v2_temp = v2; // Use temporary mutable copies for swapping
593    v3_temp = v3; // Re-initialize v3_temp as v3 might have been modified by previous swaps
594
595    if rot_flag == 1 {
596        v2_temp.swap(1, 2);
597        v3_temp.swap(1, 2);
598    } else if rot_flag == 2 {
599        v2_temp.swap(0, 2);
600        v3_temp.swap(0, 2);
601    }
602
603    let denom3 = v3_temp[1] * v2_temp[0] - v2_temp[1] * v3_temp[0];
604    if denom3 == 0.0 { return Err("check_rotation denom3 is 0"); }
605    let k1_2 = (v2_temp[1]*v3_temp[2] - v3_temp[1]*v2_temp[2]) / denom3;
606    let k2_2 = (v3_temp[1] * ca) / denom3;
607
608    let denom4 = v3_temp[0] * v2_temp[1] - v2_temp[0] * v3_temp[1];
609    if denom4 == 0.0 { return Err("check_rotation denom4 is 0"); }
610    let k3_2 = (v2_temp[0]*v3_temp[2] - v3_temp[0]*v2_temp[2]) / denom4;
611    let k4_2 = (v3_temp[0] * ca) / denom4;
612
613    let a_2 = k1_2*k1_2 + k3_2*k3_2 + 1.0;
614    let b_2 = k1_2*k2_2 + k3_2*k4_2;
615    let c_2 = k2_2*k2_2 + k4_2*k4_2 - 1.0;
616
617    let d_2 = b_2*b_2 - a_2*c_2;
618    if d_2 < 0.0 { return Err("check_rotation complex roots 2"); }
619
620    let mut r3 = (-b_2 + d_2.sqrt()) / a_2;
621    let mut p3 = k1_2*r3 + k2_2;
622    let mut q3 = k3_2*r3 + k4_2;
623    let mut r4 = (-b_2 - d_2.sqrt()) / a_2;
624    let mut p4 = k1_2*r4 + k2_2;
625    let mut q4 = k3_2*r4 + k4_2;
626
627    if rot_flag == 1 {
628        std::mem::swap(&mut q3, &mut r3);
629        std::mem::swap(&mut q4, &mut r4);
630    } else if rot_flag == 2 {
631        std::mem::swap(&mut p3, &mut r3);
632        std::mem::swap(&mut p4, &mut r4);
633    }
634
635    let e1 = (p1*p3 + q1*q3 + r1*r3).abs();
636    let e2 = (p1*p4 + q1*q4 + r1*r4).abs();
637    let e3 = (p2*p3 + q2*q3 + r2*r3).abs();
638    let e4 = (p2*p4 + q2*q4 + r2*r4).abs();
639
640    if e1 < e2 {
641        if e1 < e3 {
642            if e1 < e4 {
643                rot[0][0] = p1; rot[0][1] = q1; rot[0][2] = r1;
644                rot[1][0] = p3; rot[1][1] = q3; rot[1][2] = r3;
645            } else {
646                rot[0][0] = p2; rot[0][1] = q2; rot[0][2] = r2;
647                rot[1][0] = p4; rot[1][1] = q4; rot[1][2] = r4;
648            }
649        } else {
650            if e3 < e4 {
651                rot[0][0] = p2; rot[0][1] = q2; rot[0][2] = r2;
652                rot[1][0] = p3; rot[1][1] = q3; rot[1][2] = r3;
653            } else {
654                rot[0][0] = p2; rot[0][1] = q2; rot[0][2] = r2;
655                rot[1][0] = p4; rot[1][1] = q4; rot[1][2] = r4;
656            }
657        }
658    } else {
659        if e2 < e3 {
660            if e2 < e4 {
661                rot[0][0] = p1; rot[0][1] = q1; rot[0][2] = r1;
662                rot[1][0] = p4; rot[1][1] = q4; rot[1][2] = r4;
663            } else {
664                rot[0][0] = p2; rot[0][1] = q2; rot[0][2] = r2;
665                rot[1][0] = p4; rot[1][1] = q4; rot[1][2] = r4;
666            }
667        } else {
668            if e3 < e4 {
669                rot[0][0] = p2; rot[0][1] = q2; rot[0][2] = r2;
670                rot[1][0] = p3; rot[1][1] = q3; rot[1][2] = r3;
671            } else {
672                rot[0][0] = p2; rot[0][1] = q2; rot[0][2] = r2;
673                rot[1][0] = p4; rot[1][1] = q4; rot[1][2] = r4;
674            }
675        }
676    }
677
678    Ok(())
679}
680
681pub fn icp_get_init_xw2xc_from_planar_data(
682    mat_xc2u: &[[ARdouble; 4]; 3],
683    screen_coord: &[ICP2DCoordT],
684    world_coord: &[ICP3DCoordT],
685    num: usize,
686    init_mat_xw2xc: &mut [[ARdouble; 4]; 3]
687) -> Result<(), &'static str> {
688    if num < 4 { return Err("Needs at least 4 points"); }
689    for i in 0..num {
690        if world_coord[i].z != 0.0 { return Err("Points must be planar (Z=0)"); }
691    }
692    if mat_xc2u[0][0] == 0.0 { return Err("mat_xc2u[0][0] is zero"); }
693    if mat_xc2u[1][0] != 0.0 { return Err("mat_xc2u[1][0] is not zero"); }
694    if mat_xc2u[1][1] == 0.0 { return Err("mat_xc2u[1][1] is zero"); }
695    if mat_xc2u[2][0] != 0.0 || mat_xc2u[2][1] != 0.0 || mat_xc2u[2][2] != 1.0 {
696        return Err("mat_xc2u row 2 invalid");
697    }
698    if mat_xc2u[0][3] != 0.0 || mat_xc2u[1][3] != 0.0 || mat_xc2u[2][3] != 0.0 {
699        return Err("mat_xc2u translation column invalid");
700    }
701
702    let mut mat_a = ARMat::new(num as i32 * 2, 8);
703    let mut mat_b = ARMat::new(num as i32 * 2, 1);
704
705    for i in 0..num {
706        mat_a.m[i * 16 + 0] = world_coord[i].x;
707        mat_a.m[i * 16 + 1] = world_coord[i].y;
708        mat_a.m[i * 16 + 2] = 1.0;
709        mat_a.m[i * 16 + 3] = 0.0;
710        mat_a.m[i * 16 + 4] = 0.0;
711        mat_a.m[i * 16 + 5] = 0.0;
712        mat_a.m[i * 16 + 6] = -(world_coord[i].x) * (screen_coord[i].x);
713        mat_a.m[i * 16 + 7] = -(world_coord[i].y) * (screen_coord[i].x);
714        
715        mat_a.m[i * 16 + 8]  = 0.0;
716        mat_a.m[i * 16 + 9]  = 0.0;
717        mat_a.m[i * 16 + 10] = 0.0;
718        mat_a.m[i * 16 + 11] = world_coord[i].x;
719        mat_a.m[i * 16 + 12] = world_coord[i].y;
720        mat_a.m[i * 16 + 13] = 1.0;
721        mat_a.m[i * 16 + 14] = -(world_coord[i].x) * (screen_coord[i].y);
722        mat_a.m[i * 16 + 15] = -(world_coord[i].y) * (screen_coord[i].y);
723
724        mat_b.m[i * 2 + 0] = screen_coord[i].x;
725        mat_b.m[i * 2 + 1] = screen_coord[i].y;
726    }
727    
728    let mat_at = mat_a.transpose();
729    let mut mat_ata = (&mat_at * &mat_a)?;
730    let mat_atb = (&mat_at * &mat_b)?;
731    
732    // println!("mat_ata:\n{:?}", mat_ata);
733    mat_ata.self_inv()?;
734    // println!("mat_ata_inv:\n{:?}", mat_ata);
735    
736    let mat_c = (&mat_ata * &mat_atb)?;
737    // println!("mat_c:\n{:?}", mat_c);
738
739    let mut v = [[0.0; 3]; 3];
740    let mut t = [0.0; 3];
741
742    v[0][2] = mat_c.m[6];
743    v[0][1] = (mat_c.m[3] - mat_xc2u[1][2] * v[0][2]) / mat_xc2u[1][1];
744    v[0][0] = (mat_c.m[0] - mat_xc2u[0][2] * v[0][2] - mat_xc2u[0][1] * v[0][1]) / mat_xc2u[0][0];
745    
746    v[1][2] = mat_c.m[7];
747    v[1][1] = (mat_c.m[4] - mat_xc2u[1][2] * v[1][2]) / mat_xc2u[1][1];
748    v[1][0] = (mat_c.m[1] - mat_xc2u[0][2] * v[1][2] - mat_xc2u[0][1] * v[1][1]) / mat_xc2u[0][0];
749    
750    t[2] = 1.0;
751    t[1] = (mat_c.m[5] - mat_xc2u[1][2] * t[2]) / mat_xc2u[1][1];
752    t[0] = (mat_c.m[2] - mat_xc2u[0][2] * t[2] - mat_xc2u[0][1] * t[1]) / mat_xc2u[0][0];
753
754    let mut l1 = (v[0][0]*v[0][0] + v[0][1]*v[0][1] + v[0][2]*v[0][2]).sqrt();
755    let l2 = (v[1][0]*v[1][0] + v[1][1]*v[1][1] + v[1][2]*v[1][2]).sqrt();
756    
757    v[0][0] /= l1;
758    v[0][1] /= l1;
759    v[0][2] /= l1;
760    
761    v[1][0] /= l2;
762    v[1][1] /= l2;
763    v[1][2] /= l2;
764    
765    t[0] /= (l1+l2)/2.0;
766    t[1] /= (l1+l2)/2.0;
767    t[2] /= (l1+l2)/2.0;
768    
769    if t[2] < 0.0 {
770        v[0][0] = -v[0][0]; v[0][1] = -v[0][1]; v[0][2] = -v[0][2];
771        v[1][0] = -v[1][0]; v[1][1] = -v[1][1]; v[1][2] = -v[1][2];
772        t[0] = -t[0]; t[1] = -t[1]; t[2] = -t[2];
773    }
774    
775    
776    // println!("v after rotation check:\n{:?}", v);
777    
778    v[2][0] = v[0][1] * v[1][2] - v[0][2] * v[1][1];
779    v[2][1] = v[0][2] * v[1][0] - v[0][0] * v[1][2];
780    v[2][2] = v[0][0] * v[1][1] - v[0][1] * v[1][0];
781    
782    l1 = (v[2][0]*v[2][0] + v[2][1]*v[2][1] + v[2][2]*v[2][2]).sqrt();
783    v[2][0] /= l1;
784    v[2][1] /= l1;
785    v[2][2] /= l1;
786    
787    init_mat_xw2xc[0][0] = v[0][0]; init_mat_xw2xc[1][0] = v[0][1]; init_mat_xw2xc[2][0] = v[0][2];
788    init_mat_xw2xc[0][1] = v[1][0]; init_mat_xw2xc[1][1] = v[1][1]; init_mat_xw2xc[2][1] = v[1][2];
789    init_mat_xw2xc[0][2] = v[2][0]; init_mat_xw2xc[1][2] = v[2][1]; init_mat_xw2xc[2][2] = v[2][2];
790    init_mat_xw2xc[0][3] = t[0];    init_mat_xw2xc[1][3] = t[1];    init_mat_xw2xc[2][3] = t[2];
791
792    Ok(())
793}
794
795#[cfg(test)]
796mod tests {
797    use super::*;
798
799    #[test]
800    fn test_icp_handle_default() {
801        let handle = ICPHandleT::default();
802        assert_eq!(handle.max_loop, 10);
803        assert_eq!(handle.break_loop_error_thresh, 0.1);
804    }
805
806    #[test]
807    fn test_icp_mat_mul() {
808        let m1 = [
809            [1.0, 2.0, 3.0, 4.0],
810            [5.0, 6.0, 7.0, 8.0],
811            [9.0, 10.0, 11.0, 12.0],
812        ];
813        let m2 = [
814            [2.0, 0.0, 0.0, 1.0],
815            [0.0, 2.0, 0.0, 2.0],
816            [0.0, 0.0, 2.0, 3.0],
817        ];
818        let mut dest = [[0.0; 4]; 3];
819        icp_mat_mul(&m1, &m2, &mut dest);
820
821        // Expected manually calculated:
822        // row 0:
823        // c0 = 1*2 = 2
824        // c1 = 2*2 = 4
825        // c2 = 3*2 = 6
826        // c3 = 1*1 + 2*2 + 3*3 + 4 = 1 + 4 + 9 + 4 = 18
827        assert_eq!(dest[0][0], 2.0);
828        assert_eq!(dest[0][1], 4.0);
829        assert_eq!(dest[0][2], 6.0);
830        assert_eq!(dest[0][3], 18.0);
831    }
832
833    #[test]
834    fn test_icp_get_xc_from_xw() {
835        let mat = [
836            [1.0, 0.0, 0.0, 10.0],
837            [0.0, 1.0, 0.0, 20.0],
838            [0.0, 0.0, 1.0, 30.0],
839        ];
840        let xw = ICP3DCoordT { x: 5.0, y: 5.0, z: 5.0 };
841        let mut xc = ICP3DCoordT::default();
842        
843        icp_get_xc_from_xw_by_mat_xw2xc(&mut xc, &mat, &xw);
844        
845        assert_eq!(xc.x, 15.0);
846        assert_eq!(xc.y, 25.0);
847        assert_eq!(xc.z, 35.0);
848    }
849
850    #[test]
851    fn test_icp_get_u_from_x() {
852        let mat = [
853            [100.0, 0.0, 50.0, 0.0],
854            [0.0, 100.0, 50.0, 0.0],
855            [0.0, 0.0, 1.0, 0.0],
856        ];
857        let xc = ICP3DCoordT { x: 10.0, y: 20.0, z: 2.0 };
858        let mut u = ICP2DCoordT::default();
859        
860        icp_get_u_from_x_by_mat_x2u(&mut u, &mat, &xc).unwrap();
861        
862        // h = 1.0 * 2.0 = 2.0
863        // hx = 100*10 + 50*2 = 1100
864        // hy = 100*20 + 50*2 = 2100
865        // u.x = 1100 / 2 = 550
866        // u.y = 2100 / 2 = 1050
867        assert_eq!(u.x, 550.0);
868        assert_eq!(u.y, 1050.0);
869    }
870}
webarkitlib_rs/icp.rs

webarkitlib_rs/
icp.rs
