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
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
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
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
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;
00053 int gap_threshold = 5;
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
00073
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
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 }