Ask Your Question
0

Removing Single Points from a sensor_msgs::PointCloud2

asked 2019-02-20 11:05:21 -0600

Arthur_Ace gravatar image

Hi,

I am trying to build a dynamic mask for robot systems using a Kinect2. My Idea is to use a naive euklidean distance search to remove the robot from the world. (I want to have a hole in the cloud where the robot is)

Unfortunately I don't know how to do that. Can you help me?

This is the part that needs help:

for (sensor_msgs::PointCloud2ConstIterator<float> it(output, "x"); it != it.end(); ++it) {
    // TODO: do something with the values of x, y, z


    try {
        transformStamped = tfBuffer.lookupTransform("kinect2_rgb_optical_frame", "RobotCenter",ros::Time(0),
                                                    ros::Duration(2));

        CamRobX = transformStamped.transform.translation.x;
        CamRobY = transformStamped.transform.translation.y;
        CamRobZ = transformStamped.transform.translation.z;
        CamPointX = it[0];
        CamPointY = it[1];
        CamPointZ = it[2];

        distance = sqrt(pow((CamPointX - CamRobX), 2) + pow((CamPointY - CamRobY), 2) + pow((CamPointZ - CamRobZ), 2));

    }
    catch (tf2::TransformException &ex) {
        ROS_WARN("%s", ex.what());
        ros::Duration(1.0).sleep();
        continue;
    }

    // distance refers to a radius of a frame in the middle of the room everything around this frame has to dissapear

    if(distance <0.4){

    }

    else{
    }
}

pub.publish(output);
edit retag flag offensive close merge delete

Comments

Do you absolutely want to solve/implement this yourself, or would a pkg/plugin that does this also be ok?

gvdhoorn gravatar imagegvdhoorn ( 2019-02-20 13:06:25 -0600 )edit

2 Answers

Sort by » oldest newest most voted
1

answered 2019-02-21 03:03:50 -0600

updated 2019-02-21 07:23:49 -0600

Given you're written the majority of the code for this it's only a tiny bit of work to get this working. Just to give a bit of background, there are two different types of point cloud that can be represented by PCL: Dense structured point clouds and sparse point clouds.

In dense point clouds, the points are stored in a grid structure a NxM matrix of 3D points with invalid points being represented as vectors of NANs. These types of point clouds are generally created by depth cameras and stereo vision algorithms.

In sparse point clouds, the points are represented as a flat list of points in no specific order these are more commonly generated by LIDAR sensors.

Getting back to your question, the kinect produces dense structured point clouds so you will be able to 'remove' a point by simply setting its x, y and z values to NAN. This means that the grid structure of the points is preserved.

Hope this helps.

Update

You can set the values of a point using the iterator you are using in your loop as shown below:

it[0] = std::numeric_limits<float>::quiet_NaN();
it[1] = std::numeric_limits<float>::quiet_NaN();
it[2] = std::numeric_limits<float>::quiet_NaN();

The iterator functions like a pointer to the pcl::Point allowing you to get and set its values.

edit flag offensive delete link more

Comments

That clears things up, thank you! Unfortunately I don't know how and where to set the NAN-Value also, this is already a sensor_msgs_PointCloud2. Can I do the same here?

Arthur_Ace gravatar imageArthur_Ace ( 2019-02-21 07:09:48 -0600 )edit

Yes this should work. I've updated my answer with an example for you now.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-02-21 07:24:24 -0600 )edit

I tried that, but when I try to compile (catkin_make) the following error code appears. it seems like the iterator won't let me override the values for some reason although it should...

Arthur_Ace gravatar imageArthur_Ace ( 2019-02-21 07:46:41 -0600 )edit

error: assignment of read-only location ‘it.sensor_msgs::PointCloud2ConstIterator<float>::<anonymous>.sensor_msgs::impl::PointCloud2IteratorBase<float, const="" float,="" const="" unsigned="" char,="" const="" sensor_msgs::pointcloud2_<std::allocator<void=""> >, sensor_msgs::PointCloud2ConstIterator>::operator

Arthur_Ace gravatar imageArthur_Ace ( 2019-02-21 07:47:53 -0600 )edit

the iterator won't let me override the values for some reason although it should...

no, it shouldn't. It's a ConstIterator ..

gvdhoorn gravatar imagegvdhoorn ( 2019-02-21 07:49:44 -0600 )edit

So, how can I circumvent this?

Arthur_Ace gravatar imageArthur_Ace ( 2019-02-21 07:52:02 -0600 )edit

Ahh if your working with the message passed to a callback it will be a const (immutable) type. You should be able to create a non-const copy of the message and work on that:

sensor_msgs::PointCloud2 newCloud = *cloud_parameter;
PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-02-21 08:01:16 -0600 )edit
0

answered 2019-02-21 08:37:42 -0600

Arthur_Ace gravatar image

Thank you all for your help! Now I know what I did wrong. First off: Yes Changing Single Coordinates to NAN works, but like somebody else pointed out, I used the wrong Iterator. Thankfully There was another Iterator which doesn't have the "const" tag in it, and with this one I am able to change points.

Here is the improved code:

for (sensor_msgs::PointCloud2Iterator<float> it(output, "x"); it != it.end(); ++it) {
    // TODO: do something with the values of x, y, z


    try {
        transformStamped = tfBuffer.lookupTransform("kinect2_rgb_optical_frame", "RobotCenter",ros::Time(0),
                                                    ros::Duration(2));

        CamRobX = transformStamped.transform.translation.x;
        CamRobY = transformStamped.transform.translation.y;
        CamRobZ = transformStamped.transform.translation.z;
        CamPointX = it[0];
        CamPointY = it[1];
        CamPointZ = it[2];

        distance = sqrt(pow((CamPointX - CamRobX), 2) + pow((CamPointY - CamRobY), 2) + pow((CamPointZ - CamRobZ), 2));

    }
    catch (tf2::TransformException &ex) {
        ROS_WARN("%s", ex.what());
        ros::Duration(1.0).sleep();
        continue;
    }
    if(distance <0.4){


        it[0] = std::numeric_limits<float>::quiet_NaN();
        it[1] = std::numeric_limits<float>::quiet_NaN();
        it[2] = std::numeric_limits<float>::quiet_NaN();


    }

    else{
    }
}

pub.publish(output);
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2019-02-20 11:05:21 -0600

Seen: 340 times

Last updated: Feb 21