pcl_util.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 
37 #include <set>
38 #include <algorithm>
39 
40 namespace jsk_recognition_utils
41 {
42  //static boost::mutex global_chull_mutex;
43  boost::mutex global_chull_mutex;
44 
45  Eigen::Affine3f affineFromYAMLNode(const YAML::Node& pose)
46  {
47  float x, y, z, rx, ry, rz, rw;
48 #ifdef USE_OLD_YAML
49  pose[0] >> x; pose[1] >> y; pose[2] >> z;
50  pose[3] >> rx; pose[4] >> ry; pose[5] >> rz; pose[6] >> rw;
51 #else
52  x = pose[0].as<float>(); y = pose[1].as<float>(); z = pose[2].as<float>();
53  rx= pose[3].as<float>(); ry= pose[4].as<float>(); rz= pose[5].as<float>(); rw = pose[6].as<float>();
54 #endif
55  Eigen::Vector3f p(x, y, z);
56  Eigen::Quaternionf q(rw, rx, ry, rz);
57  Eigen::Affine3f trans = Eigen::Translation3f(p) * Eigen::AngleAxisf(q);
58  return trans;
59  }
60 
61 
62  std::vector<int> addIndices(const std::vector<int>& a,
63  const std::vector<int>& b)
64  {
65  std::set<int> all(b.begin(), b.end());
66  for (size_t i = 0; i < a.size(); i++) {
67  all.insert(a[i]);
68  }
69  return std::vector<int>(all.begin(), all.end());
70  }
71 
72  pcl::PointIndices::Ptr addIndices(const pcl::PointIndices& a,
73  const pcl::PointIndices& b)
74  {
75  std::vector<int> indices = addIndices(a.indices, b.indices);
76  pcl::PointIndices::Ptr ret(new pcl::PointIndices);
77  ret->indices = indices;
78  return ret;
79  }
80 
81  std::vector<int> subIndices(const std::vector<int>& a,
82  const std::vector<int>& b)
83  {
84  std::set<int> all(a.begin(), a.end());
85  for (size_t i = 0; i < b.size(); i++) {
86  std::set<int>::iterator it = all.find(b[i]);
87  if (it != all.end()) {
88  all.erase(it);
89  }
90  }
91  return std::vector<int>(all.begin(), all.end());
92  }
93 
94  pcl::PointIndices::Ptr subIndices(const pcl::PointIndices& a,
95  const pcl::PointIndices& b)
96  {
97  std::vector<int> indices = subIndices(a.indices, b.indices);
98  pcl::PointIndices::Ptr ret(new pcl::PointIndices);
99  ret->indices = indices;
100  return ret;
101  }
102 
103  void Counter::add(double v)
104  {
105  acc_(v);
106  }
107 
108  double Counter::mean()
109  {
110  return boost::accumulators::mean(acc_);
111  }
112 
113  double Counter::min()
114  {
115  return boost::accumulators::min(acc_);
116  }
117 
118  double Counter::max()
119  {
120  return boost::accumulators::max(acc_);
121  }
122 
124  {
125  return boost::accumulators::count(acc_);
126  }
127 
129  {
130  return boost::accumulators::variance(acc_);
131  }
132 
134  const int from_index_arg,
135  std::vector<int>& to_indices_arg,
136  std::set<int>& output_set)
137  {
138  // convert graph_map into one-directional representation
139  IntegerGraphMap onedirectional_map(graph_map);
140  for (IntegerGraphMap::iterator it = onedirectional_map.begin();
141  it != onedirectional_map.end();
142  ++it) {
143  int from_index = it->first;
144  std::vector<int> to_indices = it->second;
145  for (size_t i = 0; i < to_indices.size(); i++) {
146  int to_index = to_indices[i];
147  if (onedirectional_map.find(to_index) == onedirectional_map.end()) {
148  // not yet initialized
149  onedirectional_map[to_index] = std::vector<int>();
150  }
151  if (std::find(onedirectional_map[to_index].begin(),
152  onedirectional_map[to_index].end(),
153  from_index) == onedirectional_map[to_index].end()) {
154  onedirectional_map[to_index].push_back(from_index);
155  }
156  }
157  }
158  _buildGroupFromGraphMap(onedirectional_map,
159  from_index_arg,
160  to_indices_arg,
161  output_set);
162  }
163 
165  const int from_index,
166  std::vector<int>& to_indices,
167  std::set<int>& output_set)
168  {
169  output_set.insert(from_index);
170  for (size_t i = 0; i < to_indices.size(); i++) {
171  int to_index = to_indices[i];
172  if (output_set.find(to_index) == output_set.end()) {
173  output_set.insert(to_index);
174  //std::cout << "__connection__: " << from_index << " --> " << to_index << std::endl;
175  std::vector<int> next_indices = graph_map[to_index];
176  _buildGroupFromGraphMap(graph_map,
177  to_index,
178  next_indices,
179  output_set);
180  }
181  }
182  }
183 
185  std::vector<std::set<int> >& output_sets)
186  {
187  std::set<int> duplication_check_set;
188  for (IntegerGraphMap::iterator it = graph_map.begin();
189  it != graph_map.end();
190  ++it) {
191  int from_index = it->first;
192  if (duplication_check_set.find(from_index)
193  == duplication_check_set.end()) {
194  std::set<int> new_graph_set;
195  buildGroupFromGraphMap(graph_map, from_index, it->second,
196  new_graph_set);
197  output_sets.push_back(new_graph_set);
198  // update duplication_check_set
199  addSet<int>(duplication_check_set, new_graph_set);
200  }
201  }
202  }
203 
205  buf_(buf_len), buf_len_(buf_len)
206  {
207  }
208 
210  {
211  }
212 
214  {
215  buf_.push_front(val);
216  }
217 
219  {
220  return (buf_.size() == buf_len_ && getValue());
221  }
222 
224  {
225  if (buf_.size() == 0) {
226  return false;
227  }
228  else {
229  for (boost::circular_buffer<bool>::iterator it = buf_.begin();
230  it != buf_.end();
231  ++it) {
232  if (!*it) {
233  return false;
234  }
235  }
236  return true;
237  }
238  }
239 
241  {
242  buf_.clear();
243  }
244 }
245 
boost::circular_buffer< bool > buf_
Definition: pcl_util.h:213
boost::mutex global_chull_mutex
Definition: pcl_util.cpp:43
void _buildGroupFromGraphMap(IntegerGraphMap graph_map, const int from_index, std::vector< int > &to_indices, std::set< int > &output_set)
Definition: pcl_util.cpp:164
virtual void addValue(bool val)
Definition: pcl_util.cpp:213
std::map< int, std::vector< int > > IntegerGraphMap
Definition: pcl_util.h:161
q
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
x
virtual void add(double v)
Definition: pcl_util.cpp:103
y
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
p
std::vector< int > subIndices(const std::vector< int > &a, const std::vector< int > &b)
Definition: pcl_util.cpp:81
z


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