pcl_util.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #include "jsk_recognition_utils/pcl_util.h"
00037 #include <set>
00038 #include <algorithm>
00039 
00040 namespace jsk_recognition_utils
00041 {
00042   //static boost::mutex global_chull_mutex;
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     // convert graph_map into one-directional representation
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           // not yet initialized
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         //std::cout << "__connection__: " << from_index << " --> " << to_index << std::endl;
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         // update duplication_check_set
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 


jsk_recognition_utils
Author(s):
autogenerated on Tue Jul 2 2019 19:40:37