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

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.

Related

Problem getting OpenCV multitracking to work

I'm in the early stages (VERY alpha) of writing the backend for an open source home video surveillance system. I'm building it on Gstreamer as a series of plugins. The idea is going to make each step of the process modular based on implementations, so that anyone can write a custom module and use that in the pipeline. For an example, take a look at "odo-lib-opencv-yolo" in the repo linked below. Using that as a template, anyone can write their own detection lib using any library or neural network model as use it within this pipeline. I also plan to expand that out to include a few "default" libraries that are optimized for different systems (Nvidia GPUs, Raspberry Pis, Coral TPUs, etc).
The issue I'm running into is the tracking portion. I wanted to start off with a simple and basic tracking system using the tracking API in OpenCV as a "filler" while I flesh out everything else. But it just doesn't seem to work right.
Here's the link to the repo: https://github.com/CeeBeeEh/Odo-Project
Of most importance is this code block in gst/gstodotrack.cpp at line 270:
if (meta->isInferenceFrame) {
LAST_COUNT = meta->detectionCount;
TRACKER->clear();
TRACKER = cv::legacy::MultiTracker::create();
for (int i=0; i<LAST_COUNT; i++) {
LAST_DETECTION[i].class_id = meta->detections[i].class_id;
LAST_DETECTION[i].confidence = meta->detections[i].confidence;
strcpy(LAST_DETECTION[i].label, meta->detections[i].label);
cv::Rect2i rect = cv::Rect2i(meta->detections[i].box.x, meta->detections[i].box.y,
meta->detections[i].box.width, meta->detections[i].box.height);
TRACKER->add(create_cvtracker(odo), img, rect);
}
GST_DEBUG("Added %zu objects to tracker", LAST_COUNT);
}
else {
meta->detectionCount = LAST_COUNT;
std::vector<cv::Rect2d> tracked;
if (!TRACKER->update(img, tracked)) GST_ERROR("Error tracking objects");
GST_DEBUG("Infer count=%lu, tracked count=%zu", LAST_COUNT, tracked.size());
for (int i=0; i<tracked.size(); i++) {
meta->detections[i].box.x = tracked[i].x;
meta->detections[i].box.y = tracked[i].y;
meta->detections[i].box.width = tracked[i].width;
meta->detections[i].box.height = tracked[i].height;
meta->detections[i].class_id = LAST_DETECTION[i].class_id;
meta->detections[i].confidence = LAST_DETECTION[i].confidence;
strcpy(meta->detections[i].label, LAST_DETECTION[i].label);
}
}
Variables at top of file:
cv::Ptr<cv::legacy::MultiTracker> TRACKER;
ulong LAST_COUNT = 0;
DetectionData LAST_DETECTION[DETECTION_MAX] = {};
I originally had
TRACKER = cv::legacy::MultiTracker::create();
in the start method to create it when the plugin is loaded, but that caused a segfault.
I've also tried creating a fixed size array of cv::Ptr<cv::Tracker> and just use each one for a single object with the same id within each frame, and then recycle each one for each inference frame. But that cause odd behaviour with each object was tracking to the same size and position within the frame.
Right now, in the current configuration, the tracking is dog-slow. It's actually faster to just inference each frame on GPU than inference a single frame and track for x additional frames. It should be the opposite.
It seems as though OpenCV's tracking API has changed as of 4.5.1, but I'm not sure if that's the cause of the issue.
I'm looking for help in sorting out the OpenCV tracking issue. I find all the docs very lacking in explaining the what and why of the multi-tracking API. So those are not of much help.

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.

calculating distance between vehicles and setting speed so the distances remain the same e.g 5 meters

