CodeFast

/**
 * @file sparse_mapping_interface.h
 * @brief sparse mapping interface
 */
#ifndef _SPARSE_MAPPING_INTERFACE_H
#define _SPARSE_MAPPING_INTERFACE_H

// keyword: SPAMAP

#ifdef __cplusplus
extern "C" {
#endif // __cplusplus

#include "mcamera_interface.h"
#include "mdewarper_interface.h"
#include <stddef.h>
#include <stdbool.h>

/**
 * @brief return value of the mapper
 *
 */
typedef enum _SparseMappingResult {
	SPAMAP_OK = 0,
	SPAMAP_ERR_ALLOC = 1, /**< Error allocate */
	SPAMAP_ERR_NON_CREATED, /**< Error create */
	SPAMAP_ERR_NULL_PARAM = 3, /**< Error parameters */
	SPAMAP_ERR_DONOT_SUPPORTED, /**< Do not support */
	SPAMAP_ERR_UNSUPPROT_CMD, /**< Un support CMD */
	SPAMAP_ERR_CAMEAR_NUM = 6, /**< Error camera number */
	SPAMAP_ERR_CAMEAR_IMG_IS_NULL, /**< Image is null */
	SPAMAP_ERR_STITCH_IMG_IS_NULL, /**< Stitch image is null */
	SPAMAP_ERR_PARAM_INOUT_BUFF_TOOSMALL, /**< Param in/out buffer is too small */
	SPAMAP_ERR_REJECTED, /**< Rejected */
	SPAMAP_ERR_NO_MAP_INFO_FOUND, /**< No map info */
	SPAMAP_ERR_INTERNAL, /**< Internal error */
	SPAMAP_ERR_UNKNOWN
} SparseMappingResult;

/**
 * @brief the status of the mapper
 *
 */
typedef enum _SparseMapping_MappingState {
	SPAMAP_STATE_UNKNOWN, /**< unknow state */
	SPAMAP_STATE_INITED, /**< mapper is created/initialized properly, you might start to feed data */
	SPAMAP_STATE_ODO_INITING, /**< visual odometry is initializing, you might continue to feed data */
	SPAMAP_STATE_WORKING, /**< sparse mapping is working properly */
	SPAMAP_STATE_LOST, /**< the visual odometry is lost, task failed */
	SPAMAP_STATE_STOPPING, /**< mapper is stopping with some post-processing */
	SPAMAP_STATE_IDLE, /**< mapper finished all the works */
	SPAMAP_STATE_ERROR, /**< general error, which is not expected to occur */
	SPAMAP_STATE_SUCCESS, /**< Mapping finished successfully */
	SPAMAP_STATE_UNVERIFIED, /**< Mapping quality is not verified */
	SPAMAP_STATE_FAILED, /**< Mapping finished but the map is bad */
	SPAMAP_STATE_TERMINATED, /**< Mapping is terminated by user */
	SPAMAP_STATE_NUM
} SparseMapping_MappingState;


static const char* SparseMapping_MappingStateInfo[] = {
    "UNKNOWN", "INITED", "ODO_INITING", "WORKING", "LOST", "STOPPING", "IDLE", "ERROR", "SUCCESS", "UNVERIFIED", "FAILED", "TERMINATED"
};

/**
 * @brief the data push option of the mapper
 *
 */
typedef enum _SparseMapping_DataPushOption {
	SPAMAP_PUSH_NOTHING			= 0x0001,
	SPAMAP_PUSH_MAPPING_STATE	= SPAMAP_PUSH_NOTHING << 1, /** push SparseMapping_MappingState in data package */
	SPAMAP_PUSH_TRAJECTORY		= SPAMAP_PUSH_NOTHING << 2, /** push SparseMapping_DataTrajectory_t in data package */
	SPAMAP_PUSH_MAP_POINTS		= SPAMAP_PUSH_NOTHING << 3, /** push SparseMapping_ in data package */
} SparseMapping_DataPushOption;

typedef enum _SparseMapping_TaskType {
	SPAMAP_TASK_PARK_IN,
	SPAMAP_TASK_PARK_OUT,
	SPAMAP_TASK_PASS_BY
} SparseMapping_TaskType;

typedef enum _SparseMapping_TrajectoryType {
	SPAMAP_TRAJ_UNKNOWN,
	SPAMAP_TRAJ_IN,
	SPAMAP_TRAJ_OUT,
	SPAMAP_TRAJ_TYPE_NUM
} SparseMapping_TrajectoryType;

/**
 * @brief trajectory data
 *
 */
typedef struct _SparseMapping_DataTrajectory_t {
    size_t timestamp = 0;                                    /**< timestamp of this trajectory pose */
    size_t counter = 0;                                      /**< trajectory  counter for double check */
    double heading = 0;                                      /**< heading direction angle w.r.t. world x-axis, in rads*/
    double qx = 0, qy = 0, qz = 0, qw = 0;                   /**< quarternion of the lastest trajectory pose */
    double px = 0, py = 0, pz = 0;                           /**< position of the latest trajectory pose */
    bool is_valid = false;                                   /**< indicating if this trajectory pose is valid or not */
    bool is_keyframe = false;                                /**< indicating if this trajectory pose is a keyframe */
    SparseMapping_TrajectoryType type = SPAMAP_TRAJ_UNKNOWN; /**< indicating different trajectories before/after picking the parking lot */
} SparseMapping_DataTrajectory_t;


/**
 * @brief map point
 *
 */
typedef struct _SparseMapping_MapPoint_t {
    double px = 0, py = 0, pz = 0; /**< 3D position of map point */
    double r = 0, g = 0, b = 0;    /**< color of the point, range [0, 1] */
} SparseMapping_MapPoint_t;


/**
 * @brief map points
 *
 */
typedef struct _SparseMapping_MapPoints_t {
    int sparse_point_num = 0;
    int semantic_point_num = 0;
    const SparseMapping_MapPoint_t* sparse_points = nullptr;
    const SparseMapping_MapPoint_t* semantic_points = nullptr;
} SparseMapping_MapPoints_t;


/**
 * @brief package data
 *
 */
typedef struct _SparseMapping_DataPackage_t {
	SparseMapping_MappingState mapping_state;
	SparseMapping_DataTrajectory_t trajectory;
	SparseMapping_MapPoints_t points;
	size_t counter = 0; /**< message counter for double check */
	double tranversed_distance = -1.0; /**< tranversed distance, negative when invalid */
	float post_processing_progress = -100.0; /**< post processing progress percentage, negative value when invalid */
} SparseMapping_DataPackage_t;


/**
 * @brief CMDs for the mapper
 *
 */
typedef enum _SparseMapping_CMDType {
    SPAMAP_CMDType_Unknow    = 0x0000,
    SPAMAP_CMDType_Stop      = 0x0001,   /**< Finish all the post-processing task and save a final map */
    SPAMAP_CMDType_Terminate = 0x0002,   /**< Terminate the mapping process right away, and prepare to exit */
} SparseMapping_CMDType;

/**
 * @brief camera data, we assume a grayscale image, i.e., channels = 1
 *
 */
typedef struct _SparseMapping_DataCamera_t {
	size_t timestamp, original_timestamp;
	int sensor_id; /** correspond to camera ID */
	unsigned char* buffer; /**< the actual camera buffer */
	int width, height, channels; /**< as is */
} SparseMapping_DataCamera_t;

/**
 * @brief Dectected 2D rectangle containing a car object
 *
 */
typedef struct SparseMapping_DataCarDetectionRect_t {
    float confidence; /**< confidence valude */
    int px, py;       /**< position of the top-left corner of the rectangle */
    int w, h;         /**< width and height of the rectangle */
} SparseMapping_DataCarDetectionRect_t;

typedef struct _SparseMapping_DataCarDetection_t {
    size_t timestamp, original_timestamp;
    int sensor_id;                               /** correspond to camera ID */
    int rect_num;                                /**< number of detected rectangles */
    SparseMapping_DataCarDetectionRect_t* rects; /**< Data of detected rectangles */
} SparseMapping_DataCarDetection_t;

/**
 * @brief wheel data
 *
 */
typedef struct _SparseMapping_DataWheel_t {
	size_t timestamp, original_timestamp;
	int sensor_id; /**< typically 0 */
	int fl, fr, rl, rr; /**< front-left, front-right, rear-left, rear-right, in pulses */
} SparseMapping_DataWheel_t;


/**
 * @brief GPS data, currently not supported
 *
 */
typedef struct _SparseMapping_DataGPS_t {
	size_t timestamp, original_timestamp;
	int sensor_id; /**< typically 0 */
} SparseMapping_DataGPS_t;


/**
 * @brief IMU data, currently not supported
 *
 */
typedef struct _SparseMapping_DataIMU_t {
	size_t timestamp, original_timestamp;
	int sensor_id; /**< typically 0 */
	double gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z; /** the 6-axis data of IMU */
} SparseMapping_DataIMU_t;


/// -- all the macros for buffer size defined here
#define SPAMAP_CAMERA_BUFFER_NUM_MAX (4)
#define SPAMAP_STITCHSEGMENT_BUFFER_NUM_MAX (1)
#define SPAMAP_IMU_BUFFER_NUM_MAX (500)
#define SPAMAP_WHEEL_BUFFER_NUM_MAX (500)
#define SPAMAP_GPS_BUFFER_NUM_MAX (10)

/**
 * @brief all the input data needed by sparse mapper
 *
 * @note p_wheel_data/p_gps_data/p_imu_data should be all the data between last and current camera buffer
 * @note p_camera_data should be fed into sparse mapper at 10Hz or more
 * @note p_stitch_segment_data is the segmented binary image of stitch image
 * @note p_wheel_data should be fed into sparse mapper at 50Hz or more
 * @note p_imu_data should be fed into sparse mapper at 50Hz or more
 */
typedef struct _SparseMapping_InputData_t {
	int camera_data_num;          /**< valid buffer size in camera_data */
	SparseMapping_DataCamera_t camera_data[SPAMAP_CAMERA_BUFFER_NUM_MAX];
	SparseMapping_DataCarDetection_t detection_data[SPAMAP_CAMERA_BUFFER_NUM_MAX];

	int stitch_segment_image_num; /**< valid buffer size in stitch_segment_data */
	SparseMapping_DataCamera_t stitch_segment_data[SPAMAP_STITCHSEGMENT_BUFFER_NUM_MAX];
	int wheel_data_num;           /**< valid buffer size in wheel_data */
	SparseMapping_DataWheel_t  wheel_data[SPAMAP_WHEEL_BUFFER_NUM_MAX];
	int gps_data_num;             /**< valid buffer size in gps_data */
	SparseMapping_DataGPS_t    gps_data[SPAMAP_GPS_BUFFER_NUM_MAX];
	int imu_data_num;             /**< valid buffer size in imu_data */
	SparseMapping_DataIMU_t    imu_data[SPAMAP_IMU_BUFFER_NUM_MAX];
} SparseMapping_InputData_t;

typedef enum _SparseMapping_POIType {
    SPAMAP_POIType_DROP_OFF = 0,
    SPAMAP_POIType_PICK_UP,
	SPAMAP_POIType_Num,
} SparseMapping_POIType;

#define SPAMAP_TRAJECTORY_MAP_BUFFER_NUM_MAX (2000)
#define SPAMAP_CARPORTS_MAP_BUFFER_NUM_MAX (50)
#define SPAMAP_MAP_NAME_MAX (1024)

/**
 * @brief the map information
 *
 */
typedef struct _SparseMapping_MapInfo_t {
	SparseMapping_TaskType task_type;
	int trajectory_num;
	SparseMapping_DataTrajectory_t trajectories[SPAMAP_TRAJECTORY_MAP_BUFFER_NUM_MAX];
	char map_name[SPAMAP_MAP_NAME_MAX];
	int carports_num;
	SparseMapping_DataTrajectory_t carports[SPAMAP_CARPORTS_MAP_BUFFER_NUM_MAX * 4]; /**< carports under the map coordinate system */
	SparseMapping_DataTrajectory_t poi_point[SPAMAP_POIType_Num];
	SparseMapping_MappingState final_state; /**< final state when this map is generated */
} SparseMapping_MapInfo_t;

/**
 * @brief the data requirments of mapper
 *
 */
typedef struct _SparseMapping_Config_t {
	bool require_stitch_segment_image, require_imu, require_wheel, require_gps; /**< user migh query these flags to feed data to mapper */
	int camera_nbr; /**< number of cameras needed in total */
} SparseMapping_Config_t;

/**
 * @brief handle property
 *
 */
typedef struct _SparseMapping_Handle_Property_t {
	bool do_stitch_segment_image; /**< perform semantic mapping or not */
	bool do_verify_map;
	int segment_width, segment_height; /**< resolution of stitch segment image */
	const char* param_files_path;
	const char* map_save_path, *map_name_prefix;
	SparseMapping_TaskType task_type;
} SparseMapping_Handle_Property_t;

/**
 * @brief frame info
 *
 */
typedef struct _SparseMapping_Frame_Info_t {
	size_t timestamp; /**< frame timestamp */
	bool frame_used; /**< if this frame is actually used */
	bool is_keyframe; /**< if this frame is a keyframe */
} SparseMapping_Frame_Info_t;

/**
 * @brief the mapper handle
 *
 */
typedef void* SparseMapping_Handle_t;

/**
 * @brief Callback function type for auto-push scheme
 *
 * @param _udata a pointer to the user-defined data structure, while
 * @param _data_package a pointer to a SparseMapping_DataPackage_t type
 */
typedef void (*SparseMapping_PushDataCallback)(void* _udata, void* _data_package);


/**
 * @brief Callback function type for frame information auto-push scheme
 *
 * @param _udata a pointer to the user-defined data structure, while
 * @param _frame_info a pointer to a SparseMapping_Frame_Info_t type
 */
typedef void (*SparseMapping_FrameInfoCallback)(void* _udata, void* _frame_info);


/**
 * @brief Tags for CLA filter
 */
typedef enum _SparseMapping_CLAFilterTag {
	SPAMAP_CLATAG_EGO_INIT_TIMEOUT,
	SPAMAP_CLATAG_EGO_LOST,
	SPAMAP_CLATAG_TOO_FEW_MAP_POINTS,
	SPAMAP_CLATAG_VERIFICATION_FAILED
} SparseMapping_CLAFilterTag;


/**
 * @brief Callback function type for CLA filter triggering
 *
 * @param _timestamp timestamp of the event
 * @param _tag one of the SparseMapping_CLAFilterTag enum values
 * @param _before recording milliseconds before _timestamp
 * @param _after recording milliseconds after _timestamp
 * @param _user_data_length length of _user_data
 * @param _user_data some user data
 */
typedef void (*SparseMapping_FiltererTriggerCallback)(size_t _timestamp, int _tag, int _before, int _after, size_t _user_data_length, const unsigned char* _user_data);

/**
 * @brief Pass CLA filter configurations using json string
 *
 * @param _handle
 * @param _filterers_config_json
 * @return SparseMappingResult
 */
SparseMappingResult SparseMapping_configureFilterers(SparseMapping_Handle_t _handle, const char *_filterers_config_json);

/**
 * @brief Set callback function to enable auto-triggered CLA filter
 *
 * @param _handle
 * @param _callback
 * @return SparseMappingResult
 */
SparseMappingResult SparseMapping_setFiltererTriggerCallback(SparseMapping_Handle_t _handle, SparseMapping_FiltererTriggerCallback _callback);


/**
 * @brief print out SDK version
 *
 */
void SparseMapping_printSDKVersion();

/**
 * @brief Get a valid Map
 *
 * @param _map_path
 * @param _map_info in/out
 * @return SparseMappingResult
 */
SparseMappingResult SparseMapping_getValidMap(const char* _map_path, SparseMapping_MapInfo_t *_map_info);

/**
 * @brief C API to create SparseMapping handle .
 *
 * @param _handle_property see SparseMapping_Handle_Property_t for details
 * @param _error_no in/out SparseMappingResult
 * @return SparseMapping_Handle_t to the created interface.
 */
SparseMapping_Handle_t SparseMapping_createHandle(const SparseMapping_Handle_Property_t* _handle_property, SparseMappingResult *_error_no);

/**
 * @brief C API to get Cameras correspond to original image.
 *
 * @param _handle mapper handle
 * @param _cameras in/out for all cameras
 * @param _max_cam_num in/out camera numbers
 * @return SparseMappingResult
 */
SparseMappingResult SparseMapping_getOriginCameras(SparseMapping_Handle_t _handle, mcamera::iMCamera **_cameres, size_t *_max_cam_num);

/**
 * @brief C API to Update Cameras.
 *
 * @param _handle mapper handle
 * @param _new_cameras new cameras
 * @param _cam_num  camera numbers
 * @return SparseMappingResult
 */
SparseMappingResult SparseMapping_updateCamera(SparseMapping_Handle_t _handle, const mcamera::iMCamera **_new_cameres, const size_t _cam_num);

/**
 * @brief C API to set cameras for car detection images.
 *
 * @param _handle mapper handle
 * @param _new_cameras new cameras
 * @param _cam_num  camera numbers
 * @return SparseMappingResult
 */
SparseMappingResult SparseMapping_setCarDetectionCameras(SparseMapping_Handle_t _handle, const mcamera::iMCamera **_new_cameres, const size_t _cam_num);

/**
 * @brief query the config of the mapper, you might call this function when a SparseMapping_Handle_t is created
 *
 * @param _handle mapper handle
 * @param _config in/out the config of mapper
 * @return SparseMappingResult
 *
 * @note this function is independent of the Sparse Mapping Interface
 */
SparseMappingResult SparseMapping_queryConfig(SparseMapping_Handle_t _handle, SparseMapping_Config_t *_config);

/**
 * @brief Set callback function to enable auto-triggered result push
 *
 * After the callback function is properly set up, the callback function will be called
 * automatically everytime when a frame is mature and out of the sliding window.
 *
 * @param _pushData Callback function to push data
 * @param _user_data User defined data
 * @param _push_option, see SparseMapping_DataPushOption
 * @return SparseMappingResult
 */
SparseMappingResult SparseMapping_setCallbacks(SparseMapping_Handle_t _handle, SparseMapping_PushDataCallback _call_back, void* _user_data, int _push_option);

/**
 * @brief Set callback function to enable auto-triggered frame information push, used when you wanted to keep your buffer size from outside
 *
 * After the callback function is properly set up, the callback function will be called
 * automatically everytime when a frame send to visual odometry
 *
 * @param _call_back Callback function to push data
 * @param _user_data user defined data
 * @return SparseMappingResult
 */
SparseMappingResult SparseMapping_setFrameInfoCallbacks(SparseMapping_Handle_t _handle, SparseMapping_FrameInfoCallback _call_back, void* _user_data);

/**
 * @brief Start the Sparse Mapping Interface Pipeline
 *
 * @param _handle
 * @return SparseMappingResult
 */
SparseMappingResult SparseMapping_startPipeline(SparseMapping_Handle_t _handle);

/**
 * @brief Stop the Sparse Mapping Interface Pipeline
 *
 * @param _handle the mapper handle
 * @return SparseMappingResult
 *
 * @note this function will kill the mapper immediately, you should always feed SPAMAP_CMDType_Stop
 * via SparseMapping_feedCMD to finish a normal mapping process
 */
SparseMappingResult SparseMapping_stopPipeline(SparseMapping_Handle_t _handle);

/**
 * @brief Set the source buffer of all sensors needed by sparse mapper
 *
 * @param _handle
 * @param _input_data data of the sensor buffer
 * @return SparseMappingResult SPAMAP_OK when correctly executed, SPAMAP_ERR otherwise
 */
SparseMappingResult SparseMapping_feedData(SparseMapping_Handle_t _handle, const SparseMapping_InputData_t *_input_data_ptr);

/**
 * @brief Set the source buffer for a specific sensor
 *
 * @param _handle
 * @param _cmd_type Type of the CMD
 * @return SparseMappingResult SPAMAP_OK when correctly executed, SPAMAP_ERR otherwise
 */
SparseMappingResult SparseMapping_feedCMD(SparseMapping_Handle_t _handle, SparseMapping_CMDType _cmd_type);


/**
 * @brief set a poi point corresponds to current car position
 *
 * @param _handle
 * @param _poi_type type of current POI(Point Of Interest)
 * @return SparseMappingResult SPAMAP_OK when correctly executed, SPAMAP_ERR otherwise
 */
SparseMappingResult SparseMapping_setCurrentPOIPoint(SparseMapping_Handle_t _handle, SparseMapping_POIType _poi_type);

/**
 * @brief
 *
 * @param _handle
 * @param carports
 * @return SparseMappingResult SPAMAP_OK when correctly executed, SPAMAP_ERR otherwise
 */
SparseMappingResult SparseMapping_setCurrentCarport(SparseMapping_Handle_t _handle, SparseMapping_DataTrajectory_t carports[4]);

/**
 * @brief C API to destroy the previously created SparseMapping_Handle_t.
 *
 * @param _handle
 * @return SparseMappingResult SPAMAP_OK when correctly executed, SPAMAP_ERR otherwise.
 *
 * @note you should call this function when mapper status is STATE_MAPPING_IDLE
 */
void SparseMapping_release(SparseMapping_Handle_t _handle);


/* ***************************************************************************** */

/**
 * @brief log level
 *
 */
typedef enum _SparseMapping_LogLevel {
	SPAMAP_LogLevel_Debug,
	SPAMAP_LogLevel_Verbose,
	SPAMAP_LogLevel_Info,
	SPAMAP_LogLevel_Vital,
	SPAMAP_LogLevel_Warning,
	SPAMAP_LogLevel_Error,
	SPAMAP_LogLevel_Fatal
} SparseMapping_LogLevel;

/**
 * @brief Set filename for the log (.txt), NULL by default
 *
 * @param _handle
 * @param _log_filename
 * @return SparseMappingResult SPAMAP_OK when correctly executed, SPAMAP_ERR otherwise
 */
SparseMappingResult SparseMapping_setLogFileName(SparseMapping_Handle_t _handle, const char* _log_filename);

/**
 * @brief Set log level for file output, SparseMapping_LogLevelInfo by default.
 *
 * @param _handle
 * @param _log_level One of SparseMapping_LogLevel enumerations
 * @return SparseMappingResult
 */
SparseMappingResult SparseMapping_setLogFileLevel(SparseMapping_Handle_t _handle, SparseMapping_LogLevel _log_level);

/**
 * @brief Set log level for console output, SparseMapping_LogLevelInfo by default.
 *
 * @param _handle
 * @param _log_level One of SparseMapping_LogLevel enumerations
 * @return SparseMappingResult
 */
SparseMappingResult SparseMapping_setLogConsoleLevel(SparseMapping_Handle_t _handle, SparseMapping_LogLevel _log_level);

/**
 * @brief Set cpu mask for alg to bind, default is cpu core number - 1 . eg. Core number is 6, then default is 5.
 *
 * @param _handle
 * @param _cpu_mask The mask, 0x21 : core 1 and 5
 * @return SparseMappingResult
 */
SparseMappingResult SparseMapping_setAlgCoreByMask(SparseMapping_Handle_t _handle, unsigned int _cpu_mask = 0x0);

/**
 * @brief Enable or disable CPU utilization statistics, default is false
 *
 * @param _handle
 * @param _enable
 * @return SparseMappingResult
 */
SparseMappingResult SparseMapping_setEnableCPUUtilization(SparseMapping_Handle_t _handle, bool _enable = false);

/**
 * @brief Set the limit of cpu loading, lower zhen _percent, default is no limits
 *
 * @param _handle
 * @param _percent, from 70 to 100
 * @return SparseMappingResult
 */
SparseMappingResult SparseMapping_setCPULimitPercent(SparseMapping_Handle_t _handle, int _percent = 100);

const char* SparseMapping_version();

#ifdef __cplusplus
}
#endif // __cplusplus

#endif // _SPARSE_MAPPING_INTERFACE_H