Skip to content

Commit

Permalink
make IndexedPointBuffer based on nanoflann instead of FLANN to remove…
Browse files Browse the repository at this point in the history 
… external dependency #176
  • Loading branch information
@hobu
hobu committed Oct 11, 2013
1 parent e52664f commit 46ff514
Show file tree
Hide file tree
Showing 5 changed files with 145 additions and 182 deletions.
4 changes: 2 additions & 2 deletions apps/pcequal.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -252,7 +252,7 @@ int PcEqual::execute()
double sy = source_data.applyScaling(sDimY, i);
double sz = source_data.applyScaling(sDimZ, i);

std::vector<boost::uint32_t> ids = candidate_data.neighbors(sx, sy, sz, 1);
std::vector<std::size_t> ids = candidate_data.neighbors(sx, sy, sz, 1);

if (!ids.size())
{
Expand All @@ -261,7 +261,7 @@ int PcEqual::execute()
throw app_runtime_error(oss.str() );
}

boost::uint32_t id = ids[0];
std::size_t id = ids[0];
double cx = candidate_data.applyScaling(cDimX, id);
double cy = candidate_data.applyScaling(cDimY, id);
double cz = candidate_data.applyScaling(cDimZ, id);
Expand Down
6 changes: 3 additions & 3 deletions apps/pdalinfo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -302,11 +302,11 @@ void PcInfo::dumpQuery(Stage const& stage, IndexedPointBuffer& data) const
count = 1;

double d(0.0);
std::vector<boost::uint32_t> ids = data.neighbors(x, y, z, d, count);
std::vector<std::size_t> ids = data.neighbors(x, y, z, d, count);

PointBuffer response(data.getSchema(), count);
typedef std::vector<boost::uint32_t>::const_iterator Iterator;
std::vector<boost::uint32_t>::size_type pos(0);
typedef std::vector<std::size_t>::const_iterator Iterator;
std::vector<std::size_t>::size_type pos(0);
for (Iterator i = ids.begin(); i != ids.end(); ++i)
{
response.copyPointFast(pos, *i, data);
Expand Down
74 changes: 66 additions & 8 deletions include/pdal/PointBuffer.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,8 @@
#include <flann/flann.hpp>
#endif

#include <pdal/third/nanoflann.hpp>

#include <vector>


Expand Down Expand Up @@ -935,8 +937,42 @@ class IndexedPointBuffer : public PointBuffer
IndexedPointBuffer(PointBuffer const& buffer);
~IndexedPointBuffer();

std::vector<boost::uint32_t> neighbors(double const& x, double const& y, double const& z, double distance, boost::uint32_t count=1);
std::vector<boost::uint32_t> radius(double const& x, double const& y, double const& z, double const& r);
// Satisfy nanoflann::DatasetAdapter
inline size_t kdtree_get_point_count() const
{
return PointBuffer::getNumPoints();
}

inline double kdtree_get_pt(const size_t idx, int dim) const
{
if (dim == 0)
return applyScaling(*m_dimX, idx);
if (dim == 1)
return applyScaling(*m_dimY, idx);
if (dim == 2)
return applyScaling(*m_dimZ, idx);

std::stringstream oss;
oss << "dimension '" << dim << "' is out of range 0-2";
throw std::runtime_error(oss.str());
}

inline double kdtree_distance(const double *p1, const size_t idx_p2,size_t size) const
{
const double d0 = applyScaling(*m_dimX, idx_p2) - applyScaling(*m_dimX, size-1);
const double d1 = applyScaling(*m_dimY, idx_p2) - applyScaling(*m_dimY, size-1);
const double d2 = applyScaling(*m_dimZ, idx_p2) - applyScaling(*m_dimZ, size-1);
// const T d0=p1[0]-pts[idx_p2].x;
// const T d1=p1[1]-pts[idx_p2].y;
// const T d2=p1[2]-pts[idx_p2].z;
return d0*d0+d1*d1+d2*d2;
}

template <class BBOX> bool kdtree_get_bbox(BBOX &bb) const ;


std::vector<size_t> neighbors(double const& x, double const& y, double const& z, double distance, boost::uint32_t count=1);
std::vector<size_t> radius(double const& x, double const& y, double const& z, double const& r);

// /// Copy constructor. The data array is simply memcpy'd.
// IndexedPointBuffer(const IndexedPointBuffer&) : PointBuffer(buffer) {}
Expand All @@ -946,13 +982,35 @@ class IndexedPointBuffer : public PointBuffer
void build(bool b3D=true);

private:
std::vector<double> m_coordinates;
#ifdef PDAL_HAVE_FLANN
flann::Matrix<double>* m_dataset;
flann::KDTreeSingleIndex<flann::L2_Simple<double> >* m_index;
#endif

pdal::Dimension const* m_dimX;
pdal::Dimension const* m_dimY;
pdal::Dimension const* m_dimZ;
bool m_is3D;
void setCoordinateDimensions();

typedef nanoflann::KDTreeSingleIndexAdaptor<
nanoflann::L2_Adaptor<double, IndexedPointBuffer> ,
IndexedPointBuffer,
3 /* dim */
> my_kd_tree_t;

my_kd_tree_t* m_index;
};

