00001
00007 #include "mex.h"
00008 #include <mexutils.h>
00009 #include <vl/kdtree.h>
00010
00026 void
00027 restore_parent_recursively (VlKDTree * tree, int nodeIndex, int * numNodesToVisit)
00028 {
00029 VlKDTreeNode * node = tree->nodes + nodeIndex ;
00030 int lowerChild = node->lowerChild ;
00031 int upperChild = node->upperChild ;
00032
00033 if (*numNodesToVisit == 0) {
00034 vlmxError (vlmxErrInconsistentData,
00035 "FOREST.TREES has an inconsitsent tree structure.") ;
00036 }
00037
00038 *numNodesToVisit -= 1 ;
00039
00040 if (lowerChild >= 0) {
00041 VlKDTreeNode * child = tree->nodes + lowerChild ;
00042 child->parent = nodeIndex ;
00043 restore_parent_recursively (tree, lowerChild, numNodesToVisit) ;
00044 }
00045 if (upperChild >= 0) {
00046 VlKDTreeNode * child = tree->nodes + upperChild ;
00047 child->parent = nodeIndex ;
00048 restore_parent_recursively (tree, upperChild, numNodesToVisit) ;
00049 }
00050 }
00051
00063 static mxArray *
00064 new_array_from_kdforest (VlKDForest const * forest)
00065 {
00066 vl_uindex ti ;
00067 mwSize dims [] = {1,1} ;
00068 mwSize treeDims [] = {1,0} ;
00069 char const * fieldNames [] = {
00070 "dimension",
00071 "numData",
00072 "trees",
00073 "distance"
00074 } ;
00075 char const * treeFieldNames [] = {
00076 "nodes",
00077 "dataIndex"
00078 } ;
00079 char const * nodesFieldNames [] = {
00080 "lowerChild",
00081 "upperChild",
00082 "splitDimension",
00083 "splitThreshold",
00084 "lowerBound" ,
00085 "upperBound"
00086 } ;
00087 mxArray * forest_array ;
00088 mxArray * trees_array ;
00089
00090 treeDims [0] = 1 ;
00091 treeDims [1] = forest->numTrees ;
00092 trees_array = mxCreateStructArray (2, treeDims,
00093 sizeof(treeFieldNames) / sizeof(treeFieldNames[0]),
00094 treeFieldNames) ;
00095
00096
00097
00098
00099
00100
00101 forest_array = mxCreateStructArray (2, dims, sizeof(fieldNames) / sizeof(fieldNames[0]), fieldNames) ;
00102 mxSetField (forest_array, 0, "dimension", vlmxCreatePlainScalar (forest->dimension)) ;
00103 mxSetField (forest_array, 0, "numData", vlmxCreatePlainScalar (forest->numData)) ;
00104 mxSetField (forest_array, 0, "trees", trees_array) ;
00105
00106 switch(forest->distance){
00107 case VlDistanceL1:
00108 mxSetField (forest_array, 0, "distance", mxCreateString("l1"));
00109 break;
00110 case VlDistanceL2:
00111 mxSetField (forest_array, 0, "distance", mxCreateString("l2")) ;
00112 break;
00113 default:
00114 abort();
00115 }
00116
00117 for (ti = 0 ; ti < forest->numTrees ; ++ ti) {
00118 VlKDTree * tree = forest->trees[ti] ;
00119 mxArray * nodes_array = mxCreateStructArray (2, dims, sizeof(nodesFieldNames) / sizeof(nodesFieldNames[0]), nodesFieldNames) ;
00120 mxArray * dataIndex_array = mxCreateNumericMatrix (1, forest->numData, mxUINT32_CLASS, mxREAL) ;
00121
00122 mxSetField (trees_array, ti, "nodes", nodes_array) ;
00123 mxSetField (trees_array, ti, "dataIndex", dataIndex_array) ;
00124
00125
00126
00127
00128
00129
00130
00131 {
00132 vl_uindex ni ;
00133 mxArray * lowerChild_array = mxCreateNumericMatrix (1, tree->numUsedNodes, mxINT32_CLASS, mxREAL) ;
00134 mxArray * upperChild_array = mxCreateNumericMatrix (1, tree->numUsedNodes, mxINT32_CLASS, mxREAL) ;
00135 mxArray * splitDimension_array = mxCreateNumericMatrix (1, tree->numUsedNodes, mxUINT32_CLASS, mxREAL) ;
00136 mxArray * splitThreshold_array = mxCreateNumericMatrix (1, tree->numUsedNodes, mxDOUBLE_CLASS, mxREAL) ;
00137 mxArray * lowerBound_array = mxCreateNumericMatrix (1, tree->numUsedNodes, mxDOUBLE_CLASS, mxREAL) ;
00138 mxArray * upperBound_array = mxCreateNumericMatrix (1, tree->numUsedNodes, mxDOUBLE_CLASS, mxREAL) ;
00139
00140 vl_uint32 * upperChild = mxGetData (upperChild_array) ;
00141 vl_uint32 * lowerChild = mxGetData (lowerChild_array) ;
00142 vl_uint32 * splitDimension = mxGetData (splitDimension_array) ;
00143 double * splitThreshold = mxGetData (splitThreshold_array) ;
00144 double * lowerBound = mxGetData (lowerBound_array) ;
00145 double * upperBound = mxGetData (upperBound_array) ;
00146
00147 for (ni = 0 ; ni < tree -> numUsedNodes ; ++ ni) {
00148 VlKDTreeNode const * node = tree -> nodes + ni ;
00149 int a = node->upperChild ;
00150 int b = node->lowerChild ;
00151 upperChild [ni] = (a>=0) ? a + 1 : a ;
00152 lowerChild [ni] = (b>=0) ? b + 1 : b ;
00153 splitDimension [ni] = node->splitDimension + 1 ;
00154 splitThreshold [ni] = node->splitThreshold ;
00155 lowerBound [ni] = node->lowerBound ;
00156 upperBound [ni] = node->upperBound ;
00157 }
00158 mxSetField (nodes_array, 0, "lowerChild", lowerChild_array) ;
00159 mxSetField (nodes_array, 0, "upperChild", upperChild_array) ;
00160 mxSetField (nodes_array, 0, "splitDimension", splitDimension_array) ;
00161 mxSetField (nodes_array, 0, "splitThreshold", splitThreshold_array) ;
00162 mxSetField (nodes_array, 0, "lowerBound", lowerBound_array) ;
00163 mxSetField (nodes_array, 0, "upperBound", upperBound_array) ;
00164 }
00165
00166
00167 {
00168 vl_uint32 * dataIndex = mxGetData (dataIndex_array) ;
00169 vl_uindex di ;
00170 for (di = 0 ; di < forest->numData ; ++ di) {
00171 dataIndex [di] = forest->trees[ti]->dataIndex[di].index + 1 ;
00172 }
00173 }
00174 }
00175 return forest_array ;
00176 }
00177
00178
00191 static VlKDForest *
00192 new_kdforest_from_array (mxArray const * forest_array, mxArray const * data_array)
00193 {
00194 VlKDForest * forest ;
00195 mxArray const * distance_array ;
00196 mxArray const * dimension_array ;
00197 mxArray const * numData_array ;
00198 mxArray const * trees_array ;
00199 mxArray const * nodes_array ;
00200 mxArray const * dataIndex_array ;
00201 mxArray const * lowerChild_array ;
00202 mxArray const * upperChild_array ;
00203 mxArray const * splitDimension_array ;
00204 mxArray const * splitThreshold_array ;
00205 mxArray const * lowerBound_array;
00206 mxArray const * upperBound_array;
00207
00208
00209 vl_int32 const * lowerChild ;
00210 vl_int32 const * upperChild ;
00211 vl_uint32 const * splitDimension ;
00212 double const * splitThreshold ;
00213 double const * upperBound ;
00214 double const * lowerBound ;
00215
00216 vl_uindex ti ;
00217 int unsigned dimension ;
00218 VlVectorComparisonType distance;
00219 vl_size numData ;
00220 vl_size numUsedNodes ;
00221 vl_size numTrees ;
00222
00223 vl_size maxNumNodes = 0;
00224
00225 vl_type dataType ;
00226
00227
00228
00229
00230
00231
00232
00233
00234 distance_array = mxGetField (forest_array, 0, "distance") ;
00235 if(distance_array && vlmxIsString (distance_array, -1)){
00236 if (vlmxCompareToStringI(distance_array, "l1") == 0) {
00237 distance = VlDistanceL1 ;
00238 } else if (vlmxCompareToStringI(distance_array, "l2") == 0) {
00239 distance = VlDistanceL2 ;
00240 } else {
00241 vlmxError(vlmxErrInconsistentData,
00242 "FOREST.DISTANCE must be either 'l1' or 'l2'.") ;
00243 }
00244 } else {
00245 vlmxError(vlmxErrInconsistentData,
00246 "FOREST.DISTANCE must be a string.") ;
00247 }
00248
00249 if (! mxIsStruct (forest_array) ||
00250 mxGetNumberOfElements (forest_array) != 1) {
00251 vlmxError (vlmxErrInconsistentData,
00252 "FOREST must be a 1 x 1 structure.") ;
00253 }
00254 dimension_array = mxGetField (forest_array, 0, "dimension") ;
00255 if (! dimension_array ||
00256 ! vlmxIsPlainScalar (dimension_array) ||
00257 (dimension = mxGetScalar (dimension_array)) < 1) {
00258 vlmxError(vlmxErrInconsistentData,
00259 "FOREST.NUMDIMENSIONS must be a poisitve integer.") ;
00260 }
00261 numData_array = mxGetField (forest_array, 0, "numData") ;
00262 if (! numData_array ||
00263 ! vlmxIsPlainScalar (numData_array) ||
00264 (numData = mxGetScalar (numData_array)) < 1) {
00265 vlmxError(vlmxErrInconsistentData,
00266 "FOREST.NUMDATA must be a poisitve integer.") ;
00267 }
00268 trees_array = mxGetField (forest_array, 0, "trees") ;
00269 if (! mxIsStruct (trees_array)) {
00270 vlmxError(vlmxErrInconsistentData,
00271 "FOREST.TREES must be a structure array.") ;
00272 }
00273 numTrees = mxGetNumberOfElements (trees_array) ;
00274 if (numTrees < 1) {
00275 vlmxError(vlmxErrInconsistentData,
00276 "FOREST.TREES must have at least one element.") ;
00277 }
00278
00279 if (! vlmxIsMatrix (data_array, dimension, numData)) {
00280 vlmxError(vlmxErrInconsistentData,
00281 "DATA dimensions are not compatible with TREE.") ;
00282 }
00283 if (! vlmxIsReal (data_array)) {
00284 vlmxError(vlmxErrInvalidArgument,
00285 "DATA must be real.") ;
00286 }
00287 switch (mxGetClassID (data_array)) {
00288 case mxSINGLE_CLASS : dataType = VL_TYPE_FLOAT ; break ;
00289 case mxDOUBLE_CLASS : dataType = VL_TYPE_DOUBLE ; break ;
00290 default :
00291 vlmxError(vlmxErrInvalidArgument,
00292 "DATA must be either SINGLE or DOUBLE.") ;
00293 }
00294
00295 forest = vl_kdforest_new (dataType, dimension, numTrees, distance) ;
00296 forest->numData = numData ;
00297 forest->trees = vl_malloc (sizeof(VlKDTree*) * numTrees) ;
00298 forest->data = mxGetData (data_array) ;
00299
00300
00301
00302
00303
00304 for (ti = 0 ; ti < numTrees ; ++ ti) {
00305 VlKDTree * tree = vl_malloc (sizeof(VlKDTree)) ;
00306 nodes_array = mxGetField (trees_array, ti, "nodes") ;
00307 dataIndex_array = mxGetField (trees_array, ti, "dataIndex") ;
00308
00309 if (! nodes_array ||
00310 ! mxIsStruct (nodes_array)) {
00311 vlmxError(vlmxErrInconsistentData,
00312 "FOREST.TREES(%d).NODES must be a struct array.", ti+1) ;
00313 }
00314
00315
00316
00317
00318
00319
00320
00321
00322 lowerChild_array = mxGetField (nodes_array, 0, "lowerChild") ;
00323 upperChild_array = mxGetField (nodes_array, 0, "upperChild") ;
00324 splitDimension_array = mxGetField (nodes_array, 0, "splitDimension") ;
00325 splitThreshold_array = mxGetField (nodes_array, 0, "splitThreshold") ;
00326 lowerBound_array = mxGetField (nodes_array, 0, "lowerBound") ;
00327 upperBound_array = mxGetField (nodes_array, 0, "upperBound") ;
00328
00329 numUsedNodes = mxGetN (lowerChild_array) ;
00330 maxNumNodes += numUsedNodes ;
00331
00332 if (! lowerChild_array ||
00333 ! vlmxIsMatrix (lowerChild_array, 1, numUsedNodes) ||
00334 mxGetClassID (lowerChild_array) != mxINT32_CLASS) {
00335 vlmxError(vlmxErrInconsistentData,
00336 "FOREST.TREES(%d).NODES.LOWERCHILD must be a 1 x NUMNODES INT32 array.",ti+1) ;
00337 }
00338 if (! upperChild_array ||
00339 ! vlmxIsMatrix (upperChild_array, 1, numUsedNodes) ||
00340 mxGetClassID (upperChild_array) != mxINT32_CLASS) {
00341 vlmxError(vlmxErrInconsistentData,
00342 "FOREST.TREES(%d).NODES.UPPERCHILD must be a 1 x NUMNODES INT32 array.",ti+1) ;
00343 }
00344 if (! splitDimension_array ||
00345 ! vlmxIsMatrix (splitDimension_array, 1, numUsedNodes) ||
00346 mxGetClassID (splitDimension_array) != mxUINT32_CLASS) {
00347 vlmxError(vlmxErrInconsistentData,
00348 "FOREST.TREES(%d).NODES.SPLITDIMENSION must be a 1 x NUMNODES UINT32 array",ti+1) ;
00349 }
00350 if (! splitThreshold_array ||
00351 ! vlmxIsMatrix (splitThreshold_array, 1, numUsedNodes) ||
00352 mxGetClassID (splitThreshold_array) != mxDOUBLE_CLASS) {
00353 vlmxError(vlmxErrInconsistentData,
00354 "FOREST.TREES(%d).NODES.SPLITTHRESHOLD must be a 1 x NUMNODES DOUBLE array",ti+1) ;
00355 }
00356 if (! splitThreshold_array ||
00357 ! vlmxIsMatrix (lowerBound_array, 1, numUsedNodes) ||
00358 mxGetClassID (lowerBound_array) != mxDOUBLE_CLASS) {
00359 vlmxError(vlmxErrInconsistentData,
00360 "FOREST.TREES(%d).NODES.LOWERBOUND must be a 1 x NUMNODES DOUBLE array",ti+1) ;
00361 }
00362 if (! splitThreshold_array ||
00363 ! vlmxIsMatrix (upperBound_array, 1, numUsedNodes) ||
00364 mxGetClassID (upperBound_array) != mxDOUBLE_CLASS) {
00365 vlmxError(vlmxErrInconsistentData,
00366 "FOREST.TREES(%d).NODES.UPPERBOUND must be a 1 x NUMNODES DOUBLE array",ti+1) ;
00367 }
00368 lowerChild = (vl_int32*) mxGetData (lowerChild_array) ;
00369 upperChild = (vl_int32*) mxGetData (upperChild_array) ;
00370 splitDimension = (vl_uint32*) mxGetData (splitDimension_array) ;
00371 splitThreshold = (double*) mxGetData (splitThreshold_array) ;
00372 lowerBound = (double*) mxGetData (lowerBound_array) ;
00373 upperBound = (double*) mxGetData (upperBound_array) ;
00374
00375 if (! dataIndex_array ||
00376 ! vlmxIsMatrix (dataIndex_array, 1, numData) ||
00377 mxGetClassID (dataIndex_array) != mxUINT32_CLASS) {
00378 vlmxError(vlmxErrInconsistentData,
00379 "FOREST.TREES(%d).DATAINDEX must be a 1 x NUMDATA array of class UINT32.",ti+1) ;
00380 }
00381
00382 tree->numAllocatedNodes = numUsedNodes ;
00383 tree->numUsedNodes = numUsedNodes ;
00384 tree->nodes = vl_malloc (sizeof(VlKDTreeNode) * numUsedNodes) ;
00385 tree->dataIndex = vl_malloc (sizeof(VlKDTreeDataIndexEntry) * numData) ;
00386
00387 {
00388 vl_uindex ni ;
00389 for (ni = 0 ; ni < numUsedNodes ; ++ ni) {
00390 vl_int32 lc = lowerChild [ni] ;
00391 vl_int32 uc = upperChild [ni] ;
00392 vl_uint32 d = splitDimension [ni] ;
00393
00394 if (uc < - (signed)numData - 1 || uc > (signed)numUsedNodes) {
00395 vlmxError (vlmxErrInconsistentData,
00396 "TREE.NODES.UPPERCHILD(%d)=%d out of bounds",
00397 ni+1,uc) ;
00398 }
00399 if (lc < - (signed)numData || lc > (signed)numUsedNodes) {
00400 vlmxError (vlmxErrInconsistentData,
00401 "TREE.NODES.LOWERCHILD(%d)=%d out of bounds",
00402 ni+1,lc) ;
00403 }
00404 if (d > dimension) {
00405 vlmxError (vlmxErrInconsistentData,
00406 "TREE.NODES.SPLITDIMENSION(%d)=%d out of bounds",
00407 ni+1,d) ;
00408 }
00409
00410 tree->nodes[ni].parent = 0 ;
00411 tree->nodes[ni].upperChild = (uc >= 1) ? uc-1 : uc ;
00412 tree->nodes[ni].lowerChild = (lc >= 1) ? lc-1 : lc ;
00413 tree->nodes[ni].splitDimension = d - 1 ;
00414 tree->nodes[ni].splitThreshold = splitThreshold[ni] ;
00415 tree->nodes[ni].lowerBound = lowerBound[ni] ;
00416 tree->nodes[ni].upperBound = upperBound[ni] ;
00417 }
00418 }
00419
00420 {
00421 vl_uindex di ;
00422 vl_uint32 * dataIndex = mxGetData (dataIndex_array) ;
00423 for (di = 0 ; di < numData ; ++ di) {
00424 tree->dataIndex[di].index = dataIndex [di] - 1 ;
00425 }
00426 }
00427
00428 {
00429 int numNodesToVisit = tree->numUsedNodes ;
00430 restore_parent_recursively (tree, 0, &numNodesToVisit) ;
00431 if (numNodesToVisit != 0) {
00432 vlmxError (vlmxErrInconsistentData,
00433 "TREE has an inconsitsent tree structure.") ;
00434 }
00435 }
00436
00437 forest->trees[ti] = tree ;
00438 }
00439
00440 forest->maxNumNodes = maxNumNodes;
00441
00442 return forest ;
00443 }