[Krobot-commits] [SCM] krobot branch, master, updated. bc96bdd7d9d8342fe018fe76ff3bf9b5a6fdc3c7

[Pierre C. <Ba...@us...>]

This is an automated email from the git hooks/post-receive script. It was
generated because a ref change was pushed to the repository containing
the project "krobot".

The branch, master has been updated
       via  bc96bdd7d9d8342fe018fe76ff3bf9b5a6fdc3c7 (commit)
       via  e031a062d10774228d8fa5d70ac5ea54070646b3 (commit)
       via  ca86b4563865e89edcc47c6fc295a0ab016c60be (commit)
       via  47300d0fa1bc6b24cbd059b39e099913f29531ba (commit)
      from  8987afa29cc0e2829cdc4618b9e59386e0eb48fb (commit)

Those revisions listed above that are new to this repository have
not appeared on any other notification email; so we list those
revisions in full, below.

- Log -----------------------------------------------------------------
commit bc96bdd7d9d8342fe018fe76ff3bf9b5a6fdc3c7
Author: Pierre Chambart <pie...@oc...>
Date:   Wed May 8 01:13:00 2013 +0200

    invert transform

commit e031a062d10774228d8fa5d70ac5ea54070646b3
Author: Pierre Chambart <pie...@oc...>
Date:   Wed May 8 00:16:50 2013 +0200

    Function to filter out borders from urg data

commit ca86b4563865e89edcc47c6fc295a0ab016c60be
Author: Pierre Chambart <pie...@oc...>
Date:   Tue May 7 23:54:40 2013 +0200

    icp registration: new dependency lacaml

commit 47300d0fa1bc6b24cbd059b39e099913f29531ba
Author: Pierre Chambart <pie...@oc...>
Date:   Mon May 6 18:09:45 2013 +0200

    Urg message update display.

-----------------------------------------------------------------------

Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index 94c60bf..85d5e9e 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -74,6 +74,18 @@ Library "krobot-can"
    Krobot_can_desc_parser
   CSources: can_stubs.c
 
+Library "krobot-icp"
+  FindlibName: icp
+  FindlibParent: krobot
+  BuildDepends: lacaml
+  XMETADescription: 2d Point cloud registration
+  XMETARequires: lacaml
+  Path: src/icp
+  Install: true
+  Modules:
+   Kd_tree,
+   Icp_minimisation,
+   Icp_utils
 # +-------------------------------------------------------------------+
 # | The driver                                                        |
 # +-------------------------------------------------------------------+