template <class BBOX> bool IndexedPointBuffer::kdtree_get_bbox(BBOX &bb) const
{
pdal::Bounds<double> const& bounds = getSpatialBounds();
if (bounds.empty())
return false;

for (unsigned i=0; i < 3; ++i)
{
bb[i].low = bounds.getMinimum(i);
bb[i].high = bounds.getMaximum(i);
}
return true;

}
#ifdef PDAL_COMPILER_MSVC
# pragma warning(pop)
#endif
Expand Down
221 changes: 65 additions & 156 deletions src/PointBuffer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -606,193 +606,105 @@ std::ostream& operator<<(std::ostream& ostr, const PointBuffer& pointBuffer)
IndexedPointBuffer::IndexedPointBuffer( const Schema& schema,
boost::uint32_t capacity)
: PointBuffer(schema, capacity)
#ifdef PDAL_HAVE_FLANN
, m_dataset(0)
, m_dimX(0)
, m_dimY(0)
, m_dimZ(0)
, m_is3D(true)
, m_index(0)
#endif
{

setCoordinateDimensions();
}

IndexedPointBuffer::IndexedPointBuffer(PointBuffer const& other)
: PointBuffer(other)
#ifdef PDAL_HAVE_FLANN
, m_dataset(0)
, m_dimX(0)
, m_dimY(0)
, m_dimZ(0)
, m_is3D(true)
, m_index(0)
#endif

{

setCoordinateDimensions();
}
IndexedPointBuffer::IndexedPointBuffer(IndexedPointBuffer const& other)
: PointBuffer(other)
, m_coordinates(other.m_coordinates)
#ifdef PDAL_HAVE_FLANN
, m_dataset(other.m_dataset)
, m_index(other.m_index)
#endif
, m_dimX(other.m_dimX)
, m_dimY(other.m_dimY)
, m_dimZ(other.m_dimZ)
, m_is3D(other.m_is3D)
, m_index(0)
{
setCoordinateDimensions();
build(m_is3D);
}

void IndexedPointBuffer::setCoordinateDimensions()
{
m_dimX = m_schema.getDimensionPtr("X");
m_dimY = m_schema.getDimensionPtr("Y");
m_dimZ = m_schema.getDimensionPtr("Z");
if (!m_dimZ)
m_is3D = false;
}

void IndexedPointBuffer::build(bool b3D)
{
Dimension const& dx = m_schema.getDimension("X");
Dimension const& dy = m_schema.getDimension("Y");
m_is3D = b3D;
size_t nDims = m_is3D && m_dimZ ? 3 : 2;
m_index = new my_kd_tree_t(nDims, *this, nanoflann::KDTreeSingleIndexAdaptorParams(10, nDims ) );

Dimension const* dz = m_schema.getDimensionPtr("Z");

for (boost::uint32_t pointIndex=0; pointIndex<getNumPoints(); pointIndex++)
if (!m_dimX)
{
double x = applyScaling(dx, pointIndex);
double y = applyScaling(dy, pointIndex);
double z = applyScaling(*dz, pointIndex);
m_coordinates.push_back(x);
m_coordinates.push_back(y);
if (dz && b3D)
{
m_coordinates.push_back(z);
}
}

boost::uint32_t num_dims = dz && b3D ? 3 : 2;


#ifdef PDAL_HAVE_FLANN
delete m_dataset;
delete m_index;

m_dataset = new flann::Matrix<double>(&m_coordinates[0], getNumPoints(), num_dims);


m_index = new flann::KDTreeSingleIndex<flann::L2_Simple<double> >(*m_dataset, flann::KDTreeIndexParams(4));
throw pdal_error("No 'X' dimension exists to build index with!");
}
if (!m_dimY)
{
throw pdal_error("No 'Y' dimension exists to build index with!");
}

m_index->buildIndex();
#endif

}

