TI Autonomous Driving Algorithms (TIADALG) Library User Guide
sfm_ti_alg_int.h
Go to the documentation of this file.
1 /*
2 * module name : Structure From Motion
3 *
4 * module descripton : Generates sparse 3D points from optical flow information in camera captured images
5 *
6 * Copyright (C) 2009-2017 Texas Instruments Incorporated - http://www.ti.com/
7 * ALL RIGHTS RESERVED
8 *
9 */
10 
22 #ifndef TI_SFM_ALG_INT_H
23 #define TI_SFM_ALG_INT_H
24 #include "c7x.h"
25 #include <./../common/tiadalg_alg_int_interface.h>
26 #include <./../common/tiadalg_common_utils.h>
27 #include "tiadalg_interface.h"
28 #include <math.h>
29 /*
30 * float.h is required for some preprocessors like DBL_MIN, FLT_MIN etc.
31 */
32 #include <float.h>
33 #include <string.h>
34 #include <./../common/profile.h>
35 
36 #include "sfm_ti_device_utils.h"
38 
39 /*
40 * Maximum number of input points or tracks
41 */
42 #define MAX_NUM_IN_POINTS (20000)
43 
44 /*
45 * Maximum number of output 3D points
46 */
47 #define MAX_NUM_OUT_POINTS MAX_NUM_IN_POINTS
48 
49 /*
50 * Depth of the circular buffer which holds user provided camera extrinsic parameters
51 */
52 #define CAM_EXT_PRM_BUF_DEPTH (VLIB_TRIANG_MAX_POINTS_IN_TRACK)
53 
54 /*
55 * Maximum number of 3D points maintained inside the algorithm
56 */
57 #define MAX_3D_POINTS (MAX_NUM_OUT_POINTS)
58 
59 /*
60 * Total number of vector processed in one call of a kernel. This will controlled by local memory size.
61 */
62 #define NUM_VECTORS_TOGATHER (48)
63 
64 /*
65 * Total number of tracks processed in single call of kernel
66 */
67 #define NUM_TRACKS_TOGATHER (NUM_VECTORS_TOGATHER * VLIB_TRIANG_NUM_TRACKS_IN_ONE_VECTOR)
68 
69 /*
70 * Size of requested scratch area in L2 of DSP. request all the scratch leaving 64 KB of cache
71 */
72 //#define L2_SCRATCH_BUF_SIZE (480*1024) //480KB sram and 64 kb cache. total size of 512 + 32.
73 #define L2_SCRATCH_BUF_SIZE (448*1024)
74 
75 /*
76 * Size of requested scratch area in DDR
77 */
78 #define DDR_SCRATCH_BUF_SIZE (8*1024)
79 
80 
81 
82 #define SIMD_WIDTH (16)
83 
84 #define TIADALG_DMA_CHANNEL_PING_PONG ((int32_t) 0)
85 #define TIADALG_DMA_CHANNEL_CBCR_PING_PONG ((int32_t) 1)
86 #define TIADALG_DMA_CHANNEL_MEMCPY_2 ((int32_t) 2)
87 #define TIADALG_DMA_CHANNEL_MEMCPY ((int32_t) 3)
88 #define TIADALG_DMA_CHANNEL_MAX ((int32_t) 4)
89 
90 #define TIADALG_FLOW_CTRL_OPT_ONLY (0)
91 #define TIADALG_FLOW_CTRL_REF_ONLY (1)
92 #define TIADALG_FLOW_CTRL_NATC_ONLY (2)
93 
94 extern TI_DSP_PrfInfo* prfInfo;
95 
101 typedef enum
102 {
103  /* Memory records for handle */
104  /* Alg Handle should be first entry in this enum table
105  as test app is expecting it to be first entry*/
115 } eMemrecs;
116 
121 typedef enum
122 {
125 } eAlgState;
126 
127 
128 
141 typedef struct
142 {
143  IVISION_Fxns * ivision;
144 
145  uint8_t algState;
146  uint32_t numMemRecs;
147  IALG_MemRec memRec[NUM_MEMRECS];
149 
150  uint8_t * intAlgHandle;
151  void * udmaDrvObj;
153 
154  uint8_t memcpyTr[64U];
155  uint8_t pingPongDofTr[64U];
156  uint8_t pingPongCbCrTr[64U];
157 } SFM_TI_Obj;
158 
160 
161 /*--------------------------------------------------------*/
162 /* IALG functions */
163 /* Refer XDAIS ialg.h file for details on these functions */
164 /*--------------------------------------------------------*/
165 int32_t SFM_TI_numAlloc(void);
186 int32_t SFM_TI_alloc
187  (
188  const IALG_Params *params,
189  struct IALG_Fxns **parentFxns,
190  IALG_MemRec *memRec
191  );
211 int32_t SFM_TI_init
212  (
213  IALG_Handle handle,
214  const IALG_MemRec *memRec,
215  IALG_Handle parent,
216  const IALG_Params *params
217  );
236 void SFM_TI_activate(IALG_Handle handle);
255 void SFM_TI_deactivate(IALG_Handle handle);
274 int32_t SFM_TI_free(IALG_Handle handle, IALG_MemRec *memRec);
275 
276 int32_t SFM_TI_control
277  (
278  IVISION_Handle handle,
279  IALG_Cmd cmd,
280  const IALG_Params *inParams,
281  IALG_Params *outParams
282  );
325 int32_t SFM_TI_process
326  (
327  IVISION_Handle handle,
328  IVISION_InBufs *inBufs,
329  IVISION_OutBufs *outBufs,
330  IVISION_InArgs *inArgs,
331  IVISION_OutArgs *outArgs
332  );
333 #define TIADALG_DOF_CONFIDENCE_TO_UINT8_SCALING (17)
334 
358 typedef struct
359 {
360  uint8_t age;
361  uint8_t idx;
365 
383 typedef struct{
384 
386  float matrixAtb[VLIB_TRIANG_MAT_AROW][NUM_TRACKS_TOGATHER]; //[][3 + 1]
389 
405 typedef struct{
407  float matrixb[VLIB_TRIANG_MAT_COL][NUM_TRACKS_TOGATHER]; //[][12]
411 
422 typedef struct{
423  float reprojErr[NUM_TRACKS_TOGATHER];
424  float subAngle[NUM_TRACKS_TOGATHER];
425  uint8_t valid[NUM_TRACKS_TOGATHER];
427 
439 typedef union {
444 
477 #define SE_PARAM_BASE (0x0000)
478 #define SE0_PARAM_OFFSET (SE_PARAM_BASE)
479 #define SE1_PARAM_OFFSET (SE0_PARAM_OFFSET + SE_PARAM_SIZE)
480 #define SE2_PARAM_OFFSET (SE1_PARAM_OFFSET + SE_PARAM_SIZE)
481 #define SE3_PARAM_OFFSET (SE2_PARAM_OFFSET + SE_PARAM_SIZE)
482 #define SE4_PARAM_OFFSET (SE3_PARAM_OFFSET + SE_PARAM_SIZE)
483 #define SE5_PARAM_OFFSET (SE4_PARAM_OFFSET + SE_PARAM_SIZE)
484 #define MAX_NUM_STRM_TEMPLATE (6)
485 
486 #define SE_PARAM_WORDS ((uint32_t)SE_PARAM_SIZE/sizeof(uint32_t))
487 
488 typedef struct
489 {
490  /* Below two elements are used in F matrix computation from R & T parameters,
491  Also inverse one is used in subtended angle calculation
492  */
495 
496  /* Used in data matrix formation for triangulation
497  */
499 
500  /* Used in re projection error calculation
501  */
503 
504  /* Used in image co-ordinate calculation
505  */
507 
508  /* Used in F matrix calculation from R &T
509  */
511 
512  /*First frame camera intrinsic parameter
513  */
515 
517 
530 typedef struct
531 {
532  /*Block based track processing. based on available L2 size below structure
533  can be enlarged by increasing NUM_VECTORS_TOGATHER*/
534  /*This structure is used in track loop, not dependent of image reoslution.
535  size of structure is fix for given resolution and given algorithm.
536  */
539  uint16_t trackId[NUM_TRACKS_TOGATHER];
540  uint8_t curFeatInTrack[NUM_TRACKS_TOGATHER];
541  float Xcam[NUM_TRACKS_TOGATHER][3];
544 
546 
588 typedef struct
589 {
590  // track data is persistant in DDR, and brought to L3/L2 at start.
591  // Currently it is allocated in L3
594 
595  // occupancy grid buffer data is persistant in DDR, and brought to L3/L2 at start.
596  // Currently it is allocated in L3
597  int8_t *ogBuf ;
598  int8_t *ogBufInt ;
599  uint8_t *constYBuf;
600  uint8_t *constCbCrBuf;
601  int32_t isLocalOg;
602 
603  /*pointers of l2 memory. Pointers can be overlapping based on where/when it is used in flow*/
604  /*Any ping-pong buffer pointers will come here*/
605 
606  /*Needed in track maintainance code*/
607  uint8_t *keyPointBitMap;
608  uint16_t *curInTrackIdx;
609  uint16_t *curOutTrackIdx;
610  uint8_t *dofBufInt;
611 
612  /*used in F matrix estimation and pruning. Total size is 19*max_tracks */
613  uint8_t *isInlier; //[MAX_NUM_IN_POINTS]
614  uint16_t *indexMap;
615  float *curPoints; // x and y
616  float *prevPoints; // x and y
617 
618  /*used in visualization module*/
620  uint8_t *tempImgPtCldL2;
621  uint8_t *tempImgOgL2;
622  uint8_t *colorIndx;
623  uint16_t *blkIndx;
624  uint16_t *curBlkIndx;
625 
626  /*l2 static buffers pointers related to track loop*/
629  uint16_t *trackId;
630  uint8_t *curFeatInTrack;
631  float (*Xcam)[3];
633  uint32_t (*pBlock)[SE_PARAM_WORDS];
634 
635  /*l1 static buffers pointers*/
636  /*Important matrices which are used in track loop is placed inL1D*/
637  /*used in chirality pruning*/
639  /*used in SFM_TI_subtendedAnglePrun*/
641  /*used in triangulation*/
642  float (*arrangedCamExtPrmL1)[VLIB_TRIANG_CAMERA_EXTRENSIC_PARAM_ASIZE];
643  /*used in reprojection error*/
644  float (*camIntXExtPrmL1)[VLIB_TRIANG_CAMERA_EXTRENSIC_PARAM_ASIZE];
645  /*used in transforming camera points into image points*/
646  float *camIntPrmL1;
647  /*used in SFM_TI_getCurTrack API*/
649  /*First camera intrinsic parameter*/
651 
652  /*Camera parameters*/
658  uint8_t scratchBuf[DDR_SCRATCH_BUF_SIZE];
659 
660  uint16_t num3DPoints;
661  uint32_t curFrameNum;
663  int32_t maxNumTracks;
664  void* fMatParams;
665  uint16_t profileEn;
666 
671  int32_t numDofBlocks;
672 
675 
677 
680  uint8_t* sfmL3Scratch;
681 
683 
684 void SFM_TI_updatePtrs(sSfm_TI_L1DMem* sfmL1Prm, sSfm_TI_L2DMem* sfmL2Prm, sSfm_TI_ExtMem* sfmPrm, SFM_TI_CreateParams * createParams);
685 #endif /* TI_SFM_ALG_INT_H */
686 
sSFM_TI_EqSolvePrm eqSolvePrm
Definition: sfm_ti_alg_int.h:440
uint8_t * sfmL3Scratch
Definition: sfm_ti_alg_int.h:680
int32_t numRowInOgVisBuf
Definition: sfm_ti_alg_int.h:674
sSFM_TI_PrunPrm prunPrm
Definition: sfm_ti_alg_int.h:442
uint16_t * indexMap
Definition: sfm_ti_alg_int.h:614
Triangulation related mutually exclusive scratch data.
Definition: sfm_ti_alg_int.h:439
uSFM_TI_MatData * matData
Definition: sfm_ti_alg_int.h:632
#define VLIB_TRIANG_MAX_POINTS_IN_TRACK
Definition: VLIB_triangulatePoints_types.h:31
int32_t SFM_TI_numAlloc(void)
Definition: sfm_ti_alg_int.h:109
Definition: sfm_ti_alg_int.h:141
uint8_t algState
Definition: sfm_ti_alg_int.h:145
#define VLIB_TRIANG_MAT_AROW
Definition: VLIB_triangulatePoints_types.h:57
uint32_t numMemRecs
Definition: sfm_ti_alg_int.h:146
Definition: sfm_ti_alg_int.h:108
Definition: sfm_ti_alg_int.h:112
L1D scratch memory definition. Base address of ever element in below structure is double word aligned...
Definition: sfm_ti_alg_int.h:488
#define VLIB_TRIANG_CAMERA_INTRINSIC_PARAM_ASIZE
Definition: VLIB_triangulatePoints_types.h:26
L1D scratch memory definition. Base address of ever element in below structure is double word aligned...
Definition: sfm_ti_alg_int.h:588
int32_t keyPointBitMapSize
Definition: sfm_ti_alg_int.h:668
uint8_t * tempImgPtCldL2
Definition: sfm_ti_alg_int.h:620
uint8_t * tempImgOgL2
Definition: sfm_ti_alg_int.h:621
Definition: sfm_ti_alg_int.h:111
uSFM_TI_MatData matData
Definition: sfm_ti_alg_int.h:542
Definition: profile.h:125
uint8_t * constCbCrBuf
Definition: sfm_ti_alg_int.h:600
void SFM_TI_activate(IALG_Handle handle)
3D pruning related parameters
Definition: sfm_ti_alg_int.h:422
void * dmaUtilsContext
Definition: sfm_ti_alg_int.h:152
#define VLIB_TRIANG_MAT_ROW
Definition: VLIB_triangulatePoints_types.h:48
uint16_t * curBlkIndx
Definition: sfm_ti_alg_int.h:624
uint16_t * curOutTrackIdx
Definition: sfm_ti_alg_int.h:609
int32_t isLocalOg
Definition: sfm_ti_alg_int.h:601
eAlgState
State of Feature Plane computation applet.
Definition: sfm_ti_alg_int.h:121
uint8_t * intAlgHandle
Definition: sfm_ti_alg_int.h:150
uint8_t * dofBufInt
Definition: sfm_ti_alg_int.h:610
sSFM_TI_EqInitMat eqInitMat
Definition: sfm_ti_alg_int.h:441
SFM_TI_trackInfo * track
Definition: sfm_ti_alg_int.h:592
#define VLIB_TRIANG_MAT_COL
Definition: VLIB_triangulatePoints_types.h:52
int32_t SFM_TI_alloc(const IALG_Params *params, struct IALG_Fxns **parentFxns, IALG_MemRec *memRec)
uint8_t * constYBuf
Definition: sfm_ti_alg_int.h:599
Definition: sfm_ti_alg_int.h:107
int32_t dofBlockBufSize
Definition: sfm_ti_alg_int.h:669
TI_DSP_PrfInfo prfInfo
Definition: sfm_ti_alg_int.h:676
int8_t * ogBuf
Definition: sfm_ti_alg_int.h:597
eMemrecs
Memory records for Feature Plane classification applet.
Definition: sfm_ti_alg_int.h:101
Place holder for initial data matrices. From this data matrix Pseudo AtA and Pseudo Atb matrices are ...
Definition: sfm_ti_alg_int.h:405
int32_t SFM_TI_control(IVISION_Handle handle, IALG_Cmd cmd, const IALG_Params *inParams, IALG_Params *outParams)
uint16_t * blkIndx
Definition: sfm_ti_alg_int.h:623
float * prevPoints
Definition: sfm_ti_alg_int.h:616
TI_DSP_PrfInfo * prfInfo
uint32_t curFrameNum
Definition: sfm_ti_alg_int.h:661
uint16_t num3DPoints
Definition: sfm_ti_alg_int.h:660
sSfm_TI_L2DMem * sfmL2Prm
Definition: sfm_ti_alg_int.h:679
This structure defines the format in which SFM module expects a particular feature point&#39;s tracked lo...
Definition: sfm_ti_alg_int.h:358
#define CAM_EXT_PRM_BUF_DEPTH
Definition: sfm_ti_alg_int.h:52
uint16_t profileEn
Definition: sfm_ti_alg_int.h:665
uint8_t * isInlier
Definition: sfm_ti_alg_int.h:613
#define NUM_VECTORS_TOGATHER
Definition: sfm_ti_alg_int.h:62
uint8_t idx
Definition: sfm_ti_alg_int.h:361
int32_t maxNumTracks
Definition: sfm_ti_alg_int.h:663
void SFM_TI_deactivate(IALG_Handle handle)
float * firstCamExtPrmL1
Definition: sfm_ti_alg_int.h:650
Definition: sfm_ti_alg_int.h:123
IVISION_Fxns * ivision
Definition: sfm_ti_alg_int.h:143
uint8_t curLargeBufIndx
Definition: sfm_ti_alg_int.h:662
uint16_t * curInTrackIdx
Definition: sfm_ti_alg_int.h:608
Definition: sfm_ti_alg_int.h:106
void SFM_TI_updatePtrs(sSfm_TI_L1DMem *sfmL1Prm, sSfm_TI_L2DMem *sfmL2Prm, sSfm_TI_ExtMem *sfmPrm, SFM_TI_CreateParams *createParams)
SFM_TI_trackInfo * trackInt
Definition: sfm_ti_alg_int.h:593
int32_t trackIndxBufSize
Definition: sfm_ti_alg_int.h:667
Definition: sfm_ti_alg_int.h:114
SFM_TI_output * sfmOutputL2
Definition: sfm_ti_alg_int.h:619
uint8_t * curFeatInTrack
Definition: sfm_ti_alg_int.h:630
uint8_t * keyPointBitMap
Definition: sfm_ti_alg_int.h:607
Definition: sfm_ti_alg_int.h:113
#define VLIB_TRIANG_NUM_UNIQUE_ELEMENTS_IN_ATA
Definition: VLIB_triangulatePoints_types.h:44
Definition: sfm_ti_alg_int.h:110
#define NUM_TRACKS_TOGATHER
Definition: sfm_ti_alg_int.h:67
#define SE_PARAM_WORDS
Definition: sfm_ti_alg_int.h:486
uint8_t age
Definition: sfm_ti_alg_int.h:360
L2 scratch memory definition. Base address of every element in below structure is double word aligned...
Definition: sfm_ti_alg_int.h:530
#define DDR_SCRATCH_BUF_SIZE
Definition: sfm_ti_alg_int.h:78
int32_t SFM_TI_process(IVISION_Handle handle, IVISION_InBufs *inBufs, IVISION_OutBufs *outBufs, IVISION_InArgs *inArgs, IVISION_OutArgs *outArgs)
Definition: sfm_ti_alg_int.h:124
uint16_t * trackId
Definition: sfm_ti_alg_int.h:629
void * udmaDrvObj
Definition: sfm_ti_alg_int.h:151
#define MAX_NUM_STRM_TEMPLATE
Definition: sfm_ti_alg_int.h:484
uint8_t * colorIndx
Definition: sfm_ti_alg_int.h:622
Buffers required for equation solving (AtA * x = Atb) in triangulation Since total NUM_TRACKS_TOGATHE...
Definition: sfm_ti_alg_int.h:383
float * curPoints
Definition: sfm_ti_alg_int.h:615
int32_t numDofBlocks
Definition: sfm_ti_alg_int.h:671
This structure contains all the parameters which controls SFM algorithm at create time...
Definition: tiadalg_structure_from_motion.h:202
#define MAX_NUM_FRAMES_TO_TRACK
Definition: tiadalg_structure_from_motion.h:77
int32_t numRowInPtCldVisBuf
Definition: sfm_ti_alg_int.h:673
int32_t numRowsInDofBlock
Definition: sfm_ti_alg_int.h:670
sSfm_TI_L1DMem * sfmL1Prm
Definition: sfm_ti_alg_int.h:678
#define VLIB_TRIANG_NUM_TRACKS_IN_ONE_VECTOR
Definition: VLIB_triangulatePoints_types.h:37
float * camIntPrmL1
Definition: sfm_ti_alg_int.h:646
int32_t SFM_TI_init(IALG_Handle handle, const IALG_MemRec *memRec, IALG_Handle parent, const IALG_Params *params)
int32_t SFM_TI_free(IALG_Handle handle, IALG_MemRec *memRec)
#define VLIB_TRIANG_CAMERA_EXTRENSIC_PARAM_ASIZE
Definition: VLIB_triangulatePoints_types.h:22
SFM_TI_CreateParams createParams
Definition: sfm_ti_alg_int.h:148
SFM_TI_Obj * SFM_TI_Handle
Definition: sfm_ti_alg_int.h:159
void * fMatParams
Definition: sfm_ti_alg_int.h:664
float * camInvIntPrmL1
Definition: sfm_ti_alg_int.h:648
This structure defines the format of output buffer given out from algorithm.
Definition: tiadalg_structure_from_motion.h:416
int8_t * ogBufInt
Definition: sfm_ti_alg_int.h:598

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