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;
}
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 );
}
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;
}
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));
}
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;
}
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;
}
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;
}
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;
}
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;
}
}
}
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>();
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.");
}
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);
}
