Click here to Skip to main content
15,885,366 members
Search within:


principal components are not perpendicular to the object surfaces.
Please Sign up or sign in to vote.
0.00/5 (No votes)
See more:
C++
3D
Kinect
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.
C++
#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>

 // Intialization
  int _cloudID=0,_cloudNum=0, call_count=0;
  static int id=0;
  double infinity=std::numeric_limits<double>::infinity();
 //cloud declaration
  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>);
   //cluster indices
   std::vector<pcl::PointIndices> cluster_indices;
   std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> cloud_vector;                                    
  //clock start                                     
  clock_t tStart = clock();   
 
  //visualization
  pcl::visualization::PCLVisualizer::Ptr _viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  //~ pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
  
  //Inserting centroid to cloud point
  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]; //returning the point.
 }
 
 //Inserting eigenvectors in to the cloud.
 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];  //returning the point.
 }
 
 
 double angle(Eigen::Vector4f& main_axis, Eigen::Vector4f& eigen_axis)
 {
 }
 
 //Visualization updater
 void
 updateVisualization(){
	_viewer->removeAllPointClouds();
	if (_cloudID==0){
		// add input cloud
		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){
		// add passthrough cloud
		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){
		// add downsized cloud
		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){
	 	// add segmented cloud
		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);
	}
	
	
 
 

  //Finding centroid and eigenvectors of the cluster indices
 void calculateEigen(pcl::PointCloud<pcl::PointXYZRGB>::Ptr _cloud_indv){
	 
	    Eigen::Vector4f centroid;
        Eigen::Matrix3f covariance_matrix;
    
        // Extract the eigenvalues and eigenvectors
        Eigen::Vector3f eigen_values;
        Eigen::Matrix3f eigen_vectors;
         //~ _cloud_downsize->width =3000;	
	    //~ _cloud_downsize->height = 1;
	    //~ 
	    //~ _cloud_downsize->points.resize(_cloud_downsize->width*_cloud_downsize->height);
	  //~ 
      //~ for(float i =0; i<_cloud_downsize->points.size();i++) {
			 //~ _cloud_downsize->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
			//~ _cloud_downsize->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
			//~ _cloud_downsize->points[i].x = 1.00000000;
					//~ 
	   //~ }
      //~ for (size_t i = 0; i < _cloud_downsize->points.size (); ++i)
     //~ std::cerr << "    " << _cloud_downsize->points[i].x << " "
                        //~ << _cloud_downsize->points[i].y << " "
                        //~ << _cloud_downsize->points[i].z << std::endl;
      //~ std::cerr << "Point cloud data: " << _cloud_downsize->points.size () << " points" << std::endl;
 
        //Compute centroid
        pcl::compute3DCentroid(*_cloud_indv,centroid);
        std::cout << "centroid:"<<centroid<<std::endl;
        
        // Compute the 3x3 covariance matrix 
        pcl::computeCovarianceMatrix (*_cloud_indv, centroid, covariance_matrix);
        
        //Eigenvectors
        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;
    
        
		 //Draw line from centroid to eigenvector
		 
        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);
			//Add to final cloud
            *_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 ())
	{
		//change cloud
		_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;                                      
                                        
  // Read in the cloud data
  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();

  // Define a translation of 2.5 meters on the x axis.
  transform_2.translation() << 0.0, 0.0, 0.0;     

  // The same rotation matrix as before; tetha radians arround Z axis
  transform_2.rotate (Eigen::AngleAxisf (-theta, Eigen::Vector3f::UnitZ()) * Eigen::AngleAxisf (-theta, Eigen::Vector3f::UnitX()));


  // Executing the transformation
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr _transformed_cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
  // You can either apply transform_1 or transform_2; they are the same
  pcl::transformPointCloud (*_cloud_one, *_transformed_cloud, transform_2);
  
  //Passthrough filter
  pcl::PassThrough<pcl::PointXYZRGB> pass;
  pass.setInputCloud (_transformed_cloud);
  pass.setFilterFieldName ("x");
  pass.setFilterLimits (0.0, 2.0);
  //pass.setFilterLimitsNegative (true);
  pass.filter (*_cloud_passthrough);
  _cloudNum++;
  
  // Create the filtering object: downsample the dataset using a leaf size of 1cm
  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;
  
  
  // Create the segmentation object for the planar model and set all the parameters
  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)
  {
        // Segment the largest planar component from the remaining cloud
		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;
		}
	   
		// Extract the planar inliers from the input cloud
		pcl::ExtractIndices<pcl::PointXYZRGB> extract;
		extract.setInputCloud (_cloud_segmented);
		extract.setIndices (inliers);
		extract.setNegative (false);

		// Get the points associated with the planar surface
		extract.filter (*_cloud_plane);
		std::cout << "PointCloud representing the planar component: " << _cloud_plane->points.size () << " data points." << std::endl;
        
		// Remove the planar inliers, extract the rest
		extract.setNegative (true);
		extract.filter (*_cloud_segmented);	
  }
   _cloudNum++;
   
   // Create the filtering object
  //~ pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
  //~ sor.setInputCloud (_cloud_segmented);
  //~ sor.setMeanK (50);
  //~ sor.setStddevMulThresh (1.0);
  //~ sor.filter (*_cloud_segmented);
   
  // Creating the KdTree object for the search method of the extraction
  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
  tree->setInputCloud (_cloud_segmented);
  
  
  //cluster extraction
  pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
  ec.setClusterTolerance (0.01); // 2cm
  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);
}

Posted
Add a Solution
Comments
Sergey Alexandrovich Kryukov 21-Mar-15 17:43pm    
Not clear. What do you mean by "dimension of object"? Which concept of "dimension" do you mean, there are several different once. How having something not perpendicular the object's surface can affect obtaining the dimension?... and so on... You need to explain the mathematics part properly first, and then programming, if you have to...
—SA

This content, along with any associated source code and files, is licensed under The Code Project Open License (CPOL)

  Print Answers RSS
Top Experts
Last 24hrsThis month

Advertise
Privacy
Cookies
Terms of Use
Last Updated 21 Mar 2015
Layout: fixed | fluid
Copyright © CodeProject, 1999-2024
All Rights Reserved.

Web02 2.8:2024-04-02:1

CodeProject, 20 Bay Street, 11th Floor Toronto, Ontario, Canada M5J 2N8 +1 (416) 849-8900