cylindrical_shell.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2014, Andreas ten Pas
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  *
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
20  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
21  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
22  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
23  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
24  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
25  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
26  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
27  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
28  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29  * POSSIBILITY OF SUCH DAMAGE.
30  */
31 
32 #ifndef CYLINDRICAL_SHELL_H
33 #define CYLINDRICAL_SHELL_H
34 
35 #include "Eigen/Dense"
36 #include <pcl/kdtree/kdtree_flann.h>
37 #include <pcl/point_cloud.h>
38 #include <pcl/search/organized.h>
39 
40 typedef pcl::PointCloud<pcl::PointXYZRGBA> PointCloud;
41 
50 {
51 public:
52 
61  void
62  fitCylinder(const PointCloud::Ptr &cloud, const std::vector<int> &indices, const Eigen::Vector3d &normal,
63  const Eigen::Vector3d &curvature_axis);
64 
71  bool
72  hasClearance(const PointCloud::Ptr &cloud, const std::vector<int>& nn_indices, double maxHandAperture,
73  double handleGap);
74 
77  inline double getExtent() const
78  {
79  return this->extent;
80  }
81  ;
82 
86  inline void setExtent(double extent)
87  {
88  this->extent = extent;
89  }
90  ;
91 
94  inline double getRadius() const
95  {
96  return this->radius;
97  }
98  ;
99 
103  inline int getNeighborhoodCentroidIndex() const
104  {
105  return this->neighborhood_centroid_index;
106  }
107  ;
108 
113  inline void setNeighborhoodCentroidIndex(int index)
114  {
115  this->neighborhood_centroid_index = index;
116  }
117  ;
118 
121  inline Eigen::Vector3d getCentroid() const
122  {
123  return this->centroid;
124  }
125  ;
126 
129  inline Eigen::Vector3d getCurvatureAxis() const
130  {
131  return this->curvature_axis;
132  }
133  ;
134 
137  inline Eigen::Vector3d getNormal() const
138  {
139  return this->normal;
140  }
141  ;
142 
143 private:
144 
145  Eigen::Vector3d centroid;
146  Eigen::Vector3d curvature_axis;
147  double extent;
148  double radius;
149  Eigen::Vector3d normal;
151 };
152 
153 #endif
double getExtent() const
Get the extent of the cylindrical shell.
CylindricalShell represents a cylindrical shell that consists of two colinear cylinders. A shell consists of an inner and an outer cylinder. The portion of the object to be grasped must fit inside the inner cylinder, and the radius of that cylinder must be no larger than the maximum hand aperture. The gap between the inner and outer cylinder must be free of obstacles and wide enough to be able to contain the robot fingers.
Eigen::Vector3d centroid
void setExtent(double extent)
Set the extent of the cylindrical shell.
void fitCylinder(const PointCloud::Ptr &cloud, const std::vector< int > &indices, const Eigen::Vector3d &normal, const Eigen::Vector3d &curvature_axis)
Fit a cylinder to a set of points in the cloud, using their indices, and the normal and the curvature...
bool hasClearance(const PointCloud::Ptr &cloud, const std::vector< int > &nn_indices, double maxHandAperture, double handleGap)
Check whether the gap between the inner and outer cylinder of the shell is free of obstacles and wide...
Eigen::Vector3d getNormal() const
Get the normal axis of the cylindrical shell.
Eigen::Vector3d normal
Eigen::Vector3d getCurvatureAxis() const
Get the curvature axis of the cylindrical shell.
pcl::PointCloud< pcl::PointXYZRGBA > PointCloud
Eigen::Vector3d getCentroid() const
Get the centroid of the cylindrical shell.
double getRadius() const
Get the radius of the cylindrical shell.
Eigen::Vector3d curvature_axis
void setNeighborhoodCentroidIndex(int index)
Set the index of the centroid of the neighborhood associated with the cylindrical shell...
int getNeighborhoodCentroidIndex() const
Get the index of the centroid of the neighborhood associated with the cylindrical shell...


handle_detector
Author(s):
autogenerated on Mon Jun 10 2019 13:29:00