pcl::PointCloud<pcl::PointXYZRGB>::Ptr RegionGrowingHSV::getColoredCloud()
{
// 随机得到颜色集合
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb(new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::fromROSMsg(m_polymesh.cloud,*cloud_rgb);
srand(static_cast<unsigned int> (time(0)));
std::vector<unsigned char> colors;
for(size_t i_segment = 0;i_segment<clusters_.size();i_segment++ )
{
colors.push_back (static_cast<unsigned char> ((rand ()+45) % 256));
colors.push_back (static_cast<unsigned char> ((rand ()+23) % 256));
colors.push_back (static_cast<unsigned char> (rand () % 256));
}
std::vector<std::vector<int> >::iterator i_segment;
int next_color = 0;
for( i_segment = clusters_.begin();i_segment != clusters_.end(); i_segment++ )
{
std::vector<int>::iterator i_point;
for( i_point = (*i_segment).begin();i_point != (*i_segment).end();i_point++)
{
int index = *i_point;
cloud_rgb->points[index].r = colors[3*next_color];
cloud_rgb->points[index].g = colors[3*next_color+1];
cloud_rgb->points[index].b = colors[3*next_color+2];
}
next_color++;
}
return cloud_rgb;
}
void RegionGrowingHSV::setInputmesh(pcl::PolygonMesh& mesh)
{
m_polymesh = mesh;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb(new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::fromROSMsg(m_polymesh.cloud,*cloud_rgb);
num_pts = cloud_rgb->points.size();
cloud_hsv_.resize(num_pts);
is_seed_.resize(num_pts,false);
// FILE *file;
// file = fopen("D:\\test.txt","w");
for(int i=0;i<num_pts;i++)
{
cloud_hsv_[i] = RGBToHSV(cloud_rgb->points[i].r,cloud_rgb->points[i].g,cloud_rgb->points[i].b);
//std::cout<<i<<" "<<cloud_hsv_[i]<<std::endl;
// fprintf(file,"%f\n",cloud_hsv_[i]);
}
// fclose(file);
}
/**
* texture function
*
* This function colors the 3D model returned by the mergePointClouds function
*
* @param point cloud mesh
* @param std::vector<Frame3D> frames
*/
void Functions3D::texture(pcl::PolygonMesh mesh, std::vector<Frame3D> frames, std::string filename)
{
// load values from the mesh
auto polygons = mesh.polygons;
// create cloud to save the points in
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());;
// create clouds to add the rgb points to
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb(new pcl::PointCloud<pcl::PointXYZRGB>());;
// copy mesh cloud to both placeholders
pcl::fromPCLPointCloud2(mesh.cloud, *cloud);
pcl::fromPCLPointCloud2(mesh.cloud, *cloud_rgb);
// create texture mapping variable
auto texture_mapping = new pcl::TextureMapping<pcl::PointXYZ>();
// save uv coordinates
Eigen::Vector2f uv_coordinates;
/**
* Loop over all frames in the vector
*/
for (int i = frames.size()-1; i>=0; i--)
{
// Save reference instead of copy
const Frame3D ¤t_frame = frames[i];
// get data from the frame
auto depth = current_frame.depth_image_;
auto focal = current_frame.focal_length_;
auto depth_map_size = depth.size();
auto rgb_image = current_frame.rgb_image_;
// save and resize rgb image
cv::Mat rgb_img;
cv::resize(rgb_image, rgb_img, depth_map_size);
// inverse the camera pose
auto camera_pose = current_frame.getEigenTransform();
camera_pose(3,3) = 1;
auto inv_camera = camera_pose.inverse().eval();
// create texture mapping camera
pcl::texture_mapping::Camera camera;
camera.focal_length = focal;
camera.height = rgb_img.size().height;
camera.width = rgb_img.size().width;
// reverse the point cloud transformation
pcl::PointCloud<pcl::PointXYZ>::Ptr trans_cloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::transformPointCloud(*cloud, *trans_cloud, inv_camera);
// create octree for texture mapping
pcl::TextureMapping<pcl::PointXYZ>::Octree::Ptr octree(new pcl::TextureMapping<pcl::PointXYZ>::Octree(0.01));
octree->setInputCloud(trans_cloud->makeShared());
octree->addPointsFromInputCloud();
/**
* Loop over all polygons
*/
for (pcl::Vertices polygon : polygons)
{
/**
* Loop over all vertices in the polygon
*/
for (uint32_t vertex : polygon.vertices)
{
// check if the point is occluded
auto vertex_point = trans_cloud->points.at(vertex);
if (!texture_mapping->isPointOccluded(vertex_point, octree))
{
// if it is not, get the uv coordinates
if (texture_mapping->getPointUVCoordinates(vertex_point, camera, uv_coordinates))
{
// get the coordinates
int x = round(uv_coordinates.x() * camera.width);
int y = round(camera.height - uv_coordinates.y() * camera.height);
float z = depth.at<ushort>(y, x) * 0.001;
// if they have the correct depth, save them
if (z < 1.2)
{
auto pixel = rgb_img.at<cv::Vec3b>(cv::Point(x, y));
cloud_rgb->points.at(vertex).b = pixel[0];
cloud_rgb->points.at(vertex).g = pixel[1];
cloud_rgb->points.at(vertex).r = pixel[2];
}
}
}
}
}
}
// set the rgb cloud as point cloud for the mesh
pcl::toPCLPointCloud2(*cloud_rgb, mesh.cloud);
// save the mesh to a file
pcl::io::saveVTKFile("../data/" + filename, mesh);
}
int ExtractSIFT::compute()
{
ccPointCloud* cloud = getSelectedEntityAsCCPointCloud();
if (!cloud)
return -1;
PCLCloud::Ptr sm_cloud (new PCLCloud);
std::vector<std::string> req_fields;
req_fields.resize(2);
req_fields[0] = "xyz"; // always needed
switch (m_mode)
{
case RGB:
req_fields[1] = "rgb";
break;
case SCALAR_FIELD:
req_fields[1] = m_field_to_use;
break;
}
cc2smReader converter;
converter.setInputCloud(cloud);
converter.getAsSM(req_fields, *sm_cloud);
//Now change the name of the field to use to a standard name, only if in OTHER_FIELD mode
if (m_mode == SCALAR_FIELD)
{
int field_index = pcl::getFieldIndex(*sm_cloud, m_field_to_use_no_space);
sm_cloud->fields.at(field_index).name = "intensity"; //we always use intensity as name... even if it is curvature or another field.
}
//initialize all possible clouds
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_i (new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud (new pcl::PointCloud<pcl::PointXYZ>);
//Now do the actual computation
if (m_mode == SCALAR_FIELD)
{
FROM_PCL_CLOUD(*sm_cloud, *cloud_i);
estimateSIFT<pcl::PointXYZI, pcl::PointXYZ>(cloud_i, out_cloud, m_nr_octaves, m_min_scale, m_nr_scales_per_octave, m_min_contrast );
}
else if (m_mode == RGB)
{
FROM_PCL_CLOUD(*sm_cloud, *cloud_rgb);
estimateSIFT<pcl::PointXYZRGB, pcl::PointXYZ>(cloud_rgb, out_cloud, m_nr_octaves, m_min_scale, m_nr_scales_per_octave, m_min_contrast );
}
PCLCloud::Ptr out_cloud_sm (new PCLCloud);
TO_PCL_CLOUD(*out_cloud, *out_cloud_sm);
if ( (out_cloud_sm->height * out_cloud_sm->width) == 0)
{
//cloud is empty
return -53;
}
ccPointCloud* out_cloud_cc = sm2ccConverter(out_cloud_sm).getCCloud();
if (!out_cloud_cc)
{
//conversion failed (not enough memory?)
return -1;
}
std::stringstream name;
if (m_mode == RGB)
name << "SIFT Keypoints_" << m_nr_octaves << "_" << "rgb" << "_" << m_min_scale << "_" << m_nr_scales_per_octave << "_" << m_min_contrast;
else
name << "SIFT Keypoints_" << m_nr_octaves << "_" << m_field_to_use_no_space << "_" << m_min_scale << "_" << m_nr_scales_per_octave << "_" << m_min_contrast;
out_cloud_cc->setName(name.str().c_str());
out_cloud_cc->setDisplay(cloud->getDisplay());
if (cloud->getParent())
cloud->getParent()->addChild(out_cloud_cc);
emit newEntity(out_cloud_cc);
return 1;
}
int ExtractSIFT::compute()
{
ccPointCloud* cloud = getSelectedEntityAsCCPointCloud();
if (!cloud)
return -1;
std::list<std::string> req_fields;
try
{
req_fields.push_back("xyz"); // always needed
switch (m_mode)
{
case RGB:
req_fields.push_back("rgb");
break;
case SCALAR_FIELD:
req_fields.push_back(qPrintable(m_field_to_use)); //DGM: warning, toStdString doesn't preserve "local" characters
break;
default:
assert(false);
break;
}
}
catch (const std::bad_alloc&)
{
//not enough memory
return -1;
}
PCLCloud::Ptr sm_cloud = cc2smReader(cloud).getAsSM(req_fields);
if (!sm_cloud)
return -1;
//Now change the name of the field to use to a standard name, only if in OTHER_FIELD mode
if (m_mode == SCALAR_FIELD)
{
int field_index = pcl::getFieldIndex(*sm_cloud, m_field_to_use_no_space);
sm_cloud->fields.at(field_index).name = "intensity"; //we always use intensity as name... even if it is curvature or another field.
}
//initialize all possible clouds
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_i (new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud (new pcl::PointCloud<pcl::PointXYZ>);
//Now do the actual computation
if (m_mode == SCALAR_FIELD)
{
FROM_PCL_CLOUD(*sm_cloud, *cloud_i);
estimateSIFT<pcl::PointXYZI, pcl::PointXYZ>(cloud_i, out_cloud, m_nr_octaves, m_min_scale, m_nr_scales_per_octave, m_min_contrast );
}
else if (m_mode == RGB)
{
FROM_PCL_CLOUD(*sm_cloud, *cloud_rgb);
estimateSIFT<pcl::PointXYZRGB, pcl::PointXYZ>(cloud_rgb, out_cloud, m_nr_octaves, m_min_scale, m_nr_scales_per_octave, m_min_contrast );
}
PCLCloud::Ptr out_cloud_sm (new PCLCloud);
TO_PCL_CLOUD(*out_cloud, *out_cloud_sm);
if ( out_cloud_sm->height * out_cloud_sm->width == 0)
{
//cloud is empty
return -53;
}
ccPointCloud* out_cloud_cc = sm2ccConverter(out_cloud_sm).getCloud();
if (!out_cloud_cc)
{
//conversion failed (not enough memory?)
return -1;
}
std::stringstream name;
if (m_mode == RGB)
name << "SIFT Keypoints_" << m_nr_octaves << "_" << "rgb" << "_" << m_min_scale << "_" << m_nr_scales_per_octave << "_" << m_min_contrast;
else
name << "SIFT Keypoints_" << m_nr_octaves << "_" << m_field_to_use_no_space << "_" << m_min_scale << "_" << m_nr_scales_per_octave << "_" << m_min_contrast;
out_cloud_cc->setName(name.str().c_str());
out_cloud_cc->setDisplay(cloud->getDisplay());
//copy global shift & scale
out_cloud_cc->setGlobalScale(cloud->getGlobalScale());
out_cloud_cc->setGlobalShift(cloud->getGlobalShift());
if (cloud->getParent())
cloud->getParent()->addChild(out_cloud_cc);
emit newEntity(out_cloud_cc);
return 1;
}
void EuclidianClusters::compute(pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud,std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& vec_clusters){
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud(pointcloud);
std::vector <pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance(2);
ec.setMinClusterSize(400);
ec.setMaxClusterSize(100000);
ec.setSearchMethod(tree);
ec.setInputCloud(pointcloud);
ec.extract(cluster_indices);
int j = 0;
int r = 200, g = 200, b = 0;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb_memory (new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<std::vector<float>> point2D;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb (new pcl::PointCloud<pcl::PointXYZRGB>);
cloud_rgb->points.resize(cloud_cluster->points.size());
int min = 50000, max = -5000;
//pcl::PointCloud<pcl::PointXYZ>::Ptr vecxyz(new pcl::PointCloud<pcl::PointXYZ>);
std::vector<float> pointF;
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit) {
cloud_cluster->points.push_back(pointcloud->points[*pit]);
pcl::PointXYZRGB p;
uint32_t rgb = (static_cast<uint32_t>(r) << 16 |
static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
p.x = pointcloud->points[*pit].x;
p.y = pointcloud->points[*pit].y;
p.z = pointcloud->points[*pit].z;
p.rgb = *reinterpret_cast<float*>(&rgb);
if(p.y<min)
min = p.y;
if(p.y > max)
max = p.y;
pointF.push_back(p.x);
pointF.push_back(p.y);
//if(p.z > biggerZ/3)
cloud_rgb->points.push_back(p);
}
point2D.push_back(pointF);
//arrayVector.push_back(vecxyz);
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_rgb->width = cloud_cluster->points.size ();
cloud_rgb->height = 1;
cloud_cluster->is_dense = true;
*cloud_rgb_memory += *cloud_rgb;
vec_clusters.push_back(cloud_rgb);
j++;
}
}
