Moving a group of point clouds in rviz

edit

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