Commits

bergsoe  committed f0c3560

Uniform random random sampling of rotations.

The rotations can either be sampled among all possible rotations or
among the rotations for which the magnitude of the rotation is below
a given threshold.

  • Participants
  • Parent commits bb1f655

Comments (0)

Files changed (2)

File src/PaplTransform.ml

 
 type rng_t = PaplRandom.rng_t
 
+let pi = BatFloat.pi
+let pi_x_2 = 2. *. pi
+
 module SO2 = struct
   module V2D = PaplVector.V2D
 
   type t = float
   type vec = V2D.t
 
-  let pi = acos(-1.)
-  let pi_x_2 = 2. *. pi
-
   let rotate a = a
 
   let step a b = mod_float (b -. a) pi_x_2
   type t = V4D.t
   type vec = PaplVector.V3D.t
 
-  let pi = 4.0 *. atan 1.
-
   let is_zero x eps = abs_float x < eps
 
   let dot = V4D.dot
         slerp (-. ca) q0 (neg q1)
       else
         slerp ca q0 q1
+
+  module Sampler = struct
+
+    (*
+      A random unit quaternion.
+
+      See Section 5.2.2 of Steven LaValle's Planning Algorithms.
+      http://planning.cs.uiuc.edu/node198.html 
+    *)
+    let get_uniform_all ?rng =
+      let float_fun = PaplRandom.float ?rng in
+      let r () = float_fun 1.0 in
+        fun () ->
+          let open BatFloat in
+          let u1, u2, u3 = r (), r (), r () in
+          let a2 = pi_x_2 * u2 in
+          let a3 = pi_x_2 * u3 in
+          let s2 = sqrt (1. - u1) in
+          let s3 = sqrt u1 in
+            (s2 * sin a2, s2 * cos a2, s3 * sin a3, s3 * cos a3)
+
+    let uniform_all ?rng () =
+      BatEnum.from (get_uniform_all ?rng)
+
+    let inv_pi = 1. /. pi
+
+    (* [angle_to_frequency a] is the fraction of random rotations that will have
+       a magnitude less than or equal to [a]. *)
+    let angle_to_frequency a = inv_pi *. (a -. sin a)
+
+    (* The derivative of the [angle_to_frequency] function with respect to
+       [a]. *)
+    let angle_to_frequency_deriv a = inv_pi *. (1. -. cos a)
+
+    (* [frequency_to_angle r] finds the angle [a] for which a fraction [r] of
+       all random rotations will have a magnitude less than or equal to [a]. *)
+    let frequency_to_angle r =
+      (* We wish to solve [g r a = 0] for [a]. *)
+      let g a = angle_to_frequency a -. r in
+      (* The derivative of [g]. *)
+      let dg a = angle_to_frequency_deriv a in
+      (* A simple but OK approximation to the inverse of [g] that is used to
+         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
+      (* Iterate Newton's method until the error is below the threshold. *)
+      let rec loop a =
+        let e = g a in
+          if abs_float e <= eps then a
+          else loop (a -. e /. dg a)
+      in
+        (* By using a precomputed table instead of this approximate solution it
+           may be possibly to save 1-2 iterations of Newton's method, but I
+           don't think those little savings are worth the effort. *)
+        loop (approx_inv_g r)
+
+    (* [uniform_angle_within_offset offset] is an angle [a] of rotation in the
+       range [0, offset], 0 <= offset <= pi, randomly selected in such a way
+       that rotation matrices generated by rotation with this angle will be
+       uniformly distributed within the region constrained by the [offset]
+       value. *)
+    let uniform_angle_within_offset ?rng =
+      let float_fun = PaplRandom.float ?rng in
+        fun offset ->
+          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 get_axis = PaplVector.V3D.get_uniform_unit ?rng in
+        fun offset ->
+          let angle = get_angle offset in
+          let axis = get_axis () in
+            rotate_angle_axis angle axis
+
+    let uniform_offset ?rng =
+      let get = get_uniform_offset ?rng in
+        fun offset ->
+          BatEnum.from (fun () -> get offset)
+  end
 end
 
 module MakeSE
 
     let uniform ?rng box range =
       make
-        (PaplVector.V2D.Sampler.uniform ?rng box)
+        (PaplVector.V2D.uniform ?rng box)
         (SO2.Sampler.uniform ?rng range)
 
     let uniform_all ?rng box =
       make
-        (PaplVector.V2D.Sampler.uniform ?rng box)
+        (PaplVector.V2D.uniform ?rng box)
         (SO2.Sampler.uniform_all ?rng ())
   end
 end
       [| 1. - yy - zz; xy - zw     ; xz + yw     ; p0;
          xy + zw     ; 1. - xx - zz; yz - xw     ; p1;
          xz - yw     ; yz + xw     ; 1. - xx - yy; p2 |]
+
+  module Sampler = struct
+    type box_t = vec * vec
+
+    let make sp so = PaplSampler.product2 sp so
+
+    let uniform_offset ?rng box offset =
+      make
+        (PaplVector.V3D.uniform ?rng box)
+        (SO3.Sampler.uniform_offset ?rng offset)
+
+    let uniform_all ?rng box =
+      make
+        (PaplVector.V3D.uniform ?rng box)
+        (SO3.Sampler.uniform_all ?rng ())
+  end
 end

File src/PaplTransform.mli

 
         [dist a b] is equivalent to [angle (mul (inverse a) b)].
     *)
+
+  module Sampler : sig
+    val uniform_all : ?rng:rng_t -> unit -> t PaplSampler.t
+    (** Uniformly distributed random rotations.
+    *)
+
+    val get_uniform_all : ?rng:rng_t -> unit -> t
+    (** A single rotation selected uniformly at random. *)
+
+    val uniform_offset : ?rng:rng_t -> float -> t PaplSampler.t
+    (** [get_uniform_offset offset] is a stream of rotations selected uniformly
+        at random among the rotations with magnitude less than or equal to
+        [offset].
+
+        [offset] must be in the range [[0, pi)].
+    *)
+
+    val get_uniform_offset : ?rng:rng_t -> float -> t
+    (** [get_uniform_offset offset] is a single rotation selected uniformly at
+        random among the rotations with magnitude less than or equal to
+        [offset].
+
+        [offset] must be in the range [[0, pi)].
+    *)
+  end
 end
 
 module type METRIC = sig
 module SE3 : sig
   include SE with type vec = SO3.vec and type rot = SO3.t
 
+  module Sampler : sig
+    type box_t = vec * vec
+
+    val make : vec PaplSampler.t -> SO3.t PaplSampler.t -> t PaplSampler.t
+
+    val uniform_offset : ?rng:rng_t -> box_t -> float -> t PaplSampler.t
+    (** [uniform_offset box angle_offset] are random transformations with
+        positions sampled from [box] and rotations sampled such that their
+        magnitudes are the range [[0, angle_offset)].
+
+        Positions and rotations are sampled uniformly from within the given
+        regions.
+    *)
+
+    val uniform_all : ?rng:rng_t -> box_t -> t PaplSampler.t
+    (** [uniform_all box] are random transformations with positions sampled from
+        [box] and rotations sampled among all rotations.
+
+        Positions and rotations are sampled uniformly from within the given
+        regions.
+    *)
+  end
+
   val to_array : t -> float array
   (** Convert a transformation to 12 element array of floats.