I am using veins 5.0 and i am trying to calculate the distance between the vehicles and setting their speed. I want to calculate it every second and i want to do it by sending wsm messages.My goal is to have for example 5 vehicles, each vehicle will communicate with the front vehicle and get its position with the intention of calculating their distance and keep it static. I am new to this and i don't know how to approach it.
I tried to do something like this on handlePositionUpdate
void TraCIDemo11p::handlePositionUpdate(cObject* obj)
{
DemoBaseApplLayer::handlePositionUpdate(obj);
// stopped for for at least 10s?
if (x<simTime()){
TraCIDemo11pMessage* wsm1 = new TraCIDemo11pMessage();
populateWSM(wsm1);
wsm1->setPosition(mobility->getPositionAt(simTime()));
wsm1->setSpeed(mobility->getSpeed());
if (dataOnSch) {
startService(Channel::sch2, 42, "Traffic Information Service");
message to self to send later
scheduleAt(computeAsynchronousSendingTime(1,ChannelType::service), wsm1);
}
else {
sendDown(wsm1);
}
}
You are describing what is, effectively, a platooning application. You might want to base your source code on Plexe, the platooning extension to Veins. It already comes with state-of-the-art distance controllers like PATH or Ploeg. More information can be found on http://plexe.car2x.org/

Project Tango - 3D Reconstruction

I'm trying to use the C 3D Reconstruction Library to get a mesh from the Tango device.
In the Mesh Building Functions there's a summary of the flow to use, which shows that I have to call the Tango3DR_update function several times and then call the Tango3DR_extractFullMesh to get the mesh.
The problem is that Tango3DR_update needs the Tango3DR_PointCloud object which I don't see how I get.
I can create an empty Tango3DR_PointCloud using Tango3DR_PointCloud_create, but I don't see anywhere how I fill it with real data.
Does anyone know how to get this object?
Or anyone knows if there's any example / sample code using this library? I didn't find any.
Thanks,
Oren
You should fill the Tango3DR_PointCloud from the TangoXYZij you receive in OnXYZijAvailableRouter. Same thing for the pose struct.
// -- point cloud
Tango3DR_PointCloud cloud;
cloud.num_points = xyz_ij->xyz_count;
cloud.points = new Tango3DR_Vector4[cloud.num_points];
for (int i = 0; i < cloud.num_points; ++i) {
cloud.points[i][0] = xyz_ij->xyz[i][0];
cloud.points[i][1] = xyz_ij->xyz[i][1];
cloud.points[i][2] = xyz_ij->xyz[i][2];
// last is confidence
cloud.points[i][3] = 1;
}
cloud.timestamp = xyz_ij->timestamp;
(Do not forget to delete [] cloud.points once you're done)
The only official example I could find is in the Unity examples. They use the C API but called from C#.

Draw a multiple lines set with VTK

Can somebody point me in the right direction of how to draw a multiple lines that seem connected? I found vtkLine and its SetPoint1 and SetPoint2 functions. Then I found vtkPolyLine, but there doesn't seem to be any add, insert or set function for this. Same for vtkPolyVertex.
Is there a basic function that allows me to just push some point at the end of its internal data and the simply render it? Or if there's no such function/object, what is the way to go here?
On a related topic: I don't like vtk too much. Is there a visualization toolkit, maybe with limited functionality, that is easier to use?
Thanks in advance
For drawing multiple lines, you should first create a vtkPoints class that contains all the points, and then add in connectivity info for the points you would like connected into lines through either vtkPolyData or vtkUnstructuredGrid (which is your vtkDataSet class; a vtkDataSet class contains vtkPoints as well as the connectivity information for these points). Once your vtkDataSet is constructued, you can take the normal route to render it (mapper->actor->renderer...)
For example:
vtkPoints *pts = vtkPoints::New();
pts->InsertNextPoint(1,1,1);
...
pts->InsertNextPoint(5,5,5);
vtkPolyData *polydata = vtkPolyData::New();
polydata->Allocate();
vtkIdType connectivity[2];
connectivity[0] = 0;
connectivity[1] = 3;
polydata->InsertNextCell(VTK_LINE,2,connectivity); //Connects the first and fourth point we inserted into a line
vtkPolyDataMapper *mapper = vtkPolyDataMapper::New();
mapper->SetInput(polydata);
// And so on, need actor and renderer now
There are plenty of examples on the documentation site for all the classes
Here is vtkPoints : http://www.vtk.org/doc/release/5.4/html/a01250.html
If you click on the vtkPoints (Tests) link, you can see the tests associated with the class. It provides a bunch of different sample code.
Also, the vtk mailing list is probably going to be much more useful than stack overflow.