pcl_util.h
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 #ifndef JSK_RECOGNITION_UTILS_PCL_UTIL_H_
00037 #define JSK_RECOGNITION_UTILS_PCL_UTIL_H_
00038 #include <pcl/point_types.h>
00039 
00040 #include <boost/accumulators/accumulators.hpp>
00041 #include <boost/accumulators/statistics/stats.hpp>
00042 #include <boost/accumulators/statistics/min.hpp>
00043 #include <boost/accumulators/statistics/max.hpp>
00044 #include <boost/accumulators/statistics/variance.hpp>
00045 #include <boost/accumulators/statistics/count.hpp>
00046 
00047 #include <set>
00048 #include <map>
00049 
00050 #include <sensor_msgs/PointCloud2.h>
00051 #include <ros/ros.h>
00052 #include <pcl/PointIndices.h>
00053 #include <std_msgs/ColorRGBA.h>
00054 
00055 #include <jsk_topic_tools/time_accumulator.h>
00056 
00057 #include <diagnostic_updater/diagnostic_updater.h>
00058 #include <boost/circular_buffer.hpp>
00059 #include <jsk_topic_tools/vital_checker.h>
00060 
00061 #include <pcl/filters/extract_indices.h>
00062 #include <yaml-cpp/yaml.h>
00063 
00064 namespace jsk_recognition_utils
00065 {
00066   Eigen::Affine3f affineFromYAMLNode(const YAML::Node& pose);
00067   
00068   std::vector<int> addIndices(const std::vector<int>& a,
00069                               const std::vector<int>& b);
00070   pcl::PointIndices::Ptr addIndices(const pcl::PointIndices& a,
00071                                     const pcl::PointIndices& b);
00072   // substract indices like: a - b
00073   std::vector<int> subIndices(const std::vector<int>& a,
00074                               const std::vector<int>& b);
00075   pcl::PointIndices::Ptr subIndices(const pcl::PointIndices& a,
00076                                     const pcl::PointIndices& b);
00077 
00078   template <class PointT>
00079   std::vector<typename pcl::PointCloud<PointT> ::Ptr>
00080   convertToPointCloudArray(const typename pcl::PointCloud<PointT>::Ptr& cloud,
00081                            const std::vector<pcl::PointIndices::Ptr>& indices)
00082   {
00083     pcl::ExtractIndices<PointT> extract;
00084     extract.setInputCloud(cloud);
00085     std::vector<typename pcl::PointCloud<PointT> ::Ptr> cloud_array;
00086     for (size_t i = 0; i < indices.size(); i++) {
00087       typename pcl::PointCloud<PointT> ::Ptr
00088         segment (new pcl::PointCloud<PointT>);
00089       extract.setIndices(indices[i]);
00090       extract.filter(*segment);
00091       cloud_array.push_back(segment);
00092     }
00093     return cloud_array;
00094   }
00095 
00096 
00097   template <class T>
00098   pcl::PointCloud<pcl::PointXYZ>::Ptr convertToXYZCloud(const pcl::PointCloud<T>& cloud)
00099   {
00100     pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
00101     output->points.resize(cloud.points.size());
00102     for (size_t i = 0; i < cloud.points.size(); i++) {
00103       pcl::PointXYZ p;
00104       p.x = cloud.points[i].x;
00105       p.y = cloud.points[i].y;
00106       p.z = cloud.points[i].z;
00107       output->points[i] = p;
00108     }
00109     return output;
00110   }
00111   
00112   template<class T>
00113   void appendVector(std::vector<T>& a, const std::vector<T>& b)
00114   {
00115     for (size_t i = 0; i < b.size(); i++) {
00116       a.push_back(b[i]);
00117     }
00118   }
00119   
00120   // select color out of 20 colors
00121   std_msgs::ColorRGBA colorCategory20(int i);
00122 
00123   class Counter
00124   {
00125   public:
00126     typedef boost::accumulators::accumulator_set<
00127     double,
00128     boost::accumulators::stats<boost::accumulators::tag::count,
00129                                boost::accumulators::tag::mean,
00130                                boost::accumulators::tag::min,
00131                                boost::accumulators::tag::max,
00132                                boost::accumulators::tag::variance> > Accumulator;
00133     virtual void add(double v);
00134     virtual double mean();
00135     virtual double min();
00136     virtual double max();
00137     virtual int count();
00138     virtual double variance();
00139   protected:
00140     Accumulator acc_;
00141   };
00142 
00144   // Graph utility function
00146 
00148   // buildGroupFromGraphMap (recursive function)
00149   //   This function retrieves a directional graph, and build
00150   //   a set of the indices where can be arrived from a specified
00151   //   vertex.
00152   // 
00153   //   graph_map := A map representeing edges of the graph.
00154   //                The graph is bidirectional graph.
00155   //                the key means "from vertex" and the value
00156   //                means the "to indices" from the key vertex.
00157   //   from_index := The index to pay attension
00158   //   to_indices := The "to indices" from from_index
00159   //   output_set := result
00161   typedef std::map<int, std::vector<int> > IntegerGraphMap;
00162   
00163   void buildGroupFromGraphMap(IntegerGraphMap graph_map,
00164                               const int from_index,
00165                               std::vector<int>& to_indices,
00166                               std::set<int>& output_set);
00167   void _buildGroupFromGraphMap(IntegerGraphMap graph_map,
00168                               const int from_index,
00169                               std::vector<int>& to_indices,
00170                               std::set<int>& output_set);
00171   
00173   // buildAllGraphSetFromGraphMap
00174   //   get all the list of set represented in graph_map
00176   void buildAllGroupsSetFromGraphMap(IntegerGraphMap graph_map,
00177                                      std::vector<std::set<int> >& output_sets);
00178   
00180   // addSet<class>(A, B)
00181   //   add two set like A = A + B
00183   template <class T>
00184   void addSet(std::set<T>& output,
00185               const std::set<T>& new_set)
00186   {
00187     typedef typename std::set<T> Set;
00188     typedef typename Set::iterator Iterator;
00189     for (Iterator it = new_set.begin();
00190          it != new_set.end();
00191          ++it) {
00192       output.insert(*it);
00193     }
00194   }
00195 
00197   // add TimeAcumulator information to Diagnostics
00199   void addDiagnosticInformation(
00200     const std::string& string_prefix,
00201     jsk_topic_tools::TimeAccumulator& accumulator,
00202     diagnostic_updater::DiagnosticStatusWrapper& stat);
00203 
00205   // set error string to 
00207   void addDiagnosticErrorSummary(
00208     const std::string& string_prefix,
00209     jsk_topic_tools::VitalChecker::Ptr vital_checker,
00210     diagnostic_updater::DiagnosticStatusWrapper& stat);
00211 
00213   // add Boolean string to stat
00215   void addDiagnosticBooleanStat(
00216     const std::string& string_prefix,
00217     const bool value,
00218     diagnostic_updater::DiagnosticStatusWrapper& stat);
00219   
00221   // SeriesedBoolean
00222   //   store boolean value to limited buffer
00223   //   and return true if all the values are true.
00225   class SeriesedBoolean
00226   {
00227   public:
00228     typedef boost::shared_ptr<SeriesedBoolean> Ptr;
00229     SeriesedBoolean(const int buf_len);
00230     virtual ~SeriesedBoolean();
00231     virtual void addValue(bool val);
00232     virtual bool getValue();
00233     virtual bool isAllTrueFilled();
00234     virtual void clear();
00235   protected:
00236   private:
00237     boost::circular_buffer<bool> buf_;
00238     const int buf_len_;
00239   };
00240 
00242   // TimeredDiagnosticUpdater
00243   //   useful wrapper of DiagnosticUpdater.
00245   class TimeredDiagnosticUpdater
00246   {
00247   public:
00248     typedef boost::shared_ptr<TimeredDiagnosticUpdater> Ptr;
00249     TimeredDiagnosticUpdater(ros::NodeHandle& nh,
00250                              const ros::Duration& timer_duration);
00251     virtual ~TimeredDiagnosticUpdater();
00252     // wrapper methods of diagnostic_updater::Updater
00253     virtual void add(const std::string& name,
00254                      diagnostic_updater::TaskFunction f);
00255     //virtual void add(diagnostic_updater::DiagnosticTask task);
00256     virtual void start();
00257     virtual void setHardwareID(const std::string& name);
00258     virtual void update();
00259   protected:
00260     virtual void timerCallback(const ros::TimerEvent& event);
00261     ros::Timer timer_;
00262     boost::shared_ptr<diagnostic_updater::Updater> diagnostic_updater_;
00263   private:
00264     
00265   };
00266   
00267   extern boost::mutex global_chull_mutex;
00268   
00269 }
00270 
00271 #endif


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