jbehley / octree Goto Github PK

Fast radius neighbor search with an Octree (ICRA 2015)

License: MIT License

CMake 2.63% C++ 97.37%

octree icra2015

octree's Introduction

Efficient Radius Neighbor Search in Three-dimensional Point Clouds

This repository provides the Octree implementation of the paper "Efficient Radius Neighbor Search in Three-dimensional Point Clouds" by J. Behley, V. Steinhage, and A.B. Cremers, University of Bonn presented at the IEEE International Conference on Robotics and Automation (ICRA), 2015.

Features

Fast radius neighbor search for three-dimensional point clouds.
Fully templated for maximal flexibility to support arbitrary point representations & containers
Supports arbitrary p-norms: L1, L2 and Maximum norm included.
Nearest neighbor search with arbitrary norms (added 25. November 2015).

Building the examples & tests

The octree itself has no dependencies. However, for compiling the examples, you need CMake and Boost C++ library. For building the examples you have to first build the project:

mkdir build
cd build
cmake ..
make

To run the examples, you need some point cloud data:

wget http://jbehley.github.io/data/wachtberg_folds.zip
unzip wachtberg_folds.zip -d data

Now you can run the examples:

./example1 data/scan_001_points.dat

which perform some queries and demonstrate the flexibility of our Octree implementation to handle different implementations of points.

The different examples show some use cases of the octree. example1 demonstrates the general usage with point data types providing public access to x,y,z coordinates. example2 shows how to use a different point type, which non-public coordinates. example3 shows how to use the templated method inside an also templated descriptor.

We also provide a test case using the Google Test Framework (GTest), which is automatically build if the package is either found by Cmake or in the corresponding source directory, e.g., /usr/src/gtest/. You can invoke the testsuite with

./octree-test

Contact

Feel free to contact me (see also my academic homepage) if you have questions regarding the implementation.

Attribution

If you use the implementation or ideas from the corresponding paper in your academic work, it would be nice if you cite the corresponding paper:

J. Behley, V. Steinhage, A.B. Cremers. Efficient Radius Neighbor Search in Three-dimensional Point Clouds, Proc. of the IEEE International Conference on Robotics and Automation (ICRA), 2015.

The BibTeX entry for the paper is::

@conference{behley2015icra,
     author = {Jens Behley and Volker Steinhage and Armin B. Cremers},
      title = {{Efficient Radius Neighbor Seach in Three-dimensional Point Clouds}},
  booktitle = {Proc. of the IEEE International Conference on Robotics and Automation (ICRA)},
       year = {2015}
}

License

This project is free software made available under the MIT License. For details see the LICENSE file.

octree's People

Contributors

Stargazers

Watchers

octree's Issues

How to add it to cmake project?

I want to use it in my cmake project. I should add some line to my CMakeLists.txt like follows.

find_package(xxx)
include_directories(${xxxx})
target_link_libraries(${xxx})

What the xxx for this repo? Thanks!

findNeighbor function problem

findNeighbor function don't check that root_ variable is valid.(the func have not check if(root_ == 0) condition)
But radiusNeighbor function check that root_ variable is valid.
So if root_ is not initialized situation, findNeighbor make program fault and radiusNeighbor doesn't make fault.
Then I think should be add "check root_ code" in findNeighbor.
The code is important in dynamic cloud situation.
Then user can well know error situation.

For example,

template <typename PointT, typename ContainerT>
template
int32_t Octree<PointT, ContainerT>::findNeighbor(const PointT& query, float minDistance) const
{
if(root_==0) return -999;//****return error code that user defined
float maxDistance = std::numeric_limits::infinity();
int32_t resultIndex = -1;
findNeighbor(root_, query, minDistance, maxDistance, resultIndex);

return resultIndex;
}

Performance improvements replacing std::pow( float, 2 ) with explicit squaring, std::array<uint32_t,8> in createOctant

In the L2Distance adapter calls to std::pow can be replaced with explicit squaring, grossly improving performance under my compiler environment (VS2013). In fact, by replacing the calls to std::pow all of the adapter code more aggressively inlines, making the radiusNeighbours method much faster.

I also replaced the three std::vector<uint32_t> in createOctant with std::array<uint32_t, 8> to avoid the heap allocations during tree construction. These helped the speed of the tree build, although in my application that time is dwarfed by the time spent querying.

		std::array<uint32_t, 8> childStarts = { 0, 0, 0, 0, 0, 0, 0, 0 };
		std::array<uint32_t, 8> childEnds = { 0, 0, 0, 0, 0, 0, 0, 0 };
		std::array<uint32_t, 8> childSizes = { 0, 0, 0, 0, 0, 0, 0, 0 };

How to sort a set of 3D points?

