From cf4c2c50e75e6b21d7aa65211031417619e4dacd Mon Sep 17 00:00:00 2001 From: dpeter99 Date: Fri, 30 Mar 2018 20:54:08 +0200 Subject: Some tests --- bindings/cs/Demo/Program.cs | 46 ++------ bindings/cs/libsurvive.net/SurviveContext.cs | 93 ++++++++++++---- bindings/cs/libsurvive.net/cfunctions.cs | 132 ++++++++++++++--------- bindings/cs/libsurvive.net/libsurvive.net.csproj | 2 +- 4 files changed, 165 insertions(+), 108 deletions(-) diff --git a/bindings/cs/Demo/Program.cs b/bindings/cs/Demo/Program.cs index c03d83c..aa98b6c 100644 --- a/bindings/cs/Demo/Program.cs +++ b/bindings/cs/Demo/Program.cs @@ -7,50 +7,22 @@ using System.Threading.Tasks; namespace Demo { - internal class MyHandler : SurviveContext + + class Program { - private static void WritePose(string name, SurvivePose pose) + static void Main(string[] args) { - Console.Out.WriteLine(name); - Console.Out.Write(" [ "); - for (int i = 0; i < 3; i++) - Console.Out.Write("{0} ", pose.Pos[i]); - Console.Out.Write(" ] [ "); - for (int i = 0; i < 4; i++) - Console.Out.Write("{0} ", pose.Rot[i]); - Console.Out.Write(" ] "); - Console.Out.WriteLine(); - } + SurviveContext context = new SurviveContext(args); + context.AddPoseUpdateCallback(HMDUpdate, -1); - public MyHandler() : base() - { - } - public MyHandler(string[] args) : base(args) - { - } + while (context.Poll() == 0) { + } - protected void LightHouseEvent1(IntPtr ctx, byte lighthouse, SurvivePose lighthouse_pose, SurvivePose object_pose) - { - base.LightHouseEvent(ctx, lighthouse, lighthouse_pose, object_pose); - WritePose("Lighthouse", lighthouse_pose); - WritePose("Object", object_pose); } - - protected override void PoseEvent(IntPtr so, byte lighthouse, SurvivePose pose) - { - WritePose("Pose", pose); - base.PoseEvent(so, lighthouse, pose); - } - } - class Program - { - static void Main(string[] args) + + public static void HMDUpdate(int ObjectID, Vector3 pos) { - MyHandler handler = new MyHandler(args); - - while (handler.Poll() == 0) { - } } diff --git a/bindings/cs/libsurvive.net/SurviveContext.cs b/bindings/cs/libsurvive.net/SurviveContext.cs index bafcf2b..37f67e4 100644 --- a/bindings/cs/libsurvive.net/SurviveContext.cs +++ b/bindings/cs/libsurvive.net/SurviveContext.cs @@ -1,4 +1,5 @@ using System; +using System.Collections.Generic; namespace libsurvive { @@ -8,16 +9,17 @@ namespace libsurvive public class SurviveContext : IDisposable { + public delegate void PoseUpdate(int ObjectID, Vector3 pos); + + protected IntPtr ctx; public void Dispose() { - cfunctions.survive_close(ctx); + cfunctions.Survive_close(ctx); ctx = IntPtr.Zero; } - public SurviveContext() : this(null) { } - /** * We need to keep these delegates around for the lifespan of this object. * if we just pass the functions in, it creates temporary delegates that @@ -34,12 +36,28 @@ namespace libsurvive text_feedback_func info_func; public SurviveContext(string[] args) + { + Init(args); + } + + public SurviveContext(string replaydata) + { + string[] vs = new[] { "--playback", "lightcap-reformat.log"}; + Init(vs); + } + + internal void Init(string[] args) { string[] newArgs = new string[args.Length + 1]; newArgs[0] = System.Reflection.Assembly.GetEntryAssembly().FullName; Array.Copy(args, 0, newArgs, 1, args.Length); - ctx = cfunctions.survive_init_internal(newArgs.Length, newArgs); + ctx = cfunctions.Survive_init_internal(newArgs.Length, newArgs); + + if (ctx == IntPtr.Zero) + { + throw new Exception("There was a problem initializing the lib!"); + } light_Process_Func = LightEvent; raw_Pose_Func = PoseEvent; @@ -51,17 +69,29 @@ namespace libsurvive error_func = ErrorEvent; info_func = InfoEvent; - cfunctions.survive_install_raw_pose_fn(ctx, raw_Pose_Func); - cfunctions.survive_install_light_fn(ctx, light_Process_Func); - cfunctions.survive_install_lighthouse_pose_fn(ctx, lighthouse_Pose_Func); - cfunctions.survive_install_angle_fn(ctx, angle_Process_Func); - cfunctions.survive_install_button_fn(ctx, button_Process_Func); - cfunctions.survive_install_htc_config_fn(ctx, htc_Config_Func); - cfunctions.survive_install_imu_fn(ctx, imu_Process_Func); - cfunctions.survive_install_error_fn(ctx, error_func); - cfunctions.survive_install_info_fn(ctx, info_func); + cfunctions.Survive_install_raw_pose_fn(ctx, raw_Pose_Func); + cfunctions.Survive_install_light_fn(ctx, light_Process_Func); + cfunctions.Survive_install_lighthouse_pose_fn(ctx, lighthouse_Pose_Func); + cfunctions.Survive_install_angle_fn(ctx, angle_Process_Func); + cfunctions.Survive_install_button_fn(ctx, button_Process_Func); + cfunctions.Survive_install_htc_config_fn(ctx, htc_Config_Func); + cfunctions.Survive_install_imu_fn(ctx, imu_Process_Func); + cfunctions.Survive_install_error_fn(ctx, error_func); + cfunctions.Survive_install_info_fn(ctx, info_func); } + + //Dictionary PoseUpdateCallbacks = new Dictionary(); + PoseUpdate poseUpdate; + + public void AddPoseUpdateCallback(PoseUpdate update, int objectID) + { + //PoseUpdateCallbacks[objectID] += update; + poseUpdate += update; + } + + + virtual protected void InfoEvent(SurviveObjectPtr ctx, string fault) { Console.Out.WriteLine(fault); @@ -74,42 +104,61 @@ namespace libsurvive virtual protected void IMUEvent(SurviveObjectPtr so, int mask, double[] accelgyro, uint timecode, int id) { - cfunctions.survive_default_imu_process(so, mask, accelgyro, timecode, id); + cfunctions.Survive_default_imu_process(so, mask, accelgyro, timecode, id); } virtual protected int HTCConfigEvent(SurviveObjectPtr so, string ct0conf, int len) { - return cfunctions.survive_default_htc_config_process(so, ct0conf, len); + return cfunctions.Survive_default_htc_config_process(so, ct0conf, len); } virtual protected void ButtonEvent(SurviveObjectPtr so, byte eventType, byte buttonId, byte axis1Id, ushort axis1Val, byte axis2Id, ushort axis2Val) { - cfunctions.survive_default_button_process(so, eventType, buttonId, axis1Id, axis1Val, axis2Id, axis2Val); + cfunctions.Survive_default_button_process(so, eventType, buttonId, axis1Id, axis1Val, axis2Id, axis2Val); } virtual protected void AngleEvent(SurviveObjectPtr so, int sensor_id, int acode, uint timecode, double length, double angle, uint lh) { - cfunctions.survive_default_angle_process(so, sensor_id, acode, timecode, length, angle, lh); + cfunctions.Survive_default_angle_process(so, sensor_id, acode, timecode, length, angle, lh); } protected void LightHouseEvent(SurviveObjectPtr ctx, byte lighthouse, SurvivePose lighthouse_pose, SurvivePose object_pose) { - cfunctions.survive_default_lighthouse_pose_process(ctx, lighthouse, lighthouse_pose, object_pose); + cfunctions.Survive_default_lighthouse_pose_process(ctx, lighthouse, lighthouse_pose, object_pose); } virtual protected void LightEvent(SurviveObjectPtr so, int sensor_id, int acode, int timeinsweep, UInt32 timecode, UInt32 length, UInt32 lighthouse) { - cfunctions.survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode, length, lighthouse); + cfunctions.Survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode, length, lighthouse); + + //Console.WriteLine("LightEvent"); } - virtual protected void PoseEvent(IntPtr so, byte lighthouse, SurvivePose pose) + virtual protected void PoseEvent(SurviveObjectPtr so, byte lighthouse, SurvivePose pose) { - cfunctions.survive_default_raw_pose_process(so, lighthouse, pose); + cfunctions.Survive_default_raw_pose_process(so, lighthouse, pose); + + Console.WriteLine("PoseEvent"); } public int Poll() { - return cfunctions.survive_poll(ctx); + return cfunctions.Survive_poll(ctx); } } + + public struct Vector3 + { + public float x; + public float y; + public float z; + } + + public struct Qutarnion + { + public float x; + public float y; + public float z; + public float w; + } } \ No newline at end of file diff --git a/bindings/cs/libsurvive.net/cfunctions.cs b/bindings/cs/libsurvive.net/cfunctions.cs index 760681e..6715a36 100644 --- a/bindings/cs/libsurvive.net/cfunctions.cs +++ b/bindings/cs/libsurvive.net/cfunctions.cs @@ -18,74 +18,110 @@ namespace libsurvive public double[] Rot; // Quaternion in the form wxyz } + [StructLayout(LayoutKind.Sequential)] + public class SurviveObject + { + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)] + public char[] codename; //3 letters, null-terminated. Currently HMD, WM0, WM1. + } + class cfunctions { -#pragma warning disable IDE1006 // Naming Styles - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall) ] - public static extern SurviveContextPtr survive_init_internal(int argc, string[] args); - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - public static extern SurviveContextPtr survive_close(SurviveContextPtr ctx); - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - public static extern int survive_poll(SurviveContextPtr ctx); - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - public static extern int survive_startup(SurviveContextPtr ctx); - - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - public static extern void survive_install_htc_config_fn(SurviveContextPtr ctx, htc_config_func fbp); - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - public static extern void survive_install_info_fn(SurviveContextPtr ctx, text_feedback_func fbp); - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - public static extern void survive_install_error_fn(SurviveContextPtr ctx, text_feedback_func fbp); - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - public static extern void survive_install_imu_fn(SurviveContextPtr ctx, imu_process_func fbp); - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - public static extern void survive_install_angle_fn(SurviveContextPtr ctx, angle_process_func fbp); - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - public static extern void survive_install_button_fn(SurviveContextPtr ctx, button_process_func fbp); - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - public static extern void survive_install_raw_pose_fn(SurviveContextPtr ctx, raw_pose_func fbp); - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - public static extern void survive_install_lighthouse_pose_fn(SurviveContextPtr ctx, lighthouse_pose_func fbp); - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - public static extern void survive_install_light_fn(SurviveContextPtr ctx, light_process_func fbp); - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - public static extern void survive_cal_install(SurviveContextPtr ctx); - - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - public static extern void survive_default_light_process(SurviveObjectPtr so, int sensor_id, int acode, int timeinsweep, UInt32 timecode, UInt32 length, UInt32 lh); - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - public static extern void survive_default_imu_process(SurviveObjectPtr so, int mode, double[] accelgyro, UInt32 timecode, int id); - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - public static extern void survive_default_angle_process(SurviveObjectPtr so, int sensor_id, int acode, UInt32 timecode, double length, double angle, UInt32 lh); - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - public static extern void survive_default_button_process(SurviveObjectPtr so, byte eventType, byte buttonId, byte axis1Id, UInt16 axis1Val, byte axis2Id, UInt16 axis2Val); - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - public static extern void survive_default_raw_pose_process(SurviveObjectPtr so, byte lighthouse, SurvivePose pose); - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - public static extern void survive_default_lighthouse_pose_process(SurviveContextPtr ctx, byte lighthouse, SurvivePose lh_pose, - SurvivePose obj_pose); - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - public static extern int survive_default_htc_config_process(SurviveObjectPtr so, string ct0conf, int len); - -#pragma warning restore IDE1006 // Naming Styles +//#pragma warning disable IDE1006 // Naming Styles + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_init_internal") ] + public static extern SurviveContextPtr Survive_init_internal(int argc, string[] args); + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_close")] + public static extern SurviveContextPtr Survive_close(SurviveContextPtr ctx); + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_poll")] + public static extern int Survive_poll(SurviveContextPtr ctx); + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_startup")] + public static extern int Survive_startup(SurviveContextPtr ctx); + + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_install_htc_config_fn")] + public static extern void Survive_install_htc_config_fn(SurviveContextPtr ctx, htc_config_func fbp); + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_install_info_fn")] + public static extern void Survive_install_info_fn(SurviveContextPtr ctx, text_feedback_func fbp); + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_install_error_fn")] + public static extern void Survive_install_error_fn(SurviveContextPtr ctx, text_feedback_func fbp); + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_install_imu_fn")] + public static extern void Survive_install_imu_fn(SurviveContextPtr ctx, imu_process_func fbp); + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_install_angle_fn")] + public static extern void Survive_install_angle_fn(SurviveContextPtr ctx, angle_process_func fbp); + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_install_button_fn")] + public static extern void Survive_install_button_fn(SurviveContextPtr ctx, button_process_func fbp); + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_install_raw_pose_fn")] + public static extern void Survive_install_raw_pose_fn(SurviveContextPtr ctx, raw_pose_func fbp); + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_install_lighthouse_pose_fn")] + public static extern void Survive_install_lighthouse_pose_fn(SurviveContextPtr ctx, lighthouse_pose_func fbp); + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_install_light_fn")] + public static extern void Survive_install_light_fn(SurviveContextPtr ctx, light_process_func fbp); + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_cal_install")] + public static extern void Survive_cal_install(SurviveContextPtr ctx); + + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_default_light_process")] + public static extern void Survive_default_light_process(SurviveObjectPtr so, int sensor_id, int acode, int timeinsweep, UInt32 timecode, UInt32 length, UInt32 lh); + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_default_imu_process")] + public static extern void Survive_default_imu_process(SurviveObjectPtr so, int mode, double[] accelgyro, UInt32 timecode, int id); + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_default_angle_process")] + public static extern void Survive_default_angle_process(SurviveObjectPtr so, int sensor_id, int acode, UInt32 timecode, double length, double angle, UInt32 lh); + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_default_button_process")] + public static extern void Survive_default_button_process(SurviveObjectPtr so, byte eventType, byte buttonId, byte axis1Id, UInt16 axis1Val, byte axis2Id, UInt16 axis2Val); + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_default_raw_pose_process")] + public static extern void Survive_default_raw_pose_process(SurviveObjectPtr so, byte lighthouse, SurvivePose pose); + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_default_lighthouse_pose_process")] + public static extern void Survive_default_lighthouse_pose_process(SurviveContextPtr ctx, byte lighthouse, SurvivePose lh_pose, SurvivePose obj_pose); + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_default_htc_config_process")] + public static extern int Survive_default_htc_config_process(SurviveObjectPtr so, string ct0conf, int len); + + +//#pragma warning restore IDE1006 // Naming Styles } + + [UnmanagedFunctionPointer(CallingConvention.StdCall)] public delegate int htc_config_func(SurviveObjectPtr so, string ct0conf, int len); [UnmanagedFunctionPointer(CallingConvention.StdCall)] public delegate void text_feedback_func(SurviveContextPtr ctx, string fault); + [UnmanagedFunctionPointer(CallingConvention.StdCall)] public delegate void light_process_func(SurviveObjectPtr so, int sensor_id, int acode, int timeinsweep, UInt32 timecode, UInt32 length, UInt32 lighthouse); + [UnmanagedFunctionPointer(CallingConvention.StdCall)] public delegate void imu_process_func(SurviveObjectPtr so, int mask, double[] accelgyro, UInt32 timecode, int id); + [UnmanagedFunctionPointer(CallingConvention.StdCall)] public delegate void angle_process_func(SurviveObjectPtr so, int sensor_id, int acode, UInt32 timecode, double length, double angle, UInt32 lh); + [UnmanagedFunctionPointer(CallingConvention.StdCall)] public delegate void button_process_func(SurviveObjectPtr so, byte eventType, byte buttonId, byte axis1Id, UInt16 axis1Val, byte axis2Id, UInt16 axis2Val); + [UnmanagedFunctionPointer(CallingConvention.StdCall)] public delegate void raw_pose_func(SurviveObjectPtr so, byte lighthouse, SurvivePose pose); + [UnmanagedFunctionPointer(CallingConvention.StdCall)] public delegate void lighthouse_pose_func(SurviveContextPtr ctx, byte lighthouse, SurvivePose lighthouse_pose, SurvivePose object_pose); + } diff --git a/bindings/cs/libsurvive.net/libsurvive.net.csproj b/bindings/cs/libsurvive.net/libsurvive.net.csproj index eac3503..9f5c4f4 100644 --- a/bindings/cs/libsurvive.net/libsurvive.net.csproj +++ b/bindings/cs/libsurvive.net/libsurvive.net.csproj @@ -1,7 +1,7 @@ - netstandard1.6 + netstandard2.0 -- cgit v1.2.3 From ee3160032cd6a609a490be011019b323fe1c0c02 Mon Sep 17 00:00:00 2001 From: dpeter99 Date: Sun, 1 Apr 2018 17:36:08 +0200 Subject: Added new accesors (WIP) --- bindings/cs/libsurvive.net/cfunctions.cs | 24 +++++++++++++++++++++++- include/libsurvive/survive.h | 7 +++++++ src/survive.c | 16 +++++++++++++--- 3 files changed, 43 insertions(+), 4 deletions(-) diff --git a/bindings/cs/libsurvive.net/cfunctions.cs b/bindings/cs/libsurvive.net/cfunctions.cs index 6715a36..67812f5 100644 --- a/bindings/cs/libsurvive.net/cfunctions.cs +++ b/bindings/cs/libsurvive.net/cfunctions.cs @@ -94,7 +94,29 @@ namespace libsurvive public static extern int Survive_default_htc_config_process(SurviveObjectPtr so, string ct0conf, int len); -//#pragma warning restore IDE1006 // Naming Styles + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_codename")] + public static extern char Survive_object_codename(SurviveObjectPtr so); + + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_drivername")] + public static extern char Survive_object_drivername(SurviveObject so); + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_charge")] + public static extern byte Survive_object_charge(SurviveObject so); + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_charging")] + public static extern bool Survive_object_charging(SurviveObject so); + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_pose")] + public static extern SurvivePose Survive_object_pose(SurviveObject so); + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_sensor_locations")] + public static extern int Survive_object_sensor_locations(SurviveObjectPtr so); + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_sensor_normals")] + public static extern int Survive_object_sensor_normals(SurviveObjectPtr so); + + //#pragma warning restore IDE1006 // Naming Styles } diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 7248b1c..8a4183e 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -120,6 +120,13 @@ struct SurviveObject { // These exports are mostly for language binding against SURVIVE_EXPORT const char *survive_object_codename(SurviveObject *so); + +SURVIVE_EXPORT const char *survive_object_drivername(SurviveObject *so); +SURVIVE_EXPORT const int8_t *survive_object_charge(SurviveObject *so); +SURVIVE_EXPORT const bool *survive_object_charging(SurviveObject *so); + +SURVIVE_EXPORT const SurvivePose *survive_object_pose(SurviveObject *so); + SURVIVE_EXPORT int8_t survive_object_sensor_ct(SurviveObject *so); SURVIVE_EXPORT const FLT *survive_object_sensor_locations(SurviveObject *so); SURVIVE_EXPORT const FLT *survive_object_sensor_normals(SurviveObject *so); diff --git a/src/survive.c b/src/survive.c index 2a7aad1..e4015de 100644 --- a/src/survive.c +++ b/src/survive.c @@ -559,9 +559,19 @@ int survive_simple_inflate(struct SurviveContext *ctx, const char *input, int in return len; } + + +#endif + + const char *survive_object_codename(SurviveObject *so) { return so->codename; } + +const char *survive_object_drivername(SurviveObject *so) { return so->drivername; } +const int8_t *survive_object_charge(SurviveObject *so) { return so->charge; } +const bool *survive_object_charging(SurviveObject *so) { return so->charging; } + +const SurvivePose *survive_object_pose(SurviveObject *so) { return &so->OutPose; } + int8_t survive_object_sensor_ct(SurviveObject *so) { return so->sensor_ct; } const FLT *survive_object_sensor_locations(SurviveObject *so) { return so->sensor_locations; } -const FLT *survive_object_sensor_normals(SurviveObject *so) { return so->sensor_normals; } - -#endif +const FLT *survive_object_sensor_normals(SurviveObject *so) { return so->sensor_normals; } \ No newline at end of file -- cgit v1.2.3 From a1717cad950d9f867fbe8aa8a6ab6cf2d8704a64 Mon Sep 17 00:00:00 2001 From: dpeter99 Date: Sun, 1 Apr 2018 17:36:50 +0200 Subject: Removed the manual .def file --- winbuild/libsurvive/libsurvive.def | 30 ------------------------------ 1 file changed, 30 deletions(-) delete mode 100644 winbuild/libsurvive/libsurvive.def diff --git a/winbuild/libsurvive/libsurvive.def b/winbuild/libsurvive/libsurvive.def deleted file mode 100644 index 3a50ce6..0000000 --- a/winbuild/libsurvive/libsurvive.def +++ /dev/null @@ -1,30 +0,0 @@ -LIBRARY LIBSURVIVE -EXPORTS - survive_verify_FLT_size - survive_init_internal - survive_startup - survive_poll - survive_close - survive_configi - survive_configs - survive_install_light_fn - survive_install_imu_fn - survive_install_angle_fn - survive_install_htc_config_fn - survive_send_magic - survive_cal_install - survive_cal_get_status - survive_default_light_process - survive_default_imu_process - survive_default_angle_process - survive_install_button_fn - survive_install_raw_pose_fn - survive_install_lighthouse_pose_fn - survive_get_so_by_name - survive_haptic - survive_default_button_process - survive_default_raw_pose_process - survive_default_lighthouse_pose_process - quatnormalize - quatrotatevector - \ No newline at end of file -- cgit v1.2.3 From c05e42e756e8e1560adc22aacde87dbdf2e02b45 Mon Sep 17 00:00:00 2001 From: dpeter99 Date: Sun, 1 Apr 2018 17:37:11 +0200 Subject: Updated the launch parameters --- bindings/cs/libsurvive.net/SurviveContext.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bindings/cs/libsurvive.net/SurviveContext.cs b/bindings/cs/libsurvive.net/SurviveContext.cs index 37f67e4..084df31 100644 --- a/bindings/cs/libsurvive.net/SurviveContext.cs +++ b/bindings/cs/libsurvive.net/SurviveContext.cs @@ -42,7 +42,7 @@ namespace libsurvive public SurviveContext(string replaydata) { - string[] vs = new[] { "--playback", "lightcap-reformat.log"}; + string[] vs = new[] { "--playback", "P:/c/libsurvive-data/lightcap-reformat/lightcap-reformat.log", "--disambiguator", "StateBased", "--calibrate" }; Init(vs); } -- cgit v1.2.3 From 0eea189db82fc72463a83a678d9b8d609fd09c6e Mon Sep 17 00:00:00 2001 From: dpeter99 Date: Sun, 1 Apr 2018 18:58:19 +0200 Subject: Added the API file from Unity --- bindings/cs/libsurvive.net/LibSurViveAPI.cs | 342 ++++++++++++++++++++++++++++ bindings/cs/libsurvive.net/cfunctions.cs | 35 ++- winbuild/libsurvive/libsurvive.vcxproj | 3 +- 3 files changed, 361 insertions(+), 19 deletions(-) create mode 100644 bindings/cs/libsurvive.net/LibSurViveAPI.cs diff --git a/bindings/cs/libsurvive.net/LibSurViveAPI.cs b/bindings/cs/libsurvive.net/LibSurViveAPI.cs new file mode 100644 index 0000000..48336e8 --- /dev/null +++ b/bindings/cs/libsurvive.net/LibSurViveAPI.cs @@ -0,0 +1,342 @@ +//using UnityEngine; +using System.Collections; +using System.Collections.Generic; + +using libsurvive; +using System; +using System.Threading; + +public class LibSurViveAPI +{ + + public struct SetupConfigs + { + public string playbaskFile; + public int playbackFactor; + public Disambiguator disambiguator; + public Poser poser; + public BoolConfig calibrate; + public string configFile; + } + + public enum Disambiguator + { + StateBased, + Charles, + Turvey, + Default + } + + public enum Poser + { + CharlesSlow, + DaveOrtho, + Dummy, + EPNP, + SBA + } + + public enum BoolConfig + { + Yes, + No, + Default + } + + + private static LibSurViveAPI _instance; + public static LibSurViveAPI instance + { + get + { + if (_instance == null) + { + _instance = new LibSurViveAPI(); + } + + return _instance; + } + } + + LibSurViveAPI() + { + CreateContext(); + + CreateTread(); + + } + + ~LibSurViveAPI() + { + running = false; + } + + Thread internalPollTread; + + private void CreateTread() + { + internalPollTread = new Thread(InternalPoll); + internalPollTread.Start(); + } + + bool running = true; + + void InternalPoll() + { + while (running) + { + cfunctions.Survive_poll(context); + } + } + + //private SurviveContext _context; + public IntPtr context; + + light_process_func light_Process_Func; + raw_pose_func raw_Pose_Func; + lighthouse_pose_func lighthouse_Pose_Func; + angle_process_func angle_Process_Func; + button_process_func button_Process_Func; + htc_config_func htc_Config_Func; + imu_process_func imu_Process_Func; + text_feedback_func error_func; + text_feedback_func info_func; + + internal void CreateContext() + { + //Debug.Log("Start Init"); + + SetupConfigs configs = new SetupConfigs + { + playbaskFile = "P:/c/libsurvive-data/lightcap-reformat/lightcap-reformat.log", + configFile = "survive_conf.json", + playbackFactor = 1 + }; + + string[] args = CreateStartParameters(configs); + + //string[] vs = new[] { "--playback", "P:/c/libsurvive-data/lightcap-reformat/lightcap-reformat.log", "--disambiguator", "StateBased", "--calibrate" }; + + context = cfunctions.Survive_init_internal(args.Length, args); + + if (context == IntPtr.Zero) + { + throw new Exception("There was a problem initializing the lib!"); + } + + light_Process_Func = LightEvent; + raw_Pose_Func = PoseEvent; + lighthouse_Pose_Func = LightHouseEvent; + angle_Process_Func = AngleEvent; + button_Process_Func = ButtonEvent; + htc_Config_Func = HTCConfigEvent; + imu_Process_Func = IMUEvent; + error_func = ErrorEvent; + info_func = InfoEvent; + + cfunctions.Survive_install_raw_pose_fn(context, raw_Pose_Func); + cfunctions.Survive_install_light_fn(context, light_Process_Func); + cfunctions.Survive_install_lighthouse_pose_fn(context, lighthouse_Pose_Func); + cfunctions.Survive_install_angle_fn(context, angle_Process_Func); + cfunctions.Survive_install_button_fn(context, button_Process_Func); + cfunctions.Survive_install_htc_config_fn(context, htc_Config_Func); + cfunctions.Survive_install_imu_fn(context, imu_Process_Func); + cfunctions.Survive_install_error_fn(context, error_func); + cfunctions.Survive_install_info_fn(context, info_func); + + //Debug.Log("Finished Init"); + + //Debug.Log("Start Startup"); + + //Debug.LogError("ASD"); + + int a = 0; + try + { + a = cfunctions.Survive_startup(context); + } + catch (Exception) + { + throw; + } + + if (a != 0) + { + throw new Exception("Error in startup"); + } + + //Debug.Log("Finished Startup"); + + } + + + static public string[] CreateStartParameters(SetupConfigs configs) + { + List args = new List(); + + args.Add("unity"); + + if (configs.playbaskFile != "") + { + args.AddRange(new[] { "--playback", configs.playbaskFile }); + args.AddRange(new[] { "--playback-factor", configs.playbackFactor.ToString() }); + } + + if(configs.disambiguator != Disambiguator.Default) + args.AddRange(new[] { "--disambiguator", Enum.GetName(typeof(Disambiguator), configs.disambiguator) }); + + if (configs.calibrate != BoolConfig.Default) + args.Add(configs.calibrate == BoolConfig.Yes ? "--calibrate" : "--no-calibrate"); + + if (configs.configFile != "") + args.AddRange(new[] { "-c", configs.configFile }); + + //args.AddRange(new[] { "--disambiguator", Enum.GetName(typeof(Poser), disambiguator) }); + + + + return args.ToArray(); + } + + + + + + virtual protected void InfoEvent(IntPtr ctx, string fault) + { + //Debug.Log(fault); + } + + virtual protected void ErrorEvent(IntPtr ctx, string fault) + { + //Debug.LogError(fault); + } + + virtual protected void IMUEvent(IntPtr so, int mask, double[] accelgyro, uint timecode, int id) + { + cfunctions.Survive_default_imu_process(so, mask, accelgyro, timecode, id); + } + + virtual protected int HTCConfigEvent(IntPtr so, string ct0conf, int len) + { + return cfunctions.Survive_default_htc_config_process(so, ct0conf, len); + } + + virtual protected void ButtonEvent(IntPtr so, byte eventType, byte buttonId, byte axis1Id, ushort axis1Val, byte axis2Id, ushort axis2Val) + { + cfunctions.Survive_default_button_process(so, eventType, buttonId, axis1Id, axis1Val, axis2Id, axis2Val); + } + + virtual protected void AngleEvent(IntPtr so, int sensor_id, int acode, uint timecode, double length, double angle, uint lh) + { + cfunctions.Survive_default_angle_process(so, sensor_id, acode, timecode, length, angle, lh); + + //Debug.Log("AngleEvent"); + } + + protected void LightHouseEvent(IntPtr ctx, byte lighthouse, SurvivePose lighthouse_pose, SurvivePose object_pose) + { + cfunctions.Survive_default_lighthouse_pose_process(ctx, lighthouse, lighthouse_pose, object_pose); + + //Debug.Log("LightHouseEvent"); + } + + virtual protected void LightEvent(IntPtr so, int sensor_id, int acode, int timeinsweep, UInt32 timecode, UInt32 length, UInt32 lighthouse) + { + cfunctions.Survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode, length, lighthouse); + + //Console.WriteLine("LightEvent"); + //Debug.Log("LightEvent"); + } + + virtual protected void PoseEvent(IntPtr so, byte lighthouse, SurvivePose pose) + { + cfunctions.Survive_default_raw_pose_process(so, lighthouse, pose); + + //vaDebug.Log("PoseEvent"); + + //poseUpdate(-1, new Vector3((float)pose.Pos[0], (float)pose.Pos[1], (float)pose.Pos[2]), new Quaternion((float)pose.Rot[0], (float)pose.Rot[1], (float)pose.Rot[2], (float)pose.Rot[3])); + + /* + string a = cfunctions.Survive_object_codename(so); + if (updates.ContainsKey(a)) + { + Vector3 pos = new Vector3((float)pose.Pos[0], (float)pose.Pos[1], (float)pose.Pos[2]); + Quaternion rot = new Quaternion((float)pose.Rot[0], (float)pose.Rot[1], (float)pose.Rot[2], (float)pose.Rot[3]); + updates[a](pos, rot); + } + */ + } + + + public delegate void PoseUpdate(SurviveVector3 pos, SurviveQuaternion quat); + + public Dictionary updates = new Dictionary(); + + public void AddCalback(string ID, PoseUpdate update) + { + if (!updates.ContainsKey(ID)) + { + updates.Add(ID, update); + } + else + { + updates[ID] += update; + } + } + + /* + public void Poll() + { + cfunctions.Survive_poll(context); + } + */ + + public SurviveObject GetSurviveObjectByName(string name) + { + if (name == "") + { + throw new Exception("Empty string is not accepted"); + } + + if (context == IntPtr.Zero) + throw new Exception("The context hasn't been initialsied yet"); + + return new SurviveObject( cfunctions.Survive_get_so_by_name(context, name)); + } +} + +public class SurviveObject +{ + public SurvivePose pose { get; private set; } + + public int charge { get; private set; } + public bool charging { get; private set; } + + + public SurviveObject(IntPtr ptr) + { + if (ptr == IntPtr.Zero) + { + throw new Exception("Can't create SurviveObject with 0 pointer"); + } + + //pose = cfunctions.Survive_object_pose(ptr); + charge = cfunctions.Survive_object_charge(ptr); + charging = cfunctions.Survive_object_charging(ptr); + } +} + +public class SurviveVector3 +{ + float x; + float y; + float z; +} + +public class SurviveQuaternion +{ + float x; + float y; + float z; +} diff --git a/bindings/cs/libsurvive.net/cfunctions.cs b/bindings/cs/libsurvive.net/cfunctions.cs index 67812f5..ab5f7fd 100644 --- a/bindings/cs/libsurvive.net/cfunctions.cs +++ b/bindings/cs/libsurvive.net/cfunctions.cs @@ -12,23 +12,16 @@ namespace libsurvive [StructLayout(LayoutKind.Sequential)] public class SurvivePose { - [MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)] + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)] public double[] Pos; // Position in the form xyz [MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)] - public double[] Rot; // Quaternion in the form wxyz - } - - [StructLayout(LayoutKind.Sequential)] - public class SurviveObject - { - [MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)] - public char[] codename; //3 letters, null-terminated. Currently HMD, WM0, WM1. + public double[] Rot; // Quaternion in the form wxyz } class cfunctions { -//#pragma warning disable IDE1006 // Naming Styles - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_init_internal") ] + //#pragma warning disable IDE1006 // Naming Styles + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_init_internal")] public static extern SurviveContextPtr Survive_init_internal(int argc, string[] args); [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_close")] @@ -95,26 +88,32 @@ namespace libsurvive [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_codename")] - public static extern char Survive_object_codename(SurviveObjectPtr so); + public static extern string Survive_object_codename(SurviveObjectPtr so); [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_drivername")] - public static extern char Survive_object_drivername(SurviveObject so); + public static extern char Survive_object_drivername(SurviveObjectPtr so); [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_charge")] - public static extern byte Survive_object_charge(SurviveObject so); + public static extern byte Survive_object_charge(SurviveObjectPtr so); [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_charging")] - public static extern bool Survive_object_charging(SurviveObject so); + public static extern bool Survive_object_charging(SurviveObjectPtr so); [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_pose")] - public static extern SurvivePose Survive_object_pose(SurviveObject so); + public static extern SurvivePose Survive_object_pose(SurviveObjectPtr so); + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_sensor_locations")] - public static extern int Survive_object_sensor_locations(SurviveObjectPtr so); + public static extern double[] Survive_object_sensor_locations(SurviveObjectPtr so); [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_sensor_normals")] - public static extern int Survive_object_sensor_normals(SurviveObjectPtr so); + public static extern double[] Survive_object_sensor_normals(SurviveObjectPtr so); + + + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_get_so_by_name")] + public static extern IntPtr Survive_get_so_by_name(IntPtr ctx, string name); //#pragma warning restore IDE1006 // Naming Styles } diff --git a/winbuild/libsurvive/libsurvive.vcxproj b/winbuild/libsurvive/libsurvive.vcxproj index 5caa159..aefdc00 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj +++ b/winbuild/libsurvive/libsurvive.vcxproj @@ -91,7 +91,8 @@ true $(IntDir)Test.txt true - $(MSBuildProjectDirectory)\libsurvive.def + + -- cgit v1.2.3 From 7770495e7f14fe5094b31df5f5976c34fd68a0da Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 1 Apr 2018 11:48:56 -0600 Subject: Fixed marshalling --- bindings/cs/Demo/Program.cs | 11 ++++------- bindings/cs/libsurvive.net/LibSurViveAPI.cs | 7 +++++-- bindings/cs/libsurvive.net/cfunctions.cs | 2 +- include/libsurvive/survive.h | 4 ++-- src/survive.c | 4 ++-- 5 files changed, 14 insertions(+), 14 deletions(-) diff --git a/bindings/cs/Demo/Program.cs b/bindings/cs/Demo/Program.cs index aa98b6c..53c2307 100644 --- a/bindings/cs/Demo/Program.cs +++ b/bindings/cs/Demo/Program.cs @@ -12,13 +12,10 @@ namespace Demo { static void Main(string[] args) { - SurviveContext context = new SurviveContext(args); - context.AddPoseUpdateCallback(HMDUpdate, -1); - - - while (context.Poll() == 0) { - } - + LibSurViveAPI api = LibSurViveAPI.instance; + + var so = api.GetSurviveObjectByName("HMD"); + } public static void HMDUpdate(int ObjectID, Vector3 pos) diff --git a/bindings/cs/libsurvive.net/LibSurViveAPI.cs b/bindings/cs/libsurvive.net/LibSurViveAPI.cs index 48336e8..aa2fdae 100644 --- a/bindings/cs/libsurvive.net/LibSurViveAPI.cs +++ b/bindings/cs/libsurvive.net/LibSurViveAPI.cs @@ -5,6 +5,7 @@ using System.Collections.Generic; using libsurvive; using System; using System.Threading; +using System.Runtime.InteropServices; public class LibSurViveAPI { @@ -108,7 +109,7 @@ public class LibSurViveAPI SetupConfigs configs = new SetupConfigs { - playbaskFile = "P:/c/libsurvive-data/lightcap-reformat/lightcap-reformat.log", + playbaskFile = @"C:\Users\justin\source\libsurvive-win-update\bindings\cs\Demo\lightcap-reformat.log", configFile = "survive_conf.json", playbackFactor = 1 }; @@ -320,8 +321,10 @@ public class SurviveObject { throw new Exception("Can't create SurviveObject with 0 pointer"); } + var ptr1 = cfunctions.Survive_object_pose(ptr); + pose = (SurvivePose)Marshal.PtrToStructure(ptr1, typeof(SurvivePose)); - //pose = cfunctions.Survive_object_pose(ptr); + //pose = charge = cfunctions.Survive_object_charge(ptr); charging = cfunctions.Survive_object_charging(ptr); } diff --git a/bindings/cs/libsurvive.net/cfunctions.cs b/bindings/cs/libsurvive.net/cfunctions.cs index ab5f7fd..ce46d18 100644 --- a/bindings/cs/libsurvive.net/cfunctions.cs +++ b/bindings/cs/libsurvive.net/cfunctions.cs @@ -101,7 +101,7 @@ namespace libsurvive public static extern bool Survive_object_charging(SurviveObjectPtr so); [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_pose")] - public static extern SurvivePose Survive_object_pose(SurviveObjectPtr so); + public static extern IntPtr Survive_object_pose(SurviveObjectPtr so); [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_sensor_locations")] diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 8a4183e..b829a52 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -122,8 +122,8 @@ struct SurviveObject { SURVIVE_EXPORT const char *survive_object_codename(SurviveObject *so); SURVIVE_EXPORT const char *survive_object_drivername(SurviveObject *so); -SURVIVE_EXPORT const int8_t *survive_object_charge(SurviveObject *so); -SURVIVE_EXPORT const bool *survive_object_charging(SurviveObject *so); +SURVIVE_EXPORT const int8_t survive_object_charge(SurviveObject *so); +SURVIVE_EXPORT const bool survive_object_charging(SurviveObject *so); SURVIVE_EXPORT const SurvivePose *survive_object_pose(SurviveObject *so); diff --git a/src/survive.c b/src/survive.c index e4015de..f74de85 100644 --- a/src/survive.c +++ b/src/survive.c @@ -567,8 +567,8 @@ int survive_simple_inflate(struct SurviveContext *ctx, const char *input, int in const char *survive_object_codename(SurviveObject *so) { return so->codename; } const char *survive_object_drivername(SurviveObject *so) { return so->drivername; } -const int8_t *survive_object_charge(SurviveObject *so) { return so->charge; } -const bool *survive_object_charging(SurviveObject *so) { return so->charging; } +const int8_t survive_object_charge(SurviveObject *so) { return so->charge; } +const bool survive_object_charging(SurviveObject *so) { return so->charging; } const SurvivePose *survive_object_pose(SurviveObject *so) { return &so->OutPose; } -- cgit v1.2.3 From 4fc4476108f0ad49a4dce99ff2506c3da9b3994a Mon Sep 17 00:00:00 2001 From: dpeter99 Date: Sun, 1 Apr 2018 21:28:19 +0200 Subject: Fixed the parameters --- bindings/cs/libsurvive.net/LibSurViveAPI.cs | 40 +++++++++++++++++++++-------- 1 file changed, 29 insertions(+), 11 deletions(-) diff --git a/bindings/cs/libsurvive.net/LibSurViveAPI.cs b/bindings/cs/libsurvive.net/LibSurViveAPI.cs index aa2fdae..922a055 100644 --- a/bindings/cs/libsurvive.net/LibSurViveAPI.cs +++ b/bindings/cs/libsurvive.net/LibSurViveAPI.cs @@ -309,24 +309,42 @@ public class LibSurViveAPI public class SurviveObject { - public SurvivePose pose { get; private set; } + private SurvivePose _pose; + public SurvivePose Pose + { + get + { + var ptr1 = cfunctions.Survive_object_pose(ptr); + return (SurvivePose)Marshal.PtrToStructure(ptr1, typeof(SurvivePose)); + } + } - public int charge { get; private set; } - public bool charging { get; private set; } + private int _charge; + public int Charge + { + get + { + return cfunctions.Survive_object_charge(ptr); + } + } + public bool Charging + { + get + { + return cfunctions.Survive_object_charging(ptr); + } + } + IntPtr ptr; + private bool _charging; - public SurviveObject(IntPtr ptr) + public SurviveObject(IntPtr obj_ptr) { - if (ptr == IntPtr.Zero) + if (obj_ptr == IntPtr.Zero) { throw new Exception("Can't create SurviveObject with 0 pointer"); } - var ptr1 = cfunctions.Survive_object_pose(ptr); - pose = (SurvivePose)Marshal.PtrToStructure(ptr1, typeof(SurvivePose)); - - //pose = - charge = cfunctions.Survive_object_charge(ptr); - charging = cfunctions.Survive_object_charging(ptr); + ptr = obj_ptr; } } -- cgit v1.2.3 From cc3931e2b6a94dd7b685331bd965f7f5537622e4 Mon Sep 17 00:00:00 2001 From: dpeter99 Date: Sun, 1 Apr 2018 21:42:51 +0200 Subject: Removed unity specific stuff --- bindings/cs/libsurvive.net/LibSurViveAPI.cs | 226 +++++++++++++-------------- bindings/cs/libsurvive.net/SurviveContext.cs | 38 ++--- bindings/cs/libsurvive.net/cfunctions.cs | 2 +- 3 files changed, 127 insertions(+), 139 deletions(-) diff --git a/bindings/cs/libsurvive.net/LibSurViveAPI.cs b/bindings/cs/libsurvive.net/LibSurViveAPI.cs index 922a055..ff6d100 100644 --- a/bindings/cs/libsurvive.net/LibSurViveAPI.cs +++ b/bindings/cs/libsurvive.net/LibSurViveAPI.cs @@ -1,5 +1,4 @@ -//using UnityEngine; -using System.Collections; +using System.Collections; using System.Collections.Generic; using libsurvive; @@ -9,44 +8,8 @@ using System.Runtime.InteropServices; public class LibSurViveAPI { - - public struct SetupConfigs - { - public string playbaskFile; - public int playbackFactor; - public Disambiguator disambiguator; - public Poser poser; - public BoolConfig calibrate; - public string configFile; - } - - public enum Disambiguator - { - StateBased, - Charles, - Turvey, - Default - } - - public enum Poser - { - CharlesSlow, - DaveOrtho, - Dummy, - EPNP, - SBA - } - - public enum BoolConfig - { - Yes, - No, - Default - } - - private static LibSurViveAPI _instance; - public static LibSurViveAPI instance + public static LibSurViveAPI Instance { get { @@ -59,12 +22,32 @@ public class LibSurViveAPI } } + bool running = true; + Thread internalPollTread; + + public IntPtr context; + + light_process_func light_Process_Func; + raw_pose_func raw_Pose_Func; + lighthouse_pose_func lighthouse_Pose_Func; + angle_process_func angle_Process_Func; + button_process_func button_Process_Func; + htc_config_func htc_Config_Func; + imu_process_func imu_Process_Func; + text_feedback_func error_func; + text_feedback_func info_func; + + public delegate void Log(string message); + Log LogInfo; + + public delegate void ErrorLog(string message); + ErrorLog LogError; + LibSurViveAPI() { CreateContext(); CreateTread(); - } ~LibSurViveAPI() @@ -72,40 +55,28 @@ public class LibSurViveAPI running = false; } - Thread internalPollTread; - private void CreateTread() { internalPollTread = new Thread(InternalPoll); internalPollTread.Start(); } - bool running = true; - void InternalPoll() { while (running) { - cfunctions.Survive_poll(context); + int code = Cfunctions.Survive_poll(context); + + if (code != 0) + { + running = false; + } } } - //private SurviveContext _context; - public IntPtr context; - - light_process_func light_Process_Func; - raw_pose_func raw_Pose_Func; - lighthouse_pose_func lighthouse_Pose_Func; - angle_process_func angle_Process_Func; - button_process_func button_Process_Func; - htc_config_func htc_Config_Func; - imu_process_func imu_Process_Func; - text_feedback_func error_func; - text_feedback_func info_func; - internal void CreateContext() { - //Debug.Log("Start Init"); + LogInfo("Start Init"); SetupConfigs configs = new SetupConfigs { @@ -118,7 +89,7 @@ public class LibSurViveAPI //string[] vs = new[] { "--playback", "P:/c/libsurvive-data/lightcap-reformat/lightcap-reformat.log", "--disambiguator", "StateBased", "--calibrate" }; - context = cfunctions.Survive_init_internal(args.Length, args); + context = Cfunctions.Survive_init_internal(args.Length, args); if (context == IntPtr.Zero) { @@ -135,26 +106,24 @@ public class LibSurViveAPI error_func = ErrorEvent; info_func = InfoEvent; - cfunctions.Survive_install_raw_pose_fn(context, raw_Pose_Func); - cfunctions.Survive_install_light_fn(context, light_Process_Func); - cfunctions.Survive_install_lighthouse_pose_fn(context, lighthouse_Pose_Func); - cfunctions.Survive_install_angle_fn(context, angle_Process_Func); - cfunctions.Survive_install_button_fn(context, button_Process_Func); - cfunctions.Survive_install_htc_config_fn(context, htc_Config_Func); - cfunctions.Survive_install_imu_fn(context, imu_Process_Func); - cfunctions.Survive_install_error_fn(context, error_func); - cfunctions.Survive_install_info_fn(context, info_func); - - //Debug.Log("Finished Init"); + Cfunctions.Survive_install_raw_pose_fn(context, raw_Pose_Func); + Cfunctions.Survive_install_light_fn(context, light_Process_Func); + Cfunctions.Survive_install_lighthouse_pose_fn(context, lighthouse_Pose_Func); + Cfunctions.Survive_install_angle_fn(context, angle_Process_Func); + Cfunctions.Survive_install_button_fn(context, button_Process_Func); + Cfunctions.Survive_install_htc_config_fn(context, htc_Config_Func); + Cfunctions.Survive_install_imu_fn(context, imu_Process_Func); + Cfunctions.Survive_install_error_fn(context, error_func); + Cfunctions.Survive_install_info_fn(context, info_func); - //Debug.Log("Start Startup"); + LogInfo("Finished Init"); - //Debug.LogError("ASD"); + LogInfo("Start Startup"); int a = 0; try { - a = cfunctions.Survive_startup(context); + a = Cfunctions.Survive_startup(context); } catch (Exception) { @@ -166,16 +135,17 @@ public class LibSurViveAPI throw new Exception("Error in startup"); } - //Debug.Log("Finished Startup"); + LogInfo("Finished Startup"); } static public string[] CreateStartParameters(SetupConfigs configs) { - List args = new List(); - - args.Add("unity"); + List args = new List + { + "unity" + }; if (configs.playbaskFile != "") { @@ -201,50 +171,48 @@ public class LibSurViveAPI - - virtual protected void InfoEvent(IntPtr ctx, string fault) { - //Debug.Log(fault); + LogInfo(fault); } virtual protected void ErrorEvent(IntPtr ctx, string fault) { - //Debug.LogError(fault); + LogError(fault); } virtual protected void IMUEvent(IntPtr so, int mask, double[] accelgyro, uint timecode, int id) { - cfunctions.Survive_default_imu_process(so, mask, accelgyro, timecode, id); + Cfunctions.Survive_default_imu_process(so, mask, accelgyro, timecode, id); } virtual protected int HTCConfigEvent(IntPtr so, string ct0conf, int len) { - return cfunctions.Survive_default_htc_config_process(so, ct0conf, len); + return Cfunctions.Survive_default_htc_config_process(so, ct0conf, len); } virtual protected void ButtonEvent(IntPtr so, byte eventType, byte buttonId, byte axis1Id, ushort axis1Val, byte axis2Id, ushort axis2Val) { - cfunctions.Survive_default_button_process(so, eventType, buttonId, axis1Id, axis1Val, axis2Id, axis2Val); + Cfunctions.Survive_default_button_process(so, eventType, buttonId, axis1Id, axis1Val, axis2Id, axis2Val); } virtual protected void AngleEvent(IntPtr so, int sensor_id, int acode, uint timecode, double length, double angle, uint lh) { - cfunctions.Survive_default_angle_process(so, sensor_id, acode, timecode, length, angle, lh); + Cfunctions.Survive_default_angle_process(so, sensor_id, acode, timecode, length, angle, lh); //Debug.Log("AngleEvent"); } protected void LightHouseEvent(IntPtr ctx, byte lighthouse, SurvivePose lighthouse_pose, SurvivePose object_pose) { - cfunctions.Survive_default_lighthouse_pose_process(ctx, lighthouse, lighthouse_pose, object_pose); + Cfunctions.Survive_default_lighthouse_pose_process(ctx, lighthouse, lighthouse_pose, object_pose); //Debug.Log("LightHouseEvent"); } virtual protected void LightEvent(IntPtr so, int sensor_id, int acode, int timeinsweep, UInt32 timecode, UInt32 length, UInt32 lighthouse) { - cfunctions.Survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode, length, lighthouse); + Cfunctions.Survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode, length, lighthouse); //Console.WriteLine("LightEvent"); //Debug.Log("LightEvent"); @@ -252,24 +220,12 @@ public class LibSurViveAPI virtual protected void PoseEvent(IntPtr so, byte lighthouse, SurvivePose pose) { - cfunctions.Survive_default_raw_pose_process(so, lighthouse, pose); + Cfunctions.Survive_default_raw_pose_process(so, lighthouse, pose); - //vaDebug.Log("PoseEvent"); - - //poseUpdate(-1, new Vector3((float)pose.Pos[0], (float)pose.Pos[1], (float)pose.Pos[2]), new Quaternion((float)pose.Rot[0], (float)pose.Rot[1], (float)pose.Rot[2], (float)pose.Rot[3])); - - /* - string a = cfunctions.Survive_object_codename(so); - if (updates.ContainsKey(a)) - { - Vector3 pos = new Vector3((float)pose.Pos[0], (float)pose.Pos[1], (float)pose.Pos[2]); - Quaternion rot = new Quaternion((float)pose.Rot[0], (float)pose.Rot[1], (float)pose.Rot[2], (float)pose.Rot[3]); - updates[a](pos, rot); - } - */ + //Debug.Log("PoseEvent"); } - + /* public delegate void PoseUpdate(SurviveVector3 pos, SurviveQuaternion quat); public Dictionary updates = new Dictionary(); @@ -285,14 +241,9 @@ public class LibSurViveAPI updates[ID] += update; } } - - /* - public void Poll() - { - cfunctions.Survive_poll(context); - } */ + public SurviveObject GetSurviveObjectByName(string name) { if (name == "") @@ -303,10 +254,46 @@ public class LibSurViveAPI if (context == IntPtr.Zero) throw new Exception("The context hasn't been initialsied yet"); - return new SurviveObject( cfunctions.Survive_get_so_by_name(context, name)); + return new SurviveObject( Cfunctions.Survive_get_so_by_name(context, name)); } } + +public struct SetupConfigs +{ + public string playbaskFile; + public int playbackFactor; + public Disambiguator disambiguator; + public Poser poser; + public BoolConfig calibrate; + public string configFile; +} + +public enum Disambiguator +{ + StateBased, + Charles, + Turvey, + Default +} + +public enum Poser +{ + CharlesSlow, + DaveOrtho, + Dummy, + EPNP, + SBA +} + +public enum BoolConfig +{ + Yes, + No, + Default +} + + public class SurviveObject { private SurvivePose _pose; @@ -314,7 +301,7 @@ public class SurviveObject { get { - var ptr1 = cfunctions.Survive_object_pose(ptr); + var ptr1 = Cfunctions.Survive_object_pose(ptr); return (SurvivePose)Marshal.PtrToStructure(ptr1, typeof(SurvivePose)); } } @@ -324,14 +311,14 @@ public class SurviveObject { get { - return cfunctions.Survive_object_charge(ptr); + return Cfunctions.Survive_object_charge(ptr); } } public bool Charging { get { - return cfunctions.Survive_object_charging(ptr); + return Cfunctions.Survive_object_charging(ptr); } } @@ -350,14 +337,15 @@ public class SurviveObject public class SurviveVector3 { - float x; - float y; - float z; + double x; + double y; + double z; } public class SurviveQuaternion { - float x; - float y; - float z; + double x; + double y; + double z; + double w; } diff --git a/bindings/cs/libsurvive.net/SurviveContext.cs b/bindings/cs/libsurvive.net/SurviveContext.cs index 084df31..b1eb711 100644 --- a/bindings/cs/libsurvive.net/SurviveContext.cs +++ b/bindings/cs/libsurvive.net/SurviveContext.cs @@ -16,7 +16,7 @@ namespace libsurvive public void Dispose() { - cfunctions.Survive_close(ctx); + Cfunctions.Survive_close(ctx); ctx = IntPtr.Zero; } @@ -52,7 +52,7 @@ namespace libsurvive newArgs[0] = System.Reflection.Assembly.GetEntryAssembly().FullName; Array.Copy(args, 0, newArgs, 1, args.Length); - ctx = cfunctions.Survive_init_internal(newArgs.Length, newArgs); + ctx = Cfunctions.Survive_init_internal(newArgs.Length, newArgs); if (ctx == IntPtr.Zero) { @@ -69,15 +69,15 @@ namespace libsurvive error_func = ErrorEvent; info_func = InfoEvent; - cfunctions.Survive_install_raw_pose_fn(ctx, raw_Pose_Func); - cfunctions.Survive_install_light_fn(ctx, light_Process_Func); - cfunctions.Survive_install_lighthouse_pose_fn(ctx, lighthouse_Pose_Func); - cfunctions.Survive_install_angle_fn(ctx, angle_Process_Func); - cfunctions.Survive_install_button_fn(ctx, button_Process_Func); - cfunctions.Survive_install_htc_config_fn(ctx, htc_Config_Func); - cfunctions.Survive_install_imu_fn(ctx, imu_Process_Func); - cfunctions.Survive_install_error_fn(ctx, error_func); - cfunctions.Survive_install_info_fn(ctx, info_func); + Cfunctions.Survive_install_raw_pose_fn(ctx, raw_Pose_Func); + Cfunctions.Survive_install_light_fn(ctx, light_Process_Func); + Cfunctions.Survive_install_lighthouse_pose_fn(ctx, lighthouse_Pose_Func); + Cfunctions.Survive_install_angle_fn(ctx, angle_Process_Func); + Cfunctions.Survive_install_button_fn(ctx, button_Process_Func); + Cfunctions.Survive_install_htc_config_fn(ctx, htc_Config_Func); + Cfunctions.Survive_install_imu_fn(ctx, imu_Process_Func); + Cfunctions.Survive_install_error_fn(ctx, error_func); + Cfunctions.Survive_install_info_fn(ctx, info_func); } @@ -104,46 +104,46 @@ namespace libsurvive virtual protected void IMUEvent(SurviveObjectPtr so, int mask, double[] accelgyro, uint timecode, int id) { - cfunctions.Survive_default_imu_process(so, mask, accelgyro, timecode, id); + Cfunctions.Survive_default_imu_process(so, mask, accelgyro, timecode, id); } virtual protected int HTCConfigEvent(SurviveObjectPtr so, string ct0conf, int len) { - return cfunctions.Survive_default_htc_config_process(so, ct0conf, len); + return Cfunctions.Survive_default_htc_config_process(so, ct0conf, len); } virtual protected void ButtonEvent(SurviveObjectPtr so, byte eventType, byte buttonId, byte axis1Id, ushort axis1Val, byte axis2Id, ushort axis2Val) { - cfunctions.Survive_default_button_process(so, eventType, buttonId, axis1Id, axis1Val, axis2Id, axis2Val); + Cfunctions.Survive_default_button_process(so, eventType, buttonId, axis1Id, axis1Val, axis2Id, axis2Val); } virtual protected void AngleEvent(SurviveObjectPtr so, int sensor_id, int acode, uint timecode, double length, double angle, uint lh) { - cfunctions.Survive_default_angle_process(so, sensor_id, acode, timecode, length, angle, lh); + Cfunctions.Survive_default_angle_process(so, sensor_id, acode, timecode, length, angle, lh); } protected void LightHouseEvent(SurviveObjectPtr ctx, byte lighthouse, SurvivePose lighthouse_pose, SurvivePose object_pose) { - cfunctions.Survive_default_lighthouse_pose_process(ctx, lighthouse, lighthouse_pose, object_pose); + Cfunctions.Survive_default_lighthouse_pose_process(ctx, lighthouse, lighthouse_pose, object_pose); } virtual protected void LightEvent(SurviveObjectPtr so, int sensor_id, int acode, int timeinsweep, UInt32 timecode, UInt32 length, UInt32 lighthouse) { - cfunctions.Survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode, length, lighthouse); + Cfunctions.Survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode, length, lighthouse); //Console.WriteLine("LightEvent"); } virtual protected void PoseEvent(SurviveObjectPtr so, byte lighthouse, SurvivePose pose) { - cfunctions.Survive_default_raw_pose_process(so, lighthouse, pose); + Cfunctions.Survive_default_raw_pose_process(so, lighthouse, pose); Console.WriteLine("PoseEvent"); } public int Poll() { - return cfunctions.Survive_poll(ctx); + return Cfunctions.Survive_poll(ctx); } } diff --git a/bindings/cs/libsurvive.net/cfunctions.cs b/bindings/cs/libsurvive.net/cfunctions.cs index ce46d18..d003976 100644 --- a/bindings/cs/libsurvive.net/cfunctions.cs +++ b/bindings/cs/libsurvive.net/cfunctions.cs @@ -18,7 +18,7 @@ namespace libsurvive public double[] Rot; // Quaternion in the form wxyz } - class cfunctions + class Cfunctions { //#pragma warning disable IDE1006 // Naming Styles [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_init_internal")] -- cgit v1.2.3 From 09eb4225a93073cce8728032a1af6f6d69138307 Mon Sep 17 00:00:00 2001 From: dpeter99 Date: Sun, 1 Apr 2018 21:58:51 +0200 Subject: Removed the hard coded path --- bindings/cs/libsurvive.net/LibSurViveAPI.cs | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/bindings/cs/libsurvive.net/LibSurViveAPI.cs b/bindings/cs/libsurvive.net/LibSurViveAPI.cs index ff6d100..7e8b73a 100644 --- a/bindings/cs/libsurvive.net/LibSurViveAPI.cs +++ b/bindings/cs/libsurvive.net/LibSurViveAPI.cs @@ -80,9 +80,7 @@ public class LibSurViveAPI SetupConfigs configs = new SetupConfigs { - playbaskFile = @"C:\Users\justin\source\libsurvive-win-update\bindings\cs\Demo\lightcap-reformat.log", - configFile = "survive_conf.json", - playbackFactor = 1 + configFile = "survive_conf.json" }; string[] args = CreateStartParameters(configs); -- cgit v1.2.3 From a87071facfd896a7d9cd2939d69a8a7b80db8779 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 2 Apr 2018 00:13:42 -0600 Subject: Removed def file from proj --- winbuild/libsurvive/libsurvive.vcxproj | 1 - winbuild/libsurvive/libsurvive.vcxproj.filters | 1 - 2 files changed, 2 deletions(-) diff --git a/winbuild/libsurvive/libsurvive.vcxproj b/winbuild/libsurvive/libsurvive.vcxproj index aefdc00..4b76c99 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj +++ b/winbuild/libsurvive/libsurvive.vcxproj @@ -214,7 +214,6 @@ - Designer diff --git a/winbuild/libsurvive/libsurvive.vcxproj.filters b/winbuild/libsurvive/libsurvive.vcxproj.filters index ffddee9..1f98c68 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj.filters +++ b/winbuild/libsurvive/libsurvive.vcxproj.filters @@ -190,7 +190,6 @@ - \ No newline at end of file -- cgit v1.2.3 From b449d5228c3c9b3f60e427f7427eb35c1509ca2f Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 2 Apr 2018 00:14:15 -0600 Subject: Fixed case issue --- bindings/cs/Demo/Program.cs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bindings/cs/Demo/Program.cs b/bindings/cs/Demo/Program.cs index 53c2307..c838a4e 100644 --- a/bindings/cs/Demo/Program.cs +++ b/bindings/cs/Demo/Program.cs @@ -12,8 +12,8 @@ namespace Demo { static void Main(string[] args) { - LibSurViveAPI api = LibSurViveAPI.instance; - + LibSurViveAPI api = LibSurViveAPI.Instance; + var so = api.GetSurviveObjectByName("HMD"); } -- cgit v1.2.3 From d77155ee57f715288d73f7c77a14dc05ebc601da Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 2 Apr 2018 00:14:26 -0600 Subject: Fixed windows init issue --- src/poser_charlesslow.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/poser_charlesslow.c b/src/poser_charlesslow.c index b44225e..f725334 100644 --- a/src/poser_charlesslow.c +++ b/src/poser_charlesslow.c @@ -256,7 +256,7 @@ static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int pr int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2; if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue; - FLT out[2] = {}; + FLT out[2] = { 0 }; survive_apply_bsd_calibration(hmd->ctx, lh, fs->angles[p][lh], out); //Find out where our ray shoots forth from. @@ -343,7 +343,7 @@ static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int pr if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue; //Find out where our ray shoots forth from. - FLT out[2] = {}; + FLT out[2] = { 0 }; survive_apply_bsd_calibration(hmd->ctx, lh, fs->angles[p][lh], out); // Find out where our ray shoots forth from. -- cgit v1.2.3 From 3f2cd96e959494f296c46ce66d70fda0ca9a41f4 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 2 Apr 2018 00:21:49 -0600 Subject: Fixed crashes, typo --- bindings/cs/libsurvive.net/LibSurViveAPI.cs | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/bindings/cs/libsurvive.net/LibSurViveAPI.cs b/bindings/cs/libsurvive.net/LibSurViveAPI.cs index 7e8b73a..1fb8cae 100644 --- a/bindings/cs/libsurvive.net/LibSurViveAPI.cs +++ b/bindings/cs/libsurvive.net/LibSurViveAPI.cs @@ -38,10 +38,10 @@ public class LibSurViveAPI text_feedback_func info_func; public delegate void Log(string message); - Log LogInfo; + Log LogInfo = delegate { }; public delegate void ErrorLog(string message); - ErrorLog LogError; + ErrorLog LogError = delegate { }; LibSurViveAPI() { @@ -145,9 +145,9 @@ public class LibSurViveAPI "unity" }; - if (configs.playbaskFile != "") + if (configs.playbackFile != "" && configs.playbackFile != null) { - args.AddRange(new[] { "--playback", configs.playbaskFile }); + args.AddRange(new[] { "--playback", configs.playbackFile }); args.AddRange(new[] { "--playback-factor", configs.playbackFactor.ToString() }); } @@ -259,7 +259,7 @@ public class LibSurViveAPI public struct SetupConfigs { - public string playbaskFile; + public string playbackFile; public int playbackFactor; public Disambiguator disambiguator; public Poser poser; -- cgit v1.2.3 From 83d304551bb0052f6b67365c558ea6d825585e39 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 2 Apr 2018 10:05:05 -0600 Subject: Modified showreproject for video --- tools/showreproject/showreproject.cc | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/tools/showreproject/showreproject.cc b/tools/showreproject/showreproject.cc index caa7a66..8cde992 100644 --- a/tools/showreproject/showreproject.cc +++ b/tools/showreproject/showreproject.cc @@ -87,7 +87,7 @@ static void redraw(SurviveContext *ctx) { // survive_apply_bsd_calibration(so->ctx, lh, _a, a); auto l = scene->lengths[sensor][lh]; - double r = std::max(1., (l[0] + l[1]) / 1000.); + double r = std::max(3., (l[0] + l[1]) / 1000.); // std::cerr << lh << "\t" << sensor << "\t" << ((l[0] + l[1]) / 2000.) << "\t" << l[0] << "\t" << // l[1] << std::endl; if (region.data) @@ -111,8 +111,10 @@ static void redraw(SurviveContext *ctx) { cv::Scalar(255, 255, 255)); if (region.data) { - cv::line(region, map(a) + cv::Point(ex * 10000, -ey * 10000), map(a), - cv::Scalar(255, 255, 255)); + if (err_add < .01) { + cv::line(region, map(a) + cv::Point(ex * 10000, -ey * 10000), map(a), + cv::Scalar(255, 255, 255)); + } cv::rectangle(region, map(out) - cv::Point(r, r), map(out) + cv::Point(r, r), rcolor); } } @@ -235,7 +237,7 @@ int main(int argc, char **argv) { err[lh].setTo(cv::Vec3b(0, 0, 0)); } - if (img.data && false) { + if (img.data) { cv::imshow("Reprojection", img); cv::waitKey(0); } -- cgit v1.2.3 From 0f5bf3c2f9fd7481d6b3c7c5364a08fe3ecdb2fb Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 2 Apr 2018 16:38:04 -0600 Subject: Added documentation about writing a poser --- README.md | 16 ++++++----- docs/writing_a_poser.md | 66 ++++++++++++++++++++++++++++++++++++++++++++++ include/libsurvive/poser.h | 29 +++++++++++++++++++- 3 files changed, 104 insertions(+), 7 deletions(-) create mode 100644 docs/writing_a_poser.md diff --git a/README.md b/README.md index 4b18c43..8a28bb0 100644 --- a/README.md +++ b/README.md @@ -118,9 +118,10 @@ Poser | [poser_dummy.c](src/poser_dummy.c) | Template for posers | [@cnlohr](htt Poser | [poser_octavioradii.c](src/poser_octavioradii.c) | A potentially very fast poser that works by finding the best fit of the distances from the lighthouse to each sensor that matches the known distances between sensors, given the known angles of a lighthouse sweep. Incomplete- distances appear to be found correctly, but more work needed to turn this into a pose. | [@mwturvey](https://github.com/mwturvey) and [@octavio2895](https://github.com/octavio2895) Poser | [poser_turveytori.c](src/poser_turveytori.c) | A moderately fast, fairly high precision poser that works by determine the angle at the lighthouse between many sets of two sensors. Using the inscirbed angle theorom, each set defines a torus of possible locations of the lighthouse. Multiple sets define multiple tori, and this poser finds most likely location of the lighthouse using least-squares distance. Best suited for calibration, but is can be used for real-time tracking on a powerful system. | [@mwturvey](https://github.com/mwturvey) Poser | [poser_epnp.c](src/poser_epnp.c) | Reasonably fast and accurate calibration and tracker that uses the [EPNP algorithm](https://en.wikipedia.org/wiki/Perspective-n-Point#EPnP) to solve the perspective and points problem. Suitable for fast tracking, but does best with >5-6 sensor readings. | [@jdavidberger](https://github.com/jdavidberger) -Poser | [poser_sba.c](src/poser_sba.c) | Reasonably fast and accurate calibration and tracker but is dependent on a 'seed' poser to give it an initial estimate. This then performs [bundle adjustment](https://en.wikipedia.org/wiki/Bundle_adjustment) to minimize reprojection error given both ligthhouse readings. This has the benefit of greatly increasing accuracy by incorporating all the light data that is available. Set 'SBASeedPoser' config option to specify the seed poser; default is EPNP. | [@jdavidberger](https://github.com/jdavidberger) -Disambiguator | [survive_data.c](src/survive_data.c) (currently #ifdefed out) | The old disambiguator - very fast, but slightly buggy. | [@cnlohr](https://github.com/cnlohr) -Disambiguator | [survive_data.c](src/survive_data.c) (current disambiguator) | More complicated but much more robust disambiguator | [@mwturvey](https://github.com/mwturvey) +Poser | [poser_sba.c](src/poser_sba.c) (default) | Reasonably fast and accurate calibration and tracker but is dependent on a 'seed' poser to give it an initial estimate. This then performs [bundle adjustment](https://en.wikipedia.org/wiki/Bundle_adjustment) to minimize reprojection error given both ligthhouse readings. This has the benefit of greatly increasing accuracy by incorporating all the light data that is available. Set 'SBASeedPoser' config option to specify the seed poser; default is EPNP. | [@jdavidberger](https://github.com/jdavidberger) +Disambiguator | [survive_data.c](src/survive_charlesbiguator.c) | The old disambiguator - very fast, but slightly buggy. | [@cnlohr](https://github.com/cnlohr) +Disambiguator | [survive_data.c](src/survive_turveybiguator.c) (default) | More complicated but much more robust disambiguator | [@mwturvey](https://github.com/mwturvey) +Disambiguator | [survive_data.c](src/survive_statebased_disambiguator.c) | A fast disambiguator that was times the state shifts between pulses. Experimental. Made to allow tracking very close to the lighthouse | [@jdavidberger](https://github.com/jdavidberger) Dismabiguator | superceded disambiguator | A more sophisticated disambiguator, development abandoned. Removed from tree. | [@jpicht](https://github.com/jpicht) Driver | [survive_vive.c](src/survive_vive.c) | Driver for HTC Vive HMD, Watchmen (wired+wireless) and Tracker | [@cnlohr](https://github.com/cnlohr) and [@mwturvey](https://github.com/mwturvey) OOTX Decoder | [ootx_decoder.c](src/ootx_decoder.c) | The system that takes the pulse-codes from the sync pulses from the lighthouses and get [OOTX Data](https://github.com/nairol/LighthouseRedox/blob/master/docs/Light%20Emissions.md) | [@axlecrusher](https://github.com/axlecrusher) @@ -131,7 +132,7 @@ Component Type | Pluggability method --- | --- Driver | Dynamically loadable runtime, can co-exist with other drivers. Poser | Selectable by configuration at runtime -Disambiguator | Selectable by #define +Disambiguator | Selectable by configuration at runtime OOTX Decoder | Not Pluggable @@ -146,7 +147,7 @@ To support the Vive on HDMI, you either need a newer version of HDMI, or you nee ## General Information -The default configuration of libsurvive requires both basestations and both controllers to be active, but currently libsurvive can not make proper use of both basestations at the same time - it outputs a different pose for each basestation and does not fuse the data for one more accurate pose. This will hopefully be implemented soon. For now it's a good idea to change the configuration to only require one basestation. +The default configuration of libsurvive requires both basestations and both controllers to be active. Here is an example of a default configuration file that libsurvive will create as `config.json` in the current working directory when any libsurvive client is executed: @@ -172,7 +173,7 @@ Here is an example of a default configuration file that libsurvive will create a } ``` -To make libsurvive calibrate and run with one basestation, `lighthousecount` needs to be changed to `1`. +To make libsurvive calibrate and run with one basestation, `lighthousecount` needs to be changed to `1`. You can also pass in `-l 1` as command line arguments. It may be annoying to always require the controllers for calibration. To make libsurvive calibrate by using the HMD, `RequiredTrackersForCal` needs to be changed to the magic string `HMD`. The strings for the controllers are `WM0` and `WM1`, short for "Watchman". Other possible values are `WW0` (Wired Watchman) for a controller directly connected with USB or `TR0` for a Vive tracker directly connected with USB (When connected wirelessly, the tracker uses the dongles, so uses `WM0` or `WM1`). @@ -225,6 +226,9 @@ Sometimes libsurvive goes very quickly through these steps and fills in all pose [Here is a short demo video how successfuly running ./test should look like](https://haagch.frickel.club/Peek%202018-02-21%2023-23.webm). +If there is already calibration data present, the library will use it. Pass `--calibrate` to force a new calibration pass. +Conversely, if there isn't calibration data the library will auto-calibrate. Pass `--no-calibrate` to disable this calibration. + ## Using libsurvive in your own application Example code for libsurvive can be found in [test.c](https://github.com/cnlohr/libsurvive/blob/master/test.c). [calibrate.c](https://github.com/cnlohr/libsurvive/blob/master/calibrate.c) may contain some interesting code too. diff --git a/docs/writing_a_poser.md b/docs/writing_a_poser.md new file mode 100644 index 0000000..ac450e3 --- /dev/null +++ b/docs/writing_a_poser.md @@ -0,0 +1,66 @@ +# How to write a poser + +Posers in libsurvive serve two related purposes: + +1) If the lighthouse position isn't known, solve for them. +2) If the lighthouse position is known, solve for the position of objects in the scene. + +To do this, a poser is given a variety of different signals about the current state of the input. + +# Registering a Poser + +In an effort to decouple posers from the underlying mechanics, posers are registered through a link-time +constructor. Make a function in the following form: + +``` +#include "survive.h" + +int PoserMyPoser(SurviveObject *so, PoserData *pd) { + switch (pd->pt) { + case POSERDATA_LIGHT: { + PoserDataLight *lightData = (PoserDataLight *)pd; + return -1; + } + case POSERDATA_FULL_SCENE: { + PoserDataFullScene pdfs = (PoserDataFullScene *)(pd); + return -1; + } + case POSERDATA_IMU: { + PoserDataIMU *imuData = (PoserDataIMU *)pd; + return -1; + } + return -1; +} + +REGISTER_LINKTIME(PoserMyPoser); + +``` + +Note that the prefix of the function MUST be "Poser" for it to work. + +Now compile it into the shared object, and it will be one of the poser options available. + +## Input to a poser + +`PoserData` is a tagged union that contains the new input to the poser. See above for how to get the concrete +types for each type of event. + +If you don't handle a given input, return -1. If you do, return a 0. If you have a more specific +error number, you can return that as well. + +## Output from a poser + +To report an objects location for any one of the events above, call the function + +`PoserData_poser_raw_pose_func`. See the documentation in poser.h for more info. + +To report an initial solution to the lighthouse solution, call the function `PoserData_lighthouse_pose_func`. Again, +see documention in poser.h + +## Calibration data + +Note that by default, no angle data is presented in a calibrated fashion. Depending on your use case, you +may want to treat the data by grouping X, Y pairs and trying to undistort them. You can do this with the library +function `survive_apply_bsd_calibration` from `survive_reproject.h`. Please understand though that this is, at best, an +approximation and can be far off if the angle values are too spread out in time. + diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h index b0b1a7b..dcd0e93 100644 --- a/include/libsurvive/poser.h +++ b/include/libsurvive/poser.h @@ -30,9 +30,36 @@ typedef struct void *userdata; } PoserData; +/** + * Meant to be used by individual posers to report back their findings on the pose of an object back to the invoker of + * the call. + * + * @param poser_data the data pointer passed into the poser function invocation + * @param so The survive object which we are giving a solution for. + * @param lighthouse @deprecated The lighthouse which observed that position. Make it -1 if it was a combination of + * lighthouses. Will be removed in the future. + * @param pose The actual object pose. This is in world space, not in LH space. It must represent a transformation from + * object space of the SO to global space. + */ void PoserData_poser_raw_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose); + +/** + * Meant to be used by individual posers to report back their findings on the pose of a lighthouse. + * + * Note that you are free to assume the position of the lighthouse and solve for the object or vice versa. Most solvers + * assume that the object is at 0,0,0 but this isn't a hard requirement. + * + * @param poser_data the data pointer passed into the poser function invocation + * @param so The survive object which gave us the info for the solution + * @param lighthouse The lighthouse which to solve for + * @param objUp2world For use when solving for both ligthhouse positions. For the first invocation of this function, + * pass in a zero-inited SurvivePose. This function will set that to a relative transform to normalize the space. + * pass the same pose in again for the second lighthouse to get accurate results. + * @param lighthouse_pose This is the assumed or derived position of the given lighthouse. + * @param object_pose This is the assumed or derived position of the tracked object. + */ void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, - /* OUTPARAM */ SurvivePose *objUp2world, SurvivePose *lighthouse_poses, + /* OUTPARAM */ SurvivePose *objUp2world, SurvivePose *lighthouse_pose, SurvivePose *object_pose); typedef struct PoserDataIMU { -- cgit v1.2.3 From ed2be004a368a2a34cabe6e25d3b02aba8de4a52 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Tue, 3 Apr 2018 00:56:56 -0400 Subject: For the same of simplifying testing, this seems like the best tool. Its original usefulness has since dried up but it can live again as a slightly different tool. --- simple_pose_test.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/simple_pose_test.c b/simple_pose_test.c index 0772abb..5153d15 100644 --- a/simple_pose_test.c +++ b/simple_pose_test.c @@ -168,6 +168,7 @@ int main( int argc, char ** argv ) survive_install_raw_pose_fn(ctx, testprog_raw_pose_process); //survive_install_angle_fn(ctx, testprog_angle_process ); +#if 0 //Don't reset poses ctx->bsd[0].PositionSet = ctx->bsd[1].PositionSet = 1; int i; for( i = 0; i < 2; i++ ) @@ -181,7 +182,7 @@ int main( int argc, char ** argv ) p->Rot[2] = 0; p->Rot[3] = 0; } - +#endif OGCreateThread( GUIThread, 0 ); if( !ctx ) -- cgit v1.2.3 From 8964003b98025168bacf524065f0fb854ed040f7 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Tue, 3 Apr 2018 01:05:48 -0400 Subject: Throw in my Wip poser. --- Makefile | 2 +- src/poser_charlesrefine.c | 302 ++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 303 insertions(+), 1 deletion(-) create mode 100644 src/poser_charlesrefine.c diff --git a/Makefile b/Makefile index 62ed5e8..dcef7ba 100644 --- a/Makefile +++ b/Makefile @@ -39,7 +39,7 @@ REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/minimal_ope ifeq ($(UNAME), Darwin) REDISTS:=$(REDISTS) redist/hid-osx.c endif -LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_charlesbiguator.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/epnp/epnp.o src/survive_sensor_activations.o src/survive_turveybiguator.o src/survive_disambiguator.o src/survive_statebased_disambiguator.o +LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_charlesbiguator.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/epnp/epnp.o src/survive_sensor_activations.o src/survive_turveybiguator.o src/survive_disambiguator.o src/survive_statebased_disambiguator.o src/poser_charlesrefine.o #If you want to use HIDAPI on Linux. #CFLAGS:=$(CFLAGS) -DHIDAPI diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c new file mode 100644 index 0000000..4d44722 --- /dev/null +++ b/src/poser_charlesrefine.c @@ -0,0 +1,302 @@ +//EXPERIMENTAL DRIVER - DO NOT USE + +#include +#include +#include + +#include "epnp/epnp.h" +#include "linmath.h" +#include +#include +#include + +#define MAX_PT_PER_SWEEP 32 + + +typedef struct +{ + int sweepaxis; + int sweeplh; + FLT normal_at_errors[MAX_PT_PER_SWEEP][3]; + FLT quantity_errors[MAX_PT_PER_SWEEP] + uint8_t sensor_ids[MAX_PT_PER_SWEEP]; + int ptsweep; +} CharlesPoserData; + + + +int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { + CharlesPoserData * dd = so->PoserData; + if( !dd ) so->PoserData = dd = calloc( sizeof(CharlesPoserData), 1 ); + + SurviveSensorActivations *scene = &so->activations; + switch (pd->pt) { + case POSERDATA_IMU: { + // Really should use this... + PoserDataIMU *imuData = (PoserDataIMU *)pd; + return 0; + } + case POSERDATA_LIGHT: { + int i; + PoserDataLight *ld = (PoserDataLight *)pd; + int lhid = ld->lh; + int senid = ld->sensor_id; + BaseStationData * bsd = &so->ctx->bsd[ld->lh]; + if( !bsd->PositionSet ) break; + SurvivePose * lhp = &bsd->Pose; + FLT angle = ld->angle; + int sensor_id = ld->sensor_id; + int axis = dd->sweepaxis; + const SurvivePose * object_pose = &so->OutPose; + dd->sweeplh = lhid; + + //FOR NOW, drop LH1. + if( lhid == 1 ) break; + + +// const FLT * sensor_normal = &so->sensor_normals[senid*3]; +// FLT sensor_normal_worldspace[3]; +// ApplyPoseToPoint(sensor_normal_worldspace, object_pose, sensor_inpos); + + const FLT * sensor_inpos = &so->sensor_locations[senid*3]; + FLT sensor_position_worldspace[3]; + ApplyPoseToPoint(sensor_position_worldspace, object_pose, sensor_inpos); + //printf( "%f %f %f == > %f %f %f\n", sensor_inpos[0], sensor_inpos[1], sensor_inpos[2], sensor_position_worldspace[0], sensor_position_worldspace[1], sensor_position_worldspace[2] ); + // = sensor position, relative to lighthouse center. + FLT sensorpos_rel_lh[3]; + sub3d( sensorpos_rel_lh, sensor_position_worldspace, lhp->Pos ); + + //Next, define a normal in global space of the plane created by the sweep hit. + //Careful that this must be normalized. + FLT sweep_normal[3]; + + //If 1, the "y" axis. //XXX Check me. + if( axis ) //XXX Just FYI this should include account for skew + { + sweep_normal[0] = 0; + sweep_normal[1] = cos(angle ); + sweep_normal[2] = sin( angle ); + //printf( "+" ); + } + else + { + sweep_normal[0] = cos( angle ); + sweep_normal[1] = 0; + sweep_normal[2] = -sin( angle ); + //printf( "-" ); + } + + //Need to apply the lighthouse's transformation to the sweep's normal. + quatrotatevector( sweep_normal, lhp->Rot, sweep_normal); + + //Compute point-line distance between sensorpos_rel_lh and the plane defined by sweep_normal. + //Do this by projecting sensorpos_rel_lh (w) onto sweep_normal (v). + //You can do this by |v dot w| / |v| ... But we know |v| is 1. So... + FLT dist = dot3d( sensorpos_rel_lh, sweep_normal ); + + if( (i = dd->ptsweep) < MAX_PT_PER_SWEEP ) + { + dd->normal_at_errors[i] = sweep_normal; + dd->quantity_errors[i] = dist; + dd->sensor_ids[i] = sensor_id; + dd->ptsweep++: + } + +#if 0 + printf( "D %d %d: %f [%f %f %f]\n", lhid, axis, dist, sweep_normal[0], sweep_normal[1], sweep_normal[2] ); + + + //Naieve approach... Push it in the right direction + SurvivePose object_pose_out; + quatcopy( object_pose_out.Rot, object_pose->Rot ); + scale3d(sweep_normal, sweep_normal, -0.1*dist); + add3d(object_pose_out.Pos, sweep_normal, object_pose->Pos); + + if( so->PoseConfidence < .01 ) + { + dd->average_nudge[0] = 0; + dd->average_nudge[1] = 0; + dd->average_nudge[2] = 0; + + memcpy( &object_pose_out, &LinmathPose_Identity, sizeof( LinmathPose_Identity ) ); + object_pose_out.Pos[1] = 2.5; + object_pose_out.Pos[2] = 1.8; + so->PoseConfidence = 1.0; + } +// printf( "%f %f %f %f\n", object_pose->Rot[0], object_pose->Rot[1], object_pose->Rot[2], object_pose->Rot[3] ); + + PoserData_poser_raw_pose_func(pd, so, lhid, &object_pose_out); +#endif + +#if 0 +// = { + axis?0.0:sin(angle), + axis?sin(angle):0.0, + cos(angle) }; + + = sensor_locations; + LinmathPoint3d + int8_t sensor_ct; // sensor count + FLT *sensor_locations; // size is sensor_ct*3. Contains x,y,z values for each sensor + FLT *sensor_normals; // size is nrlocations*3. cointains normal vector for each sensor + + + // This is the quat equivalent of 'pout = pose * pin' if pose were a 4x4 matrix in homogenous space +void ApplyPoseToPoint(LinmathPoint3d pout, const LinmathPose *pose, const LinmathPoint3d pin); + + + + SurvivePose obj2world; +// ApplyPoseToPose(&obj2world, &lh2world, &objpose); + memcpy( &obj2world, &LinmathPose_Identity, sizeof( obj2world ) ); + obj2world.Pos[1] = 1; + PoserData_poser_raw_pose_func(pd, so, lhid, &obj2world); +// PoserData_poser_raw_pose_func(pd, so, ld->lh, &posers[lightData->lh]); + + + // Pose Information, also "poser" field. + ///from SurviveObject + // FLT PoseConfidence; // 0..1 + // SurvivePose OutPose; // Final pose? (some day, one can dream!) + // SurvivePose FromLHPose[NUM_LIGHTHOUSES]; // Filled out by poser, contains computed position from each lighthouse. + // void *PoserData; // Initialized to zero, configured by poser, can be anything the poser wants. + // PoserCB PoserFn; + + + // printf( "%d %d %f\n", ld->sensor_id, ld->lh, ld->angle ); + +/* + SurvivePose *object_pose, void *user); + +typedef struct +{ + PoserType pt; + poser_raw_pose_func rawposeproc; + poser_lighthouse_pose_func lighthouseposeproc; + void *userdata; +} PoserData; + + PoserData hdr; + int sensor_id; + int acode; //OOTX Code associated with this sweep. bit 1 indicates vertical(1) or horizontal(0) sweep + int lh; //Lighthouse making this sweep + uint32_t timecode; //In object-local ticks. + FLT length; //In seconds + FLT angle; //In radians from center of lighthouse. +} PoserDataLight; + + + +*/ +#if 0 + SurvivePose posers[2]; + int meas[2] = {0, 0}; + for (int lh = 0; lh < so->ctx->activeLighthouses; lh++) { + if (so->ctx->bsd[lh].PositionSet) { + epnp pnp = {.fu = 1, .fv = 1}; + epnp_set_maximum_number_of_correspondences(&pnp, so->sensor_ct); + + add_correspondences(so, &pnp, scene, lightData->timecode, lh); + static int required_meas = -1; + if (required_meas == -1) + required_meas = survive_configi(so->ctx, "epnp-required-meas", SC_GET, 4); + + if (pnp.number_of_correspondences > required_meas) { + + SurvivePose objInLh = solve_correspondence(so, &pnp, false); + if (quatmagnitude(objInLh.Rot) != 0) { + SurvivePose *lh2world = &so->ctx->bsd[lh].Pose; + + SurvivePose txPose = {.Rot = {1}}; + ApplyPoseToPose(&txPose, lh2world, &objInLh); + posers[lh] = txPose; + meas[lh] = pnp.number_of_correspondences; + } + } + + epnp_dtor(&pnp); + } + } + + if (meas[0] > 0 && meas[1] > 0) { + SurvivePose interpolate = {0}; + bool winnerTakesAll = true; // Not convinced slerp does the right thing, will change this when i am + + if (winnerTakesAll) { + int winner = meas[0] > meas[1] ? 0 : 1; + PoserData_poser_raw_pose_func(pd, so, winner, &posers[winner]); + } else { + double a, b; + a = meas[0] * meas[0]; + b = meas[1] * meas[1]; + + double t = a + b; + for (size_t i = 0; i < 3; i++) { + interpolate.Pos[i] = (posers[0].Pos[i] * a + posers[1].Pos[i] * b) / (t); + } + quatslerp(interpolate.Rot, posers[0].Rot, posers[1].Rot, b / (t)); + PoserData_poser_raw_pose_func(pd, so, lightData->lh, &interpolate); + } + } else { + if (meas[lightData->lh]) + PoserData_poser_raw_pose_func(pd, so, lightData->lh, &posers[lightData->lh]); + } +#endif + +#endif + return 0; + } + + case POSERDATA_SYNC: { + PoserDataLight *l = (PoserDataLight *)pd; + int lhid = l->lh; + + //you can get sweepaxis and sweeplh. + + if( dd->ptsweep ) + { + int i; + int lhid = dd->lhid; + int pts = dd->ptsweep; + const SurvivePose * object_pose = &so->OutPose; + + FLT avg_err[3] = { 0, 0, 0 }; + FLT avgtot = 0.0; + for( i = 0; i < pts; i++ ) + { + FLT * nrm = dd->normal_at_errors[pts]; + FLT qty = quantity_errors[pts]; + avgtot += qty; + avg_err[0] = avg_err[0] + nrm[0] * qty; + avg_err[1] = avg_err[1] + nrm[1] * qty; + avg_err[2] = avg_err[2] + nrm[2] * qty; + } + scale3d(avg_err, avg_err, 1./pts); + //We have "Average error" now. This is a world space value. + //This can correct for lateral error, but not distance from camera. + + //Next we need to find out what the weighting is to determine "zoom" + //How do we do this? ??? Too tired to math. + FLT weight = 0.0; + for( i = 0; i < pts; i++ ) + { + //??!?!? Sturfff + } + + dd->ptsweep = 0; + + //Update PoserData_poser_raw_pose_func(pd, so, lhid, &object_pose_out); + } + + dd->nextaxis = l->acode & 1; + printf( "SYNC %d %p\n", l->acode, dd ); + break; + } + case POSERDATA_FULL_SCENE: { + //return opencv_solver_fullscene(so, (PoserDataFullScene *)(pd)); + } + } + return -1; +} + +REGISTER_LINKTIME(PoserCharlesRefine); -- cgit v1.2.3 From d3d159772f1684082d7a545fbb3028d98d803308 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 3 Apr 2018 11:45:37 -0600 Subject: Added necessary exports for winbuild --- include/libsurvive/survive.h | 4 +-- redist/linmath.h | 83 ++++++++++++++++++++++++-------------------- 2 files changed, 47 insertions(+), 40 deletions(-) diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index b829a52..93ac9b7 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -239,7 +239,7 @@ struct SurviveContext { struct survive_calibration_config calibration_config; }; -void survive_verify_FLT_size( +SURVIVE_EXPORT void survive_verify_FLT_size( uint32_t user_size); // Baked in size of FLT to verify users of the library have the correct setting. SURVIVE_EXPORT SurviveContext *survive_init_internal(int argc, char *const *argv); @@ -278,7 +278,7 @@ SURVIVE_EXPORT SurviveObject *survive_get_so_by_name(SurviveContext *ctx, const // Utilitiy functions. int survive_simple_inflate(SurviveContext *ctx, const char *input, int inlen, char *output, int outlen); -int survive_send_magic(SurviveContext *ctx, int magic_code, void *data, int datalen); +SURVIVE_EXPORT int survive_send_magic(SurviveContext *ctx, int magic_code, void *data, int datalen); // These functions search both the stored-general and temporary sections for a parameter and return it. #define SC_GET 0 // Get, only. diff --git a/redist/linmath.h b/redist/linmath.h index 76e1322..094c230 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -7,6 +7,13 @@ extern "C" { #endif + +#ifdef _WIN32 +#define LINMATH_EXPORT __declspec(dllexport) +#else +#define LINMATH_EXPORT __attribute__((visibility("default"))) +#endif + // Yes, I know it's kind of arbitrary. #define DEFAULT_EPSILON 0.001 @@ -64,85 +71,85 @@ extern LinmathQuat LinmathQuat_Identity; extern LinmathPose LinmathPose_Identity; // NOTE: Inputs may never be output with cross product. -void cross3d(FLT *out, const FLT *a, const FLT *b); +LINMATH_EXPORT void cross3d(FLT *out, const FLT *a, const FLT *b); -void sub3d(FLT *out, const FLT *a, const FLT *b); +LINMATH_EXPORT void sub3d(FLT *out, const FLT *a, const FLT *b); -void add3d(FLT *out, const FLT *a, const FLT *b); +LINMATH_EXPORT void add3d(FLT *out, const FLT *a, const FLT *b); -void scale3d(FLT *out, const FLT *a, FLT scalar); +LINMATH_EXPORT void scale3d(FLT *out, const FLT *a, FLT scalar); -void normalize3d(FLT *out, const FLT *in); +LINMATH_EXPORT void normalize3d(FLT *out, const FLT *in); FLT dot3d(const FLT *a, const FLT *b); // Returns 0 if equal. If either argument is null, 0 will ALWAYS be returned. int compare3d(const FLT *a, const FLT *b, FLT epsilon); -void copy3d(FLT *out, const FLT *in); +LINMATH_EXPORT void copy3d(FLT *out, const FLT *in); FLT magnitude3d(const FLT *a); FLT dist3d(const FLT *a, const FLT *b); FLT anglebetween3d(FLT *a, FLT *b); -void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle); -void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest); -void axisanglefromquat(FLT *angle, FLT *axis, LinmathQuat quat); +LINMATH_EXPORT void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle); +LINMATH_EXPORT void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest); +LINMATH_EXPORT void axisanglefromquat(FLT *angle, FLT *axis, LinmathQuat quat); // Quaternion things... typedef FLT LinmathEulerAngle[3]; -void quatsetnone(LinmathQuat q); -void quatcopy(LinmathQuat q, const LinmathQuat qin); -void quatfromeuler(LinmathQuat q, const LinmathEulerAngle euler); -void quattoeuler(LinmathEulerAngle euler, const LinmathQuat q); -void quatfromaxisangle(LinmathQuat q, const FLT *axis, FLT radians); +LINMATH_EXPORT void quatsetnone(LinmathQuat q); +LINMATH_EXPORT void quatcopy(LinmathQuat q, const LinmathQuat qin); +LINMATH_EXPORT void quatfromeuler(LinmathQuat q, const LinmathEulerAngle euler); +LINMATH_EXPORT void quattoeuler(LinmathEulerAngle euler, const LinmathQuat q); +LINMATH_EXPORT void quatfromaxisangle(LinmathQuat q, const FLT *axis, FLT radians); FLT quatmagnitude(const LinmathQuat q); FLT quatinvsqmagnitude(const LinmathQuat q); -void quatnormalize(LinmathQuat qout, const LinmathQuat qin); // Safe for in to be same as out. -void quattomatrix(FLT *matrix44, const LinmathQuat q); -void quatfrommatrix(LinmathQuat q, const FLT *matrix44); -void quatfrommatrix33(LinmathQuat q, const FLT *matrix33); -void quatgetconjugate(LinmathQuat qout, const LinmathQuat qin); -void quatgetreciprocal(LinmathQuat qout, const LinmathQuat qin); -void quatsub(LinmathQuat qout, const LinmathQuat a, const LinmathQuat b); -void quatadd(LinmathQuat qout, const LinmathQuat a, const LinmathQuat b); -void quatrotateabout(LinmathQuat qout, const LinmathQuat a, +LINMATH_EXPORT void quatnormalize(LinmathQuat qout, const LinmathQuat qin); // Safe for in to be same as out. +LINMATH_EXPORT void quattomatrix(FLT *matrix44, const LinmathQuat q); +LINMATH_EXPORT void quatfrommatrix(LinmathQuat q, const FLT *matrix44); +LINMATH_EXPORT void quatfrommatrix33(LinmathQuat q, const FLT *matrix33); +LINMATH_EXPORT void quatgetconjugate(LinmathQuat qout, const LinmathQuat qin); +LINMATH_EXPORT void quatgetreciprocal(LinmathQuat qout, const LinmathQuat qin); +LINMATH_EXPORT void quatsub(LinmathQuat qout, const LinmathQuat a, const LinmathQuat b); +LINMATH_EXPORT void quatadd(LinmathQuat qout, const LinmathQuat a, const LinmathQuat b); +LINMATH_EXPORT void quatrotateabout(LinmathQuat qout, const LinmathQuat a, const LinmathQuat b); // same as quat multiply, not piecewise multiply. -void quatscale(LinmathQuat qout, const LinmathQuat qin, FLT s); +LINMATH_EXPORT void quatscale(LinmathQuat qout, const LinmathQuat qin, FLT s); FLT quatinnerproduct(const LinmathQuat qa, const LinmathQuat qb); -void quatouterproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat qb); -void quatevenproduct(LinmathQuat q, LinmathQuat qa, LinmathQuat qb); -void quatoddproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat qb); -void quatslerp(LinmathQuat q, const LinmathQuat qa, const LinmathQuat qb, FLT t); -void quatrotatevector(FLT *vec3out, const LinmathQuat quat, const FLT *vec3in); -void quatfrom2vectors(LinmathQuat q, const FLT *src, const FLT *dest); +LINMATH_EXPORT void quatouterproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat qb); +LINMATH_EXPORT void quatevenproduct(LinmathQuat q, LinmathQuat qa, LinmathQuat qb); +LINMATH_EXPORT void quatoddproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat qb); +LINMATH_EXPORT void quatslerp(LinmathQuat q, const LinmathQuat qa, const LinmathQuat qb, FLT t); +LINMATH_EXPORT void quatrotatevector(FLT *vec3out, const LinmathQuat quat, const FLT *vec3in); +LINMATH_EXPORT void quatfrom2vectors(LinmathQuat q, const FLT *src, const FLT *dest); // This is the quat equivalent of 'pout = pose * pin' if pose were a 4x4 matrix in homogenous space -void ApplyPoseToPoint(LinmathPoint3d pout, const LinmathPose *pose, const LinmathPoint3d pin); +LINMATH_EXPORT void ApplyPoseToPoint(LinmathPoint3d pout, const LinmathPose *pose, const LinmathPoint3d pin); // This is the quat equivalent of 'pout = lhs_pose * rhs_pose' if poses were a 4x4 matrix in homogenous space -void ApplyPoseToPose(LinmathPose *pout, const LinmathPose *lhs_pose, const LinmathPose *rhs_pose); +LINMATH_EXPORT void ApplyPoseToPose(LinmathPose *pout, const LinmathPose *lhs_pose, const LinmathPose *rhs_pose); // This is the quat equivlant of 'pose_in^-1'; so that ApplyPoseToPose(..., InvertPose(..., pose_in), pose_in) == // Identity ( [0, 0, 0], [1, 0, 0, 0] ) // by definition. -void InvertPose(LinmathPose *poseout, const LinmathPose *pose_in); +LINMATH_EXPORT void InvertPose(LinmathPose *poseout, const LinmathPose *pose_in); -void PoseToMatrix(FLT *mat44, const LinmathPose *pose_in); +LINMATH_EXPORT void PoseToMatrix(FLT *mat44, const LinmathPose *pose_in); // Matrix Stuff typedef struct { FLT val[3][3]; // row, column } Matrix3x3; -void rotate_vec(FLT *out, const FLT *in, Matrix3x3 rot); -void rotation_between_vecs_to_m3(Matrix3x3 *m, const FLT v1[3], const FLT v2[3]); +LINMATH_EXPORT void rotate_vec(FLT *out, const FLT *in, Matrix3x3 rot); +LINMATH_EXPORT void rotation_between_vecs_to_m3(Matrix3x3 *m, const FLT v1[3], const FLT v2[3]); Matrix3x3 inverseM33(const Matrix3x3 mat); -void matrix44copy(FLT *mout, const FLT *minm); -void matrix44transpose(FLT *mout, const FLT *minm); +LINMATH_EXPORT void matrix44copy(FLT *mout, const FLT *minm); +LINMATH_EXPORT void matrix44transpose(FLT *mout, const FLT *minm); #ifdef __cplusplus } -- cgit v1.2.3 From 18b20af7195b94889924156de2b4aa704b2c7391 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 3 Apr 2018 15:11:12 -0600 Subject: Refactor pose function to get timecode and not lh --- include/libsurvive/poser.h | 6 +++--- include/libsurvive/survive.h | 8 +++++--- include/libsurvive/survive_types.h | 2 +- simple_pose_test.c | 12 ++++++------ src/poser.c | 26 ++++++++++++++++++++++---- src/poser_daveortho.c | 2 +- src/poser_epnp.c | 6 +++--- src/poser_sba.c | 6 +++--- src/poser_turveytori.c | 4 ++-- src/survive.c | 8 ++++---- src/survive_process.c | 6 +++--- test.c | 8 ++++---- winbuild/test/test.vcxproj | 2 +- 13 files changed, 58 insertions(+), 38 deletions(-) diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h index dcd0e93..4cddf89 100644 --- a/include/libsurvive/poser.h +++ b/include/libsurvive/poser.h @@ -18,14 +18,14 @@ typedef enum PoserType_t { POSERDATA_SYNC, // Sync pulse. } PoserType; -typedef void (*poser_raw_pose_func)(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose, void *user); +typedef void (*poser_pose_func)(SurviveObject *so, uint32_t lighthouse, SurvivePose *pose, void *user); typedef void (*poser_lighthouse_pose_func)(SurviveObject *so, uint8_t lighthouse, SurvivePose *lighthouse_pose, SurvivePose *object_pose, void *user); typedef struct { PoserType pt; - poser_raw_pose_func rawposeproc; + poser_pose_func poseproc; poser_lighthouse_pose_func lighthouseposeproc; void *userdata; } PoserData; @@ -41,7 +41,7 @@ typedef struct * @param pose The actual object pose. This is in world space, not in LH space. It must represent a transformation from * object space of the SO to global space. */ -void PoserData_poser_raw_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose); +void PoserData_poser_pose_func(PoserData *poser_data, SurviveObject *so, SurvivePose *pose); /** * Meant to be used by individual posers to report back their findings on the pose of a lighthouse. diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 93ac9b7..0558dc8 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -75,6 +75,7 @@ struct SurviveObject { // Pose Information, also "poser" field. FLT PoseConfidence; // 0..1 SurvivePose OutPose; // Final pose? (some day, one can dream!) + uint32_t OutPose_timecode; SurvivePose FromLHPose[NUM_LIGHTHOUSES]; // Filled out by poser, contains computed position from each lighthouse. void *PoserData; // Initialized to zero, configured by poser, can be anything the poser wants. PoserCB PoserFn; @@ -114,6 +115,7 @@ struct SurviveObject { haptic_func haptic; SurviveSensorActivations activations; + void* user_ptr; // Debug int tsl; }; @@ -206,7 +208,7 @@ struct SurviveContext { imu_process_func imuproc; angle_process_func angleproc; button_process_func buttonproc; - raw_pose_func rawposeproc; + pose_func poseproc; lighthouse_pose_func lighthouseposeproc; htc_config_func configfunction; handle_lightcap_func lightcapfunction; @@ -268,7 +270,7 @@ SURVIVE_EXPORT void survive_install_light_fn(SurviveContext *ctx, light_process_ SURVIVE_EXPORT void survive_install_imu_fn(SurviveContext *ctx, imu_process_func fbp); SURVIVE_EXPORT void survive_install_angle_fn(SurviveContext *ctx, angle_process_func fbp); SURVIVE_EXPORT void survive_install_button_fn(SurviveContext *ctx, button_process_func fbp); -SURVIVE_EXPORT void survive_install_raw_pose_fn(SurviveContext *ctx, raw_pose_func fbp); +SURVIVE_EXPORT void survive_install_pose_fn(SurviveContext *ctx, pose_func fbp); SURVIVE_EXPORT void survive_install_lighthouse_pose_fn(SurviveContext *ctx, lighthouse_pose_func fbp); SURVIVE_EXPORT int survive_startup(SurviveContext *ctx); SURVIVE_EXPORT int survive_poll(SurviveContext *ctx); @@ -310,7 +312,7 @@ SURVIVE_EXPORT void survive_default_angle_process(SurviveObject *so, int sensor_ SURVIVE_EXPORT void survive_default_button_process(SurviveObject *so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); -SURVIVE_EXPORT void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose); +SURVIVE_EXPORT void survive_default_raw_pose_process(SurviveObject *so, uint32_t timecode, SurvivePose *pose); SURVIVE_EXPORT void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lh_pose, SurvivePose *obj_pose); SURVIVE_EXPORT int survive_default_htc_config_process(SurviveObject *so, char *ct0conf, int len); diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index 7a7dbf1..edce3e9 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -49,7 +49,7 @@ typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id ); typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh); typedef void(*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); -typedef void (*raw_pose_func)(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose); +typedef void (*pose_func)(SurviveObject *so, uint32_t timecode, SurvivePose *pose); typedef void (*lighthouse_pose_func)(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose, SurvivePose *object_pose); // For lightcap, etc. Don't change this structure at all. Regular vive is dependent on it being exactly as-is. diff --git a/simple_pose_test.c b/simple_pose_test.c index 0772abb..1696366 100644 --- a/simple_pose_test.c +++ b/simple_pose_test.c @@ -42,10 +42,10 @@ void HandleDestroy() FLT hpos[3]; FLT hpos2[3]; -void testprog_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { - survive_default_raw_pose_process(so, lighthouse, pose ); +void testprog_raw_pose_process(SurviveObject *so, uint32_t timecode, SurvivePose *pose) { + survive_default_raw_pose_process(so, timecode, pose ); - if( lighthouse != 0 || strcmp( so->codename, "HMD" ) != 0 ) + if( strcmp( so->codename, "HMD" ) != 0 ) return; // print the pose; @@ -64,7 +64,7 @@ void testprog_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePos hy = pos[1]; hz = pos[2];*/ - printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [ang:%08.2f %08.2f %08.2f %08.2f]\n", lighthouse, so->codename, + printf("Pose: [%u][%s][% 08.8f,% 08.8f,% 08.8f] [ang:%08.2f %08.2f %08.2f %08.2f]\n", timecode, so->codename, pose->Pos[0], pose->Pos[1], pose->Pos[2], pose->Rot[0], pose->Rot[1], pose->Rot[2], pose->Rot[3]); hpos[0] = pose->Pos[0]; @@ -163,9 +163,9 @@ int main( int argc, char ** argv ) } //survive_install_button_fn(ctx, testprog_button_process); - survive_install_raw_pose_fn(ctx, testprog_raw_pose_process); + survive_install_pose_fn(ctx, testprog_raw_pose_process); //survive_install_imu_fn(ctx, testprog_imu_process); - survive_install_raw_pose_fn(ctx, testprog_raw_pose_process); + survive_install_pose_fn(ctx, testprog_raw_pose_process); //survive_install_angle_fn(ctx, testprog_angle_process ); ctx->bsd[0].PositionSet = ctx->bsd[1].PositionSet = 1; diff --git a/src/poser.c b/src/poser.c index 9a0de24..20334f7 100644 --- a/src/poser.c +++ b/src/poser.c @@ -7,11 +7,29 @@ #define _USE_MATH_DEFINES // for C #include -void PoserData_poser_raw_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { - if (poser_data->rawposeproc) { - poser_data->rawposeproc(so, lighthouse, pose, poser_data->userdata); +static uint32_t PoserData_timecode(PoserData *poser_data) { + switch (poser_data->pt) { + case POSERDATA_LIGHT: { + PoserDataLight *lightData = (PoserDataLight *)poser_data; + return lightData->timecode; + } + case POSERDATA_FULL_SCENE: { + PoserDataFullScene* pdfs = (PoserDataFullScene *)(poser_data); + return -1; + } + case POSERDATA_IMU: { + PoserDataIMU *imuData = (PoserDataIMU *)poser_data; + return imuData->timecode; + } + } + return -1; +} + +void PoserData_poser_pose_func(PoserData *poser_data, SurviveObject *so, SurvivePose *pose) { + if (poser_data->poseproc) { + poser_data->poseproc(so, PoserData_timecode(poser_data), pose, poser_data->userdata); } else { - so->ctx->rawposeproc(so, lighthouse, pose); + so->ctx->poseproc(so, PoserData_timecode(poser_data), pose); } } diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index 9cdab45..330e7e8 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -107,7 +107,7 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) SurvivePose obj2world; ApplyPoseToPose(&obj2world, &lh2world, &objpose); - PoserData_poser_raw_pose_func(pd, so, lhid, &obj2world); + PoserData_poser_pose_func(pd, so, &obj2world); if (0) { fprintf(stderr,"INQUAT: %f %f %f %f = %f [%f %f %f]\n", objpose.Rot[0], objpose.Rot[1], objpose.Rot[2], diff --git a/src/poser_epnp.c b/src/poser_epnp.c index c05450a..eaa1659 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -164,7 +164,7 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) { if (winnerTakesAll) { int winner = meas[0] > meas[1] ? 0 : 1; - PoserData_poser_raw_pose_func(pd, so, winner, &posers[winner]); + PoserData_poser_pose_func(pd, so, &posers[winner]); } else { double a, b; a = meas[0] * meas[0]; @@ -175,11 +175,11 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) { interpolate.Pos[i] = (posers[0].Pos[i] * a + posers[1].Pos[i] * b) / (t); } quatslerp(interpolate.Rot, posers[0].Rot, posers[1].Rot, b / (t)); - PoserData_poser_raw_pose_func(pd, so, lightData->lh, &interpolate); + PoserData_poser_pose_func(pd, so, &interpolate); } } else { if (meas[lightData->lh]) - PoserData_poser_raw_pose_func(pd, so, lightData->lh, &posers[lightData->lh]); + PoserData_poser_pose_func(pd, so, &posers[lightData->lh]); } return 0; } diff --git a/src/poser_sba.c b/src/poser_sba.c index bd7d520..e74bb20 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -129,7 +129,7 @@ typedef struct { SurvivePose poses; } sba_set_position_t; -static void sba_set_position(SurviveObject *so, uint8_t lighthouse, SurvivePose *new_pose, void *_user) { +static void sba_set_position(SurviveObject *so, uint32_t timecode, SurvivePose *new_pose, void *_user) { sba_set_position_t *user = _user; assert(user->hasInfo == false); user->hasInfo = 1; @@ -220,7 +220,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive PoserData hdr = pdl->hdr; memset(&pdl->hdr, 0, sizeof(pdl->hdr)); // Clear callback functions pdl->hdr.pt = hdr.pt; - pdl->hdr.rawposeproc = sba_set_position; + pdl->hdr.poseproc = sba_set_position; sba_set_position_t locations = {0}; pdl->hdr.userdata = &locations; @@ -278,7 +278,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive if (status > 0 && (info[1] / meas_size * 2) < d->max_error) { d->failures_to_reset_cntr = d->failures_to_reset; quatnormalize(soLocation.Rot, soLocation.Rot); - PoserData_poser_raw_pose_func(&pdl->hdr, so, 1, &soLocation); + PoserData_poser_pose_func(&pdl->hdr, so, &soLocation); } { diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 4628207..243051e 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1631,9 +1631,9 @@ static void QuickPose(SurviveObject *so, PoserData *pd, SurvivePose *additionalT SolveForLighthouse(&pose.Pos[0], &pose.Rot[0], to, so, pd, 0, additionalTx, lh, 0); //printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); - if (so->ctx->rawposeproc) + if (so->ctx->poseproc) { - so->ctx->rawposeproc(so, lh, &pose); + so->ctx->poseproc(so, lh, &pose); } if (ttDebug) printf("!\n"); diff --git a/src/survive.c b/src/survive.c index f74de85..f6f98fc 100644 --- a/src/survive.c +++ b/src/survive.c @@ -217,7 +217,7 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) { ctx->angleproc = survive_default_angle_process; ctx->lighthouseposeproc = survive_default_lighthouse_pose_process; ctx->configfunction = survive_default_htc_config_process; - ctx->rawposeproc = survive_default_raw_pose_process; + ctx->poseproc = survive_default_raw_pose_process; ctx->calibration_config = survive_calibration_config_ctor(); ctx->calibration_config.use_flag = (enum SurviveCalFlag)survive_configi(ctx, "bsd-cal", SC_GET, SVCal_All); @@ -381,11 +381,11 @@ void survive_install_button_fn(SurviveContext *ctx, button_process_func fbp) { ctx->buttonproc = survive_default_button_process; } -void survive_install_raw_pose_fn(SurviveContext *ctx, raw_pose_func fbp) { +void survive_install_pose_fn(SurviveContext *ctx, pose_func fbp) { if (fbp) - ctx->rawposeproc = fbp; + ctx->poseproc = fbp; else - ctx->rawposeproc = survive_default_raw_pose_process; + ctx->poseproc = survive_default_raw_pose_process; } void survive_install_lighthouse_pose_fn(SurviveContext *ctx, lighthouse_pose_func fbp) { diff --git a/src/survive_process.c b/src/survive_process.c index 62459f2..e013327 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -139,12 +139,12 @@ void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8 //} } -void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { +void survive_default_raw_pose_process(SurviveObject *so, uint32_t timecode, SurvivePose *pose) { // print the pose; //printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); so->OutPose = *pose; - so->FromLHPose[lighthouse] = *pose; - survive_recording_raw_pose_process(so, lighthouse, pose); + so->OutPose_timecode = timecode; + survive_recording_raw_pose_process(so, timecode, pose); } void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose, diff --git a/test.c b/test.c index ad7aaa0..9fda5b1 100644 --- a/test.c +++ b/test.c @@ -91,11 +91,11 @@ void testprog_lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, Surviv pose->Pos[1], pose->Pos[2], pose->Rot[0], pose->Rot[1], pose->Rot[2], pose->Rot[3]); } -void testprog_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { - survive_default_raw_pose_process(so, lighthouse, pose); +void testprog_raw_pose_process(SurviveObject *so, uint32_t timecode, SurvivePose *pose) { + survive_default_raw_pose_process(so, timecode, pose); // print the pose; - printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, + printf("Pose: [%u][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", timecode, so->codename, pose->Pos[0], pose->Pos[1], pose->Pos[2], pose->Rot[0], pose->Rot[1], pose->Rot[2], pose->Rot[3]); } @@ -147,7 +147,7 @@ int main( int argc, char ** argv ) } survive_install_button_fn(ctx, testprog_button_process); - survive_install_raw_pose_fn(ctx, testprog_raw_pose_process); + survive_install_pose_fn(ctx, testprog_raw_pose_process); survive_install_imu_fn(ctx, testprog_imu_process); survive_install_lighthouse_pose_fn(ctx, testprog_lighthouse_process); diff --git a/winbuild/test/test.vcxproj b/winbuild/test/test.vcxproj index 5ceff3d..8df96d3 100644 --- a/winbuild/test/test.vcxproj +++ b/winbuild/test/test.vcxproj @@ -95,7 +95,7 @@ Console - setupapi.lib;dbghelp.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;libblas.lib;liblapacke.lib;liblapack.lib;%(AdditionalDependencies) + setupapi.lib;dbghelp.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) true UseFastLinkTimeCodeGeneration -- cgit v1.2.3 From 52c11484ca8ada2e163cbbae72d0d417f0342a38 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Tue, 3 Apr 2018 21:56:50 -0400 Subject: The reuslts are incredibly stable. Need to test on the opi. --- src/poser_charlesrefine.c | 366 +++++++++++++++++++++++++--------------------- 1 file changed, 201 insertions(+), 165 deletions(-) diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 4d44722..e6f6a57 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -9,6 +9,7 @@ #include #include #include +#include #define MAX_PT_PER_SWEEP 32 @@ -17,8 +18,10 @@ typedef struct { int sweepaxis; int sweeplh; - FLT normal_at_errors[MAX_PT_PER_SWEEP][3]; - FLT quantity_errors[MAX_PT_PER_SWEEP] + FLT normal_at_errors[MAX_PT_PER_SWEEP][3]; //Value is actually normalized, not just normal to sweep plane. + FLT quantity_errors[MAX_PT_PER_SWEEP]; + FLT angles_at_pts[MAX_PT_PER_SWEEP]; + SurvivePose object_pose_at_hit[MAX_PT_PER_SWEEP]; uint8_t sensor_ids[MAX_PT_PER_SWEEP]; int ptsweep; } CharlesPoserData; @@ -51,7 +54,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { dd->sweeplh = lhid; //FOR NOW, drop LH1. - if( lhid == 1 ) break; + //if( lhid == 1 ) break; // const FLT * sensor_normal = &so->sensor_normals[senid*3]; @@ -60,7 +63,11 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { const FLT * sensor_inpos = &so->sensor_locations[senid*3]; FLT sensor_position_worldspace[3]; + //XXX Once I saw this get pretty wild (When in playback) + //I had to invert the values of sensor_inpos. Not sure why. ApplyPoseToPoint(sensor_position_worldspace, object_pose, sensor_inpos); + + //printf( "%f %f %f == > %f %f %f\n", sensor_inpos[0], sensor_inpos[1], sensor_inpos[2], sensor_position_worldspace[0], sensor_position_worldspace[1], sensor_position_worldspace[2] ); // = sensor position, relative to lighthouse center. FLT sensorpos_rel_lh[3]; @@ -96,200 +103,229 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { if( (i = dd->ptsweep) < MAX_PT_PER_SWEEP ) { - dd->normal_at_errors[i] = sweep_normal; + memcpy( dd->normal_at_errors[i], sweep_normal, sizeof(FLT)*3 ); dd->quantity_errors[i] = dist; + dd->angles_at_pts[i] = angle; dd->sensor_ids[i] = sensor_id; - dd->ptsweep++: + memcpy( &dd->object_pose_at_hit[i], object_pose, sizeof(SurvivePose) ); + dd->ptsweep++; } -#if 0 - printf( "D %d %d: %f [%f %f %f]\n", lhid, axis, dist, sweep_normal[0], sweep_normal[1], sweep_normal[2] ); + return 0; + } + case POSERDATA_SYNC: { + PoserDataLight *l = (PoserDataLight *)pd; + int lhid = l->lh; - //Naieve approach... Push it in the right direction - SurvivePose object_pose_out; - quatcopy( object_pose_out.Rot, object_pose->Rot ); - scale3d(sweep_normal, sweep_normal, -0.1*dist); - add3d(object_pose_out.Pos, sweep_normal, object_pose->Pos); - if( so->PoseConfidence < .01 ) + //you can get sweepaxis and sweeplh. + if( dd->ptsweep ) { - dd->average_nudge[0] = 0; - dd->average_nudge[1] = 0; - dd->average_nudge[2] = 0; - - memcpy( &object_pose_out, &LinmathPose_Identity, sizeof( LinmathPose_Identity ) ); - object_pose_out.Pos[1] = 2.5; - object_pose_out.Pos[2] = 1.8; - so->PoseConfidence = 1.0; - } -// printf( "%f %f %f %f\n", object_pose->Rot[0], object_pose->Rot[1], object_pose->Rot[2], object_pose->Rot[3] ); - - PoserData_poser_raw_pose_func(pd, so, lhid, &object_pose_out); -#endif - -#if 0 -// = { - axis?0.0:sin(angle), - axis?sin(angle):0.0, - cos(angle) }; - - = sensor_locations; - LinmathPoint3d - int8_t sensor_ct; // sensor count - FLT *sensor_locations; // size is sensor_ct*3. Contains x,y,z values for each sensor - FLT *sensor_normals; // size is nrlocations*3. cointains normal vector for each sensor - - - // This is the quat equivalent of 'pout = pose * pin' if pose were a 4x4 matrix in homogenous space -void ApplyPoseToPoint(LinmathPoint3d pout, const LinmathPose *pose, const LinmathPoint3d pin); + int i; + int lhid = dd->sweeplh; + int axis = dd->sweepaxis; + int pts = dd->ptsweep; + const SurvivePose * object_pose = &so->OutPose; //XXX TODO Should pull pose from approximate time when LHs were scanning it. + BaseStationData * bsd = &so->ctx->bsd[lhid]; + SurvivePose * lh_pose = &bsd->Pose; + int validpoints = 0; + int ptvalid[MAX_PT_PER_SWEEP]; + FLT avgerr = 0.0; + FLT vec_correct[3] = { 0., 0. , 0. }; + FLT avgang = 0.0; - SurvivePose obj2world; -// ApplyPoseToPose(&obj2world, &lh2world, &objpose); - memcpy( &obj2world, &LinmathPose_Identity, sizeof( obj2world ) ); - obj2world.Pos[1] = 1; - PoserData_poser_raw_pose_func(pd, so, lhid, &obj2world); -// PoserData_poser_raw_pose_func(pd, so, ld->lh, &posers[lightData->lh]); +//Tunable parameters: +#define MIN_HIT_QUALITY 0.5 //Determines which hits to cull. +#define HIT_QUALITY_BASELINE 0.0001 //Determines which hits to cull. Actually SQRT(baseline) if 0.0001, it is really 1cm +#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.2 //Explodes if you exceed 1.0 +#define CORRECT_TELESCOPTION_COEFFICIENT 1.0 //Converges even as high as 10.0 and doesn't explode. +#define CORRECT_ROTATION_COEFFICIENT 5.0 //This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. +#define ROTATIONAL_CORRECTION_MAXFORCE 0.10 - // Pose Information, also "poser" field. - ///from SurviveObject - // FLT PoseConfidence; // 0..1 - // SurvivePose OutPose; // Final pose? (some day, one can dream!) - // SurvivePose FromLHPose[NUM_LIGHTHOUSES]; // Filled out by poser, contains computed position from each lighthouse. - // void *PoserData; // Initialized to zero, configured by poser, can be anything the poser wants. - // PoserCB PoserFn; + //Step 1: Determine standard of deviation, and average in order to + // drop points that are likely in error. + { + //Calculate average + FLT avgerr_orig = 0.0; + FLT stddevsq = 0.0; + for( i = 0; i < pts; i++ ) + avgerr_orig += dd->quantity_errors[i]; + avgerr_orig/=pts; + + //Calculate standard of deviation. + for( i = 0; i < pts; i++ ) + { + FLT diff = dd->quantity_errors[i]-avgerr_orig; + stddevsq += diff*diff; + } + stddevsq/=pts; + + for( i = 0; i < pts; i++ ) + { + FLT err = dd->quantity_errors[i]; + FLT diff = err-avgerr_orig; + diff *= diff; + int isptvalid = (diff * MIN_HIT_QUALITY <= stddevsq + HIT_QUALITY_BASELINE)?1:0; + ptvalid[i] = isptvalid; + if( isptvalid ) + { + avgang += dd->angles_at_pts[i]; + avgerr += err; + validpoints ++; + } + } + avgang /= validpoints; + avgerr /= validpoints; + } + //Step 2: Determine average lateral error. + //We can actually always perform this operation. Even with only one point. + { + FLT avg_err[3] = { 0, 0, 0 }; //Positional error. + for( i = 0; i < pts; i++ ) + { + if( !ptvalid[i] ) continue; + FLT * nrm = dd->normal_at_errors[i]; + FLT err = dd->quantity_errors[i]; + avg_err[0] = avg_err[0] + nrm[0] * err; + avg_err[1] = avg_err[1] + nrm[1] * err; + avg_err[2] = avg_err[2] + nrm[2] * err; + } - // printf( "%d %d %f\n", ld->sensor_id, ld->lh, ld->angle ); + //NOTE: The "avg_err" is not geometrically centered. This is actually + //probably okay, since if you have sevearl data points to one side, you + //can probably trust that more. + scale3d(avg_err, avg_err, 1./validpoints); -/* - SurvivePose *object_pose, void *user); + //We have "Average error" now. A vector in worldspace. + //This can correct for lateral error, but not distance from camera. -typedef struct -{ - PoserType pt; - poser_raw_pose_func rawposeproc; - poser_lighthouse_pose_func lighthouseposeproc; - void *userdata; -} PoserData; - - PoserData hdr; - int sensor_id; - int acode; //OOTX Code associated with this sweep. bit 1 indicates vertical(1) or horizontal(0) sweep - int lh; //Lighthouse making this sweep - uint32_t timecode; //In object-local ticks. - FLT length; //In seconds - FLT angle; //In radians from center of lighthouse. -} PoserDataLight; - - - -*/ -#if 0 - SurvivePose posers[2]; - int meas[2] = {0, 0}; - for (int lh = 0; lh < so->ctx->activeLighthouses; lh++) { - if (so->ctx->bsd[lh].PositionSet) { - epnp pnp = {.fu = 1, .fv = 1}; - epnp_set_maximum_number_of_correspondences(&pnp, so->sensor_ct); - - add_correspondences(so, &pnp, scene, lightData->timecode, lh); - static int required_meas = -1; - if (required_meas == -1) - required_meas = survive_configi(so->ctx, "epnp-required-meas", SC_GET, 4); - - if (pnp.number_of_correspondences > required_meas) { - - SurvivePose objInLh = solve_correspondence(so, &pnp, false); - if (quatmagnitude(objInLh.Rot) != 0) { - SurvivePose *lh2world = &so->ctx->bsd[lh].Pose; - - SurvivePose txPose = {.Rot = {1}}; - ApplyPoseToPose(&txPose, lh2world, &objInLh); - posers[lh] = txPose; - meas[lh] = pnp.number_of_correspondences; - } - } - - epnp_dtor(&pnp); + //XXX TODO: Should we check to see if we only have one or + //two points to make sure the error on this isn't unusually high? + //If calculated error is unexpectedly high, then we should probably + //Not apply the transform. + scale3d( avg_err, avg_err, -CORRECT_LATERAL_POSITION_COEFFICIENT ); + add3d( vec_correct, vec_correct, avg_err ); } - } - if (meas[0] > 0 && meas[1] > 0) { - SurvivePose interpolate = {0}; - bool winnerTakesAll = true; // Not convinced slerp does the right thing, will change this when i am - - if (winnerTakesAll) { - int winner = meas[0] > meas[1] ? 0 : 1; - PoserData_poser_raw_pose_func(pd, so, winner, &posers[winner]); - } else { - double a, b; - a = meas[0] * meas[0]; - b = meas[1] * meas[1]; - - double t = a + b; - for (size_t i = 0; i < 3; i++) { - interpolate.Pos[i] = (posers[0].Pos[i] * a + posers[1].Pos[i] * b) / (t); + //Step 3: Control telecoption from lighthouse. + // we need to find out what the weighting is to determine "zoom" + if( validpoints > 1 ) //Can't correct "zoom" with only one point. + { + FLT zoom = 0.0; + FLT rmsang = 0.0; + for( i = 0; i < pts; i++ ) + { + if( !ptvalid[i] ) continue; + FLT delang = dd->angles_at_pts[i] - avgang; + FLT delerr = dd->quantity_errors[i] - avgerr; + if( axis ) delang *= -1; //Flip sign on alternate axis because it's measured backwards. + zoom += delerr * delang; + rmsang += delang * delang; } - quatslerp(interpolate.Rot, posers[0].Rot, posers[1].Rot, b / (t)); - PoserData_poser_raw_pose_func(pd, so, lightData->lh, &interpolate); - } - } else { - if (meas[lightData->lh]) - PoserData_poser_raw_pose_func(pd, so, lightData->lh, &posers[lightData->lh]); - } -#endif -#endif - return 0; - } + //Control into or outof lighthouse. + //XXX Check to see if we need to sqrt( the rmsang), need to check convergance behavior close to lighthouse. + //This is a questionable step. + zoom /= sqrt(rmsang); - case POSERDATA_SYNC: { - PoserDataLight *l = (PoserDataLight *)pd; - int lhid = l->lh; + zoom *= CORRECT_TELESCOPTION_COEFFICIENT; - //you can get sweepaxis and sweeplh. + FLT veccamalong[3]; + sub3d( veccamalong, lh_pose->Pos, object_pose->Pos ); + normalize3d( veccamalong, veccamalong ); + scale3d( veccamalong, veccamalong, zoom ); + add3d( vec_correct, veccamalong, vec_correct ); + } - if( dd->ptsweep ) - { - int i; - int lhid = dd->lhid; - int pts = dd->ptsweep; - const SurvivePose * object_pose = &so->OutPose; - FLT avg_err[3] = { 0, 0, 0 }; - FLT avgtot = 0.0; - for( i = 0; i < pts; i++ ) - { - FLT * nrm = dd->normal_at_errors[pts]; - FLT qty = quantity_errors[pts]; - avgtot += qty; - avg_err[0] = avg_err[0] + nrm[0] * qty; - avg_err[1] = avg_err[1] + nrm[1] * qty; - avg_err[2] = avg_err[2] + nrm[2] * qty; + SurvivePose object_pose_out; + add3d(object_pose_out.Pos, vec_correct, object_pose->Pos); + + quatcopy( object_pose_out.Rot, object_pose->Rot ); + + //Stage 4: "Tug" on the rotation of the object, from all of the sensor's pov. + //If we were able to determine likliehood of a hit in the sweep instead of afterward + //we would actually be able to perform this on a per-hit basis. + if( 1 ) { + LinmathQuat correction; + quatcopy( correction, LinmathQuat_Identity ); + for( i = 0; i < pts; i++ ) + { + if( !ptvalid[i] ) continue; + FLT dist = dd->quantity_errors[i]-avgerr; + FLT angle = dd->angles_at_pts[i]; + int sensor_id = dd->sensor_ids[i]; + FLT * normal = dd->normal_at_errors[i]; + const SurvivePose * object_pose_at_hit = &dd->object_pose_at_hit[i]; + const FLT * sensor_inpos = &so->sensor_locations[sensor_id*3]; + + LinmathQuat world_to_object_space; + quatgetreciprocal(world_to_object_space, object_pose_at_hit->Rot); + FLT correction_in_object_space[3]; //The amount across the surface of the object the rotation should happen. + + quatrotatevector(correction_in_object_space, world_to_object_space, normal ); + dist *= CORRECT_ROTATION_COEFFICIENT; + if( dist > ROTATIONAL_CORRECTION_MAXFORCE ) dist = ROTATIONAL_CORRECTION_MAXFORCE; + if( dist <-ROTATIONAL_CORRECTION_MAXFORCE ) dist =-ROTATIONAL_CORRECTION_MAXFORCE; + + //Now, we have a "tug" vector in object-local space. Need to apply the torque. + FLT vector_from_center_of_object[3]; + normalize3d( vector_from_center_of_object, sensor_inpos ); + //scale3d(vector_from_center_of_object, sensor_inpos, 10.0 ); + // vector_from_center_of_object[2]*=-1; + // vector_from_center_of_object[1]*=-1; +// vector_from_center_of_object[0]*=-1; + //vector_from_center_of_object + scale3d(vector_from_center_of_object,vector_from_center_of_object, 1); + + + FLT new_vector_in_object_space[3]; + //printf( "%f %f %f %f\n", object_pose_at_hit->Rot[0], object_pose_at_hit->Rot[1], object_pose_at_hit->Rot[2], object_pose_at_hit->Rot[3] ); + //printf( "%f %f %f // %f %f %f // %f\n", vector_from_center_of_object[0], vector_from_center_of_object[1], vector_from_center_of_object[2], correction_in_object_space[0], correction_in_object_space[1], correction_in_object_space[2], dist ); + scale3d( correction_in_object_space, correction_in_object_space, -dist ); + add3d( new_vector_in_object_space, vector_from_center_of_object, correction_in_object_space ); + + normalize3d( new_vector_in_object_space, new_vector_in_object_space ); + + LinmathQuat corrective_quaternion; + quatfrom2vectors(corrective_quaternion, vector_from_center_of_object, new_vector_in_object_space ); + quatrotateabout( correction, correction, corrective_quaternion ); + //printf( "%f -> %f %f %f => %f %f %f [%f %f %f %f]\n", dist, vector_from_center_of_object[0], vector_from_center_of_object[1], vector_from_center_of_object[2], + //correction_in_object_space[0], correction_in_object_space[1], correction_in_object_space[2], + //corrective_quaternion[0],corrective_quaternion[1],corrective_quaternion[1],corrective_quaternion[3]); + } + printf( "Applying: %f %f %f %f\n", correction[0], correction[1], correction[2], correction[3] ); + //Apply our corrective quaternion to the output. + quatrotateabout( object_pose_out.Rot, object_pose_out.Rot, correction ); + quatnormalize( object_pose_out.Rot, object_pose_out.Rot ); } - scale3d(avg_err, avg_err, 1./pts); - //We have "Average error" now. This is a world space value. - //This can correct for lateral error, but not distance from camera. - - //Next we need to find out what the weighting is to determine "zoom" - //How do we do this? ??? Too tired to math. - FLT weight = 0.0; - for( i = 0; i < pts; i++ ) + + //Janky need to do this somewhere else... This initializes the pose estimator. + if( so->PoseConfidence < .01 ) { - //??!?!? Sturfff + memcpy( &object_pose_out, &LinmathPose_Identity, sizeof( LinmathPose_Identity ) ); + object_pose_out.Pos[0] = -0.14372776; + object_pose_out.Pos[1] = 0.06856518; + object_pose_out.Pos[2] = 0.01960009; + object_pose_out.Rot[0] = 1.0; + object_pose_out.Rot[1] = -0.0; + object_pose_out.Rot[2] = 0.0; + object_pose_out.Rot[3] = 0.0; + so->PoseConfidence = 1.0; } - dd->ptsweep = 0; + PoserData_poser_raw_pose_func(pd, so, lhid, &object_pose_out); - //Update PoserData_poser_raw_pose_func(pd, so, lhid, &object_pose_out); + dd->ptsweep = 0; } - dd->nextaxis = l->acode & 1; - printf( "SYNC %d %p\n", l->acode, dd ); + dd->sweepaxis = l->acode & 1; + //printf( "SYNC %d %p\n", l->acode, dd ); break; } case POSERDATA_FULL_SCENE: { -- cgit v1.2.3 From 872829b375ec0bc5bdbf9b223cf49ac1b4055de2 Mon Sep 17 00:00:00 2001 From: Charles Lohr Date: Wed, 4 Apr 2018 03:10:21 +0000 Subject: Ok, it works! --- src/poser_charlesrefine.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index e6f6a57..61bd575 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -141,9 +141,9 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { #define MIN_HIT_QUALITY 0.5 //Determines which hits to cull. #define HIT_QUALITY_BASELINE 0.0001 //Determines which hits to cull. Actually SQRT(baseline) if 0.0001, it is really 1cm -#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.2 //Explodes if you exceed 1.0 -#define CORRECT_TELESCOPTION_COEFFICIENT 1.0 //Converges even as high as 10.0 and doesn't explode. -#define CORRECT_ROTATION_COEFFICIENT 5.0 //This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. +#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.1 //Explodes if you exceed 1.0 +#define CORRECT_TELESCOPTION_COEFFICIENT 0.5 //Converges even as high as 10.0 and doesn't explode. +#define CORRECT_ROTATION_COEFFICIENT 1.0 //This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. #define ROTATIONAL_CORRECTION_MAXFORCE 0.10 //Step 1: Determine standard of deviation, and average in order to @@ -299,7 +299,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { //correction_in_object_space[0], correction_in_object_space[1], correction_in_object_space[2], //corrective_quaternion[0],corrective_quaternion[1],corrective_quaternion[1],corrective_quaternion[3]); } - printf( "Applying: %f %f %f %f\n", correction[0], correction[1], correction[2], correction[3] ); + //printf( "Applying: %f %f %f %f\n", correction[0], correction[1], correction[2], correction[3] ); //Apply our corrective quaternion to the output. quatrotateabout( object_pose_out.Rot, object_pose_out.Rot, correction ); quatnormalize( object_pose_out.Rot, object_pose_out.Rot ); -- cgit v1.2.3 From 34c6afbd52b7a6109d563ce17d082072b16ee032 Mon Sep 17 00:00:00 2001 From: Charles Lohr Date: Wed, 4 Apr 2018 03:34:25 +0000 Subject: Add gyro support (Still need madgwick's) --- src/poser_charlesrefine.c | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 61bd575..9aa44d9 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -37,6 +37,21 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { case POSERDATA_IMU: { // Really should use this... PoserDataIMU *imuData = (PoserDataIMU *)pd; + + + //TODO: Actually do Madgwick's algorithm + LinmathQuat applymotion; + const SurvivePose * object_pose = &so->OutPose; + imuData->gyro[0] *= -0.001; + imuData->gyro[1] *= -0.001; + imuData->gyro[2] *= 0.001; + quatfromeuler( applymotion, imuData->gyro ); + //printf( "%f %f %f\n", imuData->gyro [0], imuData->gyro [1], imuData->gyro [2] ); + SurvivePose object_pose_out; + quatrotateabout(object_pose_out.Rot, object_pose->Rot, applymotion ); + copy3d( object_pose_out.Pos, object_pose->Pos ); + PoserData_poser_raw_pose_func(pd, so, 0, &object_pose_out); + return 0; } case POSERDATA_LIGHT: { -- cgit v1.2.3 From 22301a092b4009c4f2ca2bc8be4511d75e3082b6 Mon Sep 17 00:00:00 2001 From: Charles Lohr Date: Wed, 4 Apr 2018 04:06:31 +0000 Subject: Tweak to paraemters to fix performance of gyro --- src/poser_charlesrefine.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 9aa44d9..c1cc6de 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -42,9 +42,9 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { //TODO: Actually do Madgwick's algorithm LinmathQuat applymotion; const SurvivePose * object_pose = &so->OutPose; - imuData->gyro[0] *= -0.001; - imuData->gyro[1] *= -0.001; - imuData->gyro[2] *= 0.001; + imuData->gyro[0] *= -0.0005; + imuData->gyro[1] *= -0.0005; + imuData->gyro[2] *= 0.0005; quatfromeuler( applymotion, imuData->gyro ); //printf( "%f %f %f\n", imuData->gyro [0], imuData->gyro [1], imuData->gyro [2] ); SurvivePose object_pose_out; @@ -156,8 +156,8 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { #define MIN_HIT_QUALITY 0.5 //Determines which hits to cull. #define HIT_QUALITY_BASELINE 0.0001 //Determines which hits to cull. Actually SQRT(baseline) if 0.0001, it is really 1cm -#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.1 //Explodes if you exceed 1.0 -#define CORRECT_TELESCOPTION_COEFFICIENT 0.5 //Converges even as high as 10.0 and doesn't explode. +#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.8 //Explodes if you exceed 1.0 +#define CORRECT_TELESCOPTION_COEFFICIENT 8.0 //Converges even as high as 10.0 and doesn't explode. #define CORRECT_ROTATION_COEFFICIENT 1.0 //This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. #define ROTATIONAL_CORRECTION_MAXFORCE 0.10 -- cgit v1.2.3 From fdbe85e242cec2765041404214eb03a16dde91fc Mon Sep 17 00:00:00 2001 From: cnlohr Date: Tue, 3 Apr 2018 00:56:56 -0400 Subject: For the same of simplifying testing, this seems like the best tool. Its original usefulness has since dried up but it can live again as a slightly different tool. --- simple_pose_test.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/simple_pose_test.c b/simple_pose_test.c index 1696366..d28e78d 100644 --- a/simple_pose_test.c +++ b/simple_pose_test.c @@ -168,6 +168,7 @@ int main( int argc, char ** argv ) survive_install_pose_fn(ctx, testprog_raw_pose_process); //survive_install_angle_fn(ctx, testprog_angle_process ); +#if 0 //Don't reset poses ctx->bsd[0].PositionSet = ctx->bsd[1].PositionSet = 1; int i; for( i = 0; i < 2; i++ ) @@ -181,7 +182,7 @@ int main( int argc, char ** argv ) p->Rot[2] = 0; p->Rot[3] = 0; } - +#endif OGCreateThread( GUIThread, 0 ); if( !ctx ) -- cgit v1.2.3 From acba33deca862be657f57876de7de950dd1d9af4 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Tue, 3 Apr 2018 01:05:48 -0400 Subject: Throw in my Wip poser. --- Makefile | 2 +- src/poser_charlesrefine.c | 302 ++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 303 insertions(+), 1 deletion(-) create mode 100644 src/poser_charlesrefine.c diff --git a/Makefile b/Makefile index 62ed5e8..dcef7ba 100644 --- a/Makefile +++ b/Makefile @@ -39,7 +39,7 @@ REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/minimal_ope ifeq ($(UNAME), Darwin) REDISTS:=$(REDISTS) redist/hid-osx.c endif -LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_charlesbiguator.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/epnp/epnp.o src/survive_sensor_activations.o src/survive_turveybiguator.o src/survive_disambiguator.o src/survive_statebased_disambiguator.o +LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_charlesbiguator.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/epnp/epnp.o src/survive_sensor_activations.o src/survive_turveybiguator.o src/survive_disambiguator.o src/survive_statebased_disambiguator.o src/poser_charlesrefine.o #If you want to use HIDAPI on Linux. #CFLAGS:=$(CFLAGS) -DHIDAPI diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c new file mode 100644 index 0000000..4d44722 --- /dev/null +++ b/src/poser_charlesrefine.c @@ -0,0 +1,302 @@ +//EXPERIMENTAL DRIVER - DO NOT USE + +#include +#include +#include + +#include "epnp/epnp.h" +#include "linmath.h" +#include +#include +#include + +#define MAX_PT_PER_SWEEP 32 + + +typedef struct +{ + int sweepaxis; + int sweeplh; + FLT normal_at_errors[MAX_PT_PER_SWEEP][3]; + FLT quantity_errors[MAX_PT_PER_SWEEP] + uint8_t sensor_ids[MAX_PT_PER_SWEEP]; + int ptsweep; +} CharlesPoserData; + + + +int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { + CharlesPoserData * dd = so->PoserData; + if( !dd ) so->PoserData = dd = calloc( sizeof(CharlesPoserData), 1 ); + + SurviveSensorActivations *scene = &so->activations; + switch (pd->pt) { + case POSERDATA_IMU: { + // Really should use this... + PoserDataIMU *imuData = (PoserDataIMU *)pd; + return 0; + } + case POSERDATA_LIGHT: { + int i; + PoserDataLight *ld = (PoserDataLight *)pd; + int lhid = ld->lh; + int senid = ld->sensor_id; + BaseStationData * bsd = &so->ctx->bsd[ld->lh]; + if( !bsd->PositionSet ) break; + SurvivePose * lhp = &bsd->Pose; + FLT angle = ld->angle; + int sensor_id = ld->sensor_id; + int axis = dd->sweepaxis; + const SurvivePose * object_pose = &so->OutPose; + dd->sweeplh = lhid; + + //FOR NOW, drop LH1. + if( lhid == 1 ) break; + + +// const FLT * sensor_normal = &so->sensor_normals[senid*3]; +// FLT sensor_normal_worldspace[3]; +// ApplyPoseToPoint(sensor_normal_worldspace, object_pose, sensor_inpos); + + const FLT * sensor_inpos = &so->sensor_locations[senid*3]; + FLT sensor_position_worldspace[3]; + ApplyPoseToPoint(sensor_position_worldspace, object_pose, sensor_inpos); + //printf( "%f %f %f == > %f %f %f\n", sensor_inpos[0], sensor_inpos[1], sensor_inpos[2], sensor_position_worldspace[0], sensor_position_worldspace[1], sensor_position_worldspace[2] ); + // = sensor position, relative to lighthouse center. + FLT sensorpos_rel_lh[3]; + sub3d( sensorpos_rel_lh, sensor_position_worldspace, lhp->Pos ); + + //Next, define a normal in global space of the plane created by the sweep hit. + //Careful that this must be normalized. + FLT sweep_normal[3]; + + //If 1, the "y" axis. //XXX Check me. + if( axis ) //XXX Just FYI this should include account for skew + { + sweep_normal[0] = 0; + sweep_normal[1] = cos(angle ); + sweep_normal[2] = sin( angle ); + //printf( "+" ); + } + else + { + sweep_normal[0] = cos( angle ); + sweep_normal[1] = 0; + sweep_normal[2] = -sin( angle ); + //printf( "-" ); + } + + //Need to apply the lighthouse's transformation to the sweep's normal. + quatrotatevector( sweep_normal, lhp->Rot, sweep_normal); + + //Compute point-line distance between sensorpos_rel_lh and the plane defined by sweep_normal. + //Do this by projecting sensorpos_rel_lh (w) onto sweep_normal (v). + //You can do this by |v dot w| / |v| ... But we know |v| is 1. So... + FLT dist = dot3d( sensorpos_rel_lh, sweep_normal ); + + if( (i = dd->ptsweep) < MAX_PT_PER_SWEEP ) + { + dd->normal_at_errors[i] = sweep_normal; + dd->quantity_errors[i] = dist; + dd->sensor_ids[i] = sensor_id; + dd->ptsweep++: + } + +#if 0 + printf( "D %d %d: %f [%f %f %f]\n", lhid, axis, dist, sweep_normal[0], sweep_normal[1], sweep_normal[2] ); + + + //Naieve approach... Push it in the right direction + SurvivePose object_pose_out; + quatcopy( object_pose_out.Rot, object_pose->Rot ); + scale3d(sweep_normal, sweep_normal, -0.1*dist); + add3d(object_pose_out.Pos, sweep_normal, object_pose->Pos); + + if( so->PoseConfidence < .01 ) + { + dd->average_nudge[0] = 0; + dd->average_nudge[1] = 0; + dd->average_nudge[2] = 0; + + memcpy( &object_pose_out, &LinmathPose_Identity, sizeof( LinmathPose_Identity ) ); + object_pose_out.Pos[1] = 2.5; + object_pose_out.Pos[2] = 1.8; + so->PoseConfidence = 1.0; + } +// printf( "%f %f %f %f\n", object_pose->Rot[0], object_pose->Rot[1], object_pose->Rot[2], object_pose->Rot[3] ); + + PoserData_poser_raw_pose_func(pd, so, lhid, &object_pose_out); +#endif + +#if 0 +// = { + axis?0.0:sin(angle), + axis?sin(angle):0.0, + cos(angle) }; + + = sensor_locations; + LinmathPoint3d + int8_t sensor_ct; // sensor count + FLT *sensor_locations; // size is sensor_ct*3. Contains x,y,z values for each sensor + FLT *sensor_normals; // size is nrlocations*3. cointains normal vector for each sensor + + + // This is the quat equivalent of 'pout = pose * pin' if pose were a 4x4 matrix in homogenous space +void ApplyPoseToPoint(LinmathPoint3d pout, const LinmathPose *pose, const LinmathPoint3d pin); + + + + SurvivePose obj2world; +// ApplyPoseToPose(&obj2world, &lh2world, &objpose); + memcpy( &obj2world, &LinmathPose_Identity, sizeof( obj2world ) ); + obj2world.Pos[1] = 1; + PoserData_poser_raw_pose_func(pd, so, lhid, &obj2world); +// PoserData_poser_raw_pose_func(pd, so, ld->lh, &posers[lightData->lh]); + + + // Pose Information, also "poser" field. + ///from SurviveObject + // FLT PoseConfidence; // 0..1 + // SurvivePose OutPose; // Final pose? (some day, one can dream!) + // SurvivePose FromLHPose[NUM_LIGHTHOUSES]; // Filled out by poser, contains computed position from each lighthouse. + // void *PoserData; // Initialized to zero, configured by poser, can be anything the poser wants. + // PoserCB PoserFn; + + + // printf( "%d %d %f\n", ld->sensor_id, ld->lh, ld->angle ); + +/* + SurvivePose *object_pose, void *user); + +typedef struct +{ + PoserType pt; + poser_raw_pose_func rawposeproc; + poser_lighthouse_pose_func lighthouseposeproc; + void *userdata; +} PoserData; + + PoserData hdr; + int sensor_id; + int acode; //OOTX Code associated with this sweep. bit 1 indicates vertical(1) or horizontal(0) sweep + int lh; //Lighthouse making this sweep + uint32_t timecode; //In object-local ticks. + FLT length; //In seconds + FLT angle; //In radians from center of lighthouse. +} PoserDataLight; + + + +*/ +#if 0 + SurvivePose posers[2]; + int meas[2] = {0, 0}; + for (int lh = 0; lh < so->ctx->activeLighthouses; lh++) { + if (so->ctx->bsd[lh].PositionSet) { + epnp pnp = {.fu = 1, .fv = 1}; + epnp_set_maximum_number_of_correspondences(&pnp, so->sensor_ct); + + add_correspondences(so, &pnp, scene, lightData->timecode, lh); + static int required_meas = -1; + if (required_meas == -1) + required_meas = survive_configi(so->ctx, "epnp-required-meas", SC_GET, 4); + + if (pnp.number_of_correspondences > required_meas) { + + SurvivePose objInLh = solve_correspondence(so, &pnp, false); + if (quatmagnitude(objInLh.Rot) != 0) { + SurvivePose *lh2world = &so->ctx->bsd[lh].Pose; + + SurvivePose txPose = {.Rot = {1}}; + ApplyPoseToPose(&txPose, lh2world, &objInLh); + posers[lh] = txPose; + meas[lh] = pnp.number_of_correspondences; + } + } + + epnp_dtor(&pnp); + } + } + + if (meas[0] > 0 && meas[1] > 0) { + SurvivePose interpolate = {0}; + bool winnerTakesAll = true; // Not convinced slerp does the right thing, will change this when i am + + if (winnerTakesAll) { + int winner = meas[0] > meas[1] ? 0 : 1; + PoserData_poser_raw_pose_func(pd, so, winner, &posers[winner]); + } else { + double a, b; + a = meas[0] * meas[0]; + b = meas[1] * meas[1]; + + double t = a + b; + for (size_t i = 0; i < 3; i++) { + interpolate.Pos[i] = (posers[0].Pos[i] * a + posers[1].Pos[i] * b) / (t); + } + quatslerp(interpolate.Rot, posers[0].Rot, posers[1].Rot, b / (t)); + PoserData_poser_raw_pose_func(pd, so, lightData->lh, &interpolate); + } + } else { + if (meas[lightData->lh]) + PoserData_poser_raw_pose_func(pd, so, lightData->lh, &posers[lightData->lh]); + } +#endif + +#endif + return 0; + } + + case POSERDATA_SYNC: { + PoserDataLight *l = (PoserDataLight *)pd; + int lhid = l->lh; + + //you can get sweepaxis and sweeplh. + + if( dd->ptsweep ) + { + int i; + int lhid = dd->lhid; + int pts = dd->ptsweep; + const SurvivePose * object_pose = &so->OutPose; + + FLT avg_err[3] = { 0, 0, 0 }; + FLT avgtot = 0.0; + for( i = 0; i < pts; i++ ) + { + FLT * nrm = dd->normal_at_errors[pts]; + FLT qty = quantity_errors[pts]; + avgtot += qty; + avg_err[0] = avg_err[0] + nrm[0] * qty; + avg_err[1] = avg_err[1] + nrm[1] * qty; + avg_err[2] = avg_err[2] + nrm[2] * qty; + } + scale3d(avg_err, avg_err, 1./pts); + //We have "Average error" now. This is a world space value. + //This can correct for lateral error, but not distance from camera. + + //Next we need to find out what the weighting is to determine "zoom" + //How do we do this? ??? Too tired to math. + FLT weight = 0.0; + for( i = 0; i < pts; i++ ) + { + //??!?!? Sturfff + } + + dd->ptsweep = 0; + + //Update PoserData_poser_raw_pose_func(pd, so, lhid, &object_pose_out); + } + + dd->nextaxis = l->acode & 1; + printf( "SYNC %d %p\n", l->acode, dd ); + break; + } + case POSERDATA_FULL_SCENE: { + //return opencv_solver_fullscene(so, (PoserDataFullScene *)(pd)); + } + } + return -1; +} + +REGISTER_LINKTIME(PoserCharlesRefine); -- cgit v1.2.3 From 3298e29594c0655e92afca195e6ef13582e5f017 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Tue, 3 Apr 2018 21:56:50 -0400 Subject: The reuslts are incredibly stable. Need to test on the opi. --- src/poser_charlesrefine.c | 366 +++++++++++++++++++++++++--------------------- 1 file changed, 201 insertions(+), 165 deletions(-) diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 4d44722..e6f6a57 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -9,6 +9,7 @@ #include #include #include +#include #define MAX_PT_PER_SWEEP 32 @@ -17,8 +18,10 @@ typedef struct { int sweepaxis; int sweeplh; - FLT normal_at_errors[MAX_PT_PER_SWEEP][3]; - FLT quantity_errors[MAX_PT_PER_SWEEP] + FLT normal_at_errors[MAX_PT_PER_SWEEP][3]; //Value is actually normalized, not just normal to sweep plane. + FLT quantity_errors[MAX_PT_PER_SWEEP]; + FLT angles_at_pts[MAX_PT_PER_SWEEP]; + SurvivePose object_pose_at_hit[MAX_PT_PER_SWEEP]; uint8_t sensor_ids[MAX_PT_PER_SWEEP]; int ptsweep; } CharlesPoserData; @@ -51,7 +54,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { dd->sweeplh = lhid; //FOR NOW, drop LH1. - if( lhid == 1 ) break; + //if( lhid == 1 ) break; // const FLT * sensor_normal = &so->sensor_normals[senid*3]; @@ -60,7 +63,11 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { const FLT * sensor_inpos = &so->sensor_locations[senid*3]; FLT sensor_position_worldspace[3]; + //XXX Once I saw this get pretty wild (When in playback) + //I had to invert the values of sensor_inpos. Not sure why. ApplyPoseToPoint(sensor_position_worldspace, object_pose, sensor_inpos); + + //printf( "%f %f %f == > %f %f %f\n", sensor_inpos[0], sensor_inpos[1], sensor_inpos[2], sensor_position_worldspace[0], sensor_position_worldspace[1], sensor_position_worldspace[2] ); // = sensor position, relative to lighthouse center. FLT sensorpos_rel_lh[3]; @@ -96,200 +103,229 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { if( (i = dd->ptsweep) < MAX_PT_PER_SWEEP ) { - dd->normal_at_errors[i] = sweep_normal; + memcpy( dd->normal_at_errors[i], sweep_normal, sizeof(FLT)*3 ); dd->quantity_errors[i] = dist; + dd->angles_at_pts[i] = angle; dd->sensor_ids[i] = sensor_id; - dd->ptsweep++: + memcpy( &dd->object_pose_at_hit[i], object_pose, sizeof(SurvivePose) ); + dd->ptsweep++; } -#if 0 - printf( "D %d %d: %f [%f %f %f]\n", lhid, axis, dist, sweep_normal[0], sweep_normal[1], sweep_normal[2] ); + return 0; + } + case POSERDATA_SYNC: { + PoserDataLight *l = (PoserDataLight *)pd; + int lhid = l->lh; - //Naieve approach... Push it in the right direction - SurvivePose object_pose_out; - quatcopy( object_pose_out.Rot, object_pose->Rot ); - scale3d(sweep_normal, sweep_normal, -0.1*dist); - add3d(object_pose_out.Pos, sweep_normal, object_pose->Pos); - if( so->PoseConfidence < .01 ) + //you can get sweepaxis and sweeplh. + if( dd->ptsweep ) { - dd->average_nudge[0] = 0; - dd->average_nudge[1] = 0; - dd->average_nudge[2] = 0; - - memcpy( &object_pose_out, &LinmathPose_Identity, sizeof( LinmathPose_Identity ) ); - object_pose_out.Pos[1] = 2.5; - object_pose_out.Pos[2] = 1.8; - so->PoseConfidence = 1.0; - } -// printf( "%f %f %f %f\n", object_pose->Rot[0], object_pose->Rot[1], object_pose->Rot[2], object_pose->Rot[3] ); - - PoserData_poser_raw_pose_func(pd, so, lhid, &object_pose_out); -#endif - -#if 0 -// = { - axis?0.0:sin(angle), - axis?sin(angle):0.0, - cos(angle) }; - - = sensor_locations; - LinmathPoint3d - int8_t sensor_ct; // sensor count - FLT *sensor_locations; // size is sensor_ct*3. Contains x,y,z values for each sensor - FLT *sensor_normals; // size is nrlocations*3. cointains normal vector for each sensor - - - // This is the quat equivalent of 'pout = pose * pin' if pose were a 4x4 matrix in homogenous space -void ApplyPoseToPoint(LinmathPoint3d pout, const LinmathPose *pose, const LinmathPoint3d pin); + int i; + int lhid = dd->sweeplh; + int axis = dd->sweepaxis; + int pts = dd->ptsweep; + const SurvivePose * object_pose = &so->OutPose; //XXX TODO Should pull pose from approximate time when LHs were scanning it. + BaseStationData * bsd = &so->ctx->bsd[lhid]; + SurvivePose * lh_pose = &bsd->Pose; + int validpoints = 0; + int ptvalid[MAX_PT_PER_SWEEP]; + FLT avgerr = 0.0; + FLT vec_correct[3] = { 0., 0. , 0. }; + FLT avgang = 0.0; - SurvivePose obj2world; -// ApplyPoseToPose(&obj2world, &lh2world, &objpose); - memcpy( &obj2world, &LinmathPose_Identity, sizeof( obj2world ) ); - obj2world.Pos[1] = 1; - PoserData_poser_raw_pose_func(pd, so, lhid, &obj2world); -// PoserData_poser_raw_pose_func(pd, so, ld->lh, &posers[lightData->lh]); +//Tunable parameters: +#define MIN_HIT_QUALITY 0.5 //Determines which hits to cull. +#define HIT_QUALITY_BASELINE 0.0001 //Determines which hits to cull. Actually SQRT(baseline) if 0.0001, it is really 1cm +#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.2 //Explodes if you exceed 1.0 +#define CORRECT_TELESCOPTION_COEFFICIENT 1.0 //Converges even as high as 10.0 and doesn't explode. +#define CORRECT_ROTATION_COEFFICIENT 5.0 //This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. +#define ROTATIONAL_CORRECTION_MAXFORCE 0.10 - // Pose Information, also "poser" field. - ///from SurviveObject - // FLT PoseConfidence; // 0..1 - // SurvivePose OutPose; // Final pose? (some day, one can dream!) - // SurvivePose FromLHPose[NUM_LIGHTHOUSES]; // Filled out by poser, contains computed position from each lighthouse. - // void *PoserData; // Initialized to zero, configured by poser, can be anything the poser wants. - // PoserCB PoserFn; + //Step 1: Determine standard of deviation, and average in order to + // drop points that are likely in error. + { + //Calculate average + FLT avgerr_orig = 0.0; + FLT stddevsq = 0.0; + for( i = 0; i < pts; i++ ) + avgerr_orig += dd->quantity_errors[i]; + avgerr_orig/=pts; + + //Calculate standard of deviation. + for( i = 0; i < pts; i++ ) + { + FLT diff = dd->quantity_errors[i]-avgerr_orig; + stddevsq += diff*diff; + } + stddevsq/=pts; + + for( i = 0; i < pts; i++ ) + { + FLT err = dd->quantity_errors[i]; + FLT diff = err-avgerr_orig; + diff *= diff; + int isptvalid = (diff * MIN_HIT_QUALITY <= stddevsq + HIT_QUALITY_BASELINE)?1:0; + ptvalid[i] = isptvalid; + if( isptvalid ) + { + avgang += dd->angles_at_pts[i]; + avgerr += err; + validpoints ++; + } + } + avgang /= validpoints; + avgerr /= validpoints; + } + //Step 2: Determine average lateral error. + //We can actually always perform this operation. Even with only one point. + { + FLT avg_err[3] = { 0, 0, 0 }; //Positional error. + for( i = 0; i < pts; i++ ) + { + if( !ptvalid[i] ) continue; + FLT * nrm = dd->normal_at_errors[i]; + FLT err = dd->quantity_errors[i]; + avg_err[0] = avg_err[0] + nrm[0] * err; + avg_err[1] = avg_err[1] + nrm[1] * err; + avg_err[2] = avg_err[2] + nrm[2] * err; + } - // printf( "%d %d %f\n", ld->sensor_id, ld->lh, ld->angle ); + //NOTE: The "avg_err" is not geometrically centered. This is actually + //probably okay, since if you have sevearl data points to one side, you + //can probably trust that more. + scale3d(avg_err, avg_err, 1./validpoints); -/* - SurvivePose *object_pose, void *user); + //We have "Average error" now. A vector in worldspace. + //This can correct for lateral error, but not distance from camera. -typedef struct -{ - PoserType pt; - poser_raw_pose_func rawposeproc; - poser_lighthouse_pose_func lighthouseposeproc; - void *userdata; -} PoserData; - - PoserData hdr; - int sensor_id; - int acode; //OOTX Code associated with this sweep. bit 1 indicates vertical(1) or horizontal(0) sweep - int lh; //Lighthouse making this sweep - uint32_t timecode; //In object-local ticks. - FLT length; //In seconds - FLT angle; //In radians from center of lighthouse. -} PoserDataLight; - - - -*/ -#if 0 - SurvivePose posers[2]; - int meas[2] = {0, 0}; - for (int lh = 0; lh < so->ctx->activeLighthouses; lh++) { - if (so->ctx->bsd[lh].PositionSet) { - epnp pnp = {.fu = 1, .fv = 1}; - epnp_set_maximum_number_of_correspondences(&pnp, so->sensor_ct); - - add_correspondences(so, &pnp, scene, lightData->timecode, lh); - static int required_meas = -1; - if (required_meas == -1) - required_meas = survive_configi(so->ctx, "epnp-required-meas", SC_GET, 4); - - if (pnp.number_of_correspondences > required_meas) { - - SurvivePose objInLh = solve_correspondence(so, &pnp, false); - if (quatmagnitude(objInLh.Rot) != 0) { - SurvivePose *lh2world = &so->ctx->bsd[lh].Pose; - - SurvivePose txPose = {.Rot = {1}}; - ApplyPoseToPose(&txPose, lh2world, &objInLh); - posers[lh] = txPose; - meas[lh] = pnp.number_of_correspondences; - } - } - - epnp_dtor(&pnp); + //XXX TODO: Should we check to see if we only have one or + //two points to make sure the error on this isn't unusually high? + //If calculated error is unexpectedly high, then we should probably + //Not apply the transform. + scale3d( avg_err, avg_err, -CORRECT_LATERAL_POSITION_COEFFICIENT ); + add3d( vec_correct, vec_correct, avg_err ); } - } - if (meas[0] > 0 && meas[1] > 0) { - SurvivePose interpolate = {0}; - bool winnerTakesAll = true; // Not convinced slerp does the right thing, will change this when i am - - if (winnerTakesAll) { - int winner = meas[0] > meas[1] ? 0 : 1; - PoserData_poser_raw_pose_func(pd, so, winner, &posers[winner]); - } else { - double a, b; - a = meas[0] * meas[0]; - b = meas[1] * meas[1]; - - double t = a + b; - for (size_t i = 0; i < 3; i++) { - interpolate.Pos[i] = (posers[0].Pos[i] * a + posers[1].Pos[i] * b) / (t); + //Step 3: Control telecoption from lighthouse. + // we need to find out what the weighting is to determine "zoom" + if( validpoints > 1 ) //Can't correct "zoom" with only one point. + { + FLT zoom = 0.0; + FLT rmsang = 0.0; + for( i = 0; i < pts; i++ ) + { + if( !ptvalid[i] ) continue; + FLT delang = dd->angles_at_pts[i] - avgang; + FLT delerr = dd->quantity_errors[i] - avgerr; + if( axis ) delang *= -1; //Flip sign on alternate axis because it's measured backwards. + zoom += delerr * delang; + rmsang += delang * delang; } - quatslerp(interpolate.Rot, posers[0].Rot, posers[1].Rot, b / (t)); - PoserData_poser_raw_pose_func(pd, so, lightData->lh, &interpolate); - } - } else { - if (meas[lightData->lh]) - PoserData_poser_raw_pose_func(pd, so, lightData->lh, &posers[lightData->lh]); - } -#endif -#endif - return 0; - } + //Control into or outof lighthouse. + //XXX Check to see if we need to sqrt( the rmsang), need to check convergance behavior close to lighthouse. + //This is a questionable step. + zoom /= sqrt(rmsang); - case POSERDATA_SYNC: { - PoserDataLight *l = (PoserDataLight *)pd; - int lhid = l->lh; + zoom *= CORRECT_TELESCOPTION_COEFFICIENT; - //you can get sweepaxis and sweeplh. + FLT veccamalong[3]; + sub3d( veccamalong, lh_pose->Pos, object_pose->Pos ); + normalize3d( veccamalong, veccamalong ); + scale3d( veccamalong, veccamalong, zoom ); + add3d( vec_correct, veccamalong, vec_correct ); + } - if( dd->ptsweep ) - { - int i; - int lhid = dd->lhid; - int pts = dd->ptsweep; - const SurvivePose * object_pose = &so->OutPose; - FLT avg_err[3] = { 0, 0, 0 }; - FLT avgtot = 0.0; - for( i = 0; i < pts; i++ ) - { - FLT * nrm = dd->normal_at_errors[pts]; - FLT qty = quantity_errors[pts]; - avgtot += qty; - avg_err[0] = avg_err[0] + nrm[0] * qty; - avg_err[1] = avg_err[1] + nrm[1] * qty; - avg_err[2] = avg_err[2] + nrm[2] * qty; + SurvivePose object_pose_out; + add3d(object_pose_out.Pos, vec_correct, object_pose->Pos); + + quatcopy( object_pose_out.Rot, object_pose->Rot ); + + //Stage 4: "Tug" on the rotation of the object, from all of the sensor's pov. + //If we were able to determine likliehood of a hit in the sweep instead of afterward + //we would actually be able to perform this on a per-hit basis. + if( 1 ) { + LinmathQuat correction; + quatcopy( correction, LinmathQuat_Identity ); + for( i = 0; i < pts; i++ ) + { + if( !ptvalid[i] ) continue; + FLT dist = dd->quantity_errors[i]-avgerr; + FLT angle = dd->angles_at_pts[i]; + int sensor_id = dd->sensor_ids[i]; + FLT * normal = dd->normal_at_errors[i]; + const SurvivePose * object_pose_at_hit = &dd->object_pose_at_hit[i]; + const FLT * sensor_inpos = &so->sensor_locations[sensor_id*3]; + + LinmathQuat world_to_object_space; + quatgetreciprocal(world_to_object_space, object_pose_at_hit->Rot); + FLT correction_in_object_space[3]; //The amount across the surface of the object the rotation should happen. + + quatrotatevector(correction_in_object_space, world_to_object_space, normal ); + dist *= CORRECT_ROTATION_COEFFICIENT; + if( dist > ROTATIONAL_CORRECTION_MAXFORCE ) dist = ROTATIONAL_CORRECTION_MAXFORCE; + if( dist <-ROTATIONAL_CORRECTION_MAXFORCE ) dist =-ROTATIONAL_CORRECTION_MAXFORCE; + + //Now, we have a "tug" vector in object-local space. Need to apply the torque. + FLT vector_from_center_of_object[3]; + normalize3d( vector_from_center_of_object, sensor_inpos ); + //scale3d(vector_from_center_of_object, sensor_inpos, 10.0 ); + // vector_from_center_of_object[2]*=-1; + // vector_from_center_of_object[1]*=-1; +// vector_from_center_of_object[0]*=-1; + //vector_from_center_of_object + scale3d(vector_from_center_of_object,vector_from_center_of_object, 1); + + + FLT new_vector_in_object_space[3]; + //printf( "%f %f %f %f\n", object_pose_at_hit->Rot[0], object_pose_at_hit->Rot[1], object_pose_at_hit->Rot[2], object_pose_at_hit->Rot[3] ); + //printf( "%f %f %f // %f %f %f // %f\n", vector_from_center_of_object[0], vector_from_center_of_object[1], vector_from_center_of_object[2], correction_in_object_space[0], correction_in_object_space[1], correction_in_object_space[2], dist ); + scale3d( correction_in_object_space, correction_in_object_space, -dist ); + add3d( new_vector_in_object_space, vector_from_center_of_object, correction_in_object_space ); + + normalize3d( new_vector_in_object_space, new_vector_in_object_space ); + + LinmathQuat corrective_quaternion; + quatfrom2vectors(corrective_quaternion, vector_from_center_of_object, new_vector_in_object_space ); + quatrotateabout( correction, correction, corrective_quaternion ); + //printf( "%f -> %f %f %f => %f %f %f [%f %f %f %f]\n", dist, vector_from_center_of_object[0], vector_from_center_of_object[1], vector_from_center_of_object[2], + //correction_in_object_space[0], correction_in_object_space[1], correction_in_object_space[2], + //corrective_quaternion[0],corrective_quaternion[1],corrective_quaternion[1],corrective_quaternion[3]); + } + printf( "Applying: %f %f %f %f\n", correction[0], correction[1], correction[2], correction[3] ); + //Apply our corrective quaternion to the output. + quatrotateabout( object_pose_out.Rot, object_pose_out.Rot, correction ); + quatnormalize( object_pose_out.Rot, object_pose_out.Rot ); } - scale3d(avg_err, avg_err, 1./pts); - //We have "Average error" now. This is a world space value. - //This can correct for lateral error, but not distance from camera. - - //Next we need to find out what the weighting is to determine "zoom" - //How do we do this? ??? Too tired to math. - FLT weight = 0.0; - for( i = 0; i < pts; i++ ) + + //Janky need to do this somewhere else... This initializes the pose estimator. + if( so->PoseConfidence < .01 ) { - //??!?!? Sturfff + memcpy( &object_pose_out, &LinmathPose_Identity, sizeof( LinmathPose_Identity ) ); + object_pose_out.Pos[0] = -0.14372776; + object_pose_out.Pos[1] = 0.06856518; + object_pose_out.Pos[2] = 0.01960009; + object_pose_out.Rot[0] = 1.0; + object_pose_out.Rot[1] = -0.0; + object_pose_out.Rot[2] = 0.0; + object_pose_out.Rot[3] = 0.0; + so->PoseConfidence = 1.0; } - dd->ptsweep = 0; + PoserData_poser_raw_pose_func(pd, so, lhid, &object_pose_out); - //Update PoserData_poser_raw_pose_func(pd, so, lhid, &object_pose_out); + dd->ptsweep = 0; } - dd->nextaxis = l->acode & 1; - printf( "SYNC %d %p\n", l->acode, dd ); + dd->sweepaxis = l->acode & 1; + //printf( "SYNC %d %p\n", l->acode, dd ); break; } case POSERDATA_FULL_SCENE: { -- cgit v1.2.3 From 438ae07af28fbf6f63b7598bc2de21a61222a989 Mon Sep 17 00:00:00 2001 From: Charles Lohr Date: Wed, 4 Apr 2018 03:10:21 +0000 Subject: Ok, it works! --- src/poser_charlesrefine.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index e6f6a57..61bd575 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -141,9 +141,9 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { #define MIN_HIT_QUALITY 0.5 //Determines which hits to cull. #define HIT_QUALITY_BASELINE 0.0001 //Determines which hits to cull. Actually SQRT(baseline) if 0.0001, it is really 1cm -#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.2 //Explodes if you exceed 1.0 -#define CORRECT_TELESCOPTION_COEFFICIENT 1.0 //Converges even as high as 10.0 and doesn't explode. -#define CORRECT_ROTATION_COEFFICIENT 5.0 //This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. +#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.1 //Explodes if you exceed 1.0 +#define CORRECT_TELESCOPTION_COEFFICIENT 0.5 //Converges even as high as 10.0 and doesn't explode. +#define CORRECT_ROTATION_COEFFICIENT 1.0 //This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. #define ROTATIONAL_CORRECTION_MAXFORCE 0.10 //Step 1: Determine standard of deviation, and average in order to @@ -299,7 +299,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { //correction_in_object_space[0], correction_in_object_space[1], correction_in_object_space[2], //corrective_quaternion[0],corrective_quaternion[1],corrective_quaternion[1],corrective_quaternion[3]); } - printf( "Applying: %f %f %f %f\n", correction[0], correction[1], correction[2], correction[3] ); + //printf( "Applying: %f %f %f %f\n", correction[0], correction[1], correction[2], correction[3] ); //Apply our corrective quaternion to the output. quatrotateabout( object_pose_out.Rot, object_pose_out.Rot, correction ); quatnormalize( object_pose_out.Rot, object_pose_out.Rot ); -- cgit v1.2.3 From bd7afcf2d1cdc49fc0bf7593cba12d576b227461 Mon Sep 17 00:00:00 2001 From: Charles Lohr Date: Wed, 4 Apr 2018 03:34:25 +0000 Subject: Add gyro support (Still need madgwick's) --- src/poser_charlesrefine.c | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 61bd575..9aa44d9 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -37,6 +37,21 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { case POSERDATA_IMU: { // Really should use this... PoserDataIMU *imuData = (PoserDataIMU *)pd; + + + //TODO: Actually do Madgwick's algorithm + LinmathQuat applymotion; + const SurvivePose * object_pose = &so->OutPose; + imuData->gyro[0] *= -0.001; + imuData->gyro[1] *= -0.001; + imuData->gyro[2] *= 0.001; + quatfromeuler( applymotion, imuData->gyro ); + //printf( "%f %f %f\n", imuData->gyro [0], imuData->gyro [1], imuData->gyro [2] ); + SurvivePose object_pose_out; + quatrotateabout(object_pose_out.Rot, object_pose->Rot, applymotion ); + copy3d( object_pose_out.Pos, object_pose->Pos ); + PoserData_poser_raw_pose_func(pd, so, 0, &object_pose_out); + return 0; } case POSERDATA_LIGHT: { -- cgit v1.2.3 From 3ee1f6d7e9c2e2318dddc34495e6c6af25be3ffe Mon Sep 17 00:00:00 2001 From: Charles Lohr Date: Wed, 4 Apr 2018 04:06:31 +0000 Subject: Tweak to paraemters to fix performance of gyro --- src/poser_charlesrefine.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 9aa44d9..c1cc6de 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -42,9 +42,9 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { //TODO: Actually do Madgwick's algorithm LinmathQuat applymotion; const SurvivePose * object_pose = &so->OutPose; - imuData->gyro[0] *= -0.001; - imuData->gyro[1] *= -0.001; - imuData->gyro[2] *= 0.001; + imuData->gyro[0] *= -0.0005; + imuData->gyro[1] *= -0.0005; + imuData->gyro[2] *= 0.0005; quatfromeuler( applymotion, imuData->gyro ); //printf( "%f %f %f\n", imuData->gyro [0], imuData->gyro [1], imuData->gyro [2] ); SurvivePose object_pose_out; @@ -156,8 +156,8 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { #define MIN_HIT_QUALITY 0.5 //Determines which hits to cull. #define HIT_QUALITY_BASELINE 0.0001 //Determines which hits to cull. Actually SQRT(baseline) if 0.0001, it is really 1cm -#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.1 //Explodes if you exceed 1.0 -#define CORRECT_TELESCOPTION_COEFFICIENT 0.5 //Converges even as high as 10.0 and doesn't explode. +#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.8 //Explodes if you exceed 1.0 +#define CORRECT_TELESCOPTION_COEFFICIENT 8.0 //Converges even as high as 10.0 and doesn't explode. #define CORRECT_ROTATION_COEFFICIENT 1.0 //This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. #define ROTATIONAL_CORRECTION_MAXFORCE 0.10 -- cgit v1.2.3 From 736443dca52d6800224db4d0646b9617f79a219b Mon Sep 17 00:00:00 2001 From: Charles Lohr Date: Wed, 4 Apr 2018 04:33:01 +0000 Subject: Fix Merge --- src/poser_charlesrefine.c | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index c1cc6de..372ccb1 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -1,4 +1,4 @@ -//EXPERIMENTAL DRIVER - DO NOT USE +//Driver works, but you _must_ start it near the origin looking in +Z. #include #include @@ -50,7 +50,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { SurvivePose object_pose_out; quatrotateabout(object_pose_out.Rot, object_pose->Rot, applymotion ); copy3d( object_pose_out.Pos, object_pose->Pos ); - PoserData_poser_raw_pose_func(pd, so, 0, &object_pose_out); + PoserData_poser_pose_func(pd, so, &object_pose_out); return 0; } @@ -324,17 +324,10 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { if( so->PoseConfidence < .01 ) { memcpy( &object_pose_out, &LinmathPose_Identity, sizeof( LinmathPose_Identity ) ); - object_pose_out.Pos[0] = -0.14372776; - object_pose_out.Pos[1] = 0.06856518; - object_pose_out.Pos[2] = 0.01960009; - object_pose_out.Rot[0] = 1.0; - object_pose_out.Rot[1] = -0.0; - object_pose_out.Rot[2] = 0.0; - object_pose_out.Rot[3] = 0.0; so->PoseConfidence = 1.0; } - PoserData_poser_raw_pose_func(pd, so, lhid, &object_pose_out); + PoserData_poser_pose_func(pd, so, &object_pose_out); dd->ptsweep = 0; } -- cgit v1.2.3 From 779a7466efc0e0657e7048108be35645faede030 Mon Sep 17 00:00:00 2001 From: Charles Lohr Date: Wed, 4 Apr 2018 04:35:12 +0000 Subject: Fix merge conflicts --- src/poser_charlesrefine.c | 19 ------------------- 1 file changed, 19 deletions(-) diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 266248c..f9c60e2 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -50,11 +50,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { SurvivePose object_pose_out; quatrotateabout(object_pose_out.Rot, object_pose->Rot, applymotion ); copy3d( object_pose_out.Pos, object_pose->Pos ); -<<<<<<< HEAD PoserData_poser_pose_func(pd, so, &object_pose_out); -======= - PoserData_poser_raw_pose_func(pd, so, 0, &object_pose_out); ->>>>>>> 22301a092b4009c4f2ca2bc8be4511d75e3082b6 return 0; } @@ -328,25 +324,10 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { if( so->PoseConfidence < .01 ) { memcpy( &object_pose_out, &LinmathPose_Identity, sizeof( LinmathPose_Identity ) ); -<<<<<<< HEAD so->PoseConfidence = 1.0; } PoserData_poser_pose_func(pd, so, &object_pose_out); -======= - object_pose_out.Pos[0] = -0.14372776; - object_pose_out.Pos[1] = 0.06856518; - object_pose_out.Pos[2] = 0.01960009; - object_pose_out.Rot[0] = 1.0; - object_pose_out.Rot[1] = -0.0; - object_pose_out.Rot[2] = 0.0; - object_pose_out.Rot[3] = 0.0; - so->PoseConfidence = 1.0; - } - - PoserData_poser_raw_pose_func(pd, so, lhid, &object_pose_out); ->>>>>>> 22301a092b4009c4f2ca2bc8be4511d75e3082b6 - dd->ptsweep = 0; } -- cgit v1.2.3