pcl::PointCloud<pcl::PointXYZRGB>::Ptr
open_point_cloud_xyz_to_xyzrgb(pcl::PointCloud<pcl::PointXYZ>::Ptr xyz_cloud)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr rgb_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
//assign return cloud
#pragma omp parallel for
for (int i = 0; i < xyz_cloud->points.size(); i++)
{
pcl::PointXYZRGB point;
//temporal point
point.x = rgb_cloud->points[i].x;
point.y = rgb_cloud->points[i].y;
point.z = rgb_cloud->points[i].z;
point.r = 255;
point.g = 255;
point.b = 255;
#pragma omp critical(dataupdate)
{
rgb_cloud->points.push_back(point);
}
}
rgb_cloud->width = (int) xyz_cloud->points.size();
rgb_cloud->height = 1;
return rgb_cloud;
}
void AddColorFromImage::addColor(
const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
const sensor_msgs::Image::ConstPtr& image_msg,
const sensor_msgs::CameraInfo::ConstPtr& info_msg)
{
if ((cloud_msg->header.frame_id != image_msg->header.frame_id) ||
(cloud_msg->header.frame_id != info_msg->header.frame_id)) {
JSK_NODELET_FATAL("frame_id is not collect: [%s, %s, %s",
cloud_msg->header.frame_id.c_str(),
image_msg->header.frame_id.c_str(),
info_msg->header.frame_id.c_str());
return;
}
vital_checker_->poke();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*cloud_msg, *cloud);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr rgb_cloud
(new pcl::PointCloud<pcl::PointXYZRGB>);
rgb_cloud->points.resize(cloud->points.size());
rgb_cloud->is_dense = cloud->is_dense;
rgb_cloud->width = cloud->width;
rgb_cloud->height = cloud->height;
cv::Mat image = cv_bridge::toCvCopy(
image_msg, sensor_msgs::image_encodings::BGR8)->image;
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(info_msg);
for (size_t i = 0; i < cloud->points.size(); i++) {
pcl::PointXYZRGB p;
p.x = cloud->points[i].x;
p.y = cloud->points[i].y;
p.z = cloud->points[i].z;
//p.getVector3fMap() = cloud->points[i].getVector3fMap();
if (!isnan(p.x) && !isnan(p.y) && !isnan(p.z)) {
// compute RGB
cv::Point2d uv = model.project3dToPixel(cv::Point3d(p.x, p.y, p.z));
if (uv.x > 0 && uv.x < image_msg->width &&
uv.y > 0 && uv.y < image_msg->height) {
cv::Vec3b rgb = image.at<cv::Vec3b>(uv.y, uv.x);
p.r = rgb[2];
p.g = rgb[1];
p.b = rgb[0];
}
}
rgb_cloud->points[i] = p;
}
sensor_msgs::PointCloud2 ros_cloud;
pcl::toROSMsg(*rgb_cloud, ros_cloud);
ros_cloud.header = cloud_msg->header;
pub_.publish(ros_cloud);
}
int
pcl::io::vtk2mesh (const vtkSmartPointer<vtkPolyData>& poly_data, pcl::PolygonMesh& mesh)
{
mesh.polygons.resize (0);
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);
// First get the xyz information
pcl::PointCloud<pcl::PointXYZ>::Ptr xyz_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
xyz_cloud->points.resize (nr_points);
xyz_cloud->width = static_cast<uint32_t> (xyz_cloud->points.size ());
xyz_cloud->height = 1;
xyz_cloud->is_dense = true;
double point_xyz[3];
for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints (); i++)
{
mesh_points->GetPoint (i, &point_xyz[0]);
xyz_cloud->points[i].x = static_cast<float> (point_xyz[0]);
xyz_cloud->points[i].y = static_cast<float> (point_xyz[1]);
xyz_cloud->points[i].z = static_cast<float> (point_xyz[2]);
}
// And put it in the mesh cloud
pcl::toPCLPointCloud2 (*xyz_cloud, mesh.cloud);
// Then the color information, if any
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_data->GetPointData () != NULL)
poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("scalars"));
if (!poly_colors && poly_data->GetPointData () != NULL)
poly_colors = vtkUnsignedCharArray::SafeDownCast (poly_data->GetPointData ()->GetScalars ("RGB"));
// TODO: currently only handles rgb values with 3 components
if (poly_colors && (poly_colors->GetNumberOfComponents () == 3))
{
pcl::PointCloud<pcl::RGB>::Ptr rgb_cloud (new pcl::PointCloud<pcl::RGB> ());
rgb_cloud->points.resize (nr_points);
rgb_cloud->width = static_cast<uint32_t> (rgb_cloud->points.size ());
rgb_cloud->height = 1;
rgb_cloud->is_dense = true;
unsigned char point_color[3];
for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints (); i++)
{
poly_colors->GetTupleValue (i, &point_color[0]);
rgb_cloud->points[i].r = point_color[0];
rgb_cloud->points[i].g = point_color[1];
rgb_cloud->points[i].b = point_color[2];
}
pcl::PCLPointCloud2 rgb_cloud2;
pcl::toPCLPointCloud2 (*rgb_cloud, rgb_cloud2);
pcl::PCLPointCloud2 aux;
pcl::concatenateFields (rgb_cloud2, mesh.cloud, aux);
mesh.cloud = aux;
}
// Then handle the normals, if any
vtkFloatArray* normals = NULL;
if (poly_data->GetPointData () != NULL)
normals = vtkFloatArray::SafeDownCast (poly_data->GetPointData ()->GetNormals ());
if (normals != NULL)
{
pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal> ());
normal_cloud->resize (nr_points);
normal_cloud->width = static_cast<uint32_t> (xyz_cloud->points.size ());
normal_cloud->height = 1;
normal_cloud->is_dense = true;
for (vtkIdType i = 0; i < mesh_points->GetNumberOfPoints (); i++)
{
float normal[3];
normals->GetTupleValue (i, normal);
normal_cloud->points[i].normal_x = normal[0];
normal_cloud->points[i].normal_y = normal[1];
normal_cloud->points[i].normal_z = normal[2];
}
pcl::PCLPointCloud2 normal_cloud2;
pcl::toPCLPointCloud2 (*normal_cloud, normal_cloud2);
pcl::PCLPointCloud2 aux;
pcl::concatenateFields (normal_cloud2, mesh.cloud, aux);
mesh.cloud = aux;
}
// Now handle the polygons
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<int> (cell_points[i]);
++id_poly;
}
return (static_cast<int> (nr_points));
}
