pcl_conversion_util.h
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2014, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 #ifndef JSK_RECOGNITION_UTILS_PCL_CONVERSION_UTIL_H_
37 #define JSK_RECOGNITION_UTILS_PCL_CONVERSION_UTIL_H_
38 #include <pcl/point_types.h>
39 #include <pcl/point_cloud.h>
41 #include <geometry_msgs/Point32.h>
44 #include <pcl/range_image/range_image_planar.h>
45 #include <visualization_msgs/Marker.h>
46 #if ROS_VERSION_MINIMUM(1, 10, 0)
47 // hydro and later
48 typedef pcl_msgs::PointIndices PCLIndicesMsg;
49 typedef pcl_msgs::ModelCoefficients PCLModelCoefficientMsg;
50 #else
51 // groovy
52 typedef pcl::PointIndices PCLIndicesMsg;
53 typedef pcl::ModelCoefficients PCLModelCoefficientMsg;
54 #endif
55 
56 #include <opencv2/opencv.hpp>
57 
58 namespace jsk_recognition_utils
59 {
60 
69  void rangeImageToCvMat(const pcl::RangeImage& range_image,
70  cv::Mat& mat);
71 
72  template<class FromT, class ToT>
73  void pointFromXYZToVector(const FromT& msg,
74  ToT& p)
75  {
76  p[0] = msg.x; p[1] = msg.y; p[2] = msg.z;
77  }
78 
79  template<class FromT, class ToT>
80  void pointFromVectorToXYZ(const FromT& p,
81  ToT& msg)
82  {
83  msg.x = p[0]; msg.y = p[1]; msg.z = p[2];
84  }
85 
86  template<class FromT, class ToT>
87  void pointFromXYZToXYZ(const FromT& from,
88  ToT& to)
89  {
90  to.x = from.x; to.y = from.y; to.z = from.z;
91  }
92 
93  template<class FromT, class ToT>
94  void pointFromVectorToVector(const FromT& from,
95  ToT& to)
96  {
97  to[0] = from[0]; to[1] = from[1]; to[2] = from[2];
98  }
99 
100  template<class FromT, class ToT>
101  void convertMatrix4(const FromT& from,
102  ToT& to)
103  {
104  for (size_t i = 0; i < 4; i++) {
105  for (size_t j = 0; j < 4; j++) {
106  to(i, j) = from(i, j);
107  }
108  }
109  }
110 
111  void convertEigenAffine3(const Eigen::Affine3d& from,
112  Eigen::Affine3f& to);
113  void convertEigenAffine3(const Eigen::Affine3f& from,
114  Eigen::Affine3d& to);
115 
116  template <class PointT>
117  void markerMsgToPointCloud(const visualization_msgs::Marker& input_marker,
118  int sample_nums,
119  pcl::PointCloud<PointT>& output_cloud)
120  {
121  std::vector<double> cumulative_areas;
122  double total_area = 0;
123 
124  if (input_marker.points.size() != input_marker.colors.size()){
125  ROS_ERROR("Color and Points nums is different in markerMsgToPointCloud");
126  return;
127  }
128 
129  //Gether the triangle areas
130  for (int i = 0; i < (int)input_marker.points.size()/3; i++){
131  geometry_msgs::Point p0_1;
132  p0_1.x = input_marker.points[i*3].x - input_marker.points[i*3+2].x;
133  p0_1.y = input_marker.points[i*3].y - input_marker.points[i*3+2].y;
134  p0_1.z = input_marker.points[i*3].z - input_marker.points[i*3+2].z;
135  geometry_msgs::Point p1_2;
136  p1_2.x = input_marker.points[i*3+1].x - input_marker.points[i*3+2].x;
137  p1_2.y = input_marker.points[i*3+1].y - input_marker.points[i*3+2].y;
138  p1_2.z = input_marker.points[i*3+1].z - input_marker.points[i*3+2].z;
139  geometry_msgs::Point outer_product;
140  outer_product.x = p0_1.y * p1_2.z - p0_1.z * p1_2.y;
141  outer_product.y = p0_1.z * p1_2.x - p0_1.x * p1_2.z;
142  outer_product.z = p0_1.x * p1_2.y - p0_1.y * p1_2.x;
143  double tmp_triangle_area = abs(sqrt( pow(outer_product.x*1000, 2) + pow(outer_product.y*1000, 2) + pow(outer_product.z*1000, 2)))/2;
144  total_area += tmp_triangle_area;
145  cumulative_areas.push_back(total_area);
146  }
147 
148  //Gether Random sampling points in propotion to area size
149  for(int i = 0; i < sample_nums; i++){
150  double r = rand() * (1.0 / (RAND_MAX + 1.0)) * total_area;
151  std::vector<double>::iterator low = std::lower_bound (cumulative_areas.begin (), cumulative_areas.end (), r);
152  int index = int(low - cumulative_areas.begin ());
153 
154  //Get Target Triangle
155  PointT p;
156  std_msgs::ColorRGBA color;
157  geometry_msgs::Point p0 = input_marker.points[index*3];
158  geometry_msgs::Point p1 = input_marker.points[index*3+1];
159  geometry_msgs::Point p2 = input_marker.points[index*3+2];
160  std_msgs::ColorRGBA c0= input_marker.colors[index*3];
161  std_msgs::ColorRGBA c1 = input_marker.colors[index*3+1];
162  std_msgs::ColorRGBA c2 = input_marker.colors[index*3+2];
163  r = rand() * (1.0 / (RAND_MAX + 1.0));
164 
165  geometry_msgs::Point point_on_p1_p2;
166  point_on_p1_p2.x = p1.x*r + p2.x*(1.0 - r);
167  point_on_p1_p2.y = p1.y*r + p2.y*(1.0 - r);
168  point_on_p1_p2.z = p1.z*r + p2.z*(1.0 - r);
169 
170  color.r = c1.r*r + c2.r*(1.0 - r);
171  color.g = c1.g*r + c2.g*(1.0 - r);
172  color.b = c1.b*r + c2.b*(1.0 - r);
173 
174  r = sqrt(rand() * (1.0 / (RAND_MAX + 1.0)));
175  geometry_msgs::Point target;
176  target.x = point_on_p1_p2.x*r + p0.x*(1.0 - r);
177  target.y = point_on_p1_p2.y*r + p0.y*(1.0 - r);
178  target.z = point_on_p1_p2.z*r + p0.z*(1.0 - r);
179  color.r = color.r*r + c0.r*(1.0 - r);
180  color.g = color.g*r + c0.g*(1.0 - r);
181  color.b = color.b*r + c0.b*(1.0 - r);
182  p.x = target.x;
183  p.y = target.y;
184  p.z = target.z;
185  p.r = color.r * 256;
186  p.g = color.g * 256;
187  p.b = color.b * 256;
188 
189  output_cloud.points.push_back(p);
190  }
191  output_cloud.width = sample_nums;
192  output_cloud.height = 1;
193  }
194 
195  inline bool isValidPoint(const pcl::PointXYZ& p)
196  {
197  return !std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.z);
198  }
199 
200  template <class PointT>
201  inline bool isValidPoint(const PointT& p)
202  {
203  return !std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.z);
204  }
205 
206 }
207 // extend pcl_conversions package's toPCL and fromPCL functions
208 namespace pcl_conversions
209 {
210  std::vector<pcl::PointIndices::Ptr>
211  convertToPCLPointIndices(const std::vector<PCLIndicesMsg>& cluster_indices);
212 
213  std::vector<pcl::ModelCoefficients::Ptr>
215  const std::vector<PCLModelCoefficientMsg>& coefficients);
216 
217  std::vector<PCLIndicesMsg>
219  const std::vector<pcl::PointIndices::Ptr> cluster_indices,
220  const std_msgs::Header& header);
221 
222  std::vector<PCLIndicesMsg>
224  const std::vector<pcl::PointIndices> cluster_indices,
225  const std_msgs::Header& header);
226 
227  std::vector<PCLModelCoefficientMsg>
229  const std::vector<pcl::ModelCoefficients::Ptr>& coefficients,
230  const std_msgs::Header& header);
231 
232 }
233 
234 namespace tf
235 {
236  // for eigen float
237  void poseMsgToEigen(const geometry_msgs::Pose& msg, Eigen::Affine3f& eigen);
238  void poseEigenToMsg(Eigen::Affine3f& eigen, geometry_msgs::Pose& msg);
239  void transformMsgToEigen(const geometry_msgs::Transform& msg, Eigen::Affine3f& eigen);
240  void transformEigenToMsg(Eigen::Affine3f& eigen, geometry_msgs::Transform& msg);
241  void transformTFToEigen(const tf::Transform& t, Eigen::Affine3f& eigen);
242  void transformEigenToTF(Eigen::Affine3f& eigen , tf::Transform& t);
243  void vectorTFToEigen(const tf::Vector3& t, Eigen::Vector3f& e);
244  void vectorEigenToTF(const Eigen::Vector3f& e, tf::Vector3& t);
245 }
246 
247 #endif
msg
t
void transformEigenToMsg(Eigen::Affine3f &eigen, geometry_msgs::Transform &msg)
void pointFromVectorToXYZ(const FromT &p, ToT &msg)
void pointFromXYZToXYZ(const FromT &from, ToT &to)
void transformEigenToTF(Eigen::Affine3f &eigen, tf::Transform &t)
std::vector< PCLModelCoefficientMsg > convertToROSModelCoefficients(const std::vector< pcl::ModelCoefficients::Ptr > &coefficients, const std_msgs::Header &header)
std::vector< pcl::ModelCoefficients::Ptr > convertToPCLModelCoefficients(const std::vector< PCLModelCoefficientMsg > &coefficients)
void convertMatrix4(const FromT &from, ToT &to)
void poseMsgToEigen(const geometry_msgs::Pose &msg, Eigen::Affine3f &eigen)
std::vector< pcl::PointIndices::Ptr > convertToPCLPointIndices(const std::vector< PCLIndicesMsg > &cluster_indices)
r
void transformMsgToEigen(const geometry_msgs::Transform &msg, Eigen::Affine3f &eigen)
void markerMsgToPointCloud(const visualization_msgs::Marker &input_marker, int sample_nums, pcl::PointCloud< PointT > &output_cloud)
void rangeImageToCvMat(const pcl::RangeImage &range_image, cv::Mat &mat)
Convert pcl::RangeImage to cv::Mat. Distance is normalized to 0-1 and colorized.
void convertEigenAffine3(const Eigen::Affine3d &from, Eigen::Affine3f &to)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
void poseEigenToMsg(Eigen::Affine3f &eigen, geometry_msgs::Pose &msg)
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
void vectorEigenToTF(const Eigen::Vector3f &e, tf::Vector3 &t)
std::vector< PCLIndicesMsg > convertToROSPointIndices(const std::vector< pcl::PointIndices > cluster_indices, const std_msgs::Header &header)
void pointFromVectorToVector(const FromT &from, ToT &to)
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
void transformTFToEigen(const tf::Transform &t, Eigen::Affine3f &eigen)
p
bool isValidPoint(const pcl::PointXYZ &p)
pcl::PointIndices PCLIndicesMsg
void vectorTFToEigen(const tf::Vector3 &t, Eigen::Vector3f &e)
#define ROS_ERROR(...)
void pointFromXYZToVector(const FromT &msg, ToT &p)
pcl::ModelCoefficients PCLModelCoefficientMsg


jsk_recognition_utils
Author(s):
autogenerated on Mon May 3 2021 03:03:03