C++ (Cpp) cloud_xyz_rgb_ptr Examples

Example #1

File: PoseEstimation6D.cpp Project: dvanthienen/youbot-ros-pkg-erf-demo

void PoseEstimation6D::kinectCloudCallback(const sensor_msgs::PointCloud2 &cloud){

	std::cout << "Estimating Pose 6D :) :)"<< std::endl;

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_xyz_rgb_ptr(new pcl::PointCloud<pcl::PointXYZRGB>());

	BRICS_3D::ColoredPointCloud3D *in_cloud = new BRICS_3D::ColoredPointCloud3D();
	BRICS_3D::PointCloud3D *color_based_roi = new BRICS_3D::ColoredPointCloud3D();
	std::vector<BRICS_3D::PointCloud3D*> extracted_clusters;

	//Transform sensor_msgs::PointCloud2 msg to pcl::PointCloud
	pcl::fromROSMsg (cloud, *cloud_xyz_rgb_ptr);

	// cast PCL to BRICS_3D type
	pclTypecaster.convertToBRICS3DDataType(cloud_xyz_rgb_ptr, in_cloud);
	ROS_INFO("Size of input cloud: %d ", in_cloud->getSize());

	if(in_cloud->getSize() < euclideanClusterExtractor.getMinClusterSize()) return;
	/**==================================================================================================== [color based roi extraction]**/
	//perform HSV color based extraction
	hsvBasedRoiExtractor.extractColorBasedROI(in_cloud, color_based_roi);
	ROS_INFO("[%s] Size of extracted cloud : %d ", regionLabel.c_str(),color_based_roi->getSize());
	if(color_based_roi->getSize() < euclideanClusterExtractor.getMinClusterSize()) return;
	/**==================================================================================================== [cluster extraction]**/
	//extract the clusters
	euclideanClusterExtractor.extractClusters(color_based_roi, &extracted_clusters);
	ROS_INFO("[%s] No of clusters found: %d", regionLabel.c_str(), extracted_clusters.size());


	/**==================================================================================================== [6D pose estimation]**/
	//Estimate 6D Pose
	int regions;
	if(extracted_clusters.size() < abs(maxNoOfObjects)) {
		regions = extracted_clusters.size();
	} else {
		regions = maxNoOfObjects;
	}

	for (int i = 0; i < regions; i++){
		if(extracted_clusters[i]->getSize() > 0){
			//Estimate Pose
			estimatePose(extracted_clusters[i],i+1);
		}
	}


	delete in_cloud;
	delete color_based_roi;
	extracted_clusters.clear();

}

Example #2

File: ColorBasedRoiExtractor.cpp Project: Abulala/youbot-ros-pkg

void ColorBasedRoiExtractor::kinectCloudCallback(const sensor_msgs::PointCloud2 &cloud){

//	ROS_INFO("\ntransferred kinect_raw message successfully...... :)");

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_xyz_rgb_ptr(new pcl::PointCloud<pcl::PointXYZRGB>());
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr hsv_extracted_roi_ptr(new pcl::PointCloud<pcl::PointXYZRGB>());


    brics_3d::PointCloud3D *in_cloud = new brics_3d::PointCloud3D();
    brics_3d::PointCloud3D *extracted_cloud = new brics_3d::PointCloud3D();


    //Transform sensor_msgs::PointCloud2 msg to pcl::PointCloud
    pcl::fromROSMsg (cloud, *cloud_xyz_rgb_ptr);

    // cast PCL to brics_3d type
    pclTypeCaster.convertToBRICS3DDataType(cloud_xyz_rgb_ptr, in_cloud);
    ROS_INFO("Size of input cloud: %d ", in_cloud->getSize());

	//perform HSV color based extraction
	hsvBasedRoiExtractor.filter(in_cloud, extracted_cloud);
	ROS_INFO("Size of extracted cloud : %d ", extracted_cloud->getSize());

	//convert back to PCl format for publishing
	//pclTypeCaster.convertToPCLDataType(hsv_extracted_roi_ptr, &extracted_cloud);
	pclTypeCaster.convertToPCLDataType(hsv_extracted_roi_ptr, extracted_cloud);

	//setup frame_id of extracted cloud for publishing
	hsv_extracted_roi_ptr->header.frame_id = "/openni_rgb_optical_frame";

	//publish extracted region
	extractedRegionPublisher->publish(*hsv_extracted_roi_ptr);

		delete in_cloud;
		delete extracted_cloud;

}