Ask Your Question
0

Moving a group of point clouds in rviz

asked 2021-02-12 01:43:03 -0500

Alireza_msb gravatar image

updated 2021-02-12 18:22:24 -0500

Hi all, Thank you for your help beforehand. I am pretty new to PCL library so I apologize if my question seems easy. For my project, I need to show the point clouds of the environment to the operator in VR, and the operator should be able to pick and place a cylinder. I have written the code at the bottom of this message for object segmentation. So in VR, the operator should be able to move the cylinder's point clouds. My question is that after finding the cylinder: 1. How can I move the cylinder point clouds? 2. How can I merge the cylinder point clouds with the environment point clouds? Meaning that the operator would see the environment too( without the cylinder point cloud), while he/she is moving the cylinder.

As I am completely new to PCL, any comments would be appreciated so much. I look forward to your kind help!


#include <ros/ros.h>
#include<iostream>
#include <sensor_msgs/PointCloud2.h>
#include<pcl-1.8/pcl/point_cloud.h>
#include<pcl-1.8/pcl/point_types.h>
#include<pcl_conversions/pcl_conversions.h>

#include<pcl-1.8/pcl/filters/extract_indices.h>
#include<pcl-1.8/pcl/filters/voxel_grid.h>
#include<pcl-1.8/pcl/filters/passthrough.h>

#include<pcl-1.8/pcl/segmentation/sac_segmentation.h>
#include<pcl-1.8/pcl/sample_consensus/method_types.h>
#include<pcl-1.8/pcl/sample_consensus/model_types.h>

#include<pcl-1.8/pcl/features/normal_3d.h>

#include<pcl-1.8/pcl/ModelCoefficients.h>
ros::Publisher pln_pub;

ros::Publisher cyl_pub;

//Everything happens in this call-back function 

void cloud_cb(const pcl::PCLPointCloud2ConstPtr &input)

