ANNkd_tree *CreateSearchTree(const std::vector<KeypointWithDesc> &k,
bool spatial, double alpha)
{
/* Create a new array of points */
int num_pts = (int) k.size();
int dim = 128;
if (spatial) dim = 130;
ANNpointArray pts = annAllocPts(num_pts, dim);
int offset = 0;
if (spatial) offset = 2;
for (int i = 0; i < num_pts; i++) {
int j;
assert(k[i].m_d != NULL);
if (spatial) {
pts[i][0] = alpha * k[i].m_x;
pts[i][1] = alpha * k[i].m_y;
}
for (j = 0; j < 128; j++)
pts[i][j+offset] = k[i].m_d[j];
}
/* Create a search tree for k2 */
ANNkd_tree *tree = new ANNkd_tree(pts, num_pts, dim, 4);
// annDeallocPts(pts);
return tree;
}
CANNObject::CANNObject(int m_k, int m_maxPts, enumStreets m_br, int m_timesActed, double m_eps)
: k(m_k), maxPts(m_maxPts), br(m_br), timesActed(m_timesActed), eps(m_eps)
{
if (br == ePreflop)
dim = 6 + timesActed;
else
dim = 8 + timesActed;
dataPts = annAllocPts(maxPts, dim); // allocate data points
nnIdx = new ANNidx[k]; // allocate near neigh indices
dists = new ANNdist[k]; // allocate near neighbor dists
nPts = gDB.FillAnnArray(dataPts, br, timesActed);
/*
if (DEBUG_ANN)
{
for (int i = 0; i < nPts; i++)
{
printf("Data Point: ");
PrintPt(std::cout, dataPts[i]);
}
}
*/
// build search structure
kdTree = new ANNkd_tree(dataPts, // the data points
nPts, // number of points
dim); // dimension of space
}
void ANNWrapper::SetPoints(const std::vector <Point3D> &P)
{
Free(); // Free, if theres anything to free
m_nnidx = new ANNidx[m_k]; // allocate near neigh indices
m_dists = new ANNdist[m_k]; // allocate near neighbor m_dists
m_query_pt = annAllocPt(dim); // allocate query point
m_npts = P.size();
m_data_pts = annAllocPts(P.size(), dim); // allocate data points
for(unsigned int i=0; i < P.size(); i++)
{
m_data_pts[i][0] = P[i].x;
m_data_pts[i][1] = P[i].y;
m_data_pts[i][2] = P[i].z;
}
m_kdtree = new ANNkd_tree( // build search structure
m_data_pts, // the data points
m_npts, // number of points
dim); // dimension of space
m_anything_to_free = true;
}
ANNpointArray KNNClassifier::KNNFeatureExtraction(SkeData* inputData, int* pFrameNum, int* pFeatureNum, double wt)
{
*pFeatureNum = FEATURENUM;
*pFrameNum = inputData->GetFrameSaved();
ANNpointArray ret = annAllocPts(*pFrameNum, FEATURENUM);
for (int i = 0; i < *pFrameNum; i++)
{
for (int j = 0; j < FEATURENUM / 3; j++)
{
FEATURE_VEC* pVec = new FEATURE_VEC;
if (!CalFeatureVector(inputData, i+1, j+1, pVec, wt))
return NULL;
ret[i][j*3] = pVec->x;
ret[i][j*3 + 1] = pVec->y;
ret[i][j*3 + 2] = pVec->z;
delete pVec;
}
}
return ret;
}
void mitk::PointLocator::SetPoints(vtkPointSet *pointSet)
{
if (pointSet == nullptr)
{
itkWarningMacro("Points are NULL!");
return;
}
vtkPoints *points = pointSet->GetPoints();
if (m_VtkPoints)
{
if ((m_VtkPoints == points) && (m_VtkPoints->GetMTime() == points->GetMTime()))
{
return; // no need to recalculate search tree
}
}
m_VtkPoints = points;
size_t size = points->GetNumberOfPoints();
if (m_ANNDataPoints != nullptr)
delete[] m_ANNDataPoints;
m_ANNDataPoints = annAllocPts(size, m_ANNDimension);
m_IndexToPointIdContainer.clear();
m_IndexToPointIdContainer.resize(size);
for (vtkIdType i = 0; (unsigned)i < size; ++i)
{
double *currentPoint = points->GetPoint(i);
(m_ANNDataPoints[i])[0] = currentPoint[0];
(m_ANNDataPoints[i])[1] = currentPoint[1];
(m_ANNDataPoints[i])[2] = currentPoint[2];
m_IndexToPointIdContainer[i] = i;
}
InitANN();
}
//*****************************************************
// Function: computeMI_ANN
//*****************************************************
double computeMI_ANN( ANNpointArray dataXY,
unsigned int dimX, unsigned int dimY,
unsigned int k, unsigned int N, double eps )
{
ANNpointArray dataX, dataY;
double* distsXY;
double MI_est;
ANNkd_tree* kdTreeX;
ANNkd_tree* kdTreeY;
unsigned int dimXY = dimX + dimY;
// Allocate memory
dataX = annAllocPts(N,dimX);
dataY = annAllocPts(N,dimY);
distsXY = new double[N];
// Normalize data and populate the marginals dataX, dataY
normalizeANN_XY( dataXY, dimXY, dataX, dimX, dataY, dimY, N);
// Get distance to knn for each point
kdTreeX = new ANNkd_tree( dataX, N, dimX );
kdTreeY = new ANNkd_tree( dataY, N, dimY );
distANN_XY( dataXY, dataXY, distsXY, dimXY, dimXY, N, N, k, eps );
// Compute mutual information
double marginal_contrib = 0.0;
for( unsigned int i = 0; i < N; i++ ) {
// get the number of points within a specified radius
int no_pts_X = kdTreeX->annkFRSearch( dataX[ i ], distsXY[ i ], 0, NULL, NULL, eps);
int no_pts_Y = kdTreeY->annkFRSearch( dataY[ i ], distsXY[ i ], 0, NULL, NULL, eps);
// digamma evaluations
marginal_contrib += gsl_sf_psi_int( no_pts_X+1 ) + gsl_sf_psi_int( no_pts_Y+1 );
}
MI_est = gsl_sf_psi_int( k ) + gsl_sf_psi_int( N ) - marginal_contrib / (double)N;
// Deallocate memory
delete kdTreeX;
delete kdTreeY;
delete [] distsXY;
annDeallocPts( dataX );
annDeallocPts( dataY );
return MI_est;
}
ANNpointArray CMDPointVector::GetANNpointArray(const vector<int>& in)const
{
const CMDPointVector& me = *this;
ANNpointArray ptsArray = annAllocPts((int)size(), (int)in.size());
for (size_t i = 0; i < size(); i++)
{
ASSERT(i == 0 || at(i - 1).GetNbDimension() == at(i).GetNbDimension());
for (size_t j = 0; j < in.size(); j++)
{
ASSERT(in[j] < (int)at(i).GetNbDimension());
if (m_bStandardized)
{
double e = m_dimStats[in[j]][STD_DEV];
if (e>0)
ptsArray[i][j] = (me[i][in[j]] - m_dimStats[in[j]][MEAN]) / e;
else ptsArray[i][j] = 0;
}
else
{
ptsArray[i][j] = me[i][in[j]];
}
}
}
//ANNpointArray ptsArray = annAllocPts((int)size(), (int)198);
//for(size_t i=0; i<size(); i++)
//{
// ASSERT( i==0 || at(i-1).GetNbDimension() == at(i).GetNbDimension());
//
// for(size_t j=0; j<in.size(); j++)
// ptsArray[i][GetK(in[j])]=0;
//
//
// for(size_t j=0; j<in.size(); j++)
// {
// ASSERT( in[j] < (int)at(i).GetNbDimension() );
// int k = in[j];
// if( m_bStandardized )
// {
// double e = m_dimStats[k][STD_DEV];
// if( e>0)
// ptsArray[i][GetK(in[j])] += (me[i][k]-m_dimStats[k][MEAN])/e;
//
// }
// else
// {
// ptsArray[i][GetK(in[j])] += me[i][k];
// }
// }
//}
return ptsArray;
}
PView *GMSH_NearestNeighborPlugin::execute(PView *v)
{
int iView = (int)NearestNeighborOptions_Number[0].def;
PView *v1 = getView(iView, v);
if(!v1) return v;
PViewData *data1 = v1->getData();
int totpoints = data1->getNumPoints();
if(!totpoints){
Msg::Error("View[%d] contains no points", iView);
return 0;
}
#if defined(HAVE_ANN)
ANNpointArray zeronodes = annAllocPts(totpoints, 3);
int k = 0, step = 0;
for(int ent = 0; ent < data1->getNumEntities(step); ent++){
for(int ele = 0; ele < data1->getNumElements(step, ent); ele++){
if(data1->skipElement(step, ent, ele)) continue;
int numNodes = data1->getNumNodes(step, ent, ele);
if(numNodes != 1) continue;
data1->getNode(step, ent, ele, 0, zeronodes[k][0], zeronodes[k][1],
zeronodes[k][2]);
k++;
}
}
ANNkd_tree *kdtree = new ANNkd_tree(zeronodes, totpoints, 3);
ANNidxArray index = new ANNidx[2];
ANNdistArray dist = new ANNdist[2];
v1->setChanged(true);
for(int ent = 0; ent < data1->getNumEntities(step); ent++){
for(int ele = 0; ele < data1->getNumElements(step, ent); ele++){
if(data1->skipElement(step, ent, ele)) continue;
int numNodes = data1->getNumNodes(step, ent, ele);
if(numNodes != 1) continue;
double xyz[3];
data1->getNode(step, ent, ele, 0, xyz[0], xyz[1], xyz[2]);
kdtree->annkSearch(xyz, 2, index, dist);
data1->setValue(step, ent, ele, 0, 0, sqrt(dist[1]));
}
}
delete kdtree;
annDeallocPts(zeronodes);
delete [] index;
delete [] dist;
#else
Msg::Error("Nearest neighbor computation requires ANN");
#endif
data1->setName(v1->getData()->getName() + "_NearestNeighbor");
data1->finalize();
return v1;
}
// allocate an ANN point array and copy points from mxArray to ANN point array
ANNpointArray CreateANNPointArray(int d, int n, const mxArray* arr)
{
// allocate the ANN array
ANNpointArray pts = annAllocPts(n, d);
// copy the point coordinates
::memcpy(pts[0], mxGetPr(arr), sizeof(double) * d * n);
return pts;
}
NearestNeighbor::NearestNeighbor(int nPts)
{
this->nPts = nPts;
try {
dataPts = annAllocPts(nPts, dim); // allocate data points
}
catch (const std::bad_alloc& e) {
FILE_LOG(logINFO)
<< "Bad Allocation in Nearest Neighbor - annAllocPts("
<< nPts << ")";
}
}
void ICP::createKDTree()
{
m_modPts = annAllocPts(m_modSet.rows, ICP_DIMS);
#pragma omp parallel for
for (int r = 0; r < m_modSet.rows; r++)
{
Vec3d p = m_modSet.at<Vec3d>(r, 0);
m_modPts[r][0] = p[0];
m_modPts[r][1] = p[1];
m_modPts[r][2] = p[2];
}
m_kdTree = new ANNkd_tree(m_modPts, m_modSet.rows, ICP_DIMS);
}
ANNpointArray read_points(const char *filename, int &count, int &dim)
{
int res;
FILE *f = fopen(filename, "r");
if (!f) {
fprintf(stderr, "error: could not open file: %s", filename);
exit(1);
}
res = fscanf(f, "%d %d\n", &count, &dim);
if (res != 2) {
fprintf(stderr, "error: invalid header: %s\n", filename);
exit(1);
}
if (count < 0) {
fprintf(stderr, "error: invalid point count: %s: %d\n", filename, count);
exit(1);
}
if (dim < 2) {
fprintf(stderr, "error: invalid dimension: %s: %d\n", filename, dim);
exit(1);
}
ANNpointArray pts = annAllocPts(count, dim);
char buffer[256];
for (int i = 0; i < count; ++i) {
double value;
if (fgets(buffer, 255, f) == 0) {
fprintf(stderr, "error: short file: %s\n", filename);
exit(1);
}
char *start = buffer;
char *end = start;
for (int d = 0; d < dim; ++d) {
while (*end != ',' && *end != ' ' && *end != 0) ++end;
*end = 0;
value = atof(start);
start = end + 1;
pts[i][d] = value;
}
}
fclose(f);
return pts;
}
//construit une instance de la classe a partir des images sources A et Ap
//à un level l, on cherchera une correspondance en utilisant un vosinage mgk
//au niveau l et mpk au niveau inferrieur
annUse::annUse(Image* A,Image* Ap,int l, int mpk,int mgk, FeatureType Type)
{
pk=mpk;
gk=mgk;
int m,M;
uint32 largeurA = A->getLargeur(l);
if (largeurA/2 != (largeurA-1)/2) {
largeurA--;
}
uint32 hauteurA = A->getHauteur(l);
if (hauteurA/2 != (hauteurA-1)/2) {
hauteurA--;
}
if(gk>=2*pk) {
m=gk;
M=gk+1;
} else {
m=pk*2;
M=pk*2+1;
}
int gK=gk*2+1;
int pK=pk*2+1;
Features f;
//calcul de la taille d'un point en fonction de la taille du voisinnage
dim=(gK*gK+pK*pK*2+(gK*gK)/2)*f.getNBChamps(Type);
eps=0;
//calcul du nombre de points
nPts= (largeurA-m-M)*(hauteurA-m-M);
//allocation des ressources
dataPts = annAllocPts(nPts, dim);
nnIdx = new ANNidx[1];
dists = new ANNdist[1];
//remplissage du tableau dataPts
int i=0;
for(uint32 y=m; y<hauteurA-M ; y++)
for(uint32 x=m; x<largeurA-M; x++)
{
makeANNPoint(A,Ap,l,x,y,Type,dataPts[i]);
i++;
}
//construction de l'arbre de recherche
kdTree = new ANNkd_tree( dataPts, nPts, dim);
}
bool arlCore::Mesh::simplify( void )
{
#ifndef ANN
return false;
#else // ANN
unsigned int i, j;
const unsigned int Size = m_pointList->visibleSize();
const unsigned int Dimension = m_pointList->getDimension();
ANNpointArray ANNPoints = annAllocPts( Size, Dimension );
for( i=0 ; i<m_pointList->size() ; ++i )
for( j=0 ; j<Dimension ; ++j )
ANNPoints[i][j]=(*m_pointList)[i]->get(j);
const int BucketSize = 1;
ANNkd_tree* ANNtree = new ANNkd_tree( ANNPoints, Size, Dimension, BucketSize, ANN_KD_SL_MIDPT );
const double Epsilon = 1e-8;// Error bound
const double SquaredEpsilon = Epsilon*Epsilon;
ANNpoint ANNPt = annAllocPt(Dimension); // Query point
const unsigned int NbNeighbors = 20;
ANNidxArray Nn_idx = new ANNidx[NbNeighbors]; // near neighbor indices
ANNdistArray SquaredDists = new ANNdist[NbNeighbors]; // near neighbor distances
for( i=0 ; i<m_pointList->size() ; ++i )
if((*m_pointList)[i]->isVisible())
{
std::vector<unsigned int> oldIndex;
for( j=0 ; j<Dimension; ++j )
ANNPt[j] = (*m_pointList)[i]->get(j);
ANNtree->annkSearch( ANNPt, NbNeighbors, Nn_idx, SquaredDists, Epsilon );
// Cherche points les plus proches
for( j=0 ; j<NbNeighbors ; ++j )
if(SquaredDists[j]<=SquaredEpsilon)
{
releasePoint(Nn_idx[j]);
oldIndex.push_back(Nn_idx[j]);
}
replacePointIndex(oldIndex, i);
}
delete ANNtree;
annDeallocPt( ANNPt );
annDeallocPts( ANNPoints );
delete[] Nn_idx;
delete[] SquaredDists;
annClose();
return true;
#endif // ANN
}
/**
* Creates an ANN Point array from a memory block (d x n doubles)
*/
ANNpointArray createAnnPointArray(const double *data, int d, int n)
{
// allocate the ANN point array
ANNpointArray pts = annAllocPts(n, d);
// copy the points
//
// In the annAllocPts implementation all point coordinates are
// continuous in the memory,
// so all points can be copied in a batch
//
if (n > 0)
{
memcpy(pts[0], data, sizeof(double) * d * n);
}
return pts;
}
void KMeans::initAnn() const {
// This is threadsafe now
mutex_.lock();
{
if (kd_tree_)
delete kd_tree_;
if (ann_points_)
annDeallocPts( ann_points_ );
ann_points_ = annAllocPts( n_center_, feature_size_ );
for( int i=0, k=0; i<n_center_; i++ )
for( int j=0; j<feature_size_; j++, k++ )
ann_points_[i][j] = center_[k];
kd_tree_ = new ANNkd_tree( ann_points_, n_center_, feature_size_ );
}
mutex_.unlock();
}
void Centerline::buildKdTree()
{
FILE * f = Fopen("myPOINTS.pos","w");
fprintf(f, "View \"\"{\n");
int nbPL = 3; //10 points per line
//int nbNodes = (lines.size()+1) + (nbPL*lines.size());
int nbNodes = (colorp.size()) + (nbPL*lines.size());
ANNpointArray nodes = annAllocPts(nbNodes, 3);
int ind = 0;
std::map<MVertex*, int>::iterator itp = colorp.begin();
while (itp != colorp.end()){
MVertex *v = itp->first;
nodes[ind][0] = v->x();
nodes[ind][1] = v->y();
nodes[ind][2] = v->z();
itp++; ind++;
}
for(unsigned int k = 0; k < lines.size(); ++k){
MVertex *v0 = lines[k]->getVertex(0);
MVertex *v1 = lines[k]->getVertex(1);
SVector3 P0(v0->x(),v0->y(), v0->z());
SVector3 P1(v1->x(),v1->y(), v1->z());
for (int j= 1; j < nbPL+1; j++){
double inc = (double)j/(double)(nbPL+1);
SVector3 Pj = P0+inc*(P1-P0);
nodes[ind][0] = Pj.x();
nodes[ind][1] = Pj.y();
nodes[ind][2] = Pj.z();
ind++;
}
}
kdtree = new ANNkd_tree(nodes, nbNodes, 3);
for(int i = 0; i < nbNodes; ++i){
fprintf(f, "SP(%g,%g,%g){%g};\n",
nodes[i][0], nodes[i][1],nodes[i][2],1.0);
}
fprintf(f,"};\n");
fclose(f);
}
double estimateMI_ANN( const RV_1<P_V,P_M>& xRV,
const RV_2<P_V,P_M>& yRV,
const unsigned int xDimSel[], unsigned int dimX,
const unsigned int yDimSel[], unsigned int dimY,
unsigned int k, unsigned int N, double eps )
{
ANNpointArray dataXY;
double MI_est;
unsigned int dimXY = dimX + dimY;
// Allocate memory
dataXY = annAllocPts(N,dimXY);
// Copy samples in ANN data structure
P_V smpRV_x( xRV.imageSet().vectorSpace().zeroVector() );
P_V smpRV_y( yRV.imageSet().vectorSpace().zeroVector() );
for( unsigned int i = 0; i < N; i++ ) {
// get a sample from the distribution
xRV.realizer().realization( smpRV_x );
yRV.realizer().realization( smpRV_y );
// copy the vector values in the ANN data structure
for( unsigned int j = 0; j < dimX; j++ ) {
dataXY[ i ][ j ] = smpRV_x[ xDimSel[j] ];
}
for( unsigned int j = 0; j < dimY; j++ ) {
dataXY[ i ][ dimX + j ] = smpRV_y[ yDimSel[j] ];
}
// annPrintPt( dataXY[i], dimXY, std::cout ); std::cout << std::endl;
}
MI_est = computeMI_ANN( dataXY,
dimX, dimY,
k, N, eps );
// Deallocate memory
annDeallocPts( dataXY );
return MI_est;
}
void Slave::operator()() {
ANNpointArray child_centers = annAllocPts(Globals::pow2dim, Globals::dim);
int it;
for (int i=0; i<Globals::pow2dim; i++) {
it = 1;
for (int j=0; j<Globals::dim; j++) {
if ((i&it) != 0) {
child_centers[i][j] = 0.75;
} else {
child_centers[i][j] = 0.25;
}
it = it << 1;
}
if (i%Globals::threads_num == id) {
//TODO make this dynamically allocate pieces to threads
std::cout << "Thread " << id << " gets branch " << i << std::endl;
fil_in_reprs(Globals::qb->children[0]+i,&child_centers[i],0.5);
}
}
annDeallocPts(child_centers);
}
void mitk::PointLocator::SetPoints(mitk::PointSet *points)
{
if (points == nullptr)
{
itkWarningMacro("Points are NULL!");
return;
}
if (m_MitkPoints)
{
if ((m_MitkPoints == points) && (m_MitkPoints->GetMTime() == points->GetMTime()))
{
return; // no need to recalculate search tree
}
}
m_MitkPoints = points;
size_t size = points->GetSize();
if (m_ANNDataPoints != nullptr)
delete[] m_ANNDataPoints;
m_ANNDataPoints = annAllocPts(size, m_ANNDimension);
m_IndexToPointIdContainer.clear();
m_IndexToPointIdContainer.resize(size);
size_t counter = 0;
mitk::PointSet::PointsContainer *pointsContainer = points->GetPointSet()->GetPoints();
mitk::PointSet::PointsContainer::Iterator it;
mitk::PointSet::PointType currentPoint;
mitk::PointSet::PointsContainer::ElementIdentifier currentId;
for (it = pointsContainer->Begin(); it != pointsContainer->End(); ++it, ++counter)
{
currentPoint = it->Value();
currentId = it->Index();
(m_ANNDataPoints[counter])[0] = currentPoint[0];
(m_ANNDataPoints[counter])[1] = currentPoint[1];
(m_ANNDataPoints[counter])[2] = currentPoint[2];
m_IndexToPointIdContainer[counter] = currentId;
}
InitANN();
}
void Update() {
skipCalculation = true;
unsigned int modelsNFeats = 0;
foreach( model, *models )
modelsNFeats += model->IPs[DescriptorType].size();
correspModel.resize( modelsNFeats );
correspFeat.resize( modelsNFeats );
ANNpointArray refPts = annAllocPts( modelsNFeats , DescriptorSize );
int x=0;
for( int nModel = 0; nModel < (int)models->size(); nModel++ ) {
vector<Model::IP> &IPs = (*models)[nModel]->IPs[DescriptorType];
for( int nFeat = 0; nFeat < (int)IPs.size(); nFeat++ ) {
correspModel[x] = nModel;
correspFeat[x] = &IPs[nFeat].coord3D;
norm( IPs[nFeat].descriptor );
for( int i=0; i<DescriptorSize; i++ )
refPts[x][i] = IPs[nFeat].descriptor[i];
x++;
}
}
if( modelsNFeats > 1 ) {
skipCalculation = false;
if( kdtree ) delete kdtree;
kdtree = new ANNkd_tree(refPts, modelsNFeats, DescriptorSize );
}
configUpdated = false;
}
void KDTree3::BuildTree(const Vector<Vec3f> &Points)
{
FreeMemory();
UINT PointCount = Points.Length();
Console::WriteString(String("Building KD tree, ") + String(PointCount) + String(" points..."));
queryPt = annAllocPt(3); // allocate query point
dataPts = annAllocPts(PointCount, 3); // allocate data points
nnIdx = new ANNidx[KDTree3MaxK]; // allocate near neigh indices
dists = new ANNdist[KDTree3MaxK]; // allocate near neighbor dists
for(UINT i = 0; i < PointCount; i++)
{
for(UINT ElementIndex = 0; ElementIndex < 3; ElementIndex++)
{
dataPts[i][ElementIndex] = Points[i][ElementIndex];
}
}
kdTree = new ANNkd_tree( // build search structure
dataPts, // the data points
PointCount, // number of points
3); // dimension of space
Console::WriteString(String("done.\n"));
}
ANN::ANN(ANNpointArray points, int n1, int n2, int size, int dim, int *topology, ANNpoint scaling, int NumNeighbors) {
int n = 0, i = 0, j = 0;
anncount++;
if (anncount == 1)
std::cout << "MPNN library" << std::endl;
numPoints = size;
dimension = dim;
query_pt = annAllocPt(dimension); // allocate query point
data_pts = annAllocPts(numPoints, dimension); // allocate data points
nn_idx = new ANNidx[NumNeighbors]; // allocate near neighbor indices
dists = new ANNdist[NumNeighbors]; // allocate near neighbor dists
epsilon = 0.0;
node_indices = new int[numPoints]; // allocate indices of the points
// Copy nodes to data_pts
i = 0;
for (n = n1; n!= n2; n++) {
node_indices[i] = n;
for (j = 0; j < dimension; j++) {
data_pts[i][j] = scaling[j]*points[n][j];
}
i++;
}
// Initialize ANN
the_tree = new ANNkd_tree(data_pts, // the data points
numPoints, // number of points
dimension, // dimension of space
scaling, // scaling of the coordinates
topology); // topology of the space
}
//process all that shit in vectors.... very gay
void fill_vectors(QNode* qb, ANNpoint* center, double side) {
if (qb->children == 0) {
Globals::qt_nodes.push_back(qb);
Globals::qt_centers.push_back(center);
Globals::qt_sides.push_back(side);
//std::cout << side << std::endl;
} else {
int it;
ANNpointArray child_centers = annAllocPts(Globals::pow2dim, Globals::dim);
for (int i=0; i<Globals::pow2dim; i++) {
it = 1;
for (int j=0; j<Globals::dim; j++) {
if ((i&it) != 0) {
child_centers[i][j] = (*center)[j]+(side/4.0);
} else {
child_centers[i][j] = (*center)[j]-(side/4.0);
}
it = it << 1;
}
fill_vectors(qb->children[0]+i,&child_centers[i],side/2.0);
}
annDeallocPts(child_centers);
}
}
Worker::Worker(int id) : id(id) {
//TODO:delete
asdf = new ANNmin_k(1);
//nnIdx = new ANNidx[1];
//dists = new ANNdist[1];
leaf_centers=annAllocPts(pow(2,Globals::dimm2),Globals::dim);
leaf_reprs=new int[Globals::pow2dimm2];
qb_center = annAllocPt(Globals::dim);
std::vector<int> indices;
//leaf_center=annAllocPt(Globals::dim);
for (int i=0; i<Globals::dimm2; i++) {
indices.push_back(0);
result.push_back(0.0);
pos_neg_result.push_back(0.0);
Vc pos_neg_dim;
pos_neg_dim.push_back(0.0);
pos_neg_dim.push_back(0.0);
pos_neg.push_back(pos_neg_dim);
}
for (int i=0; i<pow(2,Globals::dimm2); i++) {
Vc pos_neg_dim_final;
for (int j=0; j<Globals::dim; j++)
pos_neg_dim_final.push_back(0.0);
pos_neg_final.push_back(pos_neg_dim_final);
}
for (int i=0; i<Globals::dimm2; i++) {
Digits d;
vd2.push_back(d);
}
for (int i=0; i<Globals::dimm2; i++) {
Digits d;
vd1.push_back(d);
}
kdtree = new ANNkd_tree(Globals::ann_points, Globals::len, Globals::dim);
}
void Centerline::distanceToSurface()
{
Msg::Info("Centerline: computing distance to surface mesh ");
//COMPUTE WITH REVERSE ANN TREE (SURFACE POINTS IN TREE)
std::set<MVertex*> allVS;
for(unsigned int j = 0; j < triangles.size(); j++)
for(int k = 0; k<3; k++) allVS.insert(triangles[j]->getVertex(k));
int nbSNodes = allVS.size();
ANNpointArray nodesR = annAllocPts(nbSNodes, 3);
vertices.resize(nbSNodes);
int ind = 0;
std::set<MVertex*>::iterator itp = allVS.begin();
while (itp != allVS.end()){
MVertex *v = *itp;
nodesR[ind][0] = v->x();
nodesR[ind][1] = v->y();
nodesR[ind][2] = v->z();
vertices[ind] = v;
itp++; ind++;
}
kdtreeR = new ANNkd_tree(nodesR, nbSNodes, 3);
for(unsigned int i = 0; i < lines.size(); i++){
MLine *l = lines[i];
MVertex *v1 = l->getVertex(0);
MVertex *v2 = l->getVertex(1);
double midp[3] = {0.5*(v1->x()+v2->x()), 0.5*(v1->y()+v1->y()),0.5*(v1->z()+v2->z())};
ANNidx index[1];
ANNdist dist[1];
kdtreeR->annkSearch(midp, 1, index, dist);
double minRad = sqrt(dist[0]);
radiusl.insert(std::make_pair(lines[i], minRad));
}
}
void NormalEstimator::fitNormal(){
//
//compute surface normal
int ptCnt = m_pt.size();
if (ptCnt <= 0)
{
std::cout << "no point set provided" << std::endl;
return;
}
ANNpointArray ptArray = annAllocPts(ptCnt, 3);
//assign point values
for (int i=0; i<ptCnt; i++) {
cv::Vec3f pt = m_pt[i];
ANNpoint ptPtr = ptArray[i];
ptPtr[0] = pt[0];
ptPtr[1] = pt[1];
ptPtr[2] = pt[2];
}
///
ANNkd_tree kdt(ptArray, ptCnt, 3);
ANNpoint queryPt = annAllocPt(3);
int nnCnt = 100;
ANNidxArray nnIdx = new ANNidx[nnCnt];
ANNdistArray nnDist = new ANNdist[nnCnt];
float sigma = -1;
float evalRatio = 0.05;
//estimate sigma
for (int i=0; i < ptCnt; i++) {
cv::Vec3f pt = m_pt[i];
queryPt[0] = pt[0];
queryPt[1] = pt[1];
queryPt[2] = pt[2];
//kdt.annkSearch(queryPt,nnCnt, nnIdx, nnDist);
kdt.annkSearch(queryPt,50, nnIdx, nnDist);
if (nnDist[49] < sigma ||sigma == -1 )
{
sigma = nnDist[49];
}
}
sigma = 0.001;
std::cout << "search radius:" << sigma << std::endl;
std::cout << "estimating normals for point set by PCA, be patient..." << std::endl;
for (int i=0; i < ptCnt; i++) {
cv::Vec3f pt = m_pt[i];
queryPt[0] = pt[0];
queryPt[1] = pt[1];
queryPt[2] = pt[2];
//kdt.annkSearch(queryPt,nnCnt, nnIdx, nnDist);
kdt.annkFRSearch(queryPt, sigma, nnCnt, nnIdx, nnDist);
int validCnt = 0;
for (int j = 0; j < nnCnt; j++)
{
if (nnIdx[j] == ANN_NULL_IDX)
{
break;
}
validCnt++;
}
//std::cout << validCnt << std::endl;
if (validCnt < 3)
{
continue;
}
cv::Mat pcaVec(validCnt,3,CV_64FC1);
cv::Mat pcaMean(1,3,CV_64FC1);
for (int j = 0; j < validCnt; j++)
{
pcaVec.at<double>(j,0) = m_pt[nnIdx[j]][0];
pcaVec.at<double>(j,1) = m_pt[nnIdx[j]][1];
pcaVec.at<double>(j,2) = m_pt[nnIdx[j]][2];
}
cv::PCA pca(pcaVec,cv::Mat(),CV_PCA_DATA_AS_ROW);
if (pca.eigenvalues.at<double>(2,0) / pca.eigenvalues.at<double>(1,0) > evalRatio)
{
continue;
}
m_ptNorm[i] = cv::Vec3f(pca.eigenvectors.at<double>(2,0),pca.eigenvectors.at<double>(2,1),pca.eigenvectors.at<double>(2,2));
float nr = cv::norm(m_ptNorm[i]);
m_ptNorm[i][0] /= nr;
m_ptNorm[i][1] /= nr;
m_ptNorm[i][2] /= nr;
//std::cout << m_ptNorm[i][0] << " " << m_ptNorm[i][1] << " " << m_ptNorm[i][2] << std::endl;
m_ptNormFlag[i] = true;
}
//
std::cout << "done..." << std::endl;
//////////////////////////////////////////////////////////////////////////
//std::cout << "correct normal direction..." << std::endl;
//sigma *= 1; //
//nnCnt *= 1; //
////reallocate the space for nn idx and nn dist array
//delete [] nnDist;
//delete [] nnIdx;
//nnIdx = new ANNidx[nnCnt];
//nnDist = new ANNdist[nnCnt];
//int invertCnt = 0;
//for (int i = 0; i < ptCnt; i++)
//{
// if (!m_ptNormFlag[i])
// {
// continue;
// }
//
// cv::Vec3f pt = m_pt[i];
// queryPt[0] = pt[0];
// queryPt[1] = pt[1];
// queryPt[2] = pt[2];
// kdt.annkFRSearch(queryPt, sigma, nnCnt, nnIdx, nnDist);
// int validCnt = 0, normConsCnt = 0, distConsCnt = 0;
// for (int j = 0; j < nnCnt; j++)
// {
// if (nnIdx[j] == ANN_NULL_IDX)
// {
// break;
// }
// else{
// //check the direction
// cv::Vec3f v1 = m_ptNorm[i];
// cv::Vec3f v2 = m_ptNorm[nnIdx[j]];
//
// if (!m_ptNormFlag[nnIdx[j]])
// {
// continue;
// }else{
// //
// validCnt++;
// if( v2.ddot(v1) > 0 )
// normConsCnt++;
// }
// }
// }
// //inconsistency detected, invert the direction
// if (normConsCnt / validCnt < 0.9)
// {
// //std::cout << "invert" << std::endl;
// invertCnt++;
// m_ptNorm[i] = cv::Vec3f(0,0,0) - m_ptNorm[i];
// }
//}
//std::cout << "# of inverted vertex:" << invertCnt << std::endl;
////////////////////////////////////////////////////////////////////////////
annDeallocPt(queryPt);
annDeallocPts(ptArray);
delete [] nnDist;
delete [] nnIdx;
}
void FeatureSearch::initSearch()
{
m_countFeatures = 0;
for(std::vector<SearchPairInfo>::iterator itSPI = m_infos.begin(); itSPI != m_infos.end(); itSPI++)
{
m_countFeatures += itSPI->area.width() * itSPI->area.height();
}
// Allocate the array of points
m_pointArray = annAllocPts( m_countFeatures, m_spaceDimension );
for(std::vector<SearchPairInfo>::iterator itSPI = m_infos.begin(); itSPI != m_infos.end(); itSPI++)
{
// Initialize the descriptors
int cacheLineCount = (itSPI->area.width() + diameter());
size_t cacheLineSize = cacheLineCount * sizeof(Feature);
// pixels will hold a cache of the luminosity to feed the descriptors
Feature** pixels = new Feature*[diameter()];
for(int i = 0; i < diameter(); i++)
{
pixels[i] = new Feature[ cacheLineCount ];
}
// KisColorSpace* cs = dev->colorSpace();
// Read the first 'radius' line to initialize the cache
KisHLineIterator itDevA = itSPI->devA->createHLineIterator(itSPI->area.x(), itSPI->area.y(), itSPI->area.width(), false);
KisHLineIterator itDevAPrime = itSPI->devAPrime->createHLineIterator(itSPI->area.x(), itSPI->area.y(), itSPI->area.width(), false);
// Intialize the first row of the cache, as the cache needs to be full before it is possible to start creating descriptors for the key points
for(int indexInPixels = radius(); indexInPixels < diameter() - 1; ++indexInPixels)
{
deviceToCache(itDevA, pixels[indexInPixels], radius());
itDevA.nextRow();
}
// Copy the first line, as the first 'radius' columns are initialized with the same pixel value
for(int i = 0; i < radius(); ++i)
{
memcpy(pixels[i], pixels[radius()], cacheLineSize);
}
// Main loop
int posInAP = 0;
for(int y = 0; y < itSPI->area.height(); y++)
{
// Fill the last line of the cache
if( itDevA.y() <= itSPI->area.bottom())
{
deviceToCache(itDevA, pixels[diameter() - 1], radius());
} else { // No more line in the device, so copy the last line
memcpy(pixels[diameter() - 1], pixels[diameter() - 2], cacheLineSize);
}
itDevA.nextRow();
// Use the cache to fill the array of points
for(int x = 0; x < itSPI->area.width(); x++)
{
int subPos = 0;
// kdDebug() << " Feature : " << posInAP << endl;
for(int i = 0; i < diameter(); i++)
{
for(int j = 0; j < diameter(); j++)
{
pixels[i][j + x].convertToArray((m_pointArray[ posInAP ]) + subPos );
// kdDebug() << subPos << " "<< pixels[i][j + x] << endl;
subPos++;
}
}
m_values.push_back(*reinterpret_cast<float*>(itDevAPrime.rawData()));
++itDevAPrime;
++posInAP;
}
itDevAPrime.nextRow();
swapCache(pixels, diameter());
}
// delete the luminosity cache
for(int i = 0; i < diameter(); i++)
{
delete[] pixels[i];
}
delete[] pixels;
}
// Initialize the search tree
m_tree = new ANNkd_tree( m_pointArray, m_countFeatures, m_spaceDimension);
}
// Driver program
int main(int argc, char **argv)
{
int num_points = 0; // Actual number of data points
ANNpointArray data_points; // Data points
ANNpoint query_point; // Query point
ANNidxArray near_neighbor_idx; // Near neighbor indices
ANNdistArray near_neighbor_distances;// Near neighbor distances
ANNkd_tree * kd_tree_adt; // ADT search structure
UserInterface UI;
// Read command-line arguments
UI.getArgs(argc, argv);
// Allocate query point
query_point = annAllocPt(UI.dimension);
// Allocate data points
data_points = annAllocPts(UI.max_points, UI.dimension);
// Allocate near neighbor indices
near_neighbor_idx = new ANNidx[UI.k];
// Allocate near neighbor distances
near_neighbor_distances = new ANNdist[UI.k];
// Echo data points
cout << "Data Points: \n";
if (UI.results_out != NULL)
*(UI.results_out) << "Data points: \n";
while (num_points < UI.max_points && UI.readPoint(*(UI.data_in), data_points[num_points]))
{
UI.printPoint(cout, data_points[num_points], num_points);
if (UI.results_out != NULL)
{
UI.printPoint(*(UI.results_out), data_points[num_points], num_points);
}
num_points++;
}
// Construct k-d tree abstract data type search structure
// Params: data points, number of points, dimension of space
kd_tree_adt = new ANNkd_tree(data_points, num_points, UI.dimension);
// Echo query point(s)
cout << "\n\nQuery points: \n";
if (UI.results_out != NULL)
*(UI.results_out) << "\n\nQuery points: \n";
// Read query points
while (UI.readPoint(*(UI.query_in), query_point))
{
UI.printPoint(cout, query_point, UI.dimension);
if (UI.results_out != NULL)
{
UI.printPoint(*(UI.results_out), query_point, UI.dimension);
}
// Perform the search
// Params: query point, number of near neighbors, nearest neighbors (returned), distance (returned), error bound
kd_tree_adt->annkSearch(query_point, UI.k, near_neighbor_idx, near_neighbor_distances, UI.eps);
UI.printSummary(cout, &near_neighbor_idx, &near_neighbor_distances);
if (UI.results_out != NULL)
UI.printSummary(*(UI.results_out), &near_neighbor_idx, &near_neighbor_distances);
}
// Perform house cleaning tasks
delete kd_tree_adt;
delete [] near_neighbor_idx;
delete [] near_neighbor_distances;
annClose();
cin.get();
return EXIT_SUCCESS;
}
arlCore::ICP::ICP( arlCore::PointList::csptr model, arlCore::PointList::csptr cloud, bool justVisible ):
m_point2PointMode(true),
m_modelMesh(),
m_cloud(),
m_initialization(false),
m_justVisible(justVisible),
m_modelSize(0),
m_cloudSize(0),
m_modelPoints(0),
m_cloudPoints(0),
m_Pk(0),
m_Yk(0),
m_Pi(0),
m_ANNtree(0),
m_nn_idx(0),
m_squaredDists(0),
m_maxIterations(50),
m_nbIterations(0),
m_startError(-1),
m_endError(-1)
{
m_solution.setIdentity();
#ifdef ANN
const unsigned int m_dimension = model->getDimension();
assert(m_dimension==cloud->getDimension());
m_modelGravity.set_size(m_dimension);
m_cloudGravity.set_size(m_dimension);
unsigned int i, j, n;
if(justVisible) m_modelSize = model->visibleSize();
else m_modelSize = model->size();
if(m_modelSize<1) return;
m_modelPoints = annAllocPts( m_modelSize, m_dimension );
for( i=0, n=0 ; i<m_modelSize ; ++i )
if((*model)[i])
if(!justVisible || (justVisible && (*model)[i]->isVisible()))
{
for( j=0 ; j<m_dimension ; ++j )
m_modelPoints[n][j]=(*model)[i]->get(j);
++n;
}
m_modelSize = n;
if(justVisible) m_cloudSize = cloud->visibleSize();
else m_cloudSize = cloud->size();
if(m_modelSize<1 || m_cloudSize<1)
{
annDeallocPts( m_modelPoints );
m_modelPoints = 0;
return;
}
m_cloudPoints = annAllocPts( m_cloudSize, m_dimension );
for( i=0, n=0 ; i<m_cloudSize ; ++i )
if((*cloud)[i])
if(!justVisible || (justVisible && (*cloud)[i]->isVisible()))
{
for( j=0 ; j<m_dimension ; ++j )
m_cloudPoints[n][j]=(*cloud)[i]->get(j);
++n;
}
m_cloudSize = n;
if(m_cloudSize<1)
{
annDeallocPts( m_modelPoints );
annDeallocPts( m_cloudPoints );
m_modelPoints = 0;
m_cloudPoints = 0;
return;
}
m_Pk = annAllocPts( m_cloudSize, m_dimension );
m_Yk = annAllocPts( m_cloudSize, m_dimension );
m_Pi = annAllocPts( m_cloudSize, m_dimension );
const int BucketSize = 1;
m_ANNtree = new ANNkd_tree( m_modelPoints, m_modelSize, m_dimension, BucketSize, ANN_KD_SL_MIDPT );
m_nbNN = 1;
m_nn_idx = new ANNidx[m_nbNN];
m_squaredDists = new ANNdist[m_nbNN];
#endif // ANN
}