TI Autonomous Driving Algorithms (TIADALG) Library User Guide
sfm_ti_math.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 
20 #ifndef TI_SFM_MATH_H
21 #define TI_SFM_MATH_H
22 
23 #include "float.h"
24 
25 static inline float16 VXLIB_oneByXVecF32(float16 vX);
26 static inline float16 VXLIB_oneBySqrtXVecF32(float16 vX);
27 
28 #if HOST_EMULATION
29 /* float absolute*/
30 #define _fabsf fabs
31 
32 /*double absolute*/
33 #define _fabs fabs
34 #endif
35 
36 #define DSP_INTRINSICS
37 
38 #ifdef DSP_INTRINSICS
39 #define DSP_INTRINSIC_FOR_RECIPORCAL
40 //#define RCP_BOUND_CHK
41 #endif
42 
43 #define VLIB_F32 float
44 #define VLIB_D64 double
45 #define Void void
46 #define VLIB_OneByXF32 my_OneByX
47 #define VLIB_OneByX1X0F32 my_OneByX1X0
48 #define VLIB_SqrtXF32 my_SqrtX
49 
50 #ifndef __mmax
51 #define __mmax(x,y) ((x>y)?x:y)
52 #endif
53 
54 #ifndef __mmin
55 #define __mmin(x,y) ((x>y)?y:x)
56 #endif
57 
58 #ifndef SWAP_ME
59 #define SWAP_ME(X,Y) {temp=(X);(X)=(Y);(Y)=temp;}
60 #endif
61 
62 #define SIGN(a, b) ((b) >= 0.0 ? _fabsf(a) : -_fabsf(a))
63 
64 #ifndef bool
65 #define bool uint8_t
66 #endif
67 
68 #ifndef true
69 #define true 1
70 #endif
71 
72 #ifndef false
73 #define false 0
74 #endif
75 
76 
77 #define SFM_TI_PI (3.14159265358979323846f)
78 
79 static inline float my_OneByX(float x)
80 {
81 #ifdef DSP_INTRINSIC_FOR_RECIPORCAL
82  float TWO = 2.0f;
83  float X;
84 
85 #ifdef RCP_BOUND_CHK
86  float Big = FLT_MAX;
87  float Y;
88  Y = _fabsf(x);
89 #endif
90 
91  X = __recip(x);
92  X = X * (TWO - x*X);
93  X = X * (TWO - x*X);
94 
95 #ifdef RCP_BOUND_CHK
96  if (Y > Big) {
97  X = 0.0f;
98  }
99  if((x == 0.0f) | (x == -0.0f))
100  X = Big;
101 #endif
102 
103  return (X);
104 #else
105  float den;
106  den = 1/x;
107  return(den);
108 #endif
109 }
110 
111 static inline float my_OneBySqrtX(float x)
112 {
113 #ifdef DSP_INTRINSIC_FOR_RECIPORCAL
114  float normInv, val;
115 
116  normInv = __recip_sqrt(x);
117  val = normInv*(3.0f-x*normInv*normInv)*0.5f;
118  normInv = val*(3.0f-x*val*val)*0.5f;
119 #ifdef RCP_BOUND_CHK
120  @TODO enable boundry check
121 #endif
122 
123  return (normInv);
124 #else
125  return((1.0/sqrtf(x)));
126 #endif
127 }
128 
129 static inline float16 VXLIB_oneByXVecF32(float16 vX)
130 {
131 
132  float16 vX0;
133 
134 #ifdef DSP_INTRINSIC_FOR_RECIPORCAL
135  float16 f2Pkd = (float16)2.0f;
136 
137  vX0 = __recip(vX);
138 
139  vX0 = vX0 * (f2Pkd - vX0 * vX);
140 
141  vX0 = vX0 * (f2Pkd - vX0 * vX);
142 #ifdef RCP_BOUND_CHK
143  __vpred vp;
144  float16 vY;
145 
146  vY = __abs(vX);
147 
148  /* If input value is higher or equal to FLT_MAX
149  */
150  vp = __cmp_le_pred((float16)FLT_MAX, vY);
151 
152  vX0 = __select(vp,(float16)FLT_MIN,vX0);
153 
154  /* If input value is lesser or equal to FLT_MIN
155  */
156  vp = __cmp_le_pred(vY, (float16)FLT_MIN);
157 
158  vX0 = __select(vp,(float16)FLT_MAX,vX0);
159 #endif
160 #else
161  vX0.s0 = 1.0f/vX.s0; vX0.s1 = 1.0f/vX.s1; vX0.s2 = 1.0f/vX.s2; vX0.s3 = 1.0f/vX.s3;
162  vX0.s4 = 1.0f/vX.s4; vX0.s5 = 1.0f/vX.s5; vX0.s6 = 1.0f/vX.s6; vX0.s7 = 1.0f/vX.s7;
163  vX0.s8 = 1.0f/vX.s8; vX0.s9 = 1.0f/vX.s9; vX0.sa = 1.0f/vX.sa; vX0.sb = 1.0f/vX.sb;
164  vX0.sc = 1.0f/vX.sc; vX0.sd = 1.0f/vX.sd; vX0.se = 1.0f/vX.se; vX0.sf = 1.0f/vX.sf;
165 #endif
166  return (vX0);
167 }
168 static inline float16 VXLIB_oneBySqrtXVecF32(float16 vX)
169 {
170  float16 vX0;
171  float16 f3Pkd = (float16)3.0f;
172 #ifdef DSP_INTRINSIC_FOR_RECIPORCAL
173  vX0 = __recip_sqrt(vX);
174 
175  vX0 = vX0 * (f3Pkd - vX * vX0 * vX0) * (float16)0.5f;
176  vX0 = vX0 * (f3Pkd - vX * vX0 * vX0) * (float16)0.5f;
177 
178 #ifdef RCP_BOUND_CHK
179  __vpred vp;
180  float16 vY;
181 
182  vY = __abs(vX);
183 
184  /* If input value is higher or equal to FLT_MAX
185  */
186  vp = __cmp_le_pred((float16)FLT_MAX, vY);
187 
188  vX0 = __select(vp,(float16)FLT_MIN,vX0);
189 
190  /* If input value is lesser or equal to FLT_MIN
191  */
192  vp = __cmp_le_pred(vY, (float16)FLT_MIN);
193 
194  vX0 = __select(vp,(float16)FLT_MAX,vX0);
195 
196 
197  /* If input is negative then this is undefined scenarion.
198  * For now just making output as FLT_MIN in this undefined scenarion.
199  */
200  vp = __cmp_le_pred(vX, (float16)0.0f);
201 
202  vX0 = __select(vp,(float16)FLT_MIN,vX0);
203 #endif
204 
205 #else
206  vX0.s0 = 1.0f/sqrt(vX.s0); vX0.s1 = 1.0f/sqrt(vX.s1); vX0.s2 = 1.0f/sqrt(vX.s2); vX0.s3 = 1.0f/sqrt(vX.s3);
207  vX0.s4 = 1.0f/sqrt(vX.s4); vX0.s5 = 1.0f/sqrt(vX.s5); vX0.s6 = 1.0f/sqrt(vX.s6); vX0.s7 = 1.0f/sqrt(vX.s7);
208  vX0.s8 = 1.0f/sqrt(vX.s8); vX0.s9 = 1.0f/sqrt(vX.s9); vX0.sa = 1.0f/sqrt(vX.sa); vX0.sb = 1.0f/sqrt(vX.sb);
209  vX0.sc = 1.0f/sqrt(vX.sc); vX0.sd = 1.0f/sqrt(vX.sd); vX0.se = 1.0f/sqrt(vX.se); vX0.sf = 1.0f/sqrt(vX.sf);
210 #endif
211  return (vX0);
212 }
213 static inline float my_YByX(float y, float x)
214 {
215  return(y*my_OneByX(x));
216 }
217 
218 static inline float my_SqrtX(float x)
219 {
220  return (x * my_OneBySqrtX(x));
221 }
222 
223 static inline float16 approxAtan(float16 z)
224 {
225  float16 n1 = (float16)0.97239411f;
226  float16 n2 = (float16)-0.19194795f;
227  return (n1 + n2 * z * z) * z;
228 }
229 #endif
static float my_OneBySqrtX(float x)
Definition: sfm_ti_math.h:111
static float16 approxAtan(float16 z)
Definition: sfm_ti_math.h:223
static float my_YByX(float y, float x)
Definition: sfm_ti_math.h:213
static float my_OneByX(float x)
Definition: sfm_ti_math.h:79
static float16 VXLIB_oneByXVecF32(float16 vX)
Definition: sfm_ti_math.h:129
static float16 VXLIB_oneBySqrtXVecF32(float16 vX)
Definition: sfm_ti_math.h:168
static float my_SqrtX(float x)
Definition: sfm_ti_math.h:218

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