pcl::PCLPointCloud2 usage - c++

I'm confused with when to use pcl::PointCloud2 vs pcl::PointCloudPointCloud
For example, using these definitions for pcl1_ptrA, pcl1_ptrB and pcl1_ptrC:
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcl1_ptrA(new pcl::PointCloud<pcl::PointXYZRGB>); //pointer for color version of pointcloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcl1_ptrB(new pcl::PointCloud<pcl::PointXYZRGB>); //ptr to hold filtered Kinect image
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcl1_ptrC(new pcl::PointCloud<pcl::PointXYZRGB>); //ptr to hold filtered Kinect image
I can invoke the following PCL functions:
pcl::VoxelGrid<pcl::PointXYZRGB> vox;
vox.setInputCloud(pcl1_ptrA);
vox.setLeafSize(0.02f, 0.02f, 0.02f);
vox.filter(*pcl1_ptrB);
cout<<"done voxel filtering"<<endl;
cout<<"num bytes in original cloud data = "<<pcl1_ptrA->points.size()<<endl;
cout<<"num bytes in filtered cloud data = "<<pcl1_ptrB->points.size()<<endl; // ->data.size()<<endl;
Eigen::Vector4f xyz_centroid;
pcl::compute3DCentroid (*pcl1_ptrB, xyz_centroid);
float curvature;
Eigen::Vector4f plane_parameters;
pcl::computePointNormal(*pcl1_ptrB, plane_parameters, curvature); //pcl fnc to compute plane fit to point cloud
Eigen::Affine3f A(Eigen::Affine3f::Identity());
pcl::transformPointCloud(*pcl1_ptrB, *pcl1_ptrC, A);
However, if I instead use pcl::PCLPointCloud2 objects, e.g.:
pcl::PCLPointCloud2::Ptr pcl2_ptrA (new pcl::PCLPointCloud2 ());
pcl::PCLPointCloud2::Ptr pcl2_ptrB (new pcl::PCLPointCloud2 ());
pcl::PCLPointCloud2::Ptr pcl2_ptrC (new pcl::PCLPointCloud2 ());
This function works:
pcl::VoxelGrid<pcl::PCLPointCloud2> vox;
vox.setInputCloud(pcl2_ptrA);
vox.setLeafSize(0.02f, 0.02f, 0.02f);
vox.filter(*pcl2_ptrB);
But these do not even compile:
//the next 3 functions do NOT compile:
Eigen::Vector4f xyz_centroid;
pcl::compute3DCentroid (*pcl2_ptrB, xyz_centroid);
float curvature;
Eigen::Vector4f plane_parameters;
pcl::computePointNormal(*pcl2_ptrB, plane_parameters, curvature);
Eigen::Affine3f A(Eigen::Affine3f::Identity());
pcl::transformPointCloud(*pcl2_ptrB, *pcl2_ptrC, A);
I am having trouble discovering which functions accept which objects. Ideally, wouldn't all PCL functions accept pcl::PCLPointCloud2 arguments?

pcl::PCLPointCloud2 is a ROS (Robot Operating System) message type replacing the old sensors_msgs::PointCloud2. Hence, it should only be used when interacting with ROS. (see an example here)
If needed, PCL provides two functions to convert from one type to the other:
void fromPCLPointCloud2 (const pcl::PCLPointCloud2& msg, cl::PointCloud<PointT>& cloud);
void toPCLPointCloud2 (const pcl::PointCloud<PointT>& cloud, pcl::PCLPointCloud2& msg);
Extra Information
fromPCLPointCloud2 and toPCLPointCloud2 are PCL library functions for conversions. ROS has wrappers for those functions in pcl_conversions/pcl_conversions.h that you should use instead. These will call the right combinations of functions to convert between the message and templated format.

As a follow-up for what Albert said, For ROS Hydro and later (if you are using ROS), PCL has done a work to remove all its dependencies from ROS. What this means is the pcl has created a set of classes which are almost identical to the corresponding ROS messages, in order to minimize API breakage for pcl users. PointCloud users using "now depracated" sensor_msgs::PointCloud2 are now asked to use pcl_conversions package, this package implements conversions from/to sensor_msgs::PointCloud2 to/from) pcl::PointCloud and should include :
#include <pcl_conversions/pcl_conversions.h>
and the ROS code should be modified as the following :
void foo(const sensor_msgs::PointCloud2 &pc)
{
pcl::PCLPointCloud2 pcl_pc;
pcl_conversions::toPCL(pc, pcl_pc);
pcl::SomePCLFunction(pcl_pc);
...
}
Also, pcl is no longer packaged by the ROS community as a catkin package, so any packages which directly depend on pcl should instead use the new rosdep rules libpcl-all and libpcl-all-dev and follow the PCL developer's guidelines for using PCL in your CMake. Generally this means that a package.xml will change like this:
find_package(PCL REQUIRED)
...
include_directories(${PCL_INCLUDE_DIRS})
...
target_link_libraries(<YOUR_TARGET> ${PCL_LIBRARIES})
More on the migration rules can be found here, here and here.

