cylinder.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2014, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 #define BOOST_PARAMETER_MAX_ARITY 7
37 
40 
41 namespace jsk_recognition_utils
42 {
43  Cylinder::Cylinder(Eigen::Vector3f point, Eigen::Vector3f direction, double radius):
44  point_(point), direction_(direction), radius_(radius)
45  {
46 
47  }
48 
49  void Cylinder::filterPointCloud(const pcl::PointCloud<pcl::PointXYZ>& cloud,
50  const double threshold,
51  pcl::PointIndices& output)
52  {
53  Line line(direction_, point_);
54  output.indices.clear();
55  for (size_t i = 0; i < cloud.points.size(); i++) {
56  Eigen::Vector3f p = cloud.points[i].getVector3fMap();
57  double d = line.distanceToPoint(p);
58  if (d < radius_ + threshold && d > radius_ - threshold) {
59  output.indices.push_back(i);
60  }
61  }
62  }
63 
64  void Cylinder::estimateCenterAndHeight(const pcl::PointCloud<pcl::PointXYZ>& cloud,
65  const pcl::PointIndices& indices,
66  Eigen::Vector3f& center,
67  double& height)
68  {
69  Line line(direction_, point_);
70  Vertices points;
71  for (size_t i = 0; i < indices.indices.size(); i++) {
72  int point_index = indices.indices[i];
73  points.push_back(cloud.points[point_index].getVector3fMap());
74  }
75  PointPair min_max = line.findEndPoints(points);
76  Eigen::Vector3f min_point = min_max.get<0>();
77  Eigen::Vector3f max_point = min_max.get<1>();
78  Eigen::Vector3f min_point_projected, max_point_projected;
79  line.foot(min_point, min_point_projected);
80  line.foot(max_point, max_point_projected);
81  height = (min_point_projected - max_point_projected).norm();
82  center = (min_point_projected + max_point_projected) / 2.0;
83  }
84 
85  void Cylinder::toMarker(visualization_msgs::Marker& marker,
86  const Eigen::Vector3f& center,
87  const Eigen::Vector3f& uz,
88  const double height)
89  {
90  marker.type = visualization_msgs::Marker::CYLINDER;
91  marker.pose.position.x = center[0];
92  marker.pose.position.y = center[1];
93  marker.pose.position.z = center[2];
94  Eigen::Vector3f orig_z(0, 0, 1);
95  Eigen::Quaternionf q;
96  q.setFromTwoVectors(orig_z, uz);
97  marker.pose.orientation.x = q.x();
98  marker.pose.orientation.y = q.y();
99  marker.pose.orientation.z = q.z();
100  marker.pose.orientation.w = q.w();
101  marker.scale.x = radius_ * 2;
102  marker.scale.y = radius_ * 2;
103  marker.scale.z = height;
104  marker.color.a = 1.0;
105  marker.color.g = 1.0;
106  marker.color.b = 1.0;
107  }
108 
109  Eigen::Vector3f Cylinder::getDirection()
110  {
111  return direction_;
112  }
113 
114 }
Cylinder(Eigen::Vector3f point, Eigen::Vector3f direction, double radius)
Definition: cylinder.cpp:43
d
Definition: setup.py:9
virtual void foot(const Eigen::Vector3f &point, Eigen::Vector3f &output) const
compute a point which gives perpendicular projection.
Definition: line.cpp:69
virtual void toMarker(visualization_msgs::Marker &marker, const Eigen::Vector3f &center, const Eigen::Vector3f &uz, const double height)
Definition: cylinder.cpp:85
virtual PointPair findEndPoints(const Vertices &points) const
Extract end points from the points on the lines.
Definition: line.cpp:173
virtual Eigen::Vector3f getDirection()
Definition: cylinder.cpp:109
q
virtual void filterPointCloud(const pcl::PointCloud< pcl::PointXYZ > &cloud, const double threshold, pcl::PointIndices &output)
Definition: cylinder.cpp:49
virtual double distanceToPoint(const Eigen::Vector3f &from) const
compute a distance to a point
Definition: line.cpp:82
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > Vertices
Definition: types.h:48
virtual void estimateCenterAndHeight(const pcl::PointCloud< pcl::PointXYZ > &cloud, const pcl::PointIndices &indices, Eigen::Vector3f &center, double &height)
Definition: cylinder.cpp:64
boost::tuple< Point, Point > PointPair
Definition: types.h:49
p
Eigen::Vector3f direction_
Definition: cylinder.h:68
Class to represent 3-D straight line.
Definition: line.h:49


jsk_recognition_utils
Author(s):
autogenerated on Wed May 27 2020 03:59:48