std::vector<cv::Point2f> findLabelPositions(const cv::Mat3b& img) { std::vector<std::vector<cv::Point2f>> candidate_points(3*9); for (auto threshold : {4, 6, 8, 10, 12, 14, 16}) { std::vector<cv::Point2f> points_top = findLabelPositions(img, threshold, false); if (!points_top.empty()) { for (int i = 0; i < 3*9; ++i) { candidate_points[i].push_back(points_top[i]); } } } std::vector<cv::Point2f> median_points; for (const auto& points : candidate_points) { assert(!points.empty()); std::vector<float> px; std::vector<double> py; for (const auto& point : points) { px.push_back(point.x); py.push_back(point.y); } size_t mid = points.size() / 2; std::nth_element(px.begin(), px.begin() + mid, px.end()); std::nth_element(py.begin(), py.begin() + mid, py.end()); median_points.emplace_back(px[mid], py[mid]); } return median_points; }
bool SnapIt::processModelCylinder(jsk_pcl_ros::CallSnapIt::Request& req, jsk_pcl_ros::CallSnapIt::Response& res) { pcl::PointCloud<pcl::PointXYZ>::Ptr candidate_points (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointIndices::Ptr inliers(new pcl::PointIndices); Eigen::Vector3f n, C_orig; if (!extractPointsInsideCylinder(req.request.center, req.request.direction, req.request.radius, req.request.height, inliers, n, C_orig, 1.3)) { return false; } if (inliers->indices.size() < 3) { NODELET_ERROR("not enough points inside of the target_plane"); return false; } geometry_msgs::PointStamped centroid; centroid.point.x = C_orig[0]; centroid.point.y = C_orig[1]; centroid.point.z = C_orig[2]; centroid.header = req.request.header; pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud(input_); extract.setIndices(inliers); extract.filter(*candidate_points); publishPointCloud(debug_candidate_points_pub_, candidate_points); // first, to remove plane we estimate the plane pcl::ModelCoefficients::Ptr plane_coefficients (new pcl::ModelCoefficients); pcl::PointIndices::Ptr plane_inliers (new pcl::PointIndices); extractPlanePoints(candidate_points, plane_inliers, plane_coefficients, n, req.request.eps_angle); if (plane_inliers->indices.size() == 0) { NODELET_ERROR ("plane estimation failed"); return false; } // remove the points blonging to the plane pcl::PointCloud<pcl::PointXYZ>::Ptr points_inside_pole_wo_plane (new pcl::PointCloud<pcl::PointXYZ>); extract.setInputCloud (candidate_points); extract.setIndices (plane_inliers); extract.setNegative (true); extract.filter (*points_inside_pole_wo_plane); publishPointCloud(debug_candidate_points_pub2_, points_inside_pole_wo_plane); // normal estimation pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>); pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne; pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ()); ne.setSearchMethod (tree); ne.setInputCloud (points_inside_pole_wo_plane); ne.setKSearch (50); ne.compute (*cloud_normals); // segmentation pcl::ModelCoefficients::Ptr cylinder_coefficients (new pcl::ModelCoefficients); pcl::PointIndices::Ptr cylinder_inliers (new pcl::PointIndices); // Create the segmentation object pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg; // Optional seg.setOptimizeCoefficients (true); // Mandatory seg.setModelType (pcl::SACMODEL_CYLINDER); seg.setMethodType (pcl::SAC_RANSAC); seg.setRadiusLimits (0.01, req.request.radius * 1.2); seg.setDistanceThreshold (0.05); seg.setInputCloud(points_inside_pole_wo_plane); seg.setInputNormals (cloud_normals); seg.setMaxIterations (10000); seg.setNormalDistanceWeight (0.1); seg.setAxis(n); if (req.request.eps_angle != 0.0) { seg.setEpsAngle(req.request.eps_angle); } else { seg.setEpsAngle(0.35); } seg.segment (*cylinder_inliers, *cylinder_coefficients); if (cylinder_inliers->indices.size () == 0) { NODELET_ERROR ("Could not estimate a cylinder model for the given dataset."); return false; } debug_centroid_pub_.publish(centroid); pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_points (new pcl::PointCloud<pcl::PointXYZ>); extract.setInputCloud (points_inside_pole_wo_plane); extract.setIndices (cylinder_inliers); extract.setNegative (false); extract.filter (*cylinder_points); publishPointCloud(debug_candidate_points_pub3_, cylinder_points); Eigen::Vector3f n_prime; Eigen::Vector3f C_new; for (size_t i = 0; i < 3; i++) { C_new[i] = cylinder_coefficients->values[i]; n_prime[i] = cylinder_coefficients->values[i + 3]; } double radius = fabs(cylinder_coefficients->values[6]); // inorder to compute centroid, we project all the points to the center line. // and after that, get the minimum and maximum points in the coordinate system of the center line double min_alpha = DBL_MAX; double max_alpha = -DBL_MAX; for (size_t i = 0; i < cylinder_points->points.size(); i++ ) { pcl::PointXYZ q = cylinder_points->points[i]; double alpha = (q.getVector3fMap() - C_new).dot(n_prime); if (alpha < min_alpha) { min_alpha = alpha; } if (alpha > max_alpha) { max_alpha = alpha; } } // the center of cylinder Eigen::Vector3f C_new_prime = C_new + (max_alpha + min_alpha) / 2.0 * n_prime; Eigen::Vector3f n_cross = n.cross(n_prime); if (n.dot(n_prime)) { n_cross = - n_cross; } double theta = asin(n_cross.norm()); Eigen::Quaternionf trans (Eigen::AngleAxisf(theta, n_cross.normalized())); Eigen::Vector3f C = trans * C_orig; Eigen::Affine3f A = Eigen::Translation3f(C) * Eigen::Translation3f(C_new_prime - C) * trans * Eigen::Translation3f(C_orig).inverse(); tf::poseEigenToMsg((Eigen::Affine3d)A, res.transformation); geometry_msgs::PointStamped centroid_transformed; centroid_transformed.point.x = C_new_prime[0]; centroid_transformed.point.y = C_new_prime[1]; centroid_transformed.point.z = C_new_prime[2]; centroid_transformed.header = req.request.header; debug_centroid_after_trans_pub_.publish(centroid_transformed); // publish marker visualization_msgs::Marker marker; marker.type = visualization_msgs::Marker::CYLINDER; marker.scale.x = radius; marker.scale.y = radius; marker.scale.z = (max_alpha - min_alpha); marker.pose.position.x = C_new_prime[0]; marker.pose.position.y = C_new_prime[1]; marker.pose.position.z = C_new_prime[2]; // n_prime -> z // n_cross.normalized() -> x Eigen::Vector3f z_axis = n_prime.normalized(); Eigen::Vector3f y_axis = n_cross.normalized(); Eigen::Vector3f x_axis = (y_axis.cross(z_axis)).normalized(); Eigen::Matrix3f M; for (size_t i = 0; i < 3; i++) { M(i, 0) = x_axis[i]; M(i, 1) = y_axis[i]; M(i, 2) = z_axis[i]; } Eigen::Quaternionf q (M); marker.pose.orientation.x = q.x(); marker.pose.orientation.y = q.y(); marker.pose.orientation.z = q.z(); marker.pose.orientation.w = q.w(); marker.color.g = 1.0; marker.color.a = 1.0; marker.header = input_header_; marker_pub_.publish(marker); return true; }