AsyncPointCloudBuilder::PointCloud::Ptr PointCloudAggregator::build()
{
AsyncPointCloudBuilder::PointCloud::Ptr highres_cloud(new AsyncPointCloudBuilder::PointCloud);
AsyncPointCloudBuilder::PointCloud::Ptr downsampled_cloud(new AsyncPointCloudBuilder::PointCloud);
PointCloudMap local;
{
boost::mutex::scoped_lock(impl_->clouds_mutex_);
local = impl_->clouds_;
}
if(local.empty())
{
highres_cloud->points.push_back(AsyncPointCloudBuilder::PointCloud::PointType());
return highres_cloud;
}
pcl::ApproximateVoxelGrid<AsyncPointCloudBuilder::PointCloud::PointType> vg;
vg.setLeafSize(0.005f, 0.005f, 0.005f);
for(PointCloudMap::iterator it = local.begin(); it != local.end(); ++it)
{
(*highres_cloud) += *(it->second());
}
vg.setInputCloud(highres_cloud);
vg.filter(*downsampled_cloud);
return downsampled_cloud;
}
void RansacDominantPlaneExtractor::extract(PlanarPolygon& planar_polygon)
{
// Part 1: find inliers and coefficients for the dominant plane
pcl::PointIndices::Ptr plane_inliers_(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr plane_coefficients_(new pcl::ModelCoefficients);
PointCloud::Ptr downsampled_cloud(new PointCloud);
PointCloudN::Ptr downsampled_normals(new PointCloudN);
// Downsample input
vg_.setInputCloud(input_);
vg_.filter(*downsampled_cloud);
// Estimate normals for the downsampled cloud
ne_.setInputCloud(downsampled_cloud);
ne_.compute(*downsampled_normals);
// Segment plane
ssfn_.setInputCloud(downsampled_cloud);
ssfn_.setInputNormals(downsampled_normals);
ssfn_.segment(*plane_inliers_, *plane_coefficients_);
if (plane_inliers_->indices.size() == 0)
{
ROS_WARN("[RansacDominantPlaneExtractor::extract] No planar regions found! Aborting.");
return;
}
// Part 2: create corresponding PlanarPolygon
PointCloud::Ptr plane_cloud(new PointCloud);
PointCloud::Ptr plane_hull(new PointCloud);
// Project inliers onto the plane
pi_.setInputCloud(downsampled_cloud);
pi_.setIndices(plane_inliers_);
pi_.setModelCoefficients(plane_coefficients_);
pi_.filter(*plane_cloud);
// Calculate convex hull for inliers
pcl::ConvexHull<PointT> convex_hull;
convex_hull.setInputCloud(plane_cloud);
convex_hull.reconstruct(*plane_hull);
// Create PlanarPolygon
Eigen::Vector4f coefficients;
coefficients << plane_coefficients_->values[0],
plane_coefficients_->values[1],
plane_coefficients_->values[2],
plane_coefficients_->values[3];
shrinkPointCloud(plane_hull, shrink_plane_polygon_ratio_);
planar_polygon = PlanarPolygon(plane_hull->points, coefficients);
}
void TorusFinder::segment(
const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
{
if (!done_initialization_) {
return;
}
boost::mutex::scoped_lock lock(mutex_);
vital_checker_->poke();
pcl::PointCloud<pcl::PointNormal>::Ptr cloud
(new pcl::PointCloud<pcl::PointNormal>);
pcl::fromROSMsg(*cloud_msg, *cloud);
jsk_recognition_utils::ScopedWallDurationReporter r
= timer_.reporter(pub_latest_time_, pub_average_time_);
if (voxel_grid_sampling_) {
pcl::PointCloud<pcl::PointNormal>::Ptr downsampled_cloud
(new pcl::PointCloud<pcl::PointNormal>);
pcl::VoxelGrid<pcl::PointNormal> sor;
sor.setInputCloud (cloud);
sor.setLeafSize (voxel_size_, voxel_size_, voxel_size_);
sor.filter (*downsampled_cloud);
cloud = downsampled_cloud;
}
pcl::SACSegmentation<pcl::PointNormal> seg;
if (use_normal_) {
pcl::SACSegmentationFromNormals<pcl::PointNormal, pcl::PointNormal> seg_normal;
seg_normal.setInputNormals(cloud);
seg = seg_normal;
}
seg.setOptimizeCoefficients (true);
seg.setInputCloud(cloud);
seg.setRadiusLimits(min_radius_, max_radius_);
if (algorithm_ == "RANSAC") {
seg.setMethodType(pcl::SAC_RANSAC);
}
else if (algorithm_ == "LMEDS") {
seg.setMethodType(pcl::SAC_LMEDS);
}
else if (algorithm_ == "MSAC") {
seg.setMethodType(pcl::SAC_MSAC);
}
else if (algorithm_ == "RRANSAC") {
seg.setMethodType(pcl::SAC_RRANSAC);
}
else if (algorithm_ == "RMSAC") {
seg.setMethodType(pcl::SAC_RMSAC);
}
else if (algorithm_ == "MLESAC") {
seg.setMethodType(pcl::SAC_MLESAC);
}
else if (algorithm_ == "PROSAC") {
seg.setMethodType(pcl::SAC_PROSAC);
}
seg.setDistanceThreshold (outlier_threshold_);
seg.setMaxIterations (max_iterations_);
seg.setModelType(pcl::SACMODEL_CIRCLE3D);
if (use_hint_) {
seg.setAxis(hint_axis_);
seg.setEpsAngle(eps_hint_angle_);
}
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
seg.segment(*inliers, *coefficients);
NODELET_INFO("input points: %lu", cloud->points.size());
if (inliers->indices.size() > min_size_) {
// check direction. Torus should direct to origin of pointcloud
// always.
Eigen::Vector3f dir(coefficients->values[4],
coefficients->values[5],
coefficients->values[6]);
if (dir.dot(Eigen::Vector3f::UnitZ()) < 0) {
dir = - dir;
}
Eigen::Affine3f pose = Eigen::Affine3f::Identity();
Eigen::Vector3f pos = Eigen::Vector3f(coefficients->values[0],
coefficients->values[1],
coefficients->values[2]);
Eigen::Quaternionf rot;
rot.setFromTwoVectors(Eigen::Vector3f::UnitZ(), dir);
pose = pose * Eigen::Translation3f(pos) * Eigen::AngleAxisf(rot);
PCLIndicesMsg ros_inliers;
ros_inliers.indices = inliers->indices;
ros_inliers.header = cloud_msg->header;
pub_inliers_.publish(ros_inliers);
PCLModelCoefficientMsg ros_coefficients;
ros_coefficients.values = coefficients->values;
ros_coefficients.header = cloud_msg->header;
pub_coefficients_.publish(ros_coefficients);
jsk_recognition_msgs::Torus torus_msg;
torus_msg.header = cloud_msg->header;
tf::poseEigenToMsg(pose, torus_msg.pose);
torus_msg.small_radius = 0.01;
torus_msg.large_radius = coefficients->values[3];
pub_torus_.publish(torus_msg);
jsk_recognition_msgs::TorusArray torus_array_msg;
torus_array_msg.header = cloud_msg->header;
torus_array_msg.toruses.push_back(torus_msg);
pub_torus_array_.publish(torus_array_msg);
// publish pose stamped
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header = torus_msg.header;
pose_stamped.pose = torus_msg.pose;
pub_pose_stamped_.publish(pose_stamped);
pub_torus_array_with_failure_.publish(torus_array_msg);
pub_torus_with_failure_.publish(torus_msg);
}
else {
jsk_recognition_msgs::Torus torus_msg;
torus_msg.header = cloud_msg->header;
torus_msg.failure = true;
pub_torus_with_failure_.publish(torus_msg);
jsk_recognition_msgs::TorusArray torus_array_msg;
torus_array_msg.header = cloud_msg->header;
torus_array_msg.toruses.push_back(torus_msg);
pub_torus_array_with_failure_.publish(torus_array_msg);
NODELET_INFO("failed to find torus");
}
}
