//*****************************************************
// Function: distANN_XY
//*****************************************************
void distANN_XY( const ANNpointArray dataX, const ANNpointArray dataY,
double* distsXY,
unsigned int dimX, unsigned int dimY,
unsigned int xN, unsigned int yN,
unsigned int k, double eps )
{
ANNkd_tree* kdTree;
ANNdistArray nnDist;
ANNidxArray nnIdx;
// Allocate memory
nnIdx = new ANNidx[ k+1 ];
nnDist = new ANNdist[ k+1 ];
kdTree = new ANNkd_tree( dataY, yN, dimY );
// Get the distances to all the points
for( unsigned int i = 0; i < xN ; i++ )
{
kdTree->annkSearch( dataX[ i ], k+1, nnIdx, nnDist, eps );
double my_dist = nnDist[ k ];
// check to see if the dist is zero (query point same as the kNN)
// if so find the next k that gives the next positive distance
if( my_dist == 0.0 )
{
ANNdistArray nnDist_tmp = new ANNdist[ yN ];
ANNidxArray nnIdx_tmp = new ANNidx[ yN ];
kdTree->annkSearch( dataX[ i ], yN, nnIdx_tmp, nnDist_tmp, eps );
for( unsigned int my_k = k + 1; my_k < yN; ++my_k )
if( nnDist_tmp[ my_k ] > 0.0 )
{
my_dist = nnDist_tmp[ my_k ];
break;
}
delete [] nnIdx_tmp;
delete [] nnDist_tmp;
}
distsXY[ i ] = my_dist;
}
// Deallocate memory
delete [] nnIdx;
delete [] nnDist;
delete kdTree;
annClose();
return;
}
object search(ANNkd_tree& kdtree, object q, int k, double eps, bool priority = false)
{
BOOST_ASSERT(k <= kdtree.nPoints() && kdtree.theDim() == len(q));
ANNpointManaged annq(kdtree.theDim());
for (int c = 0; c < kdtree.theDim(); ++c)
annq.pt[c] = extract<ANNcoord>(q[c]);
npy_intp dims[] = { k};
PyObject *pydists = PyArray_SimpleNew(1,dims, sizeof(ANNdist)==8 ? PyArray_DOUBLE : PyArray_FLOAT);
BOOST_ASSERT(!!pydists);
PyObject *pyidx = PyArray_SimpleNew(1,dims, PyArray_INT);
if( !pyidx )
Py_DECREF(pydists);
BOOST_ASSERT(!!pyidx);
ANNdist* pdists = (ANNdist*)PyArray_DATA(pydists);
ANNidx* pidx = (ANNidx*)PyArray_DATA(pyidx);
std::vector<ANNidx> nn_idx(k);
std::vector<ANNdist> dists(k);
if (priority)
kdtree.annkPriSearch(annq.pt, k, pidx, pdists, eps);
else
kdtree.annkSearch(annq.pt, k, pidx, pdists, eps);
return boost::python::make_tuple(static_cast<numeric::array>(handle<>(pyidx)), static_cast<numeric::array>(handle<>(pydists)));
}
bool is_valid(tree_struct stree, vector<int> vals, int temp_dim) {
ANNkd_tree *kdTree = stree.kdTree;
int dim = stree.dim;
ANNpointArray dataPts = stree.dataPts;
// over-write the value of k to be 1
int k_temp = 1;
bool return_val=0;
ANNidxArray nnIdx; // near neighbor indices
ANNdistArray dists; // near neighbor distances
nnIdx = new ANNidx[k_temp]; // allocate near neigh indices
dists = new ANNdist[k_temp];
cout << " is valid is testing this point:::: " << vector_to_string(vals) << endl;
ANNpoint queryPt = make_point_from_vector(vals, temp_dim);
kdTree->annkSearch( // search
queryPt, // query point
k_temp, // number of near neighbors
nnIdx, // nearest neighbors (returned)
dists, // distance (returned)
eps); // error bound
cout << "The distance is::::: " << dists[0] << endl;
// just check if the distance is 0.
if(dists[0] == 0)
return_val=1;
return return_val;
}
vector<double> HDBScan::ComputeCoreDistance(double** input_data, int n_pts,
int n_dim, int min_samples,
char dist)
{
vector<double> core_d;
core_d.resize(n_pts);
double eps = 0; // error bound
if (dist == 'e') ANN_DIST_TYPE = 2; // euclidean
else if (dist == 'b') ANN_DIST_TYPE = 1; // manhattan
// since KNN search will always return the query point itself, so add 1
// to make sure returning min_samples number of results
//min_samples = min_samples + 1;
ANNkd_tree* kdTree = new ANNkd_tree(input_data, n_pts, n_dim);
ANNidxArray nnIdx = new ANNidx[min_samples];
ANNdistArray dists = new ANNdist[min_samples];
for (size_t i=0; i<n_pts; ++i) {
kdTree->annkSearch(input_data[i], min_samples, nnIdx, dists, eps);
core_d[i] = sqrt(dists[min_samples-1]);
}
delete[] nnIdx;
delete[] dists;
delete kdTree;
return core_d;
}
// given a pointer to a kdTree and a query point, this returns k
// nearest neighbors
string get_neighbor(tree_struct stree, ANNpoint queryPt, int k_) {
ANNkd_tree *kdTree = stree.kdTree;
int dim = stree.dim;
ANNpointArray dataPts = stree.dataPts;
int k=2;
int nPts; // actual number of data points
ANNidxArray nnIdx; // near neighbor indices
ANNdistArray dists; // near neighbor distances
nnIdx = new ANNidx[k]; // allocate near neigh indices
dists = new ANNdist[k];
string return_string ="";
kdTree->annkSearch( // search
queryPt, // query point
k, // number of near neighbors
nnIdx, // nearest neighbors (returned)
dists, // distance (returned)
eps); // error bound
int i = 1;
return_string += " ";
for(int j=0; j < dim; j++) {
stringstream ss;
double d = (dataPts[nnIdx[i]])[j];
ss << d;
string buf =ss.str();
return_string += buf;
if(j!=dim-1)
return_string += " ";
}
return_string += "\0";
cout << "from get neighbor ::: " << return_string << "\n";
return return_string;
}
//The actual classification. Where all the magic happens
void Classify()
{
ANNidxArray nnIdx;
ANNdistArray dists;
ANNkd_tree *kdTree;
dataPts = annAllocPts(maxPts, DIM);
nnIdx = new ANNidx[k];
dists = new ANNdist[k];
kdTree = new ANNkd_tree(
dataPts,
nPts,
DIM);
kdTree->annkSearch(
queryPt,
k,
nnIdx,
dists,
eps);
cout << "NN: Index Distance\n";
//Unsquare the distances and print
for (uint i = 0; i < k; i++)
{
dists[i] = sqrt(dists[i]);
cout << i << " " << nnIdx[i] << " " << dists[i] << " protocol: " << dataPointsWithClass[nnIdx[i]]->protocol << "\n";
}
protocolCountTable protocolCount;
for (uint i = 0; i < k; i++)
{
//TODO: Make a more sophisticated final guess than mere plurality vote
protocolCount[dataPointsWithClass[nnIdx[i]]->protocol]++;
}
int protocolWinner = 0;
int highestVotes = 0;
//Go through and see which protocol had the most...
for (protocolCountTable::iterator it = protocolCount.begin();
it != protocolCount.end(); it++ )
{
if ( it->second > highestVotes)
{
highestVotes = it->second;
protocolWinner = it->first;
}
}
classification = protocolWinner;
cout << "Classified as: " << classification << "\n";
delete [] nnIdx;
delete [] dists;
delete kdTree;
annClose();
}
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;
}
void operator()( tbb::blocked_range<int> rng ) const{
ANNidx id;
ANNdist d;
ANNpoint pt = annAllocPt( feature_size );
for( int i=rng.begin(); i<rng.end(); i++ ){
for( int j=0; j<feature_size; j++ )
pt[j] = features[i*feature_size+j];
kd_tree->annkSearch(pt, 1, &id, &d );
res[i] = id;
}
annDeallocPt( pt );
}
int main(int argc, char **argv)
{
int nPts; // actual number of data points
ANNpointArray dataPts; // data points
ANNpoint queryPt; // query point
ANNidxArray nnIdx; // near neighbor indices
ANNdistArray dists; // near neighbor distances
ANNkd_tree* kdTree; // search structure
getArgs(argc, argv); // read command-line arguments
queryPt = annAllocPt(dim); // allocate query point
dataPts = annAllocPts(maxPts, dim); // allocate data points
nnIdx = new ANNidx[k]; // allocate near neigh indices
dists = new ANNdist[k]; // allocate near neighbor dists
nPts = 0; // read data points
cout << "Data Points:\n";
while (nPts < maxPts && readPt(*dataIn, dataPts[nPts])) {
printPt(cout, dataPts[nPts]);
nPts++;
}
kdTree = new ANNkd_tree( // build search structure
dataPts, // the data points
nPts, // number of points
dim); // dimension of space
while (readPt(*queryIn, queryPt)) { // read query points
cout << "Query point: "; // echo query point
printPt(cout, queryPt);
kdTree->annkSearch( // search
queryPt, // query point
k, // number of near neighbors
nnIdx, // nearest neighbors (returned)
dists, // distance (returned)
eps); // error bound
cout << "\tNN:\tIndex\tDistance\n";
for (int i = 0; i < k; i++) { // print summary
dists[i] = sqrt(dists[i]); // unsquare distance
cout << "\t" << i << "\t" << nnIdx[i] << "\t" << dists[i] << "\n";
}
}
delete [] nnIdx; // clean things up
delete [] dists;
delete kdTree;
annClose(); // done with ANN
return EXIT_SUCCESS;
}
LidarSegment* ClosestSegmentSearch::findClosestSegment(const DVector &searchPoint)
{
// use kd-Tree to search closest point
queryPt[0] = searchPoint(0);
queryPt[1] = searchPoint(1);
queryPt[2] = searchPoint(2);
kdTree->annkSearch(queryPt, 1 // number of neighbors searched
,nnIdx // nearest neighbors (returned)
,dists); // distance (returned)
if (nnIdx[0] < 0 ) {
// this should not happen
cerr << endl << "ClosesSegmentSearch returned NULL-pointer" << flush;
return NULL;
} else {
return idxMap[nnIdx[0]];
}
}
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 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;
}
}
}
object search_array(ANNkd_tree& kdtree, object qarray, int k, double eps, bool priority = false)
{
BOOST_ASSERT(k <= kdtree.nPoints());
int N = len(qarray);
if( N == 0 )
return boost::python::make_tuple(numeric::array(boost::python::list()).astype("i4"),numeric::array(boost::python::list()));
BOOST_ASSERT(len(qarray[0])==kdtree.theDim());
ANNpointManaged annq(kdtree.theDim());
npy_intp dims[] = { N,k};
PyObject *pydists = PyArray_SimpleNew(2,dims, sizeof(ANNdist)==8 ? PyArray_DOUBLE : PyArray_FLOAT);
BOOST_ASSERT(!!pydists);
PyObject *pyidx = PyArray_SimpleNew(2,dims, PyArray_INT);
if( !pyidx ) {
Py_DECREF(pydists);
}
BOOST_ASSERT(!!pyidx);
ANNdist* pdists = (ANNdist*)PyArray_DATA(pydists);
ANNidx* pidx = (ANNidx*)PyArray_DATA(pyidx);
std::vector<ANNdist> dists(k);
std::vector<ANNidx> nn_idx(k);
for(int i = 0; i < N; ++i) {
object q = qarray[i];
for (int c = 0; c < kdtree.theDim(); ++c)
annq.pt[c] = extract<ANNcoord>(q[c]);
if (priority)
kdtree.annkPriSearch(annq.pt, k, &nn_idx[0], &dists[0], eps);
else
kdtree.annkSearch(annq.pt, k, &nn_idx[0], &dists[0], eps);
std::copy(nn_idx.begin(),nn_idx.end(),pidx);
pidx += k;
std::copy(dists.begin(),dists.end(),pdists);
pdists += k;
}
return boost::python::make_tuple(static_cast<numeric::array>(handle<>(pyidx)), static_cast<numeric::array>(handle<>(pydists)));
}
void LoOP_outlier(vector<point3d>& xyz, vector<point3d>& out3d, double lambda, double alpha, int k_search) {
int kn = k_search;
ANNpointArray dataPts;
ANNpoint queryPt;
ANNidxArray nnIdx;
ANNdistArray dists;
ANNkd_tree* kdTree;
queryPt = annAllocPt(3);
point3d threeD;
dataPts = annAllocPts(xyz.size(),3);
nnIdx = new ANNidx [kn];
dists = new ANNdist [kn];
double *expect = new double [kn];
double *expect2 = new double [kn];
int p = 0;
vector<point3d>::iterator it;
for(it=xyz.begin(); it < xyz.end();it++) {
threeD = *it;
dataPts[p][0] = threeD.x;
dataPts[p][1] = threeD.y;
dataPts[p++][2] = threeD.z;
}
kdTree = new ANNkd_tree (dataPts, xyz.size(),3);
double lof;
double LoOP;
// std::vector<point3d> out3d;
for(int i=0;i<xyz.size();i++) {
double x,y,z,kdistA = 0.;
x = queryPt[0] = xyz[i].x;
y = queryPt[1] = xyz[i].y;
z = queryPt[2] = xyz[i].z; //S;
kdTree ->annkSearch(queryPt,kn,nnIdx,dists,1e-3);
for(int k=0;k<kn;k++)
kdistA += dists[k];
double S = sqrt(x*x+y*y+z*z);
double sigm = sqrt(kdistA/S);
double pdist = lambda * sigm;
double pdistE = 0.;
for(int k=0;k<kn;k++) {
x = queryPt[0] = xyz[nnIdx[k]].x;
y = queryPt[1] = xyz[nnIdx[k]].y;
z = queryPt[2] = xyz[nnIdx[k]].y;
kdTree->annkSearch(queryPt,kn,nnIdx,dists,1e-3);
kdistA = 0.;
for(int n=0;n<kn;n++)
kdistA += dists[n];
double Ss = sqrt(x*x+y*y+z*z);
kdistA = sqrt(kdistA/Ss);
expect[k] = lambda*kdistA;
expect2[k] = expect[k]*expect[k];
}
double exAv = 0.;
double exAv2 =0.;
for(int k=0;k<kn;k++) {
exAv += expect[k];
exAv2 += expect2[k];
}
for(int k=0;k<kn;k++) {
expect[k] = fabs(expect[k]-exAv/kn);
expect2[k] = fabs(expect2[k]-exAv2/kn);
}
exAv =0.;
exAv2 = 0.;
for(int k=0;k<kn;k++) {
exAv += expect[k];
exAv2 += expect2[k];
}
double Ex2 = exAv2/kn;
pdistE = exAv/kn;
double plof=pdist/pdistE-1;
double nPlof = lambda* sqrt(Ex2*plof*plof);
double LoOP = MAX(0,erf(plof/1.414*nPlof));
if(LoOP < alpha)
out3d.push_back(make_point(xyz[i].x,xyz[i].y,xyz[i].z));
}
delete [] expect;
delete [] expect2;
}
// get_neighbors_at_dist
string get_neighbor_at_dist(tree_struct stree, int dist,
ANNpoint queryPt,
int k_, int group)
{
ANNkd_tree *kdTree = stree.kdTree;
int dim = stree.dim;
ANNpointArray dataPts = stree.dataPts;
int k=k_;
int done = 1;
int nPts; // actual number of data points
ANNidxArray nnIdx; // near neighbor indices
ANNdistArray dists; // near neighbor distances
nnIdx = new ANNidx[k]; // allocate near neigh indices
dists = new ANNdist[k];
string return_string ="";
print_point(cout, queryPt, dim);
printf("looking for %d neighbors within the radius of %d \n", k, dist);
// first make sure it is a valid point
kdTree->annkSearch( // search
queryPt, // query point
k, // number of near neighbors
nnIdx, // nearest neighbors (returned)
dists, // distance (returned)
eps); // error bound
char buf [8];
sprintf(buf, "%d", group);
string group_str(buf);
// return_string = "global init__" + group_str +"\n set init__" +
// group_str + " [list ";
return_string += " [list ";
for (int i=0; i < k; i++) {
if (dists[i] == dist) {
return_string += " [list ";
for(int j=0; j < dim; j++) {
stringstream ss;
double d = (dataPts[nnIdx[i]])[j];
ss << d;
string buf =ss.str();
return_string += buf;
if(j!=dim-1)
return_string += " ";
}
return_string += "]";
if(i!=k-1)
return_string += " ";
}
}
return_string += "]\0";
/*
FILE * pFile;
pFile = fopen (filename.c_str(),"a");
if (pFile!=NULL)
{
fputs (return_string.c_str(),pFile);
fclose (pFile);
}
else {
done = 0;
}
return done;
*/
return return_string;
}
void ann_test()
{
pts pts;
pts.readFile("sphere.ptsn");
ANNkd_tree* kdTree = new ANNkd_tree( // build search structure
pts.dataPts, // the data points
pts.nbPoints, // number of points
pts.dim); // dimension of space
ANNpoint queryPt = new ANNcoord[pts.dim]; // query point
queryPt[0] = 0;
queryPt[1] = 0;
queryPt[2] = 0.5;
int k = 3; // number of nearest neighbors
double eps = 0; // error bound
ANNidxArray nnIdx = new ANNidx[k]; // allocate near neigh indices
ANNdistArray dists = new ANNdist[k]; // allocate near neighbor dists
bool* marked = new bool[pts.nbPoints];
for (int i = 0; i<pts.nbPoints; ++i) marked[i] = false;
kdTree->annkSearch( // search
queryPt, // query point
k, // number of near neighbors
nnIdx, // nearest neighbors (returned)
dists, // distance (returned)
eps, // error bound
marked);
std::cout << "\tNN:\tIndex\tDistance\n";
for (int i = 0; i < k; i++)
{ // print summary
dists[i] = sqrt(dists[i]); // unsquare distance
std::cout << "\t" << i << "\t" << nnIdx[i] << "\t" << dists[i] << "\n";
marked[nnIdx[i]] = true;
}
kdTree->annkSearch( // search
queryPt, // query point
k, // number of near neighbors
nnIdx, // nearest neighbors (returned)
dists, // distance (returned)
eps, // error bound
marked);
std::cout << "\tNN:\tIndex\tDistance\n";
for (int i = 0; i < k; i++)
{ // print summary
dists[i] = sqrt(dists[i]); // unsquare distance
std::cout << "\t" << i << "\t" << nnIdx[i] << "\t" << dists[i] << "\n";
}
delete [] marked;
delete [] nnIdx; // clean things up
delete [] dists;
delete kdTree;
annClose(); // done with ANN
}
void GlobalFun::computeAnnNeigbhors(vector<CVertex> &datapts, vector<CVertex> &querypts, int knn, bool need_self_included = false, string purpose = "?_?")
{
cout << endl <<"Compute ANN for: " << purpose << endl;
int numKnn = knn + 1;
if (querypts.size() <= numKnn+2)
{
vector<CVertex>::iterator vi;
for(vi = datapts.begin(); vi != datapts.end(); ++vi)
{
for(int j = 0; j < 3; j++)
{
vi->neighbors.clear();
}
}
return;
}
int nPts; // actual number of data points
ANNpointArray dataPts; // data points
ANNpoint queryPt; // query point
ANNidxArray nnIdx; // near neighbor indices
ANNdistArray dists; // near neighbor distances
ANNkd_tree* kdTree; // search structure
int k = numKnn; // number of nearest neighbors
int dim = 3; // dimension
double eps = 0; // error bound
int maxPts = numKnn + 30000; // maximum number of data points
if (datapts.size() >= maxPts)
{
cout << "Too many data" << endl;
return;
}
queryPt = annAllocPt(dim); // allocate query point
dataPts = annAllocPts(maxPts, dim); // allocate data points
nnIdx = new ANNidx[k]; // allocate near neigh indices
dists = new ANNdist[k]; // allocate near neighbor dists
nPts = datapts.size(); // read data points
vector<CVertex>::iterator vi;
int index = 0;
for(vi = datapts.begin(); vi != datapts.end(); ++vi)
{
for(int j = 0; j < 3; j++)
{
dataPts[index][j] = double(vi->P()[j]);
}
index++;
}
knn++;
kdTree = new ANNkd_tree( // build search structure
dataPts, // the data points
nPts, // number of points
dim); // dimension of space
knn--;
for (vi = querypts.begin(); vi != querypts.end(); ++vi)
{
vi->neighbors.clear();
for (int j = 0; j < 3; j++)
{
queryPt[j] = vi->P()[j];
}
kdTree->annkSearch( // search
queryPt, // query point
k, // number of near neighbors
nnIdx, // nearest neighbors (returned)
dists, // distance (returned)
eps); // error bound
for (int k = 1; k < numKnn; k++)
{
vi->neighbors.push_back(nnIdx[k]);
}
}
delete [] nnIdx; // clean things up
delete [] dists;
delete kdTree;
annClose(); // done with ANN
}
/******************************
* I am a file that sets up models/features for creating Viewpoint Invariant Patches
* If you don't compile me with the ann library I will be sad :( (See ann user guide)
* Also needs to be compiled with OpenCV now
Compile command: (Probably don't need all the OpenCV libraries listed here)
g++ warpSIFT.cpp -I/path/to/ann_1.1.2/include -L/path/to/ann_1.1.2/lib -lANN -g -I/usr/local/include/opencv -I/usr/local/include -L/usr/local/lib -lopencv_shape -lopencv_stitching -lopencv_objdetect -lopencv_superres -lopencv_videostab -lopencv_calib3d -lopencv_features2d -lopencv_highgui -lopencv_videoio -lopencv_imgcodecs -lopencv_video -lopencv_photo -lopencv_ml -lopencv_imgproc -lopencv_flann -lopencv_core
Lio: g++ -std=c++11 warpSIFT.cpp -I/home/lionelt/ann_1.1.2/include -L/home/lionelt/ann_1.1.2/lib -lANN -g -I/usr/local/include/opencv -I/usr/local/include -L/usr/local/lib -lopencv_shape -lopencv_stitching -lopencv_objdetect -lopencv_superres -lopencv_videostab -lopencv_calib3d -lopencv_features2d -lopencv_highgui -lopencv_videoio -lopencv_imgcodecs -lopencv_video -lopencv_photo -lopencv_ml -lopencv_imgproc -lopencv_flann -lopencv_core
g++ -std=c++11 warpSIFT.cpp -I/home/lionelt/ann_1.1.2/include -L/home/lionelt/ann_1.1.2/lib -lANN -g -I/usr/local/include/opencv -I/usr/local/include -L/usr/local/lib-lopencv_core
To run, something like that:
./a.out -df ../ETH_example_3D_model/dense3.nvm.cmvs/00/models/option-0000.ply -sf ../ETH_example_3D_model/dense3.nvm -sift ../ETH_example_3D_model/eth_photos_night
* Must be given 3 flags:
* -df: path to dense file
* -sf: path to sparse file
* -sift: path to folder containing images and SIFT files
* that generated the model. (Plz no '/' at end of path)
*******************************/
int main(int argc, char **argv)
{
ifstream denseStream;
ifstream sparseStream;
string siftFolder;
int maxPoints = 1024;
char line[1024];
// Read in command line arguments and open the file streams
int i = 1;
while (i < argc) {
if (!strcmp(argv[i], "-df")) {
denseStream.open(argv[++i], ios::in);
if (!denseStream) {
cerr << "Cannot open dense file\n";
exit(1);
}
}
else if (!strcmp(argv[i], "-sf")) {
sparseStream.open(argv[++i], ios::in);
if (!sparseStream) {
cerr << "Cannot open sparse file\n";
exit(1);
}
}
else if (!strcmp(argv[i],"-sift")){
siftFolder = argv[++i];
}
else if (!strcmp(argv[i],"-n")){
maxPoints = stoi(argv[++i]);
}
else {
cerr << "Unrecognized option.\n";
exit(1);
}
i++;
}
// Store cameras and sift features
// Creates a vector of Camera objects. Each Camera object
// has image name, focal length, quaternion, center, distortion
// and a vector of all SIFT features of that camera/image
sparseStream.getline(line, 1028);
sparseStream.getline(line, 1028);
sparseStream.getline(line, 1028);
int numCameras = stoi(line);
vector<Camera> cameras(numCameras);
// Read a camera line and store all camera parameters
for (int i = 0; i < numCameras; i++){
sparseStream.getline(line, 1028);
istringstream ss(line);
Camera c;
ss >> c.name;
ss >> c.focalLength;
for (int j = 0; j < 4; j++){
ss >> c.quaternion[j];
}
for (int j = 0; j < 3; j++){
ss >> c.center[j];
}
ss >> c.radialDistortion;
// Iterate through the correpsonding SIFT file and parse all SIFT features
// readSIFT returns a vector of SIFT features for the corresponding image/camera
c.SiftVector = readSIFT(siftFolder + "/" + c.name.substr(0,c.name.find(".")) + ".sift",i);
if (c.SiftVector.size() == 0){
cerr << "Error importing SIFT features for " << c.name << "\n";
}
cameras[i] = c;
}
// Read in dense model for use in nearest neighbour calculations later
ANNpointArray densePts = annAllocPts(maxPoints, 3);
double eps = 0.01;
int k = 5; //was 5 before ???
int dim = 3;
ANNidxArray nnIdx = new ANNidx[k];
ANNdistArray dists = new ANNdist[k];
double normals[maxPoints][dim];
int numPoints = 0;
for (int i = 0; i < 13; i++){
denseStream.getline(line, 1028);
}
int check = 1;
if(!denseStream.getline(line, 1028)){
check=0;
}
while(numPoints < maxPoints && check) {
istringstream ss(line);
for (int i = 0; i < dim; i++){
ss >> densePts[numPoints][i];
}
for (int i = 0; i < dim; i++){
ss >> normals[numPoints][i];
}
numPoints++;
if(!denseStream.getline(line, 1028)){
check=0;
}
}
ANNkd_tree* kdTree = new ANNkd_tree(densePts, numPoints, dim); // ??? + where is nearest neighbor search???
denseStream.close();
// Create sparse point with corresponding feature indices and normal plane
// For now, normal plane is just taken directly from the nearest neighbour
// Aggregate SIFT features for sparse point and calculate homography using
// rotation and translation. For each camera.
sparseStream.getline(line, 1028);
sparseStream.getline(line, 1028);
int numSparsePoints = stoi(line);
vector<sparseModelPoint> sparsePoints(numSparsePoints);
ANNpoint queryPt = annAllocPt(dim);
for (int i = 0; i < 1; i++){
sparseStream.getline(line, 1028);
istringstream ss(line);
sparseModelPoint smp;
for (int j = 0; j < 3; j++){
ss >> smp.point[j];
queryPt[j] = smp.point[j];
}
kdTree->annkSearch(queryPt, k, nnIdx, dists, eps);
// for (int k2 = 0; k2 < k; k2++){
// for (int j = 0; j < 3; j++){
// smp.normal[j] += normals[nnIdx[k2]][j];
// }
// }
// for (int j = 0; j < 3; j++){
// smp.normal[j] = smp.normal[j]/k;
// }
for (int j = 0; j < 3; j++){
smp.normal[j] = normals[nnIdx[0]][j];
// smp.normal[i] = no
}
// for (int j = 0; j < 3; j++){
// smp.normal[j] = normals[nnIdx[0]][j];
// }
// for (int j = 0; j < 3; j++){
// smp.normal[j] = 0.001;
// }
// smp.normal[2] = 1;
int temp;
for (int j = 0; j < 3; j++){
ss >> temp;
}
string ns;
ss >> ns;
int numSift = stoi(ns);
vector<sparseSiftFeature> sparseSifts(numSift);
cout << "Point" << i << ": (" << smp.point[0] << "," << smp.point[1] << "," << smp.point[2] << "): \n";
// CREATION OF VIP
for (int j = 0; j < 1; j++){
sparseSiftFeature ssf;
ss >> ns;
int imageIndex = stoi(ns);
ss >> ns;
int featIndex = stoi(ns);
Camera cam = cameras[imageIndex];
ssf.Sift = cam.SiftVector[featIndex];
/** Side Note: ssf.modelXY and ssf.Sfit.point both refer to
* same point in the image, just with different coordinate
* systems.
* The modelXY has the origin at the centre of the image.
* The pointXY obtained from SIFT has the origin in the bottom left corner
**/
ss >> ssf.modelXY[0];
ss >> ssf.modelXY[1];
// cam: list feature index and camera index for each point. It doesn't tell you which camera
// belongs to which sift descriptor. Tian created cam to place it into camera. c is the same.
// cam and smp are both COPIES, while &ssf is a POINTER => working on ssf edits the real cam.
// ssf: sparse sift feature
// smp: sparse model point
cout << "\t" << ssf.Sift.point[0] << "," << ssf.Sift.point[1] << " in image " << cam.name << "\n";
computeRotation(cam, &ssf, smp);
computeTranslation(cam, &ssf, smp);
computeHomography(cam, &ssf, smp.normal);
// a P becomes a VIP
// Lionel:createVIP(cam, "/home/lionelt/3D_vision_pollefeys/circle_warped.jpeg", &ssf, "Point"+to_string(i)+"Sift"+to_string(j));
// PLEASE DON'T EDIT BEFORE HAVING PULLED THE LATEST CHANGES, I am getting maaad restoring my stuff everytime.
// Also beware of the stuff you push XD
createVIP(cam, "../circle_warped.jpeg", &ssf, "Point"+to_string(i)+"Sift"+to_string(j));
sparseSifts[j] = ssf;
}
smp.features = sparseSifts;
sparsePoints[i] = smp;
} // END OF VIP
sparseStream.close();
// Every point in sparsePoints should now have a list of VIPs
// Get rid of extra information
delete [] nnIdx;
delete [] dists;
delete kdTree;
annClose();
}
int ann(int k, double *ref, int refDim1, int refDim2, double *target, int tarDim1, int tarDim2, int *knnIndxMtrx, double *knnDistMtrx){
int i,j;
int cnt = 0;
int rcnt = 0;
double eps = 0;
int bucketSize = 1;
ANNsplitRule splitRule = (ANNsplitRule) 3;
ANNshrinkRule shrinkRule = (ANNshrinkRule) 0;
/**************************
ref and tree structure
***************************/
ANNpointArray dataPts;
ANNkd_tree* kdTree = NULL;
dataPts = annAllocPts(refDim1, refDim2);
//read in pts
for(i = 0; i < refDim1; i++){
for(j = 0; j < refDim2; j++){
(dataPts[i])[j] = ref[j*refDim1+i];
}
}
kdTree = new ANNkd_tree(dataPts, refDim1, refDim2, bucketSize, splitRule);
/**************************
target and search
***************************/
ANNpoint queryPt;
queryPt = annAllocPt(refDim2);
ANNidxArray nnIdx = new ANNidx[k];
ANNdistArray dists = new ANNdist[k];
for(i = 0; i < tarDim1; i++){
//mk query pt
for(j = 0; j < tarDim2; j++){
queryPt[j] = target[j*tarDim1+i];
}
kdTree->annkSearch(queryPt, k, nnIdx, dists, eps);
//write nnIdx and dist to return mtrx
for(j = 0; j < k; j++){
knnIndxMtrx[j*tarDim1+i] = ++nnIdx[j];
knnDistMtrx[j*tarDim1+i] = dists[j];
}
}
// for(i=0; i<20; i++){
// for(j = 0; j < k; j++){
// cout<<knnIndxMtrx[j*tarDim1+i]<<"\t";
// }
// cout<<endl;
// }
// cout<<endl;
/**************************
clean-up
***************************/
delete kdTree;
delete [] nnIdx;
delete [] dists;
annDeallocPts(dataPts);
annClose();
return(0);
}
int main (int argc, char **argv)
{
ANNpointArray dataPts;
ANNpointArray queryPt;
vector<ANNidxArray> nnIdx;
vector<ANNdistArray> dists;
ANNkd_tree* kdTree;
int dim = atoi(argv[3]);
int k = atoi(argv[4]);
int maxPts = 100000;
dataPts = annAllocPts(maxPts, dim);
queryPt = annAllocPts(maxPts, dim);
int num = readPt(dataPts, dim, argv[1]);
cout << "Load " << num << " Points. " << endl;
int num_query = readPt(queryPt, dim, argv[2]);
cout << "Load " << num_query << " Query points. " << endl;
clock_t start = clock();
// build the kd tree
cout << "Building the kd tree..." << endl;
kdTree = new ANNkd_tree(dataPts, num, dim);
// query
cout << "knn searching..." << endl;
for (int i = 0; i<num_query; i++) {
ANNidxArray nnIdx0 = new ANNidx[k];
ANNdistArray dists0 = new ANNdist[k];
kdTree->annkSearch( queryPt[i], k, nnIdx0, dists0, 0);
nnIdx.push_back(nnIdx0);
dists.push_back(dists0);
}
cout << "Duration = " << ( std::clock() - start ) / (double) CLOCKS_PER_SEC << " sec." << endl;
//print out index
// for (int j = 0; j<num_query; j++) {
// cout << j+1 << " ";
// for (int i = 0; i < k; i++) {
// cout << nnIdx[j][i]+1 << " ";
// }
// cout << endl;
// }
// free memory
for (int i = 0; i<num_query; i++) {
delete [] nnIdx[i];
delete [] dists[i];
}
annDeallocPts(dataPts);
annDeallocPts(queryPt);
delete kdTree;
annClose();
return 1;
}
/**
* Builds a map based on the given number of neighbours
* @param noNeighbours :: The number of nearest neighbours to use to build
* the graph
*/
void NearestNeighbours::build(const int noNeighbours) {
std::map<specid_t, IDetector_const_sptr> spectraDets =
getSpectraDetectors(m_instrument, m_spectraMap);
if (spectraDets.empty()) {
throw std::runtime_error(
"NearestNeighbours::build - Cannot find any spectra");
}
const int nspectra =
static_cast<int>(spectraDets.size()); // ANN only deals with integers
if (noNeighbours >= nspectra) {
throw std::invalid_argument(
"NearestNeighbours::build - Invalid number of neighbours");
}
// Clear current
m_graph.clear();
m_specToVertex.clear();
m_noNeighbours = noNeighbours;
BoundingBox bbox;
// Base the scaling on the first detector, should be adequate but we can look
// at this
IDetector_const_sptr firstDet = (*spectraDets.begin()).second;
firstDet->getBoundingBox(bbox);
m_scale.reset(new V3D(bbox.width()));
ANNpointArray dataPoints = annAllocPts(nspectra, 3);
MapIV pointNoToVertex;
std::map<specid_t, IDetector_const_sptr>::const_iterator detIt;
int pointNo = 0;
for (detIt = spectraDets.begin(); detIt != spectraDets.end(); ++detIt) {
IDetector_const_sptr detector = detIt->second;
const specid_t spectrum = detIt->first;
V3D pos = detector->getPos() / (*m_scale);
dataPoints[pointNo][0] = pos.X();
dataPoints[pointNo][1] = pos.Y();
dataPoints[pointNo][2] = pos.Z();
Vertex vertex = boost::add_vertex(spectrum, m_graph);
pointNoToVertex[pointNo] = vertex;
m_specToVertex[spectrum] = vertex;
++pointNo;
}
ANNkd_tree *annTree = new ANNkd_tree(dataPoints, nspectra, 3);
pointNo = 0;
// Run the nearest neighbour search on each detector, reusing the arrays
ANNidxArray nnIndexList = new ANNidx[m_noNeighbours];
ANNdistArray nnDistList = new ANNdist[m_noNeighbours];
for (detIt = spectraDets.begin(); detIt != spectraDets.end(); ++detIt) {
ANNpoint scaledPos = dataPoints[pointNo];
annTree->annkSearch(scaledPos, // Point to search nearest neighbours of
m_noNeighbours, // Number of neighbours to find (8)
nnIndexList, // Index list of results
nnDistList, // List of distances to each of these
0.0 // Error bound (?) is this the radius to search in?
);
// The distances that are returned are in our scaled coordinate
// system. We store the real space ones.
V3D realPos = V3D(scaledPos[0], scaledPos[1], scaledPos[2]) * (*m_scale);
for (int i = 0; i < m_noNeighbours; i++) {
ANNidx index = nnIndexList[i];
V3D neighbour = V3D(dataPoints[index][0], dataPoints[index][1],
dataPoints[index][2]) *
(*m_scale);
V3D distance = neighbour - realPos;
double separation = distance.norm();
boost::add_edge(m_specToVertex[detIt->first], // from
pointNoToVertex[index], // to
distance, m_graph);
if (separation > m_cutoff) {
m_cutoff = separation;
}
}
pointNo++;
}
delete[] nnIndexList;
delete[] nnDistList;
delete annTree;
annDeallocPts(dataPoints);
annClose();
pointNoToVertex.clear();
m_vertexID = get(boost::vertex_name, m_graph);
m_edgeLength = get(boost::edge_name, m_graph);
}
void mlpackMain()
{
// Get all the parameters.
const string referenceFile = CLI::GetParam<string>("reference_file");
const string queryFile = CLI::GetParam<string>("query_file");
int lsInt = CLI::GetParam<int>("leaf_size");
size_t k = CLI::GetParam<int>("k");
bool monoSearch = false;
arma::mat referenceData;
arma::mat queryData; // So it doesn't go out of scope.
data::Load(referenceFile, referenceData, true);
Log::Info << "Loaded reference data from '" << referenceFile << "' ("
<< referenceData.n_rows << " x " << referenceData.n_cols << ")." << endl;
if (queryFile != "")
{
data::Load(queryFile, queryData, true);
Log::Info << "Loaded query data from '" << queryFile << "' ("
<< queryData.n_rows << " x " << queryData.n_cols << ")." << endl;
}
else
{
monoSearch = true;
++k;
queryData = referenceData;
}
// Sanity check on k value: must be greater than 0, must be less than the
// number of reference points.
if (k > referenceData.n_cols)
{
Log::Fatal << "Invalid k: " << k << "; must be greater than 0 and less ";
Log::Fatal << "than or equal to the number of reference points (";
Log::Fatal << referenceData.n_cols << ")." << endl;
}
// Sanity check on epsilon.
const double epsilon = CLI::GetParam<double>("epsilon");
if (epsilon < 0)
Log::Fatal << "Invalid epsilon: " << epsilon << ". Must be "
"non-negative. " << endl;
// Sanity check on leaf size.
if (lsInt < 0)
{
Log::Fatal << "Invalid leaf size: " << lsInt << ". Must be greater "
"than or equal to 0." << endl;
}
size_t leafSize = lsInt;
size_t maxPts = referenceData.n_elem;
size_t dim = referenceData.n_rows;
ANNidxArray nnIdx = new ANNidx[k];
ANNdistArray dists = new ANNdist[k];
ANNpointArray dataPts = annAllocPts(referenceData.n_cols, referenceData.n_rows);
for (int i = 0; i < referenceData.n_cols; ++i)
{
for (int j = 0; j < referenceData.n_rows; ++j)
{
dataPts[i][j] = referenceData(j,i);
}
}
arma::mat distances(monoSearch ? k - 1 : k, queryData.n_cols);
arma::Mat<size_t> neighbors(monoSearch ? k - 1 : k, queryData.n_cols);
Timer::Start("tree_building");
ANNkd_tree* kdTree = new ANNkd_tree(dataPts, referenceData.n_cols, referenceData.n_rows, lsInt);
Timer::Stop("tree_building");
Timer::Start("computing_neighbors");
arma::vec queryPoint;
for (int i = 0; i < queryData.n_cols; ++i)
{
queryPoint = queryData.col(i);
kdTree->annkSearch(queryPoint.memptr(), k, nnIdx, dists, epsilon);
if (monoSearch)
for (int j = 1; j < k; j++)
{
distances(j - 1, i) = sqrt(dists[j]);
neighbors(j - 1, i) = nnIdx[j];
}
else
for (int j = 0; j < k; j++)
{
distances(j, i) = sqrt(dists[j]);
neighbors(j, i) = nnIdx[j];
}
}
Timer::Stop("computing_neighbors");
// Save output, if desired.
if (CLI::HasParam("distances_file"))
data::Save(CLI::GetParam<string>("distances_file"), distances);
if (CLI::HasParam("neighbors_file"))
data::Save(CLI::GetParam<string>("neighbors_file"), neighbors);
delete [] nnIdx;
delete [] dists;
delete kdTree;
annClose();
}
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;
}