{
// All the data;

pcl::PCLPointCloud2::Ptr filtered(new pcl::PCLPointCloud2);
pcl::PCLPointCloud2::Ptr ex_pln(new pcl::PCLPointCloud2);
pcl::PCLPointCloud2::Ptr ex_cyl(new pcl::PCLPointCloud2);

pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ> ::Ptr flt_pln(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr flt_cyl(new pcl::PointCloud<pcl::PointXYZ>);

pcl::PointCloud<pcl::Normal>::Ptr cloud_normals1(new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2(new pcl::PointCloud<pcl::Normal>);

pcl::ModelCoefficients::Ptr coeff_pln(new pcl::ModelCoefficients),coeff_cyl(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers_pln(new pcl::PointIndices), inliers_cyl(new pcl::PointIndices);

// Objects needed 
pcl::NormalEstimation<pcl::PointXYZ,pcl::Normal> ne;
pcl::SACSegmentationFromNormals<pcl::PointXYZ,pcl::Normal> seg;

pcl::ExtractIndices<pcl::PCLPointCloud2> extract;
pcl::ExtractIndices<pcl::Normal> extract_normal;
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());

// You can use the following line to convert PointCloud2 data to pcl::PointXYZ
pcl::fromPCLPointCloud2(*input,*input_cloud);
//estimate normals
ne.setSearchMethod(tree);
ne.setInputCloud(input_cloud);
ne.setKSearch(50);
ne.compute(*cloud_normals1);

// Here we create the segmentation object for the plane
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SacModel::SACMODEL_NORMAL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(100);
seg.setDistanceThreshold(0.03);
seg.setInputCloud(input_cloud);
seg.setInputNormals(cloud_normals1);

seg.segment(*inliers_pln,*coeff_pln);
// extracting the plane
extract.setInputCloud(input);
extract.setIndices(inliers_pln);
extract.setNegative(false);
extract.filter(*ex_pln);
pln_pub.publish(*ex_pln);

//removing the plane completely
extract.setNegative(true);
extract.filter(*filtered);
extract_normal.setNegative(true);
extract_normal.setInputCloud(cloud_normals1);
extract_normal.setIndices(inliers_pln);
extract_normal.filter(*cloud_normals2);

pcl::PointCloud<pcl::PointXYZ>::Ptr xyz_filtered(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(*filtered,*xyz_filtered);
//from the new normals find what we are looking for 
seg.setOptimizeCoefficients ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2021-02-13 05:30:18 -0500

Alireza_msb gravatar image

updated 2021-02-13 05:31:57 -0500

I could solve the problem by changing my prespective a little bit. So instead of moving the pointClouds, we can creat a Marker which is consisted of many points which have the same color as the points of the pointcloud and then we can move or rotate the frame of the marker. Here's a code for making the marker ( If anyone has any other recommendation I would be very happy to hear that!!):

  #include<ros/ros.h>
  #include<visualization_msgs/Marker.h>
  #include<pcl-1.8/pcl/point_cloud.h>
  #include<pcl_conversions/pcl_conversions.h>
  #include<pcl-1.8/pcl/point_types.h>
  #include <sensor_msgs/PointCloud.h>
  #include <sensor_msgs/PointCloud2.h>
  #include <sensor_msgs/point_cloud_conversion.h>
  #include<pcl-1.8/pcl/filters/passthrough.h>


  ros::Publisher pub;
  ros::Publisher pass_pub;

   void cloud_cb(const pcl::PCLPointCloud2ConstPtr &input)
   {
    // Getting the target point cloud
    pcl::PassThrough<pcl::PCLPointCloud2> pass;
    pcl::PCLPointCloud2::Ptr flt_input(new pcl::PCLPointCloud2);

    pass.setInputCloud(input);
    pass.setFilterFieldName("x");
    pass.setFilterLimits(0,2);       
    pass.filter(*flt_input);

    pass.setInputCloud(flt_input);
    pass.setFilterFieldName("y");
    pass.setFilterLimits(-2,0);
    pass.filter(*flt_input);

    pass.setInputCloud(flt_input);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(0,2);
    pass.filter(*flt_input);
    pass_pub.publish(*flt_input);
    // Converting PointCloud2 to PointXYZRGBA 
    pcl::PointCloud<pcl::PointXYZRGBA> input_xyz;
    pcl::fromPCLPointCloud2(*flt_input,input_xyz);
    //create the marker
    visualization_msgs::Marker marker;
    marker.header.frame_id="/camera1";
    marker.header.stamp=ros::Time::now();
    marker.ns = "pcl_marker";
    marker.id = 1;
    marker.type = visualization_msgs::Marker::POINTS;
    marker.action = visualization_msgs::Marker::ADD;
    marker.lifetime = ros::Duration();


    marker.scale.x = 0.004;
    marker.scale.y = 0.004;
    //marker.scale.z = 0.004;

    for(size_t i=0; i<input_xyz.points.size(); i++) {

        std_msgs::ColorRGBA c;
        // getting the collor of points[i]
        c.r = (float)input_xyz.points[i].r/256;
        c.g = (float)input_xyz.points[i].g/256;
        c.b = (float)input_xyz.points[i].b/256;
        c.a=1;

        geometry_msgs::Point p;
        p.x = input_xyz.points[i].x;            
        p.y = input_xyz.points[i].y;
        p.z = input_xyz.points[i].z;
        marker.points.push_back(p);
        marker.colors.push_back(c);
    }
    pub.publish(marker);
    }



    int main(int argc, char** argv)
   {
   ros::init(argc, argv,"kinect");
   ros::NodeHandle n;

   pub= n.advertise<visualization_msgs::Marker>("marker",10);
   pass_pub= n.advertise<sensor_msgs::PointCloud2>("filteredPnt",1);

   ros::Subscriber sub= n.subscribe("kinect2/sd/points",1,cloud_cb);
   ros::spin();
   }
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

1 follower

Stats

Asked: 2021-02-12 01:43:03 -0500

Seen: 97 times

Last updated: Feb 13