cylindrical_shell.cpp
Go to the documentation of this file.
00001 #include <handle_detector/cylindrical_shell.h>
00002 
00003 void CylindricalShell::fitCylinder(const PointCloud::Ptr &cloud, const std::vector<int> &indices,
00004                                    const Eigen::Vector3d &normal, const Eigen::Vector3d &curvature_axis)
00005 {
00006   // rotate points into axis aligned coordinate frame
00007   int n = indices.size();
00008   Eigen::Matrix3d R;
00009   R.col(0) = normal;
00010   R.col(1) = curvature_axis;
00011   R.col(2) = normal.cross(curvature_axis);
00012   Eigen::Matrix<double, 3, Eigen::Dynamic> pts(3, n);
00013   for (int i = 0; i < n; i++)
00014     pts.col(i) << cloud->points[indices[i]].x, cloud->points[indices[i]].y, cloud->points[indices[i]].z;
00015   Eigen::MatrixXd Rpts = R.transpose() * pts;
00016 
00017   // fit circle
00018   Eigen::RowVectorXd x = Rpts.row(0);
00019   Eigen::RowVectorXd y = Rpts.row(1);
00020   Eigen::RowVectorXd z = Rpts.row(2);
00021   Eigen::RowVectorXd x2 = x.cwiseProduct(x);
00022   Eigen::RowVectorXd z2 = z.cwiseProduct(z);
00023   Eigen::RowVectorXd one = Eigen::MatrixXd::Ones(1, n);
00024 
00025   Eigen::MatrixXd l(3, n);
00026   l << x, z, one;
00027   Eigen::RowVectorXd gamma = x2 + z2;
00028 
00029   Eigen::Matrix3d Q = l * l.transpose();
00030   Eigen::Vector3d c = ((gamma.replicate(3, 1)).cwiseProduct(l)).rowwise().sum();
00031 
00032   Eigen::Vector3d paramOut = -1 * Q.inverse() * c;
00033 
00034   // compute circle parameters
00035   Eigen::Vector2d circleCenter = -0.5 * paramOut.segment(0, 2);
00036   double circleRadius = sqrt(0.25 * (paramOut(0) * paramOut(0) + paramOut(1) * paramOut(1)) - paramOut(2));
00037   double axisCoord = y.sum() / n;
00038 
00039   // get cylinder parameters from circle parameters
00040   Eigen::Vector3d centroid_cyl_no_rot;
00041   centroid_cyl_no_rot << circleCenter(0), axisCoord, circleCenter(1);
00042   this->centroid = R * centroid_cyl_no_rot;
00043   this->radius = circleRadius;
00044   this->extent = y.maxCoeff() - y.minCoeff();
00045   this->curvature_axis = curvature_axis;
00046   this->normal = normal;
00047 }
00048 
00049 bool CylindricalShell::hasClearance(const PointCloud::Ptr &cloud, const std::vector<int>& nn_indices,
00050                                     double maxHandAperture, double handleGap)
00051 {
00052   int min_points_inner = 40; // min number of points required to be within the inner cylinder
00053   int gap_threshold = 5; // threshold below which the gap is considered to be large enough
00054 
00055   std::vector<int> cropped_indices;
00056   for (std::size_t i = 0; i < nn_indices.size(); i++)
00057   {
00058     Eigen::Vector3d cropped = cloud->points[nn_indices[i]].getVector3fMap().cast<double>();
00059     double axialDist = this->curvature_axis.dot(cropped - centroid);
00060     if (fabs(axialDist) < this->extent / 2)
00061       cropped_indices.push_back(i);
00062   }
00063 
00064   Eigen::Matrix<double, 3, Eigen::Dynamic> croppedPts(3, cropped_indices.size());
00065   for (std::size_t i = 0; i < cropped_indices.size(); i++)
00066     croppedPts.col(i) = cloud->points[nn_indices[cropped_indices[i]]].getVector3fMap().cast<double>();
00067 
00068   Eigen::MatrixXd normalDiff = (Eigen::MatrixXd::Identity(3, 3) - curvature_axis * curvature_axis.transpose())
00069       * (croppedPts - centroid.replicate(1, croppedPts.cols()));
00070   Eigen::VectorXd normalDist = normalDiff.cwiseProduct(normalDiff).colwise().sum().cwiseSqrt();
00071 
00072   /* increase cylinder radius until number of points in gap is smaller than <gap_threshold> and
00073    * number of points within the inner cylinder is larger than <min_points_inner> */
00074   for (double r = this->radius; r <= maxHandAperture; r += 0.001)
00075   {
00076     int numInGap = ((normalDist.array() > r) * (normalDist.array() < r + handleGap) == true).count();
00077     int numInside = (normalDist.array() <= r).count();
00078     //~ printf("numInGap: %i, numInside: %i, \n", numInGap, numInside);
00079 
00080     if (numInGap < gap_threshold && numInside > min_points_inner)
00081     {
00082       this->radius = r;
00083       return true;
00084     }
00085   }
00086 
00087   return false;
00088 }


handle_detector
Author(s):
autogenerated on Thu Jun 6 2019 17:36:23