Squashed commit of the sb-vbs branch.

Includes the SD-VBS benchmarks modified to:
- Use libextra to loop as realtime jobs
- Preallocate memory before starting their main computation
- Accept input via stdin instead of via argc

Does not include the SD-VBS matlab code.

Fixes libextra execution in LITMUS^RT.

1 files changed, 156 insertions, 0 deletions

diff --git a/SD-VBS/benchmarks/tracking/src/c/getANMS.c b/SD-VBS/benchmarks/tracking/src/c/getANMS.c
new file mode 100644
index 0000000..7ecce8b
--- /dev/null
+++ b/SD-VBS/benchmarks/tracking/src/c/getANMS.c
@@ -0,0 +1,156 @@
+/********************************
+Author: Sravanthi Kota Venkata
+********************************/
+
+#include "tracking.h"
+
+/** ANMS - Adaptive Non-Maximal Suppression
+    This function takes features as input and suppresses those which
+    are close to each other (within the SUPPRESSION_RADIUS) and have
+    similar feature strength
+**/
+F2D *getANMS (F2D *points, float r)
+{
+    float MAX_LIMIT = 100000;
+    F2D *suppressR;
+    float C_ROBUST = 1.0;
+    F2D *srtdPnts;
+    int n;
+    I2D *srtdVIdx, *supId;
+    float t, t1, r_sq;
+    F2D *tempF, *srtdV, *interestPnts;
+    int i, j, validCount, cnt, end, k;
+    int iter, rows, cols;
+    F2D *temp;
+    int supIdPtr = 0;
+    
+    /** Concatenate x,y,v to form points matrix **/
+    r_sq = r * r;
+    n = points->height;
+
+    srtdVIdx = iMallocHandle(points->height, 1);
+    for (i = 0; i < srtdVIdx->height; i++) {
+        asubsref(srtdVIdx,i) = i;
+        }
+    srtdPnts = fMallocHandle (srtdVIdx->height, points->width);
+    for (i = 0; i < srtdVIdx->height; i++) {
+        for(j=0; j<points->width; j++) {
+            subsref(srtdPnts,i,j) = subsref(points, asubsref(srtdVIdx,i), j);
+                }
+        }
+    temp = fSetArray (1, 3, 0);
+    suppressR = fSetArray(n, 1, MAX_LIMIT);
+    validCount = n;
+    iter = 0;
+    
+    /** Allocate supId for #validCount and fill the values of
+        supId with the indices where suppressR>r_sq **/
+    k = 0;
+    supId = iMallocHandle(validCount, 1);
+    for (i = 0; i < (suppressR->height*suppressR->width); i++)
+    {
+        if ( asubsref(suppressR,i) > r_sq)
+        {
+            asubsref(supId,k++) = i;
+        }
+    }
+         
+    /** While number of features not-inspected is >0,  **/
+    while (validCount > 0)
+    {
+        F2D *tempp, *temps;
+        
+        /** Inspect the strongest feature point in srtdPnts
+            The index of that feature is in supId and the
+            index values in supId are arranged in descending order **/
+        asubsref(temp,0) = subsref(srtdPnts, asubsref(supId,0), 0);
+        asubsref(temp,1) = subsref(srtdPnts, asubsref(supId,0), 1);
+        asubsref(temp,2) = subsref(srtdPnts, asubsref(supId,0), 2);
+       
+        /** Stacking up the interestPnts matrix with top features
+            post suppression **/
+        if(iter == 0)
+            interestPnts = fDeepCopy(temp);
+        else
+        {
+            tempp = fDeepCopy(interestPnts);
+            fFreeHandle(interestPnts); 
+            interestPnts = ffVertcat(tempp, temp);
+            fFreeHandle(tempp);
+        }
+        iter++;
+ 
+        tempp = fDeepCopy(srtdPnts);
+        temps = fDeepCopy(suppressR);
+
+        fFreeHandle(srtdPnts);
+        fFreeHandle(suppressR);
+
+        /** Remove the feature that has been added to interestPnts **/
+        srtdPnts = fMallocHandle(supId->height-1, 3);
+        suppressR = fMallocHandle(supId->height-1, 1);
+        k=0;
+
+        for(i=1; i<(supId->height); i++)    /** Filling srtdPnts after removing the feature that was added to interestPnts**/
+        {
+            subsref(srtdPnts,(i-1),0) = subsref(tempp, asubsref(supId,i) ,0);
+            subsref(srtdPnts,(i-1),1) = subsref(tempp, asubsref(supId,i) ,1);
+            subsref(srtdPnts,(i-1),2) = subsref(tempp, asubsref(supId,i) ,2);
+            subsref(suppressR,(i-1),0) = subsref(temps, asubsref(supId,i) ,0);
+        }
+         
+        fFreeHandle(tempp);
+        fFreeHandle(temps);
+        rows = interestPnts->height-1;
+        cols = interestPnts->width;
+
+        /** For each feature, find how robust it is compared to the one in interestPnts **/
+        for (i = 0; i < srtdPnts->height; i++)
+            {
+            t = 0;
+                t1 = 0;
+
+                if ((C_ROBUST * subsref(interestPnts,rows,2)) >= subsref(srtdPnts, i,2))
+                {
+                        t = subsref(srtdPnts, i,0) - subsref(interestPnts,rows,0);
+                        t1 = subsref(srtdPnts, i,1) - subsref(interestPnts,rows,1);
+                        t = t * t + t1 * t1;
+                t1 = 0;
+            }
+
+                if ((C_ROBUST * subsref(interestPnts,rows,2)) < subsref(srtdPnts, i,2))
+                t1 = 1 * MAX_LIMIT;
+
+                if ( asubsref(suppressR, i) > (t + t1))
+            {
+                    asubsref(suppressR, i) = t + t1;
+            }  
+        }
+        
+        /** Inspect the new suppressR to find how many valid features left **/
+        validCount=0;
+        for (i = 0; i < suppressR->height; i++) {
+                if ( asubsref(suppressR,i) > r_sq) {
+                validCount++;
+                        } 
+                }
+        k = 0;
+        iFreeHandle(supId);
+        /** Allocate supId for #validCount and fill the values of
+        supId with the indices where suppressR>r_sq **/
+        supId = iMallocHandle(validCount, 1);
+        for (i = 0; i < suppressR->height*suppressR->width; i++) {
+            if ( asubsref(suppressR,i) > r_sq)
+                asubsref(supId,k++) = i;
+                }
+    }
+    
+    iFreeHandle(supId);
+    iFreeHandle(srtdVIdx);
+    fFreeHandle(srtdPnts);
+    fFreeHandle(temp);
+    fFreeHandle(suppressR);
+
+    return interestPnts;
+}
+