Пример #1
1
visualization_msgs::Marker SatDetector3DMonitor::getMarker(int marker_id, Eigen::Vector4f centroid, Eigen::Matrix3f covariance_matrix){

  //------- Compute Principal Componets for Marker publishing


  //Get the principal components and the quaternion
  Eigen::Matrix3f evecs;
  Eigen::Vector3f evals;
  //pcl::eigen33 (covariance_matrix, evecs, evals);
  Eigen::EigenSolver<Eigen::Matrix3f> es(covariance_matrix);
	
  evecs = es.eigenvectors().real();
  evals = es.eigenvalues().real();
	    
  Eigen::Matrix3f rotation;
  rotation.row (0) = evecs.col (0);
  rotation.row (1) = evecs.col (1);
  rotation.row (2) = rotation.row (0).cross (rotation.row (1));
	    
  rotation.transposeInPlace ();
  Eigen::Quaternion<float> qt (rotation);
  qt.normalize ();
	    
  //Publish Marker for cluster
  visualization_msgs::Marker marker;	
		
  marker.header.frame_id = base_frame_;
  marker.header.stamp = ros::Time().now();
  marker.ns = "Perception";
  marker.action = visualization_msgs::Marker::ADD;
  marker.id = marker_id;	
  marker.lifetime = ros::Duration(1);	
		
  //centroid position
  marker.type = visualization_msgs::Marker::SPHERE;
  marker.pose.position.x = centroid[0];
  marker.pose.position.y = centroid[1];
  marker.pose.position.z = centroid[2];	
  marker.pose.orientation.x = qt.x();
  marker.pose.orientation.y = qt.y();
  marker.pose.orientation.z = qt.z();
  marker.pose.orientation.w = qt.w();			

  //std::cout << "e1: " << evals(0) << " e2: " << evals(1) << " e3: " << evals(2) << std::endl;

  marker.scale.x = sqrt(evals(0)) * 4;
  marker.scale.y = sqrt(evals(1)) * 4;
  marker.scale.z = sqrt(evals(2)) * 4;
	

  //give it some color!
  marker.color.a = 0.75;
  satToRGB(marker.color.r, marker.color.g, marker.color.b);

  //std::cout << "marker being published" << std::endl;

  return marker;
}
Пример #2
0
void fuzzyAffines()
{
    std::vector<Eigen::Matrix4f> trans;
    trans.reserve(count/10);
    for( size_t i=0; i<count/10; i++ )
    {
        Eigen::Vector3f x = Eigen::Vector3f::Random();
        Eigen::Vector3f y = Eigen::Vector3f::Random();

        x.normalize();
        y.normalize();

        Eigen::Vector3f z = x.cross(y);
        z.normalize();

        y = z.cross(x);
        y.normalize();

        Eigen::Affine3f t = Eigen::Affine3f::Identity();
        Eigen::Matrix3f r = Eigen::Matrix3f::Identity();

        r.col(0) = x;
        r.col(1) = y;
        r.col(2) = z;

        t.rotate(r);
        t.translate( 0.5f * Eigen::Vector3f::Random() + Eigen::Vector3f(0.5,0.5,0.5) );

        trans.push_back( t.matrix() );
    }

    s_plot.setColor( Eigen::Vector4f(1,0,0,1) );
    s_plot.setLineWidth( 3.0 );
    s_plot( trans, nox::plot<float>::Pos | nox::plot<float>::CS );
}
Пример #3
0
void SQ_fitter<PointT>::getBoundingBox(const PointCloudPtr &_cloud,
				       double _dim[3],
				       double _trans[3],
				       double _rot[3] ) {

  // 1. Compute the bounding box center
  Eigen::Vector4d centroid;
  pcl::compute3DCentroid( *_cloud, centroid );
  _trans[0] = centroid(0);
  _trans[1] = centroid(1); 
  _trans[2] = centroid(2);

  // 2. Compute main axis orientations
  pcl::PCA<PointT> pca;
  pca.setInputCloud( _cloud );
  Eigen::Vector3f eigVal = pca.getEigenValues();
  Eigen::Matrix3f eigVec = pca.getEigenVectors();
  // Make sure 3 vectors are normal w.r.t. each other
  Eigen::Vector3f temp;
  eigVec.col(2) = eigVec.col(0); // Z
  Eigen::Vector3f v3 = (eigVec.col(1)).cross( eigVec.col(2) );
  eigVec.col(0) = v3;
  Eigen::Vector3f rpy = eigVec.eulerAngles(2,1,0);
 
  _rot[0] = (double)rpy(2);
  _rot[1] = (double)rpy(1);
  _rot[2] = (double)rpy(0);

  // Transform _cloud
  Eigen::Matrix4f transf = Eigen::Matrix4f::Identity();
  transf.block(0,3,3,1) << (float)centroid(0), (float)centroid(1), (float)centroid(2);
  transf.block(0,0,3,3) = eigVec;

  Eigen::Matrix4f tinv; tinv = transf.inverse();
  PointCloudPtr cloud_temp( new pcl::PointCloud<PointT>() );
  pcl::transformPointCloud( *_cloud, *cloud_temp, tinv );

  // Get maximum and minimum
  PointT minPt; PointT maxPt;
  pcl::getMinMax3D( *cloud_temp, minPt, maxPt );
  
  _dim[0] = ( maxPt.x - minPt.x ) / 2.0;
  _dim[1] = ( maxPt.y - minPt.y ) / 2.0;
  _dim[2] = ( maxPt.z - minPt.z ) / 2.0;

}
Пример #4
0
void
drawBoundingBox(PointCloudT::Ptr cloud,
				boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer,
				int z)
{
	
	//Eigen::Vector4f centroid;
	pcl::compute3DCentroid(*cloud, centroid);
	
	//Eigen::Matrix3f covariance;
	computeCovarianceMatrixNormalized(*cloud, centroid, covariance);
	//Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance,
	//Eigen::ComputeEigenvectors);

	eigen_solver.compute(covariance,Eigen::ComputeEigenvectors);
	
//	eigen_solver = boost::shared_ptr<Eigen::SelfAdjointEigenSolver>
//		(covariance,Eigen::ComputeEigenvectors);

	eigDx = eigen_solver.eigenvectors();
    eigDx.col(2) = eigDx.col(0).cross(eigDx.col(1));


	//Eigen::Matrix4f p2w(Eigen::Matrix4f::Identity());
	p2w.block<3,3>(0,0) = eigDx.transpose();
	p2w.block<3,1>(0,3) = -1.f * (p2w.block<3,3>(0,0) * centroid.head<3>());
    //pcl::PointCloud<PointT> cPoints;
    pcl::transformPointCloud(*cloud, cPoints, p2w);


	//PointT min_pt, max_pt;
    pcl::getMinMax3D(cPoints, min_pt, max_pt);
    const Eigen::Vector3f mean_diag = 0.5f*(max_pt.getVector3fMap() + min_pt.getVector3fMap());

	const Eigen::Quaternionf qfinal(eigDx);
    const Eigen::Vector3f tfinal = eigDx*mean_diag + centroid.head<3>();
	
	//viewer->addPointCloud(cloud);
	viewer->addCube(tfinal, qfinal, max_pt.x - min_pt.x, max_pt.y - min_pt.y, max_pt.z - min_pt.z,boost::lexical_cast<std::string>((z+1)*200));

}
Пример #5
0
bool Utils::
factorViewMatrix(const Eigen::Projective3f& iMatrix,
                 Eigen::Matrix3f& oCalib, Eigen::Isometry3f& oPose,
                 bool& oIsOrthographic) {
  oIsOrthographic = isOrthographic(iMatrix.matrix());

  // get appropriate rows
  std::vector<int> rows = {0,1,2};
  if (!oIsOrthographic) rows[2] = 3;

  // get A matrix (upper left 3x3) and t vector
  Eigen::Matrix3f A;
  Eigen::Vector3f t;
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      A(i,j) = iMatrix(rows[i],j);
    }
    t[i] = iMatrix(rows[i],3);
  }

  // determine translation vector
  oPose.setIdentity();
  oPose.translation() = -(A.inverse()*t);

  // determine calibration matrix
  Eigen::Matrix3f AAtrans = A*A.transpose();
  AAtrans.col(0).swap(AAtrans.col(2));
  AAtrans.row(0).swap(AAtrans.row(2));
  Eigen::LLT<Eigen::Matrix3f, Eigen::Upper> llt(AAtrans);
  oCalib = llt.matrixU();
  oCalib.col(0).swap(oCalib.col(2));
  oCalib.row(0).swap(oCalib.row(2));
  oCalib.transposeInPlace();

  // compute rotation matrix
  oPose.linear() = (oCalib.inverse()*A).transpose();

  return true;
}
bool PositionBasedElasticRod::ComputeDarbouxVector(const Eigen::Matrix3f& dA, const Eigen::Matrix3f& dB, const float mid_edge_length, Eigen::Vector3f& darboux_vector)
{

	float factor = 1.0f + dA.col(0).dot(dB.col(0)) + dA.col(1).dot(dB.col(1)) + dA.col(2).dot(dB.col(2));

	factor = 2.0f / (mid_edge_length * factor);

	for (int c = 0; c < 3; ++c)
	{
		const int i = permutation[c][0];
		const int j = permutation[c][1];
		const int k = permutation[c][2];

		darboux_vector[i] = dA.col(j).dot(dB.col(k)) - dA.col(k).dot(dB.col(j));
	}

	darboux_vector *= factor;
	return true;
}
Пример #7
0
bool ObjectFinder::find_toy_block(float surface_height, geometry_msgs::PoseStamped &object_pose) {
    Eigen::Vector3f plane_normal;
    double plane_dist;
    //bool valid_plane;
    Eigen::Vector3f major_axis;
    Eigen::Vector3f centroid;
    bool found_object = true; //should verify this
    double block_height = 0.035; //this height is specific to the TOY_BLOCK model
    //if insufficient points in plane, find_plane_fit returns "false"
    //should do more sanity testing on found_object status
    //hard-coded search bounds based on a block of width 0.035
    found_object = pclUtils_.find_plane_fit(0.4, 1, -0.5, 0.5, surface_height + 0.025, surface_height + 0.045, 0.001,
            plane_normal, plane_dist, major_axis, centroid);
    //should have put a return value on find_plane_fit;
    //
    if (plane_normal(2) < 0) plane_normal(2) *= -1.0; //in world frame, normal must point UP
    Eigen::Matrix3f R;
    Eigen::Vector3f y_vec;
    R.col(0) = major_axis;
    R.col(2) = plane_normal;
    R.col(1) = plane_normal.cross(major_axis);
    Eigen::Quaternionf quat(R);
    object_pose.header.frame_id = "base_link";
    object_pose.pose.position.x = centroid(0);
    object_pose.pose.position.y = centroid(1);
    //the TOY_BLOCK model has its origin in the middle of the block, not the top surface
    //so lower the block model origin by half the block height from upper surface
    object_pose.pose.position.z = centroid(2)-0.5*block_height;
    //create R from normal and major axis, then convert R to quaternion

    object_pose.pose.orientation.x = quat.x();
    object_pose.pose.orientation.y = quat.y();
    object_pose.pose.orientation.z = quat.z();
    object_pose.pose.orientation.w = quat.w();
    return found_object;
}
// ----------------------------------------------------------------------------------------------
bool PositionBasedElasticRod::ComputeMaterialFrame(
	const Eigen::Vector3f& pA, 
	const Eigen::Vector3f& pB, 
	const Eigen::Vector3f& pG, 
	Eigen::Matrix3f& frame)
{

	frame.col(2) = (pB - pA);
	frame.col(2).normalize();

	frame.col(1) = (frame.col(2).cross(pG - pA));
	frame.col(1).normalize();

	frame.col(0) = frame.col(1).cross(frame.col(2));
//	frame.col(0).normalize();
	return true;
}
Пример #9
0
int main(int argc, char** argv) {

    ros::init(argc, argv, "eigen_test");
    ros::NodeHandle nh_jntPub; // node handle for joint command publisher

 //   ros::Publisher pub_joint_commands; //
//    pub_joint_commands = nh_jntPub.advertise<atlas_msgs::AtlasCommand>("/atlas/atlas_command", 1, true); 
    
    ROS_INFO("test eigen program");
    
    Eigen::Matrix3f A;
    Eigen::Vector3f b;
    A << 1,2,3, 4,5,6, 7,8,10;
    
    A(1,2)=0; // how to access one element of matrix; start from 0; no warning out of range...
    
 
    b << 3,3,4;
    std::cout <<"b = "<<b <<std::endl;   
    
    // column operaton: replace first column of A with vector b:
    A.col(0)= b;  // could copy columns of matrices w/ A.col(0) = B.col(0);
    
    std::cout <<"A = "<<A <<std::endl;

    Eigen::MatrixXd mat1 = Eigen::MatrixXd::Zero(6, 6); //6x6 matrix full of zeros
    Eigen::MatrixXd mat2 = Eigen::MatrixXd::Identity(6, 6); //6x6 identity matrix  

    std::cout<<mat1<<std::endl;
    std::cout<<mat2<<std::endl;

    Eigen::Vector3f xtest = A.colPivHouseholderQr().solve(b);
    std::cout<<"soln xtest = "<<xtest<<std::endl;
    
    Eigen::Vector3f x = A.partialPivLu().solve(b); //dec.solve(b); //A.colPivHouseholderQr().solve(b);
    std::cout<<"soln x = "<<x<<std::endl;
    
    Eigen::Vector3f btest = A*x;
    std::cout<<"test soln: A*x = " <<btest<<std::endl;
    
    //extend to 6x6 test: v = M*z, find z using 2 methods
    // use double-precision matrices/vectors
    Eigen::MatrixXd M = Eigen::MatrixXd::Random(6,6);

    std::cout<<"test 6x6: M = "<<M<<std::endl;
    Eigen::VectorXd v(6);   
    v << 1,2,3,4,5,6;
    std::cout<<"v = "<<v<<std::endl;
    Eigen::VectorXd z(6); 
    Eigen::VectorXd ztest(6);   
    ztest = M.colPivHouseholderQr().solve(v);
    std::cout<<"soln ztest = "<<ztest<<std::endl;
    z = M.partialPivLu().solve(v);   
    std::cout<<"soln 6x6: z = "<<z<<std::endl;
    Eigen::VectorXd vtest(6);
    vtest = M*z;
    std::cout<<"test soln: M*z = "<<vtest<<std::endl;

    // .norm() operator...
    double relative_error = (M*z - v).norm() / v.norm(); // norm() is L2 norm
    std::cout << "The relative error is:\n" << relative_error << std::endl;

    
    std::cout<<"dot prod, v, z: "<< v.dot(z)<<std::endl;
    std::cout<<"cross prod, b-cross-x: " << b.cross(x)<<std::endl;
    
    
    
    return 0;
}
Пример #10
0
int main(int argc, char** argv)
{

// Punktwolke laden
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_unfiltered (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile <pcl::PointXYZ> ("Arena_24_7_2014.pcd",*cloud_unfiltered) == -1)
{
std::cout<<"Cloud reading failed"<<std::endl;
return (-1);
}

pcl::PCDWriter writer;

//Filter für Höhen- und Tiefenbegrenzung:
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(cloud_unfiltered);
pass.setFilterFieldName("z");
pass.setFilterLimits(-0.2,0.3);//Für Stativ ca. (-0.9,-0.2)
pass.filter(*cloud);

// Berechnen der Punktnormalen
pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> > (new pcl::search::KdTree<pcl::PointXYZ>);
pcl::PointCloud <pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
normal_estimator.setSearchMethod(tree);
normal_estimator.setInputCloud(cloud);
normal_estimator.setKSearch(100);// Ursprünglich:50
// http://pointclouds.org/documentation/tutorials/normal_estimation.php
normal_estimator.compute(*normals);

// Segmentierung mit dem RegionGrowing-Algorithmus (Setzen der Parameter)
pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
reg.setMinClusterSize (200);
//reg.setMaxClusterSize (10000);
reg.setSearchMethod (tree);
reg.setNumberOfNeighbours (50); // Ursprünglich 50 (Bei ca. 150 werden die Kanten schön glatt!)
// aber es wird nach zu vielen Nachbarn gecheckt-> Mehrere Banden fallen in ein Cluster
reg.setInputCloud (cloud);
//reg.setIndices (indices);
reg.setInputNormals (normals);
reg.setSmoothnessThreshold (4.0 / 180.0 * M_PI); // Ursprünglich: 7.0/180*M_PI
// je kleiner desto weniger punkte sind "smooth" genug ->Klares Abgrenzen der Cluster!
reg.setCurvatureThreshold (.3);//Ursprünglich:1.0 // je kleiner desto geringer darf sich das Cluster krümmen

// Anwendung des Cluster-Filters auf Input-Wolke "cloud"
std::vector <pcl::PointIndices> clusters;
reg.extract (clusters);

pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr planes_cloud (new pcl::PointCloud<pcl::PointXYZ>);

//vtkSmartPointer<vtkAppendPolyData> appendFilter =
//vtkSmartPointer<vtkAppendPolyData>::New();
//vtkSmartPointer<vtkPolyData> polyplane =
//vtkSmartPointer<vtkPolyData>::New();

int b=0;

// Schleife über alle detektierten Cluster
for(int a=0;a<clusters.size();a++)
{
if(clusters[a].indices.size() >100 )
{
pcl::IndicesPtr indices_ptr (new std::vector<int> (clusters[a].indices.size ()));
for (int i=0;i<indices_ptr->size();i++)
{
(*indices_ptr)[i]=clusters[a].indices[i]; // http://www.pcl-users.org/Removing-a-cluster-Problem-with-pointer-td4023699.html
} // Indizes des jeweiligen Clusters werden alle in indices_ptr gespeichert
// Punkte des Clusters werden in cluster_cloud geschrieben
extract.setIndices(indices_ptr);
extract.setNegative(false);
extract.filter(*cluster_cloud);// Punkte des Cluster a werden in cluster_cloud geschrieben
std::cout<<"cluster_cloud["<<a<<"] hat "<<cluster_cloud->width*cluster_cloud->height<<" Punkte."<<std::endl;

//Erzeugen einer Ebene aus Cluster_cloud
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(1000);
seg.setDistanceThreshold(0.1);
seg.setInputCloud(cluster_cloud);
seg.segment(*inliers, *coefficients);

// Wenn Ebene vertikal: Abspeichern in Cluster_i.pcd
if(coefficients->values[2]<.9 && coefficients->values[2]>(-.9))//ax+by+cz+d=0 (wenn c=0 => Ebene parallel zur z-Achse)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr planes_projected (new pcl::PointCloud<pcl::PointXYZ>);
//Inliers auf Ebene projizieren:
pcl::PCA<pcl::PointXYZ> pca2;
pcl::ProjectInliers<pcl::PointXYZ> proj2;
proj2.setModelType(pcl::SACMODEL_PLANE);
proj2.setIndices(inliers);
proj2.setInputCloud(cluster_cloud);
proj2.setModelCoefficients(coefficients);
proj2.filter(*planes_projected);

// Punkte in Eigenraum transformieren, um bounding box zu berechnen (Quelle: pcl-users Forum (http://www.pcl-users.org/Finding-oriented-bounding-box-of-a-cloud-td4024616.html)
// compute principal direction
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*planes_projected, centroid);
Eigen::Matrix3f covariance;
computeCovarianceMatrixNormalized(*planes_projected, centroid, covariance);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
Eigen::Matrix3f eigDx = eigen_solver.eigenvectors();
eigDx.col(2) = eigDx.col(0).cross(eigDx.col(1));

// move the points to the that reference frame
Eigen::Matrix4f p2w(Eigen::Matrix4f::Identity());
p2w.block<3,3>(0,0) = eigDx.transpose();
p2w.block<3,1>(0,3) = -1.f * (p2w.block<3,3>(0,0) * centroid.head<3>());
pcl::PointCloud<pcl::PointXYZ> cPoints;
pcl::transformPointCloud(*planes_projected, cPoints, p2w);
pcl::PointXYZ min_pt, max_pt;
pcl::getMinMax3D(cPoints, min_pt, max_pt);
const Eigen::Vector3f mean_diag = 0.5f*(max_pt.getVector3fMap() + min_pt.getVector3fMap());

// final transform
const Eigen::Quaternionf qfinal(eigDx);
const Eigen::Vector3f tfinal = eigDx*mean_diag + centroid.head<3>();

// Punktwolke und bounding box im viewer anzeigen
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer);
viewer->addCoordinateSystem ();
viewer->addPointCloud(planes_projected);// Danach auskommentieren
viewer->addCube(tfinal, qfinal, max_pt.x - min_pt.x, max_pt.y - min_pt.y, max_pt.z - min_pt.z);


// Ausgabe der Eckpunkte (kann gelöscht werden)
std::cout << " min.x= " << min_pt.x << " max.x= " << max_pt.x << " min.y= " << min_pt.y << " max.y= " << max_pt.y << " min.z= " << min_pt.z << " max.z= " << max_pt.z<< std::endl;
std::cout << "Punkte: " << min_pt.x <<";" << min_pt.y << ";" << min_pt.z <<std::endl;
std::cout << min_pt.x <<";" << min_pt.y << ";" << max_pt.z <<std::endl;
std::cout << min_pt.x <<";" << max_pt.y << ";" << min_pt.z <<std::endl;
std::cout << min_pt.x <<";" << max_pt.y << ";" << max_pt.z <<std::endl;
std::cout << max_pt.x <<";" << min_pt.y << ";" << min_pt.z <<std::endl;
std::cout << max_pt.x <<";" << min_pt.y << ";" << max_pt.z <<std::endl;
std::cout << max_pt.x <<";" << max_pt.y << ";" << min_pt.z <<std::endl;
std::cout << max_pt.x <<";" << max_pt.y << ";" << max_pt.z <<std::endl;

// Erzeugen einer Punktwolke mit Punkten, die zwischen den Extrema liegen (um anschließend eine dichte Oberfläche zu erzeugen)
pcl::PointCloud<pcl::PointXYZ>::Ptr Fillcloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointXYZ P1,P2,P3,P4;
P1.x=max_pt.x;P2.x=max_pt.x; P3.x=max_pt.x; P4.x=max_pt.x;
P1.y=min_pt.y;P2.y=min_pt.y; P3.y=max_pt.y; P4.y=max_pt.y;
P1.z=min_pt.z;P2.z=max_pt.z; P3.z=min_pt.z; P4.z=max_pt.z;

//P4.x=min_pt.x;P3.x=min_pt.x; P2.x=min_pt.x; P1.x=min_pt.x;
//P4.y=min_pt.y;P3.y=min_pt.y; P2.y=max_pt.y; P1.y=max_pt.y;
//P4.z=min_pt.z;P3.z=max_pt.z; P2.z=min_pt.z; P1.z=max_pt.z;

Fillcloud->push_back(P1);
Fillcloud->push_back(P2);
Fillcloud->push_back(P3);
Fillcloud->push_back(P4);
// Fillcloud->push_back (pcl::PointXYZ (min_pt.x, min_pt.y, min_pt.z)); // P1
// Fillcloud->push_back (pcl::PointXYZ (min_pt.x, min_pt.y, max_pt.z)); // P2
// Fillcloud->push_back (pcl::PointXYZ (min_pt.x, max_pt.y, min_pt.z)); // P3
// Fillcloud->push_back (pcl::PointXYZ (min_pt.x, max_pt.y, max_pt.z)); // P4 // Enweder alle x_max oder alle x_min nehmen WARUM?????
// Fillcloud->push_back (pcl::PointXYZ (max_pt.x, min_pt.y, min_pt.z));
// Fillcloud->push_back (pcl::PointXYZ (max_pt.x, min_pt.y, max_pt.z));
// Fillcloud->push_back (pcl::PointXYZ (max_pt.x, max_pt.y, min_pt.z));
// Fillcloud->push_back (pcl::PointXYZ (max_pt.x, max_pt.y, max_pt.z));
// Schleife, um BoundingBox-"Fläche" mit Punkten zu füllen (um ein Mesh erzeugen zu können


int AnzahlPunktehoch = 80;
int AnzahlPunktebreit = 400;
for( int ii = 0; ii < AnzahlPunktebreit+1; ii++ ){
    Fillcloud->push_back( pcl::PointXYZ (P1.x+((P2.x-P1.x)/AnzahlPunktebreit)*ii , P1.y+((P2.y-P1.y)/AnzahlPunktebreit)*ii , P1.z+((P2.z-P1.z)/AnzahlPunktebreit)*ii) );
    for( int jj = 0; jj < AnzahlPunktehoch+1; jj++ ){
        Fillcloud->push_back( pcl::PointXYZ (P1.x+((P2.x-P1.x)/AnzahlPunktebreit)*ii + (P3.x-P1.x)/AnzahlPunktehoch*jj , P1.y+((P2.y-P1.y)/AnzahlPunktebreit)*ii + (P3.y-P1.y)/AnzahlPunktehoch*jj , P1.z+((P2.z-P1.z)/AnzahlPunktebreit)*ii + (P3.z-P1.z)/AnzahlPunktehoch*jj));
    }
}

// Erzeugte Punktwolke, die die Extrema "verbindet" rücktransformieren
pcl::PointCloud<pcl::PointXYZ>::Ptr Fillcloud_transformiert (new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*Fillcloud,*Fillcloud_transformiert,tfinal,qfinal);

// Fillcloud wird zum Viewer hinzugefügt; "spin" auskommentieren wenn nicht jedes Fenster einzeln weggeklickt werden soll
//viewer->addPointCloud(Fillcloud_transformiert);
//viewer->spin();

//Alle Clusterebenen, die vertikal sind werden in planes_cloud gespeichert
*planes_cloud+=*Fillcloud_transformiert;

// Aus aktuellen Punkten vtkPlaneSource erzeugen
vtkPlaneSource *plane= vtkPlaneSource::New();
vtkPlaneSource *plane2= vtkPlaneSource::New();
plane->SetOrigin(min_pt.x,min_pt.y,min_pt.z);
plane->SetPoint1(min_pt.x,min_pt.y,max_pt.z);
plane->SetPoint2(min_pt.x,max_pt.y,min_pt.z);
plane->SetXResolution(10);
//plane->SetResolution(10,200); // Set the number of x-y subdivisions in the plane
plane->Update();
plane2->SetOrigin(min_pt.x,min_pt.y,max_pt.z);
plane2->SetPoint1(min_pt.x,max_pt.y,max_pt.z);
plane2->SetPoint2(min_pt.x,max_pt.y,min_pt.z);
//plane->SetResolution(1,1);
plane2->Update();
// appender->AddInputConnection(plane->GetOutputPort());
// appender->Update();
// actor->SetMapper(mapper);
// // Aktuelle Plane zu PlaneCollection hinzufügen
// PlaneCollection->AddItem(plane->GetOutputPort());
// PlaneCollection->AddItem(plane);
// mapper->SetInputConnection(plane->GetOutputPort());
// plane->Delete();
// mapper->SetInputConnection(plane->GetOutputPort());
// actor->SetMapper(mapper);
// renderer->AddActor(plane);
// vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
// polydata->SetVerts();
// polyplane->ShallowCopy(plane->GetOutput());
// appendFilter->AddInputConnection(polyplane->GetProducerPort());

//appendFilter->AddInput(plane->GetOutput());
//appendFilter->Update();

//appendFilter->AddInput(plane2->GetOutput());
//appendFilter->Update();
b=b+1;
}
}
}
// PlanesCollection als STL abspeichern
//vtkSmartPointer<vtkSTLWriter> schreiber = vtkSmartPointer<vtkSTLWriter>::New();
//schreiber->SetInput(appendFilter->GetOutput());
//schreiber->SetFileName("funktioniert nicht!");
//schreiber->Update();
//schreiber->Write();

