I have applied PCA on a set of points which represents an object. But the result is not perpendicular to the object surfaces(i.e the eigenvectors are not perpendicular to the surface of the objects). Because of which I am not to calculate real dimensions of the object.
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/passthrough.h>
#include <pcl/common/common.h>
#include <pcl/visualization/cloud_viewer.h>
#include <time.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/statistical_outlier_removal.h>
int _cloudID=0,_cloudNum=0, call_count=0;
static int id=0;
double infinity=std::numeric_limits<double>::infinity();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr _cloud_one (new pcl::PointCloud<pcl::PointXYZRGB>), _cloud_passthrough (new pcl::PointCloud<pcl::PointXYZRGB>),
_cloud_downsize (new pcl::PointCloud<pcl::PointXYZRGB>),_cloud_final(new pcl::PointCloud<pcl::PointXYZRGB>),
_cloud_segmented (new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<pcl::PointIndices> cluster_indices;
std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> cloud_vector;
clock_t tStart = clock();
pcl::visualization::PCLVisualizer::Ptr _viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
pcl::PointXYZRGB
centroidToPoints(Eigen::Vector4f& c){
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_eigen (new pcl::PointCloud<pcl::PointXYZRGB>);
cloud_eigen->width = 15;
cloud_eigen->height = 1;
cloud_eigen->points.resize (cloud_eigen->width * cloud_eigen->height);
cloud_eigen->points[0].x=c[0];
cloud_eigen->points[0].y=c[1];
cloud_eigen->points[0].z=c[2];
return cloud_eigen->points[0]; }
pcl::PointXYZRGB
eigenToPoints(Eigen::DenseBase<Eigen::Matrix<float, 3, 3> >::ColXpr ev, Eigen::Vector4f& c){
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_eigen (new pcl::PointCloud<pcl::PointXYZRGB>);
for(int i=0; i<3;i++)
{
ev[i]=c[i]+ev[i];
}
cloud_eigen->width = 15;
cloud_eigen->height = 1;
cloud_eigen->points.resize (cloud_eigen->width * cloud_eigen->height);
cloud_eigen->points[0].x=ev[0];
cloud_eigen->points[0].y=ev[1];
cloud_eigen->points[0].z=ev[2];
return cloud_eigen->points[0]; }
double angle(Eigen::Vector4f& main_axis, Eigen::Vector4f& eigen_axis)
{
}
void
updateVisualization(){
_viewer->removeAllPointClouds();
if (_cloudID==0){
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> cloud_handler (_cloud_one,255,255,0);
_viewer->addPointCloud<pcl::PointXYZRGB> (_cloud_one, cloud_handler, "input cloud");
_viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "input cloud");
}
else if (_cloudID==1){
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> cloud_handler (_cloud_passthrough);
_viewer->addPointCloud<pcl::PointXYZRGB> (_cloud_passthrough, cloud_handler, "passthrough cloud");
_viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "passthrough cloud");
}
else if (_cloudID==2){
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> cloud_handler (_cloud_downsize);
_viewer->addPointCloud<pcl::PointXYZRGB> (_cloud_downsize, cloud_handler, "downsized cloud");
_viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "downsized cloud");
}
else if (_cloudID==3){
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> cloud_handler (_cloud_segmented);
_viewer->addPointCloud<pcl::PointXYZRGB> (_cloud_segmented, cloud_handler, "segmented cloud");
_viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "segmented cloud");
}
else if (_cloudID==4){
for(int i=0;i<cloud_vector.size();i++){
std::stringstream name;
name<<"clustercloud"<<id;
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> cloud_handler (cloud_vector[i],std::rand()/5,std::rand()/10,0);
_viewer->addPointCloud<pcl::PointXYZRGB> (cloud_vector[i], cloud_handler, name.str());
_viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, name.str() );
++id;
}
}
else if (_cloudID==5){
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> cloud_handler (_cloud_final);
_viewer->addPointCloud<pcl::PointXYZRGB> (_cloud_final, cloud_handler, "final cloud");
_viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "final cloud");
}
}
int eigenAssigner(std::vector<double> diff)
{
std::vector<double>::iterator result = std::min_element(diff.begin(), diff.end());
int pos=std::distance(diff.begin(), result);
return pos;
}
double magnitude(Eigen::Vector3f axis){
return sqrt(axis[0]*axis[0]+axis[1]*axis[1]+axis[2]*axis[2]);
}
double angle(Eigen::Vector3f main_axis, Eigen::Vector3f eigen_axis){
return (acos(main_axis.dot(eigen_axis)/(magnitude(main_axis)*magnitude(eigen_axis)))*180)/M_PI;
}
void calculateWidth(pcl::PointCloud<pcl::PointXYZRGB>::Ptr _cloud_changed, Eigen::Matrix3f& eigen_vectors){
Eigen::Vector3f ev;
std::vector<double> zdiff,xdiff,ydiff;
double maxdist[3]={0,0,0};
double mindist[3]={0,0,0};
for(int i=0; i<3; i++)
{
ev<<eigen_vectors.col(i);
for(int j=0; j<_cloud_changed->points.size(); j++){
double d=Eigen::Vector3f(_cloud_changed->points[j].getArray3fMap()).dot(ev);
maxdist[i]=std::max(d,maxdist[i]);
mindist[i]=std::min(d,mindist[i]);
}
std::cout<< "max and min" << i << ":"<<maxdist[i]<<" "<<mindist[i]<<std::endl;
}
for(int j=0; j<3;j++){
ev<<eigen_vectors.col(j);
Eigen::Vector3f z,x,y;
z<<0,0,1;
x<<1,0,0;
y<<0,1,0;
if(angle(x,ev) < 140)
{
xdiff.push_back(angle(x,ev));
}
else
{
xdiff.push_back(180-angle(x,ev));
}
if(angle(z,ev) < 140)
{
zdiff.push_back(angle(z,ev));
}
else
{
zdiff.push_back(180-angle(z,ev));
}
if(angle(y,ev) < 140)
{
ydiff.push_back(angle(y,ev));
}
else
{
ydiff.push_back(180-angle(y,ev));
}
std::cout<<"angle btw x and eigenvector "<<j+1<<":"<<xdiff[j]<<std::endl;
std::cout<<"angle btw y and eigenvector"<<j+1<<":"<<ydiff[j]<<std::endl;
std::cout<<"angle btw z and eigenvector"<<j+1<<":"<<zdiff[j]<<std::endl;
}
double width = maxdist[eigenAssigner(xdiff)]-mindist[eigenAssigner(xdiff)];
double length = maxdist[eigenAssigner(ydiff)]-mindist[eigenAssigner(ydiff)];
double height = maxdist[eigenAssigner(zdiff)]-mindist[eigenAssigner(zdiff)];
std::cout<<width<<":"<<height<<":"<<length;
}
void changeCoordinate(pcl::PointCloud<pcl::PointXYZRGB>::Ptr _cloud_indv, Eigen::Matrix3f& eigen_vectors, Eigen::Vector4f& centroid)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr _cloud_changed(new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Vector3f centroid_point;
_cloud_changed->width = _cloud_indv->points.size();
_cloud_changed->height = 1;
_cloud_changed->points.resize(_cloud_changed->width*_cloud_changed->height);
for(int i=0; i<_cloud_indv->points.size(); i++)
{
_cloud_changed->points[i].x= _cloud_indv->points[i].x-centroid[0];
_cloud_changed->points[i].y= _cloud_indv->points[i].y-centroid[1];
_cloud_changed->points[i].z= _cloud_indv->points[i].z-centroid[2];
}
calculateWidth(_cloud_changed, eigen_vectors);
}
void calculateEigen(pcl::PointCloud<pcl::PointXYZRGB>::Ptr _cloud_indv){
Eigen::Vector4f centroid;
Eigen::Matrix3f covariance_matrix;
Eigen::Vector3f eigen_values;
Eigen::Matrix3f eigen_vectors;
pcl::compute3DCentroid(*_cloud_indv,centroid);
std::cout << "centroid:"<<centroid<<std::endl;
pcl::computeCovarianceMatrix (*_cloud_indv, centroid, covariance_matrix);
pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
std::cout << "eigenvalues:"<<std::endl<<eigen_values<<std::endl;
std::cout << "eigenvector:"<<std::endl<<eigen_vectors<<std::endl;
for(int i=0; i<3;i++){
std::stringstream line;
line<<"line"<<id;
_viewer->addLine<pcl::PointXYZRGB> (centroidToPoints(centroid), eigenToPoints(eigen_vectors.col(i),centroid), line.str());
id++;
}
changeCoordinate(_cloud_downsize ,eigen_vectors,centroid);
}
void extractCloudCluster()
{
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr _cloud_cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
_cloud_cluster->points.push_back (_cloud_segmented->points[*pit]); _cloud_cluster->width = _cloud_cluster->points.size ();
_cloud_cluster->height = 1;
_cloud_cluster->is_dense = true;
cloud_vector.push_back(_cloud_cluster);
*_cloud_final=*_cloud_final+*_cloud_cluster;
calculateEigen(_cloud_cluster);
}
_cloudNum++;
}
void keyboardEventOccurred (const pcl::visualization::KeyboardEvent &event, void * object_void){
if (event.getKeySym () == "n" && event.keyDown ())
{
_cloudID++;
if (_cloudID > _cloudNum) _cloudID = 0;
std::cout << "Current cloud: " << _cloudID << std::endl;
}
updateVisualization();
return;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr rectangular(const int& pixel_row,const int& pixel_column,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
const int& width,const int& height)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rect (new pcl::PointCloud<pcl::PointXYZRGB>);
for (int i=0; i<height; i++)
for(int j=0; j<width; j++)
{
cloud_rect->points.push_back(cloud->points[(cloud->width*((pixel_row+i)-1))+((pixel_column+j)-1)]);
}
cloud_rect->width = cloud_rect->points.size ();
cloud_rect->height = 1;
return cloud_rect;
}
int
main (int argc, char** argv)
{
double theta=M_PI_2;
pcl::PCDReader reader;
reader.read ("../pcds/obj3.pcd", *_cloud_one);
std::cout << "PointCloud before filtering has: " << _cloud_one->points.size () << " data points." << std::endl;
Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
transform_2.translation() << 0.0, 0.0, 0.0;
transform_2.rotate (Eigen::AngleAxisf (-theta, Eigen::Vector3f::UnitZ()) * Eigen::AngleAxisf (-theta, Eigen::Vector3f::UnitX()));
pcl::PointCloud<pcl::PointXYZRGB>::Ptr _transformed_cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
pcl::transformPointCloud (*_cloud_one, *_transformed_cloud, transform_2);
pcl::PassThrough<pcl::PointXYZRGB> pass;
pass.setInputCloud (_transformed_cloud);
pass.setFilterFieldName ("x");
pass.setFilterLimits (0.0, 2.0);
pass.filter (*_cloud_passthrough);
_cloudNum++;
pcl::VoxelGrid<pcl::PointXYZRGB> vg;
vg.setInputCloud (_cloud_passthrough);
vg.setLeafSize (0.001f, 0.001f, 0.001f);
vg.filter (*_cloud_downsize);
std::cout << "PointCloud after filtering has: " << _cloud_downsize->points.size () << " data points." << std::endl; _cloudNum++;
*_cloud_segmented = *_cloud_downsize;
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr _cloud_plane (new pcl::PointCloud<pcl::PointXYZRGB> ());
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.01);
int i=0, nr_points = (int) _cloud_segmented->points.size ();
while (_cloud_segmented->points.size () > 0.3* nr_points)
{
seg.setInputCloud (_cloud_segmented);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud (_cloud_segmented);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*_cloud_plane);
std::cout << "PointCloud representing the planar component: " << _cloud_plane->points.size () << " data points." << std::endl;
extract.setNegative (true);
extract.filter (*_cloud_segmented);
}
_cloudNum++;
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud (_cloud_segmented);
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
ec.setClusterTolerance (0.01); ec.setMinClusterSize (300);
ec.setMaxClusterSize (9000);
ec.setSearchMethod (tree);
ec.setInputCloud (_cloud_segmented);
ec.extract (cluster_indices);
extractCloudCluster();
_cloudNum++;
std::cout<<"Time taken:"<< (clock() - tStart)/(double)CLOCKS_PER_SEC<<std::endl;
_viewer->registerKeyboardCallback (keyboardEventOccurred, (void*)&_viewer);
_viewer->addCoordinateSystem();
updateVisualization();
while (!_viewer->wasStopped ())
{
_viewer->spin();
}
return (0);
}