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();
}
void MultiRigidDetector::estimatePose(const Mat &image, int object_index,
geometry_msgs::Pose &pose) {
pose::TranslationRotation3D pose_tr;
estimatePose(image, object_index, pose_tr);
double t[3];
pose_tr.getT(t);
double x, y, z, w;
pose_tr.getQuaternion(x, y, z, w);
pose.position.x = t[0];
pose.position.y = t[1];
pose.position.z = t[2];
pose.orientation.x = x;
pose.orientation.y = y;
pose.orientation.z = z;
pose.orientation.w = w;
}
