Separating Axis Theorem

main.cpp

#include <iostream>
#include <pcl/visualization/cloud_viewer.h>
#include "SeparatingAxisTheorem.hpp"

using namespace pcl;
using namespace pcl::visualization;
using namespace std;

typedef pcl::PointCloud<PointXYZ> Cloud;
typedef pcl::ConvexHull<PointXYZ>::Ptr HullPtr;
typedef Cloud::Ptr CloudPtr;
typedef Eigen::Vector2f EigenPt;


boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("Viewer"));
int testNum = 0;

static void keyboardCallback(const KeyboardEvent kb)
{
    switch (kb.getKeyCode())
    {
    case 0x00000053: // S
        viewer->saveScreenshot("test.png");
    case 0x00000020: // Space
        viewer->close();
        break;
    case 0x0000001b:
        exit(0);
        break;
    default:
        break;
    }
    ++testNum;
}


int main(int argc, char** argv)
{
    // Seed rand() so we get different "random" numbers between runs
    srand(time(NULL));
    int size = 10;

    viewer->setBackgroundColor (0, 0, 0);
    viewer->initCameraParameters ();
    viewer->setCameraPosition(0, 0, 5, 0, 0, 0, 0, 0, 0);
    viewer->registerKeyboardCallback(keyboardCallback);

    for (;;)
    {
        PointCloud<PointXYZ>::Ptr cloudA(new PointCloud<PointXYZ>);
        PointCloud<PointXYZ>::Ptr cloudB(new PointCloud<PointXYZ>);
        PointCloud<PointXYZ>::Ptr reconA(new PointCloud<PointXYZ>);
        PointCloud<PointXYZ>::Ptr reconB(new PointCloud<PointXYZ>);
        ConvexHull<PointXYZ>::Ptr hullA(new ConvexHull<PointXYZ>),
            hullB(new ConvexHull<PointXYZ>);
        
        cloudA->points.resize(size);
        cloudB->points.resize(size);

        // Values will fall in [(-1,-1), (1,1)] range +/- 0.65 on X in order
        // to produce a healthy number of non-colliding polygons for testing.
        cout << "Use these if you want to experiment with a particular configuration:\n\n";
        for (size_t i = 0; i<size; ++i)
        {
            cloudA->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f) - 0.65;
            cloudA->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
            cloudA->points[i].z = 1;

            cloudB->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f) + 0.65;
            cloudB->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
            cloudB->points[i].z = 1;

            cout << "cloudA->emplace_back( " << cloudA->points[i].x << ", "
                 << cloudA->points[i].y << ", 1);" << endl;
            cout << "cloudB->emplace_back( " << cloudB->points[i].x << ", "
                 << cloudB->points[i].y << ", 1);" << endl;
        }
        
        hullA->setInputCloud(cloudA);
        hullA->reconstruct(*reconA);
        hullB->setInputCloud(cloudB);
        hullB->reconstruct(*reconB);

        for (int i=0; i<reconA->points.size(); ++i)
        {
            int j = (i+1)%(reconA->points.size());
            viewer->addLine<PointXYZ>(reconA->points[i], reconA->points[j],
                                      0, 255, 0, "lineA" + to_string(i));
        }
        for (int i=0; i<reconB->points.size(); ++i)
        {
            int j = (i+1)%(reconB->points.size());
            viewer->addLine<PointXYZ>(reconB->points[i], reconB->points[j],
                                      255, 0, 0, "lineB" + to_string(i));
        }
        
        bool overlap = SeparatingAxisTheorem::overlap(hullA, hullB);
        string text = "Overlap: ";
        if (overlap) text += "YES";
        else text += "NO";
        cout << text << endl;

        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> green_color(cloudA, 0, 255, 0);
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> red_color(cloudB, 255, 0, 0);
        viewer->addPointCloud<PointXYZ> (cloudA, green_color, "cloudA");
        viewer->addPointCloud<PointXYZ> (cloudB, red_color, "cloudB");
        viewer->setPointCloudRenderingProperties (PCL_VISUALIZER_POINT_SIZE, 3, "cloudA");
        viewer->setPointCloudRenderingProperties (PCL_VISUALIZER_POINT_SIZE, 3, "cloudB");            
        viewer->addText(text, 400, 10, 30, 1, 1, 1);

        viewer->spin();
        viewer->resetStoppedFlag();
        viewer->removeAllPointClouds();
        viewer->removeAllShapes();
    }
    
    return 0;
}

