diff options
29 files changed, 1120 insertions, 246 deletions
@@ -40,7 +40,7 @@ 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 src/survive_imu.o src/poser_imu.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 src/survive_imu.o src/poser_imu.o #If you want to use HIDAPI on Linux. #CFLAGS:=$(CFLAGS) -DHIDAPI @@ -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/bindings/cs/Demo/Program.cs b/bindings/cs/Demo/Program.cs index c03d83c..c838a4e 100644 --- a/bindings/cs/Demo/Program.cs +++ b/bindings/cs/Demo/Program.cs @@ -7,50 +7,19 @@ 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(); - } + LibSurViveAPI api = LibSurViveAPI.Instance; - public MyHandler() : base() - { - } - - public MyHandler(string[] args) : base(args) - { + var so = api.GetSurviveObjectByName("HMD"); + } - protected void LightHouseEvent1(IntPtr ctx, byte lighthouse, SurvivePose lighthouse_pose, SurvivePose object_pose) + public static void HMDUpdate(int ObjectID, Vector3 pos) { - 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) - { - MyHandler handler = new MyHandler(args); - - while (handler.Poll() == 0) { - } } diff --git a/bindings/cs/libsurvive.net/LibSurViveAPI.cs b/bindings/cs/libsurvive.net/LibSurViveAPI.cs new file mode 100644 index 0000000..1fb8cae --- /dev/null +++ b/bindings/cs/libsurvive.net/LibSurViveAPI.cs @@ -0,0 +1,349 @@ +using System.Collections; +using System.Collections.Generic; + +using libsurvive; +using System; +using System.Threading; +using System.Runtime.InteropServices; + +public class LibSurViveAPI +{ + private static LibSurViveAPI _instance; + public static LibSurViveAPI Instance + { + get + { + if (_instance == null) + { + _instance = new LibSurViveAPI(); + } + + return _instance; + } + } + + 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 = delegate { }; + + public delegate void ErrorLog(string message); + ErrorLog LogError = delegate { }; + + LibSurViveAPI() + { + CreateContext(); + + CreateTread(); + } + + ~LibSurViveAPI() + { + running = false; + } + + private void CreateTread() + { + internalPollTread = new Thread(InternalPoll); + internalPollTread.Start(); + } + + void InternalPoll() + { + while (running) + { + int code = Cfunctions.Survive_poll(context); + + if (code != 0) + { + running = false; + } + } + } + + internal void CreateContext() + { + LogInfo("Start Init"); + + SetupConfigs configs = new SetupConfigs + { + configFile = "survive_conf.json" + }; + + 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); + + LogInfo("Finished Init"); + + LogInfo("Start Startup"); + + int a = 0; + try + { + a = Cfunctions.Survive_startup(context); + } + catch (Exception) + { + throw; + } + + if (a != 0) + { + throw new Exception("Error in startup"); + } + + LogInfo("Finished Startup"); + + } + + + static public string[] CreateStartParameters(SetupConfigs configs) + { + List<string> args = new List<string> + { + "unity" + }; + + if (configs.playbackFile != "" && configs.playbackFile != null) + { + args.AddRange(new[] { "--playback", configs.playbackFile }); + 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) + { + LogInfo(fault); + } + + virtual protected void ErrorEvent(IntPtr ctx, string 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); + } + + 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); + + //Debug.Log("PoseEvent"); + } + + /* + public delegate void PoseUpdate(SurviveVector3 pos, SurviveQuaternion quat); + + public Dictionary<string, PoseUpdate> updates = new Dictionary<string, PoseUpdate>(); + + public void AddCalback(string ID, PoseUpdate update) + { + if (!updates.ContainsKey(ID)) + { + updates.Add(ID, update); + } + else + { + updates[ID] += update; + } + } + */ + + + 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 struct SetupConfigs +{ + public string playbackFile; + 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; + public SurvivePose Pose + { + get + { + var ptr1 = Cfunctions.Survive_object_pose(ptr); + return (SurvivePose)Marshal.PtrToStructure(ptr1, typeof(SurvivePose)); + } + } + + 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 obj_ptr) + { + if (obj_ptr == IntPtr.Zero) + { + throw new Exception("Can't create SurviveObject with 0 pointer"); + } + ptr = obj_ptr; + } +} + +public class SurviveVector3 +{ + double x; + double y; + double z; +} + +public class SurviveQuaternion +{ + 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 bafcf2b..b1eb711 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 @@ -35,11 +37,27 @@ namespace libsurvive public SurviveContext(string[] args) { + Init(args); + } + + public SurviveContext(string replaydata) + { + string[] vs = new[] { "--playback", "P:/c/libsurvive-data/lightcap-reformat/lightcap-reformat.log", "--disambiguator", "StateBased", "--calibrate" }; + 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<int,PoseUpdate> PoseUpdateCallbacks = new Dictionary<int, PoseUpdate>(); + 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..d003976 100644 --- a/bindings/cs/libsurvive.net/cfunctions.cs +++ b/bindings/cs/libsurvive.net/cfunctions.cs @@ -12,80 +12,137 @@ 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 + public double[] Rot; // Quaternion in the form wxyz } - class cfunctions + 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); + + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_codename")] + 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(SurviveObjectPtr so); + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_charge")] + 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(SurviveObjectPtr so); + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_pose")] + public static extern IntPtr Survive_object_pose(SurviveObjectPtr so); + + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_sensor_locations")] + public static extern double[] Survive_object_sensor_locations(SurviveObjectPtr so); + + [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall, EntryPoint = "survive_object_sensor_normals")] + 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 } + + [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 @@ <Project Sdk="Microsoft.NET.Sdk"> <PropertyGroup> - <TargetFramework>netstandard1.6</TargetFramework> + <TargetFramework>netstandard2.0</TargetFramework> </PropertyGroup> </Project> 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..4cddf89 100644 --- a/include/libsurvive/poser.h +++ b/include/libsurvive/poser.h @@ -18,21 +18,48 @@ 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; -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 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_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. + * + * 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 { diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 7248b1c..65343b7 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,12 +115,20 @@ struct SurviveObject { haptic_func haptic; SurviveSensorActivations activations; + void *user_ptr; // Debug int tsl; }; // 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); @@ -199,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; @@ -232,7 +241,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); @@ -261,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); @@ -271,7 +280,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. @@ -303,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 160adda..367c391 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); diff --git a/redist/linmath.h b/redist/linmath.h index 76e1322..e268e96 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -7,6 +7,12 @@ 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 +70,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, - const LinmathQuat b); // same as quat multiply, not piecewise multiply. -void quatscale(LinmathQuat qout, const LinmathQuat qin, FLT s); +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. +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 } diff --git a/simple_pose_test.c b/simple_pose_test.c index 0772abb..06a9901 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,11 +163,12 @@ 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 ); +#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 ) diff --git a/src/poser.c b/src/poser.c index 9a0de24..499e0ff 100644 --- a/src/poser.c +++ b/src/poser.c @@ -7,11 +7,29 @@ #define _USE_MATH_DEFINES // for C #include <math.h> -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_charlesrefine.c b/src/poser_charlesrefine.c new file mode 100644 index 0000000..388ba77 --- /dev/null +++ b/src/poser_charlesrefine.c @@ -0,0 +1,342 @@ +// Driver works, but you _must_ start it near the origin looking in +Z. + +#include <poser.h> +#include <survive.h> +#include <survive_reproject.h> + +#include "epnp/epnp.h" +#include "linmath.h" +#include <math.h> +#include <stdint.h> +#include <stdio.h> +#include <string.h> + +#define MAX_PT_PER_SWEEP 32 + +typedef struct { + int sweepaxis; + int sweeplh; + 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; + +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; + + // TODO: Actually do Madgwick's algorithm + LinmathQuat applymotion; + const SurvivePose *object_pose = &so->OutPose; + 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; + quatrotateabout(object_pose_out.Rot, object_pose->Rot, applymotion); + copy3d(object_pose_out.Pos, object_pose->Pos); + PoserData_poser_pose_func(pd, so, &object_pose_out); + + 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]; + // 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]; + 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) { + 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; + memcpy(&dd->object_pose_at_hit[i], object_pose, sizeof(SurvivePose)); + dd->ptsweep++; + } + + 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->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; + +// 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.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 + + // 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; + } + + // 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); + + // We have "Average error" now. A vector in worldspace. + // This can correct for lateral error, but not distance from camera. + + // 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); + } + + // 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; + } + + // 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); + + zoom *= CORRECT_TELESCOPTION_COEFFICIENT; + + FLT veccamalong[3]; + sub3d(veccamalong, lh_pose->Pos, object_pose->Pos); + normalize3d(veccamalong, veccamalong); + scale3d(veccamalong, veccamalong, zoom); + add3d(vec_correct, veccamalong, vec_correct); + } + + 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); + } + + // Janky need to do this somewhere else... This initializes the pose estimator. + if (so->PoseConfidence < .01) { + memcpy(&object_pose_out, &LinmathPose_Identity, sizeof(LinmathPose_Identity)); + so->PoseConfidence = 1.0; + } + + PoserData_poser_pose_func(pd, so, &object_pose_out); + dd->ptsweep = 0; + } + + dd->sweepaxis = 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); diff --git a/src/poser_charlesslow.c b/src/poser_charlesslow.c index b44225e..5cac973 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. 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_imu.c b/src/poser_imu.c index 02ff8e9..7615170 100644 --- a/src/poser_imu.c +++ b/src/poser_imu.c @@ -20,7 +20,7 @@ int PoserIMU(SurviveObject *so, PoserData *pd) { survive_imu_tracker_integrate(so, dd, imu); - PoserData_poser_raw_pose_func(pd, so, -1, &dd->pose); + PoserData_poser_pose_func(pd, so, &dd->pose); // if(magnitude3d(dd->pose.Pos) > 1) // SV_ERROR("IMU drift"); diff --git a/src/poser_sba.c b/src/poser_sba.c index f6b1131..f0d5645 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -133,7 +133,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; @@ -224,7 +224,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; @@ -282,7 +282,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); } { @@ -456,8 +456,8 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { PoserDataIMU * imu = (PoserDataIMU*)pd; if (ctx->calptr && ctx->calptr->stage < 5) { } else if(d->useIMU){ - survive_imu_tracker_integrate(so, &d->tracker, imu); - PoserData_poser_raw_pose_func(pd, so, 1, &d->tracker.pose); + survive_imu_tracker_integrate(so, &d->tracker, imu); + PoserData_poser_pose_func(pd, so, &d->tracker.pose); } } // INTENTIONAL FALLTHROUGH default: { diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 4628207..b2eed7f 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1631,9 +1631,8 @@ 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) - { - so->ctx->rawposeproc(so, lh, &pose); + if (so->ctx->poseproc) { + so->ctx->poseproc(so, lh, &pose); } if (ttDebug) printf("!\n"); diff --git a/src/survive.c b/src/survive.c index 1a782a2..b024bbb 100644 --- a/src/survive.c +++ b/src/survive.c @@ -216,7 +216,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); @@ -380,11 +380,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) { @@ -558,9 +558,16 @@ 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 diff --git a/src/survive_process.c b/src/survive_process.c index e05e809..abde02d 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, @@ -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/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); } 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 diff --git a/winbuild/libsurvive/libsurvive.vcxproj b/winbuild/libsurvive/libsurvive.vcxproj index 5caa159..4b76c99 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj +++ b/winbuild/libsurvive/libsurvive.vcxproj @@ -91,7 +91,8 @@ <GenerateMapFile>true</GenerateMapFile> <MapFileName>$(IntDir)Test.txt</MapFileName> <MapExports>true</MapExports> - <ModuleDefinitionFile>$(MSBuildProjectDirectory)\libsurvive.def</ModuleDefinitionFile> + <ModuleDefinitionFile> + </ModuleDefinitionFile> </Link> <ProjectReference /> <Lib> @@ -213,7 +214,6 @@ <ClInclude Include="..\..\src\survive_internal.h" /> </ItemGroup> <ItemGroup> - <None Include="libsurvive.def" /> <None Include="packages.config"> <SubType>Designer</SubType> </None> 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 @@ </ClInclude> </ItemGroup> <ItemGroup> - <None Include="libsurvive.def" /> <None Include="packages.config" /> </ItemGroup> </Project>
\ No newline at end of file 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 @@ </ClCompile> <Link> <SubSystem>Console</SubSystem> - <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;libblas.lib;liblapacke.lib;liblapack.lib;%(AdditionalDependencies)</AdditionalDependencies> + <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)</AdditionalDependencies> <GenerateDebugInformation>true</GenerateDebugInformation> <LinkTimeCodeGeneration>UseFastLinkTimeCodeGeneration</LinkTimeCodeGeneration> </Link> |