//sample cylinder
void sampleCylinder(Point cenPoint0, Point cenPoint1, Eigen::Vector3f& direction, float r, float grid_length, MyPointCloud& mpt){
float height=sqrt(pow(cenPoint0.x-cenPoint1.x, 2)+pow(cenPoint0.y-cenPoint1.y, 2)+pow(cenPoint0.z-cenPoint1.z, 2));
int num_cir=(int)((2*PII*r)/grid_length);
int num_h=(int)(height/grid_length);
Eigen::Vector3d direction_normal;
direction_normal << direction[0], direction[1], direction[2];
direction_normal.normalize();
double angle0=acos(direction_normal.dot(Eigen::Vector3d(0,0,1)));
Eigen::Vector3d axis0=direction_normal.cross(Eigen::Vector3d(0,0,1));
axis0.normalize();
Eigen::Matrix4d matrix0;
getRotationMatrix(axis0, angle0, matrix0);
PointCloud cloud;
cloud.push_back(cenPoint0);
cloud.push_back(cenPoint1);
PointCloudPtr cloud_up(new PointCloud);
PointCloudPtr cloud_temp(new PointCloud);
Eigen::Matrix4f matrix_transform0 = matrix0.cast<float>();
pcl::copyPointCloud(cloud,*cloud_temp);
pcl::transformPointCloud (*cloud_temp, cloud, matrix_transform0);
Point firstPoint0(cloud.at(0).x,cloud.at(0).y+r,cloud.at(0).z);
Point firstPoint1(cloud.at(1).x,cloud.at(1).y+r,cloud.at(1).z);
float ang=grid_length/r;
int pos_neg_flag=1;
if(firstPoint0.z>firstPoint1.z){
pos_neg_flag=-1;
}
for(int i=0;i<num_cir;i++){
float new_x=0;
float new_y=0;
rotatePoint2ByPoint2(firstPoint0.x,firstPoint0.y,cloud.at(0).x,cloud.at(0).y,ang*i,&new_x,&new_y);
for(int j=0;j<num_h;j++){
cloud_up->push_back(Point(new_x,new_y,cloud.at(0).z+pos_neg_flag*(j+1)*grid_length));
}
}
Eigen::Matrix4d matrix1;
getRotationMatrix(axis0, -angle0, matrix1);
Eigen::Matrix4f matrix_transform1 = matrix1.cast<float>();
pcl::copyPointCloud(*cloud_up,*cloud_temp);
pcl::transformPointCloud (*cloud_temp, *cloud_up, matrix_transform1);
PointCloud2MyPointCloud(cloud_up, mpt);
}
void DenseReconstruction::planeSegmentation(const pcl::PointCloud<pcl::PointXYZLRegionF>::Ptr &cloud,
pcl::ModelCoefficients &coefficients, pcl::PointIndices &inliers) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud,*cloud_temp);
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(1000);
seg.setDistanceThreshold(0.01);
seg.setInputCloud(cloud_temp);
seg.segment(inliers, coefficients);
}
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;
}
int
pcl::VTKUtils::vtk2mesh (const vtkSmartPointer<vtkPolyData>& poly_data, pcl::PolygonMesh& mesh)
{
mesh.polygons.clear ();
mesh.cloud.data.clear ();
mesh.cloud.width = mesh.cloud.height = 0;
mesh.cloud.is_dense = true;
vtkSmartPointer<vtkPoints> mesh_points = poly_data->GetPoints ();
vtkIdType nr_points = mesh_points->GetNumberOfPoints ();
vtkIdType nr_polygons = poly_data->GetNumberOfPolys ();
if (nr_points == 0)
return 0;
vtkUnsignedCharArray* poly_colors = NULL;
if (poly_data->GetPointData() != NULL)
poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("Colors"));
// Some applications do not save the name of scalars (including PCL's native vtk_io)
if (!poly_colors)
poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("scalars"));
// TODO: currently only handles rgb values with 3 components
if (poly_colors && (poly_colors->GetNumberOfComponents () == 3))
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_temp (new pcl::PointCloud<pcl::PointXYZRGB>());
cloud_temp->points.resize (nr_points);
double point_xyz[3];
unsigned char point_color[3];
for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints (); ++i)
{
mesh_points->GetPoint (i, &point_xyz[0]);
cloud_temp->points[i].x = static_cast<float> (point_xyz[0]);
cloud_temp->points[i].y = static_cast<float> (point_xyz[1]);
cloud_temp->points[i].z = static_cast<float> (point_xyz[2]);
poly_colors->GetTupleValue (i, &point_color[0]);
cloud_temp->points[i].r = point_color[0];
cloud_temp->points[i].g = point_color[1];
cloud_temp->points[i].b = point_color[2];
}
cloud_temp->width = static_cast<uint32_t> (cloud_temp->points.size ());
cloud_temp->height = 1;
cloud_temp->is_dense = true;
pcl::toROSMsg (*cloud_temp, mesh.cloud);
}
else // in case points do not have color information:
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp (new pcl::PointCloud<pcl::PointXYZ> ());
cloud_temp->points.resize (nr_points);
double point_xyz[3];
for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints (); ++i)
{
mesh_points->GetPoint (i, &point_xyz[0]);
cloud_temp->points[i].x = static_cast<float> (point_xyz[0]);
cloud_temp->points[i].y = static_cast<float> (point_xyz[1]);
cloud_temp->points[i].z = static_cast<float> (point_xyz[2]);
}
cloud_temp->width = static_cast<uint32_t> (cloud_temp->points.size ());
cloud_temp->height = 1;
cloud_temp->is_dense = true;
pcl::toROSMsg (*cloud_temp, mesh.cloud);
}
mesh.polygons.resize (nr_polygons);
vtkIdType* cell_points;
vtkIdType nr_cell_points;
vtkCellArray * mesh_polygons = poly_data->GetPolys ();
mesh_polygons->InitTraversal ();
int id_poly = 0;
while (mesh_polygons->GetNextCell (nr_cell_points, cell_points))
{
mesh.polygons[id_poly].vertices.resize (nr_cell_points);
for (int i = 0; i < nr_cell_points; ++i)
mesh.polygons[id_poly].vertices[i] = static_cast<unsigned int> (cell_points[i]);
++id_poly;
}
return (static_cast<int> (nr_points));
}
/*
* it has a small bug that is deleting the regions some times.
*/
void DenseReconstruction::addSideWall(std::vector<pcl::PointIndices::Ptr> &clusters_input){
for (int i=0;i<clusters_input.size();i++){
pcl::PointCloud<pcl::PointXYZLRegionF>::Ptr cloud_temp(new pcl::PointCloud<pcl::PointXYZLRegionF>);
pcl::copyPointCloud(*region_grow_point_cloud_,*clusters_input[i],*cloud_temp);
clusters_vec_point_cloud.push_back(cloud_temp);
}
for(int how_many=0;how_many<6;how_many++){
// std::vector<pcl::PointCloud<pcl::PointXYZLRegionF>::Ptr> clusters_vec_point_cloud;
std::vector<pcl::PointCloud<pcl::Normal>::Ptr> clusters_vec_normals;
std::vector<pcl::PointCloud<pcl::Boundary> > clusters_vec_boundaries;
// std::vector<pcl::PointCloud<pcl::PointXYZLRegionF>::Ptr> clusters_vec_only_boudaries;
std::vector<pcl::PointCloud<pcl::PointXYZRGBA>::Ptr> clusters_vec_temp;
clusters_vec_only_boudaries.clear();
for (int i=0;i<clusters_vec_point_cloud.size();i++){
pcl::PointCloud<pcl::Normal>::Ptr normals_temp(new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<pcl::Boundary> boundary_temp;
normalsEstimation(clusters_vec_point_cloud[i],normals_temp);
clusters_vec_normals.push_back(normals_temp);
boundaryEstimation(clusters_vec_point_cloud[i],normals_temp,boundary_temp);
clusters_vec_boundaries.push_back(boundary_temp);
}
for (int i=0;i<clusters_vec_boundaries.size();i++){
pcl::PointCloud<pcl::PointXYZLRegionF>::Ptr cloud_temp_boudary(new pcl::PointCloud<pcl::PointXYZLRegionF>);
for(int j = 0; j < clusters_vec_boundaries[i].size(); j++) {
if (clusters_vec_boundaries[i].points[j].boundary_point != 0) {
cloud_temp_boudary->push_back(clusters_vec_point_cloud[i]->points.at(j));
}
}
clusters_vec_only_boudaries.push_back(cloud_temp_boudary);
}
for (int i=0;i<clusters_vec_only_boudaries.size();i++){
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_temp(new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::copyPointCloud(*clusters_vec_only_boudaries[i],*cloud_temp);
clusters_vec_temp.push_back(cloud_temp);
}
float score_array[clusters_vec_temp.size()][clusters_vec_temp.size()];
for(int i=0;i<clusters_vec_temp.size();i++){
for(int j=0;j<clusters_vec_temp.size();j++){
score_array[i][j]=0;
}
}
for (int i=0;i<clusters_vec_temp.size();i++){
for (int k=0;k<clusters_vec_temp.size();k++){
if(k==i){
// score_array[i][k]=0;
continue;
}
for(int j=0;j<clusters_vec_temp[i]->size();j++){
pcl::PointXYZRGBA searchPointTemp=clusters_vec_temp[i]->points[j];
pcl::KdTreeFLANN<pcl::PointXYZRGBA> kdtree;
if(clusters_vec_temp[k]->size()<1)
continue;
kdtree.setInputCloud(clusters_vec_temp[k]);
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
float radius=0.005;
if (kdtree.radiusSearch(searchPointTemp, radius, pointIdxRadiusSearch,
pointRadiusSquaredDistance) > 0) {
score_array[i][k]=score_array[i][k]+1;
}
}
score_array[i][k]=score_array[i][k]/clusters_vec_temp[i]->size();
}
}
std::cout<<"Table: "<<std::endl;
for(int i=0;i<clusters_vec_temp.size();i++){
for(int j=0;j<clusters_vec_temp.size();j++){
std::cout<<score_array[i][j]<<" ";
}
std::cout<<std::endl;
}
//merging
std::vector<int> is_to_del;
for(int i=0;i<clusters_vec_temp.size();i++){
for(int j=0;j<clusters_vec_temp.size();j++){
if(score_array[i][j]>0.4){
clusters_vec_point_cloud[j]->points.insert(clusters_vec_point_cloud[j]->points.end(), clusters_vec_point_cloud[i]->points.begin(), clusters_vec_point_cloud[i]->points.end());
is_to_del.push_back(i);
}
}
}
if(is_to_del.size()>0){
std::cout<<"IS TO DEL SIZE: "<<is_to_del.size()<<std::endl;
std::sort(is_to_del.begin(), is_to_del.end());
is_to_del.erase(std::unique(is_to_del.begin(), is_to_del.end()), is_to_del.end());
}
for(int i=0;i<is_to_del.size();i++){
std::cout<<"IS TO DEL: "<<is_to_del[i]<<std::endl;
// is_to_del[i]=is_to_del[i]-i;
clusters_vec_point_cloud.erase(clusters_vec_point_cloud.begin()+is_to_del[i]);
}
}
// std::cerr<<"size of ADDSIDEWALL pcls: "<<clusters_vec_point_cloud.size()<<std::endl;
// std::cerr<<"size of ADDSIDEWALL normals: "<<clusters_vec_normals.size()<<std::endl;
}