SeparatingAxisTheorem.cpp

#include "SeparatingAxisTheorem.hpp"
#include <vector>

using namespace std;


void SeparatingAxisTheorem::getProjectionAxes(const vector<EigenPt>& verts,
                                              vector<EigenPt>& axes)
{
    for (int i=0; i<verts.size(); ++i) {
        int j = (i+1)%(verts.size());
        auto pt1 = verts[i];
        auto pt2 = verts[j];
        EigenPt edgeNormal(pt2[1] - pt1[1], -(pt2[0] - pt1[0]));
        axes.push_back(edgeNormal);
    }        
}

void SeparatingAxisTheorem::projectOntoAxis(const vector<EigenPt>& hullPts,
                                            const EigenPt axis,
                                            vector<EigenPt>& projPts)
{
    for (auto p : hullPts)
    {
        EigenPt pp = (p.dot(axis) / axis.dot(axis)) * axis;
        projPts.push_back(pp);
    }
}

    
// Check whether two sets of collinear points overlap each other
bool SeparatingAxisTheorem::pointsOverlap(vector<EigenPt>& ptsA, vector<EigenPt>& ptsB)
{
    // Find endpoints by sorting points
    auto sortFunc = [](const EigenPt a, const EigenPt b) {
        return a[0] == b[0] ? a[1] < b[1] : a[0] < b[0]; };
    sort(ptsA.begin(), ptsA.end(), sortFunc);
    sort(ptsB.begin(), ptsB.end(), sortFunc);
    
    // Determine overlap via: A--B, C--D, B >= C 0 && D >= A        
    return !sortFunc(ptsB[ptsB.size()-1], ptsA[0]) &&
           !sortFunc(ptsA[ptsA.size()-1], ptsB[0]);
}


// Check whether two hulls, projected onto a set of axes
// (lines formed by vectors), overlap each other
bool SeparatingAxisTheorem::projectionOverlap(const vector<EigenPt>& ptsA,
                                              const vector<EigenPt>& ptsB,
                                              const vector<EigenPt>& axes)
{    
    vector<EigenPt> projA, projB;
    
    for (auto axis : axes)
    {
        // Project onto axis
        projectOntoAxis(ptsA, axis, projA);
        projectOntoAxis(ptsB, axis, projB);
        
        // If no overlap, return false
        if (!pointsOverlap(projA, projB)) return false;
        
        projA.clear();
        projB.clear();
    }
    return true;
}


// Check whether two 2D convex hulls overlap each other
bool SeparatingAxisTheorem::overlap(HullPtr a, HullPtr b) {
    int dimension = a->getDimension();
    assert(b->getDimension() == dimension);
    assert(2 == dimension); // Currently only support 2D
    
    vector<EigenPt> axesA, axesB;
    vector<EigenPt> pointsA, pointsB;

    // Reconstruct with PointXYZ point cloud, regardless of how the hull was formed
    CloudPtr vertsA(new Cloud);
    CloudPtr vertsB(new Cloud);
    a->reconstruct(*vertsA);
    b->reconstruct(*vertsB);

    // if no vertices, cloud is empty
    assert(vertsA->points.size() > 0 && vertsB->points.size() > 0);

    // Convert to Eigen vector array
    // TODO how to acheive this????
//        Eigen::MatrixXf eMap = vertsA->getMatrixXfMap(dimension, 4, 0);

    // Get the points from cloud
    std::for_each(vertsA->points.begin(), vertsA->points.end(), [&pointsA](pcl::PointXYZ p) {
            pointsA.emplace_back(p.data);
        });
    std::for_each(vertsB->points.begin(), vertsB->points.end(), [&pointsB](pcl::PointXYZ p) {
            pointsB.emplace_back(p.data);
        });

    // Find the axes onto which we'll project the hulls
    getProjectionAxes(pointsA, axesA);
    getProjectionAxes(pointsB, axesB);
        
    return projectionOverlap(pointsA, pointsB, axesA) &&
        projectionOverlap(pointsA, pointsB, axesB);
}