Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include "jsk_recognition_utils/pcl_util.h"
00037 #include <set>
00038 #include <algorithm>
00039
00040 namespace jsk_recognition_utils
00041 {
00042
00043 boost::mutex global_chull_mutex;
00044
00045 Eigen::Affine3f affineFromYAMLNode(const YAML::Node& pose)
00046 {
00047 float x, y, z, rx, ry, rz, rw;
00048 #ifdef USE_OLD_YAML
00049 pose[0] >> x; pose[1] >> y; pose[2] >> z;
00050 pose[3] >> rx; pose[4] >> ry; pose[5] >> rz; pose[6] >> rw;
00051 #else
00052 x = pose[0].as<float>(); y = pose[1].as<float>(); z = pose[2].as<float>();
00053 rx= pose[3].as<float>(); ry= pose[4].as<float>(); rz= pose[5].as<float>(); rw = pose[6].as<float>();
00054 #endif
00055 Eigen::Vector3f p(x, y, z);
00056 Eigen::Quaternionf q(rw, rx, ry, rz);
00057 Eigen::Affine3f trans = Eigen::Translation3f(p) * Eigen::AngleAxisf(q);
00058 return trans;
00059 }
00060
00061
00062 std::vector<int> addIndices(const std::vector<int>& a,
00063 const std::vector<int>& b)
00064 {
00065 std::set<int> all(b.begin(), b.end());
00066 for (size_t i = 0; i < a.size(); i++) {
00067 all.insert(a[i]);
00068 }
00069 return std::vector<int>(all.begin(), all.end());
00070 }
00071
00072 pcl::PointIndices::Ptr addIndices(const pcl::PointIndices& a,
00073 const pcl::PointIndices& b)
00074 {
00075 std::vector<int> indices = addIndices(a.indices, b.indices);
00076 pcl::PointIndices::Ptr ret(new pcl::PointIndices);
00077 ret->indices = indices;
00078 return ret;
00079 }
00080
00081 std::vector<int> subIndices(const std::vector<int>& a,
00082 const std::vector<int>& b)
00083 {
00084 std::set<int> all(a.begin(), a.end());
00085 for (size_t i = 0; i < b.size(); i++) {
00086 std::set<int>::iterator it = all.find(b[i]);
00087 if (it != all.end()) {
00088 all.erase(it);
00089 }
00090 }
00091 return std::vector<int>(all.begin(), all.end());
00092 }
00093
00094 pcl::PointIndices::Ptr subIndices(const pcl::PointIndices& a,
00095 const pcl::PointIndices& b)
00096 {
00097 std::vector<int> indices = subIndices(a.indices, b.indices);
00098 pcl::PointIndices::Ptr ret(new pcl::PointIndices);
00099 ret->indices = indices;
00100 return ret;
00101 }
00102
00103 void Counter::add(double v)
00104 {
00105 acc_(v);
00106 }
00107
00108 double Counter::mean()
00109 {
00110 return boost::accumulators::mean(acc_);
00111 }
00112
00113 double Counter::min()
00114 {
00115 return boost::accumulators::min(acc_);
00116 }
00117
00118 double Counter::max()
00119 {
00120 return boost::accumulators::max(acc_);
00121 }
00122
00123 int Counter::count()
00124 {
00125 return boost::accumulators::count(acc_);
00126 }
00127
00128 double Counter::variance()
00129 {
00130 return boost::accumulators::variance(acc_);
00131 }
00132
00133 void buildGroupFromGraphMap(IntegerGraphMap graph_map,
00134 const int from_index_arg,
00135 std::vector<int>& to_indices_arg,
00136 std::set<int>& output_set)
00137 {
00138
00139 IntegerGraphMap onedirectional_map(graph_map);
00140 for (IntegerGraphMap::iterator it = onedirectional_map.begin();
00141 it != onedirectional_map.end();
00142 ++it) {
00143 int from_index = it->first;
00144 std::vector<int> to_indices = it->second;
00145 for (size_t i = 0; i < to_indices.size(); i++) {
00146 int to_index = to_indices[i];
00147 if (onedirectional_map.find(to_index) == onedirectional_map.end()) {
00148
00149 onedirectional_map[to_index] = std::vector<int>();
00150 }
00151 if (std::find(onedirectional_map[to_index].begin(),
00152 onedirectional_map[to_index].end(),
00153 from_index) == onedirectional_map[to_index].end()) {
00154 onedirectional_map[to_index].push_back(from_index);
00155 }
00156 }
00157 }
00158 _buildGroupFromGraphMap(onedirectional_map,
00159 from_index_arg,
00160 to_indices_arg,
00161 output_set);
00162 }
00163
00164 void _buildGroupFromGraphMap(IntegerGraphMap graph_map,
00165 const int from_index,
00166 std::vector<int>& to_indices,
00167 std::set<int>& output_set)
00168 {
00169 output_set.insert(from_index);
00170 for (size_t i = 0; i < to_indices.size(); i++) {
00171 int to_index = to_indices[i];
00172 if (output_set.find(to_index) == output_set.end()) {
00173 output_set.insert(to_index);
00174
00175 std::vector<int> next_indices = graph_map[to_index];
00176 _buildGroupFromGraphMap(graph_map,
00177 to_index,
00178 next_indices,
00179 output_set);
00180 }
00181 }
00182 }
00183
00184 void buildAllGroupsSetFromGraphMap(IntegerGraphMap graph_map,
00185 std::vector<std::set<int> >& output_sets)
00186 {
00187 std::set<int> duplication_check_set;
00188 for (IntegerGraphMap::iterator it = graph_map.begin();
00189 it != graph_map.end();
00190 ++it) {
00191 int from_index = it->first;
00192 if (duplication_check_set.find(from_index)
00193 == duplication_check_set.end()) {
00194 std::set<int> new_graph_set;
00195 buildGroupFromGraphMap(graph_map, from_index, it->second,
00196 new_graph_set);
00197 output_sets.push_back(new_graph_set);
00198
00199 addSet<int>(duplication_check_set, new_graph_set);
00200 }
00201 }
00202 }
00203
00204 SeriesedBoolean::SeriesedBoolean(const int buf_len):
00205 buf_(buf_len), buf_len_(buf_len)
00206 {
00207 }
00208
00209 SeriesedBoolean::~SeriesedBoolean()
00210 {
00211 }
00212
00213 void SeriesedBoolean::addValue(bool val)
00214 {
00215 buf_.push_front(val);
00216 }
00217
00218 bool SeriesedBoolean::isAllTrueFilled()
00219 {
00220 return (buf_.size() == buf_len_ && getValue());
00221 }
00222
00223 bool SeriesedBoolean::getValue()
00224 {
00225 if (buf_.size() == 0) {
00226 return false;
00227 }
00228 else {
00229 for (boost::circular_buffer<bool>::iterator it = buf_.begin();
00230 it != buf_.end();
00231 ++it) {
00232 if (!*it) {
00233 return false;
00234 }
00235 }
00236 return true;
00237 }
00238 }
00239
00240 void SeriesedBoolean::clear()
00241 {
00242 buf_.clear();
00243 }
00244 }
00245