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
}
void ImplicitKdTree::closest_point_normal_query(Vector3 _query, Vector3& point, Vector3& normal) {
ANNidxArray nnIdx = new ANNidx[1];
ANNdistArray dists = new ANNdist[1];
ANNpoint query_pt = annAllocPt(3);
query_pt[0] = _query.x;
query_pt[1] = _query.y;
query_pt[2] = _query.z;
kd_tree->annkSearch(query_pt, 1, nnIdx, dists);
ANNpoint found_pt = point_array[nnIdx[0]];
point = Vector3(found_pt[0], found_pt[1], found_pt[2]);
normal = shape->normal(point);
delete [] nnIdx;
delete [] dists;
}
void process( FrameData &frameData ) {
if( configUpdated ) Update();
if( skipCalculation ) return;
vector< FrameData::DetectedFeature > &corresp = frameData.detectedFeatures[DescriptorType];
if( corresp.empty() ) return;
vector< vector< FrameData::Match > > &matches = frameData.matches;
matches.resize( models->size() );
ANNpoint pt = annAllocPt(DescriptorSize);
ANNidxArray nx = new ANNidx[2];
ANNdistArray ds = new ANNdist[2];
for( int i=0; i<(int)corresp.size(); i++) {
norm( corresp[i].descriptor);
for (int j = 0; j < DescriptorSize; j++)
pt[j] = corresp[i].descriptor[j];
#pragma omp critical(ANN)
kdtree->annkSearch(pt, 2, nx, ds, Quality);
if( ds[0]/ds[1] < Ratio ) {
int nModel1 = correspModel[nx[0]];
if( matches[nModel1].capacity() < 1000 ) matches[nModel1].reserve(1000);
matches[nModel1].resize( matches[nModel1].size() +1 );
FrameData::Match &match = matches[nModel1].back();
match.imageIdx = corresp[i].imageIdx;
match.coord3D = *correspFeat[nx[0]];
match.coord2D = corresp[i].coord2D;
}
}
}
vector<int> PointKdTree::ball_query(Vector3 _query, double _radius) {
ANNpoint query_pt = annAllocPt(3);
query_pt[0] = _query.x;
query_pt[1] = _query.y;
query_pt[2] = _query.z;
double sqd_radius = _radius*_radius;
int the_k = kd_tree->annkFRSearch(query_pt, sqd_radius, 0);
ANNidxArray nnIdx = new ANNidx[the_k];
ANNdistArray dists = new ANNdist[the_k];
kd_tree->annkFRSearch(query_pt, sqd_radius, the_k, nnIdx, dists);
vector<int> ball_indices;
for(int i = 0; i < the_k; i++)
ball_indices.push_back(nnIdx[i]);
delete [] nnIdx;
delete [] dists;
return ball_indices;
}
ANNpoint CMDPointVector::GetANNpoint(const CMDPoint& pt, const vector<int>& in)const
{
ASSERT(pt.GetNbDimension() == GetNbDimension());
ANNpoint q = annAllocPt((int)in.size());
for (size_t i = 0; i < in.size(); i++)
{
int k = in[i];
if (m_bStandardized)
{
ASSERT(abs(sqrt(m_dimStats[k][VARIANCE]) - m_dimStats[k][STD_DEV]) < 0.000001);
q[i] = (pt[k] - m_dimStats[k][MEAN]) / m_dimStats[k][STD_DEV];
}
else
{
q[i] = pt[k];
}
}
/*
ANNpoint q = annAllocPt(198);
for(size_t i=0; i<in.size(); i++)
q[GetK(in[i])]=0;
for(size_t i=0; i<in.size(); i++)
{
int k = in[i];
if( m_bStandardized )
{
ASSERT( abs( sqrt(m_dimStats[k][VARIANCE]) - m_dimStats[k][STD_DEV]) < 0.000001);
q[GetK(in[i])] += (pt[k]-m_dimStats[k][MEAN])/m_dimStats[k][STD_DEV];
}
else
{
q[GetK(in[i])] += pt[k];
}
}
*/
return q;
}
//retourne le point de p correspondant au point q des images B et Bp dans A et Ap
//au niveau l
Point* annUse::bestApproximateMatch(Image* B, Image* Bp,int l, Point* q,Image* A, FeatureType Type)
{
int marge;
if(gk==0 && pk==0)
marge=2;
else
marge=(gk>=pk*2)?gk*3:pk*4+2;
//construction de l'ANNpoint correspondant à q et son voisinnage
ANNpoint queryPt = annAllocPt(dim);
makeANNPoint(B,Bp,l,q->getX(),q->getY(), Type, queryPt);
//recherche du point correspondant dans A et Ap
kdTree->annkSearch(queryPt, 1, nnIdx, dists, eps);
//calcul et retour des coordonnées du point
uint32 x=nnIdx[0]%(A->getLargeur(l)-marge);
uint32 y=nnIdx[0]/(A->getLargeur(l)-marge);
//libération de l'ANNpoint
annDeallocPt(queryPt);
return new Point(x+gk,y+gk);
}
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"));
}
int CANNObject::GetNdxFromVector(std::vector<int>& point)
{
// allocate query point
ANNpoint queryPt = annAllocPt(point.size());
// read query point
ReadPointFromVector(point, queryPt);
// echo query point
if (DEBUG_ANN)
{
printf("Query point: ");
PrintPt(std::cout, queryPt);
}
// search
kdTree->annkSearch( queryPt, // query point
k, // number of near neighbors
nnIdx, // nearest neighbors (returned)
dists, // distance (returned)
eps); // error bound
// print summary
if (DEBUG_ANN)
{
std::cout << "\tNN:\tIndex\tDistance\n";
for (int i = 0; i < k; i++)
{
// unsquare distance
dists[i] = sqrt(dists[i]);
std::cout << "\t" << i << "\t" << nnIdx[i] << "\t" << dists[i] << std::endl;
}
}
annDeallocPt(queryPt);
return nnIdx[0];
}
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);
}
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
}
double arlCore::ICP::computeCriterion( const arlCore::vnl_rigid_matrix &M, vnl_vector< double > &fx )
{
vnl_vector<double> RMS(1, 0.0), nbPoints(1, 0.0);
const arlCore::vnl_rigid_matrix InvM = M.computeInverse();
unsigned int i, j, noTriangle;
double n = 0.0, result = 0.0;
if(m_point2PointMode)
{ // Points to points
#ifdef ANN
const unsigned int Dimension = 3;
vnl_vector_fixed<double,3> traInit = InvM.getTranslation();
vnl_matrix_fixed<double,3,3> rotInit = InvM.getRotation();
const double Epsilon = 0.0;// Error bound
ANNpoint Pt = annAllocPt(Dimension); // Query point
for( i=0 ; i<m_cloudSize ; ++i )
{ // Search the matching point for every point of cloud
for( j=0 ; j<3; ++j )
Pt[j] = rotInit[j][0]*m_cloudPoints[i][0]+rotInit[j][1]*m_cloudPoints[i][1]+rotInit[j][2]*m_cloudPoints[i][2]+traInit[j];
m_ANNtree->annkSearch( Pt, m_nbNN, m_nn_idx, m_squaredDists, Epsilon );
if(fx.size()>i) fx[i] = m_squaredDists[0];
RMS[0] += m_squaredDists[0];
}
annDeallocPt(Pt);
n = (double)m_cloudSize;
#endif // ANN
}else
{ // Point to mesh
assert(m_cloud!=0 && m_modelMesh!=0);
RMS.set_size((unsigned int)m_modelMesh->getTriangles().size());
RMS.fill(0.0);
nbPoints.set_size(RMS.size());
nbPoints.fill(0.0);
unsigned int i;
arlCore::Point::sptr point = arlCore::Point::New(3);
for( i=0 ; i<m_cloud->size() ; ++i )
if(m_cloud->get(i))
if(!m_justVisible || m_cloud->get(i)->isVisible())
{
InvM.trf(m_cloud->get(i), point);
const double SquaredDist = m_modelMesh->computeDistance2(point, noTriangle);
if(SquaredDist>0.0)
{
RMS[noTriangle] += SquaredDist;
if(fx.size()>i) fx[i] = SquaredDist;
nbPoints[noTriangle] += 1;
}
}
}
assert(nbPoints.size()==RMS.size());
if(RMS.size()==1)
{
if(nbPoints[0]>0) return sqrt(RMS[i]/nbPoints[i]);
else return -1.0;
}
for( i=0 ; i<RMS.size() ; ++i )
if(nbPoints[i]>0)
{
result += sqrt(RMS[i]/nbPoints[i]);
n += 1.0;
}
if(n>0) return result/n;
else return -1;
}
void ANNCluster::doCluster(mat_f& data, grpEle_vect& grpVect, float radius) {
//
int ptCnt = data.size().height;
int dim = data.size().width;
//this is ANN implementation, but it is changed to FLANN now, please refer the updated block below
float *ptWeight = new float[ptCnt];
std::vector<bool> mFlag(ptCnt,false);
#ifdef __USE_ANN__
ANNpointArray ptArray = annAllocPts(ptCnt, dim);
//assign point values
for (int i=0; i<ptCnt; i++) {
ANNpoint ptPtr = ptArray[i];
for(int j = 0; j < dim; j++){
float tmp = data(i,j);
ptPtr[j] = data(i , j);
}
}
///
ANNkd_tree kdt(ptArray, ptCnt, dim);
ANNpoint queryPt = annAllocPt(dim);
ANNidxArray nnIdx = new ANNidx[ptCnt];
ANNdistArray nnDist = new ANNdist[ptCnt];
int grpCounter = 0;
for (int i=0; i<ptCnt; i++) {
if(mFlag[i])
continue; //skip matched point
for(int j = 0; j < dim; j++){
queryPt[j] = data(i,j);
}
int resultCnt = kdt.annkFRSearch(queryPt, radius, ptCnt, nnIdx, nnDist);
std::vector<cv::Vec3f> nnPt;
GrpEle ge;
ge.m_gID = grpCounter++ ;
for (int j=0; j < resultCnt; j++) {
ANNidx idx = nnIdx[j];
mFlag[idx] = true;
ANNpoint tmpNNPt = ptArray[idx];
cv::Vec3f cvPt(tmpNNPt[0], tmpNNPt[1], tmpNNPt[2]);
ptWeight[j] = exp(-nnDist[j]/radius);
ge.m_eMember.push_back(Element(idx,ptWeight[j]));
}
grpVect.push_back(ge);
}
annDeallocPt(queryPt);
annDeallocPts(ptArray);
delete [] nnDist;
delete [] nnIdx;
delete [] ptWeight;
#else
cv::flann::KDTreeIndexParams idxParams(4);
cv::flann::SearchParams scParams(32);
cv::flann::Index indexer(data,idxParams);
std::vector<float> nnDists(dim);
std::vector<int> nnInds(dim);
std::vector<float> queryPt(dim);
for(int i = 0; i < ptCnt; i++){
if(mFlag[i])
continue;
for(int j = 0; j < dim; j++)
queryPt[j] = data(i,j);
int nnCnt = indexer.radiusSearch(queryPt,nnInds,nnDists,radius,scParams);
GrpEle ge;
for(int k = 0; k < nnCnt; k++){
int tmpIdx = nnInds[k];
float tmpDist = nnDists[k];
float tmpWeight = exp(-tmpDist/radius);
ge.m_eMember.push_back(Element(tmpIdx,tmpWeight));
}
grpVect.push_back(ge);
}
#endif
}
void Worker::operator()() {
for (private_index=get_index(); private_index!=-1; private_index=get_index()) {
{boost::mutex::scoped_lock lock(io_mutex);
std::cout << "THREAD " << id << " GOT " << private_index+1 << "/" << Globals::wspd_centers.size() << std::endl;}
//Find the side length, radius, radius squared and center of circle/ring for
//each one of the circle/rings that I'm going to draw
for (int j=0; j<=Globals::wspd_circs_num[private_index]; j++) {
//1 as argument because I insert parents of leaves
//This means that when I do find a square with this side, I'm going to call
//bear_children method again. This essentially reduces processing time by 3/4
sides.push_back(side_discr(pow(2,j)*Globals::wspd_distances[private_index]/denominator,3));//1
// sides.push_back(side_discr(pow(2,j)*Globals::wspd_distances[private_index]/denominator,0));
//make radius an exact multiple of side
radiuses.push_back(radius_discr(pow(2,j)*Globals::wspd_distances[private_index], sides[j]));
radiuses_sqr.push_back(radiuses[j]*radiuses[j]);
//Center of circle should fall neatly on the center of a grid square
ANNpoint center = annAllocPt(Globals::dim);
for (int k=0; k<Globals::dim; k++) {
center[k] = center_discr(Globals::wspd_centers[private_index][k], sides[j], 0.5);
}
centers.push_back(center);
}
//first iteration is for the core, the inner circle which is completely filled
//the rest of the iteration are rings, essentially 2 concentric cirles where I fill
//the difference
core=true;
for (int circ=0; circ<=Globals::wspd_circs_num[private_index]; circ++) {
//Generate all the possible values of a coordinate, based on side and radius
//for example if radius is 8 and side is 2 this is going to be 0,2,4,6,8
for (ANNcoord v=0.0; v<=radiuses[circ]; v+=sides[circ]) {
all_values.push_back(v);
}
//cart_prod_counter=0;
cart_product(all_values, result, circ);
if (Globals::qbs[id]->children!=0)
depth_all_reprs(Globals::qbs[id]);
// find_all_reprs(Globals::qbs[id], &Globals::hypercube_center, 1.0, kdtree, asdf, id);
//CLEANUP
all_values.clear();
core=false;
}
//CLEANUP
radiuses.clear();
radiuses_sqr.clear();
sides.clear();
for (unsigned int d=0; d<centers.size(); d++)
annDeallocPt(centers[d]);
centers.clear();
}
std::cout << "THREAD " << id << " DIES" << std::endl;
annDeallocPt(qb_center);
annDeallocPts(leaf_centers);
delete[] leaf_reprs;
pos_neg_result.clear();
pos_neg_final.clear();
vd1.clear();
vd2.clear();
delete asdf;
delete kdtree;
// delete qb;
return;
}
void PointKdTree::add_point(int _ind, Vector3 _point) {
point_array[_ind] = annAllocPt(3);
point_array[_ind][0] = _point.x;
point_array[_ind][1] = _point.y;
point_array[_ind][2] = _point.z;
}
Mat ICP::getClosestPointsSet( const Mat &objSet, Mat &lambda, Method method )
{
int rows = objSet.rows;
Mat closestSet(rows, 1, DataType<Point3d>::type);
vector<double> dists(rows);
double threshold = 0;
int cnt = 0;
switch (method)
{
case BASIC:
for (int i = 0; i < rows; i++)
{
int minIndex = 0;
double minDistance = DISTANCE_MAX;
Point3d oPoint = objSet.at<Point3d>(i, 0);
for (int j = 0; j < m_modSet.rows; j++)
{
Point3d p = m_modSet.at<Point3d>(j, 0) - oPoint;
double distance = sqrt(p.dot(p));
if (distance < minDistance)
{
minDistance = distance;
minIndex = j;
}
}
closestSet.at<Point3d>(i, 0) = m_modSet.at<Point3d>(minIndex, 0);
dists[i] = minDistance;
threshold += minDistance;
cnt++;
}
break;
case KDTREE:
default:
for (int i = 0; i < rows; i++)
{
Vec3d oPoint = objSet.at<Vec3d>(i, 0);
ANNpoint qp = annAllocPt(ICP_DIMS);
qp[0] = oPoint[0];
qp[1] = oPoint[1];
qp[2] = oPoint[2];
ANNidx idx[1];
ANNdist dist[1];
m_kdTree->annkSearch(qp, 1, idx, dist);
closestSet.at<Point3d>(i, 0) = Point3d(
m_modPts[idx[0]][0], m_modPts[idx[0]][1],
m_modPts[idx[0]][2]);
dists[i] = dist[0];
threshold += dist[0];
cnt++;
}
break;
case POINT_TO_PLANE:
{
// int cnt = 0;
// for (int i = 0; i < rows; i++)
// {
// Vec3d oPoint = Vec3d(objSet.at<Point3d>(i, 0));
// _Examplar exm(ICP_DIMS);
// for (int j = 0; j < ICP_DIMS; j++)
// {
// exm[j] = oPoint[j];
// }
// vector<pair<_Examplar, double>> results;
// if (m_kdTree.findNearest(exm, DISTANCE_RANGE, results) < 3)
// {
// closestSet.at<Point3d>(i, 0) = Point3d(0, 0, 0);
// dists[i] = -1;
// }
// else
// {
// ExamplarPairCompare epc;
// sort(results.begin(), results.end(), epc);
// Vec3d normal = computeNormal(results);
// _Examplar mExm = results[0].first;
// Vec3d v((double)mExm[0], (double)mExm[1], (double)mExm[2]);
// v = v - oPoint;
// double dotProd = v.dot(normal);
// double distance = fabs(dotProd);
// Vec3d mPoint = oPoint - dotProd * normal;
// closestSet.at<Point3d>(i, 0) = Point3d(mPoint);
// dists[i] = (double)distance;
// threshold += distance;
// cnt++;
// }
// }
}
break;
case CUDA:
/* cuda_getClosestPoints(objSet, m_modSet, dists, threshold, &closestSet);*/
break;
}
threshold /= (double)cnt;
for (int r = 0; r < rows; r++)
{
double dist = dists[r];
if (dist < threshold && dist > 0)
{
double l = 1.0f;
lambda.at<double>(r, 0) = l;
}
else
{
lambda.at<double>(r, 0) = 0;
}
}
return closestSet.clone();
}
Foam::labelListList Foam::parcelCloud::findParticlesIn()
{
if(useAllParticles_)
{
particlesIn_.resize(particlesInSubDomain_.size(),labelList(nPInParcel_,-1));
}else
{
particlesIn_.resize(particlesInSubDomain_.size()/nPInParcel_,labelList(nPInParcel_,-1));
}
// Number of particles in a parcel
int k = nPInParcel_;
// Dimensions, exact OR approximate
int dim =3; double eps = 0;
// Number of points
int nPts;
nPts = particlesInSubDomain_.size();
Pout << tab << "Number of particles in a parcel = " << nPInParcel_ << endl;
// Domain Min/Max
const fvMesh& mesh = refCast<const fvMesh>(obr_);
const pointField& pp = mesh.points();
// Min, max x-coordinates
scalar minX = Foam::min(pp & vector(1,0,0));
scalar maxX = Foam::max(pp & vector(1,0,0));
// Min, max y-coordinates
scalar minY = Foam::min(pp & vector(0,1,0));
scalar maxY = Foam::max(pp & vector(0,1,0));
// Min, max z-coordinates
scalar minZ = Foam::min(pp & vector(0,0,1));
scalar maxZ = Foam::max(pp & vector(0,0,1));
// Squared radius
const scalar sqRad = pow( (maxX - minX) * (maxX - minX)
+(maxY - minY) * (maxY - minY)
+(maxZ - minZ) * (maxZ - minZ), 0.5);
Pout << tab << "Squared radius = " << sqRad << endl;
// Data points
ANNpointArray dataPts;
// Query points
ANNpoint queryPt;
ANNidxArray nnIdx; // near neighbour indices
ANNdistArray dists; // near neighbour distances
ANNkd_tree* kdTree; // search structure
Pout << tab << "Created kdTree variables " << endl;
// Allocate
queryPt = annAllocPt(dim);
dataPts = annAllocPts(nPts, dim);
nnIdx = new ANNidx[k];
dists = new ANNdist[k];
Pout << tab << "Allocated kdTree variables " << endl;
labelList particleCreateParcelList(particlesInSubDomain_.size());
for(int ii =0; ii < particlesInSubDomain_.size(); ii++)
{
label particleGlobalID = particlesInSubDomain_[ii];
dataPts[ii][0] = sm_.position(particleGlobalID).x();
dataPts[ii][1] = sm_.position(particleGlobalID).y();
dataPts[ii][2] = sm_.position(particleGlobalID).z();
particleCreateParcelList[ii] = particleGlobalID;
}
Pout << tab << "Creating kdTree..." << endl;
kdTree = new ANNkd_tree(dataPts, nPts, dim);
Pout << tab << "Entering particle loops to create parcel" << endl;
label parcelI = 0;
forAll(particlesInSubDomain_,ii)
{
label particleGlobalID = particlesInSubDomain_[ii];
if ( particleCreateParcelList[ii] > -1 )
{
queryPt[0] = sm_.position(particleGlobalID).x();
queryPt[1] = sm_.position(particleGlobalID).y();
queryPt[2] = sm_.position(particleGlobalID).z();
kdTree->annkFRSearch(
queryPt, // query point
sqRad, // squared radius
k, // number of the near neighbours to return
nnIdx, // nearest neighbor array
dists, // dist to near neighbours
eps );
int partSum = 0;
int i = 0;
while( i < k && partSum < nPInParcel_ )
{
if ( particleCreateParcelList[nnIdx[i]] != -1 )
{
//Info << " parcelI " << parcelI << " partSum " << partSum << " Neighbour part "
// << nnIdx[i] << " particlesInSubDomain " << particlesInSubDomain_[nnIdx[i]]
// << " particlesIn " << particlesIn_[parcelI][partSum] << endl;
if (!useAllParticles_) particleCreateParcelList[nnIdx[i]] = -1 ;
particlesIn_[parcelI][partSum] = particlesInSubDomain_[nnIdx[i]];
partSum++;
if( partSum == nPInParcel_ ) parcelI++;
};
i++;
};
}
}
mitk::ContourElement::VertexType* mitk::ContourElement::OptimizedGetVertexAt(const mitk::Point3D &point, float eps)
{
if( (eps > 0) && (this->m_Vertices->size()>0) )
{
int k = 1;
int dim = 3;
int nPoints = this->m_Vertices->size();
ANNpointArray pointsArray;
ANNpoint queryPoint;
ANNidxArray indexArray;
ANNdistArray distanceArray;
ANNkd_tree* kdTree;
queryPoint = annAllocPt(dim);
pointsArray = annAllocPts(nPoints, dim);
indexArray = new ANNidx[k];
distanceArray = new ANNdist[k];
int i = 0;
//fill points array with our control points
for(VertexIterator it = this->m_Vertices->begin(); it != this->m_Vertices->end(); it++, i++)
{
mitk::Point3D cur = (*it)->Coordinates;
pointsArray[i][0]= cur[0];
pointsArray[i][1]= cur[1];
pointsArray[i][2]= cur[2];
}
//create the kd tree
kdTree = new ANNkd_tree(pointsArray,nPoints, dim);
//fill mitk::Point3D into ANN query point
queryPoint[0] = point[0];
queryPoint[1] = point[1];
queryPoint[2] = point[2];
//k nearest neighbour search
kdTree->annkSearch(queryPoint, k, indexArray, distanceArray, eps);
VertexType* ret = NULL;
try
{
ret = this->m_Vertices->at(indexArray[0]);
}
catch(std::out_of_range ex)
{
//ret stays NULL
return ret;
}
//clean up ANN
delete [] indexArray;
delete [] distanceArray;
delete kdTree;
annClose();
return ret;
}
return NULL;
}
// 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;
}
//Should be a method inside Worker
//this one both fills out the leaves and also merges all reprs in one map
void replace_reprs(QNode* qb, ANNpoint* center, double side, ANNidxArray nnIdx, ANNdistArray dists) {
if (qb->children == 0) {
myset rs(qb->representatives[0]);
if (Globals::fill) {
if (rs.size() < Globals::reprs_num) {
Globals::kdtree->annkSearch(center[0], Globals::reprs_num, nnIdx, dists, 0.0);
for (unsigned int i=0; i<Globals::reprs_num; i++) {
if ((!repr_exists(&rs, nnIdx[i])) && (rs.size()<Globals::reprs_num)) {
rs.insert(nnIdx[i]);
}
}
}
}
//if (qb->representatives==0) {
// std::cout << "err" << std::endl;
//}
//This really doesn't work as expected mmm
/* qb->bear_children();
for (int i=0; i<Globals::pow2dim; i++) {
qb->children[0][i].representatives = qb->representatives;
}
qb->remove_representatives();
int it;
int new_repr;
ANNpoint child_center = annAllocPt(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_center[j] = (*center)[j]+(side/4.0);
} else {
child_center[j] = (*center)[j]-(side/4.0);
}
it = it << 1;
}
//new_repr=kdtree->ann1Search(child_center,asdf);
//new_repr = Globals::kdtree->annkSearch(child_center, 1, nnIdx, dists, 0.0);
Globals::kdtree->annkSearch(child_center, 1, nnIdx, dists, 0.0);
new_repr=nnIdx[0];
myset rs(qb->children[0][i].representatives[0]);
if (rs.size() == Globals::reprs_num-1) {
//remove the furthest away
double mdist=0.0;
double tmp_dist;
myset::iterator it_rem;
for (myset::iterator it = rs.begin(); it != rs.end(); it++) {
tmp_dist = annDist(Globals::dim,child_center,Globals::points[*it]);
if (tmp_dist > mdist) {
mdist=tmp_dist;
it_rem=it;
}
}
rs.erase(it_rem);
}
rs.insert(new_repr);
if (Globals::final_set.count(rs) == 0)
Globals::final_set[rs] = new myset(rs);
qb->children[0][i].representatives = Globals::final_set[rs];
}*/
if (Globals::final_set.count(rs) == 0)
Globals::final_set[rs]=new myset(rs);
qb->representatives = Globals::final_set[rs];
/*if (Globals::final_set.count(qb->representatives[0]) == 0)
Globals::final_set[qb->representatives[0]] = new myset(qb->representatives[0]);
qb->representatives = Globals::final_set[qb->representatives[0]];*/
// annDeallocPt(child_center);
} else {
/* This is an inner node. First we recurse to each leaf.
* Notice that we need to compute the coordinates of the centers
* of each child, since that information is not stored in QNode
*/
int it;
//A local container to consume the result of our recursive calls
//std::vector<Globals::QLeaf*> new_children;
//Just a fancy array of arrays. We have pow2dim children and each one
//needs dim coordinates to express its center
ANNpoint child_center = annAllocPt(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_center[j] = (*center)[j]+(side/4.0);
} else {
child_center[j] = (*center)[j]-(side/4.0);
}
it = it << 1;
}
replace_reprs(qb->children[0]+i, &child_center, side/2.0, nnIdx, dists);
}
annDeallocPt(child_center);
}
}
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 createParcels
(
const fvMesh& mesh,
cfdemCloud& sm,
const int& parcelSize_,
int**& parcelCloud_,
double**& parcelPositions_,
double**& parcelVelocities_,
int*& parcelNparts_,
double** & parcelKinStress_,
scalar& aveSubQparcel2_,
vector& meanParcelVel_,
const bool verbose_
)
{
if ( parcelSize_ * parcelSize_ * parcelSize_ > sm.numberOfParticles() )
{
FatalError << " Number of particles in a parcel > number of particles" << abort(FatalError);
}
if ( parcelSize_ < 1 )
{
FatalError << " Number of particles < 0 in a parcel " << abort(FatalError);
}
// Number of particles in a parcel
int k = parcelSize_ * parcelSize_ * parcelSize_;
// Dimensions, exact OR approximate
int dim =3; double eps = 0;
// Number of points
int nPts;
nPts = sm.numberOfParticles();
// Data points
ANNpointArray dataPts;
// Query points
ANNpoint queryPt;
ANNidxArray nnIdx; // near neighbour indices
ANNdistArray dists; // near neighbour distances
ANNkd_tree* kdTree; // search structure
// Allocate
queryPt = annAllocPt(dim);
dataPts = annAllocPts(nPts, dim);
nnIdx = new ANNidx[k];
dists = new ANNdist[k];
for(int index = 0; index < sm.numberOfParticles(); index++)
{
dataPts[index][0] = sm.position(index).x();
dataPts[index][1] = sm.position(index).y();
dataPts[index][2] = sm.position(index).z();
}
kdTree = new ANNkd_tree(dataPts, nPts, dim);
// Initialize sub-parcel agitation
aveSubQparcel2_ = 0.;
// Initialize parcel velocity
meanParcelVel_ = vector(0,0,0);
for(int index = 0; index < sm.numberOfParticles(); index++)
{
// Particle neighbouring search distance
scalar sqRad = parcelSize_ * sm.radius(index);
queryPt[0] = sm.position(index).x();
queryPt[1] = sm.position(index).y();
queryPt[2] = sm.position(index).z();
kdTree->annkFRSearch(
queryPt, // query point
sqRad, // squared radius
k, // number of the near neighbours to return
nnIdx, // nearest neighbor array
dists, // dist to near neighbours
eps );
int nParts = 0;
scalar dist = 0;
// Initialize parcel velocities & positions & kinetic stresses
for(int j=0;j<3;j++)
{
parcelVelocities_[index][j] = 0.;
parcelPositions_[index][j] = 0.;
parcelKinStress_[index][j] = 0.;
parcelKinStress_[index][2*j] = 0.;
}
for (int i = 0; i < k; i++)
{
parcelCloud_[index][i] = nnIdx[i];
dist = mag( sm.position(nnIdx[i]) - sm.position(index) ) - sqRad;
if ( dist < SMALL )
{
for(int j=0;j<3;j++)
{
// Parcel velocity
parcelVelocities_[index][j] += sm.velocity(nnIdx[i])[j];
// Parcel center of mass
parcelPositions_[index][j] += sm.position(nnIdx[i])[j];
}
nParts++;
}
}
for(int j=0;j<3;j++) parcelPositions_[index][j] /= nParts;
parcelNparts_[index] = nParts;
// Parcel kinetic stresses
for(int i = 0; i < parcelNparts_[index]; i++)
{
int particleID = parcelCloud_[index][i];
// U'xU'x
parcelKinStress_[index][0] += ( sm.velocity(particleID)[0] - parcelVelocities_[index][0] )
*( sm.velocity(particleID)[0] - parcelVelocities_[index][0] );
// U'yU'y
parcelKinStress_[index][1] += ( sm.velocity(particleID)[1] - parcelVelocities_[index][1] )
*( sm.velocity(particleID)[1] - parcelVelocities_[index][1] );
// U'zU'z
parcelKinStress_[index][2] += ( sm.velocity(particleID)[2] - parcelVelocities_[index][2] )
*( sm.velocity(particleID)[2] - parcelVelocities_[index][2] );
// U'xU'y
parcelKinStress_[index][3] += ( sm.velocity(particleID)[0] - parcelVelocities_[index][0] )
*( sm.velocity(particleID)[1] - parcelVelocities_[index][1] );
// U'xU'z
parcelKinStress_[index][4] += ( sm.velocity(particleID)[0] - parcelVelocities_[index][0] )
*( sm.velocity(particleID)[2] - parcelVelocities_[index][2] );
// U'yU'z
parcelKinStress_[index][5] += ( sm.velocity(particleID)[1] - parcelVelocities_[index][1] )
*( sm.velocity(particleID)[2] - parcelVelocities_[index][2] );
}
// Mean parcel velocity
for(int j=0;j<3;j++) meanParcelVel_[j] += parcelVelocities_[index][j];
// Domain-averaged parcel agitation
aveSubQparcel2_ += 1./2. * ( parcelKinStress_[index][0] + parcelKinStress_[index][1] + parcelKinStress_[index][2] );
}
for(int j=0;j<3;j++) meanParcelVel_[j] /= sm.numberOfParticles();
if ( verbose_ )
{
int index = 0;
Info << " Parcel particle list ";
for (int i = 0; i < parcelNparts_[index]; i++)
{
Info << parcelCloud_[index][i] << " " ;
}
Info << endl;
Info << " Parcel center " << parcelPositions_[index][0] << "," << parcelPositions_[index][1] << "," << parcelPositions_[index][2] << endl;
Info << " Parcel velocity " << parcelVelocities_[index][0] << "," << parcelVelocities_[index][1] << "," << parcelVelocities_[index][2] << endl;
for (int i = 0; i < parcelNparts_[index]; i++)
{
Info << " Particle " << parcelCloud_[index][i] << endl;
Info << " Particle center " << sm.position(parcelCloud_[index][i]) << endl;
Info << " Particle velocity " << sm.velocity(parcelCloud_[index][i]) << endl;
}
}
}
