bergsoe avatar bergsoe committed c657542

SO3 revision and new SE3.of_array.

- SE3.of_array utility for converting transformation matrices to
the internal rigid body transformation type.

- New SE.dist_pos and SE.dist_rot for convenient construction of metrics
for position or rotation only.

- Revision of SO3 after some much needed testing. [uniform_offset] had
large round-off errors for small offset angles. [dist] and [angle] were
returning values in the range [0] to [2 pi] instead of [0] to [pi].

Comments (0)

Files changed (2)

src/PaplTransform.ml

   let inverse q = conj q /: norm_sqr q
 
   let clamp x (a, b) eps =
-    if a <= x && x <= b then x
-    else
-      let da = a -. x in
-        if eps < da then a
-        else
-          let db = x -. b in
-            if eps < db then b
-            else
-              invalid_arg
-                "clamp: x is outside interval by more than eps"
+    let die () = invalid_arg
+      (Printf.sprintf
+         "clamp: x=%.16f is outside interval (%.16f, %.16f) by more than eps=%.16f"
+         x a b eps)
+    in
+      if x < a then
+        if a -. x < eps
+        then a
+        else die ()
+      else if b < x then
+        if x -. b < eps
+        then b
+        else die ()
+      else
+        x
 
-  let angle (a, _, _, _) =
-    2.0 *. acos (clamp a (-.1., 1.) 1e-12)
+  let get_angle a = 2.0 *. acos (clamp a (0., 1.) 1e-12)
 
-  let dist q0 q1 =
-    let ca = dot q0 q1 in
-      2.0 *. acos (clamp ca (-1., 1.) 1e-12)
+  let angle (a, _, _, _) = get_angle (abs_float a)
+
+  let dist q0 q1 = get_angle (abs_float (dot q0 q1))
 
   let angle_axis ((a, b, c, d) as q) =
     let len = V3D.norm2 (b, c, d) in
       let eps = 1e-7 in
         if is_zero (1.0 -. ca) eps then lerp q0 q1
         else
-          let a = acos (clamp ca (-1., 1.) 1e-10) in
+          let a = acos (clamp ca (0., 1.) 1e-10) in
             fun s ->
               let f1 = sin ((1. -. s) *. a) in
               let f2 = sin (s *. a) in
       A random unit quaternion.
 
       See Section 5.2.2 of Steven LaValle's Planning Algorithms.
-      http://planning.cs.uiuc.edu/node198.html 
+      http://planning.cs.uiuc.edu/node198.html
     *)
     let get_uniform_all ?rng =
       let float_fun = PaplRandom.float ?rng in
          compute a seed to Newton's method. *)
       let approx_inv_g r = sqrt (pi *. pi *. r) in
       (* The maximum error allowed. *)
-      let eps = 1e-5 in
+      let eps = 1e-6 in
       (* Iterate Newton's method until the error is below the threshold. *)
       let rec loop a =
         let e = g a in
        uniformly distributed within the region constrained by the [offset]
        value. *)
     let uniform_angle_within_offset ?rng =
+      let small_offset = pi /. 50. in
       let float_fun = PaplRandom.float ?rng in
         fun offset ->
-          let r = angle_to_frequency offset in
-            frequency_to_angle (float_fun r)
+          if offset < small_offset then
+            (* Approximate formula for when the surface is approximately a
+               disk. See e.g.
+               http://mathworld.wolfram.com/DiskPointPicking.html
+               or derive for yourself that taking the square root gives the
+               suitable scaling for the sampling of a disk.
+            *)
+            sqrt (float_fun 1.) *. offset
+          else
+            let r = angle_to_frequency offset in
+              frequency_to_angle (float_fun r)
 
     let get_uniform_offset ?rng =
       let get_angle = uniform_angle_within_offset ?rng in
       let dp = V.dist2 p0 p1 in
       let dr = Misc.dist r0 r1 in
         Pervasives.max (wp *. dp) (wo *. dr)
+
+  let dist_pos dist = (); fun (x0, _) (x1, _) -> dist x0 x1
+  let dist_rot dist = (); fun (_, x0) (_, x1) -> dist x0 x1
 end
 
 module type SE_METRIC = sig
   type t