std::vector<boost::uint32_t> IndexedPointBuffer::radius(double const& x, double const& y, double const& z, double const& r)
std::vector<size_t> IndexedPointBuffer::radius( double const& x,
double const& y,
double const& z,
double const& r)
{
std::vector<boost::uint32_t> output;

#ifdef PDAL_HAVE_FLANN

if (!m_index)
{
throw pdal_error("Index is not initialized! Unable to query!");
}
Dimension const* dz = m_schema.getDimensionPtr("Z");
std::vector<size_t> output;
std::vector<std::pair<size_t,double> > ret_matches;
nanoflann::SearchParams params;
//params.sorted = false;

boost::uint32_t num_dimensions = dz && m_dataset->cols == 3 ? 3 : 2;

std::vector< std::vector<double> > distances_vec;

std::vector< std::vector<size_t> > indices_vec;

std::vector<double> query_vec(num_dimensions);
query_vec[0] = x;
query_vec[1] = y;
if (num_dimensions > 2)
query_vec[2] = z;

flann::Matrix<double> query_mat(&query_vec[0], 1, num_dimensions);

int checks(flann::FLANN_CHECKS_UNLIMITED);
flann::SearchParams parameters(checks, 0 /*eps*/, false /*sorted*/);
std::vector<double> pt;
pt.push_back(x); pt.push_back(y);
if (m_is3D) pt.push_back(z);
const size_t count = m_index->radiusSearch(&pt[0], r, ret_matches, params);

// FLANN is more efficient using an internal heap
// search structure for large numbers of points. For radiusSearch,
// we're just guessing how many neighbors are going to
// come back, however.
if (getNumPoints() > 16777216) // 2^24
for (size_t i = 0; i < count; ++i)
{
parameters.use_heap = flann::FLANN_True;
output.push_back(ret_matches[i].first);
}

#if 0
m_index->radiusSearch(query_mat,
indices_vec,
distances_vec,
r,
parameters);

for (unsigned i=0; i < indices_vec[0].size() ; ++i)
{
output.push_back(indices_vec[0][i]);
}
#endif
#else
boost::ignore_unused_variable_warning(x);
boost::ignore_unused_variable_warning(y);
boost::ignore_unused_variable_warning(z);
boost::ignore_unused_variable_warning(r);
#endif

return output;
}

std::vector<boost::uint32_t> IndexedPointBuffer::neighbors(double const& x, double const& y, double const& z, double distance, boost::uint32_t k)
std::vector<size_t> IndexedPointBuffer::neighbors( double const& x,
double const& y,
double const& z,
double distance,
boost::uint32_t k)
{
std::vector<boost::uint32_t> output;

#ifdef PDAL_HAVE_FLANN

if (!m_index)
{
throw pdal_error("Index is not initialized! Unable to query!");
}
Dimension const* dz = m_schema.getDimensionPtr("Z");
boost::uint32_t num_dimensions = dz && m_dataset->cols == 3 ? 3 : 2;

std::vector<double> distances_vec;
distances_vec.resize(k);

std::vector<boost::int32_t> indices_vec;
indices_vec.resize(k);
indices_vec.assign(indices_vec.size(), -1);

std::vector<double> query_vec(num_dimensions);
query_vec[0] = x;
query_vec[1] = y;
if (num_dimensions > 2)
query_vec[2] = z;


flann::Matrix<int> indices_mat(&indices_vec[0], 1, k);
flann::Matrix<double> distances_mat(&distances_vec[0], 1, k);
flann::Matrix<double> query_mat(&query_vec[0], 1, num_dimensions);

m_index->knnSearch(query_mat,
indices_mat,
distances_mat,
k,
flann::SearchParams(128));
for (unsigned i=0; i < k; ++i)
{
// if distance is 0, just return the nearest one, otherwise filter by distance
if (Utils::compare_distance<double>(distance, 0))
{
if (indices_vec[i] != -1)
output.push_back(indices_vec[i]);

}
else
{
if (::sqrt(distances_vec[i]) < distance)
{
if (indices_vec[i] != -1)
output.push_back(indices_vec[i]);
}
std::vector<size_t> output(k);
std::vector<double> out_dist_sqr(k);
nanoflann::KNNResultSet<double> resultSet(k);

resultSet.init(&output[0], &out_dist_sqr[0] );

std::vector<double> pt;
pt.push_back(x); pt.push_back(y);
if (m_is3D) pt.push_back(z);
m_index->findNeighbors(resultSet, &pt[0], nanoflann::SearchParams(10));

}
}
#else
boost::ignore_unused_variable_warning(x);
boost::ignore_unused_variable_warning(y);
boost::ignore_unused_variable_warning(z);
boost::ignore_unused_variable_warning(distance);
boost::ignore_unused_variable_warning(k);
#endif

return output;
}
Expand All @@ -801,9 +713,6 @@ std::vector<boost::uint32_t> IndexedPointBuffer::neighbors(double const& x, doub

IndexedPointBuffer::~IndexedPointBuffer()
{
#ifdef PDAL_HAVE_FLANN
delete m_index;
delete m_dataset;
#endif
}
} // namespace pdal
Loading

0 comments on commit 46ff514

Please sign in to comment.