// according to http://www.pcl-users.org/Finding-oriented-bounding-box-of-a-cloud-td4024616.html
FitCube PCLTools::fitBox(PointCloud<PointXYZ>::Ptr cloud) {
FitCube retCube;
PCA<PointXYZ> pca;
PointCloud<PointXYZ> proj;
pca.setInputCloud(cloud);
pca.project(*cloud, proj);
PointXYZ proj_min;
PointXYZ proj_max;
getMinMax3D(proj, proj_min, proj_max);
PointXYZ min;
PointXYZ max;
pca.reconstruct(proj_min, min);
pca.reconstruct(proj_max, max);
// Rotation of PCA
Eigen::Matrix3f rot_mat = pca.getEigenVectors();
// Translation of PCA
Eigen::Vector3f cl_translation = pca.getMean().head(3);
Eigen::Matrix3f affine_trans;
// Reordering of principal components
affine_trans.col(2) << (rot_mat.col(0).cross(rot_mat.col(1))).normalized();
affine_trans.col(0) << rot_mat.col(0);
affine_trans.col(1) << rot_mat.col(1);
retCube.rotation = Eigen::Quaternionf(affine_trans);
Eigen::Vector4f t = pca.getMean();
retCube.translation = Eigen::Vector3f(t.x(), t.y(), t.z());
retCube.width = fabs(proj_max.x - proj_min.x);
retCube.height = fabs(proj_max.y - proj_min.y);
retCube.depth = fabs(proj_max.z - proj_min.z);
return retCube;
}
