From: Pierre C. <Ba...@us...> - 2012-05-08 21:15:59
|
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 2f4db51988fbdeabce5d24892650c2db962d73bb (commit) via e6ba7806e1a53d4bde4ea704f6b6aa20cd35859c (commit) via a1bf36f03c3dc667452b5e8ccc2155b2a6b3ab26 (commit) from b64fde9d0632479d027d3ebba220d7de086b645a (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 2f4db51988fbdeabce5d24892650c2db962d73bb Author: chicco <cha...@cr...> Date: Tue May 8 23:14:31 2012 +0200 [camlcv] some more commit e6ba7806e1a53d4bde4ea704f6b6aa20cd35859c Author: chicco <cha...@cr...> Date: Sat May 5 18:25:00 2012 +0200 [camlcv] Update camlcv commit a1bf36f03c3dc667452b5e8ccc2155b2a6b3ab26 Author: chicco <cha...@cr...> Date: Sat May 5 15:55:27 2012 +0200 [krobot_ax12_clean] tool to clean ax12 sequences, krobot_ax12_control can play sequances ----------------------------------------------------------------------- Changes: diff --git a/info/control2011/_oasis b/info/control2011/_oasis index 7409387..587f26e 100644 --- a/info/control2011/_oasis +++ b/info/control2011/_oasis @@ -234,6 +234,12 @@ Executable "krobot-dump-ax12" MainIs: krobot_dump_ax12.ml BuildDepends: krobot.can, lwt.syntax +Executable "krobot-ax12-clean" + Path: src/tools + Install: true + CompiledObject: best + MainIs: krobot_ax12_clean.ml + # +-------------------------------------------------------------------+ # | Examples | # +-------------------------------------------------------------------+ diff --git a/info/control2011/_tags b/info/control2011/_tags index 3049ac4..8e8eb2b 100644 --- a/info/control2011/_tags +++ b/info/control2011/_tags @@ -2,7 +2,7 @@ <src/interfaces/*.ml>: -syntax_camlp4o # OASIS_START -# DO NOT EDIT (digest: 77bb0a0cf22171b21400c7924cce86fe) +# DO NOT EDIT (digest: c7b4424d285a5d14617154af42598aa7) # Ignore VCS directories, you can use the same kind of rule outside # OASIS_START/STOP if you want to exclude directories that contains # useless stuff for the build process @@ -110,6 +110,7 @@ <src/tools/krobot_hil_simulator.{native,byte}>: pkg_lwt.unix <src/tools/krobot_hil_simulator.{native,byte}>: pkg_lwt.syntax <src/tools/krobot_hil_simulator.{native,byte}>: pkg_lwt.react +# Executable krobot-ax12-clean # Executable krobot-hub <src/tools/krobot_hub.{native,byte}>: pkg_lwt.unix <src/tools/krobot_hub.{native,byte}>: pkg_lwt.syntax diff --git a/info/control2011/setup.ml b/info/control2011/setup.ml index ef14fdf..a2b32ae 100644 --- a/info/control2011/setup.ml +++ b/info/control2011/setup.ml @@ -1,7 +1,7 @@ (* setup.ml generated for the first time by OASIS v0.2.0~alpha1 *) (* OASIS_START *) -(* DO NOT EDIT (digest: 3f1f69c4008e4511460ad8227132b513) *) +(* DO NOT EDIT (digest: 8dd92996e901c6afd5668c8d5fed679a) *) (* Regenerated by OASIS v0.3.0~rc3 Visit http://oasis.forge.ocamlcore.org for more information and @@ -5724,6 +5724,32 @@ let setup_t = }); Executable ({ + cs_name = "krobot-ax12-clean"; + cs_data = PropList.Data.create (); + cs_plugin_data = []; + }, + { + bs_build = [(OASISExpr.EBool true, true)]; + bs_install = [(OASISExpr.EBool true, true)]; + bs_path = "src/tools"; + bs_compiled_object = Best; + bs_build_depends = []; + bs_build_tools = [ExternalTool "ocamlbuild"]; + bs_c_sources = []; + bs_data_files = []; + bs_ccopt = [(OASISExpr.EBool true, [])]; + bs_cclib = [(OASISExpr.EBool true, [])]; + bs_dlllib = [(OASISExpr.EBool true, [])]; + bs_dllpath = [(OASISExpr.EBool true, [])]; + bs_byteopt = [(OASISExpr.EBool true, [])]; + bs_nativeopt = [(OASISExpr.EBool true, [])]; + }, + { + exec_custom = false; + exec_main_is = "krobot_ax12_clean.ml"; + }); + Executable + ({ cs_name = "krobot-hub"; cs_data = PropList.Data.create (); cs_plugin_data = []; @@ -6165,13 +6191,13 @@ let setup_t = }; oasis_fn = Some "_oasis"; oasis_version = "0.3.0~rc3"; - oasis_digest = Some "q\1674\179\017\0115F\190\024gsXI=$"; + oasis_digest = Some "[\188\237\193\021xw\196\204\204<Y\157o\194F"; oasis_exec = None; oasis_setup_args = []; };; let setup () = BaseSetup.setup setup_t;; -# 6176 "setup.ml" +# 6202 "setup.ml" (* OASIS_STOP *) let () = setup ();; diff --git a/info/control2011/src/tools/krobot_ax12_clean.ml b/info/control2011/src/tools/krobot_ax12_clean.ml new file mode 100644 index 0000000..62b5030 --- /dev/null +++ b/info/control2011/src/tools/krobot_ax12_clean.ml @@ -0,0 +1,105 @@ +open Arg + +type ax12_info = + { id : int; + pos : float; + speed : float; + torque : float; + time : float } + +let fscan_info ic = + Scanf.fscanf ic "id %i,pos %f, speed %f, torque %f, time %f\n" + (fun id pos speed torque time -> { id; pos; speed; torque; time; }) + +let read ic = + let rec aux acc = + try + let v = fscan_info ic in + aux (v::acc) + with + | End_of_file + | Scanf.Scan_failure _ -> List.rev acc + in + aux [] + +module IntMap = Map.Make(struct type t = int let compare = compare end) + +let separate l = + let f map v = + if IntMap.mem v.id map + then let r = IntMap.find v.id map in + r := v :: !r; + map + else IntMap.add v.id (ref [v]) map in + IntMap.map (fun r -> List.rev !r) (List.fold_left f IntMap.empty l) + +let soft window l = + let rec aux (l,v_win) v = + let v_win = List.filter (fun t -> v.time -. t.time <= window) l in + let v_win = v::v_win in + let mean = (List.fold_left (fun s v -> s +. v.pos) 0. v_win) /. float (List.length v_win) in + { v with pos = mean }::l, v_win in + List.rev (fst (List.fold_left aux ([],[]) l)) + +let remove_useless remove_below l = + let rec aux prev acc = function + | [] -> List.rev acc + | t::q -> + if abs_float (t.pos -. prev.pos) <= remove_below + then aux prev acc q + else aux t (t::acc) q + in + aux (List.hd l) [List.hd l] (List.tl l) + +let add_speed speed_coef l = + let rec aux prev acc = function + | [] -> List.rev acc + | t::q -> + let dt = t.time -. prev.time in + let dpos = t.pos -. prev.pos in + let speed = (dpos /. dt) *. speed_coef in + if abs_float speed > 200. + then aux prev acc q + else aux t ({ t with speed }::acc) q + in + aux (List.hd l) [List.hd l] (List.tl l) + +let window = 0.1 +let remove_below = 1. +let speed_coef = 1. (* ratio ax12 speed -> rad /. second *) + +let prepare l = + let l = List.map (fun v -> { v with speed = 0.; torque = 0. }) l in + let l = List.sort (fun v1 v2 -> compare v1.time v2.time) l in + let first_time = (List.hd l).time in + let l = List.map (fun v -> { v with time = v.time -. first_time }) l in + let map = separate l in + let map = IntMap.map (soft window) map in + let map = IntMap.map (remove_useless remove_below) map in + let map = IntMap.map (add_speed speed_coef) map in + let l = List.flatten (IntMap.fold (fun _ l acc -> l::acc) map []) in + let l = List.sort (fun v1 v2 -> compare v1.time v2.time) l in + l + +let map_speed l c = + let f v = { v with speed = v.speed *. c; time = v.time /. c } in + List.map f l + +let print_info { id; pos; speed; torque; time } = + Printf.printf "id %i,pos %f, speed %f, torque %f, time %f\n%!" id pos speed torque time + +let file = ref None +let speed = ref 1. + +let spec = ["-s", Set_float speed, "speed";] +let msg = "clean ax12 command sequence" +let message _ = usage spec msg; flush stdout; exit 0 +let () = Arg.parse spec (fun s -> file := Some s) msg +let () = + match !file with + | None -> message () + | Some f -> + let l = read (open_in f) in + let l2 = prepare l in + let l3 = map_speed l2 !speed in + List.iter print_info l3 diff --git a/info/control2011/src/tools/krobot_ax12_control.ml b/info/control2011/src/tools/krobot_ax12_control.ml index b281c36..853ba44 100644 --- a/info/control2011/src/tools/krobot_ax12_control.ml +++ b/info/control2011/src/tools/krobot_ax12_control.ml @@ -6,6 +6,8 @@ type action = | Sleep of float | Do of Krobot_message.t +let run_file = ref None + let ax12_id = ref 0 let ax12_speed = ref 1 let commands = ref [] @@ -24,11 +26,13 @@ let spec = "-t", Bool add_torque, "-torque"; "-state", Unit add_request, "request the ax12 state"; "-sleep", Float add_sleep, "sleep"; + "-file", String (fun s -> run_file := Some s), "run sequence from file"; ] + let msg = "do things with ax12" let message _ = usage spec msg; flush stdout; exit 0 let () = Arg.parse spec message msg -let () = if !commands = [] then message () +let () = if !commands = [] && !run_file = None then message () lwt bus = Krobot_bus.get () let run = function | Sleep t -> Lwt_unix.sleep t @@ -40,4 +44,37 @@ let rec go = function lwt () = run t in go q -lwt () = go (List.rev !commands) +type ax12_info = + { id : int; + pos : float; + speed : float; + torque : float; + time : float } + +let fscan_info ic = + Scanf.fscanf ic "id %i,pos %f, speed %f, torque %f, time %f\n" + (fun id pos speed torque time -> { id; pos; speed; torque; time; }) + +let read ic = + let rec aux acc = + try + let v = fscan_info ic in + aux (v::acc) + with + | End_of_file + | Scanf.Scan_failure _ -> List.rev acc + in + aux [] + +let to_actions l = + let f (t,l) v = + v.time, ( Do (Ax12_Goto (v.id, int_of_float v.pos, int_of_float v.speed)) ) :: (Sleep (v.time -. t)) :: l in + let _, l = List.fold_left f (0.,[]) l in + List.rev l + +lwt () = + match !run_file with + | None -> go (List.rev !commands) + | Some f -> + let actions = to_actions (read (open_in f)) in + go actions diff --git a/info/control2011/src/tools/krobot_vm.ml b/info/control2011/src/tools/krobot_vm.ml index d4dc507..a613692 100644 --- a/info/control2011/src/tools/krobot_vm.ml +++ b/info/control2011/src/tools/krobot_vm.ml @@ -198,6 +198,13 @@ type effect = | Send of Krobot_message.t (* Send a message. *) +let string_of_test = function + | `Eq -> "Eq" + | `Gt -> "Gt" + | `Ge -> "Ge" + | `Lt -> "Lt" + | `Le -> "Le" + (* [exec robot actions] searches for the first action to execute in a tree of actions and returns a new tree of actions and an effect. *) let rec exec robot actions = @@ -216,7 +223,8 @@ let rec exec robot actions = (actions, Wait) | Wait_for_moving state :: rest -> if robot.moving = state then - exec robot rest + (ignore (Lwt_log.info_f "Wait_for_moving %b done" state); + exec robot rest) else (actions, Wait) | Wait_for_odometry(test, value) :: rest -> @@ -226,7 +234,9 @@ let rec exec robot actions = | `Ge -> robot.curve_parameter >= value | `Lt -> robot.curve_parameter < value | `Le -> robot.curve_parameter <= value) then - exec robot rest + (ignore (Lwt_log.info_f "Wait_for_odometry %i %s %i done" + robot.curve_parameter (string_of_test test) value); + exec robot rest) else (actions, Wait) | Wait_for t :: rest -> @@ -252,6 +262,7 @@ let rec exec robot actions = exec robot rest end | Follow_path vertices :: rest -> begin + ignore (Lwt_log.info_f "Follow_path %i vertices" (List.length vertices)); (* Compute bezier curves. *) let vector = { vx = cos robot.orientation; vy = sin robot.orientation } in let curves = List.rev (Bezier.fold_vertices (fun sign p q r s acc -> (sign, p, q, r, s) :: acc) vector (robot.position :: vertices) []) in @@ -262,6 +273,7 @@ let rec exec robot actions = | [] -> [Wait_for_moving false] | [(sign, p, q, r, s)] -> + ignore (Lwt_log.info_f "add last %f %f" p.x p.y); [ (* Wait for the odometry to reach the middle of the current trajectory. *) @@ -278,6 +290,7 @@ let rec exec robot actions = Set_curve None; ] | (sign, p, q, r, s) :: rest -> + ignore (Lwt_log.info_f "add middle %f %f" p.x p.y); Wait_for_odometry(`Ge, 128) :: Bezier(sign, p, q, r, s, 0.5) :: Wait_for_odometry(`Lt, 128) @@ -291,15 +304,19 @@ let rec exec robot actions = exec robot (Node [ Set_curve(Some(Bezier.of_vertices p q r s)); Bezier(sign, p, q, r, s, 0.01); + Wait_for_odometry(`Le, 128); + Wait_for_odometry(`Ge, 128); Wait_for_moving false; Set_curve None; ] :: rest) | (sign, p, q, r, s) :: curves -> exec robot (Node(Set_curve(Some(Bezier.of_vertices p q r s)) :: Bezier(sign, p, q, r, s, 0.5) + :: Wait_for_odometry(`Le, 128) :: loop curves) :: rest) end | Bezier(sign, p, q, r, s, v_end) :: rest -> + ignore (Lwt_log.info "Bezier"); (* Compute parameters. *) let d1 = sign *. distance p q and d2 = distance r s in let v = vector r s in @@ -420,6 +437,10 @@ lwt () = (* Display all informative messages. *) Lwt_log.append_rule "*" Lwt_log.Info; + Lwt_log.default := + Lwt_log.channel ~template:"$(name): $(section): $(message) $(date) $(milliseconds)" + ~close_mode:`Keep ~channel:Lwt_io.stderr (); + (* Open the krobot bus. *) lwt bus = Krobot_bus.get () in diff --git a/info/vision/camlcv/_oasis b/info/vision/camlcv/_oasis index edf16a3..e586f57 100644 --- a/info/vision/camlcv/_oasis +++ b/info/vision/camlcv/_oasis @@ -25,8 +25,9 @@ Description: Library "cv" Path: src/ Modules: - OpenCv + CvCore, + CvHighGui CSources: cv_caml.c XMETADescription: Core Opencv - BuildDepends: + BuildDepends: bigarray diff --git a/info/vision/camlcv/_tags b/info/vision/camlcv/_tags index d6cef63..b018bdb 100644 --- a/info/vision/camlcv/_tags +++ b/info/vision/camlcv/_tags @@ -1,3 +1,22 @@ # OASIS_START +# DO NOT EDIT (digest: b8f60d120ef9d7618b2ed02d64b8ef03) +# Ignore VCS directories, you can use the same kind of rule outside +# OASIS_START/STOP if you want to exclude directories that contains +# useless stuff for the build process +<**/.svn>: -traverse +<**/.svn>: not_hygienic +".bzr": -traverse +".bzr": not_hygienic +".hg": -traverse +".hg": not_hygienic +".git": -traverse +".git": not_hygienic +"_darcs": -traverse +"_darcs": not_hygienic +# Library cv +"src/cv.cmxs": use_cv +<src/cv.{cma,cmxa}>: use_libcv_stubs +<src/*.ml{,i}>: pkg_bigarray +"src/cv_caml.c": pkg_bigarray # OASIS_STOP <src/*>: use_C_opencv diff --git a/info/vision/camlcv/myocamlbuild.ml b/info/vision/camlcv/myocamlbuild.ml index 1b02616..ed0acd9 100644 --- a/info/vision/camlcv/myocamlbuild.ml +++ b/info/vision/camlcv/myocamlbuild.ml @@ -1,4 +1,561 @@ (* OASIS_START *) +(* DO NOT EDIT (digest: 4691146961f957a78ce95a45557aba80) *) +module OASISGettext = struct +# 21 "/media/data/ocaml/oasis/src/oasis/OASISGettext.ml" + + let ns_ str = + str + + let s_ str = + str + + let f_ (str : ('a, 'b, 'c, 'd) format4) = + str + + let fn_ fmt1 fmt2 n = + if n = 1 then + fmt1^^"" + else + fmt2^^"" + + let init = + [] + +end + +module OASISExpr = struct +# 21 "/media/data/ocaml/oasis/src/oasis/OASISExpr.ml" + + + + open OASISGettext + + type test = string + + type flag = string + + type t = + | EBool of bool + | ENot of t + | EAnd of t * t + | EOr of t * t + | EFlag of flag + | ETest of test * string + + + type 'a choices = (t * 'a) list + + let eval var_get t = + let rec eval' = + function + | EBool b -> + b + + | ENot e -> + not (eval' e) + + | EAnd (e1, e2) -> + (eval' e1) && (eval' e2) + + | EOr (e1, e2) -> + (eval' e1) || (eval' e2) + + | EFlag nm -> + let v = + var_get nm + in + assert(v = "true" || v = "false"); + (v = "true") + + | ETest (nm, vl) -> + let v = + var_get nm + in + (v = vl) + in + eval' t + + let choose ?printer ?name var_get lst = + let rec choose_aux = + function + | (cond, vl) :: tl -> + if eval var_get cond then + vl + else + choose_aux tl + | [] -> + let str_lst = + if lst = [] then + s_ "<empty>" + else + String.concat + (s_ ", ") + (List.map + (fun (cond, vl) -> + match printer with + | Some p -> p vl + | None -> s_ "<no printer>") + lst) + in + match name with + | Some nm -> + failwith + (Printf.sprintf + (f_ "No result for the choice list '%s': %s") + nm str_lst) + | None -> + failwith + (Printf.sprintf + (f_ "No result for a choice list: %s") + str_lst) + in + choose_aux (List.rev lst) + +end + + +module BaseEnvLight = struct +# 21 "/media/data/ocaml/oasis/src/base/BaseEnvLight.ml" + + module MapString = Map.Make(String) + + type t = string MapString.t + + let default_filename = + Filename.concat + (Sys.getcwd ()) + "setup.data" + + let load ?(allow_empty=false) ?(filename=default_filename) () = + if Sys.file_exists filename then + begin + let chn = + open_in_bin filename + in + let st = + Stream.of_channel chn + in + let line = + ref 1 + in + let st_line = + Stream.from + (fun _ -> + try + match Stream.next st with + | '\n' -> incr line; Some '\n' + | c -> Some c + with Stream.Failure -> None) + in + let lexer = + Genlex.make_lexer ["="] st_line + in + let rec read_file mp = + match Stream.npeek 3 lexer with + | [Genlex.Ident nm; Genlex.Kwd "="; Genlex.String value] -> + Stream.junk lexer; + Stream.junk lexer; + Stream.junk lexer; + read_file (MapString.add nm value mp) + | [] -> + mp + | _ -> + failwith + (Printf.sprintf + "Malformed data file '%s' line %d" + filename !line) + in + let mp = + read_file MapString.empty + in + close_in chn; + mp + end + else if allow_empty then + begin + MapString.empty + end + else + begin + failwith + (Printf.sprintf + "Unable to load environment, the file '%s' doesn't exist." + filename) + end + + let var_get name env = + let rec var_expand str = + let buff = + Buffer.create ((String.length str) * 2) + in + Buffer.add_substitute + buff + (fun var -> + try + var_expand (MapString.find var env) + with Not_found -> + failwith + (Printf.sprintf + "No variable %s defined when trying to expand %S." + var + str)) + str; + Buffer.contents buff + in + var_expand (MapString.find name env) + + let var_choose lst env = + OASISExpr.choose + (fun nm -> var_get nm env) + lst +end + + +module MyOCamlbuildFindlib = struct +# 21 "/media/data/ocaml/oasis/src/plugins/ocamlbuild/MyOCamlbuildFindlib.ml" + + (** OCamlbuild extension, copied from + * http://brion.inria.fr/gallium/index.php/Using_ocamlfind_with_ocamlbuild + * by N. Pouillard and others + * + * Updated on 2009/02/28 + * + * Modified by Sylvain Le Gall + *) + open Ocamlbuild_plugin + + (* these functions are not really officially exported *) + let run_and_read = + Ocamlbuild_pack.My_unix.run_and_read + + let blank_sep_strings = + Ocamlbuild_pack.Lexers.blank_sep_strings + + let split s ch = + let x = + ref [] + in + let rec go s = + let pos = + String.index s ch + in + x := (String.before s pos)::!x; + go (String.after s (pos + 1)) + in + try + go s + with Not_found -> !x + + let split_nl s = split s '\n' + + let before_space s = + try + String.before s (String.index s ' ') + with Not_found -> s + + (* this lists all supported packages *) + let find_packages () = + List.map before_space (split_nl & run_and_read "ocamlfind list") + + (* this is supposed to list available syntaxes, but I don't know how to do it. *) + let find_syntaxes () = ["camlp4o"; "camlp4r"] + + (* ocamlfind command *) + let ocamlfind x = S[A"ocamlfind"; x] + + let dispatch = + function + | Before_options -> + (* by using Before_options one let command line options have an higher priority *) + (* on the contrary using After_options will guarantee to have the higher priority *) + (* override default commands by ocamlfind ones *) + Options.ocamlc := ocamlfind & A"ocamlc"; + Options.ocamlopt := ocamlfind & A"ocamlopt"; + Options.ocamldep := ocamlfind & A"ocamldep"; + Options.ocamldoc := ocamlfind & A"ocamldoc"; + Options.ocamlmktop := ocamlfind & A"ocamlmktop" + + | After_rules -> + + (* When one link an OCaml library/binary/package, one should use -linkpkg *) + flag ["ocaml"; "link"; "program"] & A"-linkpkg"; + + (* For each ocamlfind package one inject the -package option when + * compiling, computing dependencies, generating documentation and + * linking. *) + List.iter + begin fun pkg -> + flag ["ocaml"; "compile"; "pkg_"^pkg] & S[A"-package"; A pkg]; + flag ["ocaml"; "ocamldep"; "pkg_"^pkg] & S[A"-package"; A pkg]; + flag ["ocaml"; "doc"; "pkg_"^pkg] & S[A"-package"; A pkg]; + flag ["ocaml"; "link"; "pkg_"^pkg] & S[A"-package"; A pkg]; + flag ["ocaml"; "infer_interface"; "pkg_"^pkg] & S[A"-package"; A pkg]; + end + (find_packages ()); + + (* Like -package but for extensions syntax. Morover -syntax is useless + * when linking. *) + List.iter begin fun syntax -> + flag ["ocaml"; "compile"; "syntax_"^syntax] & S[A"-syntax"; A syntax]; + flag ["ocaml"; "ocamldep"; "syntax_"^syntax] & S[A"-syntax"; A syntax]; + flag ["ocaml"; "doc"; "syntax_"^syntax] & S[A"-syntax"; A syntax]; + flag ["ocaml"; "infer_interface"; "syntax_"^syntax] & S[A"-syntax"; A syntax]; + end (find_syntaxes ()); + + (* The default "thread" tag is not compatible with ocamlfind. + * Indeed, the default rules add the "threads.cma" or "threads.cmxa" + * options when using this tag. When using the "-linkpkg" option with + * ocamlfind, this module will then be added twice on the command line. + * + * To solve this, one approach is to add the "-thread" option when using + * the "threads" package using the previous plugin. + *) + flag ["ocaml"; "pkg_threads"; "compile"] (S[A "-thread"]); + flag ["ocaml"; "pkg_threads"; "doc"] (S[A "-I"; A "+threads"]); + flag ["ocaml"; "pkg_threads"; "link"] (S[A "-thread"]); + flag ["ocaml"; "pkg_threads"; "infer_interface"] (S[A "-thread"]) + + | _ -> + () + +end + +module MyOCamlbuildBase = struct +# 21 "/media/data/ocaml/oasis/src/plugins/ocamlbuild/MyOCamlbuildBase.ml" + + (** Base functions for writing myocamlbuild.ml + @author Sylvain Le Gall + *) + + + + open Ocamlbuild_plugin + module OC = Ocamlbuild_pack.Ocaml_compiler + + type dir = string + type file = string + type name = string + type tag = string + +# 56 "/media/data/ocaml/oasis/src/plugins/ocamlbuild/MyOCamlbuildBase.ml" + + type t = + { + lib_ocaml: (name * dir list) list; + lib_c: (name * dir * file list) list; + flags: (tag list * (spec OASISExpr.choices)) list; + (* Replace the 'dir: include' from _tags by a precise interdepends in + * directory. + *) + includes: (dir * dir list) list; + } + + let env_filename = + Pathname.basename + BaseEnvLight.default_filename + + let dispatch_combine lst = + fun e -> + List.iter + (fun dispatch -> dispatch e) + lst + + let tag_libstubs nm = + "use_lib"^nm^"_stubs" + + let nm_libstubs nm = + nm^"_stubs" + + let dispatch t e = + let env = + BaseEnvLight.load + ~filename:env_filename + ~allow_empty:true + () + in + match e with + | Before_options -> + let no_trailing_dot s = + if String.length s >= 1 && s.[0] = '.' then + String.sub s 1 ((String.length s) - 1) + else + s + in + List.iter + (fun (opt, var) -> + try + opt := no_trailing_dot (BaseEnvLight.var_get var env) + with Not_found -> + Printf.eprintf "W: Cannot get variable %s" var) + [ + Options.ext_obj, "ext_obj"; + Options.ext_lib, "ext_lib"; + Options.ext_dll, "ext_dll"; + ] + + | Before_rules -> + (* TODO: move this into its own file and conditionnaly include it, if + * needed. + *) + (* OCaml cmxs rules: cmxs available in ocamlopt but not ocamlbuild. + Copied from ocaml_specific.ml in ocamlbuild sources. *) + let has_native_dynlink = + try + bool_of_string (BaseEnvLight.var_get "native_dynlink" env) + with Not_found -> + false + in + if has_native_dynlink && String.sub Sys.ocaml_version 0 4 = "3.11" then + begin + let ext_lib = !Options.ext_lib in + let ext_obj = !Options.ext_obj in + let ext_dll = !Options.ext_dll in + let x_o = "%"-.-ext_obj in + let x_a = "%"-.-ext_lib in + let x_dll = "%"-.-ext_dll in + let x_p_o = "%.p"-.-ext_obj in + let x_p_a = "%.p"-.-ext_lib in + let x_p_dll = "%.p"-.-ext_dll in + + rule "ocaml: mldylib & p.cmx* & p.o* -> p.cmxs & p.so" + ~tags:["ocaml"; "native"; "profile"; "shared"; "library"] + ~prods:["%.p.cmxs"; x_p_dll] + ~dep:"%.mldylib" + (OC.native_profile_shared_library_link_mldylib + "%.mldylib" "%.p.cmxs"); + + rule "ocaml: mldylib & cmx* & o* -> cmxs & so" + ~tags:["ocaml"; "native"; "shared"; "library"] + ~prods:["%.cmxs"; x_dll] + ~dep:"%.mldylib" + (OC.native_shared_library_link_mldylib + "%.mldylib" "%.cmxs"); + + rule "ocaml: p.cmx & p.o -> p.cmxs & p.so" + ~tags:["ocaml"; "native"; "profile"; "shared"; "library"] + ~prods:["%.p.cmxs"; x_p_dll] + ~deps:["%.p.cmx"; x_p_o] + (OC.native_shared_library_link ~tags:["profile"] + "%.p.cmx" "%.p.cmxs"); + + rule "ocaml: p.cmxa & p.a -> p.cmxs & p.so" + ~tags:["ocaml"; "native"; "profile"; "shared"; "library"] + ~prods:["%.p.cmxs"; x_p_dll] + ~deps:["%.p.cmxa"; x_p_a] + (OC.native_shared_library_link ~tags:["profile"; "linkall"] + "%.p.cmxa" "%.p.cmxs"); + + rule "ocaml: cmx & o -> cmxs" + ~tags:["ocaml"; "native"; "shared"; "library"] + ~prods:["%.cmxs"] + ~deps:["%.cmx"; x_o] + (OC.native_shared_library_link "%.cmx" "%.cmxs"); + + rule "ocaml: cmx & o -> cmxs & so" + ~tags:["ocaml"; "native"; "shared"; "library"] + ~prods:["%.cmxs"; x_dll] + ~deps:["%.cmx"; x_o] + (OC.native_shared_library_link "%.cmx" "%.cmxs"); + + rule "ocaml: cmxa & a -> cmxs & so" + ~tags:["ocaml"; "native"; "shared"; "library"] + ~prods:["%.cmxs"; x_dll] + ~deps:["%.cmxa"; x_a] + (OC.native_shared_library_link ~tags:["linkall"] + "%.cmxa" "%.cmxs"); + end + + | After_rules -> + (* Declare OCaml libraries *) + List.iter + (function + | nm, [] -> + ocaml_lib nm + | nm, dir :: tl -> + ocaml_lib ~dir:dir (dir^"/"^nm); + List.iter + (fun dir -> + List.iter + (fun str -> + flag ["ocaml"; "use_"^nm; str] (S[A"-I"; P dir])) + ["compile"; "infer_interface"; "doc"]) + tl) + t.lib_ocaml; + + (* Declare directories dependencies, replace "include" in _tags. *) + List.iter + (fun (dir, include_dirs) -> + Pathname.define_context dir include_dirs) + t.includes; + + (* Declare C libraries *) + List.iter + (fun (lib, dir, headers) -> + (* Handle C part of library *) + flag ["link"; "library"; "ocaml"; "byte"; tag_libstubs lib] + (S[A"-dllib"; A("-l"^(nm_libstubs lib)); A"-cclib"; + A("-l"^(nm_libstubs lib))]); + + flag ["link"; "library"; "ocaml"; "native"; tag_libstubs lib] + (S[A"-cclib"; A("-l"^(nm_libstubs lib))]); + + flag ["link"; "program"; "ocaml"; "byte"; tag_libstubs lib] + (S[A"-dllib"; A("dll"^(nm_libstubs lib))]); + + (* When ocaml link something that use the C library, then one + need that file to be up to date. + *) + dep ["link"; "ocaml"; "program"; tag_libstubs lib] + [dir/"lib"^(nm_libstubs lib)^"."^(!Options.ext_lib)]; + + dep ["compile"; "ocaml"; "program"; tag_libstubs lib] + [dir/"lib"^(nm_libstubs lib)^"."^(!Options.ext_lib)]; + + (* TODO: be more specific about what depends on headers *) + (* Depends on .h files *) + dep ["compile"; "c"] + headers; + + (* Setup search path for lib *) + flag ["link"; "ocaml"; "use_"^lib] + (S[A"-I"; P(dir)]); + ) + t.lib_c; + + (* Add flags *) + List.iter + (fun (tags, cond_specs) -> + let spec = + BaseEnvLight.var_choose cond_specs env + in + flag tags & spec) + t.flags + | _ -> + () + + let dispatch_default t = + dispatch_combine + [ + dispatch t; + MyOCamlbuildFindlib.dispatch; + ] + +end + + +open Ocamlbuild_plugin;; +let package_default = + { + MyOCamlbuildBase.lib_ocaml = [("cv", ["src"])]; + lib_c = [("cv", "src/", [])]; + flags = []; + includes = []; + } + ;; + +let dispatch_default = MyOCamlbuildBase.dispatch_default package_default;; + +# 559 "myocamlbuild.ml" (* OASIS_STOP *) open Ocamlbuild_plugin diff --git a/info/vision/camlcv/src/META b/info/vision/camlcv/src/META new file mode 100644 index 0000000..26e4895 --- /dev/null +++ b/info/vision/camlcv/src/META @@ -0,0 +1,10 @@ +# OASIS_START +# DO NOT EDIT (digest: 3f508b1b1a49417ae0d748f2f8bc10e1) +version = "0.1" +description = "Core Opencv" +requires = "bigarray" +archive(byte) = "cv.cma" +archive(native) = "cv.cmxa" +exists_if = "cv.cma" +# OASIS_STOP + diff --git a/info/vision/camlcv/src/cv.mllib b/info/vision/camlcv/src/cv.mllib new file mode 100644 index 0000000..7e8b7f4 --- /dev/null +++ b/info/vision/camlcv/src/cv.mllib @@ -0,0 +1,5 @@ +# OASIS_START +# DO NOT EDIT (digest: 789af667e1aace6d4731799b5a02a2b0) +CvCore +CvHighGui +# OASIS_STOP diff --git a/info/vision/camlcv/src/cvCore.ml b/info/vision/camlcv/src/cvCore.ml new file mode 100644 index 0000000..155690a --- /dev/null +++ b/info/vision/camlcv/src/cvCore.ml @@ -0,0 +1,462 @@ + +type ('a,'b) iplImage +type cvType_ (* int *) +type color_conversion_ (* int *) + +type channel_num_ = [ `Channel_1 | `Channel_2 | `Channel_3 | `Channel_4 ] +type 'a channel_num = int + +let channel_1 = 1 +let channel_2 = 2 +let channel_3 = 3 +let channel_4 = 4 + +type cvType = + | IPL_DEPTH_1U + | IPL_DEPTH_8U + | IPL_DEPTH_16U + | IPL_DEPTH_32F + | IPL_DEPTH_8S + | IPL_DEPTH_16S + | IPL_DEPTH_32S + +type depth_ = [ `U1 | `U8 | `U16 | `F32 | `S8 | `S16 | `S32 ] +type 'a depth = cvType + +let depth_u1 = IPL_DEPTH_1U +let depth_u8 = IPL_DEPTH_8U +let depth_u16 = IPL_DEPTH_16U +let depth_f32 = IPL_DEPTH_32F + +type 'a image_depth = 'a depth + +type color_conversion = + | CV_BGR2BGRA + | CV_RGB2RGBA + | CV_BGRA2BGR + | CV_RGBA2RGB + | CV_BGR2RGBA + | CV_RGB2BGRA + | CV_RGBA2BGR + | CV_BGRA2RGB + | CV_BGR2RGB + | CV_RGB2BGR + | CV_BGRA2RGBA + | CV_RGBA2BGRA + | CV_BGR2GRAY + | CV_RGB2GRAY + | CV_GRAY2BGR + | CV_GRAY2RGB + | CV_GRAY2BGRA + | CV_GRAY2RGBA + | CV_BGRA2GRAY + | CV_RGBA2GRAY + | CV_BGR2BGR565 + | CV_RGB2BGR565 + | CV_BGR5652BGR + | CV_BGR5652RGB + | CV_BGRA2BGR565 + | CV_RGBA2BGR565 + | CV_BGR5652BGRA + | CV_BGR5652RGBA + | CV_GRAY2BGR565 + | CV_BGR5652GRAY + | CV_BGR2BGR555 + | CV_RGB2BGR555 + | CV_BGR5552BGR + | CV_BGR5552RGB + | CV_BGRA2BGR555 + | CV_RGBA2BGR555 + | CV_BGR5552BGRA + | CV_BGR5552RGBA + | CV_GRAY2BGR555 + | CV_BGR5552GRAY + | CV_BGR2XYZ + | CV_RGB2XYZ + | CV_XYZ2BGR + | CV_XYZ2RGB + | CV_BGR2YCrCb + | CV_RGB2YCrCb + | CV_YCrCb2BGR + | CV_YCrCb2RGB + | CV_BGR2HSV + | CV_RGB2HSV + | CV_BGR2Lab + | CV_RGB2Lab + | CV_BayerBG2BGR + | CV_BayerGB2BGR + | CV_BayerRG2BGR + | CV_BayerGR2BGR + | CV_BayerBG2RGB + | CV_BayerGB2RGB + | CV_BayerRG2RGB + | CV_BayerGR2RGB + | CV_BGR2Luv + | CV_RGB2Luv + | CV_BGR2HLS + | CV_RGB2HLS + | CV_HSV2BGR + | CV_HSV2RGB + | CV_Lab2BGR + | CV_Lab2RGB + | CV_Luv2BGR + | CV_Luv2RGB + | CV_HLS2BGR + | CV_HLS2RGB + +type threshold_type = + | CV_THRESH_BINARY + | CV_THRESH_BINARY_INV + | CV_THRESH_TRUNC + | CV_THRESH_TOZERO + | CV_THRESH_TOZERO_INV + | CV_THRESH_MASK + | CV_THRESH_OTSU + +type adaptive_method = + | CV_ADAPTIVE_THRESH_MEAN_C + | CV_ADAPTIVE_THRESH_GAUSSIAN_C + +type ('a,'b) conversion = color_conversion + +let bgr2rgb = CV_BGR2RGB +let rgb2bgr = CV_RGB2BGR +let bgr2hsv = CV_BGR2HSV +let hsv2bgr = CV_HSV2BGR +let rgb2hsv = CV_RGB2HSV +let hsv2rgb = CV_HSV2RGB +let bgr2gray = CV_BGR2GRAY +let gray2bgr = CV_GRAY2BGR +let bgr2bgra = CV_BGR2BGRA +let rgb2gray = CV_RGB2GRAY +let gray2rgb = CV_GRAY2RGB +let rgb2rgba = CV_RGB2RGBA + +external cvCreateImage : (int*int) -> cvType_ -> int -> ('a,'b) iplImage = "ocaml_cvCreateImage" +external cvCvtColor : ('a,'b) iplImage -> ('d,'e) iplImage -> color_conversion_ -> unit = "ocaml_cvCvtColor" +external threshold : ('a,'b) iplImage -> ('d,'e) iplImage -> float -> float -> threshold_type -> unit = "ocaml_cvThreshold" +external adaptive_threshold : ('a,'b) iplImage -> ('d,'e) iplImage -> float -> adaptive_method -> threshold_type -> int -> float -> unit = "ocaml_cvAdaptiveThreshold_bytecode" "ocaml_cvAdaptiveThreshold" + +external cvSplit : ('a,'b) iplImage -> ([`Channel_1],'b) iplImage option -> + ([`Channel_1],'b) iplImage option -> ([`Channel_1],'b) iplImage option -> + ([`Channel_1],'b) iplImage option -> unit = "ocaml_cvSplit" + +external cvMerge : ([`Channel_1],'b) iplImage option -> ([`Channel_1],'b) iplImage option -> + ([`Channel_1],'b) iplImage option -> ([`Channel_1],'b) iplImage option -> + ('a,'b) iplImage -> unit = "ocaml_cvMerge" + +let split_3 src c1 c2 c3 = + cvSplit src (Some c1) (Some c2) (Some c3) None + +let merge_3 c1 c2 c3 dst = + cvMerge (Some c1) (Some c2) (Some c3) None dst + +external get_cvType : cvType -> cvType_ = "ocaml_get_cvType" +external get_color_conversion : color_conversion -> color_conversion_ + = "ocaml_get_color_conversion" +external image_size : ('a,'b) iplImage -> (int*int) = "ocaml_image_size" +external image_channels : ('a,'b) iplImage -> int = "ocaml_image_channels" +external image_depth : ('a,'b) iplImage -> int = "ocaml_image_depth" +external image_data_order : ('a,'b) iplImage -> int = "ocaml_image_data_order" + +(* type float_3point = *) +(* { mutable f0 : float; *) +(* mutable f1 : float; *) +(* mutable f2 : float; } *) + +(* type int_3point = *) +(* { mutable i0 : int; *) +(* mutable i1 : int; *) +(* mutable i2 : int; } *) + +(* external get_float_3point : *) +(* ([`One],[`F32],[`BGR | `RGB | `HSV ]) iplImage -> *) +(* float_point -> unit *) +(* = "ocaml_get_float_3point" "noalloc" *) + +(* external set_float_3point : *) +(* ([`One],[`F32],[`BGR | `RGB | `HSV ]) iplImage -> *) +(* float_point -> unit *) +(* = "ocaml_set_float_3point" "noalloc" *) + +(* external unsafe_get_int_3point : *) +(* ([`One],[`U8],[<`BGR | `RGB | `HSV ]) iplImage -> *) +(* int -> int_3point -> unit *) +(* = "ocaml_get_int_3point" "noalloc" *) + +(* external unsafe_set_int_3point : *) +(* ([`One],[`U8],[<`BGR | `RGB | `HSV ]) iplImage -> *) +(* int -> int_3point -> unit *) +(* = "ocaml_set_int_3point" "noalloc" *) + +type image_data = (int, Bigarray.int8_unsigned_elt, Bigarray.c_layout) Bigarray.Array2.t + +external image_data : ('a,[`U8]) iplImage -> image_data = "ocaml_image_to_bigarray" + +external zero_image : ('a,'b) iplImage -> unit = "ocaml_cvZero" + +external clone_image : ('a,'b) iplImage -> ('a,'b) iplImage = "ocaml_cvCloneImage" + +let create_image ~x ~y _type i = cvCreateImage (x,y) (get_cvType _type) i +let convert_color ~src ~dst color_conversion = + cvCvtColor src dst (get_color_conversion color_conversion) + +type cvScalar = float * float * float * float +type cvPoint = int * int +type cvPoint2D32f = float * float +type cvPoint3D32f = float * float * float +type cvPoint2D64f = float * float +type cvPoint3D64f = float * float * float +type cvSize = int * int +type cvTermCriteria = + { termcrit_iter : bool; + termcrit_epsilon : bool; + max_iter : int; + epsilon : float; } + +external cvCircle : ('a,[`U8]) iplImage -> cvPoint -> int -> cvScalar -> int -> unit = "ocaml_cvCircle" +external cvEllipse : ('a,[`U8]) iplImage -> cvPoint -> cvSize -> + float -> float -> float -> cvScalar -> int -> unit = + "ocaml_cvEllipse_bytecode" "ocaml_cvEllipse" +external cvRectangle : ('a,[`U8]) iplImage -> cvPoint -> cvPoint -> cvScalar -> int -> unit = "ocaml_cvRectangle" +external cvLine : ('a,[`U8]) iplImage -> cvPoint -> cvPoint -> cvScalar -> int -> unit = "ocaml_cvLine" + +(* memory handling *) +type cvMemStorage + +external create_CvMemStorage : unit -> cvMemStorage = "ocaml_create_CvMemStorage" +external free_CvMemStorage : cvMemStorage -> unit = "ocaml_free_CvMemStorage" + +type contour_retrieval_mode = + | CV_RETR_EXTERNAL + | CV_RETR_LIST + | CV_RETR_CCOMP + | CV_RETR_TREE + +type contour_approximation_method = + | CV_CHAIN_CODE + | CV_CHAIN_APPROX_NONE + | CV_CHAIN_APPROX_SIMPLE + | CV_CHAIN_APPROX_TC89_L1 + | CV_CHAIN_APPROX_TC89_KCOS + | CV_LINK_RUNS + +type cvSeq + +external cvFindContours : ([`Channel_1],[`U8]) iplImage -> cvMemStorage -> + contour_retrieval_mode -> contour_approximation_method -> cvPoint -> cvSeq = + "ocaml_cvFindContours" + +type cvSeq_info = { + cv_h_prev : cvSeq option; + cv_h_next : cvSeq option; + cv_v_prev : cvSeq option; + cv_v_next : cvSeq option; +} + +external cvSeq_info : cvSeq -> cvSeq_info = "ocaml_CvSeq_info" + +type seq = { + seq_stor : cvMemStorage; + seq : cvSeq; +} + +type seq_info = { + h_prev : seq option; + h_next : seq option; + v_prev : seq option; + v_next : seq option; +} + +let add_stor s = function + | None -> None + | Some v -> Some { seq_stor = s; seq = v } + +let seq_info seq = + let info = cvSeq_info seq.seq in + { h_prev = add_stor seq.seq_stor info.cv_h_prev; + h_next = add_stor seq.seq_stor info.cv_h_next; + v_prev = add_stor seq.seq_stor info.cv_v_prev; + v_next = add_stor seq.seq_stor info.cv_v_next; } + +let find_contours ?(mode=CV_RETR_LIST) ?(meth=CV_CHAIN_APPROX_SIMPLE) image = + let stor = create_CvMemStorage () in + let cvSeq = cvFindContours image stor mode meth (0,0) in + { seq_stor = stor; + seq = cvSeq } + +external cvDrawContours : ('a,[`U8]) iplImage -> cvSeq -> cvScalar -> cvScalar -> + int -> int -> cvPoint -> unit = "ocaml_cvDrawContours_bytecode" "ocaml_cvDrawContours" + +let draw_contours image seq in_color out_color level thickness offset = + cvDrawContours image seq.seq in_color out_color level thickness offset + +type ellipse = { + ellipse_center : float * float; + ellipse_size : float * float; + ellipse_angle : float; +} + +external cvFitEllipse2 : cvSeq -> (float * float * float * float * float) option = "ocaml_cvFitEllipse2" + +let fit_ellipse seq = + match cvFitEllipse2 seq.seq with + | None -> None + | Some (x,y,width,height,angle) -> + Some { ellipse_center = x,y; + ellipse_size = width,height; + ellipse_angle = angle } + +type calib_cb = + | CV_CALIB_CB_ADAPTIVE_THRESH + | CV_CALIB_CB_NORMALIZE_IMAGE + | CV_CALIB_CB_FILTER_QUADS + | CV_CALIB_CB_FAST_CHECK + +type chessboard_corners = cvSize * bool * int * + (float, Bigarray.float32_elt, Bigarray.c_layout) Bigarray.Array3.t + +external cvFindChessboardCorners : ('a,[`U8]) iplImage -> cvSize -> calib_cb list -> + bool * int * (float, Bigarray.float32_elt, Bigarray.c_layout) Bigarray.Array3.t + = "ocaml_cvFindChessboardCorners" + +let find_chessboard_corners ?(flags=[CV_CALIB_CB_ADAPTIVE_THRESH]) image size = + let (found,count,ba) = cvFindChessboardCorners image size flags in + (size,found,count,ba) + +let found_chessboard_corners (_,found,_,_) = found + +let array_of_chessboard_corners (size,found,count,ba) = + if found + then + Some (Array.init (Bigarray.Array3.dim1 ba) + (fun i -> Array.init (Bigarray.Array3.dim2 ba) + (fun j -> ba.{i,j,0}, ba.{i,j,1}))) + else + None + +external cvDrawChessboardCorners : ('a,[`U8]) iplImage -> cvSize -> + (float, Bigarray.float32_elt, Bigarray.c_layout) Bigarray.Array3.t -> + int -> bool -> unit + = "ocaml_cvDrawChessboardCorners" + +let draw_chessboard_corners i (size,found,count,ba) = + cvDrawChessboardCorners i size ba count found + +external cvFindCornerSubPix : ('a,[`U8]) iplImage -> + (float, Bigarray.float32_elt, Bigarray.c_layout) Bigarray.Array3.t -> + int -> cvSize -> cvSize -> cvTermCriteria -> unit + = "ocaml_cvFindCornerSubPix_bytecode" "ocaml_cvFindCornerSubPix" + +let default_criteria = + { termcrit_iter = true; + termcrit_epsilon = true; + max_iter = 30; + epsilon = 0.1; } + +let find_corner_subpix ?(criteria=default_criteria) ?(winSize=11,11) ?(zeroZone=(-1,-1)) + i (size,found,count,ba) = + if found + then + begin + cvFindCornerSubPix i ba count (11,11) (-1,-1) criteria; + end + + +(** camera calibration *) + +type ('channel) cvMat_float32 = + (float, Bigarray.float32_elt, Bigarray.c_layout) Bigarray.Array3.t + +type ('channel) cvMat_int = + (int, Bigarray.int_elt, Bigarray.c_layout) Bigarray.Array3.t + +external cvCalibrateCamera2 : [`Channel_3] cvMat_float32 -> [`Channel_2] cvMat_float32 -> + [`Channel_1] cvMat_int -> cvSize -> [`Channel_1] cvMat_float32 -> + [`Channel_1] cvMat_float32 -> float + = "ocaml_cvCalibrateCamera2_bytecode" "ocaml_cvCalibrateCamera2" + +type calibration = cvSize * float * cvSize option * ((float*float) array array*(float*float*float) array array) list + +let init_calibration cb_size square_size : calibration = (cb_size, square_size, None, []) + +let corner_positions (x,y) square_size = + Array.init x + (fun i -> Array.init y + (fun j -> (float j) *. square_size, (float i) *. square_size, 0.)) + +let add_calibration_image ((cb_size, square_size, i_size, state):calibration) image : calibration = + let i_size = match i_size with + | None -> image_size image + | Some s -> + if s <> image_size image + then raise (Invalid_argument "not same size"); + s in + let r = find_chessboard_corners image cb_size in + let a = corner_positions cb_size square_size in + find_corner_subpix image r; + let state = + match array_of_chessboard_corners r with + | None -> state + | Some v -> (v,a)::state in + (cb_size, square_size, Some i_size, state) + +let map_a2 a = Array.map (fun e -> Array.map (fun (x,y) -> [|x;y|]) e) a +let map_a3 a = Array.map (fun e -> Array.map (fun (x,y,z) -> [|x;y;z|]) e) a + +let calibrate (cal:calibration) = + let (cb_x,cb_y), square_size, i_size, state = cal in + let i_size = match i_size with + | None -> raise (Invalid_argument "no calibration data") + | Some v -> v in + let points = cb_x * cb_y in + let obj_pos = Array.concat (List.map (fun (_,a) -> map_a3 a) state) in + let obj_pos = Bigarray.Array3.of_array Bigarray.float32 Bigarray.c_layout obj_pos in + let obj_pos = Bigarray.reshape_3 + (Bigarray.genarray_of_array3 obj_pos) + ((List.length state) * points) 1 3 in + let img_pos = Array.concat (List.map (fun (a,_) -> map_a2 a) state) in + let img_pos = Bigarray.Array3.of_array Bigarray.float32 Bigarray.c_layout img_pos in + let img_pos = Bigarray.reshape_3 + (Bigarray.genarray_of_array3 img_pos) + ((List.length state) * points) 1 2 in + let point_count = Array.create (List.length state) points in + let point_count = Bigarray.Array1.of_array Bigarray.int Bigarray.c_layout point_count in + let point_count = Bigarray.reshape_3 + (Bigarray.genarray_of_array1 point_count) + (List.length state) 1 1 in + let out_mat = Bigarray.Array3.create Bigarray.float32 Bigarray.c_layout 3 3 1 in + Bigarray.Array3.fill out_mat 0.; + out_mat.{0,0,0} <- 1.; + out_mat.{1,1,0} <- 1.; + let out_vect = Bigarray.Array3.create Bigarray.float32 Bigarray.c_layout 5 1 1 in + let r = cvCalibrateCamera2 + obj_pos img_pos point_count i_size out_mat out_vect in + let r_mat = Array.init 3 (fun i -> Array.init 3 (fun j -> out_mat.{i,j,0})) in + let r_vect = Array.init 5 (fun i -> out_vect.{i,0,0}) in + r, (r_mat, r_vect) + +external cvInitUndistortMap : + [`Channel_1] cvMat_float32 -> + [`Channel_1] cvMat_float32 -> + ([`Channel_1],[`F32]) iplImage -> + ([`Channel_1],[`F32]) iplImage -> + unit + = "cvInitUndistortMap" + +let init_undistort_map (x,y) (mat,v) = + let mat = Bigarray.Array3.of_array Bigarray.float32 Bigarray.c_layout + ((Array.map (Array.map (fun v -> [|v|]))) mat) in + let v = Bigarray.Array3.of_array Bigarray.float32 Bigarray.c_layout + (Array.map (fun v -> [|[|v|]|]) v) in + let i1 = create_image ~x ~y depth_f32 channel_1 in + let i2 = create_image ~x ~y depth_f32 channel_1 in + cvInitUndistortMap mat v i1 i2; + i1, i2 + +external cvRemap : + ('a,'b) iplImage -> ('a,'b) iplImage -> + ([`Channel_1],[`F32]) iplImage -> + ([`Channel_1],[`F32]) iplImage -> + unit + = "cvRemap" diff --git a/info/vision/camlcv/src/cvCore.mli b/info/vision/camlcv/src/cvCore.mli new file mode 100644 index 0000000..658edc5 --- /dev/null +++ b/info/vision/camlcv/src/cvCore.mli @@ -0,0 +1,176 @@ + +type cvScalar = float * float * float * float +type cvPoint = int * int +type cvPoint2D32f = float * float +type cvPoint3D32f = float * float * float +type cvPoint2D64f = float * float +type cvPoint3D64f = float * float * float +type cvSize = int * int +type cvTermCriteria = + { termcrit_iter : bool; + termcrit_epsilon : bool; + max_iter : int; + epsilon : float; } + +type ('chan_num,'depth) iplImage + +type channel_num_ = [ `Channel_1 | `Channel_2 | `Channel_3 | `Channel_4 ] +type 'num channel_num + +val channel_1 : [ `Channel_1 ] channel_num +val channel_2 : [ `Channel_2 ] channel_num +val channel_3 : [ `Channel_3 ] channel_num +val channel_4 : [ `Channel_4 ] channel_num + +type 'depth image_depth + +val depth_u1 : [`U1] image_depth +val depth_u8 : [`U8] image_depth +val depth_u16 : [`U16] image_depth +val depth_f32 : [`F32] image_depth + +val create_image : x:int -> y:int -> 'depth image_depth -> 'chan_num channel_num -> + ('chan_num,'depth) iplImage +val clone_image : ('a,'b) iplImage -> ('a,'b) iplImage +val zero_image : ('a,'b) iplImage -> unit + +val image_size : ('a,'b) iplImage -> int * int +val image_channels : ('a,'b) iplImage -> int +val image_depth : ('a,'b) iplImage -> int + +(** color conversions *) +type ('c_src,'c_dst) conversion + +val bgr2rgb : ([`Channel_3],[`Channel_3]) conversion +val rgb2bgr : ([`Channel_3],[`Channel_3]) conversion +val bgr2hsv : ([`Channel_3],[`Channel_3]) conversion +val hsv2bgr : ([`Channel_3],[`Channel_3]) conversion +val rgb2hsv : ([`Channel_3],[`Channel_3]) conversion +val hsv2rgb : ([`Channel_3],[`Channel_3]) conversion +val bgr2gray : ([`Channel_3],[`Channel_1]) conversion +val gray2bgr : ([`Channel_1],[`Channel_3]) conversion +val bgr2bgra : ([`Channel_3],[`Channel_4]) conversion +val rgb2gray : ([`Channel_3],[`Channel_1]) conversion +val gray2rgb : ([`Channel_1],[`Channel_3]) conversion +val rgb2rgba : ([`Channel_3],[`Channel_4]) conversion + +val convert_color : + src:('c_src,'b) iplImage -> + dst:('c_dst,'b) iplImage -> + ('c_src,'c_dst) conversion -> + unit + +val split_3 : ([`Channel_3],'b) iplImage -> ([`Channel_1],'b) iplImage -> + ([`Channel_1],'b) iplImage -> ([`Channel_1],'b) iplImage -> unit + +val merge_3 : ([`Channel_1],'b) iplImage -> ([`Channel_1],'b) iplImage -> + ([`Channel_1],'b) iplImage -> ([`Channel_3],'b) iplImage -> unit + +(** threshold *) + +type threshold_type = + | CV_THRESH_BINARY + | CV_THRESH_BINARY_INV + | CV_THRESH_TRUNC + | CV_THRESH_TOZERO + | CV_THRESH_TOZERO_INV + | CV_THRESH_MASK + | CV_THRESH_OTSU + +type adaptive_method = + | CV_ADAPTIVE_THRESH_MEAN_C + | CV_ADAPTIVE_THRESH_GAUSSIAN_C + +val threshold : ('a,'b) iplImage -> ('d,'e) iplImage -> float -> float -> threshold_type -> unit +(** [threshold src dst threshold maxValue thresholdType] *) +val adaptive_threshold : ('a,'b) iplImage -> ('d,'e) iplImage -> float -> adaptive_method -> threshold_type -> int -> float -> unit +(** [adaptive_threshold src dst maxValue adaptiveMethod thresholdType blockSize param1] *) + +(** drawing *) + +val cvCircle : ('a,[`U8]) iplImage -> cvPoint -> int -> cvScalar -> int -> unit +val cvEllipse : ('a,[`U8]) iplImage -> cvPoint -> cvSize -> float -> float -> float -> cvScalar -> int -> unit +val cvRectangle : ('a,[`U8]) iplImage -> cvPoint -> cvPoint -> cvScalar -> int -> unit +val cvLine : ('a,[`U8]) iplImage -> cvPoint -> cvPoint -> cvScalar -> int -> unit + +(**/**) +val image_data_order : ('a,'b) iplImage -> int +type image_data = (int, Bigarray.int8_unsigned_elt, Bigarray.c_layout) Bigarray.Array2.t +external image_data : ('a,[`U8]) iplImage -> image_data = "ocaml_image_to_bigarray" + +(* contour finding *) + +type contour_retrieval_mode = + | CV_RETR_EXTERNAL + | CV_RETR_LIST + | CV_RETR_CCOMP + | CV_RETR_TREE + +type contour_approximation_method = + | CV_CHAIN_CODE + | CV_CHAIN_APPROX_NONE + | CV_CHAIN_APPROX_SIMPLE + | CV_CHAIN_APPROX_TC89_L1 + | CV_CHAIN_APPROX_TC89_KCOS + | CV_LINK_RUNS + +type seq + +type seq_info = { + h_prev : seq option; + h_next : seq option; + v_prev : seq option; + v_next : seq option; +} + +val seq_info : seq -> seq_info +val find_contours : ?mode:contour_retrieval_mode -> ?meth:contour_approximation_method -> + ([`Channel_1],[`U8]) iplImage -> seq + +val draw_contours : ('a,[`U8]) iplImage -> seq -> cvScalar -> cvScalar -> int -> int -> cvPoint -> unit + +type ellipse = { + ellipse_center : float * float; + ellipse_size : float * float; + ellipse_angle : float; +} + +val fit_ellipse : seq -> ellipse option + +type calib_cb = + | CV_CALIB_CB_ADAPTIVE_THRESH + | CV_CALIB_CB_NORMALIZE_IMAGE + | CV_CALIB_CB_FILTER_QUADS + | CV_CALIB_CB_FAST_CHECK + +type chessboard_corners + +val find_chessboard_corners : ?flags:calib_cb list -> ('a,[`U8]) iplImage -> cvSize -> + chessboard_corners + +val found_chessboard_corners : chessboard_corners -> bool + +val array_of_chessboard_corners : chessboard_corners -> (float*float) array array option + +val draw_chessboard_corners : ([`Channel_3],[`U8]) iplImage -> + chessboard_corners -> unit + +val find_corner_subpix : ?criteria:cvTermCriteria -> ?winSize:cvSize -> + ?zeroZone:cvSize -> ([`Channel_1],[`U8]) iplImage -> chessboard_corners -> unit + +type calibration + +val init_calibration : cvSize -> float -> calibration +val add_calibration_image : + calibration -> ([ `Channel_1 ], [ `U8 ]) iplImage -> calibration +val calibrate : calibration -> float * ( float array array * float array ) + +val init_undistort_map : cvSize -> ( float array array * float array ) -> + ([ `Channel_1 ], [ `F32 ]) iplImage * ([ `Channel_1 ], [ `F32 ]) iplImage + +external cvRemap : + ('a,'b) iplImage -> ('a,'b) iplImage -> + ([`Channel_1],[`F32]) iplImage -> + ([`Channel_1],[`F32]) iplImage -> + unit + = "cvRemap" diff --git a/info/vision/camlcv/src/cvHighGui.ml b/info/vision/camlcv/src/cvHighGui.ml new file mode 100644 index 0000000..bc5ada3 --- /dev/null +++ b/info/vision/camlcv/src/cvHighGui.ml @@ -0,0 +1,53 @@ +open CvCore + +(* image loading from file *) + +type iscolor_ (* int *) +type iscolor = + | CV_LOAD_IMAGE_UNCHANGED + | CV_LOAD_IMAGE_GRAYSCALE + | CV_LOAD_IMAGE_COLOR + | CV_LOAD_IMAGE_ANYDEPTH + | CV_LOAD_IMAGE_ANYCOLOR + +type 'a load_color = iscolor + +let load_color = CV_LOAD_IMAGE_COLOR +let load_grayscale = CV_LOAD_IMAGE_GRAYSCALE + +external get_iscolor : iscolor -> iscolor_ = "ocaml_get_iscolor" +external cvLoadImage : string -> iscolor_ -> ('a,'b) iplImage = "ocaml_cvLoadImage" + +let load_image file iscolor = cvLoadImage file (get_iscolor iscolor) + +(* capture *) + +type cvCapture +external capture_from_cam : int -> cvCapture = "ocaml_cvCaptureFromCAM" +external query_frame : cvCapture -> ('a,'b) iplImage = "ocaml_cvQueryFrame" + +(* interface *) + +type window_option = + | CV_WINDOW_DEFAULT + | CV_WINDOW_AUTOSIZE + +external cvNamedWindow : string -> int -> unit = "ocaml_cvNamedWindow" +let named_window ?(option=CV_WINDOW_DEFAULT) name = + let opt = + match option with + | CV_WINDOW_DEFAULT -> 0 + | CV_WINDOW_AUTOSIZE -> 1 in + cvNamedWindow name opt + +external wait_key : int -> char = "ocaml_cvWaitKey" +external show_image : string -> ('a,'b) iplImage -> unit = "ocaml_cvShowImage" + +type int_var + +external create_int_var : int -> int_var = "ocaml_create_int_var" +external get_int_var : int_var -> int = "ocaml_get_int_var" +external set_int_var : int_var -> int -> unit = "ocaml_set_int_var" +external cvCreateTrackbar : string -> string -> int_var -> int -> int = "ocaml_cvCreateTrackbar" + +let create_trackbar ~name ~window var max = ignore (cvCreateTrackbar name window var max) diff --git a/info/vision/camlcv/src/cvHighGui.mli b/info/vision/camlcv/src/cvHighGui.mli new file mode 100644 index 0000000..8e93588 --- /dev/null +++ b/info/vision/camlcv/src/cvHighGui.mli @@ -0,0 +1,41 @@ +open CvCore + +(** image loading from file *) + +type 'depth load_color + +val load_color : [`Channel_3] load_color +val load_grayscale : [`Channel_1] load_color + +val load_image : string -> 'depth load_color -> ('depth,[`U8]) iplImage + +(** camera capture *) + +type cvCapture + +val capture_from_cam : int -> cvCapture +val query_frame : cvCapture -> ([`Channel_3],[`U8]) iplImage + +(** Gui interface *) + +type window_option = + | CV_WINDOW_DEFAULT + | CV_WINDOW_AUTOSIZE + +val named_window : ?option:window_option -> string -> unit +(** [named_window name] create a window named [name] *) + +val show_image : string -> ('a,'b) iplImage -> unit +(** [show_image window image] display the image [image] in the window + [window] *) +val wait_key : int -> char +(** [wait_key time] wait for [time] milliseconds for an event to + occur. If [time < 0] it waits indefinitely. Nothing happens in + the gui until this function is called. *) + +type int_var + +val create_int_var : int -> int_var +val get_int_var : int_var -> int +val set_int_var : int_var -> int -> unit +val create_trackbar : name:string -> window:string -> int_var -> int -> unit diff --git a/info/vision/camlcv/src/cv_caml.c b/info/vision/camlcv/src/cv_caml.c index a0fa283..7b70980 100644 --- a/info/vision/camlcv/src/cv_caml.c +++ b/info/vision/camlcv/src/cv_caml.c @@ -49,6 +49,43 @@ int ocaml_get_iscolor(int iscolor) return (Val_int(iscolor_table[Int_val(iscolor)])); } +#define CvMemStorage_val(v) (*(CvMemStorage**) (Data_custom_val(v))) + +void caml_finalize_CvMemStorage(value vstor) +{ + CvMemStorage* stor = CvMemStorage_val(vstor); + if (stor) { + cvReleaseMemStorage(&stor); + CvMemStorage_val(vstor) = 0; + } +} + +static struct custom_operations CvMemStorage_operations = { + "ocaml_opencv_CvMemStorage", + caml_finalize_CvMemStorage, + custom_compare_default, + custom_hash_default, + custom_serialize_default, + custom_deserialize_default +}; + +CAMLexport value ocaml_create_CvMemStorage(value vunit) +{ + CAMLparam1(vunit); + CAMLlocal1 (res); + CvMemStorage *stor = cvCreateMemStorage(0); + res = caml_alloc_custom(&CvMemStorage_operations, sizeof(CvMemStorage*), 1, 10); + CvMemStorage_val(res) = stor; + CAMLreturn (res); +} + +CAMLexport value ocaml_free_CvMemStorage(value vstor) +{ + CAMLparam1(vstor); + caml_finalize_CvMemStorage(vstor); + CAMLreturn(Val_unit); +} + struct image { IplImage* image; @@ -75,8 +112,8 @@ static struct custom_operations IplImage_operations = { CAMLexport value caml_alloc_IplImage(IplImage *image) { CAMLparam0(); - CAMLlocal1 (res); - res = caml_alloc_custom(&IplImage_operations, sizeof(struct image), 1, 1000); + CAMLlocal1(res); + res = caml_alloc_custom(&IplImage_operations, sizeof(struct image), 1, 10); Image_val(res)->image = image; Imag... [truncated message content] |