aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorJustin Berger <j.david.berger@gmail.com>2018-04-01 07:52:00 -0600
committerJustin Berger <j.david.berger@gmail.com>2018-04-01 15:37:48 -0600
commit22d40fb360fdb65da7916fb87f9b199f4f401f05 (patch)
tree5a916c5c6f7bbb1fb32dc31e5f8a475de90a6838
parent558429f4d01462edb606fe51d592e9f9e3889425 (diff)
downloadlibsurvive-22d40fb360fdb65da7916fb87f9b199f4f401f05.tar.gz
libsurvive-22d40fb360fdb65da7916fb87f9b199f4f401f05.tar.bz2
overhal to calibration
-rw-r--r--include/libsurvive/survive.h308
-rw-r--r--include/libsurvive/survive_reproject.h35
-rw-r--r--src/poser_epnp.c10
-rw-r--r--src/poser_sba.c30
-rw-r--r--src/survive.c2
-rwxr-xr-xsrc/survive_cal.c26
-rw-r--r--src/survive_config.c22
-rw-r--r--src/survive_process.c9
-rw-r--r--src/survive_reproject.c129
-rw-r--r--src/survive_sensor_activations.c3
-rw-r--r--tools/findoptimalconfig/Makefile3
-rw-r--r--tools/findoptimalconfig/findoptimalconfig.cc160
12 files changed, 496 insertions, 241 deletions
diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index ed43ddc..e435752 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -10,11 +10,10 @@
extern "C" {
#endif
-
#ifdef _WIN32
#define SURVIVE_EXPORT __declspec(dllexport)
#else
-#define SURVIVE_EXPORT
+#define SURVIVE_EXPORT __attribute__((visibility("default")))
#endif
/**
@@ -32,6 +31,7 @@ typedef struct {
struct PoserDataLight;
struct PoserDataIMU;
+
/**
* Adds a lightData packet to the table.
*/
@@ -53,69 +53,68 @@ bool SurviveSensorActivations_isPairValid(const SurviveSensorActivations *self,
*/
extern uint32_t SurviveSensorActivations_default_tolerance;
-//DANGER: This structure may be redefined. Note that it is logically split into 64-bit chunks
-//for optimization on 32- and 64-bit systems.
+// DANGER: This structure may be redefined. Note that it is logically split into 64-bit chunks
+// for optimization on 32- and 64-bit systems.
-struct SurviveObject
-{
- SurviveContext * ctx;
+struct SurviveObject {
+ SurviveContext *ctx;
- char codename[4]; //3 letters, null-terminated. Currently HMD, WM0, WM1.
- char drivername[4]; //3 letters for driver. Currently "HTC"
- void *driver;
+ char codename[4]; // 3 letters, null-terminated. Currently HMD, WM0, WM1.
+ char drivername[4]; // 3 letters for driver. Currently "HTC"
+ void *driver;
int32_t buttonmask;
int16_t axis1;
int16_t axis2;
int16_t axis3;
- int8_t charge;
- int8_t charging:1;
- int8_t ison:1;
- int8_t additional_flags:6;
-
- //Pose Information, also "poser" field.
- 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.
+ int8_t charge;
+ int8_t charging : 1;
+ int8_t ison : 1;
+ int8_t additional_flags : 6;
+
+ // Pose Information, also "poser" field.
+ 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;
- //Device-specific information about the location of the sensors. This data will be used by the poser.
- 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
-
- //Timing sensitive data (mostly for disambiguation)
- int32_t timebase_hz; //48,000,000 for normal vive hardware. (checked)
- int32_t timecenter_ticks; //200,000 for normal vive hardware. (checked) (This doubles-up as 2x this = full sweep length)
- int32_t pulsedist_max_ticks; //500,000 for normal vive hardware. (guessed)
- int32_t pulselength_min_sync; //2,200 for normal vive hardware. (guessed)
- int32_t pulse_in_clear_time; //35,000 for normal vive hardware. (guessed)
- int32_t pulse_max_for_sweep; //1,800 for normal vive hardware. (guessed)
- int32_t pulse_synctime_offset; //20,000 for normal vive hardware. (guessed)
- int32_t pulse_synctime_slack; //5,000 for normal vive hardware. (guessed)
-
- //Flood info, for calculating which laser is currently sweeping.
- void * disambiguator_data;
- int8_t oldcode;
- int8_t sync_set_number; //0 = master, 1 = slave, -1 = fault.
- int8_t did_handle_ootx; //If unset, will send lightcap data for sync pulses next time a sensor is hit.
+ // Device-specific information about the location of the sensors. This data will be used by the poser.
+ 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
+
+ // Timing sensitive data (mostly for disambiguation)
+ int32_t timebase_hz; // 48,000,000 for normal vive hardware. (checked)
+ int32_t timecenter_ticks; // 200,000 for normal vive hardware. (checked) (This doubles-up as 2x this = full
+ // sweep length)
+ int32_t pulsedist_max_ticks; // 500,000 for normal vive hardware. (guessed)
+ int32_t pulselength_min_sync; // 2,200 for normal vive hardware. (guessed)
+ int32_t pulse_in_clear_time; // 35,000 for normal vive hardware. (guessed)
+ int32_t pulse_max_for_sweep; // 1,800 for normal vive hardware. (guessed)
+ int32_t pulse_synctime_offset; // 20,000 for normal vive hardware. (guessed)
+ int32_t pulse_synctime_slack; // 5,000 for normal vive hardware. (guessed)
+
+ // Flood info, for calculating which laser is currently sweeping.
+ void *disambiguator_data;
+ int8_t oldcode;
+ int8_t sync_set_number; // 0 = master, 1 = slave, -1 = fault.
+ int8_t did_handle_ootx; // If unset, will send lightcap data for sync pulses next time a sensor is hit.
uint32_t last_sync_time[NUM_LIGHTHOUSES];
uint32_t last_sync_length[NUM_LIGHTHOUSES];
uint32_t recent_sync_time;
- uint32_t last_lighttime; //May be a 24- or 32- bit number depending on what device.
-
+ uint32_t last_lighttime; // May be a 24- or 32- bit number depending on what device.
- FLT* acc_bias; // size is FLT*3. contains x,y,z
- FLT* acc_scale; // size is FLT*3. contains x,y,z
- FLT* gyro_bias; // size is FLT*3. contains x,y,z
- FLT* gyro_scale; // size is FLT*3. contains x,y,z
+ FLT *acc_bias; // size is FLT*3. contains x,y,z
+ FLT *acc_scale; // size is FLT*3. contains x,y,z
+ FLT *gyro_bias; // size is FLT*3. contains x,y,z
+ FLT *gyro_scale; // size is FLT*3. contains x,y,z
haptic_func haptic;
SurviveSensorActivations activations;
- //Debug
+ // Debug
int tsl;
};
@@ -125,34 +124,37 @@ 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);
-struct BaseStationData
-{
- uint8_t PositionSet:1;
+typedef struct BaseStationCal {
+ FLT phase[2];
+ FLT tilt[2];
+ FLT curve[2];
+ FLT gibpha[2];
+ FLT gibmag[2];
+} BaseStationCal;
+
+struct BaseStationData {
+ uint8_t PositionSet : 1;
SurvivePose Pose;
- uint8_t OOTXSet:1;
+ uint8_t OOTXSet : 1;
uint32_t BaseStationID;
- FLT fcalphase[2];
- FLT fcaltilt[2];
- FLT fcalcurve[2];
- FLT fcalgibpha[2];
- FLT fcalgibmag[2];
- int8_t accel[3]; //"Up" vector
+
+ BaseStationCal fcal;
+
+ int8_t accel[3]; //"Up" vector
+ uint8_t mode;
};
struct config_group;
#define BUTTON_QUEUE_MAX_LEN 32
-
-
// note: buttonId and axisId are 1-indexed values.
// a value of 0 for an id means that no data is present in that value
// additionally, when x and y values are both present in axis data,
// axis1 will be x, axis2 will be y.
-typedef struct
-{
- uint8_t isPopulated; //probably can remove this given the semaphore in the parent struct. helps with debugging
+typedef struct {
+ uint8_t isPopulated; // probably can remove this given the semaphore in the parent struct. helps with debugging
uint8_t eventType;
uint8_t buttonId;
uint8_t axis1Id;
@@ -162,20 +164,28 @@ typedef struct
SurviveObject *so;
} ButtonQueueEntry;
-typedef struct
-{
- uint8_t nextReadIndex; //init to 0
+typedef struct {
+ uint8_t nextReadIndex; // init to 0
uint8_t nextWriteIndex; // init to 0
- void* buttonservicesem;
+ void *buttonservicesem;
ButtonQueueEntry entry[BUTTON_QUEUE_MAX_LEN];
} ButtonQueue;
typedef enum { SURVIVE_STOPPED = 0, SURVIVE_RUNNING, SURVIVE_CLOSING, SURVIVE_STATE_MAX } SurviveState;
struct SurviveRecordingData;
+struct survive_calibration_config;
+
+enum SurviveCalFlag {
+ SVCal_None = 0,
+ SVCal_Phase = 1,
+ SVCal_Tilt = 2,
+ SVCal_Curve = 4,
+ SVCal_Gib = 8,
+ SVCal_All = SVCal_Gib | SVCal_Curve | SVCal_Tilt | SVCal_Phase
+};
-struct SurviveContext
-{
+struct SurviveContext {
text_feedback_func faultfunction;
text_feedback_func notefunction;
light_process_func lightproc;
@@ -187,35 +197,39 @@ struct SurviveContext
htc_config_func configfunction;
handle_lightcap_func lightcapfunction;
- struct config_group* global_config_values;
- struct config_group* lh_config; //lighthouse configs
- struct config_group* temporary_config_values; //Set per-session, from command-line. Not saved but override global_config_values
+ struct config_group *global_config_values;
+ struct config_group *lh_config; // lighthouse configs
+ struct config_group
+ *temporary_config_values; // Set per-session, from command-line. Not saved but override global_config_values
- //Calibration data:
+ // Calibration data:
int activeLighthouses;
BaseStationData bsd[NUM_LIGHTHOUSES];
- SurviveCalData * calptr; //If and only if the calibration subsystem is attached.
+ SurviveCalData *calptr; // If and only if the calibration subsystem is attached.
struct SurviveRecordingData *recptr; // Iff recording is attached
- SurviveObject ** objs;
+ SurviveObject **objs;
int objs_ct;
- void ** drivers;
- DeviceDriverCb * driverpolls;
- DeviceDriverCb * drivercloses;
- DeviceDriverMagicCb * drivermagics;
+ void **drivers;
+ DeviceDriverCb *driverpolls;
+ DeviceDriverCb *drivercloses;
+ DeviceDriverMagicCb *drivermagics;
int driver_ct;
SurviveState state;
- void* buttonservicethread;
+ void *buttonservicethread;
ButtonQueue buttonQueue;
void *user_ptr;
+ enum SurviveCalFlag calibration_flag;
+ struct survive_calibration_config *calibration_config;
};
-void survive_verify_FLT_size(uint32_t user_size); // Baked in size of FLT to verify users of the library have the correct setting.
+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 );
+SURVIVE_EXPORT SurviveContext *survive_init_internal(int argc, char *const *argv);
/**
* Call survive_init to get a populated SurviveContext pointer.
@@ -227,98 +241,106 @@ SURVIVE_EXPORT SurviveContext * survive_init_internal( int argc, char * const *
* Note that this function _can_ return null based on command line arguments,
* notably if -h was passed in.
*/
-static inline SurviveContext * survive_init( int argc, char * const * argv )
-{
+static inline SurviveContext *survive_init(int argc, char *const *argv) {
survive_verify_FLT_size(sizeof(FLT));
return survive_init_internal(argc, argv);
}
-
-//For any of these, you may pass in 0 for the function pointer to use default behavior.
-//In general unless you are doing wacky things like recording or playing back data, you won't need to use this.
-SURVIVE_EXPORT void survive_install_htc_config_fn( SurviveContext *ctx, htc_config_func fbp );
-SURVIVE_EXPORT void survive_install_info_fn( SurviveContext * ctx, text_feedback_func fbp );
-SURVIVE_EXPORT void survive_install_error_fn( SurviveContext * ctx, text_feedback_func fbp );
-SURVIVE_EXPORT void survive_install_light_fn( SurviveContext * ctx, light_process_func fbp );
-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);
+// For any of these, you may pass in 0 for the function pointer to use default behavior.
+// In general unless you are doing wacky things like recording or playing back data, you won't need to use this.
+SURVIVE_EXPORT void survive_install_htc_config_fn(SurviveContext *ctx, htc_config_func fbp);
+SURVIVE_EXPORT void survive_install_info_fn(SurviveContext *ctx, text_feedback_func fbp);
+SURVIVE_EXPORT void survive_install_error_fn(SurviveContext *ctx, text_feedback_func fbp);
+SURVIVE_EXPORT void survive_install_light_fn(SurviveContext *ctx, light_process_func fbp);
+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_lighthouse_pose_fn(SurviveContext *ctx, lighthouse_pose_func fbp);
-SURVIVE_EXPORT int survive_startup( SurviveContext * ctx );
-SURVIVE_EXPORT int survive_poll( SurviveContext * ctx );
-SURVIVE_EXPORT void survive_close( SurviveContext * ctx );
-
-SURVIVE_EXPORT SurviveObject * survive_get_so_by_name( SurviveContext * ctx, const char * name );
+SURVIVE_EXPORT int survive_startup(SurviveContext *ctx);
+SURVIVE_EXPORT int survive_poll(SurviveContext *ctx);
+SURVIVE_EXPORT void survive_close(SurviveContext *ctx);
-//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 SurviveObject *survive_get_so_by_name(SurviveContext *ctx, const char *name);
-//These functions search both the stored-general and temporary sections for a parameter and return it.
-#define SC_GET 0 //Get, only.
-#define SC_SET 1 //Set, if not present
-#define SC_OVERRIDE 2 //Set, to new default value.
-#define SC_SETCONFIG 4 //Set, both in-memory and config file. Use in conjunction with SC_OVERRIDE.
+// 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 FLT survive_configf( SurviveContext * ctx, const char *tag, char flags, FLT def );
-SURVIVE_EXPORT uint32_t survive_configi( SurviveContext * ctx, const char *tag, char flags, uint32_t def );
-SURVIVE_EXPORT const char * survive_configs( SurviveContext * ctx, const char *tag, char flags, const char *def );
+// These functions search both the stored-general and temporary sections for a parameter and return it.
+#define SC_GET 0 // Get, only.
+#define SC_SET 1 // Set, if not present
+#define SC_OVERRIDE 2 // Set, to new default value.
+#define SC_SETCONFIG 4 // Set, both in-memory and config file. Use in conjunction with SC_OVERRIDE.
+SURVIVE_EXPORT FLT survive_configf(SurviveContext *ctx, const char *tag, char flags, FLT def);
+SURVIVE_EXPORT uint32_t survive_configi(SurviveContext *ctx, const char *tag, char flags, uint32_t def);
+SURVIVE_EXPORT const char *survive_configs(SurviveContext *ctx, const char *tag, char flags, const char *def);
-//Install the calibrator.
-SURVIVE_EXPORT void survive_cal_install( SurviveContext * ctx ); //XXX This will be removed if not already done so.
+// Install the calibrator.
+SURVIVE_EXPORT void survive_cal_install(SurviveContext *ctx); // XXX This will be removed if not already done so.
// Read back a human-readable string description of the calibration status
-SURVIVE_EXPORT int survive_cal_get_status( SurviveContext * ctx, char * description, int description_length );
+SURVIVE_EXPORT int survive_cal_get_status(SurviveContext *ctx, char *description, int description_length);
// Induce haptic feedback
-SURVIVE_EXPORT int survive_haptic(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh, uint16_t pulseLow, uint16_t repeatCount);
-
-//Call these from your callback if overridden.
-//Accept higher-level data.
-SURVIVE_EXPORT void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , uint32_t lh);
-SURVIVE_EXPORT void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id );
-SURVIVE_EXPORT void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh );
-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 int survive_haptic(SurviveObject *so, uint8_t reserved, uint16_t pulseHigh, uint16_t pulseLow,
+ uint16_t repeatCount);
+
+// Call these from your callback if overridden.
+// Accept higher-level data.
+SURVIVE_EXPORT void survive_default_light_process(SurviveObject *so, int sensor_id, int acode, int timeinsweep,
+ uint32_t timecode, uint32_t length, uint32_t lh);
+SURVIVE_EXPORT void survive_default_imu_process(SurviveObject *so, int mode, FLT *accelgyro, uint32_t timecode, int id);
+SURVIVE_EXPORT void survive_default_angle_process(SurviveObject *so, int sensor_id, int acode, uint32_t timecode,
+ FLT length, FLT angle, uint32_t lh);
+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_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);
-
-
+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);
////////////////////// Survive Drivers ////////////////////////////
-void RegisterDriver(const char * name, void * data);
+void RegisterDriver(const char *name, void *data);
#ifdef _MSC_VER
-#define REGISTER_LINKTIME( func ) \
- __pragma(comment(linker,"/export:REGISTER"#func));\
+#define REGISTER_LINKTIME(func) \
+ __pragma(comment(linker, "/export:REGISTER" #func)); \
void REGISTER##func() { RegisterDriver(#func, &func); }
#else
-#define REGISTER_LINKTIME( func ) \
+#define REGISTER_LINKTIME(func) \
void __attribute__((constructor)) REGISTER##func() { RegisterDriver(#func, &func); }
#endif
-
-
///////////////////////// General stuff for writing drivers ///////
-//For device drivers to call. This actually attaches them.
-int survive_add_object( SurviveContext * ctx, SurviveObject * obj );
-void survive_add_driver( SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic );
-
-//This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
-void handle_lightcap( SurviveObject * so, LightcapElement * le );
-
-#define SV_INFO( ... ) { char stbuff[1024]; sprintf( stbuff, __VA_ARGS__ ); ctx->notefunction( ctx, stbuff ); }
-#define SV_ERROR( ... ) { char stbuff[1024]; sprintf( stbuff, __VA_ARGS__ ); ctx->faultfunction( ctx, stbuff ); }
-#define SV_KILL() exit(0) //XXX This should likely be re-defined.
+// For device drivers to call. This actually attaches them.
+int survive_add_object(SurviveContext *ctx, SurviveObject *obj);
+void survive_add_driver(SurviveContext *ctx, void *payload, DeviceDriverCb poll, DeviceDriverCb close,
+ DeviceDriverMagicCb magic);
+
+// This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
+void handle_lightcap(SurviveObject *so, LightcapElement *le);
+
+#define SV_INFO(...) \
+ { \
+ char stbuff[1024]; \
+ sprintf(stbuff, __VA_ARGS__); \
+ ctx->notefunction(ctx, stbuff); \
+ }
+#define SV_ERROR(...) \
+ { \
+ char stbuff[1024]; \
+ sprintf(stbuff, __VA_ARGS__); \
+ ctx->faultfunction(ctx, stbuff); \
+ }
+#define SV_KILL() exit(0) // XXX This should likely be re-defined.
#ifdef __cplusplus
};
#endif
#endif
-
diff --git a/include/libsurvive/survive_reproject.h b/include/libsurvive/survive_reproject.h
index 961db2a..c90d39c 100644
--- a/include/libsurvive/survive_reproject.h
+++ b/include/libsurvive/survive_reproject.h
@@ -1,6 +1,8 @@
#pragma once
+
#include "survive.h"
#include <stdbool.h>
+#include <stdio.h>
#include <stdlib.h>
#ifdef __cplusplus
@@ -13,7 +15,7 @@ typedef struct {
bool swap;
} survive_calibration_options_config;
-typedef struct {
+typedef struct survive_calibration_config {
survive_calibration_options_config phase, tilt, curve, gibMag, gibPhase;
@@ -21,10 +23,10 @@ typedef struct {
} survive_calibration_config;
-void survive_calibration_options_config_apply(
- const survive_calibration_options_config *option, const FLT *input,
- FLT *output);
-const survive_calibration_config *survive_calibration_default_config();
+void survive_calibration_options_config_apply(const survive_calibration_options_config *option, const FLT *input,
+ FLT *output);
+
+const survive_calibration_config *survive_calibration_default_config(const SurviveContext *ctx);
size_t survive_calibration_config_max_idx();
@@ -32,10 +34,9 @@ survive_calibration_config survive_calibration_config_create_from_idx(size_t v);
size_t survive_calibration_config_index(const survive_calibration_config *config);
-void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d,
- FLT *out);
-void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse,
- const SurvivePose *pose, FLT *point3d,
+void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, FLT *out);
+
+void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, const SurvivePose *pose, FLT *point3d,
FLT *out);
// This is given a lighthouse -- in the same system as stored in BaseStationData, and
@@ -44,9 +45,19 @@ void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse,
// While this is typically opposite of what we want to do -- we want to find the 3d
// position from a 2D coordinate, this is helpful since the minimization of reprojection
// error is a core mechanism to many types of solvers.
-void survive_reproject_from_pose_with_config(
- const SurviveContext *ctx, const survive_calibration_config *config,
- int lighthouse, const SurvivePose *pose, const FLT *point3d, FLT *out);
+void survive_reproject_from_pose_with_config(const SurviveContext *ctx, const survive_calibration_config *config,
+ int lighthouse, const SurvivePose *pose, const FLT *point3d, FLT *out);
+
+void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const survive_calibration_config *config,
+ const SurvivePose *pose, const FLT *point3d, FLT *out);
+
+void survive_reproject_with_config(const SurviveContext *ctx, const survive_calibration_config *config, int lighthouse,
+ const FLT *point3d, FLT *out);
+
+void survive_calibration_config_fprint(FILE *file, const survive_calibration_config *config);
+
+void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, enum SurviveCalFlag f, const FLT *in, FLT *out);
+void survive_apply_bsd_calibration(SurviveContext *ctx, int lh, const FLT *in, FLT *out);
#ifdef __cplusplus
}
diff --git a/src/poser_epnp.c b/src/poser_epnp.c
index 7e86542..c05450a 100644
--- a/src/poser_epnp.c
+++ b/src/poser_epnp.c
@@ -6,6 +6,7 @@
#include <poser.h>
#include <survive.h>
+#include <survive_reproject.h>
#include "epnp/epnp.h"
#include "linmath.h"
@@ -71,7 +72,9 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs)
for (size_t i = 0; i < so->sensor_ct; i++) {
FLT *lengths = pdfs->lengths[i][lh];
- FLT *ang = pdfs->angles[i][lh];
+ FLT *_ang = pdfs->angles[i][lh];
+ FLT ang[2];
+ survive_apply_bsd_calibration(so->ctx, lh, _ang, ang);
if (lengths[0] < 0 || lengths[1] < 0)
continue;
@@ -103,7 +106,10 @@ static void add_correspondences(SurviveObject *so, epnp *pnp, SurviveSensorActiv
for (size_t sensor_idx = 0; sensor_idx < so->sensor_ct; sensor_idx++) {
if (SurviveSensorActivations_isPairValid(scene, SurviveSensorActivations_default_tolerance, timecode,
sensor_idx, lh)) {
- double *angles = scene->angles[sensor_idx][lh];
+ FLT *_angles = scene->angles[sensor_idx][lh];
+ FLT angles[2];
+ survive_apply_bsd_calibration(so->ctx, lh, _angles, angles);
+
epnp_add_correspondence(pnp, so->sensor_locations[sensor_idx * 3 + 0],
so->sensor_locations[sensor_idx * 3 + 1], so->sensor_locations[sensor_idx * 3 + 2],
tan(angles[0]), tan(angles[1]));
diff --git a/src/poser_sba.c b/src/poser_sba.c
index df28a2d..4a4ed8f 100644
--- a/src/poser_sba.c
+++ b/src/poser_sba.c
@@ -40,6 +40,8 @@ typedef struct SBAData {
int successes_to_reset;
int successes_to_reset_cntr;
+ FLT max_error;
+
FLT sensor_variance;
FLT sensor_variance_per_second;
int sensor_time_window;
@@ -71,7 +73,9 @@ static size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs,
continue;
}
- double *angles = pdfs->angles[sensor][lh];
+ double *_angles = pdfs->angles[sensor][lh];
+ double angles[2];
+ survive_apply_bsd_calibration(so->ctx, lh, _angles, angles);
vmask[sensor * NUM_LIGHTHOUSES + lh] = 1;
meas[measCount++] = angles[0];
@@ -89,7 +93,9 @@ static size_t construct_input_from_scene(SBAData *d, PoserDataLight *pdl, Surviv
for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) {
for (size_t lh = 0; lh < 2; lh++) {
if (SurviveSensorActivations_isPairValid(scene, d->sensor_time_window, pdl->timecode, sensor, lh)) {
- double *a = scene->angles[sensor][lh];
+ double *_a = scene->angles[sensor][lh];
+ FLT a[2];
+ survive_apply_bsd_calibration(so->ctx, lh, _a, a);
vmask[sensor * NUM_LIGHTHOUSES + lh] = 1;
if (cov) {
@@ -273,7 +279,7 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o
if (distance > 1.)
status = -1;
}
- if (status > 0) {
+ 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);
@@ -283,7 +289,7 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o
SurviveContext *ctx = so->ctx;
// Docs say info[0] should be divided by meas; I don't buy it really...
static int cnt = 0;
- if (cnt++ > 1000 || meas_size < d->required_meas) {
+ if (cnt++ > 1000 || meas_size < d->required_meas || (info[1] / meas_size * 2) > d->max_error) {
SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (int)meas_size);
SV_INFO("%f cur reproj error", (info[1] / meas_size * 2));
cnt = 0;
@@ -389,11 +395,12 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
d->failures_to_reset_cntr = 0;
d->failures_to_reset = survive_configi(ctx, "sba-failures-to-reset", SC_GET, 1);
d->successes_to_reset_cntr = 0;
- d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, 1);
+ d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, 100);
d->required_meas = survive_configi(ctx, "sba-required-meas", SC_GET, 8);
-
- d->sensor_time_window = survive_configi(ctx, "sba-time-window", SC_GET, 1600000 * 4);
+ d->max_error = survive_configf(ctx, "sba-max-error", SC_GET, .0001);
+ d->sensor_time_window =
+ survive_configi(ctx, "sba-time-window", SC_GET, SurviveSensorActivations_default_tolerance * 2);
d->sensor_variance_per_second = survive_configf(ctx, "sba-sensor-variance-per-sec", SC_GET, 0.001);
d->sensor_variance = survive_configf(ctx, "sba-sensor-variance", SC_GET, 1.0);
d->so = so;
@@ -403,6 +410,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
SV_INFO("\tsba-sensor-variance: %f", d->sensor_variance);
SV_INFO("\tsba-sensor-variance-per-sec: %f", d->sensor_variance_per_second);
SV_INFO("\tsba-time-window: %d", d->sensor_time_window);
+ SV_INFO("\tsba-max-error: %f", d->max_error);
}
SBAData *d = so->PoserData;
switch (pd->pt) {
@@ -416,8 +424,8 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
// only process sweeps
FLT error = -1;
if (d->last_lh != lightData->lh || d->last_acode != lightData->acode) {
- survive_calibration_config config = *survive_calibration_default_config();
- error = run_sba_find_3d_structure(d, config, lightData, scene, 50, .5);
+ survive_calibration_config config = *survive_calibration_default_config(ctx);
+ error = run_sba_find_3d_structure(d, config, lightData, scene, 100, .5);
d->last_lh = lightData->lh;
d->last_acode = lightData->acode;
}
@@ -435,9 +443,9 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
case POSERDATA_FULL_SCENE: {
SurviveContext *ctx = so->ctx;
PoserDataFullScene *pdfs = (PoserDataFullScene *)(pd);
- survive_calibration_config config = *survive_calibration_default_config();
+ survive_calibration_config config = *survive_calibration_default_config(ctx);
SV_INFO("Running sba with %u", (int)survive_calibration_config_index(&config));
- double error = run_sba(config, pdfs, so, 50, .005);
+ double error = run_sba(config, pdfs, so, 100, .005);
// std::cerr << "Average reproj error: " << error << std::endl;
return 0;
}
diff --git a/src/survive.c b/src/survive.c
index a15e0ed..63ad2ba 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -219,6 +219,8 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) {
ctx->configfunction = survive_default_htc_config_process;
ctx->rawposeproc = survive_default_raw_pose_process;
+ ctx->calibration_flag = (enum SurviveCalFlag)survive_configi(ctx, "bsd-cal", SC_GET, SVCal_All);
+
return ctx;
}
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 2fc1896..e094e7b 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -61,19 +61,20 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
//print_lighthouse_info_v6(&v6);
b->BaseStationID = v6.id;
- b->fcalphase[0] = v6.fcal_0_phase;
- b->fcalphase[1] = v6.fcal_1_phase;
- b->fcaltilt[0] = tan(v6.fcal_0_tilt);
- b->fcaltilt[1] = tan(v6.fcal_1_tilt); //XXX??? Is this right? See https://github.com/cnlohr/libsurvive/issues/18
- b->fcalcurve[0] = v6.fcal_0_curve;
- b->fcalcurve[1] = v6.fcal_1_curve;
- b->fcalgibpha[0] = v6.fcal_0_gibphase;
- b->fcalgibpha[1] = v6.fcal_1_gibphase;
- b->fcalgibmag[0] = v6.fcal_0_gibmag;
- b->fcalgibmag[1] = v6.fcal_1_gibmag;
+ b->fcal.phase[0] = v6.fcal_0_phase;
+ b->fcal.phase[1] = v6.fcal_1_phase;
+ b->fcal.tilt[0] = (v6.fcal_0_tilt);
+ b->fcal.tilt[1] = (v6.fcal_1_tilt); // XXX??? Is this right? See https://github.com/cnlohr/libsurvive/issues/18
+ b->fcal.curve[0] = v6.fcal_0_curve;
+ b->fcal.curve[1] = v6.fcal_1_curve;
+ b->fcal.gibpha[0] = v6.fcal_0_gibphase;
+ b->fcal.gibpha[1] = v6.fcal_1_gibphase;
+ b->fcal.gibmag[0] = v6.fcal_0_gibmag;
+ b->fcal.gibmag[1] = v6.fcal_1_gibmag;
b->accel[0] = v6.accel_dir_x;
b->accel[1] = v6.accel_dir_y;
b->accel[2] = v6.accel_dir_z;
+ b->mode = v6.mode_current;
b->OOTXSet = 1;
config_set_lighthouse(ctx->lh_config,b,id);
@@ -617,8 +618,9 @@ static void handle_calibration( struct SurviveCalData *cd )
}
fsd.lengths[i][j][0] = cd->avglens[dataindex+0];
fsd.lengths[i][j][1] = cd->avglens[dataindex+1];
- fsd.angles[i][j][0] = cd->avgsweeps[dataindex+0];
- fsd.angles[i][j][1] = cd->avgsweeps[dataindex+1];
+ // fsd.angles[i][j][0] = cd->avgsweeps[dataindex+0];
+ // fsd.angles[i][j][1] = cd->avgsweeps[dataindex+1];
+ survive_apply_bsd_calibration(ctx, lh, &cd->avgsweeps[dataindex], fsd.angles[i][j]);
fsd.synctimes[i][j] = temp_syncs[i][j];
}
diff --git a/src/survive_config.c b/src/survive_config.c
index 7b68ae4..d67cd8e 100644
--- a/src/survive_config.c
+++ b/src/survive_config.c
@@ -119,12 +119,13 @@ void config_read_lighthouse(config_group *lh_config, BaseStationData *bsd, uint8
FLT defaults[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
bsd->BaseStationID = config_read_uint32(cg, "id", 0);
+ bsd->mode = config_read_uint32(cg, "mode", 0);
config_read_float_array(cg, "pose", &bsd->Pose.Pos[0], defaults, 7);
- config_read_float_array(cg, "fcalphase", bsd->fcalphase, defaults, 2);
- config_read_float_array(cg, "fcaltilt", bsd->fcaltilt, defaults, 2);
- config_read_float_array(cg, "fcalcurve", bsd->fcalcurve, defaults, 2);
- config_read_float_array(cg, "fcalgibpha", bsd->fcalgibpha, defaults, 2);
- config_read_float_array(cg, "fcalgibmag", bsd->fcalgibmag, defaults, 2);
+ config_read_float_array(cg, "fcalphase", bsd->fcal.phase, defaults, 2);
+ config_read_float_array(cg, "fcaltilt", bsd->fcal.tilt, defaults, 2);
+ config_read_float_array(cg, "fcalcurve", bsd->fcal.curve, defaults, 2);
+ config_read_float_array(cg, "fcalgibpha", bsd->fcal.gibpha, defaults, 2);
+ config_read_float_array(cg, "fcalgibmag", bsd->fcal.gibmag, defaults, 2);
bsd->PositionSet = config_read_uint32(cg, "PositionSet", 0);
}
@@ -132,12 +133,13 @@ void config_set_lighthouse(config_group *lh_config, BaseStationData *bsd, uint8_
config_group *cg = lh_config + idx;
config_set_uint32(cg, "index", idx);
config_set_uint32(cg, "id", bsd->BaseStationID);
+ config_set_uint32(cg, "mode", bsd->mode);
config_set_float_a(cg, "pose", &bsd->Pose.Pos[0], 7);
- config_set_float_a(cg, "fcalphase", bsd->fcalphase, 2);
- config_set_float_a(cg, "fcaltilt", bsd->fcaltilt, 2);
- config_set_float_a(cg, "fcalcurve", bsd->fcalcurve, 2);
- config_set_float_a(cg, "fcalgibpha", bsd->fcalgibpha, 2);
- config_set_float_a(cg, "fcalgibmag", bsd->fcalgibmag, 2);
+ config_set_float_a(cg, "fcalphase", bsd->fcal.phase, 2);
+ config_set_float_a(cg, "fcaltilt", bsd->fcal.tilt, 2);
+ config_set_float_a(cg, "fcalcurve", bsd->fcal.curve, 2);
+ config_set_float_a(cg, "fcalgibpha", bsd->fcal.gibpha, 2);
+ config_set_float_a(cg, "fcalgibmag", bsd->fcal.gibmag, 2);
config_set_uint32(cg, "PositionSet", bsd->PositionSet);
}
diff --git a/src/survive_process.c b/src/survive_process.c
index 97fbc46..62459f2 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -51,14 +51,17 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode
static int use_bsd_cal = -1;
if(use_bsd_cal == -1) {
use_bsd_cal = survive_configi(ctx, "use-bsd-cal", SC_GET, 1);
+ if (use_bsd_cal == 0) {
+ SV_INFO("Not using BSD calibration values");
+ }
}
if(use_bsd_cal) {
BaseStationData * bsd = &ctx->bsd[base_station];
- //XXX TODO: This seriously needs to be worked on. See: https://github.com/cnlohr/libsurvive/issues/18
- angle += bsd->fcalphase[axis];
+ // XXX TODO: This seriously needs to be worked on. See: https://github.com/cnlohr/libsurvive/issues/18
+ // angle += (use_bsd_cal == 2 ? -1 : 1) * bsd->fcal.phase[axis];
// angle += bsd->fcaltilt[axis] * predicted_angle(axis1);
-
+
//TODO!!!
}
diff --git a/src/survive_reproject.c b/src/survive_reproject.c
index eabdb07..845f30a 100644
--- a/src/survive_reproject.c
+++ b/src/survive_reproject.c
@@ -1,6 +1,7 @@
#include "survive_reproject.h"
#include <../redist/linmath.h>
#include <math.h>
+#include <stdio.h>
#include <string.h>
static void survive_calibration_options_config_normalize(
@@ -72,9 +73,8 @@ static FLT gibf(bool useSin, FLT v) {
return cos(v);
}
-void survive_reproject_from_pose_with_config(
- const SurviveContext *ctx, const survive_calibration_config *config,
- int lighthouse, const SurvivePose *pose, const FLT *pt, FLT *out) {
+void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const survive_calibration_config *config,
+ const SurvivePose *pose, const FLT *pt, FLT *out) {
LinmathQuat invq;
quatgetreciprocal(invq, pose->Rot);
@@ -92,33 +92,36 @@ void survive_reproject_from_pose_with_config(
double ang_x = atan(x);
double ang_y = atan(y);
- const BaseStationData *bsd = &ctx->bsd[lighthouse];
double phase[2];
- survive_calibration_options_config_apply(&config->phase, bsd->fcalphase,
- phase);
+ survive_calibration_options_config_apply(&config->phase, bsd->fcal.phase, phase);
double tilt[2];
- survive_calibration_options_config_apply(&config->tilt, bsd->fcaltilt,
- tilt);
+ survive_calibration_options_config_apply(&config->tilt, bsd->fcal.tilt, tilt);
double curve[2];
- survive_calibration_options_config_apply(&config->curve, bsd->fcalcurve,
- curve);
+ survive_calibration_options_config_apply(&config->curve, bsd->fcal.curve, curve);
double gibPhase[2];
- survive_calibration_options_config_apply(&config->gibPhase, bsd->fcalgibpha,
- gibPhase);
+ survive_calibration_options_config_apply(&config->gibPhase, bsd->fcal.gibpha, gibPhase);
double gibMag[2];
- survive_calibration_options_config_apply(&config->gibMag, bsd->fcalgibmag,
- gibMag);
+ survive_calibration_options_config_apply(&config->gibMag, bsd->fcal.gibmag, gibMag);
- out[0] = ang_x + phase[0] + tan(tilt[0]) * y + curve[0] * y * y +
+ out[0] = ang_x + phase[0] + (tilt[0]) * ang_y + curve[0] * ang_y * ang_y +
gibf(config->gibUseSin, gibPhase[0] + ang_x) * gibMag[0];
- out[1] = ang_y + phase[1] + tan(tilt[1]) * x + curve[1] * x * x +
+ out[1] = ang_y + phase[1] + (tilt[1]) * ang_x + curve[1] * ang_x * ang_x +
gibf(config->gibUseSin, gibPhase[1] + ang_y) * gibMag[1];
}
+void survive_reproject_from_pose_with_config(const SurviveContext *ctx, const survive_calibration_config *config,
+ int lighthouse, const SurvivePose *pose, const FLT *pt, FLT *out) {
+ const BaseStationData *bsd = &ctx->bsd[lighthouse];
+ survive_reproject_from_pose_with_bsd(bsd, config, pose, pt, out);
+}
+
+void survive_reproject_with_config(const SurviveContext *ctx, const survive_calibration_config *config, int lighthouse,
+ const FLT *point3d, FLT *out) {
+ survive_reproject_from_pose_with_config(ctx, config, lighthouse, &ctx->bsd[lighthouse].Pose, point3d, out);
+}
void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse,
const SurvivePose *pose, FLT *pt, FLT *out) {
- survive_reproject_from_pose_with_config(
- ctx, survive_calibration_default_config(), lighthouse, pose, pt, out);
+ survive_reproject_from_pose_with_config(ctx, survive_calibration_default_config(ctx), lighthouse, pose, pt, out);
}
void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d,
@@ -127,14 +130,15 @@ void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d,
point3d, out);
}
-const survive_calibration_config *survive_calibration_default_config() {
- static survive_calibration_config *def = 0;
- if (def == 0) {
- def = malloc(sizeof(survive_calibration_config));
- memset(def, 0, sizeof(survive_calibration_config));
- *def = survive_calibration_config_create_from_idx(0);
+const survive_calibration_config *survive_calibration_default_config(const SurviveContext *_ctx) {
+ SurviveContext *ctx = (SurviveContext *)_ctx;
+ if (ctx->calibration_config == 0) {
+ size_t idx = survive_configi(ctx, "default-cal-conf", SC_GET, 0);
+ ctx->calibration_config = malloc(sizeof(survive_calibration_config));
+ memset(ctx->calibration_config, 0, sizeof(survive_calibration_config));
+ *ctx->calibration_config = survive_calibration_config_create_from_idx(idx);
}
- return def;
+ return ctx->calibration_config;
}
size_t survive_calibration_config_max_idx() {
@@ -142,3 +146,78 @@ size_t survive_calibration_config_max_idx() {
memset(&cfg, 0x1, sizeof(survive_calibration_config));
return survive_calibration_config_index(&cfg);
}
+
+static void survive_calibration_options_config_fprint(FILE *file, const survive_calibration_options_config *self) {
+ fprintf(file, "\t");
+ if (!self->enable[0] && !self->enable[1]) {
+ fprintf(file, "disabled");
+ return;
+ }
+
+ fprintf(file, "swap: %d\n", self->swap);
+ for (int i = 0; i < 2; i++) {
+ if (self->enable[i]) {
+ fprintf(file, "\tinvert[%d]: %d", i, self->invert[i]);
+ } else {
+ fprintf(file, "\t%d: disabled", i);
+ }
+ }
+}
+
+void survive_calibration_config_fprint(FILE *file, const survive_calibration_config *self) {
+ fprintf(file, "Index: %ld\n", survive_calibration_config_index(self));
+
+ fprintf(file, "Phase: \n");
+ survive_calibration_options_config_fprint(file, &self->phase);
+ fprintf(file, "\n");
+
+ fprintf(file, "Tilt: \n");
+ survive_calibration_options_config_fprint(file, &self->tilt);
+ fprintf(file, "\n");
+
+ fprintf(file, "Curve: \n");
+ survive_calibration_options_config_fprint(file, &self->curve);
+ fprintf(file, "\n");
+
+ fprintf(file, "gibPhase: \n");
+ survive_calibration_options_config_fprint(file, &self->gibPhase);
+ fprintf(file, "\n");
+
+ fprintf(file, "gibMag: \n");
+ survive_calibration_options_config_fprint(file, &self->gibMag);
+ fprintf(file, "\n");
+
+ fprintf(file, "gibUseSin: %d\n", self->gibUseSin);
+}
+
+void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, enum SurviveCalFlag f, const FLT *in,
+ FLT *out) {
+ const BaseStationCal *cal = &ctx->bsd[lh].fcal;
+ out[0] = in[0] + cal->phase[0];
+ out[1] = in[1] + cal->phase[1];
+
+ FLT tilt_scale = 10;
+ FLT curve_scale = 10000;
+ FLT gib_scale = 10;
+ const int iterations = 4;
+ for (int i = 0; i < iterations; i++) {
+ FLT last_out[2] = {out[0], out[1]};
+ bool last_iteration = i == iterations - 1;
+ for (int j = 0; j < 2; j++) {
+ int oj = j == 0 ? 1 : 0;
+ out[j] = in[j];
+ if (!last_iteration || (f & SVCal_Phase))
+ out[j] += (cal->phase[j]);
+ if (!last_iteration || (f & SVCal_Tilt))
+ out[j] += tan(cal->tilt[j] / tilt_scale) * last_out[oj];
+ if (!last_iteration || (f & SVCal_Curve))
+ out[j] += (cal->curve[j] / curve_scale) * last_out[oj] * last_out[oj];
+ if (!last_iteration || (f & SVCal_Gib))
+ out[j] += cos(cal->gibpha[j] + last_out[j]) * cal->gibmag[1] / gib_scale;
+ }
+ }
+}
+
+void survive_apply_bsd_calibration(SurviveContext *ctx, int lh, const FLT *in, FLT *out) {
+ survive_apply_bsd_calibration_by_flag(ctx, lh, ctx->calibration_flag, in, out);
+}
diff --git a/src/survive_sensor_activations.c b/src/survive_sensor_activations.c
index e42b50e..dc5c0d4 100644
--- a/src/survive_sensor_activations.c
+++ b/src/survive_sensor_activations.c
@@ -1,3 +1,4 @@
+#include <math.h>
#include <survive.h>
bool SurviveSensorActivations_isPairValid(const SurviveSensorActivations *self, uint32_t tolerance,
@@ -28,4 +29,4 @@ void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDa
*length = lightData->length * 48000000;
}
-uint32_t SurviveSensorActivations_default_tolerance = (uint32_t)(48000000 /*mhz*/ * (16.7 * 2 /*ms*/) / 1000);
+uint32_t SurviveSensorActivations_default_tolerance = (uint32_t)(48000000 /*mhz*/ * (16.7 * 2 /*ms*/) / 1000) + 5000;
diff --git a/tools/findoptimalconfig/Makefile b/tools/findoptimalconfig/Makefile
index 5e14814..f174812 100644
--- a/tools/findoptimalconfig/Makefile
+++ b/tools/findoptimalconfig/Makefile
@@ -7,7 +7,8 @@ LIBSURVIVE:=$(SRT)/lib/libsurvive.so
CFLAGS:=-I$(SRT)/redist -I$(SRT)/include -O0 -g -DFLT=double -DUSE_DOUBLE #-fsanitize=address -fsanitize=undefined
LDFLAGS:=-lm -lpthread -llapacke -lcblas
-findoptimalconfig : findoptimalconfig.cc $(LIBSURVIVE)
+findoptimalconfig : findoptimalconfig.cc $(LIBSURVIVE)
+ cd ../..;make
g++ $(CFLAGS) -o $@ $^ $(LDFLAGS)
clean :
diff --git a/tools/findoptimalconfig/findoptimalconfig.cc b/tools/findoptimalconfig/findoptimalconfig.cc
index 543958b..874ed84 100644
--- a/tools/findoptimalconfig/findoptimalconfig.cc
+++ b/tools/findoptimalconfig/findoptimalconfig.cc
@@ -9,6 +9,8 @@
#include <vector>
#include <sba/sba.h>
+#include <survive_reproject.h>
+
std::ostream &operator<<(std::ostream &o, const survive_calibration_options_config &self) {
o << "\t";
if (!self.enable[0] && !self.enable[1]) {
@@ -49,7 +51,7 @@ struct SBAData {
FLT sensor_variance = 1.;
FLT sensor_variance_per_second = 0;
- int sensor_time_window = 1600000;
+ int sensor_time_window = SurviveSensorActivations_default_tolerance;
int required_meas = 8;
};
@@ -57,10 +59,12 @@ struct SBAData {
struct PlaybackDataInput {
SurviveObject *so = nullptr;
SurvivePose position;
-
+ uint32_t timestamp;
std::vector<char> vmask;
std::vector<double> meas, cov;
- PlaybackDataInput(SurviveObject *so, const SurvivePose &position) : so(so), position(position) {
+ SurviveSensorActivations activations;
+ PlaybackDataInput(SurviveObject *so, const SurvivePose &position)
+ : so(so), position(position), activations(so->activations) {
int32_t sensor_count = so->sensor_ct;
vmask.resize(sensor_count * NUM_LIGHTHOUSES);
cov.resize(4 * sensor_count * NUM_LIGHTHOUSES);
@@ -74,6 +78,7 @@ struct PlaybackDataInput {
};
struct PlaybackData {
+ SurviveObject *so = nullptr;
BaseStationData bsd[2];
std::vector<PlaybackDataInput> inputs;
};
@@ -116,15 +121,24 @@ void light_process(SurviveObject *so, int sensor_id, int acode, int timeinsweep,
survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode, length, lighthouse);
}
+SurvivePose lastPose = {};
void raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) {
survive_default_raw_pose_process(so, lighthouse, pose);
PlaybackData *d = (PlaybackData *)so->ctx->user_ptr;
+ d->so = so;
d->inputs.emplace_back(so, *pose);
auto &input = d->inputs.back();
+ input.timestamp = timestamp;
int meas = construct_input_from_scene(so, timestamp, &input.vmask.front(), &input.meas.front(), &input.cov.front());
input.shrink(meas / 2);
- if (meas / 2 < 12)
+
+ double dist = 0;
+ if (d->inputs.empty() == false) {
+ dist = dist3d(pose->Pos, lastPose.Pos);
+ }
+ if (meas / 2 < 8 || dist > .00009)
d->inputs.pop_back();
+ lastPose = *pose;
}
void lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose, SurvivePose *obj_pose) {
@@ -166,7 +180,6 @@ double sba_opt(SurviveContext *ctx, const survive_calibration_config &config, Pl
SurviveObject *so = data.so;
SurvivePose soLocation = data.position;
- bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]) != 0;
double opts[SBA_OPTSSZ] = {0};
double info[SBA_INFOSZ] = {0};
@@ -191,6 +204,102 @@ double sba_opt(SurviveContext *ctx, const survive_calibration_config &config, Pl
str_metric_function,
0, // jacobia of metric_func
&_ctx, // user data
+ 100, // Max iterations
+ 0, // verbosity
+ opts, // options
+ info); // info
+
+ int meas_size = data.meas.size() / 2;
+ if (meas_size == 0)
+ return 0;
+
+ {
+ SurviveContext *ctx = so->ctx;
+ // Docs say info[0] should be divided by meas; I don't buy it really...
+ static int cnt = 0;
+ if (cnt++ > 1000) {
+ SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (int)meas_size);
+ SV_INFO("%f cur reproj error", (info[1] / meas_size * 2));
+ cnt = 0;
+ }
+ }
+ assert(!isinf(info[1]));
+ return info[1] / meas_size * 2;
+}
+
+struct optimal_cal_ctx {
+ std::vector<double> sensors;
+ SurviveContext *ctx;
+ survive_calibration_config config;
+};
+
+static void metric_function(int j, int i, double *aj, double *xij, void *adata) {
+ optimal_cal_ctx *ctx = (optimal_cal_ctx *)(adata);
+
+ FLT sensorInWorld[3] = {ctx->sensors[i * 3 + 0], ctx->sensors[i * 3 + 1], ctx->sensors[i * 3 + 2]};
+
+ BaseStationData bsd = ctx->ctx->bsd[j];
+ bsd.fcal = *(BaseStationCal *)aj;
+
+ survive_reproject_from_pose_with_bsd(&bsd, &ctx->config, &ctx->ctx->bsd[j].Pose, sensorInWorld, xij);
+}
+
+double find_optimal_cal(SurviveContext *ctx, const survive_calibration_config &config, PlaybackData &data) {
+ optimal_cal_ctx _ctx;
+ std::vector<char> vmask;
+ std::vector<double> cov, meas;
+ _ctx.ctx = ctx;
+ _ctx.config = config;
+ for (auto &in : data.inputs) {
+ for (size_t sensor = 0; sensor < in.so->sensor_ct; sensor++) {
+ FLT p[3];
+ ApplyPoseToPoint(p, &in.position, &data.so->sensor_locations[sensor * 3]);
+ _ctx.sensors.emplace_back(p[0]);
+ _ctx.sensors.emplace_back(p[1]);
+ _ctx.sensors.emplace_back(p[2]);
+ for (size_t lh = 0; lh < 1; lh++) {
+ auto scene = &in.activations;
+ if (SurviveSensorActivations_isPairValid(scene, settings.sensor_time_window, in.timestamp, sensor,
+ lh)) {
+ double *a = scene->angles[sensor][lh];
+ vmask.emplace_back(1); //[sensor * NUM_LIGHTHOUSES + lh] = 1;
+
+ meas.emplace_back(a[0]);
+ meas.emplace_back(a[1]);
+ } else {
+ vmask.emplace_back(0);
+ }
+ }
+ }
+ }
+
+ double *covx = 0;
+ SurviveObject *so = data.so;
+
+ double opts[SBA_OPTSSZ] = {0};
+ double info[SBA_INFOSZ] = {0};
+
+ BaseStationCal cal[2] = {};
+
+ opts[0] = SBA_INIT_MU;
+ opts[1] = SBA_STOP_THRESH;
+ opts[2] = SBA_STOP_THRESH;
+ opts[3] = SBA_STOP_THRESH;
+ opts[3] = SBA_STOP_THRESH; // max_reproj_error * meas.size();
+ opts[4] = 0.0;
+
+ int status = sba_mot_levmar(data.inputs.size() * so->sensor_ct, // number of 3d points
+ 1, // Number of cameras -- 2 lighthouses
+ 0, // Number of cameras to not modify
+ &vmask[0], // boolean vis mask
+ (double *)cal, // camera parameters
+ 2, // sizeof(BaseStationCal) / sizeof(FLT),
+ &meas[0], // 2d points for 3d objs
+ covx, // covariance of measurement. Null sets to identity
+ 2, // 2 points per image
+ metric_function,
+ 0, // jacobia of metric_func
+ &_ctx, // user data
50, // Max iterations
0, // verbosity
opts, // options
@@ -200,7 +309,7 @@ double sba_opt(SurviveContext *ctx, const survive_calibration_config &config, Pl
} else {
assert(false);
}
- int meas_size = data.meas.size() / 2;
+ int meas_size = _ctx.sensors.size() / 2;
if (meas_size == 0)
return 0;
@@ -215,17 +324,16 @@ double sba_opt(SurviveContext *ctx, const survive_calibration_config &config, Pl
}
}
assert(!isinf(info[1]));
+ std::cerr << "Used " << meas_size << " measurements" << std::endl;
+
+ double *_cal = (double *)cal;
+ for (int i = 0; i < sizeof(BaseStationCal) / sizeof(FLT); i++)
+ std::cerr << _cal[2 * i] << ", " << _cal[2 * i + 1] << " = " << (info[1] / meas_size * 2) << std::endl;
+
return info[1] / meas_size * 2;
}
-
double find_avg_reproj_error(SurviveContext *ctx, const survive_calibration_config &config, PlaybackDataInput &data) {
- auto vmask = &data.vmask.front();
- auto cov = &data.cov.front();
- auto meas = &data.meas.front();
- double err = 0;
- size_t cnt = 0;
-
- err += sba_opt(ctx, config, data);
+ return sba_opt(ctx, config, data);
/*
for (size_t sensor = 0; sensor < data.so->sensor_ct; sensor++) {
for (size_t lh = 0; lh < 2; lh++) {
@@ -246,7 +354,6 @@ double find_avg_reproj_error(SurviveContext *ctx, const survive_calibration_conf
}
}
}*/
- return err;
}
double find_avg_reproj_error(SurviveContext *ctx, const survive_calibration_config &config, PlaybackData &data) {
@@ -259,10 +366,10 @@ double find_avg_reproj_error(SurviveContext *ctx, const survive_calibration_conf
int main(int argc, char **argv) {
std::vector<std::pair<size_t, size_t>> sections = {
- {28, 0}, // phase
- // { 5, 5 }, // tilt
- // { 5, 10 }, // curve
- // { 11, 15 } // gibs + useSin
+ {5, 0}, // phase
+ //{ 5, 5 }, // tilt
+ //{ 5, 10 }, // curve
+ //{ 11, 15 } // gibs + useSin
};
for (int i = 1; i < argc; i++) {
@@ -279,7 +386,9 @@ int main(int argc, char **argv) {
"--defaultposer",
"SBA",
"--sba-required-meas",
- "12",
+ "8",
+ "--sba-max-error",
+ ".1",
"--playback",
argv[i]};
@@ -293,10 +402,19 @@ int main(int argc, char **argv) {
while (survive_poll(ctx) == 0) {
}
+ survive_calibration_config config = {};
+ // config.tilt.enable[0] = config.tilt.enable[1] = 1;
+ // config.curve.enable[0] = config.curve.enable[1] = 1;
+ config.phase.enable[0] = config.phase.enable[1] = 1;
+ // config.gibPhase.enable[0] = config.gibPhase.enable[1] = 1;
+ // config.gibMag.enable[0] = config.gibMag.enable[1] = 1;
+
+ find_optimal_cal(ctx, config, data);
+
for (int j = 0; j < sections.size(); j++) {
auto &range = sections[j];
for (size_t _i = 0; _i < (1 << range.first); _i++) {
- int i = _i << range.second;
+ int i = (_i << range.second);
survive_calibration_config config = survive_calibration_config_create_from_idx(i);
if (i == survive_calibration_config_index(&config)) {
double error = find_avg_reproj_error(ctx, config, data);