Perception Tool Kit (PTK) API Guide
stereo_algo.h
Go to the documentation of this file.
1 /*
2  *
3  * Copyright (c) 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 #ifndef _STEREO_ALGO_H_
63 #define _STEREO_ALGO_H_
64 
65 #ifdef __cplusplus
66 extern "C" {
67 #endif
68 
69 #include <perception/perception.h>
72 #include <perception/base/matrix.h>
73 
74 
82 #define AUX_CODE_REMOVE 0
83 
84 #define TEST_MAX_FILE_PATH (256u)
85 #define MAX_FRAME_WIDTH (1920u)
86 #define MAX_FRAME_HEIGHT (1080u)
87 
88 #define ALGORITHM_PROCESS_OK 0
89 #define ALGORITHM_PROCESS_FAIL -1
90 
91 #define PI 3.14159265358979f
92 
93 
94 #define MMAX(a,b) ((a > b) ? (a) : (b))
95 #define MMIN(a,b) ((a < b) ? (a) : (b))
96 
101 #define NUM_GP_MODELS 1
102 
103 #define NUM_FRAC_BITS 4
104 
105 
110 typedef struct
111 {
113  int32_t width;
114 
116  int32_t height;
117 
119  int32_t minDisparity;
120 
122  int32_t maxDisparity;
123 
130 
132  float distCenterX;
133 
135  float distCenterY;
136 
138  float focalLength;
139 
141 
142 
143 
148 typedef struct
149 {
151  int32_t roadCenterBase[2];
152 
154  int32_t roadCenterTop[2];
155 
157  int32_t roadSizeBase;
158 
160  int32_t roadSizeTop;
161 
163  int32_t maxDepth;
164 
166  int32_t egoSize;
167 
169 
170 
175 typedef struct
176 {
178  int32_t camHeight;
179 
181  int32_t width;
182 
184  int32_t height;
185 
186  /* Camera roll angle */
187  float camRoll;
188 
189  /* Camera pitch angle */
190  float camPitch;
191 
192  /* Camera yaw angle */
193  float camYaw;
194 
195  /* sine of camera pitch */
196  float sinPitch;
197 
198  /* cosine of camera pitch */
199  float cosPitch;
200 
202  float baseline;
203 
205  float dcx;
206 
208  float dcy;
209 
211  float f;
212 
215 
218 
220 
221 
226 typedef struct
227 {
231  int8_t confTh;
232 
235  int8_t existConf;
236 
240  int32_t minDisparity;
241 
245  int32_t maxDisparity;
246 
248  int32_t numDisparity;
249 
251  int32_t fracBits;
252 
256  int32_t farZ;
257 
259 
264 typedef struct
265 {
269  int8_t dsFactor;
270 
272  uint8_t useRoadParams;
273 
276 
280  int32_t roadWidth;
281 
283  int32_t vdWidth;
284 
286  int32_t vdHeight;
287 
289  int32_t startX;
290 
292  int32_t lastX;
293 
295  int32_t startY;
296 
298  int32_t lastY;
299 
301  int32_t ransacIter;
302 
304  float ransacErrTh;
305 
307 
308 
313 typedef struct
314 {
318  uint8_t dsFactor;
319 
321  int32_t dsWidth;
322 
324  int32_t dsHeight;
325 
328 
331 
334 
337 
340 
342 
347 typedef struct
348 {
351 
356 
358 
359 
364 typedef struct
365 {
367  float * gpThArray;
368 
369 #if AUX_CODE_REMOVE
370  float * groundErrArray[2];
371 #endif
372 
374  float * yOffsetArray;
375 
377  float * objHeightArray;
378 
380 
381 
382 
387 typedef struct
388 {
390  float slopeArray[5];
391 
393  float interceptArray[5];
394 
396  int32_t frameNo;
397 
401  int32_t maxFrameNo;
402 
404 
405 
410 typedef struct
411 {
414 
416  int8_t numModels;
417 
419 
420 
425 typedef struct
426 {
428  float slope;
429 
431  float intercept;
432 
433 #if AUX_CODE_REMOVE
434  float * gpModelLine[4];
435  int32_t * igpModel;
436 #endif
437 
439  int8_t validGM;
440 
442 
443 
448 typedef struct {
451 
453  int8_t numModels;
455 
456 
457 
462 typedef struct
463 {
465  uint8_t * gpModelIdx;
466 
468  float * dsDisparity;
469 
471  int32_t * candidateFound;
472 
474  int32_t * numValidFound;
475 
478 
481 
484 
486  int32_t * obtacleBottomLine;
487 
489 
490 
503  PTK_Alg_StereoAlgo_obsProbParams * copppParams,
504  PTK_Alg_StereoAlgo_camParams * camParams,
507 
521  PTK_Alg_StereoAlgo_obsProbParams * copppParams,
522  PTK_Alg_StereoAlgo_camParams * camParams,
525  int32_t horizon);
526 
537  PTK_Alg_StereoAlgo_obsProbParams * copppParams,
540 
552  PTK_Alg_StereoAlgo_obsProbParams * copppParams,
553  int16_t * disparity,
554  float * dsDisparity);
555 
569  int32_t height,
570  int8_t existConf,
571  int8_t threshold,
572  int16_t *disparity,
573  int16_t *outDisparity);
574 
575 
589  float x,
590  float y);
591 
606  PTK_Alg_StereoAlgo_camParams * camParams,
607  int32_t x,
608  float disparity);
609 
621 float PTK_Alg_StereoAlgo_disparity2Depth(PTK_Alg_StereoAlgo_camParams * camParams, float disparity, float ypos);
622 
623 
635 int32_t PTK_Alg_StereoAlgo_randRange(int32_t min, int32_t max);
636 
647 void PTK_Alg_StereoAlgo_transpose(float* M, int32_t M_width, int32_t M_height, float* Mt);
648 
649 
665 void PTK_Alg_StereoAlgo_multiplyMatrix(float* iM1, int32_t iM1_width, int32_t iM1_height,
666  float *iM2, int32_t iM2_width, int32_t iM2_height,
667  float* oM, int32_t oM_width, int32_t oM_height);
668 
684 int32_t PTK_Alg_StereoAlgo_LUdecomposition(float* A, int32_t A_width, int32_t A_height, float* b, int32_t b_width, int32_t n_toCompute);
685 
686 
687 /* PRIVATE APIS (not visible to OVX node) */
688 
689 #ifdef __cplusplus
690 }
691 #endif
692 
693 #endif // _STEREO_ALGO_H_
float slope
Definition: stereo_algo.h:428
int32_t height
Definition: stereo_algo.h:116
uint8_t useRoadParams
Definition: stereo_algo.h:272
Stereo algorithm - V-disparity ground plane model.
Definition: stereo_algo.h:425
Stereo algorithm - V-disparity ground plane model histroy array for muliple models.
Definition: stereo_algo.h:410
int32_t farZ
Definition: stereo_algo.h:256
float ransacErrTh
Definition: stereo_algo.h:304
int32_t ransacIter
Definition: stereo_algo.h:301
int32_t obsProbWindowHeight
Definition: stereo_algo.h:327
int32_t obsProbWindowWidth
Definition: stereo_algo.h:330
int32_t * numValidFound
Definition: stereo_algo.h:474
int32_t startX
Definition: stereo_algo.h:289
void PTK_Alg_StereoAlgo_multiplyMatrix(float *iM1, int32_t iM1_width, int32_t iM1_height, float *iM2, int32_t iM2_width, int32_t iM2_height, float *oM, int32_t oM_width, int32_t oM_height)
Multiply two matrices.
int8_t dsFactor
Definition: stereo_algo.h:269
int32_t maxFrameNo
Definition: stereo_algo.h:401
int32_t height
Definition: stereo_algo.h:184
Definition of a 3x3 matrix.
Definition: matrix.h:119
int32_t * candidateFoundVertical
Definition: stereo_algo.h:477
float cosPitch
Definition: stereo_algo.h:199
int32_t numDisparity
Definition: stereo_algo.h:248
int8_t confTh
Definition: stereo_algo.h:231
float dcx
Definition: stereo_algo.h:205
float * obstacleProbability
Definition: stereo_algo.h:483
uint8_t * gpModelIdx
Definition: stereo_algo.h:465
#define NUM_GP_MODELS
Definition: stereo_algo.h:101
int32_t width
Definition: stereo_algo.h:113
float focalLength
Definition: stereo_algo.h:138
int32_t frameNo
Definition: stereo_algo.h:396
Stereo algorithm - road Parameters.
Definition: stereo_algo.h:175
float camRoll
Definition: stereo_algo.h:187
float f
Definition: stereo_algo.h:211
int32_t width
Definition: stereo_algo.h:181
float baseline
Definition: stereo_algo.h:202
int32_t dsWidth
Definition: stereo_algo.h:321
int32_t maxDisparity
Definition: stereo_algo.h:245
Stereo algorithm - V-disparity ground plane model array for multiple models.
Definition: stereo_algo.h:448
int32_t * obtacleBottomLine
Definition: stereo_algo.h:486
Stereo algorithm - road Parameters.
Definition: stereo_algo.h:148
int32_t PTK_Alg_StereoAlgo_computeObstacleProbalilityPerPixel(PTK_Alg_StereoAlgo_obsProbParams *copppParams, PTK_Alg_StereoAlgo_camParams *camParams, PTK_Alg_StereoAlgo_groundModelParams_Array *gmParamsArr, PTK_Alg_StereoAlgo_obstacleDetetionMems *algoMemData)
Compute probabilities of pixels to be obstacle configuration parameters.
int32_t * candidateFound
Definition: stereo_algo.h:471
float * objHeightArray
Definition: stereo_algo.h:377
void PTK_Alg_StereoAlgo_filterLowConfDisparity(int32_t width, int32_t height, int8_t existConf, int8_t threshold, int16_t *disparity, int16_t *outDisparity)
Filter out low-confidence disparities.
Stereo algorithm - disparity parameters.
Definition: stereo_algo.h:226
void PTK_Alg_StereoAlgo_downsampleDisparity(PTK_Alg_StereoAlgo_configParams *config, PTK_Alg_StereoAlgo_obsProbParams *copppParams, int16_t *disparity, float *dsDisparity)
Downsample input disparity map.
int32_t PTK_Alg_StereoAlgo_LUdecomposition(float *A, int32_t A_width, int32_t A_height, float *b, int32_t b_width, int32_t n_toCompute)
LU decomposition.
Stereo algorithm - V-disparity ground plane model histroy.
Definition: stereo_algo.h:387
Stereo algorithm - Data mems needed for algorithms.
Definition: stereo_algo.h:462
float minDisparityDifferenceFromModel
Definition: stereo_algo.h:339
float distCenterY
Definition: stereo_algo.h:135
int32_t minDisparity
Definition: stereo_algo.h:119
Declarations for PTK_Matrix struct+functions.
int32_t minDisparity
Definition: stereo_algo.h:240
int32_t roadSizeBase
Definition: stereo_algo.h:157
float PTK_Alg_StereoAlgo_getDisparityFromModel(PTK_Alg_StereoAlgo_groundModelParams *gmParams, float x, float y)
Get disparity for input position (X, y) from a ground plane model.
int8_t numModels
Definition: stereo_algo.h:416
int32_t * numValidFoundVertical
Definition: stereo_algo.h:480
int32_t fracBits
Definition: stereo_algo.h:251
uint8_t dsFactor
Definition: stereo_algo.h:318
int32_t vdWidth
Definition: stereo_algo.h:283
float dcy
Definition: stereo_algo.h:208
Stereo algorithm - ground plane parameters.
Definition: stereo_algo.h:364
int32_t maxDepth
Definition: stereo_algo.h:163
float distCenterX
Definition: stereo_algo.h:132
float PTK_Alg_StereoAlgo_disparity2Depth(PTK_Alg_StereoAlgo_camParams *camParams, float disparity, float ypos)
Convert disparity to depth.
int32_t PTK_Alg_StereoAlgo_findYforDisparity(PTK_Alg_StereoAlgo_groundModelParams *gmParams, PTK_Alg_StereoAlgo_camParams *camParams, int32_t x, float disparity)
Get y position of a pixel for the given horizontal position and disparity.
int32_t egoSize
Definition: stereo_algo.h:166
int32_t PTK_Alg_StereoAlgo_randRange(int32_t min, int32_t max)
Generate random number between two numbers.
int8_t numModels
Definition: stereo_algo.h:453
Stereo algorithm - config Parameters.
Definition: stereo_algo.h:110
int32_t maxDisparity
Definition: stereo_algo.h:122
PTK_3D_Matrix invK
Definition: stereo_algo.h:217
int32_t lastX
Definition: stereo_algo.h:292
int32_t roadWidth
Definition: stereo_algo.h:280
float intercept
Definition: stereo_algo.h:431
int32_t minConsecutiveVertical
Definition: stereo_algo.h:355
int8_t validGM
Definition: stereo_algo.h:439
int32_t startY
Definition: stereo_algo.h:295
int32_t camHeight
Definition: stereo_algo.h:178
int32_t roadSizeTop
Definition: stereo_algo.h:160
int32_t dsHeight
Definition: stereo_algo.h:324
uint8_t adaptiveGroundError
Definition: stereo_algo.h:275
float * gpThArray
Definition: stereo_algo.h:367
Stereo algorithm - bottom of obstacle parameter.
Definition: stereo_algo.h:347
int32_t minSearchBoxSize
Definition: stereo_algo.h:333
int32_t vdHeight
Definition: stereo_algo.h:286
PTK_3D_Matrix K
Definition: stereo_algo.h:214
float camYaw
Definition: stereo_algo.h:193
float minProbability
Definition: stereo_algo.h:350
int8_t sde_confidence_threshold
Definition: stereo_algo.h:129
int32_t pixelAboveHorizonToCheck
Definition: stereo_algo.h:336
float * dsDisparity
Definition: stereo_algo.h:468
float camPitch
Definition: stereo_algo.h:190
Stereo algorithm - obstacle probability (per pixel) parameters.
Definition: stereo_algo.h:313
Stereo algorithm - uvDisparity parameters.
Definition: stereo_algo.h:264
float sinPitch
Definition: stereo_algo.h:196
void PTK_Alg_StereoAlgo_findBottomOfObstacle(PTK_Alg_StereoAlgo_obsProbParams *copppParams, PTK_Alg_StereoAlgo_bottomObsParams *fbooParams, PTK_Alg_StereoAlgo_obstacleDetetionMems *algoMemData)
Find the closest y positions where obstacles are on every column of an image.
int8_t existConf
Definition: stereo_algo.h:235
void PTK_Alg_StereoAlgo_computeObstacleProbabilityPerPixel_verticalLoop(PTK_Alg_StereoAlgo_obsProbParams *copppParams, PTK_Alg_StereoAlgo_camParams *camParams, PTK_Alg_StereoAlgo_groundModelParams_Array *gmParamsArr, PTK_Alg_StereoAlgo_obstacleDetetionMems *algoMemData, int32_t horizon)
Compute probabilities of pixels to be obstacle configuration parameters.
int32_t lastY
Definition: stereo_algo.h:298
float * yOffsetArray
Definition: stereo_algo.h:374
void PTK_Alg_StereoAlgo_transpose(float *M, int32_t M_width, int32_t M_height, float *Mt)
Generate random number between two numbers.