TI Autonomous Driving Algorithms (TIADALG) Library User Guide
tiadalg_vl_alg_int.h
Go to the documentation of this file.
1 /*
2 *
3 * Copyright (c) {2015 - 2019} Texas Instruments Incorporated
4 *
5 * All rights reserved not granted herein.
6 *
7 * Limited License.
8 *
9 * Texas Instruments Incorporated grants a world-wide, royalty-free, non-exclusive
10 * license under copyrights and patents it now or hereafter owns or controls to make,
11 * have made, use, import, offer to sell and sell ("Utilize") this software subject to the
12 * terms herein. With respect to the foregoing patent license, such license is granted
13 * solely to the extent that any such patent is necessary to Utilize the software alone.
14 * The patent license shall not apply to any combinations which include this software,
15 * other than combinations with devices manufactured by or for TI ("TI Devices").
16 * No hardware patent is licensed hereunder.
17 *
18 * Redistributions must preserve existing copyright notices and reproduce this license
19 * (including the above copyright notice and the disclaimer and (if applicable) source
20 * code license limitations below) in the documentation and/or other materials provided
21 * with the distribution
22 *
23 * Redistribution and use in binary form, without modification, are permitted provided
24 * that the following conditions are met:
25 *
26 * * No reverse engineering, decompilation, or disassembly of this software is
27 * permitted with respect to any software provided in binary form.
28 *
29 * * any redistribution and use are licensed by TI for use only with TI Devices.
30 *
31 * * Nothing shall obligate TI to provide you with source code for the software
32 * licensed and provided to you in object code.
33 *
34 * If software source code is provided to you, modification and redistribution of the
35 * source code are permitted provided that the following conditions are met:
36 *
37 * * any redistribution and use of the source code, including any resulting derivative
38 * works, are licensed by TI for use only with TI Devices.
39 *
40 * * any redistribution and use of any object code compiled from the source code
41 * and any resulting derivative works, are licensed by TI for use only with TI Devices.
42 *
43 * Neither the name of Texas Instruments Incorporated nor the names of its suppliers
44 *
45 * may be used to endorse or promote products derived from this software without
46 * specific prior written permission.
47 *
48 * DISCLAIMER.
49 *
50 * THIS SOFTWARE IS PROVIDED BY TI AND TI'S LICENSORS "AS IS" AND ANY EXPRESS
51 * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
52 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
53 * IN NO EVENT SHALL TI AND TI'S LICENSORS BE LIABLE FOR ANY DIRECT, INDIRECT,
54 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
55 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
56 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
57 * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
58 * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
59 * OF THE POSSIBILITY OF SUCH DAMAGE.
60 *
61 */
62 
63 
75 #ifndef _TIADALG_EL_ALG_INT_H_
76 #define _TIADALG_EL_ALG_INT_H_
77 
78 /*--------------------------------------------------------------------------*/
79 /* This header file inclusion is only for XDAIS module. It accepts specific */
80 /* data types defined externally */
81 /*--------------------------------------------------------------------------*/
82 
83 #include <stdio.h>
84 #include <stdlib.h>
85 #include <string.h>
86 #include <assert.h>
87 #include <stdint.h>
88 #include <stdarg.h>
89 
90 #include "tiadalg_interface.h"
91 #include <./../common/tiadalg_alg_int_interface.h>
92 
93 #ifdef __cplusplus
94 extern "C" {
95 #endif
96 
97 //#define ENABLE_PROFILE
98 
105 typedef enum
106 {
108 
109  EL_ALG_BUFDESC_VOXEL_INFO_MEM, /*Persistant voxel info memory*/
110  EL_ALG_BUFDESC_3D_POINTS_MEM, /*Persistant 3d points memory*/
111  EL_ALG_BUFDESC_POINTS_DESC_MEM, /*Persistant points descriptor memory*/
112 
113  EL_ALG_EXT_PERSIST_MEM, /*Persistant External Memory*/
114  EL_ALG_EXT_SCRATCH_MEM, /*Scratch External Memory*/
115  EL_ALG_L2_SCRATCH_MEM, /*All L2 memory is assumed to be scratch*/
117 } eMemrecs;
118 
123 typedef enum
124 {
127 } eAlgState;
128 
129 typedef struct
130 {
131  //float t[3];
132  //float euler_angle[3];
133  float elem[6];
134 }EL_TI_Pose;
135 
136 /* Observation vector index for Kalman Filter */
137 #define KF_MAXIMUM_MEASUREMENT (3)
138 
139 /* State vector index for Kalman Filter */
140 #define KF_MAXIMUM_STATE (9)
141 
192 typedef struct
193 {
194  float transition[KF_MAXIMUM_STATE * KF_MAXIMUM_STATE];
196  float predictedErrorCov[KF_MAXIMUM_STATE * KF_MAXIMUM_STATE];
197  float state[KF_MAXIMUM_STATE];
198  float predictedState[KF_MAXIMUM_STATE];
200  float processNoiseCov[KF_MAXIMUM_STATE * KF_MAXIMUM_STATE];
201  float measurementNoiseCov[KF_MAXIMUM_MEASUREMENT * KF_MAXIMUM_MEASUREMENT];
206  float scaleFactor;
208  int32_t num_state;
210 
211 /*algorithm parameters in persistant memory*/
212 typedef struct
213 {
214  /*CREATE TIME PARAMETERS */
215  uint8_t* l2_scratch;
216  uint8_t* ext_scratch;
220  int32_t frame_idx;
221 
222  /*PROCESS TIME PARAMETERS */
223  int32_t width;
224  int32_t height;
225  uint8_t *in_img;
228  float est_loc[3];
229  float est_rot[3][3];
230 
231  /* POSE FILTERING RELATED */
232  EL_TI_Pose prev_est_pose; /* previous measurement */
233  EL_TI_Pose prev_filt_pose; /* previous filtered estimate */
234  EL_TI_Pose cur_filt_pose; /* this is not persistant data*/
235  EL_TI_Pose iir_filt_pose; /* iir filtered pose */
236 
237  EL_TI_Kalaman_filter kf[4]; /*3 kalman filter for 3 euler angles and 1 for translation parameters*/
238 
239 }EL_TI_Prms;
240 
258 typedef struct
259 {
260  const IVISION_Fxns* ivision;
261  uint8_t algState;
262  uint32_t numMemRecs;
263  IALG_MemRec memRec[NUM_MEMRECS];
266 
267 } EL_TI_Obj;
268 
270 
271 
272 /*---------------------------------------------------------------------------*/
273 /* IALG functions */
274 /* Refer XDAIS ialg.h file for details on these functions */
275 /*---------------------------------------------------------------------------*/
276 int32_t EL_TI_numAlloc(void);
277 int32_t EL_TI_alloc(const IALG_Params *params,
278  struct IALG_Fxns **parentFxns, IALG_MemRec *memRec);
279 int32_t EL_TI_init(IALG_Handle handle, const IALG_MemRec *memRec,
280  IALG_Handle parent, const IALG_Params *params);
281 void EL_TI_activate(IALG_Handle handle);
282 void EL_TI_deactivate(IALG_Handle handle);
283 int32_t EL_TI_free(IALG_Handle handle, IALG_MemRec *memRec);
284 int32_t EL_TI_control(IVISION_Handle handle, IALG_Cmd cmd,
285  const IALG_Params *inParams, IALG_Params *outParams);
286 int32_t EL_TI_process(IVISION_Handle handle, IVISION_InBufs *inBufs,
287  IVISION_OutBufs *outBufs, IVISION_InArgs *inArgs,
288  IVISION_OutArgs *outArgs);
289 
290 int32_t EL_TI_pose_filtering(float in_rot[3][3], float in_t[3], float out_rot_mat[3][3], float out_rot_angle[3], float out_t[3],
291  int32_t is_first_frame, int32_t solve_pnp_status, EL_TI_Kalaman_filter* kf,
292  EL_TI_Pose* prev_est_pose, EL_TI_Pose* prev_filt_pose,EL_TI_Pose* cur_filt_pose,EL_TI_Pose* iir_filt_pose);
293 
294 void TIADALG_KF_Correct_F32(EL_TI_Kalaman_filter *KF, const float * Z, float * Residual);
296 
297 void TIADALG_KF_Init(EL_TI_Kalaman_filter *KF, int32_t num_meas, int32_t num_state, float* tansition_matrix);
298 
299 int32_t TIADALG_gaussJordanElimination_cn(float *a, int32_t n, float *b,int32_t aMatPitch, uint8_t* scratch);
300 
301 static void inline memset_float(float* dst, float val, int32_t num)
302 {
303  int32_t i;
304  for(i =0 ;i < num; i++){
305  dst[i] = val;
306  }
307 }
308 
309 void getOrientationFromRotationMat(float rotationMat[3][3], float orientation[3]);
310 
311 #ifdef __cplusplus
312 }
313 #endif
314 
315 
316 #endif /* _TIADALG_EL_ALG_INT_H_ */
EL_TI_Pose iir_filt_pose
Definition: tiadalg_vl_alg_int.h:235
float * map_3d_points
Definition: tiadalg_vl_alg_int.h:218
static void memset_float(float *dst, float val, int32_t num)
Definition: tiadalg_vl_alg_int.h:301
VL_DESC_DATA_TYPE * ext_desc
Definition: tiadalg_vl_alg_int.h:227
Definition: tiadalg_vl_alg_int.h:107
EL_TI_Obj * EL_TI_Handle
Definition: tiadalg_vl_alg_int.h:269
eMemrecs
Definition: tiadalg_vl_alg_int.h:105
#define KF_MAXIMUM_STATE
Definition: tiadalg_vl_alg_int.h:140
uint32_t numMemRecs
Definition: tiadalg_vl_alg_int.h:262
EL_TI_Prms ti_prms
Definition: tiadalg_vl_alg_int.h:265
tiadalg_ext_feat * ext_feat
Definition: tiadalg_vl_alg_int.h:226
uint8_t algState
Definition: tiadalg_vl_alg_int.h:261
int32_t frame_idx
Definition: tiadalg_vl_alg_int.h:220
void TIADALG_KF_Init(EL_TI_Kalaman_filter *KF, int32_t num_meas, int32_t num_state, float *tansition_matrix)
int32_t num_measurement
Definition: tiadalg_vl_alg_int.h:207
int32_t width
Definition: tiadalg_vl_alg_int.h:223
Definition: tiadalg_vl_alg_int.h:212
int32_t height
Definition: tiadalg_vl_alg_int.h:224
uint8_t * l2_scratch
Definition: tiadalg_vl_alg_int.h:215
Definition: tiadalg_vl_alg_int.h:192
Definition: tiadalg_vl_alg_int.h:126
int32_t EL_TI_process(IVISION_Handle handle, IVISION_InBufs *inBufs, IVISION_OutBufs *outBufs, IVISION_InArgs *inArgs, IVISION_OutArgs *outArgs)
void TIADALG_KF_Predict_F32(EL_TI_Kalaman_filter *KF)
int32_t EL_TI_free(IALG_Handle handle, IALG_MemRec *memRec)
int32_t EL_TI_init(IALG_Handle handle, const IALG_MemRec *memRec, IALG_Handle parent, const IALG_Params *params)
int32_t EL_TI_alloc(const IALG_Params *params, struct IALG_Fxns **parentFxns, IALG_MemRec *memRec)
uint8_t * in_img
Definition: tiadalg_vl_alg_int.h:225
Definition: tiadalg_vl_alg_int.h:116
This defines the property of one voxel List of tiadalg_voxel_info objects are passed through buffer T...
Definition: tiadalg_visual_localization.h:111
Definition: tiadalg_vl_alg_int.h:129
void getOrientationFromRotationMat(float rotationMat[3][3], float orientation[3])
EL_TI_Pose cur_filt_pose
Definition: tiadalg_vl_alg_int.h:234
EL_TI_Pose prev_filt_pose
Definition: tiadalg_vl_alg_int.h:233
Algorithm create time parameters.
Definition: tiadalg_visual_localization.h:199
uint8_t VL_DESC_DATA_TYPE
Image feature Descriptor data type. Prebuilt binaries are for unsigned 8b, 64 element feature descrip...
Definition: tiadalg_visual_localization.h:88
EL_TI_Pose prev_est_pose
Definition: tiadalg_vl_alg_int.h:232
uint8_t * ext_scratch
Definition: tiadalg_vl_alg_int.h:216
This structure is the main handle of EL Module.
Definition: tiadalg_vl_alg_int.h:258
Definition: tiadalg_vl_alg_int.h:113
void EL_TI_deactivate(IALG_Handle handle)
void TIADALG_KF_Correct_F32(EL_TI_Kalaman_filter *KF, const float *Z, float *Residual)
Information of one externally computed image feature point.
Definition: tiadalg_visual_localization.h:137
int32_t num_state
Definition: tiadalg_vl_alg_int.h:208
#define KF_MAXIMUM_MEASUREMENT
Definition: tiadalg_vl_alg_int.h:137
tiadalg_voxel_info * voxel_info
Definition: tiadalg_vl_alg_int.h:217
Definition: tiadalg_vl_alg_int.h:114
Definition: tiadalg_vl_alg_int.h:115
int32_t EL_TI_pose_filtering(float in_rot[3][3], float in_t[3], float out_rot_mat[3][3], float out_rot_angle[3], float out_t[3], int32_t is_first_frame, int32_t solve_pnp_status, EL_TI_Kalaman_filter *kf, EL_TI_Pose *prev_est_pose, EL_TI_Pose *prev_filt_pose, EL_TI_Pose *cur_filt_pose, EL_TI_Pose *iir_filt_pose)
TIADALG_el_create_params algCreatePrms
Definition: tiadalg_vl_alg_int.h:264
void EL_TI_activate(IALG_Handle handle)
Definition: tiadalg_vl_alg_int.h:125
int32_t TIADALG_gaussJordanElimination_cn(float *a, int32_t n, float *b, int32_t aMatPitch, uint8_t *scratch)
Definition: tiadalg_vl_alg_int.h:111
VL_DESC_DATA_TYPE * map_desc
Definition: tiadalg_vl_alg_int.h:219
eAlgState
Definition: tiadalg_vl_alg_int.h:123
Definition: tiadalg_vl_alg_int.h:109
int32_t EL_TI_control(IVISION_Handle handle, IALG_Cmd cmd, const IALG_Params *inParams, IALG_Params *outParams)
const IVISION_Fxns * ivision
Definition: tiadalg_vl_alg_int.h:260
float scaleFactor
Definition: tiadalg_vl_alg_int.h:206
Definition: tiadalg_vl_alg_int.h:110
int32_t EL_TI_numAlloc(void)

© Copyright 2018 Texas Instruments Incorporated. All rights reserved.
Document generated by doxygen 1.8.6