testKdTree.c

Examples for using a KdTree to analyze and process local neighbourhoods.

 
#include "interface.h"
#include "algorithm.h"
#include "geometry.h"
 
#include "examples.h"
#include "io.h"
 
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
int test_KdTree()
{
  printf("\n***test_KdTree***\n");
 
  size_t i=0,numNN=0,ptsVec=0;
  ALG3D_KDTREE tree=0;      // Handle to a search tree
 
  //Read XYZ-point cloud
  const char* fname ="./sample_data/testData.xyz";
  ALG3D_Point3d* data=0;    // data array
  ALG3D_Point3d* vNN =0;    // array for nearest neighbours
  ALG3D_Point3d pt;         // input point, which is used for the queries
  ALG3D_Point3d nn;         // nearest neighbour
  const size_t  maxPoints = 15; // number of points for k-nearest-neighbour query
 
  //Box half-axes
  double vDist[3]={5,3,1};
  
  //test point used for the query
  double x=156.47, y=- 14.24, z=-637.97;
  pt.vec[0]=x; pt.vec[1]=y; pt.vec[2]=z;
 
  //read XYZ-point data from file
  ALG3D_readXYZ(fname, &data,&ptsVec);
 
 
  //create kd-tree
  if(!ALG3D_checkResult( ALG3D_createKdTree(data, ptsVec, &tree) )) return 0;
  
  //find nearest neighbour to pt;
  //pt=data[0];
 
  if(!ALG3D_checkResult( ALG3D_findNearestNeighbor(tree, pt, 1.0, &nn) )){
    printf("nearest neighbor: nothing found in range\n");
  }
  else{
    printf("nearest neighbor: %lf, %lf %lf\n",nn.vec[0],nn.vec[1],nn.vec[2]);
  }
    
  printf("ALG3D_findNearestNeighbor finished\n");
  
  //find all neighbours within a given radius
  if(!ALG3D_checkResult( ALG3D_findNeighbors(tree, pt, 0.25, &vNN, &numNN ) )) return 0;
  else{
    printf("found %I64d neighbors:\n",numNN);
    for(i=0;i<numNN;i++){
      printf("[%3I64d]: %lf, %lf, %lf\n",i,vNN[i].vec[0],vNN[i].vec[1],vNN[i].vec[2]);
    }
  }
  
  //find all points inside a box around pt
  if(!ALG3D_checkResult( ALG3D_freeMemory(vNN)  )) return 0;
  if(!ALG3D_checkResult( ALG3D_findNeighborsBox(tree, pt, vDist, &vNN, &numNN) )) return 0;
  else{
    printf("points inside box: %I64d\n",numNN);
  }
 
  //find k-nearest points around pt
  if (!ALG3D_checkResult(ALG3D_freeMemory(vNN))) return 0;
 
  if (!ALG3D_checkResult(ALG3D_findKNearestNeighbors(tree, pt, maxPoints, &vNN, &numNN))) return 0;
  else if (numNN != maxPoints){ //this is only allowed to happen if number of points in the tree is smaller than the number of points requested
    printf("warning: number of points returned (%I64d) not equals number of points requested (%I64d)\n", numNN, maxPoints);
    return 0;
  }
  else{
    printf("points found: %I64d/%I64d\n", numNN, maxPoints);
  }
  printf("ALG3D_findKNearestNeighbors finished\n");
 
 
  //clean-up und release memory
  if(!ALG3D_checkResult( ALG3D_freeKdTree     (tree) )) return 0;
  
  ALG3D_freeMemory(vNN);
  ALG3D_freeMemory(data);
  
  printf("--> passed successfully\n");
  return 1;
}
 
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
int test_KdTreeWithNormals()
{
  printf("\n***test_KdTreeWithNormals***\n");
 
  size_t i = 0, numNN = 0, ptsVec = 0;
  ALG3D_KDTREE tree = 0;      // Handle to a search tree
 
  //Read XYZ-point cloud
  const char* fname = "./sample_data/schiller05.xyz";
  ALG3D_Point3d* data = 0;    // data array
  ALG3D_Point3d* vNN = 0;    // array for nearest neighbours
  ALG3D_Point3d pt;         // input point, which is used for the queries
  ALG3D_Point3d nn;         // nearest neighbour
  const size_t  maxPoints = 15; // number of points for k-nearest-neighbour query
 
  //read XYZ-point data from file
  ALG3D_readXYZ(fname, &data, &ptsVec);
 
 
  //create kd-tree
  if (!ALG3D_checkResult(ALG3D_createKdTree(data, ptsVec, &tree))) return 0;
 
  ALG3D_Point3d* normals = (ALG3D_Point3d*)malloc(ptsVec * sizeof(ALG3D_Point3d));
  if (!ALG3D_checkResult(ALG3D_computeNormalVectorsKNNTreeFast(tree, normals, 15))) return 0;
 
  ALG3D_Point3d sensorPos;
  sensorPos.vec[0] = -133;
  sensorPos.vec[1] = 0;
  sensorPos.vec[2] = 112;
 
  // Align normals
  for (i = 0; i < ptsVec; ++i)
  {
    ALG3D_Point3d toSensor;
    toSensor.vec[0] = sensorPos.vec[0] - data[i].vec[0];
    toSensor.vec[1] = sensorPos.vec[1] - data[i].vec[1];
    toSensor.vec[2] = sensorPos.vec[2] - data[i].vec[2];
 
    if (ALG3D_dotProduct(&toSensor, &normals[i]) < 0)
    {
      normals[i].vec[0] = -normals[i].vec[0];
      normals[i].vec[1] = -normals[i].vec[1];
      normals[i].vec[2] = -normals[i].vec[2];
    }
  }
 
  //test point used for the query
  pt.vec[0] = -24.066111;
  pt.vec[1] = 15.172830;
  pt.vec[2] = 4.777431;
 
  ALG3D_Point3d ptNormal;
  ptNormal.vec[0] = -1;
  ptNormal.vec[1] = 0;
  ptNormal.vec[2] = 0;
 
  if (!ALG3D_checkResult(ALG3D_findNearestNormalCompatibleNeighbor(tree, normals, pt, ptNormal, 10.0, 0.95, &nn))){
    printf("nearest neighbor: nothing found in range\n");
  }
  else{
    printf("nearest compatible neighbor: %lf %lf %lf\n", nn.vec[0], nn.vec[1], nn.vec[2]);
  }
 
  printf("ALG3D_findNearestNormalCompatibleNeighbor finished\n");
 
  //find all neighbours within a given radius
  if (!ALG3D_checkResult(ALG3D_findNormalCompatibleNeighbors(tree, normals, pt, ptNormal, 5.0, 0.95, &vNN, &numNN))) 
    return 0;
  else{
    printf("found %I64d neighbors:\n", numNN);
    for (i = 0; i < numNN; i++){
      printf("[%3I64d]: %lf %lf %lf\n", i, vNN[i].vec[0], vNN[i].vec[1], vNN[i].vec[2]);
    }
  }
 
  printf("ALG3D_findNormalCompatibleNeighbors finished\n");
 
  //find k-nearest points around pt
  if (!ALG3D_checkResult(ALG3D_freeMemory(vNN))) return 0;
 
  if (!ALG3D_checkResult(ALG3D_findKNearestNormalCompatibleNeighbors(tree, normals, pt, ptNormal, maxPoints, 0.95, &vNN, &numNN))) 
    return 0;
  else if (numNN != maxPoints){ //this is only allowed to happen if number of points in the tree is smaller than the number of points requested
    printf("warning: number of points returned (%I64d) not equals number of points requested (%I64d)\n", numNN, maxPoints);
    return 0;
  }
  else{
    printf("points found: %I64d/%I64d\n", numNN, maxPoints);
 
    for (i = 0; i < numNN; i++){
      printf("[%3I64d]: %lf %lf %lf\n", i, vNN[i].vec[0], vNN[i].vec[1], vNN[i].vec[2]);
    }
  }
 
  printf("ALG3D_findKNearestNormalCompatibleNeighbors finished\n");
 
  //clean-up und release memory
  if (!ALG3D_checkResult(ALG3D_freeKdTree(tree))) return 0;
 
  ALG3D_freeMemory(vNN);
  ALG3D_freeMemory(data);
  
  free(normals);
 
  printf("--> passed successfully\n");
  return 1;
}
 
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
int test_ThinningTree()
{
  size_t i=0,ptsVec=0, count=0;
  ALG3D_KDTREE tree=0;            // Handle to a search tree
 
  //Read XYZ-point cloud
  const char* fname ="./sample_data/testData.xyz";
  ALG3D_Point3d *data=0;
  ALG3D_readXYZ(fname, &data,&ptsVec);
 
  printf("\n***test_ThinningTree***\n");
 
  //create kdTree
  if(!ALG3D_checkResult( ALG3D_createKdTree(data, ptsVec, &tree) )) return 0;
  
  //compute thinning using a tree (removed points are set to [0,0,0])
  //changes to original data which is referenced by the tree
  if (!ALG3D_checkResult(ALG3D_computeThinningTree(tree, 1.0))) return 0;
  else{
    count=0;
    for (i=0; i < ptsVec; ++i){
      if (data[i].vec[0] != 0 && data[i].vec[1] != 0 && data[i].vec[2] != 0)
        count++;
    }
    printf("remaining points: %I64d\n",count);
  }
 
  ALG3D_freeMemory(data);
  
  if(!ALG3D_checkResult( ALG3D_freeKdTree(tree) )) return 0;
  
  printf("--> passed successfully\n");
  return 1;
}
 
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
int test_RegistrationTree()
{
  printf("\n***test_RegistrationTree***\n");
 
  const char* fnameStat ="./sample_data/surface1.xyz";
  const char* fnameDyn  ="./sample_data/surface2.xyz";
  const char* fnameOut  ="./sample_data/icpOut2.xyz";
 
  ALG3D_Point3d *dataStat=0, *dataDyn=0;  // data arrays
  size_t ptsStat=0, ptsDyn=0;
  ALG3D_KDTREE tree=0;          // Handle to a search tree
  size_t i=0;
  double R[9]={0,0,0,0,0,0,0,0,0},T[3]={0,0,0},S[1]={0};
  icpResult res;          //result statistics
  icpParam prm;
    prm.radius    = 100;    //max. search radius
    prm.iterations= 500;    //max. number of iterations
    prm.tolerance = 0.001;  //tolerance
    prm.samples   = 2000;   //number of points used for computing the registration
    prm.options   = icpStandardDoF; //enable translation and rotation only
 
  //read static point data
  if(!ALG3D_checkResult(ALG3D_readXYZ(fnameStat,&dataStat,&ptsStat))) return 0;
 
  //read dynamic point data (will be transformed)
  if(!ALG3D_checkResult(ALG3D_readXYZ(fnameDyn ,&dataDyn ,&ptsDyn)))  return 0;
 
  //create kdTree from static data
  if(!ALG3D_checkResult(ALG3D_createKdTree(dataStat, ptsStat, &tree) )) return 0;
 
  //compute registration, i.e. transformation matrix
  if (!ALG3D_checkResult(ALG3D_computeRegistrationTree(tree, dataDyn, ptsDyn, prm, &res, R, T, S))) return 0;
 
  //apply transformation
  if (!ALG3D_checkResult(ALG3D_transformPoints(dataDyn, ptsDyn, R, T, S))) return 0;
 
  //write transformed data to file
  if(!ALG3D_checkResult(ALG3D_writeXYZ(fnameOut,dataDyn,ptsDyn))) return 0;
  
  printf("minDist: %lf, maxDist: %lf, meanDev: %lf, stdDev: %3.10lf, pairs: %I64d, iterations: %I64d\n",
          res.minDist,res.maxDist,res.meanDev,res.stdDev,res.pairs,res.iterations);
 
  //print out 3x3 rotation matrix
  printf("** R **");
  for(i=0;i<9;++i){
    if(i%3==0)printf("\n");
    printf("%lf\t",R[i]);
  }printf("\n");
  
  //print out 3x1 translation vector
  printf("** T **\n");
  for(i=0;i<3;++i){
    printf("%lf\t",T[i]);
  }printf("\n");
  
  //print out scale factor
  printf("** S **\n");
  printf("%lf\n",S[0]);
 
  //clean-up
  ALG3D_freeMemory(dataStat);
  ALG3D_freeMemory(dataDyn);
  
  if(!ALG3D_checkResult( ALG3D_freeKdTree(tree) )) return 0;
  
  printf("--> passed successfully\n");
  return 1;
}
 
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
int test_RegistrationPlaneTree()
{
  printf("\n***test_RegistrationPlaneTree***\n");
 
  const char* fnameStat ="./sample_data/surface1.xyz";
  const char* fnameDyn  ="./sample_data/surface2.xyz";
  const char* fnameOut  ="./sample_data/icpOut2Plane.xyz";
 
  ALG3D_Point3d *dataStat=0, *dataDyn=0;  // data arrays
  ALG3D_Point3d *normalsStat=0;
  size_t ptsStat=0, ptsDyn=0;
  ALG3D_KDTREE tree=0;          // Handle to a search tree
  size_t i=0;
  double R[9]={0,0,0,0,0,0,0,0,0},T[3]={0,0,0},S[1]={0};
  icpResult res;          //result statistics
  icpParam prm;
  prm.radius    = 100;    //max. search radius
  prm.iterations= 500;    //max. number of iterations
  prm.tolerance = 0.001;  //tolerance
  prm.samples   = 2000;   //number of points used for computing the registration
  prm.options   = icpStandardDoF; //enable translation and rotation only
 
  //read static point data
  if(!ALG3D_checkResult(ALG3D_readXYZ(fnameStat,&dataStat,&ptsStat))) return 0;
 
  //read dynamic point data (will be transformed)
  if(!ALG3D_checkResult(ALG3D_readXYZ(fnameDyn ,&dataDyn ,&ptsDyn)))  return 0;
 
  //create kdTree from static data
  if(!ALG3D_checkResult(ALG3D_createKdTree(dataStat, ptsStat, &tree) )) return 0;
 
  normalsStat = (ALG3D_Point3d*)malloc(ptsStat * sizeof(ALG3D_Point3d));
 
  //compute normal vectors using the KdTree
  if(!ALG3D_checkResult(ALG3D_computeNormalVectorsKNNTree(tree, normalsStat, 30) )) return 0;
 
  //compute registration, i.e. transformation matrix
  if (!ALG3D_checkResult(ALG3D_computeRegistrationPlaneTree(tree, normalsStat, dataDyn, ptsDyn, prm, &res, R, T, S))) return 0;
 
  //apply transformation
  if (!ALG3D_checkResult(ALG3D_transformPoints(dataDyn, ptsDyn, R, T, S))) return 0;
 
  //write transformed data to file
  if(!ALG3D_checkResult(ALG3D_writeXYZ(fnameOut,dataDyn,ptsDyn))) return 0;
 
  printf("minDist: %lf, maxDist: %lf, meanDev: %lf, stdDev: %3.10lf, pairs: %I64d, iterations: %I64d\n",
    res.minDist,res.maxDist,res.meanDev,res.stdDev,res.pairs,res.iterations);
 
  //print out 3x3 rotation matrix
  printf("** R **");
  for(i=0;i<9;++i){
    if(i%3==0)printf("\n");
    printf("%lf\t",R[i]);
  }printf("\n");
 
  //print out 3x1 translation vector
  printf("** T **\n");
  for(i=0;i<3;++i){
    printf("%lf\t",T[i]);
  }printf("\n");
 
  //print out scale factor
  printf("** S **\n");
  printf("%lf\n",S[0]);
 
  //clean-up
  free(normalsStat);
  ALG3D_freeMemory(dataStat);
  ALG3D_freeMemory(dataDyn);
 
  if(!ALG3D_checkResult( ALG3D_freeKdTree(tree) )) return 0;
 
  printf("--> passed successfully\n");
  return 1;
}
 
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
int test_SmoothingTree()
{
  printf("\n***test_SmoothingTree***\n");
 
  size_t i=0,ptsVec=0;
  ALG3D_KDTREE tree=0;            // Handle to search tree
 
  //read XYZ-point data
  const char* fname ="./sample_data/testData.xyz";
  ALG3D_Point3d* data=0,*resdata=0;    // data arrays
  ALG3D_readXYZ(fname, &data,&ptsVec);
 
  //create kdTree
  if(!ALG3D_checkResult( ALG3D_createKdTree(data, ptsVec, &tree) )) return 0;
 
  //allocate storage for the resulting, smoothed point data
  resdata=(ALG3D_Point3d*)  malloc(ptsVec*sizeof(ALG3D_Point3d));
  for(i=0;i<ptsVec;++i){
    resdata[i].vec[0]=0;
    resdata[i].vec[1]=0;
    resdata[i].vec[2]=0;
  }
 
  //compute smoothing of point data with given search tree
  if (!ALG3D_checkResult(ALG3D_computeSmoothingTree(tree, resdata, 1.0, Gaussian))) return 0;
  else{
    //ALG3D_writeXYZ("smoothed.xyz", resPt,ptsVec);
  }
 
  ALG3D_freeMemory(data);
  free(resdata);
  
  if(!ALG3D_checkResult( ALG3D_freeKdTree(tree) )) return 0;
  
  printf("--> passed successfully\n");
  return 1;
}
 
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
int test_RegionGrowTree()
{
  printf("\n***test_RegionGrowTree***\n");
 
  ALG3D_KDTREE tree=0;            // Handle to search tree
 
  const char* fname = "./sample_data/testData.xyz";
  ALG3D_Point3d *vec = 0; //pointer to array of 3d points
  size_t ptsVec = 0;      //number of 3d points in array
  size_t* vRegionIdx = 0; //pointer to array of indices
  size_t  numIdx = 0;     //number of indices in array
 
                          //read point data from file
  if (!ALG3D_checkResult(ALG3D_readXYZ(fname, &vec, &ptsVec))) return 0;
 
  //create the KdTree
  if (!ALG3D_checkResult(ALG3D_createKdTree(vec, ptsVec, &tree))) return 0;
 
  //storage for the region indices (one index indicates the region of one point)
  vRegionIdx = (size_t*)malloc(ptsVec*sizeof(size_t));
 
  //compute regions
  if (!ALG3D_checkResult(ALG3D_computeConnectedRegionsTree(tree, 1.0, vRegionIdx, &numIdx))) return 0;
  printf("regions found: %I64d\n", numIdx);
 
  ALG3D_freeKdTree(tree);
  ALG3D_freeMemory(vec);
  free(vRegionIdx);
 
  printf("--> passed successfully\n");
  return 1;
}
 
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
int test_DistancesTree()
{
  printf("\n***test_DistancesTree***\n");
 
  ALG3D_KDTREE tree = 0;  // Handle to search tree
  double *distances = 0;  // Pointer to result vector
  double minValue = 0, maxValue = 0, meanValue = 0;
  //read XYZ-point data
  const char* fnameRef = "./sample_data/cone.xyz";
  const char* fnameTest = "./sample_data/cone_rot.xyz";
  ALG3D_Point3d* refData = 0, *testData = 0;    // data arrays
  size_t i = 0, ptsRef = 0, ptsTest = 0;        // sizes of data arrays
  ALG3D_readXYZ(fnameRef, &refData, &ptsRef);
  ALG3D_readXYZ(fnameTest, &testData, &ptsTest);
 
  //create kdTree
  if (!ALG3D_checkResult(ALG3D_createKdTree(refData, ptsRef, &tree))) return 0;
 
  //allocate storage for the resulting distance values
  distances = (double*)malloc(ptsTest*sizeof(double));
  for (i = 0; i < ptsTest; ++i){
    distances[i] = -1;
  }
 
  //compute distances with given search tree
  if (!ALG3D_checkResult(ALG3D_computeDistances2Pointcloud(tree, testData, ptsTest, 0, distances))){
    return 0;
  }
  
  meanValue = minValue = maxValue = distances[0];
  for (i = 1; i < ptsTest; ++i){
    const double d = distances[i];
    if (d < minValue)minValue = d;
    else if (d>maxValue)maxValue = d;
    meanValue += d;
  }
  meanValue /= ptsTest;
 
  printf("[distances]: min: %lf, max: %lf, mean: %lf\n", minValue, maxValue, meanValue);
 
  //free allocated memory
  ALG3D_freeMemory(refData);
  ALG3D_freeMemory(testData);
  free(distances);
 
  if (!ALG3D_checkResult(ALG3D_freeKdTree(tree))) return 0;
 
  printf("--> passed successfully\n");
  return 1;
}
 
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
int test_SurfaceNormalsTree()
{
  printf("\n***test_SurfaceNormalsTree***\n");
 
  ALG3D_KDTREE tree=0;            // Handle to search tree
 
  const char* fname = "./sample_data/cylinder.xyz";
  ALG3D_Point3d *vec = 0;     //pointer to array of 3d points
  size_t ptsVec = 0;          //number of 3d points in array
  ALG3D_Point3d *normals = 0; //pointer to array of 3d points/vectors
 
                              //read point data from file
  if (!ALG3D_checkResult(ALG3D_readXYZ(fname, &vec, &ptsVec))) return 0;
 
  //create the KdTree
  if (!ALG3D_checkResult(ALG3D_createKdTree(vec, ptsVec, &tree))) return 0;
 
  //storage for the point-wise normal vectors
  normals = (ALG3D_Point3d*)malloc(ptsVec*sizeof(ALG3D_Point3d));
 
  //a)compute normals using K-Nearest-Neighbours
  const size_t K = 25;
  if (!ALG3D_checkResult(ALG3D_computeNormalVectorsKNNTree(tree, normals, K))) return 0;
 
  //b)compute normals using radial neighborhood
  const double radius = 1.0;
  if (!ALG3D_checkResult(ALG3D_computeNormalVectorsTree(tree, normals, radius))) return 0;
 
  ALG3D_freeKdTree(tree);
  ALG3D_freeMemory(vec);
  free(normals);
 
  printf("--> passed successfully\n");
  return 1;
}
 
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
int test_SurfaceNormalsTreeFast()
{
  printf("\n***test_SurfaceNormalsTreeFast***\n");
 
  ALG3D_KDTREE tree=0;            // Handle to search tree
 
  const char* fname = "./sample_data/cylinder.xyz";
  ALG3D_Point3d *vec = 0;     //pointer to array of 3d points
  size_t ptsVec = 0;          //number of 3d points in array
  ALG3D_Point3d *normals = 0; //pointer to array of 3d points/vectors
 
                              //read point data from file
  if (!ALG3D_checkResult(ALG3D_readXYZ(fname, &vec, &ptsVec))) return 0;
 
  //create the KdTree
  if (!ALG3D_checkResult(ALG3D_createKdTree(vec, ptsVec, &tree))) return 0;
 
  //storage for the point-wise normal vectors
  normals = (ALG3D_Point3d*)malloc(ptsVec*sizeof(ALG3D_Point3d));
 
  //a)compute normals using K-Nearest-Neighbours
  const size_t K = 25;
  if (!ALG3D_checkResult(ALG3D_computeNormalVectorsKNNTreeFast(tree, normals, K))) return 0;
 
  //b)compute normals using radial neighborhood
  const double radius = 1.0;
  if (!ALG3D_checkResult(ALG3D_computeNormalVectorsTreeFast(tree, normals, radius))) return 0;
 
  ALG3D_freeKdTree(tree);
  ALG3D_freeMemory(vec);
  free(normals);
 
  printf("--> passed successfully\n");
  return 1;
}
 
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
int test_SurfaceFeaturesTree()
{
  printf("\n***test_SurfaceFeaturesTree***\n");
 
  ALG3D_KDTREE tree=0;            // Handle to search tree
 
  const char* fname = "./sample_data/surface1.xyz";
  ALG3D_Point3d *vec = 0;           //pointer to array of 3d points
  size_t ptsVec = 0;                //number of 3d points in array
  double *intrinsic_curvatures = 0; //pointer to array of values
  double *extrinsic_curvatures = 0; //pointer to array of values
 
                                    //read point data from file
  if (!ALG3D_checkResult(ALG3D_readXYZ(fname, &vec, &ptsVec))) return 0;
 
  //create the KdTree
  if (!ALG3D_checkResult(ALG3D_createKdTree(vec, ptsVec, &tree))) return 0;
 
  //storage for the point-wise curvatures
  intrinsic_curvatures = (double*)malloc(ptsVec*sizeof(double));
  extrinsic_curvatures = (double*)malloc(ptsVec*sizeof(double));
 
  // a) K-Nearest-Neighbours
  //compute intrinsic curvatures
  if (!ALG3D_checkResult(ALG3D_computeSurfaceFeaturesKNNTree(tree, intrinsic_curvatures, 15, IntrinsicCurvature))) return 0;
  //compute extrinsic curvatures
  if (!ALG3D_checkResult(ALG3D_computeSurfaceFeaturesKNNTree(tree, extrinsic_curvatures, 15, ExtrinsicCurvature))) return 0;
 
  // b) Radius neighbours
  //compute intrinsic curvatures
  if (!ALG3D_checkResult(ALG3D_computeSurfaceFeaturesTree(tree, intrinsic_curvatures, 1.5, IntrinsicCurvature))) return 0;
  //compute extrinsic curvatures
  if (!ALG3D_checkResult(ALG3D_computeSurfaceFeaturesTree(tree, extrinsic_curvatures, 1.5, ExtrinsicCurvature))) return 0;
 
  //Compute minimum and maximum curvature values
  double mnIn, mxIn, mnEx, mxEx;    //min/max curvature values
  mnIn = mxIn = intrinsic_curvatures[0];
  mnEx = mxEx = extrinsic_curvatures[0];
  for (size_t i = 1; i < ptsVec; ++i)
  {
    if (intrinsic_curvatures[i] < mnIn) mnIn = intrinsic_curvatures[i];
    else if (intrinsic_curvatures[i] > mxIn) mxIn = intrinsic_curvatures[i];
 
    if (extrinsic_curvatures[i] < mnEx) mnEx = extrinsic_curvatures[i];
    else if (extrinsic_curvatures[i] > mxEx) mxEx = extrinsic_curvatures[i];
  }
 
  printf("intrinsic curvature: min: %lf, max: %lf\n", mnIn, mxIn);
  printf("extrinsic curvature: min: %lf, max: %lf\n", mnEx, mxEx);
 
  ALG3D_freeKdTree(tree);
  ALG3D_freeMemory(vec);
  free(intrinsic_curvatures);
  free(extrinsic_curvatures);
 
  printf("--> passed successfully\n");
  return 1;
}
 
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
int test_SurfaceFeaturesTreeFast()
{
  printf("\n***test_SurfaceFeaturesTreeFast***\n");
 
  ALG3D_KDTREE tree=0;            // Handle to search tree
 
  const char* fname = "./sample_data/surface1.xyz";
  ALG3D_Point3d *vec = 0;           //pointer to array of 3d points
  size_t ptsVec = 0;                //number of 3d points in array
  double *intrinsic_curvatures = 0; //pointer to array of values
  double *extrinsic_curvatures = 0; //pointer to array of values
 
                                    //read point data from file
  if (!ALG3D_checkResult(ALG3D_readXYZ(fname, &vec, &ptsVec))) return 0;
 
  //create the KdTree
  if (!ALG3D_checkResult(ALG3D_createKdTree(vec, ptsVec, &tree))) return 0;
 
  //storage for the point-wise curvatures
  intrinsic_curvatures = (double*)malloc(ptsVec*sizeof(double));
  extrinsic_curvatures = (double*)malloc(ptsVec*sizeof(double));
 
  // a) K-Nearest-Neighbours
  //compute intrinsic curvatures
  if (!ALG3D_checkResult(ALG3D_computeSurfaceFeaturesKNNTreeFast(tree, intrinsic_curvatures, 15, IntrinsicCurvature))) return 0;
  //compute extrinsic curvatures
  if (!ALG3D_checkResult(ALG3D_computeSurfaceFeaturesKNNTreeFast(tree, extrinsic_curvatures, 15, ExtrinsicCurvature))) return 0;
 
  // b) Radius neighbours
  //compute intrinsic curvatures
  if (!ALG3D_checkResult(ALG3D_computeSurfaceFeaturesTreeFast(tree, intrinsic_curvatures, 1.5, IntrinsicCurvature))) return 0;
  //compute extrinsic curvatures
  if (!ALG3D_checkResult(ALG3D_computeSurfaceFeaturesTreeFast(tree, extrinsic_curvatures, 1.5, ExtrinsicCurvature))) return 0;
 
  //Compute minimum and maximum curvature values
  double mnIn, mxIn, mnEx, mxEx;    //min/max curvature values
  mnIn = mxIn = intrinsic_curvatures[0];
  mnEx = mxEx = extrinsic_curvatures[0];
  for (size_t i = 1; i < ptsVec; ++i)
  {
    if (intrinsic_curvatures[i] < mnIn) mnIn = intrinsic_curvatures[i];
    else if (intrinsic_curvatures[i] > mxIn) mxIn = intrinsic_curvatures[i];
 
    if (extrinsic_curvatures[i] < mnEx) mnEx = extrinsic_curvatures[i];
    else if (extrinsic_curvatures[i] > mxEx) mxEx = extrinsic_curvatures[i];
  }
 
  printf("intrinsic curvature: min: %lf, max: %lf\n", mnIn, mxIn);
  printf("extrinsic curvature: min: %lf, max: %lf\n", mnEx, mxEx);
 
  ALG3D_freeKdTree(tree);
  ALG3D_freeMemory(vec);
  free(intrinsic_curvatures);
  free(extrinsic_curvatures);
 
  printf("--> passed successfully\n");
  return 1;
}
 
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
int test_LocalOutlierFactorTree()
{
  printf("\n***test_LocalOutlierFactorTree***\n");
 
  const char* fname = "./sample_data/testData.xyz";
  ALG3D_Point3d *vec = 0;           //pointer to array of 3d points
  size_t ptsVec = 0;                //number of 3d points in array
  double *lof_values = 0;           //pointer to array of values
 
  ALG3D_KDTREE tree = 0;            // Handle to search tree
 
  //read point data from file
  if (!ALG3D_checkResult(ALG3D_readXYZ(fname, &vec, &ptsVec))) return 0;
 
  //create the KdTree
  if (!ALG3D_checkResult(ALG3D_createKdTree(vec, ptsVec, &tree))) return 0;
 
  //storage for the outlier factors
  lof_values = (double*)malloc(ptsVec * sizeof(double));
 
  //compute local outlier factors
  if (!ALG3D_checkResult(ALG3D_computeOutlierFactorTree(tree, lof_values, 30))) return 0;
 
  //Compute minimum and maximum curvature values
  double mn, mx;    //min/max values
  mn = mx = lof_values[0];
  for (size_t i = 1; i < ptsVec; ++i)
  {
    if (lof_values[i] < mn) mn = lof_values[i];
    else if (lof_values[i] > mx) mx = lof_values[i];
  }
 
  printf("local outlier factors: min: %lf, max: %lf\n", mn, mx);
 
  ALG3D_freeKdTree(tree);
  ALG3D_freeMemory(vec);
  free(lof_values);
 
  printf("--> passed successfully\n");
  return 1;
}
 
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
int test_RemoveOutliersTree()
{
  printf("\n***test_RemoveOutliersTree***\n");
 
  const char* fname = "./sample_data/testData.xyz";
  ALG3D_Point3d *vec = 0;           //pointer to array of 3d points
  size_t ptsVec = 0;                //number of 3d points in array
 
  ALG3D_KDTREE tree = 0;            // Handle to search tree
 
  //read point data from file
  if (!ALG3D_checkResult(ALG3D_readXYZ(fname, &vec, &ptsVec))) return 0;
 
  //create the KdTree
  if (!ALG3D_checkResult(ALG3D_createKdTree(vec, ptsVec, &tree))) return 0;
 
  //remove outliers
  if (!ALG3D_checkResult(ALG3D_removeOutliersTree(tree, 30, 1.15))) return 0;
 
  size_t count = 0;
  for (int i = 0; i < ptsVec; ++i){
    if (vec[i].vec[0] != 0 && vec[i].vec[1] != 0 && vec[i].vec[2] != 0)
      count++;
  }
  printf("remaining points: %I64d\n", ptsVec - count);
 
  ALG3D_freeKdTree(tree);
  ALG3D_freeMemory(vec);
 
  printf("--> passed successfully\n");
  return 1;
}