diff --git a/info/control2011/_tags b/info/control2011/_tags
index 8609920..4b773d9 100644
--- a/info/control2011/_tags
+++ b/info/control2011/_tags
@@ -1,5 +1,6 @@
 <**/*.ml{,i}>: syntax_camlp4o
 <src/interfaces/*.ml>: -syntax_camlp4o
+<src/icp/*.ml>: -syntax_camlp4o
 
 # OASIS_START
 # DO NOT EDIT (digest: 9a283fb86531fb8de178da3ad5f872b1)
diff --git a/info/control2011/src/icp/icp_minimisation.ml b/info/control2011/src/icp/icp_minimisation.ml
new file mode 100644
index 0000000..fa545ac
--- /dev/null
+++ b/info/control2011/src/icp/icp_minimisation.ml
@@ -0,0 +1,196 @@
+
+(************************************************
+   2d Point cloud registration: based on
+   'Robust Registration of 2D and 3D Point Sets'
+   by Andrew W. Fitzgibbon
+ ************************************************)
+
+open Kd_tree
+open Lacaml.D
+
+type a = { ath : float; ax : float; ay : float }
+type data = { dx : float array; dy : float array }
+
+let transform' dth dx dy ax ay =
+  let co = cos dth in
+  let si = sin dth in
+  let len = Array.length ax in
+  let x' = Array.init len (fun i -> ax.(i) *. co -. ay.(i) *. si +. dx) in
+  let y' = Array.init len (fun i -> ax.(i) *. si +. ay.(i) *. co +. dy) in
+  x', y'
+
+let transform { ath; ax; ay } { dx; dy } =
+  let dx, dy = transform' ath ax ay dx dy in
+  { dx; dy }
+
+let make_kd_tree { dx; dy } =
+  let a = Array.init (Array.length dx) (fun i -> i, { x = dx.(i); y = dy.(i) }) in
+  Kd_tree.make a
+
+type kernel = float -> float
+
+let closest_points kd_tree { dx; dy } =
+  Array.init (Array.length dx)
+    (fun i ->
+      let pos, _, sq_dist = Kd_tree.nearest_neighbor
+          { Kd_tree.x = dx.(i); Kd_tree.y = dy.(i) } kd_tree in
+      pos, sq_dist)
+
+let distance_transform (kernel:float -> float) kd_tree a data =
+  let { dx = datax'; dy = datay' } = transform a data in
+  Array.init (Array.length datax')
+    (fun i ->
+      let _, _, sq_dist = nearest_neighbor { x = datax'.(i); y = datay'.(i) } kd_tree in
+      kernel sq_dist)
+
+let epsilon = 1e-15
+
+let jacobian ?(kernel=fun i -> i) kd_tree a data =
+  let v_base = distance_transform kernel kd_tree a data in
+  let a_dth = { a with ath = a.ath +. epsilon } in
+  let a_dx = { a with ax = a.ax +. epsilon } in
+  let a_dy = { a with ay = a.ay +. epsilon } in
+  let diff a =
+    let v_diff = distance_transform kernel kd_tree a data in
+    Array.mapi (fun i v -> (v -. v_base.(i)) /. epsilon) v_diff
+  in
+  let v_dth = diff a_dth in
+  let v_dx = diff a_dx in
+  let v_dy = diff a_dy in
+  v_base, [|v_dth; v_dx; v_dy|]
+
+
+open Lacaml.D
+
+let mat_sum m1 m2 =
+  assert(Mat.dim1 m1 = Mat.dim1 m2);
+  assert(Mat.dim2 m1 = Mat.dim2 m2);
+  let v1 = Mat.to_col_vecs m1 in
+  let v2 = Mat.to_col_vecs m2 in
+  Mat.of_col_vecs
+    (Array.init (Array.length v1) (fun i -> Vec.add v1.(i) v2.(i)))
+
+let mat_sub m1 m2 =
+  assert(Mat.dim1 m1 = Mat.dim1 m2);
+  assert(Mat.dim2 m1 = Mat.dim2 m2);
+  let v1 = Mat.to_col_vecs m1 in
+  let v2 = Mat.to_col_vecs m2 in
+  Mat.of_col_vecs
+    (Array.init (Array.length v1) (fun i -> Vec.add v1.(i) v2.(i)))
+
+let lid n l =
+  let m = Mat.identity n in
+  Mat.scal l m;
+  m
+
+(* gradiant descent step *)
+
+let step ?kernel ?(lambda=1.) kd data a =
+  let e',j' = jacobian ?kernel kd a data in
+  let e = Vec.of_array e' in
+  let jt = Mat.of_array j' in
+  let j = Mat.transpose jt in
+  let size = Lacaml.D.Mat.dim2 j in
+
+  let t1' = gemm jt j in
+  let t1'' = lid size lambda in
+  let t1 = mat_sum t1' t1'' in
+  getri t1;
+  let t2 = gemm t1 jt in
+  let update = gemv t2 e in
+  let a = Vec.of_array [|a.ath; a.ax; a.ay|] in
+  let a = Vec.sub a update in
+  let a = Vec.to_array a in
+  { ath = a.(0); ax = a.(1); ay = a.(2) }
+
+(* simpler evolution step, equivalent to [step] with [lambda = 0.] *)
+
+let simple_step ?kernel kd data a =
+  let e',j' = jacobian ?kernel kd a data in
+  let e = Vec.of_array e' in
+  let jt = Mat.of_array j' in
+  let j = Mat.transpose jt in
+
+  let t1 = gemm jt j in
+  getri t1;
+  let t2 = gemm t1 jt in
+  let update = gemv t2 e in
+  let a = Vec.of_array [|a.ath; a.ax; a.ay|] in
+  let a = Vec.sub a update in
+  let a = Vec.to_array a in
+  { ath = a.(0); ax = a.(1); ay = a.(2) }
+
+(***** default kernel ****)
+
+let sigma = 0.05
+let sigma2 = sigma*.sigma
+let default_kernel x =
+  if x < sigma2
+  then x
+  else 2. *. sigma *. (sqrt x) -. sigma2
+
+
+
+(***** data filtering *****)
+
+(* filtering multiple point associated to the same value: if more than
+   one point closest to a point [p], keep only the closest one *)
+
+let closest_match ai =
+  let n = Array.fold_left (fun m (n,_) -> max m n) (-1) ai in
+  let matcher = Array.init (n+1) (fun _ -> None) in
+  Array.iteri (fun index (matched,(d:float)) ->
+    match matcher.(matched) with
+    | None -> matcher.(matched) <- Some (index,d)
+    | Some (_,d1) ->
+      if d < d1 then matcher.(matched) <- Some (index,d)) ai;
+  matcher
+
+let get_closest matcher ((ax',ay'):float array * float array) =
+  let r = ref [] in
+  Array.iter (function
+    | None -> ()
+    | Some (i,_) ->
+      r := (ax'.(i), ay'.(i)) :: !r) matcher;
+  let x,y = List.split !r in
+  Array.of_list x, Array.of_list y
+
+let closest_filter kd a d2 =
+  let d' = transform a d2 in
+  let cp = closest_points kd d' in
+  let cm = closest_match cp in
+  let dx, dy = get_closest cm (d2.dx,d2.dy) in
+  { dx; dy }
+
+(* filter point farther than 3 time the median distance *)
+
+let far_filter kd a d2 =
+  let d' = transform a d2 in
+  let cp = closest_points kd d' in
+  let cp = Array.mapi (fun i (_,d) -> i,d) cp in
+  Array.sort (fun (_,d1) (_,d2) -> compare d1 d2) cp;
+  let (_,median) = cp.(Array.length cp/2) in
+  let cl = Array.to_list cp in
+  let too_far (i,d) = d <= median *. 9. in
+  let ai = Array.of_list (List.filter too_far cl) in
+  { dx = Array.map (fun (i,_) -> d2.dx.(i)) ai;
+    dy = Array.map (fun (i,_) -> d2.dy.(i)) ai }
+
+
+(*************************)
+
+let rec register_simple ?kernel kd data a n =
+  if n = 0 then a
+  else
+    let data' = closest_filter kd a data in
+    let data' = far_filter kd a data' in
+    let a = simple_step ?kernel kd data' a in
+    register_simple ?kernel kd data a (n-1)
+
+let rec register ?kernel ?lambda kd data a n =
+  if n = 0 then a
+  else
+    let data' = closest_filter kd a data in
+    let data' = far_filter kd a data' in
+    let a = step ?kernel ?lambda kd data' a in
+    register ?kernel ?lambda kd data a (n-1)
diff --git a/info/control2011/src/icp/icp_minimisation.mli b/info/control2011/src/icp/icp_minimisation.mli
new file mode 100644
index 0000000..88aa12e
--- /dev/null
+++ b/info/control2011/src/icp/icp_minimisation.mli
@@ -0,0 +1,32 @@
+
+type a = { ath : float; ax : float; ay : float; }
+(** representing rigid transformation: rotation by [ath] then
+    translation by [ax,ay] *)
+
+type data = { dx : float array; dy : float array; }
+
+type kernel = float -> float
+
+val transform : a -> data -> data
+(** apply transformation to data *)
+
+val make_kd_tree : data -> int Kd_tree.t
+(** create a kd tree containting index in the data *)
+
+val distance_transform : kernel -> 'a Kd_tree.t -> a -> data -> float array
+(** [distance_transform kernel kd a data] Calculates the square of the
+   distance for each point of [data] after transformation to
+   the closest point in the kd tree. the kernel is applied to the
+   result after *)
+
+val default_kernel : kernel
+(** Huber kernel *)
+
+(**********)
+
+val register_simple : ?kernel:kernel -> int Kd_tree.t -> data -> a -> int -> a
+(** Gauss-Newton gradient descent *)
+
+val register : ?kernel:kernel -> ?lambda:Lacaml.D.num_type ->
+  int Kd_tree.t -> data -> a -> int -> a
+(** Levenberg-Marquardt gradient descent *)
diff --git a/info/control2011/src/icp/icp_utils.ml b/info/control2011/src/icp/icp_utils.ml
new file mode 100644
index 0000000..a961e5e
--- /dev/null
+++ b/info/control2011/src/icp/icp_utils.ml
@@ -0,0 +1,98 @@
+open Icp_minimisation
+
+(**** table ****)
+
+let linear a0 an n =
+  let dx = (an -. a0) /. (float (n-1)) in
+  let p n = a0 +. (float n) *. dx in
+  Array.init n p
+
+let line (x0,y0) (xn,yn) n =
+  linear x0 xn n, linear y0 yn n
+
+let shuffle l =
+  let rec split (ac1,ac2) = function
+    | [] -> ac1,ac2
+    | [t] -> t::ac1,ac2
+    | t1::t2::q -> split (t1::ac1,t2::ac2) q in
+  let rec merge acc l1 l2 = match l1,l2 with
+    | [],l | l, [] -> l@acc
+    | t1::q1, t2::q2 -> merge (t1::t2::acc) q1 q2 in
+  let l1,l2 = split ([],[]) l in
+  merge [] l1 (List.rev l2)
+
+let table ~width ~length n =
+  let x1,y1 = line (0.,0.) (length,0.) n in
+  let x2,y2 = line (length,0.) (length,width) n in
+  let x3,y3 = line (length,width) (0.,width) n in
+  let x4,y4 = line (0.,width) (0.,0.) n in
+  let x = Array.concat [x1;x2;x3;x4] in
+  let y = Array.concat [y1;y2;y3;y4] in
+  let f a = Array.of_list (shuffle (shuffle (Array.to_list a))) in
+  { dx = f x; dy = f y }
+
+
+
+(**** dump loading ****)
+
+let load_float ic =
+  Scanf.bscanf ic " %f " (fun f -> f)
+
+let load_float_cpl ic =
+  Scanf.bscanf ic " %f %f " (fun f1 f2 -> f1, f2)
+
+let load_line ic =
+  let line = Pervasives.input_line ic in
+  let ic = Scanf.Scanning.from_string line in
+  let ts = load_float ic in
+  let rec aux () =
+    let c = try Some (load_float_cpl ic) with _ -> None in
+    match c with
+    | Some c -> c :: aux ()
+    | None -> []
+  in
+  ts, aux ()
+
+let load_file' f =
+  let ic = open_in f in
+  let rec aux () =
+    let l = try Some (load_line ic) with _ -> None in
+    match l with
+    | Some l -> l::aux ()
+    | None -> []
+  in
+  aux ()
+
+let filter_dist min max (x,y) =
+  let d = sqrt (x*.x +. y*.y) in
+  d >= min && d <= max
+
+let load_file ?(min_dist=0.15) ?(max_dist=6.) f =
+  let l = load_file' f in
+  let l' =
+    List.map (fun (ts,v) ->
+      let v = List.filter (filter_dist min_dist max_dist) v in
+      let x,y = List.split v in
+      ts, { dx = Array.of_list x; dy = Array.of_list y }) l in
+  Array.of_list l'
+
+
+(**** filtering ****)
+
+let far_enougth_filter kd a min_dist data =
+  let dist = distance_transform (fun i -> i) kd a data in
+  let dist = Array.mapi (fun i d -> i,d) dist in
+  let distl = Array.to_list dist in
+  let min_dist2 = min_dist *. min_dist in
+  let far_enougth (i,d) = d >= min_dist2 in
+  let ai = Array.of_list (List.filter far_enougth distl) in
+  { dx = Array.map (fun (i,_) -> data.dx.(i)) ai;
+    dy = Array.map (fun (i,_) -> data.dy.(i)) ai }
+
+let invert_transform a =
+  let co = cos (-. a.ath) in
+  let si = sin (-. a.ath) in
+  let x' = a.ax *. co -. a.ay *. si in
+  let y' = a.ax *. si +. a.ay *. co in
+  { ath = -. a.ath; ax = -. x'; ay = -. y'}
+
diff --git a/info/control2011/src/icp/icp_utils.mli b/info/control2011/src/icp/icp_utils.mli
new file mode 100644
index 0000000..1e23bba
--- /dev/null
+++ b/info/control2011/src/icp/icp_utils.mli
@@ -0,0 +1,16 @@
+open Icp_minimisation
+
+val table : width:float -> length:float -> int -> data
+
+val load_file : ?min_dist:float -> ?max_dist:float -> string ->
+  (float * data) array
+(** load a dump file in the format dumpped by [krobot_urg -listen] *)
+
+
+(* filtering *)
+
+val far_enougth_filter : 'a Kd_tree.t -> a -> float -> data -> data
+(** [far_enougth_filter kd a min_dist data] filter out values of
+   [data] that are closer than [min_dist] to a vertex of kd *)
+
+val invert_transform : a -> a
diff --git a/info/control2011/src/icp/kd_tree.ml b/info/control2011/src/icp/kd_tree.ml
new file mode 100644
index 0000000..45601d4
--- /dev/null
+++ b/info/control2011/src/icp/kd_tree.ml
@@ -0,0 +1,110 @@
+
+type direction =
+  | Vert
+  | Hori
+
+type vertice = { x : float; y : float }
+
+type 'a t =
+  | Split of direction * 'a * vertice * 'a t * 'a t
+  | Leaf of 'a * vertice
+  | Empty
+
+let splitn n l =
+  let rec aux n l acc =
+    if n = 0
+    then List.rev acc, l
+    else match l with
+      | [] -> assert false
+      | t::q -> aux (n-1) q (t::acc) in
+  aux n l []
+
+let rec make_horiz a =
+  match Array.length a with
+  | 0 -> Empty
+  | 1 ->
+    let va, vert = a.(0) in
+    Leaf (va, vert)
+  | len ->
+    Array.sort (fun (_,{ x = x1 }) (_,{ x = x2 }) -> compare x1 x2 ) a;
+    let median = (len / 2) in
+    let med_val, median_elt = a.(median) in
+    let left = Array.sub a 0 median in
+    let right = Array.sub a (median + 1) (len - median - 1) in
+    let l1 = make_vert left in
+    let l2 = make_vert right in
+    Split (Hori, med_val, median_elt, l1, l2)
+
+and make_vert a =
+  match Array.length a with
+  | 0 -> Empty
+  | 1 ->
+    let va, vert = a.(0) in
+    Leaf (va, vert)
+  | len ->
+    Array.sort (fun (_,{ y = y1 }) (_,{ y = y2 }) -> compare y1 y2 ) a;
+    let median = (len / 2) in
+    let med_val, median_elt = a.(median) in
+    let left = Array.sub a 0 median in
+    let right = Array.sub a (median + 1) (len - median - 1) in
+    let l1 = make_vert left in
+    let l2 = make_vert right in
+    Split (Vert, med_val, median_elt, l1, l2)
+
+let make a = make_horiz a
+
+let sq_dist v1 v2 =
+  let dx = v1.x -. v2.x in
+  let dy = v1.y -. v2.y in
+  dx *. dx +. dy *. dy
+
+let sq x = x *. x
+
+let rec nearest_neighbor' v ((cur_val,cur,cur_dist) as curr) t = match t with
+  | Empty -> curr
+  | Leaf (va,p) ->
+    let dist = sq_dist v p in
+    if dist < cur_dist
+    then va, p, dist
+    else curr
+  | Split (dir, med_val, median_elt, left, right) ->
+    let split_distance = match dir with
+      | Hori -> sq (v.x -. median_elt.x)
+      | Vert -> sq (v.y -. median_elt.y) in
+    let (_, _, cur_dist) as curr =
+      let dist = sq_dist v median_elt in
+      if dist < cur_dist
+      then med_val, median_elt, dist
+      else curr in
+    let cmp = match dir with
+      | Hori -> compare v.x median_elt.x
+      | Vert -> compare v.y median_elt.y in
+    if cmp < 0
+    then
+      let (_, _, cur_dist) as curr = nearest_neighbor' v curr left in
+      if split_distance >= cur_dist
+      then curr
+      else nearest_neighbor' v curr right
+    else
+      let (_, _, cur_dist) as curr = nearest_neighbor' v curr right in
+      if split_distance >= cur_dist
+      then curr
+      else nearest_neighbor' v curr left
+
+let nearest_neighbor v = function
+  | Empty -> failwith "nearest_neighbor: empty"
+  | Leaf (va, p) -> va, p, sq_dist p v
+  | Split (_, med_val, median_elt, _, _) as t ->
+    let curr = (med_val, median_elt, sq_dist median_elt v) in
+    nearest_neighbor' v curr t
+
+let rec depth = function
+  | Split (_, _, _, t1,t2 ) ->
+    1 + max (depth t1) (depth t2)
+  | Leaf _ -> 1
+  | Empty -> 0
+
+(* let a = [|{x = 1.; y = 2.}; {x = 3.; y = 4.}; {x = 2.; y = 2.}; {x = 3.; y = 1.}; {x = 4.; y = 2.}|] *)
+(* let a' = Array.mapi (fun i v -> i,v) a *)
+(* let t = make a' *)
+(* let (i,r,d) = nearest_neighbor { x = 3.; y = 2.5 } t *)
diff --git a/info/control2011/src/icp/kd_tree.mli b/info/control2011/src/icp/kd_tree.mli
new file mode 100644
index 0000000..947b1ad
--- /dev/null
+++ b/info/control2011/src/icp/kd_tree.mli
@@ -0,0 +1,14 @@
+
+type 'a t
+
+type vertice = { x : float; y : float; }
+
+val make : ('a * vertice) array -> 'a t
+
+val nearest_neighbor : vertice -> 'a t -> 'a * vertice * float
+(** [nearest_neighbor v kd] finds the closest vertice [val] of
+   [v]. returns [val,vert,dist_sqr], with [val] the value associated
+   to [vert] and [dist_sqr] the square of the distance between [v] and
+   [vert] *)
+
+val depth : 'a t -> int
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 4492bab..6e01962 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -665,7 +665,8 @@ let handle_message viewer (timestamp, message) =
       queue_draw viewer
 
     | Urg dist ->
-      viewer.urg <- project_urg viewer dist
+      viewer.urg <- project_urg viewer dist;
+      queue_draw viewer
 
     | Urg_lines lines ->
       viewer.urg_lines <- project_urg_lines viewer lines


hooks/post-receive
-- 
krobot