pcl::PointCloud<pcl::PointXYZRGB>::Ptr
point_cloud_open_pcd_file(std::string file_path)
{
pcl::PointCloud <pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud <pcl::PointXYZRGB>);
if ( pcl::io::loadPCDFile <pcl::PointXYZRGB> (file_path, *cloud) != 0 )
{
std::cout << "Cloud reading failed. may be xyz cloud " << std::endl;
pcl::PointCloud <pcl::PointXYZ>::Ptr xyz_cloud (new pcl::PointCloud <pcl::PointXYZ>);
if(pcl::io::loadPCDFile<pcl::PointXYZ>(file_path, *xyz_cloud) != 0)
{
pcl::PointCloud <pcl::PointXYZRGB>::Ptr converted_cloud (new pcl::PointCloud <pcl::PointXYZRGB>);
converted_cloud = open_point_cloud_xyz_to_xyzrgb(xyz_cloud);
cloud = converted_cloud;
}
}
return cloud;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr
point_cloud_xyzrgb_to_xyz(pcl::PointCloud<pcl::PointXYZRGB>::Ptr rgb_cloud)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr xyz_cloud (new pcl::PointCloud<pcl::PointXYZ>);
//assign return cloud
#pragma omp parallel for
for (int i = 0; i < rgb_cloud->points.size(); i++)
{
pcl::PointXYZ point;
//temporal point
point.x = rgb_cloud->points[i].x;
point.y = rgb_cloud->points[i].y;
point.z = rgb_cloud->points[i].z;
#pragma omp critical(dataupdate)
{
xyz_cloud->points.push_back(point);
}
}
xyz_cloud->width = (int) rgb_cloud->points.size();
xyz_cloud->height = 1;
return xyz_cloud;
}
void PolygonPointsSampler::sample(
const jsk_recognition_msgs::PolygonArray::ConstPtr& polygon_msg,
const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients_msg)
{
boost::mutex::scoped_lock lock(mutex_);
vital_checker_->poke();
// check frame_ids
if (!isValidMessage(polygon_msg, coefficients_msg)) {
return;
}
// Sample points...
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::PointCloud<pcl::PointXYZ>::Ptr xyz_cloud (new pcl::PointCloud<pcl::PointXYZ>);
for (size_t i = 0; i < polygon_msg->polygons.size(); i++) {
jsk_recognition_utils::Polygon polygon
= jsk_recognition_utils::Polygon::fromROSMsg(polygon_msg->polygons[i].polygon);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr one_cloud
= polygon.samplePoints<pcl::PointXYZRGBNormal>(grid_size_);
pcl::PointCloud<pcl::PointXYZ> one_xyz_cloud;
one_xyz_cloud.points.resize(one_cloud->points.size());
for (size_t i = 0; i < one_cloud->points.size(); i++) {
pcl::PointXYZ p;
p.getVector3fMap() = one_cloud->points[i].getVector3fMap();
one_xyz_cloud.points[i] = p;
}
*xyz_cloud = *xyz_cloud + one_xyz_cloud;
*cloud = *cloud + *one_cloud;
}
sensor_msgs::PointCloud2 ros_cloud;
pcl::toROSMsg(*cloud, ros_cloud);
ros_cloud.header = polygon_msg->header;
pub_.publish(ros_cloud);
sensor_msgs::PointCloud2 ros_xyz_cloud;
pcl::toROSMsg(*xyz_cloud, ros_xyz_cloud);
ros_xyz_cloud.header = polygon_msg->header;
pub_xyz_.publish(ros_xyz_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));
}
void MKinect::update(){
QImage actualFrame;
QTime updateTime;
updateTime.start();
getKinectData(actualFrame);
++_actual_frame;
renderer->initTexture(actualFrame);
if (_tracker_set) {
bool orientationIsValid = false;
int SIZE = KinectColorWidth*KinectColorHeight;
std::vector<PXCPoint3DF32> vertices(SIZE);
CloudType::Ptr xyz_cloud(new CloudType(KinectColorWidth, KinectColorHeight));
int validP = 0;
for (int i = 0; i < SIZE; i++) {
if (convertPoint(depth2xyz[i], xyz_cloud->points[i])) {
++validP;
}
}
if (validP != 0) {//Then DO SOMETHING
if (_tracker_set == false) { //Use built-in algorithm
QTime algorithmTime;
algorithmTime.start();
orientationIsValid = track();
int elapsed = algorithmTime.elapsed();
++_actual_frame;
app->addIncrementalTimeMean(elapsed);
}
else { //Use algorithm in tracker and filters
double radius_search = tracker->getParameter("radius_search").toDouble();
bool subsampling_none = tracker->getParameter("subsample") == "none";
bool subsampling_uniform = tracker->getParameter("subsample") == "uniform";
bool subsampling_random = tracker->getParameter("subsample") == "random";
QTime algorithmTime;
algorithmTime.start();
if (!_last_init) {
_last_init = true;
//pcl::copyPointCloud(*xyz_cloud, *last_cloud);
last_cloud->is_dense = false;
first_cloud->is_dense = false;
xyz_cloud->is_dense = false;
std::vector<int> indices2;
CloudType::Ptr cloud_filtered(new CloudType);
pcl::removeNaNFromPointCloud(*xyz_cloud, *cloud_filtered, indices2);
//Pass cloud to renderer (Or pass the filtered one to see differences)
if (subsampling_uniform) {
pcl::PointCloud<int> indicesIN;
uniform_sampling.setInputCloud(cloud_filtered);
uniform_sampling.setRadiusSearch(radius_search);
uniform_sampling.compute(indicesIN);
//pcl::copyPointCloud(*cloud_filtered, indicesIN.points, *first_cloud);
pcl::copyPointCloud(*cloud_filtered, indicesIN.points, *first_cloud);
pcl::copyPointCloud(*cloud_filtered, indicesIN.points, *last_cloud);
}
else if (subsampling_none) {
pcl::copyPointCloud(*cloud_filtered, *first_cloud);
pcl::copyPointCloud(*cloud_filtered, *last_cloud);
}
else if (subsampling_random) {
pcl::copyPointCloud(*cloud_filtered, *first_cloud);
pcl::copyPointCloud(*cloud_filtered, *last_cloud);
}
//Save in first_cloud the first frame, filtered and prepared :D
//pcl::io::savePCDFileASCII("../clouds/rs/init_cloud.pcd", *cloud_filtered);
}
//Filter NANS for ICP
xyz_cloud->is_dense = false;
std::vector<int> indices;
CloudType::Ptr _cloud1(new CloudType);
pcl::removeNaNFromPointCloud(*xyz_cloud, *_cloud1, indices);
//Filter points using filter object
if (subsampling_uniform) {
QTime filterTime;
filterTime.start();
pcl::PointCloud<int> indicesOUT;
uniform_sampling.setInputCloud(_cloud1);
uniform_sampling.setRadiusSearch(radius_search);
uniform_sampling.compute(indicesOUT);
pcl::copyPointCloud(*_cloud1.get(), indicesOUT.points, *actual_cloud);
++_actual_frame;
//pcl::io::savePCDFileASCII("../clouds/rs/actual_cloud_" + QString::number(_actual_frame).toStdString() + ".pcd", *actual_cloud);
//pcl::copyPointCloud(*_cloud1.get(), *actual_cloud);
int elapsedF = filterTime.elapsed();
//Save frame info to file for DOCUMENTATION
//QFile file("../"+QString::number(radius_search)+"_uniform_info.txt");
//if (file.open(QIODevice::WriteOnly | QIODevice::Append)) {
// QTextStream stream(&file);
// stream << radius_search << " " << _cloud1->size() - actual_cloud->size() << " " << elapsedF << endl;
//}
}
else if (subsampling_none) {
pcl::copyPointCloud(*_cloud1.get(), *actual_cloud);
}
else if (subsampling_random) {
pcl::copyPointCloud(*_cloud1.get(), *actual_cloud);
}
//Pass cloud to algorithm
if (_last_init) { //Compute if we have 2 frames (last_cloud is filled with the first frame)
app->setPointsAnalyzed(actual_cloud->size(), first_cloud->size());
if (actual_cloud->size() > 0) {
}
double fitness;
orientationIsValid = tracker->compute(*last_cloud.get(), *actual_cloud.get(), _roll, _pitch, _yaw, fitness);
app->setICPConverged(orientationIsValid, fitness);
}
int elapsed = algorithmTime.elapsed();
app->addIncrementalTimeMean(elapsed);
}
renderer->setFaceTracked(orientationIsValid);
app->setFaceTracked(orientationIsValid);
if (orientationIsValid) {
app->setFaceAngles(_yaw, _pitch, _roll);
}
}
}
else {
QTime algorithmTime;
algorithmTime.start();
track();
int elapsed = algorithmTime.elapsed();
app->addIncrementalTimeMean(elapsed);
}
int elapsedUpdate = updateTime.elapsed();
app->setFilterTime(elapsedUpdate);
app->setFaceAngles(_yaw, _pitch, _roll);
}
