geo_util.h
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 #ifndef JSK_RECOGNITION_UTILS_GEO_UTIL_H_
37 #define JSK_RECOGNITION_UTILS_GEO_UTIL_H_
38 //#define BOOST_PARAMETER_MAX_ARITY 7
39 
40 #include <Eigen/Core>
41 #include <Eigen/Geometry>
42 #include <Eigen/StdVector>
43 
44 #include <vector>
45 
46 #include <boost/shared_ptr.hpp>
47 #include <boost/thread.hpp>
48 #include <geometry_msgs/Polygon.h>
49 #include <jsk_recognition_msgs/BoundingBox.h>
50 #include <jsk_recognition_msgs/SimpleOccupancyGrid.h>
51 #include <boost/tuple/tuple.hpp>
52 
54 // PCL headers
56 #include <pcl/point_types.h>
57 #include <pcl/point_cloud.h>
58 #include <pcl/PointIndices.h>
59 #include <pcl/filters/extract_indices.h>
60 #include <pcl/ModelCoefficients.h>
61 #include <pcl/filters/project_inliers.h>
62 #include <pcl/surface/concave_hull.h>
63 #include <visualization_msgs/Marker.h>
64 
67 
68 #include <jsk_recognition_msgs/PolygonArray.h>
80 
81 // Utitlity macros
82 inline void ROS_INFO_EIGEN_VECTOR3(const std::string& prefix,
83  const Eigen::Vector3f& v) {
84  ROS_INFO("%s: [%f, %f, %f]", prefix.c_str(), v[0], v[1], v[2]);
85 }
86 
87 namespace jsk_recognition_utils
88 {
90  // compute quaternion from 3 unit vector
91  // these vector should be normalized and diagonal
93  Eigen::Quaternionf rotFrom3Axis(const Eigen::Vector3f& ex,
94  const Eigen::Vector3f& ey,
95  const Eigen::Vector3f& ez);
100  template<class PointT>
101  typename pcl::PointCloud<PointT>::Ptr verticesToPointCloud(const Vertices& v)
102  {
103  typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
104  for (size_t i = 0; i < v.size(); i++) {
105  PointT p;
106  // Do not use pointFromVectorToXYZ in order not to depend on
107  // pcl_conversion_util
108  //pointFromVectorToXYZ<Eigen::Vector3f, PointT>(v[i], p);
109  p.x = v[i][0];
110  p.y = v[i][1];
111  p.z = v[i][2];
112  cloud->points.push_back(p);
113  }
114  return cloud;
115  }
116 
121  template<class PointT>
122  Vertices pointCloudToVertices(const pcl::PointCloud<PointT>& cloud)
123  {
124  Vertices vs;
125  for (size_t i = 0; i < cloud.points.size(); i++) {
126  Eigen::Vector3f p(cloud.points[i].getVector3fMap());
127  vs.push_back(p);
128  }
129  return vs;
130  }
131 
132  // geoemtry classes
133 
134  std::vector<Plane::Ptr> convertToPlanes(
135  std::vector<pcl::ModelCoefficients::Ptr>);
136 
137  template <class PointT>
138  jsk_recognition_msgs::BoundingBox boundingBoxFromPointCloud(const pcl::PointCloud<PointT>& cloud)
139  {
140  Eigen::Vector4f minpt, maxpt;
141  pcl::getMinMax3D<PointT>(cloud, minpt, maxpt);
142  jsk_recognition_msgs::BoundingBox bbox;
143  bbox.dimensions.x = std::abs(minpt[0] - maxpt[0]);
144  bbox.dimensions.y = std::abs(minpt[1] - maxpt[1]);
145  bbox.dimensions.z = std::abs(minpt[2] - maxpt[2]);
146  bbox.pose.position.x = (minpt[0] + maxpt[0]) / 2.0;
147  bbox.pose.position.y = (minpt[1] + maxpt[1]) / 2.0;
148  bbox.pose.position.z = (minpt[2] + maxpt[2]) / 2.0;
149  bbox.pose.orientation.w = 1.0;
150  return bbox;
151  }
152 }
153 
154 #endif
std::vector< Plane::Ptr > convertToPlanes(std::vector< pcl::ModelCoefficients::Ptr >)
Definition: polygon.cpp:76
void ROS_INFO_EIGEN_VECTOR3(const std::string &prefix, const Eigen::Vector3f &v)
Definition: geo_util.h:82
pcl::PointCloud< PointT >::Ptr verticesToPointCloud(const Vertices &v)
Compute PointCloud from Vertices.
Definition: geo_util.h:101
Eigen::Quaternionf rotFrom3Axis(const Eigen::Vector3f &ex, const Eigen::Vector3f &ey, const Eigen::Vector3f &ez)
Definition: geo_util.cpp:55
#define ROS_INFO(...)
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > Vertices
Definition: types.h:48
Vertices pointCloudToVertices(const pcl::PointCloud< PointT > &cloud)
Compute Vertices from PointCloud.
Definition: geo_util.h:122
jsk_recognition_msgs::BoundingBox boundingBoxFromPointCloud(const pcl::PointCloud< PointT > &cloud)
Definition: geo_util.h:138
p


jsk_recognition_utils
Author(s):
autogenerated on Fri Dec 6 2019 04:02:51