Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Removing Single Points from a sensor_msgs::PointCloud2


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),

        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());

    // 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){