First time here? Check out the FAQ!


ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

Unable to remove nan points using function removeNaNFromPointCloud

asked May 21 '18

arthur0304 gravatar image

Hi, I want to remove nan points from my pointcloud data, however, the function removeNaNFromPointCloud seems not working.

Also, I set the point to nan if the distance is bigger than 10 meters using code below:

const float bad_point = std::numeric_limits<float>::quiet_NaN();
float distance = (cloud_in->points[i].x*cloud_in->points[i].x + cloud_in->points[i].y*cloud_in->points[i].y + cloud_in->points[i].z*cloud_in->points[i].z);
if (distance>=10){
  cloud_in->points[i].x = bad_point;
  cloud_in->points[i].y = bad_point;
  cloud_in->points[i].z = bad_point;
}

After that, I use the function removeNaNFromPointCloud twice as below:

std::vector<int> indices;
for (int j=0;j<2;j++){
  pcl::removeNaNFromPointCloud(*cloud_in, *cloud_in, indices);
}

However, if I print out the pointcloud's size, the number remain the same. Also, I have tried to print out all points in my pointcloud data and there are some nan points, showing that I did change point that has distance greater than 10 meters to nan point. But how come the function does not work? Please give me some hints and I will be really appreciated!

Here is the full code below:

#include <ros/ros.h>
#include <cmath>        // std::abs
#include <limits>
#include <sensor_msgs/PointCloud2.h>
#include <visualization_msgs/Marker.h>
#include <geometry_msgs/Point.h>
#include <std_msgs/ColorRGBA.h>


#include <pcl_ros/point_cloud.h>
#include <pcl/io/io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/filter.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>

#include <robotx_gazebo/ObstaclePose.h>
#include <robotx_gazebo/ObstaclePoseList.h>


//define point cloud type
typedef pcl::PointCloud<pcl::PointXYZ> PointCloudXYZ;
typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloudXYZRGB;

//declare point cloud
PointCloudXYZ::Ptr cloud_inXYZ (new PointCloudXYZ);
PointCloudXYZRGB::Ptr cloud_in (new PointCloudXYZRGB); 
PointCloudXYZRGB::Ptr cloud_filtered (new PointCloudXYZRGB);
PointCloudXYZRGB::Ptr cloud_f (new PointCloudXYZRGB);
PointCloudXYZRGB::Ptr cloud_plane (new PointCloudXYZRGB);
PointCloudXYZRGB::Ptr result (new PointCloudXYZRGB);
sensor_msgs::PointCloud2 ros_out;

//declare ROS publisher
ros::Publisher pub_result;
ros::Publisher pub_marker;
ros::Publisher pub_obstacle;

//declare global variable
bool lock = false;

//declare function
void cloud_cb(const sensor_msgs::PointCloud2ConstPtr&); //point cloud subscriber call back function
void cluster_pointcloud(void); //point cloud clustering
int point_cloud_color(int input); //set point cloud RGB color
void drawRviz(robotx_gazebo::ObstaclePoseList); //draw marker in Rviz


void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input)
{
  if (!lock){
    lock = true;
    //covert from ros type to pcl type
    pcl::fromROSMsg (*input, *cloud_inXYZ);
    copyPointCloud(*cloud_inXYZ, *cloud_in);

    //set color for point cloud
    for (size_t i = 0; i < cloud_in->points.size(); i++){
      cloud_in->points[i].r = 0;
      cloud_in->points[i].g = 0;
      cloud_in->points[i].b = 0;

      // ============== set point to nan ==============
      const float bad_point = std::numeric_limits<float>::quiet_NaN();
      float distance = (cloud_in->points[i].x*cloud_in->points[i].x + cloud_in->points[i].y*cloud_in->points[i].y + cloud_in->points[i].z*cloud_in->points[i].z);
      if (distance>=10){
        cloud_in->points[i].x = bad_point;
        cloud_in->points[i].y ...
(more)
Preview: (hide)

4 Answers

Sort by » oldest newest most voted
1

answered Sep 5 '19

SamsAutonomy gravatar image

updated Sep 5 '19

I have ran into this same situation. To remove NaN points there is another way without looping through the points etc. See the example below, which should work and let me know if it doesn't make sense.

boost::shared_ptr<std::vector<int>> indices(new std::vector<int>);
pcl::removeNaNFromPointCloud(*source_cloud, *indices);
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(source_cloud);
extract.setIndices(indices);
extract.setNegative(true);
extract.filter(*source_cloud);
Preview: (hide)
2

answered Sep 5 '19

ALI gravatar image

updated Sep 5 '19

Hi Simon,

I had the same problem. removeNaN is not working in PCL. I used alternative method, which is checking for NAN in the point cloud data, and use extract.setIndices(inliers) to remove them .

a very good explanation is in this link:

https://stackoverflow.com/questions/4...

Preview: (hide)
1

answered May 21 '18

Hi Simon, I'm not 100% sure why this code isn't removing the points, but I can suggest a tidier and faster way of achieving the same thing:

A couple of points, the distance you're calculating is actually the squared distance not the Euclidean distance is this what you were expecting? Right now you're filtering for points further than 3.16m away.

Why are you calling pcl::removeNaNFromPointCloud twice?

I would recommend directly removing the near points from the cloud this will be faster, and will be cleaner and easier to read code. Using a remove_if then erase sequence as described here would be a fast solution if you're expecting to remove a reasonable percentage of the cloud.

It should be noted that if you're input cloud is structured, it will not be structured after filtering so you should change this flag if you're working with the point values directly.

Preview: (hide)

Comments

Hi @PeteBlackerThe3rd, sorry for the late response. Yes you are right I forgot to put sqrt() before the distance. But still that is just the small error, the function still does not remove the nan points.

arthur0304 gravatar image arthur0304  ( May 22 '18 )edit

Also, I call the function twice since I thought calling it twice may have the difference, there is no other specific reasons. Thank you for your advice, I will take a look.

arthur0304 gravatar image arthur0304  ( May 22 '18 )edit
0

answered May 7 '20

wwfzs1990 gravatar image

updated May 7 '20

Be sure that the cloud is set to sparse by:

cloud_in->is_dense = false;

Ref: https://github.com/PointCloudLibrary/...

Preview: (hide)

Question Tools

2 followers

Stats

Asked: May 21 '18

Seen: 5,498 times

Last updated: May 07 '20