int main(int argc, char** argv)
{
int rows = 9000;
int cols = 128;
int tcount = 1000;
printf("Reading input data file.\n");
float* dataset = read_points("dataset.dat", rows, cols);
printf("Reading test data file.\n");
float* testset = read_points("testset.dat", tcount, cols);
int nn = 3;
int* result = (int*) malloc(tcount*nn*sizeof(int));
float* dists = (float*) malloc(tcount*nn*sizeof(float));
struct FLANNParameters p = DEFAULT_FLANN_PARAMETERS;
p.algorithm = KDTREE;
p.trees = 8;
p.log_level = LOG_INFO;
float speedup;
printf("Computing index.\n");
flann_index_t index_id = flann_build_index(dataset, rows, cols, &speedup, &p);
flann_find_nearest_neighbors_index(index_id, testset, tcount, result, dists, nn, &p);
write_results("results.dat",result, tcount, nn);
flann_free_index(index_id, &p);
free(dataset);
free(testset);
free(result);
free(dists);
return 0;
}
int main(int argc, char** argv)
{
float* dataset;
float* testset;
int nn;
int* result;
float* dists;
struct FLANNParameters p;
float speedup;
flann_index_t index_id;
int rows = 9000;
int cols = 128;
int tcount = 1000;
/*
* The files dataset.dat and testset.dat can be downloaded from:
* http://people.cs.ubc.ca/~mariusm/uploads/FLANN/datasets/dataset.dat
* http://people.cs.ubc.ca/~mariusm/uploads/FLANN/datasets/testset.dat
*/
printf("Reading input data file.\n");
dataset = read_points("dataset.dat", rows, cols);
printf("Reading test data file.\n");
testset = read_points("testset.dat", tcount, cols);
nn = 3;
result = (int*) malloc(tcount*nn*sizeof(int));
dists = (float*) malloc(tcount*nn*sizeof(float));
p = DEFAULT_FLANN_PARAMETERS;
p.algorithm = FLANN_INDEX_KDTREE;
p.trees = 8;
p.log_level = FLANN_LOG_INFO;
p.checks = 64;
printf("Computing index.\n");
index_id = flann_build_index(dataset, rows, cols, &speedup, &p);
flann_find_nearest_neighbors_index(index_id, testset, tcount, result, dists, nn, &p);
write_results("results.dat",result, tcount, nn);
flann_free_index(index_id, &p);
free(dataset);
free(testset);
free(result);
free(dists);
return 0;
}
void NearestNeighborFLANN::findNearestNeighbors(vector<double>* query, std::vector<int>* resultIndices, unsigned int k) {
assert (query != 0);
assert (resultIndices != 0);
// assert (static_cast<int>(query->size()) == dimension);
if (static_cast<int>(query->size()) != dimension) {
throw runtime_error("Mismatch of query and data dimension.");
}
if (static_cast<int>(k) > this->rows) {
throw runtime_error("Number of neighbors k is bigger than the amount of data points.");
}
resultIndices->clear();
int nn = static_cast<int>(k);
int tcount = 1;
int* result = new int[nn];
float* dists = new float[nn];
float* queryData = new float[dimension]; //TODO: is there also a double version?!?
for (int i = 0; i < dimension; ++i) {
queryData[i] = static_cast<float>( (*query)[i] );
}
flann_find_nearest_neighbors_index(index_id, queryData, tcount, result, dists, nn, parameters.checks, ¶meters);
brics_3d::Coordinate resultDistance; //distance has same data-type as Coordinate, although the meaning is different TODO: global distance typedef?
int resultIndex;
for (int i = 0; i < nn; i++) {
resultDistance = static_cast<brics_3d::Coordinate>(sqrt(dists[i])); //seems to return squared distance (although documentation does not suggest)
resultIndex = result[i];
if (resultDistance <= maxDistance || maxDistance < 0.0) { //if max distance is < 0 then the distance should have no influence
resultIndices->push_back(resultIndex);
}
}
delete[] queryData;
delete[] dists;
delete[] result;
}
void Registration::ICPFindCorrespondance(const Point3DSet* pRef, const Point3DSet* pOrigin, const MagicMath::HomoMatrix4* pTransInit,
std::vector<int>& sampleIndex, std::vector<int>& correspondIndex)
{
//DebugLog << "Registration::ICPFindCorrespondance" << std::endl;
//float timeStart = MagicCore::ToolKit::GetTime();
/*int dim = 3;
int refNum = pRef->GetPointNumber();
float* dataSet = new float[refNum * dim];
for (int i = 0; i < refNum; i++)
{
MagicMath::Vector3 pos = pRef->GetPoint(i)->GetPosition();
dataSet[dim * i + 0] = pos[0];
dataSet[dim * i + 1] = pos[1];
dataSet[dim * i + 2] = pos[2];
}*/
/*FLANNParameters searchPara;
searchPara = DEFAULT_FLANN_PARAMETERS;
searchPara.algorithm = FLANN_INDEX_KDTREE;
searchPara.trees = 8;
searchPara.log_level = FLANN_LOG_INFO;
searchPara.checks = 64;
float speedup;
flann_index_t indexId = flann_build_index(dataSet, refNum, dim, &speedup, &searchPara);
DebugLog << " Flann: " << MagicCore::ToolKit::GetTime() - timeStart << std::endl;*/
int nn = 1;
int dim = 3;
int searchNum = sampleIndex.size();
float* searchSet = new float[searchNum * dim];
for (int i = 0; i < searchNum; i++)
{
MagicMath::Vector3 pos = pTransInit->TransformPoint( pOrigin->GetPoint(sampleIndex.at(i))->GetPosition() );
searchSet[dim * i + 0] = pos[0];
searchSet[dim * i + 1] = pos[1];
searchSet[dim * i + 2] = pos[2];
}
int* pIndex = new int[searchNum * nn];
float* pDist = new float[searchNum * nn];
flann_find_nearest_neighbors_index(mFlannIndex, searchSet, searchNum, pIndex, pDist, nn, &mSearchPara);
//flann_free_index(indexId, &searchPara);
//delete []dataSet;
delete []searchSet;
//delete wrong correspondance
float distThre = 500.f;
float norThre = 0.1f; //cos(85);
std::vector<int> sampleIndexBak = sampleIndex;
sampleIndex.clear();
correspondIndex.clear();
for (int i = 0; i < searchNum; i++)
{
if (pDist[i] > distThre)
{
// DebugLog << "Large dist: " << pDist[i] << std::endl;
continue;
}
float norDist = pRef->GetPoint(pIndex[i])->GetNormal() * (pTransInit->RotateVector( pOrigin->GetPoint(sampleIndexBak.at(i))->GetNormal() ));
if (norDist < norThre || norDist > 1.0)
{
// DebugLog << "Large Nor: " << norDist << std::endl;
continue;
}
sampleIndex.push_back(sampleIndexBak.at(i));
correspondIndex.push_back(pIndex[i]);
}
//DebugLog << "Sample Number: " << sampleIndex.size() << std::endl;
delete []pIndex;
delete []pDist;
}