// schreiber->SetInputConnection(plane->GetOutputPort());
// schreiber->SetFileName("");
// schreiber->SetInput(appender->GetOutput());
// //schreiber->SetFileTypeToASCII();
// schreiber->Write();

// Ausgabe
std::cout<<"Es wurden "<<b<<" Flächen in Planes_cloud.pcd geschrieben"<<endl;
writer.write("Planes_cloud.pcd",*planes_cloud,false);
std::cout << "Number of clusters is equal to " << clusters.size () << std::endl;
std::cout << "First cluster has " << clusters[0].indices.size () << " points." << endl;
std::cout << "These are the indices of the points of the initial" <<
std::endl << "cloud that belong to the first cluster:" << std::endl;
int counter = 0;
while (counter < 5 || counter > clusters[0].indices.size ())
{
std::cout << clusters[0].indices[counter] << std::endl;
counter++;
}

// planes_cloud zu einer Oberfläche konvertieren u. als stl oder vtk speichern!
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
pcl::PointCloud<pcl::Normal>::Ptr normals_triangles (new pcl::PointCloud<pcl::Normal>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_triangles (new pcl::search::KdTree<pcl::PointXYZ>);
tree_triangles->setInputCloud(planes_cloud);
n.setInputCloud(planes_cloud);
n.setSearchMethod(tree_triangles);
n.setKSearch(20);
n.compute(*normals_triangles);

pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields (*planes_cloud,*normals_triangles,*cloud_with_normals);
pcl::search::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointNormal>);
tree2->setInputCloud(cloud_with_normals);
pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
pcl::PolygonMesh triangles;