Related

Writing / Accessing points in a PointCloud2 structure C++ / ROS / PCL Library

I am currently building a perception pipeline and am having trouble reading the points of my point cloud structure. I am reading in a point cloud into a PointCloud2 Structure and want to be able to write out the points in series of the point cloud or at the very least access them so I find a way to write them to a file later. The basic code I am working with is here:
void cloud_cropbox_cb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg)
{
// Container for original & filtered data
pcl::PCLPointCloud2 *cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl::PCLPointCloud2 cloud_filtered;
// convert given message into PCL data type
pcl_conversions::toPCL(*cloud_msg, *cloud);
I want to be able to access the x,y,z elements of each point in the cloud and ideally just write them all out to a textfile. I have tried using pcl::io::savePCDFileASCII() but I don't think that is quite applicable here. An additional note is the base program is constantly running and never exits until manually closed I don't know if that will be relavent to writing out to a file. Any help on what exact functions to use / steps I need to take would be much appreciated.
If you literally want to write out the contents of the message to a file, you can just (in launch file or commandline) use ros topic echo [args] > my_file.txt.
For ways that are compatible in C++, I recommend reading from http://wiki.ros.org/pcl_ros, which describes that:
pcl_ros extends the ROS C++ client library to support message passing with PCL native data types. Simply add the following include to your ROS node source code:
#include <pcl_ros/point_cloud.h>
This header allows you to publish and subscribe pcl::PointCloud objects as ROS messages. These appear to ROS as sensor_msgs/PointCloud2 messages, offering seamless interoperability with non-PCL-using ROS nodes. For example, you may publish a pcl::PointCloud in one of your nodes and visualize it in rviz using a Point Cloud2 display. It works by hooking into the roscpp serialization infrastructure.
This means that you can internally use PCL all you want, and it's all a ROS PointCloud2 message. But you have to declare the PCL type / interface you want to use in the message / publisher / subscriber that you access the data with. So instead of using the sensor_msgs/PointCloud2 type everywhere, use the pcl::Pointcloud<pcl::Point*> type everywhere.
Publishing:
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
ros::Publisher pub = nh.advertise<PointCloud> ("points2", 1);
PointCloud::Ptr msg (new PointCloud);
msg->points.push_back (pcl::PointXYZ(1.0, 2.0, 3.0));
pub.publish (msg);
Subscribing:
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
ros::Subscriber sub = nh.subscribe<PointCloud>("points2", 1, callback);
void callback(const PointCloud::ConstPtr& msg){
printf ("Cloud: width = %d, height = %d\n", msg->width, msg->height);
BOOST_FOREACH (const pcl::PointXYZ& pt, msg->points)
printf ("\t(%f, %f, %f)\n", pt.x, pt.y, pt.z);
}
More examples (or links to node source code) in http://wiki.ros.org/pcl_ros, tutorials on other pipeline stuff in http://wiki.ros.org/pcl_ros/Tutorials.

Drawing custom 3d shapes

I am attempting to create a custom object using Cinder C++ on windows with Visual Studio. I am hoping to find a solution that allows you to point to a object with BatchRef object, and which can be used in the same way as any other BatchRef.
I have already tried searching through the official websites tutorials and documentation, and while it does an excellent job of listing all the classes, functions, etc, it does a exceedingly slim job of covering the usage of most of those, with only the return type and arguments listed.
Ideally, we could call the custom shape something like myShape and it could be used in the following manner in my App::draw() override: (with mShader already defined someplace else)
gl::BatchRef bRef;
gl::pushModelMatrix();
bRef = gl::Batch::create( myShape() , mShader );
bRef -> draw();
gl::popModelMatrix();
If the documentation has instructions for this, feel free to point me that way. I was unable to find it, but that does not mean that it does not exist.

GICP usage in PCL

I have looked for how to implement Generalized ICP(GICP) with PCL. I found a sample code in test/registration/test_registration.cpp in PCL repository on github. The sample code uses GICP as following. Could you tell me following procedure is correct one to use GICP with PCL?
The fucntion "align" is a function of IterativeClosestPoint class. It means that "align" does not consider point-to-plane that AV Segal et al refer in their paper. I'm wondering if this is a correct procedure to use GICP with PCL. In addition, I don't know why PCL does not provide us sample codes which use estimateRigidTransformationBFGS the metod of GeneralizedIterativeClosestPoint class.
GeneralizedIterativeClosestPoint<PointT, PointT> reg_guess;
reg_guess.setInputSource (src);
reg_guess.setInputTarget (transformed_tgt);
reg_guess.setMaximumIterations (50);
reg_guess.setTransformationEpsilon (1e-8);
reg_guess.align (output, transform.matrix ());
I found the usage! Autonomous system lab in ETH Zurich opens it in their github repository. Please check robust_point_cloud_registration!

itk OtsuMultipleThresholdsImageFilter does not process

I am trying to use ITK's OtsuMultipleThresholdsImageFilter filter in a project but I do not have output.
My aim is to make a simple interface between OpenCV and ITK.
To convert my data from OpenCV's Mat container to itk::Image I use ITK's bridge to OpenCV and I could check that the data are properly sent to ITK.
I am even able to display thanks to QuickView.
But When I setup the filter inspired by this example the object returned by the method GetThresholds() is empty.
Here is the code I wrote:
typedef itk::Image<uchar,2> image_type;
typedef itk::OtsuMultipleThresholdsImageFilter<image_type, image_type> filter_type;
image_type::Pointer img = itk::OpenCVImageBridge::CVMatToITKImage<image_type>(src);
image_type::SizeType size = img->GetLargestPossibleRegion().GetSize();
filter_type::Pointer filter = filter_type::New();
filter->SetInput(img);
filter->SetNumberOfHistogramBins(256);
filter->SetNumberOfThresholds(K);
filter_type::ThresholdVectorType tmp = filter->GetThresholds();
std::cout<<"CHECK: "<<tmp.size()<<std::endl;
src is OpenCV's Mat of CV_8U(C1) type.
A fundamental and basic concept to using ITK is that it is a pipeline architecture. You must connect the input's and output's then update the pipeline.
You have connected the pipeline but you have not executed it. You must call filter->Update().
Please read the ITK Software Guide to understand the fundamentals of ITK:
https://itk.org/ItkSoftwareGuide.pdf

Extract Point Cloud from a pcl::people::PersonCluster<PointT>

I am doing a project for the university and I need to extract the point cloud of the people to work with it. I made an ROS node adapting the code of the tutorial Ground based rgdb people detection, and now I want to publish in a topic the point cloud of the first detected cluster.
But I am not able to extract that point cloud, the definition of the class is defined here person_cluster.h. And there is a public member:
typedef pcl::PointCloud<PointT> PointCloud;
So to convert it in a sensor_msgs::PointCloud2 I do:
pcl_conversions::fromPCL(clusters.at(0).PointCloud,person_msg);
where person_msg is the PointCLoud2 message, clusters is a vector of pcl::people::PersonCluster<PointT>, and I want to publish only the first point cloud because I assume that there is only one person in the scene.
The compiler gives me this error:
error: invalid use of ‘pcl::people::PersonCluster::PointCloud’
pcl_conversions::fromPCL(clusters.at(0).PointCloud,person_msg);
I don't have a lot of knowledge about C++ and I am not able to overcome this error. Googling that error it seems that it appears when you don't "define" well a class, but I doubt that in the pcl library they defined bad a class.
For those that are interested I resolved my problem.
In the forum of pcl I found a post where the same developer of the people detector I used gave the answer.
So basically:
// Get cloud after voxeling and ground plane removal:
PointCloudT::Ptr no_ground_cloud (new PointCloudT);
no_ground_cloud = people_detector.getNoGroundCloud();
// Show pointclouds of every person cluster:
PointCloudT::Ptr cluster_cloud (new PointCloudT);
for(std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it)
{
if (it->getPersonConfidence() > min_confidence)
{
// Create the pointcloud with points belonging to the current cluster:
cluster_cloud->clear();
pcl::PointIndices clusterIndices = it->getIndices(); // cluster indices
std::vector<int> indices = clusterIndices.indices;
for(unsigned int i = 0; i < indices.size(); i++) // fill cluster cloud
{
PointT* p = &no_ground_cloud->points[indices[i]];
cluster_cloud->push_back(*p);
}
// Visualization:
viewer.removeAllPointClouds();
viewer.removeAllShapes();
pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cluster_cloud);
viewer.addPointCloud<PointT> (cluster_cloud, rgb, "cluster_cloud");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cluster_cloud");
viewer.spinOnce(500);
}
}
Actually I was not able to convert such type of point cloud in a sensor message PointCloud2, even trying to convert that pointcloud in a pcl::PointCloud<pcl::PointXYZ>.
A working solution was that to use as cluster_cloud a type of pcl::PointCloud<pcl::PointXYZ> and then using a publisher of type
pcl::PointCloud<pcl::PointXYZ>
Such:
ros::Publisher person_pub = nh.advertise<PointCloud>("personPointCloud", 1000);
Anyway it was no publish anything, the rviz didnt showed anything. But the viever was displaying the point cloud of the detected person. Since that pointcloud was not such I expected (if you move the arm the algorithm does no give you all the arm) it is not useful for my project so I drop it.
So still remains the problem to publish it in ros, but the problem to get the pointcloud is resolved.