Is that possible to add an example code to show how one can use this octree package in order to sort a set of 3D points based on, for example Ecuadorian distance metric?

Enhancement: Eigen support

First, I would like to thank you for the library. It is a very nice templated octree implementation.

Based on my own requirements, I needed support for Eigen::Matrix3Xd for the data structure (didn't want to make a std::vector of Eigen::Vector3d and wrap Eigen::Vector3d with the accessor traits.

I forked the library and made my own updates (also tried to clean-up the cmake build to make it an interface library to better install the header file). Unfortunately I couldn't find a good way to keep it templated with ContainterT and PointT and support Eigen Matrix... though I was tempted to use the nanoflann approach.

With Eigen in my testing (MSVC build) it seems to actually run faster than using the Point3f structs in the examples.

No real issue, just want to let you know that the code is there.

Thanks!

Consider Linearizing the Tree

Hi all, great job on the implementation. The performance over PCLs Kdtree is remarkable. However, I've actually implemented a linear version of the recursive search operation, and I notice a 2x speedup in search over your pointer based tree search implementation. A snippet of the code is here. The linearisation step of the tree is almost negligible, but the 2x speedup in search has been very helpful. Not sure why this happens.

typedef pcl::PointXYZ Point3f;

class OctreeGPU : public unibn::Octree<Point3f>{
public:

    class OctantLinear
    {
    public:
        OctantLinear(){}
        ~OctantLinear(){}

        bool isLeaf;

        float x, y, z;
        float extent;

        uint32_t start, end;
        uint32_t size;

        int32_t childIndex[8] = { -1 };

        bool seen1 = false;
        bool seen2 = false;

        OctantLinear(Octant* octant){
            this->isLeaf = octant->isLeaf;
            this->x = octant->x;
            this->y = octant->y;
            this->z = octant->z;
            this->extent = octant->extent;
            this->start = octant->start;
            this->end = octant->start;
            this->size = octant->size;
        }
    };

    int leafCount_ = 0;
    std::vector<OctantLinear> octantLinearVector_;

    OctreeGPU() : unibn::Octree<Point3f>(){
    }

    void linearizeTree(){
        recursiveSearch(this->root_);
        cout << "Count: " << octantLinearVector_.size() << endl;
        //cout << this->root_->child[5]->child[0]->x << endl;
        //cout << octantLinearVector_[octantLinearVector_[octantLinearVector_[octantLinearVector_.size()-1].childIndex[5]].childIndex[0]].x << endl;
    }

    int recursiveSearch(Octant* octant){
        if(octant == NULL) return -1;
        OctantLinear octantLinear(octant);
        if(octantLinear.isLeaf) leafCount_++;
        for(int i=0; i<8; i++){
            octantLinear.childIndex[i] = recursiveSearch(octant->child[i]);
        }
        octantLinearVector_.push_back(octantLinear);
        return octantLinearVector_.size() - 1;
    }

    int iter_ = 0;

    bool findNeighborRecursive(const int octantIndex, Point3f& query, float minDistance, float& maxDistance, int32_t& resultIndex){

        iter_ ++;
        const std::vector<Point3f>& points = *data_;
        OctantLinear& octantLinear = octantLinearVector_[octantIndex];
        //cout << "**" << endl;
        if(DEBUG) cout << octantIndex << endl;

        if (octantLinear.isLeaf)
        {
            uint32_t idx = octantLinear.start;
            float sqrMaxDistance = unibn::L2Distance<Point3f>::sqr(maxDistance);
            float sqrMinDistance = (minDistance < 0) ? minDistance : unibn::L2Distance<Point3f>::sqr(minDistance);

            for (uint32_t i = 0; i < octantLinear.size; ++i)
            {
                const Point3f& p = points[idx];
                float dist = unibn::L2Distance<Point3f>::compute(query, p);
                if (dist > sqrMinDistance && dist < sqrMaxDistance)
                {
                    resultIndex = idx;
                    sqrMaxDistance = dist;
                }
                idx = successors_[idx];
            }

            maxDistance = unibn::L2Distance<Point3f>::sqrt(sqrMaxDistance);
            bool lastReturn = inside(query, maxDistance, octantLinear);
            if(DEBUG) cout << "inside: " << lastReturn << endl;
            return lastReturn;
        }

        //cout << "--" << endl;

        uint32_t mortonCode = 0;
        if (unibn::get<0>(query) > octantLinear.x) mortonCode |= 1;
        if (unibn::get<1>(query) > octantLinear.y) mortonCode |= 2;
        if (unibn::get<2>(query) > octantLinear.z) mortonCode |= 4;

        if(octantLinear.childIndex[mortonCode] >= 0)
        {
            if(DEBUG) cout << "morton" << endl;
            if (findNeighborRecursive(octantLinear.childIndex[mortonCode], query, minDistance, maxDistance, resultIndex)) return true;
        }

        //cout << "MID " << octantIndex << endl;

        // 2. if current best point completely inside, just return.
        float sqrMaxDistance = unibn::L2Distance<Point3f>::sqr(maxDistance);

        // 3. check adjacent octants for overlap and check these if necessary.
        bool found = false;
        for (uint32_t c = 0; c < 8; ++c)
        {
            if(DEBUG) cout << octantIndex << " loop idx: " << c << " " << octantLinear.childIndex[c] << endl;
            if(DEBUG) cout << "    test " << mortonCode << " " << sqrMaxDistance << " " << maxDistance << endl;
            if (c == mortonCode) continue;
            if (octantLinear.childIndex[c] < 0) continue;
            if (!overlaps(query, maxDistance, sqrMaxDistance, octantLinearVector_[octantLinear.childIndex[c]])) continue;
            if (findNeighborRecursive(octantLinear.childIndex[c], query, minDistance, maxDistance, resultIndex))
              return true;
        }

        return inside(query, maxDistance, octantLinear);
    }

    bool inside(const Point3f& query, float radius, const OctantLinear& octantLinear)
    {
        // we exploit the symmetry to reduce the test to test
        // whether the farthest corner is inside the search ball.
        float x = unibn::get<0>(query) - octantLinear.x;
        float y = unibn::get<1>(query) - octantLinear.y;
        float z = unibn::get<2>(query) - octantLinear.z;

        x = std::abs(x) + radius;
        y = std::abs(y) + radius;
        z = std::abs(z) + radius;

        if (x > octantLinear.extent) return false;
        if (y > octantLinear.extent) return false;
        if (z > octantLinear.extent) return false;

        return true;
    }

    bool overlaps(const Point3f& query, float radius, float sqRadius, const OctantLinear& octantLinear)
    {
        // we exploit the symmetry to reduce the test to testing if its inside the Minkowski sum around the positive quadrant.
        float x = unibn::get<0>(query) - octantLinear.x;
        float y = unibn::get<1>(query) - octantLinear.y;
        float z = unibn::get<2>(query) - octantLinear.z;

        x = std::abs(x);
        y = std::abs(y);
        z = std::abs(z);

        float maxdist = radius + octantLinear.extent;

        // Completely outside, since q' is outside the relevant area.
        if (x > maxdist || y > maxdist || z > maxdist) return false;

        int32_t num_less_extent = (x < octantLinear.extent) + (y < octantLinear.extent) + (z < octantLinear.extent);

        // Checking different cases:

        // a. inside the surface region of the octant.
        if (num_less_extent > 1) return true;

        // b. checking the corner region && edge region.
        x = std::max(x - octantLinear.extent, 0.0f);
        y = std::max(y - octantLinear.extent, 0.0f);
        z = std::max(z - octantLinear.extent, 0.0f);

        return (unibn::L2Distance<Point3f>::norm(x, y, z) < sqRadius);
    }


};

Why make the Octree not copyable and assignable?

Extend to find closest N neighbors

This is fast and works great for radius search. Is it also possible to implement a fast search for closest N neighbors with this octree structure?

Bug in overlaps method

Hi!
Thanks a lot for sharing your code!!

I believe there is a bug with the method overlaps.
More precisely in Octree.hpp line 722: "if (x < o->extent || y < o->extent || z < o->extent) return true;"
The concept seems correct in 2D, however in 3D, at least two of the three conditions have to be valid at the same time to be able to return true.

A simple example revealing the bug is the following:
query = (x = 0.025, y = 0.025, 0.025)
radius = 0.025
octant_center = (x = 0.025, y = -0.025, z = -0.025)
octant_extent = 0.025

When displaying the octant and the sphere, it is clear that the sphere does not overlap with the octant although the overlaps method returns true.

can colour be in point attribute?

now, i only found point(x, y, z), not include point-cloud other attribute, example color(r,g,b), (nx,ny,nz)

how to add other attribute?
thanks

Removal of single points

"In future, we might add also other neighbor queries and implement the removal and adding of points." - is mentioned in your comments, do you still plan to? It would certainly be a nice feature.

Stack overlow - there are no maximum depth termination conditions

First of all, thank you for this great work.

I am encountering stack overflow conditions using point clouds that are generally sparsely distributed but highly dense in one specific area. I have 650,000 points - but there is one node that ends up with ~50 points in a very dense region, causing infinite calls to createOctant(). Because there is no maximum depth termination, it seems to go on forever. Is this an expected pathological case? Eventually the extent of the octants being created is zero - it isn't clear to me why. Are you doing aggressive regression testing that I can plug my data into?

If it's possible to update the octree?

For example, provide some functions to add, remove or update points and recalculate the octree's childs.