gp3.setSearchRadius(0.4);// Ursprünglich 0.025
gp3.setMu(5.0); // Ursprünglich 2.5
gp3.setMaximumNearestNeighbors(20); // davor 100
gp3.setMaximumSurfaceAngle(M_PI/4); // ursprünglich: M_PI/4
gp3.setMinimumAngle(M_PI/18);
gp3.setMaximumAngle(2*M_PI/1); // ursprunglich: 2*M_PI/3
gp3.setNormalConsistency(false);
gp3.setInputCloud(cloud_with_normals);
gp3.setSearchMethod(tree2);
gp3.reconstruct(triangles);

std::vector<int> parts = gp3.getPartIDs();
std::vector<int> states = gp3.getPointStates();

// Oberfläche als STL-Modell abspeichern
pcl::io::savePolygonFileSTL("Arena_24_7_2014.stl", triangles);
//pcl::io::saveVTKFile("mesh.vtk", triangles);

// Farbliche Visualisierung der segmentierten Punktwolke
pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud ();
pcl::visualization::CloudViewer viewer ("Cluster viewer");
viewer.showCloud(colored_cloud);
viewer.runOnVisualizationThreadOnce(background_color);
while (!viewer.wasStopped ())
{
}
/*
pcl::PointCloud <pcl::PointXYZ>::Ptr cluster_cloud=clusters[0].indices;
pcl::visualization::CloudViewer viewer ("Cluster viewer");
viewer.showCloud(cluster_cloud);
while (!viewer.wasStopped())
{
}
*/
return (0);
}
		int process(const tendrils& inputs, const tendrils& outputs,
					      boost::shared_ptr<const ::pcl::PointCloud<Point> >& input)
		{
      centers_->clear();
      centers_->resize(static_cast<std::size_t>(clusters_->size()));
      ::pcl::ExtractIndices<Point> filter;
      filter.setInputCloud(input);
      Eigen::Vector3f firstAxis;
      Eigen::Vector3f secondAxis;
      Eigen::Matrix3f frame;

      for(std::size_t i = 0; i < clusters_->size(); ++i)
      {
        if((*clusters_)[i].indices.size() == 0)
          continue;

        frame = (*frames_)[i];
        firstAxis = frame.col(0);
        secondAxis = frame.col(1);
        boost::shared_ptr< ::pcl::PointCloud<Point> > cloud;
        cloud = boost::make_shared< ::pcl::PointCloud<Point> > ();
        // extract indices into a cloud
        filter.setIndices( ::pcl::PointIndicesPtr(
        new ::pcl::PointIndices ((*clusters_)[i])) );
        filter.filter(*cloud);
        Eigen::Vector3f point(cloud->points[0].x, 
                              cloud->points[0].y, cloud->points[0].z);
        Eigen::Vector3f minPoint(cloud->points[0].x, 
                                 cloud->points[0].y, cloud->points[0].z);
        double firstMin = point.dot(firstAxis);
        double firstMax = point.dot(firstAxis);
        double secondMin = point.dot(secondAxis);
        double secondMax = point.dot(secondAxis);
        for(std::size_t p = 0; p < cloud->points.size(); ++p)
        {
          point = Eigen::Vector3f(cloud->points[p].x, 
                                  cloud->points[p].y, cloud->points[p].z);
          if(point.dot(firstAxis) > firstMax)
          {
            firstMax = point.dot(firstAxis);
          }
          if(point.dot(firstAxis) < firstMin)
          {
            firstMin = point.dot(firstAxis);
            minPoint.x() = point.x();
          }
          if(point.dot(secondAxis) > secondMax)
          {
            secondMax = point.dot(secondAxis);
          }
          if(point.dot(secondAxis) < secondMin)
          {
            secondMin = point.dot(secondAxis);
            minPoint.y() = point.y();
          }
        }  
        Eigen::Vector3f Center = minPoint + 
                             ((firstMax-firstMin)/2)*firstAxis + 
                             ((secondMax-secondMin)/2)*secondAxis;
        centers_->at(i) = Eigen::Vector4f(Center.x(), 
                                          Center.y(), 
                                          Center.z(), 
                                          0.0); 
      }
      
			return ecto::OK;
		}
