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