Ask Your Question
0

lots of nan data when convert pointcloud2 to pcl::PointXYZ

asked 2015-04-16 12:40:46 -0500

crazymumu gravatar image

updated 2018-07-15 02:21:11 -0500

jayess gravatar image

HI all, I use Ubuntu 14.04 LTI with ROS Indigo. Then I simulate kinect in Gazebo:

 <xacro:macro name="kinect_camera" params="name parent *origin">
  <xacro:kinect_camera_model name="${name}" parent="${parent}">
    <xacro:insert_block name="origin" />
  </xacro:kinect_camera_model>

  <gazebo reference="${name}_depth_frame">
    <sensor type="depth" name="${name}">
      <update_rate>20</update_rate>
      <camera>
        <horizontal_fov>${60 * M_PI/180.0}</horizontal_fov>
        <image>
          <format>R8G8B8</format>
          <width>640</width>
          <height>480</height>
        </image>
        <clip>
          <near>0.05</near>
          <far>3</far>
        </clip>
      </camera>
      <plugin name="${name}_camera_controller" filename="libgazebo_ros_openni_kinect.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>20</updateRate>
        <imageTopicName>${name}/rgb/image_raw</imageTopicName>
        <cameraInfoTopicName>${name}/rgb/camera_info</cameraInfoTopicName>
        <depthImageTopicName>${name}/depth/image_raw</depthImageTopicName>
        <depthImageCameraInfoTopicName>${name}/depth/camera_info</depthImageCameraInfoTopicName>
        <pointCloudTopicName>${name}/depth/points</pointCloudTopicName>
        <pointCloudCutoff>0.5</pointCloudCutoff>
        <frameName>${name}_depth_optical_frame</frameName>
        <distortion_k1>0.0</distortion_k1>
        <distortion_k2>0.0</distortion_k2>
        <distortion_k3>0.0</distortion_k3>
        <distortion_t1>0.0</distortion_t1>
        <distortion_t2>0.0</distortion_t2>
      </plugin>
    </sensor>
  </gazebo>
</xacro:macro>

Then I convert pointcloud2 to pcl::PointXYZ follow the link:pcl/overview

3.1 For a subscriber that receives pcl::PointCloud<t> objects directly:

My code:

void callback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& position)  
{  
     const char *path="/home/lin/Desktop/distance.dat";  
     ofstream file(path);  
     for(int i=0; i<307200;i++)
 {    
      cout << "(x,y,z) = " << position -> points[i] << std::endl;  
      file << position -> points[i];  
  }  
}  


int main(int argc, char** argv){

    ros::init(argc, argv, "distance_publisher");
    ros::NodeHandle nh;

    std::string topic = nh.resolveName("camera/depth/points");
    uint32_t queue_size = 1;

    // create a templated subscriber
    ros::Subscriber sub = nh.subscribe<pcl::PointCloud<pcl::PointXYZ> > (topic, queue_size, callback);

        ros::Rate r(0.1);
        while(nh.ok()){

          ros::spinOnce(); 
          r.sleep();

        }

}

`
I get lots of NAN data , I just want to know when convert pointcloud2 to pcl::PointXYZ, it will remove nan data automatically or I need to filter it.

edit retag flag offensive close merge delete

3 Answers

Sort by ยป oldest newest most voted
0

answered 2015-04-17 13:24:43 -0500

crazymumu gravatar image

updated 2018-07-15 02:22:11 -0500

jayess gravatar image

I found this works fine, also please check PCL library in your CmakeLists.txt.

void callback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& inputCloud )
{

    pcl::PointCloud<pcl::PointXYZ>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZ>);
    vector<int> indices;
    pcl::removeNaNFromPointCloud(*inputCloud,*outputCloud, indices);
}
edit flag offensive delete link more
1

answered 2015-04-17 06:41:11 -0500

epoxi gravatar image

updated 2015-04-17 07:57:45 -0500

You still need to remove the NaN values from the point cloud. You can do that in the callback like this:

void callback (pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in_)
{

        pcl::PointCloud<pcl::PointXYZ> filtered_cloud_ = pcl::PointCloud<pcl::PointXYZ>::Ptr (new pcl::PointCloud<pcl::PointXYZ>);

    //remove NaN points from the cloud
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*cloud_in_,*filtered_cloud_, indices);

}
edit flag offensive delete link more

Comments

thanks for your help, but it still have error.

crazymumu gravatar imagecrazymumu ( 2015-04-17 13:21:21 -0500 )edit
-1

answered 2017-04-27 04:41:08 -0500

zubair gravatar image
void getXYZ(int x, int y)
{
    int arrayPosition = y * my_pcl.row_step + x * my_pcl.point_step;
    int arrayPosX = arrayPosition + my_pcl.fields[0].offset; // X has an offset of 0
    int arrayPosY = arrayPosition + my_pcl.fields[1].offset; // Y has an offset of 4
    int arrayPosZ = arrayPosition + my_pcl.fields[2].offset; // Z has an offset of 8

    float X;
    float Y;
    float Z;

    memcpy(&X, &my_pcl.data[arrayPosX], sizeof(float));
    memcpy(&Y, &my_pcl.data[arrayPosY], sizeof(float));
    memcpy(&Z, &my_pcl.data[arrayPosZ], sizeof(float));

    geometry_msgs::Point p;

    // put data into the point p
    p.x = X;
    p.y = Y;
    p.z = Z;
    std::cout << "x :" << p.x << "y :" << p.y << "z :" << p.z << std::endl;
}

this is how i am using pcl to find out x y and z ,, int x and y are my pixel values from blob,, but i have the same problem x y and z datat from point cloud have a lot of Nan values .. please help

thanks ya all

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: 2015-04-16 12:40:46 -0500

Seen: 1,249 times

Last updated: Jul 15 '18