/**
* @brief
* Creates view planes using vertices and a view position
*/
bool PlaneSet::CreateViewPlanes(const Array<Vector3> &lstVertices, const Vector3 &vViewPosition)
{
// Are there enough vertices provided?
const uint32 nNumOfVertices = lstVertices.GetNumOfElements();
if (nNumOfVertices >= 3) {
// Check whether there are enough planes
if (GetNumOfPlanes() < nNumOfVertices+1)
GetList().Resize(nNumOfVertices+1);
// Check whether there are not to much planes
while (GetNumOfPlanes() > nNumOfVertices+1)
RemovePlane(GetNumOfPlanes()-1);
// Near plane
m_lstPlane[0].ComputeND(lstVertices[0], lstVertices[1], lstVertices[2]);
// Side planes
for (uint32 i=0; i<nNumOfVertices; i++)
m_lstPlane[i+1].ComputeND(vViewPosition, lstVertices[i], lstVertices[(i+1) % nNumOfVertices]);
// Done
return true;
} else {
// Error!
return false;
}
}
/**
* @brief
* Creates 6 planes which enclose a box area
*/
void PlaneSet::CreateBox(const Vector3 &vMin, const Vector3 &vMax)
{
// Check whether there are enough planes
if (m_lstPlane.GetNumOfElements() < 6)
m_lstPlane.Resize(6);
// Check whether there are not to much planes
while (m_lstPlane.GetNumOfElements() > 6)
RemovePlane(m_lstPlane.GetNumOfElements()-1);
// Setup the planes
uint32 nNumOfPlanes = m_lstPlane.GetNumOfElements();
if (nNumOfPlanes > 0) {
// X
m_lstPlane[0].ComputeND(Vector3(vMin.x, 0.0f, 0.0f), Vector3::UnitX);
if (nNumOfPlanes > 1) {
m_lstPlane[1].ComputeND(Vector3(vMax.x, 0.0f, 0.0f), Vector3::NegativeUnitX);
// Y
if (nNumOfPlanes > 2) {
m_lstPlane[2].ComputeND(Vector3(0.0f, vMin.y, 0.0f), Vector3::UnitY);
if (nNumOfPlanes > 3) {
m_lstPlane[3].ComputeND(Vector3(0.0f, vMax.y, 0.0f), Vector3::NegativeUnitY);
// Z
if (nNumOfPlanes > 4) {
m_lstPlane[4].ComputeND(Vector3(0.0f, 0.0f, vMin.z), Vector3::UnitZ);
if (nNumOfPlanes > 5) {
m_lstPlane[5].ComputeND(Vector3(0.0f, 0.0f, vMax.z), Vector3::NegativeUnitZ);
}
}
}
}
}
}
}
std::vector<Box2DPoint> Cluster(const pcl::PointCloud<PointT>::Ptr input_cloud)
{
pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>);
cloud_filtered = RemovePlane (input_cloud);
//Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
tree->setInputCloud (cloud_filtered);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setMaxClusterSize (1000);//
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices);
// pcl::visualization::PCLVisualizer viewer ("cluster");
std::vector<Box2DPoint> my_points;
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<PointT>::Ptr cloud_cluster (new pcl::PointCloud<PointT>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
// viewer.addPointCloud (cloud_cluster, "cloud");
float x_min=999,y_min=999,z_min=999;
float x_max=0,y_max=0,z_max=0;
for(int i=0;i<cloud_cluster->size();i++)
{
if(cloud_cluster->points[i].x<x_min)
x_min=cloud_cluster->points[i].x;
if(cloud_cluster->points[i].y<y_min)
y_min=cloud_cluster->points[i].y;
if(cloud_cluster->points[i].z<z_min)
z_min=cloud_cluster->points[i].z;
if(cloud_cluster->points[i].x>x_max)
x_max=cloud_cluster->points[i].x;
if(cloud_cluster->points[i].y>y_max)
y_max=cloud_cluster->points[i].y;
if(cloud_cluster->points[i].z>z_max)
z_max=cloud_cluster->points[i].z;
}
// viewer.addCube(x_min, x_max, y_min, y_max, z_min, z_max, 0, 255, 0, "cloud");
Box2DPoint pp;
pp.x_left_down = x_min*FOCAL/z_min+320;
pp.y_left_down = y_min*FOCAL/z_min+240;
pp.x_right_up = x_max*FOCAL/z_max+320;
pp.y_right_up = y_max*FOCAL/z_max+240;
my_points.push_back(pp);
j++;
}
return my_points;
}
/**
* @brief
* Creates view planes using a projection and view matrix
*/
void PlaneSet::CreateViewPlanes(const Matrix4x4 &mViewProjection, bool bInfProj)
{
// Check whether there are enough planes
uint32 nPlanes = bInfProj ? 5 : 6;
if (m_lstPlane.GetNumOfElements() < nPlanes)
m_lstPlane.Resize(nPlanes);
// Check whether there are not to much planes
while (m_lstPlane.GetNumOfElements() > nPlanes)
RemovePlane(m_lstPlane.GetNumOfElements()-1);
// Setup new frustum
const float *pfClip = mViewProjection;
for (uint32 i=0; i<m_lstPlane.GetNumOfElements(); i++) {
Plane &cPlane = m_lstPlane[i];
switch (i) {
case VPNear: // Extract the NEAR clipping plane
cPlane.fN[0] = pfClip[ 3] + pfClip[ 2];
cPlane.fN[1] = pfClip[ 7] + pfClip[ 6];
cPlane.fN[2] = pfClip[11] + pfClip[10];
cPlane.fD = pfClip[15] + pfClip[14];
break;
case VPRight: // Extract the RIGHT clipping plane
cPlane.fN[0] = pfClip[ 3] - pfClip[ 0];
cPlane.fN[1] = pfClip[ 7] - pfClip[ 4];
cPlane.fN[2] = pfClip[11] - pfClip[ 8];
cPlane.fD = pfClip[15] - pfClip[12];
break;
case VPLeft: // Extract the LEFT clipping plane
cPlane.fN[0] = pfClip[ 3] + pfClip[ 0];
cPlane.fN[1] = pfClip[ 7] + pfClip[ 4];
cPlane.fN[2] = pfClip[11] + pfClip[ 8];
cPlane.fD = pfClip[15] + pfClip[12];
break;
case VPBottom: // Extract the BOTTOM clipping plane
cPlane.fN[0] = pfClip[ 3] + pfClip[ 1];
cPlane.fN[1] = pfClip[ 7] + pfClip[ 5];
cPlane.fN[2] = pfClip[11] + pfClip[ 9];
cPlane.fD = pfClip[15] + pfClip[13];
break;
case VPTop: // Extract the TOP clipping plane
cPlane.fN[0] = pfClip[ 3] - pfClip[ 1];
cPlane.fN[1] = pfClip[ 7] - pfClip[ 5];
cPlane.fN[2] = pfClip[11] - pfClip[ 9];
cPlane.fD = pfClip[15] - pfClip[13];
break;
case VPFar: // Extract the FAR clipping plane
cPlane.fN[0] = pfClip[ 3] - pfClip[ 2];
cPlane.fN[1] = pfClip[ 7] - pfClip[ 6];
cPlane.fN[2] = pfClip[11] - pfClip[10];
cPlane.fD = pfClip[15] - pfClip[14];
break;
}
// Avoid division through zero...
// Normalize it
float fU = cPlane.fN[0]*cPlane.fN[0] + cPlane.fN[1]*cPlane.fN[1] + cPlane.fN[2]*cPlane.fN[2];
if (fU) {
fU = Math::Sqrt(fU);
if (fU) {
// Scale
cPlane.fN[0] /= fU;
cPlane.fN[1] /= fU;
cPlane.fN[2] /= fU;
cPlane.fD /= fU;
}
}
}
}
