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