Ask Your Question
0

Unable to remove nan points using function removeNaNFromPointCloud

asked 2018-05-20 23:38:42 -0500

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)
edit retag flag offensive close merge delete

3 Answers

Sort by ยป oldest newest most voted
0

answered 2019-09-05 07:14:07 -0500

SamsAutonomy gravatar image

updated 2019-09-05 07:15:43 -0500

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);
edit flag offensive delete link more
0

answered 2019-09-05 05:07:35 -0500

ALI gravatar image

updated 2019-09-05 05:07:57 -0500

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

edit flag offensive delete link more
0

answered 2018-05-21 04:22:20 -0500

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.

edit flag offensive delete link more

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 imagearthur0304 ( 2018-05-22 03:03:57 -0500 )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 imagearthur0304 ( 2018-05-22 03:11:42 -0500 )edit

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: 2018-05-20 23:38:42 -0500

Seen: 695 times

Last updated: Sep 05