t_tracking.h

Go to the documentation of this file.

// Copyright 2019-2020, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
 * @file
 * @brief  Tracking API interface.
 * @author Pete Black <pblack@collabora.com>
 * @author Jakob Bornecrantz <jakob@collabora.com>
 * @author Ryan Pavlik <ryan.pavlik@collabora.com>
 * @ingroup aux_tracking
 */

#pragma once

#include "xrt/xrt_frame.h"
#include "util/u_misc.h"

#include <stdio.h>


#ifdef __cplusplus
extern "C" {
#endif


/*!
 * @defgroup aux_tracking Tracking
 * @ingroup aux
 * @brief Trackers, filters and associated helper code.
 *
 *
 * ### Coordinate system
 *
 * Right now there is no specific convention on where a tracking systems
 * coordinate system is centered, and is something we probably need to figure
 * out. Right now the stereo based tracking system used by the PSVR and PSMV
 * tracking system is centered on the camera that OpenCV decided is origin.
 *
 * To go a bit further on the PSVR/PSMV case. Think about a idealized start up
 * case, the user is wearing the HMD headset and holding two PSMV controllers.
 * The HMD's coordinate system axis are perfectly parallel with the user
 * coordinate with the user's coordinate system. Where -Z is forward. The user
 * holds the controllers with the ball pointing up and the buttons on the back
 * pointing forward. Which if you read the documentation of @ref psmv_device
 * will that the axis of the PSMV are also perfectly aligned with the users
 * coordinate system. So everything "attached" to the user have it's coordinate
 * system parallel to the user's.
 *
 * The camera on the other hand is looking directly at the user, it's Z-axis and
 * X-axis is flipped in relation to the user's. So to compare what is sees to
 * what the user sees, everything is rotated 180° around the Y-axis.
 */

/*!
 * @dir auxiliary/tracking
 * @ingroup aux
 *
 * @brief Trackers, filters and associated helper code.
 */

/*!
 * @ingroup aux_tracking
 * @{
 */


/*
 *
 * Pre-declare
 *
 */

struct xrt_tracked_psmv;
struct xrt_tracked_psvr;


/*
 *
 * Calibration data.
 *
 */

//! Maximum size of rectilinear distortion coefficient array
#define XRT_DISTORTION_MAX_DIM (5)

/*!
 * @brief Essential calibration data for a single camera, or single lens/sensor
 * of a stereo camera.
 */
struct t_camera_calibration
{
        //! Source image size
        struct xrt_size image_size_pixels;

        //! Camera intrinsics matrix
        double intrinsics[3][3];

        //! Rectilinear distortion coefficients: k1, k2, p1, p2[, k3[, k4, k5,
        //! k6[, s1, s2, s3, s4]]
        double distortion[XRT_DISTORTION_MAX_DIM];

        //! Fisheye camera distortion coefficients
        double distortion_fisheye[4];

        //! Is the camera fisheye?
        bool use_fisheye;
};

/*!
 * Stereo camera calibration data to be given to trackers.
 */
struct t_stereo_camera_calibration
{
        //! Ref counting
        struct xrt_reference reference;

        //! Calibration of individual views/sensor
        struct t_camera_calibration view[2];

        //! Translation from first to second in the stereo pair.
        double camera_translation[3];
        //! Rotation matrix from first to second in the stereo pair.
        double camera_rotation[3][3];

        //! Essential matrix.
        double camera_essential[3][3];
        //! Fundamental matrix.
        double camera_fundamental[3][3];
};

/*!
 * Allocates a new stereo calibration data, unreferences the old @p calib.
 */
void
t_stereo_camera_calibration_alloc(struct t_stereo_camera_calibration **calib);

/*!
 * Only to be called by @p t_stereo_camera_calibration_reference.
 */
void
t_stereo_camera_calibration_destroy(struct t_stereo_camera_calibration *c);

/*!
 * Update the reference counts on a stereo calibration data(s).
 *
 * @param     dst Pointer to a object reference, if the object reference is
 *                non-null will decrement it's counter. The reference that
 *                @p dst points to will be set to @p src.
 * @param[in] src Object to be have it's refcount increased @p dst is set to
 *                this.
 */
static inline void
t_stereo_camera_calibration_reference(struct t_stereo_camera_calibration **dst,
                                      struct t_stereo_camera_calibration *src)
{
        struct t_stereo_camera_calibration *old_dst = *dst;

        if (old_dst == src) {
                return;
        }

        if (src) {
                xrt_reference_inc(&src->reference);
        }

        *dst = src;

        if (old_dst) {
                if (xrt_reference_dec(&old_dst->reference)) {
                        t_stereo_camera_calibration_destroy(old_dst);
                }
        }
}

/*!
 * Load stereo calibration data from a given file.
 */
bool
t_stereo_camera_calibration_load_v1(
    FILE *calib_file, struct t_stereo_camera_calibration **out_data);

/*!
 * Load a stereo calibration struct from a hardcoded place.
 */
bool
t_stereo_camera_calibration_load_v1_hack(
    struct t_stereo_camera_calibration **out_data);

/*!
 * Save raw calibration data to file, hack until prober has storage for such
 * things.
 */
bool
t_file_save_raw_data_hack(struct t_stereo_camera_calibration *data);


/*
 *
 * Conversion functions.
 *
 */

struct t_convert_table
{
        uint8_t v[256][256][256][3];
};

void
t_convert_fill_table(struct t_convert_table *t);

void
t_convert_make_y8u8v8_to_r8g8b8(struct t_convert_table *t);

void
t_convert_make_y8u8v8_to_h8s8v8(struct t_convert_table *t);

void
t_convert_make_h8s8v8_to_r8g8b8(struct t_convert_table *t);

void
t_convert_in_place_y8u8v8_to_r8g8b8(uint32_t width,
                                    uint32_t height,
                                    size_t stride,
                                    void *data_ptr);

void
t_convert_in_place_y8u8v8_to_h8s8v8(uint32_t width,
                                    uint32_t height,
                                    size_t stride,
                                    void *data_ptr);

void
t_convert_in_place_h8s8v8_to_r8g8b8(uint32_t width,
                                    uint32_t height,
                                    size_t stride,
                                    void *data_ptr);


/*
 *
 * Filter functions.
 *
 */

#define T_HSV_SIZE 32
#define T_HSV_STEP (256 / T_HSV_SIZE)

#define T_HSV_DEFAULT_PARAMS()                                                 \
        {                                                                      \
                {                                                              \
                    {165, 30, 160, 100},                                       \
                    {135, 30, 160, 100},                                       \
                    {95, 30, 160, 100},                                        \
                },                                                             \
                    {128, 80},                                                 \
        }

struct t_hsv_filter_color
{
        uint8_t hue_min;
        uint8_t hue_range;

        uint8_t s_min;

        uint8_t v_min;
};

struct t_hsv_filter_params
{
        struct t_hsv_filter_color color[3];

        struct
        {
                uint8_t s_max;
                uint8_t v_min;
        } white;
};

struct t_hsv_filter_large_table
{
        uint8_t v[256][256][256];
};

struct t_hsv_filter_optimized_table
{
        uint8_t v[T_HSV_SIZE][T_HSV_SIZE][T_HSV_SIZE];
};

void
t_hsv_build_convert_table(struct t_hsv_filter_params *params,
                          struct t_convert_table *t);

void
t_hsv_build_large_table(struct t_hsv_filter_params *params,
                        struct t_hsv_filter_large_table *t);

void
t_hsv_build_optimized_table(struct t_hsv_filter_params *params,
                            struct t_hsv_filter_optimized_table *t);

static inline uint8_t
t_hsv_filter_sample(struct t_hsv_filter_optimized_table *t,
                    uint32_t y,
                    uint32_t u,
                    uint32_t v)
{
        return t->v[y / T_HSV_STEP][u / T_HSV_STEP][v / T_HSV_STEP];
}

int
t_hsv_filter_create(struct xrt_frame_context *xfctx,
                    struct t_hsv_filter_params *params,
                    struct xrt_frame_sink *sinks[4],
                    struct xrt_frame_sink **out_sink);


/*
 *
 * Tracker code.
 *
 */

int
t_psmv_start(struct xrt_tracked_psmv *xtmv);

int
t_psmv_create(struct xrt_frame_context *xfctx,
              struct xrt_colour_rgb_f32 *rgb,
              struct t_stereo_camera_calibration *data,
              struct xrt_tracked_psmv **out_xtmv,
              struct xrt_frame_sink **out_sink);

int
t_psvr_start(struct xrt_tracked_psvr *xtvr);

int
t_psvr_create(struct xrt_frame_context *xfctx,
              struct t_stereo_camera_calibration *data,
              struct xrt_tracked_psvr **out_xtvr,
              struct xrt_frame_sink **out_sink);


/*
 *
 * Camera calibration
 *
 */

/*!
 * Board pattern type.
 */
enum t_board_pattern
{
        T_BOARD_CHECKERS,
        T_BOARD_CIRCLES,
        T_BOARD_ASYMMETRIC_CIRCLES,
};

struct t_calibration_status
{
        //! Is calibration finished?
        bool finished;
        //! Was the target found this frame?
        bool found;
        //! Number of frames collected
        int num_collected;
        //! Number of moving frames before another capture
        int cooldown;
        //! Number of non-moving frames before capture.
        int waits_remaining;
        //! Stereo calibration data that was produced.
        struct t_stereo_camera_calibration *stereo_data;
};

struct t_calibration_params
{
        //! Should we use fisheye version of the calibration functions.
        bool use_fisheye;
        //! Is the camera a stereo sbs camera, mostly for image loading.
        bool stereo_sbs;
        //! What type of pattern are we using for calibration.
        enum t_board_pattern pattern;

        struct
        {
                int cols;
                int rows;
                float size_meters;

                bool subpixel_enable;
                int subpixel_size;
        } checkers;

        struct
        {
                int cols;
                int rows;
                float distance_meters;
        } circles;

        struct
        {
                int cols;
                int rows;
                float diagonal_distance_meters;
        } asymmetric_circles;

        struct
        {
                bool enabled;
                int num_images;
        } load;

        int num_cooldown_frames;
        int num_wait_for;
        int num_collect_total;
        int num_collect_restart;

        /*!
         * Should we mirror the RGB image?
         *
         * Before text is written out, has no effect on actual image capture.
         */
        bool mirror_rgb_image;

        bool save_images;
};

/*!
 * Sets the calibration parameters to the their default values.
 */
static inline void
t_calibration_params_default(struct t_calibration_params *p)
{
        // Camera config.
        p->use_fisheye = false;
        p->stereo_sbs = true;

        // Which board should we calibrate against.
        p->pattern = T_BOARD_CHECKERS;

        // Checker board.
        p->checkers.cols = 9;
        p->checkers.rows = 7;
        p->checkers.size_meters = 0.025f;
        p->checkers.subpixel_enable = true;
        p->checkers.subpixel_size = 5;

        // Symmetrical circles.
        p->circles.cols = 9;
        p->circles.rows = 7;
        p->circles.distance_meters = 0.025f;

        // Asymmetrical circles.
        p->asymmetric_circles.cols = 5;
        p->asymmetric_circles.rows = 17;
        p->asymmetric_circles.diagonal_distance_meters = 0.02f;

        // Loading of images.
        p->load.enabled = false;
        p->load.num_images = 20;

        // Frame collection info.
        p->num_cooldown_frames = 20;
        p->num_wait_for = 5;
        p->num_collect_total = 20;
        p->num_collect_restart = 1;

        // Misc.
        p->mirror_rgb_image = false;
        p->save_images = true;
}

/*!
 * @brief Create the camera calibration frame sink.
 *
 * @param xfctx Context for frame transport.
 * @param params Parameters to use during calibration. Values copied, pointer
 * not retained.
 * @param status Optional pointer to structure for status information. Pointer
 * retained, and pointed-to struct modified.
 * @param gui Frame sink
 * @param out_sink Output: created frame sink.
 */
int
t_calibration_stereo_create(struct xrt_frame_context *xfctx,
                            const struct t_calibration_params *params,
                            struct t_calibration_status *status,
                            struct xrt_frame_sink *gui,
                            struct xrt_frame_sink **out_sink);


/*
 *
 * Sink creation functions.
 *
 */

int
t_convert_yuv_or_yuyv_create(struct xrt_frame_sink *next,
                             struct xrt_frame_sink **out_sink);



int
t_debug_hsv_picker_create(struct xrt_frame_context *xfctx,
                          struct xrt_frame_sink *passthrough,
                          struct xrt_frame_sink **out_sink);

int
t_debug_hsv_viewer_create(struct xrt_frame_context *xfctx,
                          struct xrt_frame_sink *passthrough,
                          struct xrt_frame_sink **out_sink);

int
t_debug_hsv_filter_create(struct xrt_frame_context *xfctx,
                          struct xrt_frame_sink *passthrough,
                          struct xrt_frame_sink **out_sink);

/*!
 * @}
 */


#ifdef __cplusplus
}
#endif