TreeUtils.cpp

Go to the documentation of this file.

#include "lvr2/registration/TreeUtils.hpp"
#include "lvr2/registration/AABB.hpp"

#include <limits>
#include <vector>
using namespace std;
using namespace Eigen;

namespace lvr2
{

int splitPoints(Vector3f* points, int n, int axis, double splitValue)
{
    int l = 0, r = n - 1;

    while (l < r)
    {
        while (l < r && points[l](axis) < splitValue)
        {
            ++l;
        }
        while (r > l && points[r](axis) >= splitValue)
        {
            --r;
        }
        if (l < r)
        {
            std::swap(points[l], points[r]);
        }
    }

    return l;
}

void createOctree(Vector3f* points,
                  int n,
                  bool* flagged,
                  const Vector3f& min,
                  const Vector3f& max,
                  int level,
                  double voxelSize,
                  int maxLeafSize)
{
    if (n <= maxLeafSize)
    {
        return;
    }

    int axis = level % 3;
    Vector3f center = (max + min) / 2.0;

    if (max[axis] - min[axis] <= voxelSize)
    {
        // keep the Point closest to the center
        int closest = 0;
        double minDist = (points[closest] - center).squaredNorm();
        for (int i = 1; i < n; i++)
        {
            double dist = (points[i] - center).squaredNorm();
            if (dist < minDist)
            {
                closest = i;
                minDist = dist;
            }
        }
        // flag all other Points for deletion
        for (int i = 0; i < n; i++)
        {
            flagged[i] = i != closest;
        }
        return;
    }

    int l = splitPoints(points, n, axis, center[axis]);

    Vector3f lMin = min, lMax = max;
    Vector3f rMin = min, rMax = max;

    lMax[axis] = center[axis];
    rMin[axis] = center[axis];

    if (l > maxLeafSize)
    {
        #pragma omp task
        createOctree(points    , l    , flagged    , lMin, lMax, level + 1, voxelSize, maxLeafSize);
    }

    if (n - l > maxLeafSize)
    {
        #pragma omp task
        createOctree(points + l, n - l, flagged + l, rMin, rMax, level + 1, voxelSize, maxLeafSize);
    }
}

int octreeReduce(Vector3f* points, int n, double voxelSize, int maxLeafSize)
{
    bool* flagged = new bool[n];
    for (int i = 0; i < n; i++)
    {
        flagged[i] = false;
    }

    AABB<float> boundingBox(points, (size_t)n);

    #pragma omp parallel // allows "pragma omp task"
    #pragma omp single // only execute every task once
    createOctree(points, n, flagged, boundingBox.min(), boundingBox.max(), 0, voxelSize, maxLeafSize);

    // remove all flagged elements
    int i = 0;
    while (i < n)
    {
        if (flagged[i])
        {
            n--;
            if (i == n)
            {
                break;
            }
            points[i] = points[n];
            flagged[i] = flagged[n];
        }
        else
        {
            i++;
        }
    }

    delete[] flagged;

    return n;
}

} /* namespace lvr2 */