libsurvive

Calibration establishes the relationship between lighthouses and tracked objects.

Key Concepts:

• Automatic Calibration: On the first run, libsurvive communicates with lighthouses to determine their positions. This can take up to ten seconds.
• Continuous Integration: The system continuously integrates object data while objects are stationary. Lighthouses may shift slightly during this process until a lock is achieved.
• Persistence: Calibration data is saved to config.json in XDG_CONFIG_HOME/libsurvive.
• Handling Moved Lighthouses: If a calibrated lighthouse is moved, the calibration becomes invalid. You must force a recalibration.

Tasks:

• Force Recalibration: Use the --force-calibrate flag with any libsurvive tool or manually delete the config.json file.
• Large Space Calibration: If you cannot see all lighthouses from one spot, calibrate a few lighthouses first. Then, move a tracked object into the field of view of an uncalibrated lighthouse while keeping it in view of at least one calibrated lighthouse. Set the object down so it remains stationary to allow the remaining lighthouses to calibrate.

# Example of forcing calibration via CLI
./bin/survive-cli --force-calibrate

The Poser provides 6DoF updates to the Kalman filter to assist in bootstrapping and improving precision. It uses two main methods depending on the tracking state:

1. MPFit (Main Solver): Uses the MPFit C library for non-linear least squares via Levenberg-Marquardt optimization. It optimizes in axis-angle space and uses accelerometer data when stationary to establish an 'up' direction. It is used when the system is sufficiently overdetermined.
2. Barycentric SVD (Seed Poser): A fallback solver used during startup or when tracking is lost. It is mathematically similar to the Efficient Perspective-n-Point (EPnP) algorithm but adapted for lighthouse plane equations. It is more expensive to run and less resilient to noise than MPFit.

Initial Estimate Logic: The solver attempts to use the most recent solution as a starting point. If no recent solution exists or it is stale, the Barycentric SVD seed poser is used.

libsurvive follows a layered architecture to transform raw sensor data into stable tracking poses:

1. Driver Layer: Driver plugins (like the Vive driver) poll raw light, configuration, and IMU data via USB.
2. Data Processing: Raw data is disambiguated into angle data, OOTX configuration/calibration data is collected, and stray light/reflections are filtered out using outlier removal based on standard deviation.
3. Poser & Kalman Filter:
   • A Poser (e.g., MPFit or Barycentric SVD) solves for the initial scene and lighthouse positions, then provides 6DoF poses as measurements.
   • An Iterative Extended Kalman Filter (EKF) integrates IMU data, processed light data, and poser outputs to maintain a stable state.
4. Output: Once the Kalman filter's covariance matrix trace is sufficiently low, the system emits the position and velocity of the object via user-registerable callbacks.

To use CNKalman in a C application, you must define three callback functions that describe your system's dynamics and measurement models, then initialize the state and measurement models.

Required Callbacks

1. kalman_transition_model_fn: Defines the next state $x_1$ and the transition matrix $F$ based on a time delta dt.
2. kalman_process_noise_fn: Defines the process covariance matrix $Q$.
3. kalman_measurement_model_fn: Defines the residuals $y$ and the Jacobian of the measurement function $H_k$. This function should return true if the Jacobian and evaluation were valid.

Workflow

1. Initialize the state using cnkalman_state_init.
2. (Optional) Set kalman_state.transition_jacobian_mode to a numerical mode (e.g., cnkalman_jacobian_mode_two_sided) to use numerical Jacobians instead of analytical ones.
3. Initialize a measurement model using cnkalman_meas_model_init.
4. (Optional) Set kalman_meas_model.meas_jacobian_mode for numerical measurement Jacobians.
5. Perform the prediction and update step using cnkalman_meas_model_predict_update.

#include <cnkalman/kalman.h>
#include <stdio.h>

static inline void kalman_transition_model_fn(FLT dt, const struct cnkalman_state_s *k, const struct CnMat *x0, 
struct CnMat *x1, struct CnMat *F) {
    // Logic to fill in the next state x1 and the associated transition matrix F
}

static inline void kalman_process_noise_fn(void *user, FLT dt, const struct CnMat *x, struct CnMat *Q) {
    // Logic to fill in the process covariance Q
}

static inline bool kalman_measurement_model_fn(void *user, const struct CnMat *Z, const struct CnMat *x_t, 
struct CnMat *y, struct CnMat *H_k) {
    // Logic to fill in the residuals `y`, and the jacobian of the predicted measurement function `h`
    return true; 
}

int main() {
    int state_cnt = 1;
    cnkalman_state_t kalman_state = { 0 };
    cnkalman_state_init(&kalman_state, state_cnt, kalman_transition_model_fn, kalman_process_noise_fn, 0, 0);
    
    cnkalman_meas_model_t kalman_meas_model = { 0 };
    cnkalman_meas_model_init(&kalman_state, "Example Measurement", &kalman_meas_model, kalman_measurement_model_fn);
    
    CnMat Z, R;
    // Logic to fill in measurement matrix Z and measurement covariance matrix R
    cnkalman_meas_model_predict_update(1, &kalman_meas_model, 0, &Z, &R);
    
    printf("Output:%f\n", cn_as_vector(&kalman_state.state)[0]);
    return 0;
}