Пример #12
0
  void get3DMoments(vector<Point> feature_points, int feat_index, int index)
  {
    //    for(int i=0; i<feature_points.size(); i++)
    //   ROS_INFO("%d --> %d,%d",i,feature_points[i].x,feature_points[i].y);
    //ROS_INFO("Getting 3D Moments : %d --> %d,%d", feature_points.size(), width, height);
    
    //Extract the indices for the points in the point cloud data
    pcl::PointIndices point_indices;
     
    for(int i=0; i<feature_points.size(); i++)
      {
	//ROS_INFO("Feature Index : %d, %d",feature_points[i].x, feature_points[i].y);
	point_indices.indices.push_back(feature_points[i].y * width + feature_points[i].x);
      }
    
    //ROS_INFO("Computing 3D Centroid : %d",point_indices.indices.size());
    Eigen::Vector4f centroid;
    Eigen::Matrix3f covariance_matrix;
    
    // Estimate the XYZ centroid
    pcl::compute3DCentroid (pcl_in, point_indices, centroid); 
#ifdef DEBUG
    ROS_INFO("Centroid %d: %f, %f, %f, %f",index,centroid(0),centroid(1),centroid(2),centroid(3));
#endif

    //ROS_INFO("Computing Covariance ");
    //Compute the centroid and the covariance of the points
    computeCovarianceMatrix(pcl_in, point_indices.indices, centroid, covariance_matrix);
    
    //Print the 3D Moments
    //ROS_INFO("Centroid : %f, %f, %f, %f",centroid(0),centroid(1),centroid(2),centroid(3));
#ifdef DEBUG
    std::cout<<"Covariance : "<<std::endl<<covariance_matrix <<std::endl;
#endif

    for(int i=0; i<3; i++)
      {
	feedback_.features[feat_index].moments[index].mean[i] = centroid(i);
	for(int j=0; j<3; j++)
	  {
	    feedback_.features[feat_index].moments[index].covariance[i*3+j] = covariance_matrix(i,j);
	  }
      }

    //Get the principal components and the quaternion
    Eigen::Matrix3f evecs;
    Eigen::Vector3f evals;
    pcl::eigen33 (covariance_matrix, evecs, evals);
    
    Eigen::Matrix3f rotation;
    rotation.row (0) = evecs.col (0);
    rotation.row (1) = evecs.col (1);
    //rotation.row (2) = evecs.col (2);
    rotation.row (2) = rotation.row (0).cross (rotation.row (1));
    //rotation.transposeInPlace ();
#ifdef DEBUG
    std::cerr << "Rotation matrix: " << endl;
    std::cerr << rotation << endl;
    std::cout<<"Eigen vals : "<<evals<<std::endl;
#endif

    rotation.transposeInPlace ();
    Eigen::Quaternion<float> qt (rotation);
    qt.normalize ();

    //Publish Marker
    visualization_msgs::Marker marker;	
    
    marker.header.frame_id = "/openni_rgb_optical_frame";
    marker.header.stamp = ros::Time().now();
    marker.ns = "Triangulation";
    marker.id = index+1;	
    marker.action = visualization_msgs::Marker::ADD;
    marker.lifetime = ros::Duration(5);		
    
    //centroid position
    marker.type = visualization_msgs::Marker::SPHERE;
    
    
    marker.pose.position.x = centroid(0);
    marker.pose.position.y = centroid(1);
    marker.pose.position.z = centroid(2);	
    marker.pose.orientation.x = qt.x();
    marker.pose.orientation.y = qt.y();
    marker.pose.orientation.z = qt.z();
    marker.pose.orientation.w = qt.w();			
    
    marker.scale.x = sqrt(evals(0)) *2;
    marker.scale.y =  sqrt(evals(1)) *2;
    marker.scale.z =  sqrt(evals(2)) *2;
    
    //red
    marker.color.a = 0.5;
    marker.color.r = rand()/((double)RAND_MAX + 1);
    marker.color.g = rand()/((double)RAND_MAX + 1);
    marker.color.b = rand()/((double)RAND_MAX + 1);
    object_pose_marker_pub_.publish(marker);	
    
  }
