Unable to remove nan points using function removeNaNFromPointCloud

edit

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 ...