pcl_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_PCL_UTIL_H_
37 #define JSK_RECOGNITION_UTILS_PCL_UTIL_H_
38 #include <pcl/point_types.h>
39 
40 #include <boost/accumulators/accumulators.hpp>
41 #include <boost/accumulators/statistics/stats.hpp>
42 #include <boost/accumulators/statistics/min.hpp>
43 #include <boost/accumulators/statistics/max.hpp>
44 #include <boost/accumulators/statistics/variance.hpp>
45 #include <boost/accumulators/statistics/count.hpp>
46 
47 #include <set>
48 #include <map>
49 
50 #include <sensor_msgs/PointCloud2.h>
51 #include <ros/ros.h>
52 #include <pcl/PointIndices.h>
53 #include <std_msgs/ColorRGBA.h>
54 
57 
58 #include <boost/circular_buffer.hpp>
60 
61 #include <pcl/filters/extract_indices.h>
62 #include <yaml-cpp/yaml.h>
63 
64 namespace jsk_recognition_utils
65 {
66  Eigen::Affine3f affineFromYAMLNode(const YAML::Node& pose);
67 
68  std::vector<int> addIndices(const std::vector<int>& a,
69  const std::vector<int>& b);
70  pcl::PointIndices::Ptr addIndices(const pcl::PointIndices& a,
71  const pcl::PointIndices& b);
72  // substract indices like: a - b
73  std::vector<int> subIndices(const std::vector<int>& a,
74  const std::vector<int>& b);
75  pcl::PointIndices::Ptr subIndices(const pcl::PointIndices& a,
76  const pcl::PointIndices& b);
77 
78  template <class PointT>
79  std::vector<typename pcl::PointCloud<PointT> ::Ptr>
80  convertToPointCloudArray(const typename pcl::PointCloud<PointT>::Ptr& cloud,
81  const std::vector<pcl::PointIndices::Ptr>& indices)
82  {
83  pcl::ExtractIndices<PointT> extract;
84  extract.setInputCloud(cloud);
85  std::vector<typename pcl::PointCloud<PointT> ::Ptr> cloud_array;
86  for (size_t i = 0; i < indices.size(); i++) {
87  typename pcl::PointCloud<PointT> ::Ptr
88  segment (new pcl::PointCloud<PointT>);
89  extract.setIndices(indices[i]);
90  extract.filter(*segment);
91  cloud_array.push_back(segment);
92  }
93  return cloud_array;
94  }
95 
96 
97  template <class T>
98  pcl::PointCloud<pcl::PointXYZ>::Ptr convertToXYZCloud(const pcl::PointCloud<T>& cloud)
99  {
100  pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
101  output->points.resize(cloud.points.size());
102  for (size_t i = 0; i < cloud.points.size(); i++) {
103  pcl::PointXYZ p;
104  p.x = cloud.points[i].x;
105  p.y = cloud.points[i].y;
106  p.z = cloud.points[i].z;
107  output->points[i] = p;
108  }
109  return output;
110  }
111 
112  template<class T>
113  void appendVector(std::vector<T>& a, const std::vector<T>& b)
114  {
115  for (size_t i = 0; i < b.size(); i++) {
116  a.push_back(b[i]);
117  }
118  }
119 
120  // select color out of 20 colors
121  std_msgs::ColorRGBA colorCategory20(int i);
122 
123  class Counter
124  {
125  public:
126  typedef boost::accumulators::accumulator_set<
127  double,
128  boost::accumulators::stats<boost::accumulators::tag::count,
129  boost::accumulators::tag::mean,
130  boost::accumulators::tag::min,
131  boost::accumulators::tag::max,
132  boost::accumulators::tag::variance> > Accumulator;
133  virtual void add(double v);
134  virtual double mean();
135  virtual double min();
136  virtual double max();
137  virtual int count();
138  virtual double variance();
139  protected:
140  Accumulator acc_;
141  };
142 
144  // Graph utility function
146 
148  // buildGroupFromGraphMap (recursive function)
149  // This function retrieves a directional graph, and build
150  // a set of the indices where can be arrived from a specified
151  // vertex.
152  //
153  // graph_map := A map representeing edges of the graph.
154  // The graph is bidirectional graph.
155  // the key means "from vertex" and the value
156  // means the "to indices" from the key vertex.
157  // from_index := The index to pay attension
158  // to_indices := The "to indices" from from_index
159  // output_set := result
161  typedef std::map<int, std::vector<int> > IntegerGraphMap;
162 
163  void buildGroupFromGraphMap(IntegerGraphMap graph_map,
164  const int from_index,
165  std::vector<int>& to_indices,
166  std::set<int>& output_set);
167  void _buildGroupFromGraphMap(IntegerGraphMap graph_map,
168  const int from_index,
169  std::vector<int>& to_indices,
170  std::set<int>& output_set);
171 
173  // buildAllGraphSetFromGraphMap
174  // get all the list of set represented in graph_map
176  void buildAllGroupsSetFromGraphMap(IntegerGraphMap graph_map,
177  std::vector<std::set<int> >& output_sets);
178 
180  // addSet<class>(A, B)
181  // add two set like A = A + B
183  template <class T>
184  void addSet(std::set<T>& output,
185  const std::set<T>& new_set)
186  {
187  typedef typename std::set<T> Set;
188  typedef typename Set::iterator Iterator;
189  for (Iterator it = new_set.begin();
190  it != new_set.end();
191  ++it) {
192  output.insert(*it);
193  }
194  }
195 
197  // SeriesedBoolean
198  // store boolean value to limited buffer
199  // and return true if all the values are true.
202  {
203  public:
205  SeriesedBoolean(const int buf_len);
206  virtual ~SeriesedBoolean();
207  virtual void addValue(bool val);
208  virtual bool getValue();
209  virtual bool isAllTrueFilled();
210  virtual void clear();
211  protected:
212  private:
213  boost::circular_buffer<bool> buf_;
214  const int buf_len_;
215  };
216 
218  // TimeredDiagnosticUpdater
219  // useful wrapper of DiagnosticUpdater.
222 
223  extern boost::mutex global_chull_mutex;
224 
225 }
226 
227 #endif
boost::circular_buffer< bool > buf_
Definition: pcl_util.h:213
pcl::PointCloud< pcl::PointXYZ >::Ptr convertToXYZCloud(const pcl::PointCloud< T > &cloud)
Definition: pcl_util.h:98
jsk_topic_tools::TimeredDiagnosticUpdater TimeredDiagnosticUpdater
Definition: pcl_util.h:221
boost::mutex global_chull_mutex
Definition: pcl_util.cpp:43
std::vector< typename pcl::PointCloud< PointT >::Ptr > convertToPointCloudArray(const typename pcl::PointCloud< PointT >::Ptr &cloud, const std::vector< pcl::PointIndices::Ptr > &indices)
Definition: pcl_util.h:80
void _buildGroupFromGraphMap(IntegerGraphMap graph_map, const int from_index, std::vector< int > &to_indices, std::set< int > &output_set)
Definition: pcl_util.cpp:164
std::map< int, std::vector< int > > IntegerGraphMap
Definition: pcl_util.h:161
void appendVector(std::vector< T > &a, const std::vector< T > &b)
Definition: pcl_util.h:113
boost::shared_ptr< SeriesedBoolean > Ptr
Definition: pcl_util.h:204
Eigen::Affine3f affineFromYAMLNode(const YAML::Node &pose)
Definition: pcl_util.cpp:45
std::vector< int > addIndices(const std::vector< int > &a, const std::vector< int > &b)
Definition: pcl_util.cpp:62
virtual void add(double v)
Definition: pcl_util.cpp:103
boost::accumulators::accumulator_set< double, boost::accumulators::stats< boost::accumulators::tag::count, boost::accumulators::tag::mean, boost::accumulators::tag::min, boost::accumulators::tag::max, boost::accumulators::tag::variance > > Accumulator
Definition: pcl_util.h:132
void buildAllGroupsSetFromGraphMap(IntegerGraphMap graph_map, std::vector< std::set< int > > &output_sets)
Definition: pcl_util.cpp:184
void buildGroupFromGraphMap(IntegerGraphMap graph_map, const int from_index, std::vector< int > &to_indices, std::set< int > &output_set)
Definition: pcl_util.cpp:133
void addSet(std::set< T > &output, const std::set< T > &new_set)
Definition: pcl_util.h:184
p
std_msgs::ColorRGBA colorCategory20(int i)
std::vector< int > subIndices(const std::vector< int > &a, const std::vector< int > &b)
Definition: pcl_util.cpp:81


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