bool PositionBasedElasticRod::ComputeDarbouxGradient(
	const Eigen::Vector3f& darboux_vector, const float length,
	const Eigen::Matrix3f& da, const Eigen::Matrix3f& db,
	//const Eigen::Matrix3f(*dajpi)[3], const Eigen::Matrix3f(*dbjpi)[3], 
	const Eigen::Matrix3f dajpi[3][3], const Eigen::Matrix3f dbjpi[3][3],
	const Eigen::Vector3f& bendAndTwistKs,
	Eigen::Matrix3f& omega_pa, Eigen::Matrix3f& omega_pb, Eigen::Matrix3f& omega_pc, Eigen::Matrix3f& omega_pd, Eigen::Matrix3f& omega_pe
	)
{


	//float x = 1.0f + da[0] * db[0] + da[1] * db[1] + da[2] * db[2];
	float x = 1.0f + da.col(0).dot(db.col(0)) + da.col(1).dot(db.col(1)) + da.col(2).dot(db.col(2));
	x = 2.0f / (length * x);

	for (int c = 0; c < 3; ++c) 
	{
		const int i = permutation[c][0];
		const int j = permutation[c][1];
		const int k = permutation[c][2];
		// pa
		{
			Eigen::Vector3f term1(0,0,0);
			Eigen::Vector3f term2(0,0,0);
			Eigen::Vector3f tmp(0,0,0);
			// first term
			//dj::MulVecMatrix3x3<real>(db[k](), (real(*)[3]) &dajpi[j][0], term1());
			//dj::MulVecMatrix3x3<real>(db[j](), (real(*)[3]) &dajpi[k][0], tmp());

			//DOUBLE CHECK !!!
			term1 = dajpi[j][0].transpose() * db.col(k);
			tmp =   dajpi[k][0].transpose() * db.col(j);
			term1 = term1 - tmp;
			// second term
			for (int n = 0; n < 3; ++n) 
			{
				//dj::MulVecMatrix3x3<real>(db[n](), (real(*)[3]) &dajpi[n][0], tmp());

				//DOUBLE CHECK !!!
				tmp = dajpi[n][0].transpose() * db.col(n);
				term2 = term2 + tmp;
			}
			omega_pa.col(i) = (term1)-(0.5f * darboux_vector[i] * length) * (term2);
			omega_pa.col(i) *= (x * bendAndTwistKs[i]);
		}
		// pb
		{
			Eigen::Vector3f term1(0, 0, 0);
			Eigen::Vector3f term2(0, 0, 0);
			Eigen::Vector3f tmp(0, 0, 0);
			// first term
			//dj::MulVecMatrix3x3<real>(db[k](), (real(*)[3]) &dajpi[j][1], term1());
			//dj::MulVecMatrix3x3<real>(db[j](), (real(*)[3]) &dajpi[k][1], tmp());
			term1 = dajpi[j][1].transpose() * db.col(k);
			tmp =   dajpi[k][1].transpose() * db.col(j);
			term1 = term1 - tmp;
			// third term
			//dj::MulVecMatrix3x3<real>(da[k](), (real(*)[3]) &dbjpi[j][0], tmp());
			tmp = dbjpi[j][0].transpose() * da.col(k);
			term1 = term1 - tmp;
			
			//dj::MulVecMatrix3x3<real>(da[j](), (real(*)[3]) &dbjpi[k][0], tmp());
			tmp = dbjpi[k][0].transpose() * da.col(j);
			term1 = term1 + tmp;

			// second term
			for (int n = 0; n < 3; ++n) 
			{
				//dj::MulVecMatrix3x3<real>(db[n](), (real(*)[3]) &dajpi[n][1], tmp());
				tmp = dajpi[n][1].transpose() * db.col(n);
				term2 = term2 + tmp;
				
				//dj::MulVecMatrix3x3<real>(da[n](), (real(*)[3]) &dbjpi[n][0], tmp());
				tmp = dbjpi[n][0].transpose() * da.col(n);
				term2 = term2 + tmp;
			}
			omega_pb.col(i) = (term1)-(0.5f * darboux_vector[i] * length) * (term2);
			omega_pb.col(i) *= (x * bendAndTwistKs[i]);
		}
		// pc
		{
			Eigen::Vector3f term1(0, 0, 0);
			Eigen::Vector3f term2(0, 0, 0);
			Eigen::Vector3f tmp(0, 0, 0);
			
			// first term
			//dj::MulVecMatrix3x3<real>(da[k](), (real(*)[3]) &dbjpi[j][1], term1());
			//dj::MulVecMatrix3x3<real>(da[j](), (real(*)[3]) &dbjpi[k][1], tmp());
			term1 = dbjpi[j][1].transpose() * da.col(k);
			tmp =   dbjpi[k][1].transpose() * da.col(j);
			term1 = term1 - tmp;

			// second term
			for (int n = 0; n < 3; ++n) 
			{
				//dj::MulVecMatrix3x3<real>(da[n](), (real(*)[3]) &dbjpi[n][1], tmp());
				tmp = dbjpi[n][1].transpose() * da.col(n);
				term2 = term2 + tmp;
			}
			omega_pc.col(i) = (term1)+(0.5f * darboux_vector[i] * length) * (term2);
			omega_pc.col(i) *= (-x * bendAndTwistKs[i]);
		}
		// pd
		{
			Eigen::Vector3f term1(0, 0, 0);
			Eigen::Vector3f term2(0, 0, 0);
			Eigen::Vector3f tmp(0, 0, 0);
			// first term
			//dj::MulVecMatrix3x3<real>(db[k](), (real(*)[3]) &dajpi[j][2], term1());
			//dj::MulVecMatrix3x3<real>(db[j](), (real(*)[3]) &dajpi[k][2], tmp());
			term1 = dajpi[j][2].transpose() * db.col(k);
			tmp =   dajpi[k][2].transpose() * db.col(j);
			term1 = term1 - tmp;
			// second term
			for (int n = 0; n < 3; ++n) {
				//dj::MulVecMatrix3x3<real>(db[n](), (real(*)[3]) &dajpi[n][2], tmp());
				tmp = dajpi[n][2].transpose() * db.col(n);
				term2 = term2 + tmp;
			}
			omega_pd.col(i) = (term1)-(0.5f * darboux_vector[i] * length) * (term2);
			omega_pd.col(i) *= (x * bendAndTwistKs[i]);
		}
		// pe
		{
			Eigen::Vector3f term1(0, 0, 0);
			Eigen::Vector3f term2(0, 0, 0);
			Eigen::Vector3f tmp(0, 0, 0);
			// first term

			//dj::MulVecMatrix3x3<real>(da[k](), (real(*)[3]) &dbjpi[j][2], term1());
			//dj::MulVecMatrix3x3<real>(da[j](), (real(*)[3]) &dbjpi[k][2], tmp());
			term1 = dbjpi[j][2].transpose() * da.col(k);
			tmp = dbjpi[k][2].transpose() * da.col(j);
			term1 -= tmp;
			
			// second term
			for (int n = 0; n < 3; ++n) 
			{	//WARNING!!! &dbjpi[n][2][0][0] ???
				//dj::MulVecMatrix3x3<real>(da[n](), (real(*)[3]) &dbjpi[n][2][0][0], tmp());
				tmp = dbjpi[n][2].transpose() * da.col(n);
				term2 += tmp;
			}

			omega_pe.col(i) = (term1)+(0.5f * darboux_vector[i] * length) * (term2);
			omega_pe.col(i) *= (-x * bendAndTwistKs[i]);
		}
	}
	return true;
}
bool PositionBasedElasticRod::ComputeMaterialFrameDerivative(
	const Eigen::Vector3f& p0, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, const Eigen::Matrix3f& d,
	Eigen::Matrix3f& d1p0, Eigen::Matrix3f& d1p1, Eigen::Matrix3f& d1p2,
	Eigen::Matrix3f& d2p0, Eigen::Matrix3f& d2p1, Eigen::Matrix3f& d2p2,
	Eigen::Matrix3f& d3p0, Eigen::Matrix3f& d3p1, Eigen::Matrix3f& d3p2)
{

	// d3pi
	Eigen::Vector3f p01 = p1 - p0;
	float length_p01 = p01.norm();

	d3p0.col(0) = d.col(2)[0] * d.col(2);
	d3p0.col(1) = d.col(2)[1] * d.col(2);
	d3p0.col(2) = d.col(2)[2] * d.col(2);

	d3p0.col(0)[0] -= 1.0f;
	d3p0.col(1)[1] -= 1.0f;
	d3p0.col(2)[2] -= 1.0f;

	d3p0.col(0) *= (1.0f / length_p01);
	d3p0.col(1) *= (1.0f / length_p01);
	d3p0.col(2) *= (1.0f / length_p01);

	d3p1.col(0) = -d3p0.col(0);
	d3p1.col(1) = -d3p0.col(1);
	d3p1.col(2) = -d3p0.col(2);

	d3p2.col(0).setZero();
	d3p2.col(1).setZero();
	d3p2.col(2).setZero();

	////>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
	//// d2pi
	Eigen::Vector3f p02 = p2 - p0;
	Eigen::Vector3f p01_cross_p02 = p01.cross(p02);

	float length_cross = p01_cross_p02.norm();

	Eigen::Matrix3f mat;
	mat.col(0) = d.col(1)[0] * d.col(1);
	mat.col(1) = d.col(1)[1] * d.col(1);
	mat.col(2) = d.col(1)[2] * d.col(1);

	mat.col(0)[0] -= 1.0f;
	mat.col(1)[1] -= 1.0f;
	mat.col(2)[2] -= 1.0f;

	mat.col(0) *= (-1.0f / length_cross);
	mat.col(1) *= (-1.0f / length_cross);
	mat.col(2) *= (-1.0f / length_cross);

	Eigen::Matrix3f product_matrix;
	MathFunctions::crossProductMatrix(p2 - p1, product_matrix);
	d2p0 = mat * product_matrix;

	MathFunctions::crossProductMatrix(p0 - p2, product_matrix);
	d2p1 = mat * product_matrix;

	MathFunctions::crossProductMatrix(p1 - p0, product_matrix);
	d2p2 = mat * product_matrix;

	////>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
	//// d1pi
	Eigen::Matrix3f product_mat_d3;
	Eigen::Matrix3f product_mat_d2;
	Eigen::Matrix3f m1, m2;

	MathFunctions::crossProductMatrix(d.col(2), product_mat_d3);
	MathFunctions::crossProductMatrix(d.col(1), product_mat_d2);

	//dj::MulMatrix3x3<real>(&product_mat_d3[0][0], &d2p0[0][0], &m1[0][0]);
	//dj::MulMatrix3x3<real>(&product_mat_d2[0][0], &d3p0[0][0], &m2[0][0]);
	d1p0 = product_mat_d2 * d3p0 - product_mat_d3 * d2p0;

	//dj::MulMatrix3x3<real>(&product_mat_d3[0][0], &d2p1[0][0], &m1[0][0]);
	//dj::MulMatrix3x3<real>(&product_mat_d2[0][0], &d3p1[0][0], &m2[0][0]);
	d1p1 = product_mat_d2 * d3p1 - product_mat_d3 * d2p1;

	/*dj::MulMatrix3x3<real>(&product_mat_d3[0][0], &d2p2[0][0], &d1p2[0][0]);*/
	d1p2 = product_mat_d3 * d2p2;
	d1p2.col(0) *= -1.0f;
	d1p2.col(1) *= -1.0f;
	d1p2.col(2) *= -1.0f;
	return true;
}
Пример #15
0
        }
    }
    return corr;
}
//--------------------------------------------------------------------------------------------------------------
//Pose estimation tools
void computeCube(Eigen::Vector3f& tfinal,  Eigen::Quaternionf& qfinal_r, pcl::PointCloud<PointN>::Ptr final, float& _x, float& _y, float& _z) {
    Eigen::Vector4f centroid;
    pcl::compute3DCentroid(*final, centroid);
    std::cout << "centroid: \n" << centroid[0] << " " << centroid[1] << " " << centroid[2]<< std::endl;

    Eigen::Matrix3f covariance;
    computeCovarianceMatrixNormalized(*final, centroid, covariance);
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
    Eigen::Matrix3f eigDx = eigen_solver.eigenvectors();
    eigDx.col(2) = eigDx.col(0).cross(eigDx.col(1));

    // move the points to the that reference frame
    Eigen::Matrix4f p2w(Eigen::Matrix4f::Identity());
    p2w.block<3,3>(0,0) = eigDx.transpose();
    p2w.block<3,1>(0,3) = -1.f * (p2w.block<3,3>(0,0) * centroid.head<3>());
    pcl::PointCloud<PointN> cPoints;
    pcl::transformPointCloud(*final, cPoints, p2w);
    PointN min_pt, max_pt;
    pcl::getMinMax3D(cPoints, min_pt, max_pt);
    const Eigen::Vector3f mean_diag = 0.5f*(max_pt.getVector3fMap() + min_pt.getVector3fMap());
    // final transform

    const Eigen::Quaternionf qfinal(eigDx);

    tfinal = eigDx*mean_diag + centroid.head<3>();
Пример #16
0
BlockFitter::Result BlockFitter::
go() {
  Result result;
  result.mSuccess = false;

  if (mCloud->size() < 100) return result;

  // voxelize
  LabeledCloud::Ptr cloud(new LabeledCloud());
  pcl::VoxelGrid<pcl::PointXYZL> voxelGrid;
  voxelGrid.setInputCloud(mCloud);
  voxelGrid.setLeafSize(mDownsampleResolution, mDownsampleResolution,
                        mDownsampleResolution);
  voxelGrid.filter(*cloud);
  for (int i = 0; i < (int)cloud->size(); ++i) cloud->points[i].label = i;

  if (mDebug) {
    std::cout << "Original cloud size " << mCloud->size() << std::endl;
    std::cout << "Voxelized cloud size " << cloud->size() << std::endl;
    pcl::io::savePCDFileBinary("cloud_full.pcd", *cloud);
  }

  if (cloud->size() < 100) return result;

  // pose
  cloud->sensor_origin_.head<3>() = mOrigin;
  cloud->sensor_origin_[3] = 1;
  Eigen::Vector3f rz = mLookDir;
  Eigen::Vector3f rx = rz.cross(Eigen::Vector3f::UnitZ());
  Eigen::Vector3f ry = rz.cross(rx);
  Eigen::Matrix3f rotation;
  rotation.col(0) = rx.normalized();
  rotation.col(1) = ry.normalized();
  rotation.col(2) = rz.normalized();
  Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
  pose.linear() = rotation;
  pose.translation() = mOrigin;

  // ground removal
  if (mRemoveGround) {
    Eigen::Vector4f groundPlane;

    // filter points
    float minZ = mMinGroundZ;
    float maxZ = mMaxGroundZ;
    if ((minZ > 10000) && (maxZ > 10000)) {
      std::vector<float> zVals(cloud->size());
      for (int i = 0; i < (int)cloud->size(); ++i) {
        zVals[i] = cloud->points[i].z;
      }
      std::sort(zVals.begin(), zVals.end());
      minZ = zVals[0]-0.1;
      maxZ = minZ + 0.5;
    }
    LabeledCloud::Ptr tempCloud(new LabeledCloud());
    for (int i = 0; i < (int)cloud->size(); ++i) {
      const Eigen::Vector3f& p = cloud->points[i].getVector3fMap();
      if ((p[2] < minZ) || (p[2] > maxZ)) continue;
      tempCloud->push_back(cloud->points[i]);
    }

    // downsample
    voxelGrid.setInputCloud(tempCloud);
    voxelGrid.setLeafSize(0.1, 0.1, 0.1);
    voxelGrid.filter(*tempCloud);

    if (tempCloud->size() < 100) return result;

    // find ground plane
    std::vector<Eigen::Vector3f> pts(tempCloud->size());
    for (int i = 0; i < (int)tempCloud->size(); ++i) {
      pts[i] = tempCloud->points[i].getVector3fMap();
    }
    const float kGroundPlaneDistanceThresh = 0.01; // TODO: param
    PlaneFitter planeFitter;
    planeFitter.setMaxDistance(kGroundPlaneDistanceThresh);
    planeFitter.setRefineUsingInliers(true);
    auto res = planeFitter.go(pts);
    groundPlane = res.mPlane;
    if (groundPlane[2] < 0) groundPlane = -groundPlane;
    if (mDebug) {
      std::cout << "dominant plane: " << groundPlane.transpose() << std::endl;
      std::cout << "  inliers: " << res.mInliers.size() << std::endl;
    }

    if (std::acos(groundPlane[2]) > 30*M_PI/180) {
      std::cout << "error: ground plane not found!" << std::endl;
      std::cout << "proceeding with full segmentation (may take a while)..." <<
        std::endl;
    }

    else {
      // compute convex hull
      result.mGroundPlane = groundPlane;
      {
        tempCloud.reset(new LabeledCloud());
        for (int i = 0; i < (int)cloud->size(); ++i) {
          Eigen::Vector3f p = cloud->points[i].getVector3fMap();
          float dist = groundPlane.head<3>().dot(p) + groundPlane[3];
          if (std::abs(dist) > kGroundPlaneDistanceThresh) continue;
          p -= (groundPlane.head<3>()*dist);
          pcl::PointXYZL cloudPt;
          cloudPt.getVector3fMap() = p;
          tempCloud->push_back(cloudPt);
        }
        pcl::ConvexHull<pcl::PointXYZL> chull;
        pcl::PointCloud<pcl::PointXYZL> hull;
        chull.setInputCloud(tempCloud);
        chull.reconstruct(hull);
        result.mGroundPolygon.resize(hull.size());
        for (int i = 0; i < (int)hull.size(); ++i) {
          result.mGroundPolygon[i] = hull[i].getVector3fMap();
        }
      }

      // remove points below or near ground
      tempCloud.reset(new LabeledCloud());
      for (int i = 0; i < (int)cloud->size(); ++i) {
        Eigen::Vector3f p = cloud->points[i].getVector3fMap();
        float dist = p.dot(groundPlane.head<3>()) + groundPlane[3];
        if ((dist < mMinHeightAboveGround) ||
            (dist > mMaxHeightAboveGround)) continue;
        float range = (p-mOrigin).norm();
        if (range > mMaxRange) continue;
        tempCloud->push_back(cloud->points[i]);
      }
      std::swap(tempCloud, cloud);
      if (mDebug) {
        std::cout << "Filtered cloud size " << cloud->size() << std::endl;
      }
    }
  }

  // normal estimation
  auto t0 = std::chrono::high_resolution_clock::now();
  if (mDebug) {
    std::cout << "computing normals..." << std::flush;
  }
  RobustNormalEstimator normalEstimator;
  normalEstimator.setMaxEstimationError(0.01);
  normalEstimator.setRadius(0.1);
  normalEstimator.setMaxCenterError(0.02);
  normalEstimator.setMaxIterations(100);
  NormalCloud::Ptr normals(new NormalCloud());
  normalEstimator.go(cloud, *normals);
  if (mDebug) {
    auto t1 = std::chrono::high_resolution_clock::now();
    auto dt = std::chrono::duration_cast<std::chrono::milliseconds>(t1-t0);
    std::cout << "finished in " << dt.count()/1e3 << " sec" << std::endl;
  }

  // filter non-horizontal points
  const float maxNormalAngle = mMaxAngleFromHorizontal*M_PI/180;
  LabeledCloud::Ptr tempCloud(new LabeledCloud());
  NormalCloud::Ptr tempNormals(new NormalCloud());
  for (int i = 0; i < (int)normals->size(); ++i) {
    const auto& norm = normals->points[i];
    Eigen::Vector3f normal(norm.normal_x, norm.normal_y, norm.normal_z);
    float angle = std::acos(normal[2]);
    if (angle > maxNormalAngle) continue;
    tempCloud->push_back(cloud->points[i]);
    tempNormals->push_back(normals->points[i]);
  }
  std::swap(tempCloud, cloud);
  std::swap(tempNormals, normals);

  if (mDebug) {
    std::cout << "Horizontal points remaining " << cloud->size() << std::endl;
    pcl::io::savePCDFileBinary("cloud.pcd", *cloud);
    pcl::io::savePCDFileBinary("robust_normals.pcd", *normals);
  }

  // plane segmentation
  t0 = std::chrono::high_resolution_clock::now();
  if (mDebug) {
    std::cout << "segmenting planes..." << std::flush;
  }
  PlaneSegmenter segmenter;
  segmenter.setData(cloud, normals);
  segmenter.setMaxError(0.05);
  segmenter.setMaxAngle(5);
  segmenter.setMinPoints(100);
  PlaneSegmenter::Result segmenterResult = segmenter.go();
  if (mDebug) {
    auto t1 = std::chrono::high_resolution_clock::now();
    auto dt = std::chrono::duration_cast<std::chrono::milliseconds>(t1-t0);
    std::cout << "finished in " << dt.count()/1e3 << " sec" << std::endl;

    std::ofstream ofs("labels.txt");
    for (const int lab : segmenterResult.mLabels) {
      ofs << lab << std::endl;
    }
    ofs.close();

    ofs.open("planes.txt");
    for (auto it : segmenterResult.mPlanes) {
      auto& plane = it.second;
      ofs << it.first << " " << plane.transpose() << std::endl;
    }
    ofs.close();
  }

  // create point clouds
  std::unordered_map<int,std::vector<Eigen::Vector3f>> cloudMap;
  for (int i = 0; i < (int)segmenterResult.mLabels.size(); ++i) {
    int label = segmenterResult.mLabels[i];
    if (label <= 0) continue;
    cloudMap[label].push_back(cloud->points[i].getVector3fMap());
  }
  struct Plane {
    MatrixX3f mPoints;
    Eigen::Vector4f mPlane;
  };
  std::vector<Plane> planes;
  planes.reserve(cloudMap.size());
  for (auto it : cloudMap) {
    int n = it.second.size();
    Plane plane;
    plane.mPoints.resize(n,3);
    for (int i = 0; i < n; ++i) plane.mPoints.row(i) = it.second[i];
    plane.mPlane = segmenterResult.mPlanes[it.first];
    planes.push_back(plane);
  }

  std::vector<RectangleFitter::Result> results;
  for (auto& plane : planes) {
    RectangleFitter fitter;
    fitter.setDimensions(mBlockDimensions.head<2>());
    fitter.setAlgorithm((RectangleFitter::Algorithm)mRectangleFitAlgorithm);
    fitter.setData(plane.mPoints, plane.mPlane);
    auto result = fitter.go();
    results.push_back(result);
  }

  if (mDebug) {
    std::ofstream ofs("boxes.txt");
    for (int i = 0; i < (int)results.size(); ++i) {
      auto& res = results[i];
      for (auto& p : res.mPolygon) {
        ofs << i << " " << p.transpose() << std::endl;
      }
    }
    ofs.close();

    ofs.open("hulls.txt");
    for (int i = 0; i < (int)results.size(); ++i) {
      auto& res = results[i];
      for (auto& p : res.mConvexHull) {
        ofs << i << " " << p.transpose() << std::endl;
      }
    }
    ofs.close();

    ofs.open("poses.txt");
    for (int i = 0; i < (int)results.size(); ++i) {
      auto& res = results[i];
      auto transform = res.mPose;
      ofs << transform.matrix() << std::endl;
    }
    ofs.close();
  }

  for (int i = 0; i < (int)results.size(); ++i) {
    const auto& res = results[i];
    if (mBlockDimensions.head<2>().norm() > 1e-5) {
      float areaRatio = mBlockDimensions.head<2>().prod()/res.mConvexArea;
      if ((areaRatio < mAreaThreshMin) ||
          (areaRatio > mAreaThreshMax)) continue;
    }

    Block block;
    block.mSize << res.mSize[0], res.mSize[1], mBlockDimensions[2];
    block.mPose = res.mPose;
    block.mPose.translation() -=
      block.mPose.rotation().col(2)*mBlockDimensions[2]/2;
    block.mHull = res.mConvexHull;
    result.mBlocks.push_back(block);
  }
  if (mDebug) {
    std::cout << "Surviving blocks: " << result.mBlocks.size() << std::endl;
  }

  result.mSuccess = true;
  return result;
}
  void onGMM(const gaussian_mixture_model::GaussianMixture & mix)
  {
    visualization_msgs::MarkerArray msg;
    ROS_INFO("gmm_rviz_converter: Received message.");

    uint i;

    for (i = 0; i < mix.gaussians.size(); i++)
    {
      visualization_msgs::Marker marker;

      marker.header.frame_id = m_frame_id;
      marker.header.stamp = ros::Time::now();
      marker.ns = m_rviz_namespace;
      marker.id = i;
      marker.type = visualization_msgs::Marker::SPHERE;
      marker.action = visualization_msgs::Marker::ADD;
      marker.lifetime = ros::Duration();

      Eigen::Vector3f coords;
      for (uint ir = 0; ir < NUM_OUTPUT_COORDINATES; ir++)
        coords[ir] = m_conversion_mask[ir]->GetMean(m_conversion_mask,mix.gaussians[i]);
      marker.pose.position.x = coords.x();
      marker.pose.position.y = coords.y();
      marker.pose.position.z = coords.z();

      Eigen::Matrix3f covmat;
      for (uint ir = 0; ir < NUM_OUTPUT_COORDINATES; ir++)
        for (uint ic = 0; ic < NUM_OUTPUT_COORDINATES; ic++)
          covmat(ir,ic) = m_conversion_mask[ir]->GetCov(m_conversion_mask,mix.gaussians[i],ic);

      Eigen::EigenSolver<Eigen::Matrix3f> evsolver(covmat);

      Eigen::Matrix3f eigenvectors = evsolver.eigenvectors().real();
      if (eigenvectors.determinant() < 0.0)
        eigenvectors.col(0) = - eigenvectors.col(0);
      Eigen::Matrix3f rotation = eigenvectors;
      Eigen::Quaternionf quat = Eigen::Quaternionf(Eigen::AngleAxisf(rotation));

      marker.pose.orientation.x = quat.x();
      marker.pose.orientation.y = quat.y();
      marker.pose.orientation.z = quat.z();
      marker.pose.orientation.w = quat.w();

      Eigen::Vector3f eigenvalues = evsolver.eigenvalues().real();
      Eigen::Vector3f scale = Eigen::Vector3f(eigenvalues.array().abs().sqrt());
      if (m_normalize)
        scale.normalize();
      marker.scale.x = mix.weights[i] * scale.x() * m_scale;
      marker.scale.y = mix.weights[i] * scale.y() * m_scale;
      marker.scale.z = mix.weights[i] * scale.z() * m_scale;

      marker.color.a = 1.0;
      rainbow(float(i) / float(mix.gaussians.size()),marker.color.r,marker.color.g,marker.color.b);

      msg.markers.push_back(marker);
    }

    // this a waste of resources, but we need to delete old markers in some way
    // someone should add a "clear all" command to rviz
    // (using expiration time is not an option, here)
    for ( ; i < m_max_markers; i++)
    {
      visualization_msgs::Marker marker;
      marker.id = i;
      marker.action = visualization_msgs::Marker::DELETE;
      marker.lifetime = ros::Duration();
      marker.header.frame_id = m_frame_id;
      marker.header.stamp = ros::Time::now();
      marker.ns = m_rviz_namespace;
      msg.markers.push_back(marker);
    }

    m_pub.publish(msg);
    ROS_INFO("gmm_rviz_converter: Sent message.");
  }
Пример #18
0
int
main(int argc, char** argv)
{
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZRGB>);

	if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("../learn15.pcd", *cloud) == -1) //* load the file
	{
		PCL_ERROR("Couldn't read file test_pcd.pcd \n");
		return (-1);
	}

	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
	// Create the segmentation object
	pcl::SACSegmentation<pcl::PointXYZRGB> seg;
	// Optional
	seg.setOptimizeCoefficients(true);
	// Mandatory
	seg.setModelType(pcl::SACMODEL_PLANE);
	seg.setMethodType(pcl::SAC_RANSAC);
	seg.setDistanceThreshold(0.01);
	seg.setInputCloud(cloud);
	seg.segment(*inliers, *coefficients);

	if (inliers->indices.size() == 0)
	{
		PCL_ERROR("Could not estimate a planar model for the given dataset.");
		return (-1);
	}

	std::cerr << "Model coefficients: " << coefficients->values[0] << " "
		<< coefficients->values[1] << " "
		<< coefficients->values[2] << " "
		<< coefficients->values[3] << std::endl;

	pcl::ExtractIndices<pcl::PointXYZRGB> extract;
	extract.setInputCloud(cloud);
	extract.setIndices(inliers);
	extract.setNegative(true);
	//Removes part_of_cloud but retain the original full_cloud
	//extract.setNegative(true); // Removes part_of_cloud from full cloud  and keep the rest
	extract.filter(*cloud_plane);

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
	sor.setInputCloud(cloud_plane);
	sor.setMeanK(50);
	sor.setStddevMulThresh(1.0);
	sor.filter(*cloud_filtered);

	//cloud.swap(cloud_plane);
	/*
	pcl::visualization::CloudViewer viewer("Simple Cloud Viewer");
	viewer.showCloud(cloud_plane);
	while (!viewer.wasStopped())
	{
	}
	*/
	Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
	Eigen::Matrix<float, 1, 3> fitted_plane_norm, xyaxis_plane_norm, rotation_axis;
	fitted_plane_norm[0] = coefficients->values[0];
	fitted_plane_norm[1] = coefficients->values[1];
	fitted_plane_norm[2] = coefficients->values[2];
	xyaxis_plane_norm[0] = 0.0;
	xyaxis_plane_norm[1] = 0.0;
	xyaxis_plane_norm[2] = 1.0;
	rotation_axis = xyaxis_plane_norm.cross(fitted_plane_norm);
	float theta = acos(xyaxis_plane_norm.dot(fitted_plane_norm)); 
	//float theta = -atan2(rotation_axis.norm(), xyaxis_plane_norm.dot(fitted_plane_norm));
	transform_2.rotate(Eigen::AngleAxisf(theta, rotation_axis));
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_recentered(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::transformPointCloud(*cloud_filtered, *cloud_recentered, transform_2);

	
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>); 
	pcl::copyPointCloud(*cloud_recentered, *cloud_xyz);

	///////////////////////////////////////////////////////////////////
	Eigen::Vector4f pcaCentroid;
	pcl::compute3DCentroid(*cloud_xyz, pcaCentroid);
 	Eigen::Matrix3f covariance;
	
	computeCovarianceMatrixNormalized(*cloud_xyz, pcaCentroid, covariance);
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
	Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();
	std::cout << eigenVectorsPCA.size() << std::endl;
	if(eigenVectorsPCA.size()!=9)
	eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1));


	
	Eigen::Matrix4f projectionTransform(Eigen::Matrix4f::Identity());
	projectionTransform.block<3, 3>(0, 0) = eigenVectorsPCA.transpose();
	projectionTransform.block<3, 1>(0, 3) = -1.f * (projectionTransform.block<3, 3>(0, 0) * pcaCentroid.head<3>());
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPointsProjected(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::transformPointCloud(*cloud_xyz, *cloudPointsProjected, projectionTransform);
	// Get the minimum and maximum points of the transformed cloud.
	pcl::PointXYZ minPoint, maxPoint;
	pcl::getMinMax3D(*cloudPointsProjected, minPoint, maxPoint);
	const Eigen::Vector3f meanDiagonal = 0.5f*(maxPoint.getVector3fMap() + minPoint.getVector3fMap());



	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
	viewer = rgbVis(cloud);
	// viewer->addPointCloud<pcl::PointXYZ>(cloudPointsProjected, "Simple Cloud");
	// viewer->addPointCloud<pcl::PointXYZRGB>(cloud, "Simple Cloud2");
	viewer->addCube(minPoint.x, maxPoint.x, minPoint.y, maxPoint.y, minPoint.z , maxPoint.z);

	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}

	
	/*
	pcl::PCA< pcl::PointXYZ > pca;
	pcl::PointCloud< pcl::PointXYZ >::ConstPtr cloud;
	pcl::PointCloud< pcl::PointXYZ > proj;

	pca.setInputCloud(cloud);
	pca.project(*cloud, proj);

	Eigen::Vector4f proj_min;
	Eigen::Vector4f proj_max;
	pcl::getMinMax3D(proj, proj_min, proj_max);

	pcl::PointXYZ min;
	pcl::PointXYZ max;
	pca.reconstruct(proj_min, min);
	pca.reconstruct(proj_max, max);

	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
	viewer->addCube(min.x, max.x, min.y, max.y, min.z, max.z);

	*/


	return (0);

}