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   void addDiagnosticInformation(
00205     const std::string& string_prefix,
00206     jsk_topic_tools::TimeAccumulator& accumulator,
00207     diagnostic_updater::DiagnosticStatusWrapper& stat)
00208   {
00209     stat.add(string_prefix + " (Avg.)", accumulator.mean());
00210     if (accumulator.mean() != 0.0) {
00211       stat.add(string_prefix + " (Avg., fps)", 1.0 / accumulator.mean());
00212     }
00213     stat.add(string_prefix + " (Max)", accumulator.max());
00214     stat.add(string_prefix + " (Min)", accumulator.min());
00215     stat.add(string_prefix + " (Var.)", accumulator.variance());
00216   }
00217 
00218   void addDiagnosticErrorSummary(
00219     const std::string& string_prefix,
00220     jsk_topic_tools::VitalChecker::Ptr vital_checker,
00221     diagnostic_updater::DiagnosticStatusWrapper& stat)
00222   {
00223     stat.summary(
00224       diagnostic_msgs::DiagnosticStatus::ERROR,
00225       (boost::format("%s not running for %f sec")
00226        % string_prefix % vital_checker->deadSec()).str());
00227   }
00228   
00229   void addDiagnosticBooleanStat(
00230     const std::string& string_prefix,
00231     const bool value,
00232     diagnostic_updater::DiagnosticStatusWrapper& stat)
00233   {
00234     if (value) {
00235       stat.add(string_prefix, "True");
00236     }
00237     else {
00238       stat.add(string_prefix, "False");
00239     }
00240   }
00241 
00242   SeriesedBoolean::SeriesedBoolean(const int buf_len):
00243     buf_(buf_len), buf_len_(buf_len)
00244   {
00245   }
00246   
00247   SeriesedBoolean::~SeriesedBoolean()
00248   {
00249   }
00250 
00251   void SeriesedBoolean::addValue(bool val)
00252   {
00253     buf_.push_front(val);
00254   }
00255 
00256   bool SeriesedBoolean::isAllTrueFilled()
00257   {
00258     return (buf_.size() == buf_len_ && getValue());
00259   }
00260   
00261   bool SeriesedBoolean::getValue()
00262   {
00263     if (buf_.size() == 0) {
00264       return false;
00265     }
00266     else {
00267       for (boost::circular_buffer<bool>::iterator it = buf_.begin();
00268            it != buf_.end();
00269            ++it) {
00270         if (!*it) {
00271           return false;
00272         }
00273       }
00274       return true;
00275     }
00276   }
00277 
00278   void SeriesedBoolean::clear()
00279   {
00280     buf_.clear();
00281   }
00282 
00283   TimeredDiagnosticUpdater::TimeredDiagnosticUpdater(
00284     ros::NodeHandle& nh,
00285     const ros::Duration& timer_duration):
00286     diagnostic_updater_(new diagnostic_updater::Updater)
00287   {
00288     timer_ = nh.createTimer(
00289       timer_duration, boost::bind(
00290         &TimeredDiagnosticUpdater::timerCallback,
00291         this,
00292         _1));
00293     timer_.stop();
00294   }
00295   
00296   void TimeredDiagnosticUpdater::start()
00297   {
00298     timer_.start();
00299   }
00300 
00301   TimeredDiagnosticUpdater::~TimeredDiagnosticUpdater()
00302   {
00303   }
00304 
00305   void TimeredDiagnosticUpdater::setHardwareID(const std::string& name)
00306   {
00307     diagnostic_updater_->setHardwareID(name);
00308   }
00309   
00310   void TimeredDiagnosticUpdater::add(const std::string& name,
00311                                      diagnostic_updater::TaskFunction f)
00312   {
00313     diagnostic_updater_->add(name, f);
00314   }
00315   
00316   // void TimeredDiagnosticUpdater::add(diagnostic_updater::DiagnosticTask task)
00317   // {
00318   //   diagnostic_updater_->add(task);
00319   // }
00320 
00321   void TimeredDiagnosticUpdater::update()
00322   {
00323     diagnostic_updater_->update();
00324   }
00325   
00326   void TimeredDiagnosticUpdater::timerCallback(const ros::TimerEvent& event)
00327   {
00328     update();
00329   }
00330   
00331 }
00332 


jsk_recognition_utils
Author(s):
autogenerated on Sun Oct 8 2017 02:42:48