pcl_conversion_util.cpp
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 
37 #include <pcl/visualization/common/float_image_utils.h>
38 
39 namespace jsk_recognition_utils
40 {
41 
42  void rangeImageToCvMat(const pcl::RangeImage& range_image,
43  cv::Mat& image)
44  {
45  float min_range, max_range;
46  range_image.getMinMaxRanges(min_range, max_range);
47  float min_max_range = max_range - min_range;
48 
49  image = cv::Mat(range_image.height, range_image.width, CV_8UC3);
50  unsigned char r,g,b;
51  for (int y=0; y < range_image.height; y++) {
52  for (int x=0; x<range_image.width; x++) {
53  pcl::PointWithRange rangePt = range_image.getPoint(x,y);
54  if (!pcl_isfinite(rangePt.range)) {
55  pcl::visualization::FloatImageUtils::getColorForFloat(
56  rangePt.range, r, g, b);
57  }
58  else {
59  float value = (rangePt.range - min_range) / min_max_range;
60  pcl::visualization::FloatImageUtils::getColorForFloat(
61  value, r, g, b);
62  }
63  image.at<cv::Vec3b>(y,x)[0] = b;
64  image.at<cv::Vec3b>(y,x)[1] = g;
65  image.at<cv::Vec3b>(y,x)[2] = r;
66  }
67  }
68  return;
69  }
70 
71  void convertEigenAffine3(const Eigen::Affine3d& from,
72  Eigen::Affine3f& to)
73  {
74  Eigen::Matrix4d from_mat = from.matrix();
75  Eigen::Matrix4f to_mat;
76  convertMatrix4<Eigen::Matrix4d, Eigen::Matrix4f>(from_mat, to_mat);
77  to = Eigen::Affine3f(to_mat);
78  }
79 
80  void convertEigenAffine3(const Eigen::Affine3f& from,
81  Eigen::Affine3d& to)
82  {
83  Eigen::Matrix4f from_mat = from.matrix();
84  Eigen::Matrix4d to_mat;
85  convertMatrix4<Eigen::Matrix4f, Eigen::Matrix4d>(from_mat, to_mat);
86  to = Eigen::Affine3d(to_mat);
87  }
88 }
89 
90 namespace pcl_conversions
91 {
92  std::vector<pcl::PointIndices::Ptr>
94  const std::vector<PCLIndicesMsg>& cluster_indices)
95  {
96  std::vector<pcl::PointIndices::Ptr> ret;
97  for (size_t i = 0; i < cluster_indices.size(); i++) {
98  std::vector<int> indices = cluster_indices[i].indices;
99  pcl::PointIndices::Ptr pcl_indices (new pcl::PointIndices);
100  pcl_indices->indices = indices;
101  ret.push_back(pcl_indices);
102  }
103  return ret;
104  }
105 
106  std::vector<pcl::ModelCoefficients::Ptr>
108  const std::vector<PCLModelCoefficientMsg>& coefficients)
109  {
110  std::vector<pcl::ModelCoefficients::Ptr> ret;
111  for (size_t i = 0; i < coefficients.size(); i++) {
112  pcl::ModelCoefficients::Ptr pcl_coefficients (new pcl::ModelCoefficients);
113  pcl_coefficients->values = coefficients[i].values;
114  ret.push_back(pcl_coefficients);
115  }
116  return ret;
117  }
118 
119  std::vector<PCLIndicesMsg>
121  const std::vector<pcl::PointIndices::Ptr> cluster_indices,
122  const std_msgs::Header& header)
123  {
124  std::vector<PCLIndicesMsg> ret;
125  for (size_t i = 0; i < cluster_indices.size(); i++) {
126  PCLIndicesMsg ros_msg;
127  ros_msg.header = header;
128  ros_msg.indices = cluster_indices[i]->indices;
129  ret.push_back(ros_msg);
130  }
131  return ret;
132  }
133 
134  std::vector<PCLIndicesMsg>
136  const std::vector<pcl::PointIndices> cluster_indices,
137  const std_msgs::Header& header)
138  {
139  std::vector<PCLIndicesMsg> ret;
140  for (size_t i = 0; i < cluster_indices.size(); i++) {
141  PCLIndicesMsg ros_msg;
142  ros_msg.header = header;
143  ros_msg.indices = cluster_indices[i].indices;
144  ret.push_back(ros_msg);
145  }
146  return ret;
147  }
148 
149  std::vector<PCLModelCoefficientMsg>
151  const std::vector<pcl::ModelCoefficients::Ptr>& coefficients,
152  const std_msgs::Header& header)
153  {
154  std::vector<PCLModelCoefficientMsg> ret;
155  for (size_t i = 0; i < coefficients.size(); i++) {
156  PCLModelCoefficientMsg ros_msg;
157  ros_msg.header = header;
158  ros_msg.values = coefficients[i]->values;
159  ret.push_back(ros_msg);
160  }
161  return ret;
162  }
163 
164 }
165 
166 
167 namespace tf
168 {
169  void poseMsgToEigen(const geometry_msgs::Pose& msg, Eigen::Affine3f& eigen)
170  {
171  Eigen::Affine3d eigen_d;
172  poseMsgToEigen(msg, eigen_d);
174  }
175 
176  void poseEigenToMsg(Eigen::Affine3f& eigen, geometry_msgs::Pose& msg)
177  {
178  Eigen::Affine3d eigen_d;
180  poseEigenToMsg(eigen_d, msg);
181  }
182 
183  void transformMsgToEigen(const geometry_msgs::Transform& msg, Eigen::Affine3f& eigen)
184  {
185  Eigen::Affine3d eigen_d;
186  transformMsgToEigen(msg, eigen_d);
188  }
189 
190  void transformEigenToMsg(Eigen::Affine3f& eigen, geometry_msgs::Transform& msg)
191  {
192  Eigen::Affine3d eigen_d;
194  transformEigenToMsg(eigen_d, msg);
195  }
196 
197  void transformTFToEigen(const tf::Transform& t, Eigen::Affine3f& eigen)
198  {
199  Eigen::Affine3d eigen_d;
200  transformTFToEigen(t, eigen_d);
202  }
203 
204  void transformEigenToTF(Eigen::Affine3f& eigen , tf::Transform& t)
205  {
206  Eigen::Affine3d eigen_d;
208  transformEigenToTF(eigen_d, t);
209  }
210 
211  void vectorTFToEigen(const tf::Vector3& t, Eigen::Vector3f& e)
212  {
213  Eigen::Vector3d d;
214  tf::vectorTFToEigen(t, d);
215  e[0] = d[0];
216  e[1] = d[1];
217  e[2] = d[2];
218  }
219 
220  void vectorEigenToTF(const Eigen::Vector3f& e, tf::Vector3& t)
221  {
222  Eigen::Vector3d d(e[0], e[1], e[2]);
223  tf::vectorEigenToTF(d, t);
224  }
225 }
d
Definition: setup.py:9
void transformEigenToMsg(Eigen::Affine3f &eigen, geometry_msgs::Transform &msg)
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 poseMsgToEigen(const geometry_msgs::Pose &msg, Eigen::Affine3f &eigen)
void vectorEigenToTF(const Eigen::Vector3d &e, tf::Vector3 &t)
std::vector< pcl::PointIndices::Ptr > convertToPCLPointIndices(const std::vector< PCLIndicesMsg > &cluster_indices)
r
void transformMsgToEigen(const geometry_msgs::Transform &msg, Eigen::Affine3f &eigen)
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)
x
y
std::vector< PCLIndicesMsg > convertToROSPointIndices(const std::vector< pcl::PointIndices::Ptr > cluster_indices, const std_msgs::Header &header)
void poseEigenToMsg(Eigen::Affine3f &eigen, geometry_msgs::Pose &msg)
void vectorTFToEigen(const tf::Vector3 &t, Eigen::Vector3d &e)
void vectorEigenToTF(const Eigen::Vector3f &e, tf::Vector3 &t)
void transformTFToEigen(const tf::Transform &t, Eigen::Affine3f &eigen)
pcl::PointIndices PCLIndicesMsg
void vectorTFToEigen(const tf::Vector3 &t, Eigen::Vector3f &e)
pcl::ModelCoefficients PCLModelCoefficientMsg


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