(* Line and circle detection *) (* Using statistical correlation *) (* $Id: line-detect.ml,v 1.1 2003/01/12 15:33:00 berke Exp $ *) (* By Berke Durak on 20011026 *) open Graphics open Bigarray let pi = 4.0 *. atan 1.0 let make_matrix m n = Array2.create float64 c_layout m n and make_vector m = Array1.create float64 c_layout m and dimensions2 a = (Array2.dim1 a,Array2.dim2 a) let m = 512 and n = 512 (* From ``Numerical Recipes in C'', pp.289-290 *) let gaussian = let iset = ref false and gset = ref 0.0 in fun () -> if !iset then begin iset := false; !gset end else begin let rec loop () = let v1 = 2.0 *. (Random.float 1.0) -. 1.0 and v2 = 2.0 *. (Random.float 1.0) -. 1.0 in let rsq = v1 *. v1 +. v2 *. v2 in if rsq >= 1.0 or rsq = 0.0 then loop () else let fac = sqrt (-2.0 *. (log rsq) /. rsq) in gset := v1 *. fac; iset := true; v2 *. fac in loop () end let uniform () = (Random.float 2.0) -. 1.0 (* Convex hull computation *) let ordprod x y = if x = 0 then y else x let sign x = if x < 0.0 then -1 else if x > 0.0 then 1 else 0 let direct_vector x1 y1 x2 y2 = sign (x1 *. y2 -. x2 *. y1) let direct (_,x1,y1) (_,x2,y2) (_,x3,y3) = direct_vector (x2 -. x1) (y2 -. y1) (x3 -. x1) (y3 -. y1) let f x = int_of_float ((float (m / 2)) +. (float (m / 2)) *. x) and g i = ((float_of_int i) -. (float (m / 2))) /. (float (m / 2)) and h x = int_of_float ((float_of_int (m / 2)) *. x) let dwhl h = match h with [] -> () | (_,x0,y0)::r -> moveto (f x0) (f y0); List.iter (fun (_,x,y) -> lineto (f x) (f y)) r let hull a = let m = Array.length a in if m = 0 then [] else begin let a' = Array.mapi (fun i (x,y) -> (i,x,y)) a in Array.sort (fun (i,x,y) (i',x',y') -> ordprod (compare x x') (ordprod (compare y y') (compare i i'))) a'; let lower = [a'.(0)] and upper = [a'.(0)] and (_,_,line) = a'.(0) in let rec loop upper lower i = if i = m then match (lower,List.rev upper) with (x::r,x'::r') -> r@r' | ([],r') -> r' | (r,[]) -> r else let ((_,x,y) as q) = a'.(i) in loop (add_to_half_hull 1 q upper) (add_to_half_hull (-1) q lower) (i + 1) and add_to_half_hull sgn q = function [] -> [q] | [p] -> [q;p] | p::o::r -> if sgn = direct o p q then q::p::o::r else add_to_half_hull sgn q (o::r) in loop upper lower 1 end let square x = x *. x let distance x1 y1 x2 y2 = (square (x1 -. x2)) +. (square (y1 -. y2)) let voronoi cols a m n = Printf.printf "VORONOI\n"; flush stdout; let o = Array.length a in for i = 0 to m - 1 do for j = 0 to n - 1 do let rec loop k d l = if k = o then l else let (x,y) = a.(k) in let d' = distance (g i) (g j) x y in if d' < d then loop (k + 1) d' k else loop (k + 1) d l in let (x,y) = a.(0) in set_color cols.(loop 1 (distance (g i) (g j) x y) 0); plot i j done done let center x1 y1 x2 y2 x3 y3 = let u1 = x2 -. x1 and u2 = y2 -. y1 and v1 = x3 -. x1 and v2 = y3 -. y1 in let det = u1 *. v2 -. u2 *. v1 in if abs_float det > 0.01 then let g1 = (u1 *. 0.5 *. (x2 +. x1) +. u2 *. 0.5 *. (y2 +. y1)) and g2 = (v1 *. 0.5 *. (x3 +. x1) +. v2 *. 0.5 *. (y3 +. y1)) in Some((v2 *. g1 -. u2 *. g2) /. det, (u1 *. g2 -. v1 *. g1) /. det) else None let test () = let x0 = Random.float 1.0 and y0 = 0.0 (* Random.float 1.0 *) in Printf.printf "# center %f %f\n" x0 y0; for i = 0 to 50 do let theta = Random.float (2.0 *. pi) and theta' = Random.float (2.0 *. pi) and theta'' = Random.float (2.0 *. pi) in match center (x0 +. cos theta) (y0 +. sin theta) (x0 +. cos theta') (y0 +. sin theta') (x0 +. cos theta'') (y0 +. sin theta'') with None -> Printf.printf "# missed\n" | Some(x,y) -> Printf.printf "%f %f\n" x y done let create_sample sigma = let m = (Random.int 50) + 50 in let a = make_matrix m 2 in if Random.float 1.0 < 0.5 then match Random.int 3 with 0 -> for i = 0 to m - 1 do a.{i,0} <- uniform (); a.{i,1} <- uniform () done; (a,false) | 1 -> let x1 = uniform () and y1 = uniform () and r = 0.1 +. uniform () in for i = 0 to m - 1 do a.{i,0} <- x1 +. r *. gaussian (); a.{i,1} <- x1 +. r *. gaussian (); done; (a,false) | _ -> let x1 = uniform () and y1 = uniform () and r1 = 0.1 +. uniform () in let r2 = r1 *. (1.0 +. 0.01 *. uniform ()) in for i = 0 to m - 1 do let theta = Random.float (2.0 *. pi) in a.{i,0} <- x1 +. r1 *. cos theta +. (sigma *. gaussian ()); a.{i,1} <- y1 +. r2 *. sin theta +. (sigma *. gaussian ()) done; (a,false) else begin let theta = Random.float (2.0 *. pi) in let x1 = uniform () and y1 = uniform () in let alpha = 0.4 +. Random.float 1.0 in let x2 = x1 +. alpha *. cos theta and y2 = y1 +. alpha *. sin theta in for i = 0 to m - 1 do let alpha = Random.float 1.0 in a.{i,0} <- x1 +. alpha *. (x2 -. x1) +. sigma *. (gaussian ()); a.{i,1} <- y1 +. alpha *. (y2 -. y1) +. sigma *. (gaussian ()) done; (a,true) end let compute_correlation a = let p = Array2.dim1 a in let k = float p in let rec loop i sxy sx sy sx2 sy2 = if i = p then let ex = sx /. k and ey = sy /. k and exy = sxy /. k and ex2 = sx2 /. k and ey2 = sy2 /. k in let vx = ex2 -. ex *. ex and vy = ey2 -. ey *. ey in (exy -. ex *. ey,sqrt (vx *. vy)) else let xu = a.{i,0} and yu = a.{i,1} in loop (i + 1) (sxy +. xu *. yu) (sx +. xu) (sy +. yu) (sx2 +. xu *. xu) (sy2 +. yu *. yu) in loop 0 0.0 0.0 0.0 0.0 0.0 let rot45 a = let m = Array2.dim1 a in let b = make_matrix m 2 in for i = 0 to m - 1 do let x = a.{i,0} and y = a.{i,1} in b.{i,0} <- x +. y; b.{i,1} <- y -. x done; b let run () = auto_synchronize true; let vor = ref false in let button = ref false in let continue = ref true in let sigma = ref 0.1 in let failures = ref 0 in let total = ref 0 in let rho_threshold = ref 0.9 in let freeze = ref false in while !continue do if not !freeze then begin let (a,kind) = create_sample !sigma in let b = rot45 a in let m = Array2.dim1 a in let (cov1,vv1) = compute_correlation a and (cov2,vv2) = compute_correlation b in let rho1 = cov1 /. vv1 and rho2 = cov2 /. vv2 in incr total; let rho' = max (abs_float rho1) (abs_float rho2) in if rho' < !rho_threshold then begin set_color (rgb 0 0 0); if kind then begin Printf.printf "*** FAILURE ***\n"; freeze := true; rho_threshold := 0.9 *. !rho_threshold +. 0.1 *. (rho' *. 0.9); incr failures end end else begin set_color (rgb 255 0 0); if not kind then begin Printf.printf "*** FAILURE ***\n"; freeze := true; rho_threshold := 1.1 *. !rho_threshold +. 0.1 *. (rho' *. 0.9); incr failures end end; clear_graph (); for i = 0 to m - 1 do plot (f a.{i,0}) (f a.{i,1}) done; set_color (rgb 0 255 0); let tot_x = ref 0.0 and tot_y = ref 0.0 and tot_xx = ref 0.0 and tot_yy = ref 0.0 and tot_rad = ref 0.0 and count = ref 0 in for i = 0 to m - 1 do for j = i + 1 to m - 1 do for k = j + 1 to m - 1 do match center a.{i,0} a.{i,1} a.{j,0} a.{j,1} a.{k,0} a.{k,1} with None -> () | Some(x,y) -> tot_x := !tot_x +. x; tot_y := !tot_y +. y; tot_xx := !tot_xx +. x *. x; tot_yy := !tot_yy +. y *. y; tot_rad := !tot_rad +. (sqrt (square (a.{i,0} -. x) +. square (a.{i,1} -. y))); incr count done done done; let x = !tot_x /. (float !count) and y = !tot_y /. (float !count) in let rad = !tot_rad /. (float !count) in let s = !tot_xx /. (float !count) -. x *. x +. !tot_yy /. (float !count) -. y *. y in let sr = s /. (square rad) in if sr < 0.05 then begin plot (f x) (f y); draw_circle (f x) (f y) (h rad) end; Printf.printf "sigma^2 = %f rad = %f sigma^2/rad^2 = %f\n" s rad sr; Printf.printf "rho_t = %f %d/%d failure rate %f\n" !rho_threshold !failures !total ((float !failures) /. (float !total)); Printf.printf "cov1 = %f vv1 = %f rho1 = %f sigma = %f\n" cov1 vv1 (cov1 /. vv1) !sigma; Printf.printf "cov2 = %f vv2 = %f rho2 = %f sigma = %f\n" cov2 vv2 (cov2 /. vv2) !sigma; Printf.printf "\n"; flush stdout; synchronize () end; let st = wait_next_event (if !button then [Button_up;Key_pressed;Mouse_motion] else [Button_down;Key_pressed]) in let handle_key = function | 'q' -> continue := false | 's' -> sigma := min 0.0 (!sigma -. 0.01) | 'S' -> sigma := !sigma +. 0.01 | 'R' -> failures := 0; total := 0 | ' ' -> freeze := false | _ -> () in button := st.button; if st.keypressed then begin handle_key st.key; while key_pressed () do handle_key (read_key ()) done; end; (* if st.button then a.(0) <- (g st.mouse_x, g st.mouse_y) *) done let _ = open_graph (Printf.sprintf " %dx%d" n m); Random.self_init (); run (); close_graph ()