Eigen::Matrix4f ICPWrapper::performICP (PointCloudConstPtr source_cloud_ptr,
PointCloudConstPtr target_cloud_ptr, Eigen::Matrix4f initial_transform)
{
ROS_INFO("Performing ICP...");
PointCloud source_cloud_transformed;
icp_.setInputCloud (downsampleCloud(source_cloud_ptr));
icp_.setInputTarget (downsampleCloud(target_cloud_ptr));
pcl17::console::setVerbosityLevel (pcl17::console::L_DEBUG);
icp_.align (source_cloud_transformed, initial_transform);
ROS_INFO_STREAM ("ICP has converged: " << icp_.hasConverged() << " score: "
<< icp_.getFitnessScore () << "\n Transformation: \n" << icp_.getFinalTransformation ());
return icp_.getFinalTransformation ();
}
void ObjectDetector::cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromROSMsg (*input, *input_cloud);
// if theta and y_translation are not yet set, then run segmentation
if(theta == 0.0 && y_translation == 0.0){
performSegmentation(input_cloud);
}
else{
// downsample the point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled_cloud = downsampleCloud(input_cloud);
// perform transformation
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud = transformCloud(downsampled_cloud);
// filter floor, walls and noise so only objects remain
pcl::PointCloud<pcl::PointXYZ>::Ptr object_cloud = filterCloud(transformed_cloud);
// cluster objects & publish cluster
clusterCloud(object_cloud);
}
}
