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); } }