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 #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 #include <jsk_topic_tools/timered_diagnostic_updater.h>
00057
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
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
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
00146
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
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
00174
00176 void buildAllGroupsSetFromGraphMap(IntegerGraphMap graph_map,
00177 std::vector<std::set<int> >& output_sets);
00178
00180
00181
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
00198
00199
00201 class SeriesedBoolean
00202 {
00203 public:
00204 typedef boost::shared_ptr<SeriesedBoolean> Ptr;
00205 SeriesedBoolean(const int buf_len);
00206 virtual ~SeriesedBoolean();
00207 virtual void addValue(bool val);
00208 virtual bool getValue();
00209 virtual bool isAllTrueFilled();
00210 virtual void clear();
00211 protected:
00212 private:
00213 boost::circular_buffer<bool> buf_;
00214 const int buf_len_;
00215 };
00216
00218
00219
00221 typedef jsk_topic_tools::TimeredDiagnosticUpdater TimeredDiagnosticUpdater;
00222
00223 extern boost::mutex global_chull_mutex;
00224
00225 }
00226
00227 #endif