+  type vec
+  type rot
   val dist1 : float -> float -> t PaplMetric.t
   val dist2_sqr : float -> float -> t PaplMetric.t
   val dist2 : float -> float -> t PaplMetric.t
   val dist_inf : float -> float -> t PaplMetric.t
+  val dist_pos : vec PaplMetric.t -> t PaplMetric.t
+  val dist_rot : rot PaplMetric.t -> t PaplMetric.t
 end
 
 module type SE = sig
   type rot
 
   include PaplSpatialGroup.S with type t := t and type vec := vec
-  include SE_METRIC with type t := t
+  include SE_METRIC with type t := t and type vec := vec and type rot := rot
 
   val make : vec -> rot -> t
   val make_rotate : rot -> t
          xy + zw     ; 1. - xx - zz; yz - xw     ; p1;
          xz - yw     ; yz + xw     ; 1. - xx - yy; p2 |]
 
+  let of_array xs =
+    match xs with
+        [| r00; r01; r02; px;
+           r10; r11; r12; py;
+           r20; r21; r22; pz |] ->
+          let trace = r00 +. r11 +. r22 in
+          let rot =
+            if trace > 0. then
+              let r = sqrt (trace +. 1.) in
+              let s = 0.5 /. r in
+                (0.5 *. r,
+                 (r21 -. r12) *. s,
+                 (r02 -. r20) *. s,
+                 (r10 -. r01) *. s)
+            else if r00 > r11 && r00 > r22 then
+              let r = sqrt (r00 -. r11 -. r22 +. 1.) in
+              let s = 0.5 /. r in
+                ((r21 -. r12) *. s,
+                 0.5 *. r,
+                 (r01 +. r10) *. s,
+                 (r02 +. r20) *. s)
+            else if r11 > r22 then
+              let r = sqrt (1. +. r11 -. r00 -. r22) in
+              let s = 0.5 /. r in
+                ((r02 -. r20) *. s,
+                 (r01 +. r10) *. s,
+                 0.5 *. r,
+                 (r12 +. r21) *. s)
+            else
+              let r = sqrt (1. +. r22 -. r00 -. r11) in
+              let s = 0.5 /. r in
+                ((r10 -. r01) *. s,
+                 (r02 +. r20) *. s,
+                 (r12 +. r21) *. s,
+                 0.5 *. r)
+          in
+            (* Mathematically the scaling to a unit vector isn't needed, but it
+               may help guard against small round off errors in the matrix. *)
+            make (px, py, pz) (SO3.unit rot)
+      | _ -> invalid_arg "PaplTransform.SE3.of_array: 12 elements expected."
+
   module Sampler = struct
     type box_t = vec * vec
 

src/PaplTransform.mli

 (** Metrics for SE2 and SE3. *)
 module type SE_METRIC = sig
   type t
+  type vec
+  type rot
 
   val dist1 : float -> float -> t PaplMetric.t
     (** Manhattan distance.
 
         The weights must be non-negative.
     *)
+
+  val dist_pos : vec PaplMetric.t -> t PaplMetric.t
+  val dist_rot : rot PaplMetric.t -> t PaplMetric.t
 end
 
 (** Interface common to SE2 and SE3. *)
   (** Rotation type. *)
 
   include PaplSpatialGroup.S with type t := t and type vec := vec
-  include SE_METRIC with type t := t
+  include SE_METRIC with type t := t and type vec := vec and type rot := rot
 
   val make : vec -> rot -> t
   (** Construct a transformation from a translation and a rotation. *)
       The layout of the array is
 
       [\[| r11; r12; r13; px;
-          r21; r22; r23; py;
-          r31; r32; r33; pz |\]]
+           r21; r22; r23; py;
+           r31; r32; r33; pz |\]]
+
+      where the [rij] is the rotation matrix representation of the orientation
+      and [(px, py, pz)] is the translation vector.
+  *)
+
+  val of_array : float array -> t
+  (** Convert a 12 element array to a transformation.
+
+      The layout of the array is
+
+      [\[| r11; r12; r13; px;
+           r21; r22; r23; py;
+           r31; r32; r33; pz |\]]
 
       where the [rij] is the rotation matrix representation of the orientation
       and [(px, py, pz)] is the translation vector.
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.