Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Separating Axis Theorem
#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;
}
#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);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.