void CRSIndexMatrix::full2CRS(IndexMatrix &A){
int nz = 0; //count non-zero elements
int idx = 0; //data index
/* *** count number of non-zero entries */
for(int i=0;i<A.numRows();i++)
for(int j=0;j<A.numCols();j++)
if(A(i,j)!=0)
nz++;
/* *** initialize private variables */
_rows = A.numRows();
_cols = A.numCols();
_nonZeros = nz;
delete[] _data;
delete []_colindex;
delete[]_rowptr;
_data = new int[nz];
_colindex = new int[nz];
_rowptr = new int[_rows+1];
/* *** fill _rowptr,_colindex,_data */
for(int i=0;i<_rows;i++){
_rowptr[i] = idx;
for(int j=0; j<_cols;j++){
if(A(i,j)!=0){
_data[idx] = A(i,j);
_colindex[idx]= j;
idx++;
}
}
}
_rowptr[_rows]= nz;
}
void NearestNeighbourSearch<T, CloudType>::checkSizesKnn(const Matrix& query, const IndexMatrix& indices, const Matrix& dists2, const Index k, const unsigned optionFlags, const Vector* maxRadii) const
{
const bool allowSelfMatch(optionFlags & NearestNeighbourSearch<T, CloudType>::ALLOW_SELF_MATCH);
if (allowSelfMatch)
{
if (k > cloud.cols())
throw runtime_error((boost::format("Requesting more points (%1%) than available in cloud (%2%)") % k % cloud.cols()).str());
}
else
{
if (k > cloud.cols()-1)
throw runtime_error((boost::format("Requesting more points (%1%) than available in cloud minus 1 (%2%) (as self match is forbidden)") % k % (cloud.cols()-1)).str());
}
if (query.rows() < dim)
throw runtime_error((boost::format("Query has less dimensions (%1%) than requested for cloud (%2%)") % query.rows() % dim).str());
if (indices.rows() != k)
throw runtime_error((boost::format("Index matrix has a different number of rows (%1%) than k (%2%)") % indices.rows() % k).str());
if (indices.cols() != query.cols())
throw runtime_error((boost::format("Index matrix has a different number of columns (%1%) than query (%2%)") % indices.rows() % query.cols()).str());
if (dists2.rows() != k)
throw runtime_error((boost::format("Distance matrix has a different number of rows (%1%) than k (%2%)") % dists2.rows() % k).str());
if (dists2.cols() != query.cols())
throw runtime_error((boost::format("Distance matrix has a different number of columns (%1%) than query (%2%)") % dists2.rows() % query.cols()).str());
if (maxRadii && (maxRadii->size() != query.cols()))
throw runtime_error((boost::format("Maximum radii vector has not the same length (%1%) than query has columns (%2%)") % maxRadii->size() % k).str());
const unsigned maxOptionFlagsValue(ALLOW_SELF_MATCH|SORT_RESULTS);
if (optionFlags > maxOptionFlagsValue)
throw runtime_error((boost::format("OR-ed value of option flags (%1%) is larger than maximal valid value (%2%)") % optionFlags % maxOptionFlagsValue).str());
}
/* *** convert routines ***********************************************************************************/
void CRSIndexMatrix::CRS2full(IndexMatrix& A){
/* *** initialize and fill matrix */
A.resize(_rows,_cols);
for (int j=0;j<_rows;j++)
for(int k=_rowptr[j] ;k<_rowptr[j+1];k++)
A(j,_colindex[k])=_data[k];
}
unsigned long BruteForceSearch<T>::knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Vector& maxRadii, const Index k, const T epsilon, const unsigned optionFlags) const
{
checkSizesKnn(query, indices, dists2, k, optionFlags, &maxRadii);
const bool allowSelfMatch(optionFlags & NearestNeighbourSearch<T>::ALLOW_SELF_MATCH);
const bool sortResults(optionFlags & NearestNeighbourSearch<T>::SORT_RESULTS);
const bool collectStatistics(creationOptionFlags & NearestNeighbourSearch<T>::TOUCH_STATISTICS);
IndexHeapSTL<Index, T> heap(k);
for (int c = 0; c < query.cols(); ++c)
{
const T maxRadius(maxRadii[c]);
const T maxRadius2(maxRadius * maxRadius);
const Vector& q(query.block(0,c,dim,1));
heap.reset();
for (int i = 0; i < this->cloud.cols(); ++i)
{
const T dist(dist2<T>(this->cloud.block(0,i,dim,1), q));
if ((dist <= maxRadius2) &&
(dist < heap.headValue()) &&
(allowSelfMatch || (dist > numeric_limits<T>::epsilon())))
heap.replaceHead(i, dist);
}
if (sortResults)
heap.sort();
heap.getData(indices.col(c), dists2.col(c));
}
if (collectStatistics)
return (unsigned long)query.cols() * (unsigned long)this->cloud.cols();
else
return 0;
}
void CRSIndexMatrix::writeFull(std::string filename){
/* *** convert to full and print */
IndexMatrix A;
CRS2full(A);
A.write(filename);
}
unsigned long OpenCLSearch<T>::knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Index k, const T epsilon, const unsigned optionFlags, const T maxRadius) const
{
checkSizesKnn(query, indices, dists2, k);
const bool collectStatistics(creationOptionFlags & NearestNeighbourSearch<T>::TOUCH_STATISTICS);
// check K
if (k > MAX_K)
throw runtime_error("number of neighbors too large for OpenCL");
// check consistency of query wrt cloud
if (query.stride() != cloud.stride() ||
query.rows() != cloud.rows())
throw runtime_error("query is not of the same dimensionality as the point cloud");
// map query
if (!(query.Flags & Eigen::DirectAccessBit) || (query.Flags & Eigen::RowMajorBit))
throw runtime_error("wrong memory mapping in query data");
const size_t queryCLSize(query.cols() * query.stride() * sizeof(T));
cl::Buffer queryCL(context, CL_MEM_READ_ONLY | CL_MEM_USE_HOST_PTR, queryCLSize, const_cast<T*>(&query.coeff(0,0)));
knnKernel.setArg(1, sizeof(cl_mem), &queryCL);
// map indices
assert((indices.Flags & Eigen::DirectAccessBit) && (!(indices.Flags & Eigen::RowMajorBit)));
const int indexStride(indices.stride());
const size_t indicesCLSize(indices.cols() * indexStride * sizeof(int));
cl::Buffer indicesCL(context, CL_MEM_WRITE_ONLY | CL_MEM_USE_HOST_PTR, indicesCLSize, &indices.coeffRef(0,0));
knnKernel.setArg(2, sizeof(cl_mem), &indicesCL);
// map dists2
assert((dists2.Flags & Eigen::DirectAccessBit) && (!(dists2.Flags & Eigen::RowMajorBit)));
const int dists2Stride(dists2.stride());
const size_t dists2CLSize(dists2.cols() * dists2Stride * sizeof(T));
cl::Buffer dists2CL(context, CL_MEM_WRITE_ONLY | CL_MEM_USE_HOST_PTR, dists2CLSize, &dists2.coeffRef(0,0));
knnKernel.setArg(3, sizeof(cl_mem), &dists2CL);
// set resulting parameters
knnKernel.setArg(4, k);
knnKernel.setArg(5, (1 + epsilon)*(1 + epsilon));
knnKernel.setArg(6, maxRadius*maxRadius);
knnKernel.setArg(7, optionFlags);
knnKernel.setArg(8, indexStride);
knnKernel.setArg(9, dists2Stride);
knnKernel.setArg(10, cl_uint(cloud.cols()));
// if required, map visit count
vector<cl_uint> visitCounts;
const size_t visitCountCLSize(query.cols() * sizeof(cl_uint));
cl::Buffer visitCountCL;
if (collectStatistics)
{
visitCounts.resize(query.cols());
visitCountCL = cl::Buffer(context, CL_MEM_READ_ONLY | CL_MEM_USE_HOST_PTR, visitCountCLSize, &visitCounts[0]);
knnKernel.setArg(11, sizeof(cl_mem), &visitCountCL);
}
// execute query
queue.enqueueNDRangeKernel(knnKernel, cl::NullRange, cl::NDRange(query.cols()), cl::NullRange);
queue.enqueueMapBuffer(indicesCL, true, CL_MAP_READ, 0, indicesCLSize, 0, 0);
queue.enqueueMapBuffer(dists2CL, true, CL_MAP_READ, 0, dists2CLSize, 0, 0);
if (collectStatistics)
queue.enqueueMapBuffer(visitCountCL, true, CL_MAP_READ, 0, visitCountCLSize, 0, 0);
queue.finish();
// if required, collect statistics
if (collectStatistics)
{
unsigned long totalVisitCounts(0);
for (size_t i = 0; i < visitCounts.size(); ++i)
totalVisitCounts += (unsigned long)visitCounts[i];
return totalVisitCounts;
}
else
return 0;
}