iCub::data3D::MinimumBoundingBox Namespace Reference

The Definition of the MinimumBoundingBox. More...

Functions
iCub::data3D::BoundingBox	getMinimumBoundingBox (pcl::PointCloud< pcl::PointXYZRGB >::Ptr cloud)
	Given a point cloud (defined in the Point Cloud Library), it computes the minimum enclosing bounding box. More...

Detailed Description

The Definition of the MinimumBoundingBox.

Function Documentation

getMinimumBoundingBox()

BoundingBox iCub::data3D::MinimumBoundingBox::getMinimumBoundingBox

(

pcl::PointCloud< pcl::PointXYZRGB >::Ptr

cloud

)

Given a point cloud (defined in the Point Cloud Library), it computes the minimum enclosing bounding box.

Parameters

cloud

the input point cloud.

Returns

a BoundingBox object, which encloses the input point cloud.

Definition at line 212 of file minBoundBox.cpp.

References iCub::data3D::BoundingBox::getOrientation(), iCub::data3D::BoundingBox::setCorners(), and iCub::data3D::BoundingBox::setOrientation().

{
    BoundingBox bb;
    std::vector< pcl::Vertices > polygons;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::ConvexHull<pcl::PointXYZRGB> chull;
    chull.setInputCloud (cloud);
    chull.setDimension(3);
    chull.reconstruct (*cloud_hull,polygons);

    yarp::sig::Vector x1;
    yarp::sig::Vector x2;
    yarp::sig::Vector x3;
    yarp::sig::Vector y1;
    yarp::sig::Vector y2;
    yarp::sig::Vector y3;
    yarp::sig::Vector z1;
    yarp::sig::Vector z2;
    yarp::sig::Vector z3;

    for (int i=0; i<polygons.size(); i++)
    {
        pcl::Vertices vertex=polygons.at(i);
        x1.push_back(cloud_hull->at(vertex.vertices[0]).x);
        x2.push_back(cloud_hull->at(vertex.vertices[1]).x);
        x3.push_back(cloud_hull->at(vertex.vertices[2]).x);
        y1.push_back(cloud_hull->at(vertex.vertices[0]).y);
        y2.push_back(cloud_hull->at(vertex.vertices[1]).y);
        y3.push_back(cloud_hull->at(vertex.vertices[2]).y);
        z1.push_back(cloud_hull->at(vertex.vertices[0]).z);
        z2.push_back(cloud_hull->at(vertex.vertices[1]).z);
        z3.push_back(cloud_hull->at(vertex.vertices[2]).z);
    }

    Matrix pointCloud(cloud_hull->size(),3);
    for (int i=0; i<cloud_hull->size(); i++)
    {
        pointCloud(i,0)=cloud_hull->at(i).x;
        pointCloud(i,1)=cloud_hull->at(i).y;
        pointCloud(i,2)=cloud_hull->at(i).z;
    }

    yarp::sig::Vector v1x=x2-x1;
    yarp::sig::Vector v1y=y2-y1;
    yarp::sig::Vector v1z=z2-z1;
    std::vector<yarp::sig::Vector> edges1;

    retrieveEdges(v1x,v1y,v1z,edges1);

    yarp::sig::Vector v2x=x3-x1;
    yarp::sig::Vector v2y=y3-y1;
    yarp::sig::Vector v2z=z3-z1;
    std::vector<yarp::sig::Vector> edges2;

    retrieveEdges2(v2x,v2y,v2z,edges1,edges2);

    std::vector<yarp::sig::Vector> crossProduct;
    for (int i=0; i<edges1.size(); i++)
    {
        yarp::sig::Vector vect=cross(edges1.at(i),edges2.at(i));
        crossProduct.push_back(vect);
    }

    yarp::sig::Vector alpha;
    yarp::sig::Vector beta;
    yarp::sig::Vector gamma;
    eulerAngles(edges1, edges2, crossProduct, alpha, beta, gamma);

    Matrix minmax(2,3);
    double minVol=100000;
    Matrix rot2;

    for (int i=0; i<alpha.size(); i++)
    {
        Matrix rot;
        buildRotMat3(alpha[i],beta[i],gamma[i],rot);
        Matrix xyz_i=pointCloud*rot;
        findRotation(xyz_i,rot2);

        Matrix rot3dim=eye(3,3);
        rot3dim.setSubmatrix(rot2,0,0);
        Matrix rotation=rot*rot3dim;

        xyz_i=pointCloud*rotation;
        yarp::sig::Vector minimum;
        Helpers::min(xyz_i,minimum);
        yarp::sig::Vector maximum;
        Helpers::max(xyz_i,maximum);
        yarp::sig::Vector h=maximum-minimum;

        double prod=h[0]*h[1]*h[2];
        if (prod<minVol)
        {
            minVol=prod;
            bb.setOrientation(rotation);
            minmax.setRow(0,minimum);
            minmax.setRow(1,maximum);
        }
    }
    Matrix cornerpointsTmp(8,3);
    assignCorners(minmax,cornerpointsTmp);
    Matrix cornerpoints=cornerpointsTmp*(bb.getOrientation().transposed());

    std::vector<iCub::data3D::PointXYZ> corners;
    for (unsigned int i=0; i<cornerpoints.rows(); i++)
        corners.push_back(PointXYZ(cornerpoints(i,0),cornerpoints(i,1),cornerpoints(i,2)));

    bb.setCorners(corners);
    return bb;
}