PointCloudConversions.h
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package VisionX::Tools
19  * @author Markus Grotz ( markus dot grotz at kit dot edu )
20  * @date 2015
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 #pragma once
25 
26 #include <cassert>
27 #include <iostream>
28 
29 #include <pcl/point_cloud.h>
30 #include <pcl/point_types.h>
31 
32 #include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
33 
34 #include <VisionX/interface/core/DataTypes.h>
35 
36 namespace visionx::tools
37 {
38 
39  template <typename PointT>
40  visionx::PointContentType
41  getPointContentType()
42  {
43  return visionx::ePoints;
44  }
45 
46  template <>
47  visionx::PointContentType getPointContentType<pcl::PointXYZ>();
48  template <>
49  visionx::PointContentType getPointContentType<pcl::PointXYZI>();
50  template <>
51  visionx::PointContentType getPointContentType<pcl::PointXYZL>();
52  template <>
53  visionx::PointContentType getPointContentType<pcl::PointNormal>();
54  template <>
55  visionx::PointContentType getPointContentType<pcl::PointXYZRGBA>();
56  template <>
57  visionx::PointContentType getPointContentType<pcl::PointXYZRGBL>();
58  template <>
59  visionx::PointContentType getPointContentType<pcl::PointXYZRGBNormal>();
60 
61 
62  size_t getBytesPerPoint(visionx::PointContentType pointContent);
63  std::string toString(visionx::PointContentType pointContent);
64 
65 
66  /// Indicate whether this point type contains colors.
67  bool isColored(visionx::PointContentType type);
68  /// Indicate whether this point type contains labels.
69  bool isLabeled(visionx::PointContentType type);
70 
71 
72  /**
73  * @param sourcePtr point to the source pcl point cloud
74  */
75  //template<typename PointT>
76  // void convertFromPCL(typename pcl::PointCloud<PointT>::Ptr sourcePtr, void **target)
77  void convertFromPCL(pcl::PointCloud<pcl::PointXYZ>::Ptr sourcePtr, void** target);
78 
79 
80  /**
81  * @param targetPtr point to the pcl point cloud
82  */
83  //template<typename PointT>
84  //void convertToPCL(void **source, visionx::MetaPointCloudFormatPtr pointCloudFormat, typename pcl::PointCloud<PointT>::Ptr targetPtr)
85  void convertToPCL(void** source,
86  visionx::MetaPointCloudFormatPtr pointCloudFormat,
87  pcl::PointCloud<pcl::PointXYZ>::Ptr targetPtr);
88 
89  void convertToPCL(const visionx::ColoredPointCloud& source,
90  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr targetPtr);
91  void convertFromPCL(const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr sourcePtr,
92  visionx::ColoredPointCloud& target);
93 
94  void convertFromPCL(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr sourcePtr, void** target);
95  void convertToPCL(void** source,
96  visionx::MetaPointCloudFormatPtr pointCloudFormat,
97  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr targetPtr);
98 
99  void convertFromPCL(pcl::PointCloud<pcl::PointXYZL>::Ptr sourcePtr, void** target);
100  void convertToPCL(void** source,
101  visionx::MetaPointCloudFormatPtr pointCloudFormat,
102  pcl::PointCloud<pcl::PointXYZL>::Ptr targetPtr);
103 
104  void convertFromPCL(pcl::PointCloud<pcl::PointXYZI>::Ptr sourcePtr, void** target);
105  void convertToPCL(void** source,
106  visionx::MetaPointCloudFormatPtr pointCloudFormat,
107  pcl::PointCloud<pcl::PointXYZI>::Ptr targetPtr);
108 
109 
110  void convertFromPCL(pcl::PointCloud<pcl::PointXYZRGBL>::Ptr sourcePtr, void** target);
111  void convertToPCL(void** source,
112  visionx::MetaPointCloudFormatPtr pointCloudFormat,
113  pcl::PointCloud<pcl::PointXYZRGBL>::Ptr targetPtr);
114 
115  void convertFromPCL(pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& sourcePtr, void** target);
116  void convertToPCL(void** source,
117  visionx::MetaPointCloudFormatPtr pointCloudFormat,
118  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr targetPtr);
119 
120  void convertFromPCL(pcl::PointCloud<pcl::PointXYZRGB>::Ptr sourcePtr, void** target);
121  void convertToPCL(void** source,
122  visionx::MetaPointCloudFormatPtr pointCloudFormat,
123  pcl::PointCloud<pcl::PointXYZRGB>::Ptr targetPtr);
124 
125  enum PCLToDebugDrawerConversionMode
126  {
127  eConvertAsPoints,
128  eConvertAsColors,
129  eConvertAsLabels,
130  };
131 
132  void convertFromPCLToDebugDrawer(const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& sourcePtr,
133  armarx::DebugDrawer24BitColoredPointCloud& target,
134  PCLToDebugDrawerConversionMode mode = eConvertAsPoints,
135  float pointSize = 3);
136  void convertFromPCLToDebugDrawer(const pcl::PointCloud<pcl::PointXYZL>::Ptr& sourcePtr,
137  armarx::DebugDrawer24BitColoredPointCloud& target,
138  PCLToDebugDrawerConversionMode mode = eConvertAsPoints,
139  float pointSize = 3);
140  void convertFromPCLToDebugDrawer(const pcl::PointCloud<pcl::PointXYZ>::Ptr& sourcePtr,
141  armarx::DebugDrawer24BitColoredPointCloud& target,
142  PCLToDebugDrawerConversionMode mode = eConvertAsPoints,
143  float pointSize = 3);
144  void convertFromPCLToDebugDrawer(const pcl::PointCloud<pcl::PointXYZRGBL>::Ptr& sourcePtr,
145  armarx::DebugDrawer24BitColoredPointCloud& target,
146  PCLToDebugDrawerConversionMode mode = eConvertAsPoints,
147  float pointSize = 3);
148 
149 
150  // TEMPLATE BASED POINT CLOUD CONVERSION between PCL and VisionX (Ice) types.
151  // (without copying code over and over)
152 
153  /// Convert a PCL point cloud to a VisionX point cloud.
154  template <typename VisionXPointT, typename PclPointT>
155  void convertFromPCL(const pcl::PointCloud<PclPointT>& source,
156  std::vector<VisionXPointT>& target);
157 
158  /**
159  * @brief Convert a PCL point cloud to a VisionX point cloud (as return value).
160  *
161  * Pass the desired point type as first template argument, for example:
162  * @code
163  * pcl::PointCloud<pcl::PointXYZRGBA> source = ...;
164  * ColoredPointCloud target = visionx::tools::convertFromPCL<ColoredPoint3D>(source);
165  * ColoredPointCloud target = visionx::tools::convertFromPCL<ColoredPointCloud::value_type>(source);
166  * @endcode
167  */
168  template <typename VisionXPointT, typename PclPointT>
169  std::vector<VisionXPointT> convertFromPCL(const pcl::PointCloud<PclPointT>& source);
170 
171 
172  /// Convert a VisionX point cloud to a PCL point cloud.
173  template <typename PclPointT, typename VisionXPointT>
174  void convertToPCL(const std::vector<VisionXPointT>& source, pcl::PointCloud<PclPointT>& target);
175 
176  /**
177  * @brief Convert a VisionX point cloud to a PCL point cloud (as return value).
178  *
179  * Pass the desired point type as first template argument, for example:
180  * @code
181  * using PointCloudT = pcl::PointCloud<pcl::PointXYZRGBA>;
182  * ColoredPointCloud source = ...;
183  * pcl::PointCloud<pcl::PointXYZRGBA>::Ptr target = visionx::tools::convertToPCL<pcl::PointXYZRGBA>(source);
184  * pcl::PointCloud<pcl::PointXYZRGBA> target = *visionx::tools::convertToPCL<PointCloudT::PointType>(source);
185  * @endcode
186  */
187  template <typename PclPointT, typename VisionXPointT>
188  typename pcl::PointCloud<PclPointT>::Ptr convertToPCL(const std::vector<VisionXPointT>& source);
189 
190  // POINT CONVERSION
191 
192  /// Convert a PCL point type to a VisionX point type.
193  template <typename PclPointT, typename VisionXPointT>
194  void
195  convertPointFromPCL(const PclPointT& p, VisionXPointT& v)
196  {
197  p.CONVERSION_BETWEEN_THESE_POINT_TYPES_IS_NOT_IMPLEMENTED(v);
198  }
199 
200  /// Convert a VisionX point type to a PCL point type.
201  template <typename PclPointT, typename VisionXPointT>
202  void
203  convertPointToPCL(const VisionXPointT& v, PclPointT& p)
204  {
205  p.CONVERSION_BETWEEN_THESE_POINT_TYPES_IS_NOT_IMPLEMENTED(v);
206  }
207 
208  // POINT CONVERSION HELPERS
209 } // namespace visionx::tools
210 
211 namespace visionx::tools::detail
212 {
213  /// Convert x, y, z attributes. Use with visionx::Point3D and PCL type with XYZ.
214  template <typename SourceT, typename TargetT>
215  void
216  convertXYZ(const SourceT& source, TargetT& target)
217  {
218  target.x = source.x;
219  target.y = source.y;
220  target.z = source.z;
221  }
222 
223  /// Convert r, g, b attributes. Use with visionx::RGBA and PCL type with RGBA.
224  template <typename SourceT, typename TargetT>
225  void
226  convertRGBA(const SourceT& source, TargetT& target)
227  {
228  target.r = source.r;
229  target.g = source.g;
230  target.b = source.b;
231  target.a = source.a;
232  }
233 
234  /// Convert label attribute. Use with visionx point type and PCL type with labels.
235  template <typename SourceT, typename TargetT>
236  void
237  convertLabel(const SourceT& source, TargetT& target)
238  {
239  target.label = source.label;
240  }
241 } // namespace visionx::tools::detail
242 
243 namespace visionx::tools
244 {
245 
246  // POINT CONVERSION SPECIALIZATIONS
247 
248  // pcl::PointXYZ <-> visionx::Point3D
249  template <>
250  void convertPointFromPCL(const pcl::PointXYZ& source, visionx::Point3D& target);
251  template <>
252  void convertPointToPCL(const visionx::Point3D& source, pcl::PointXYZ& target);
253 
254  // pcl::PointXYZRGBA <-> visionx::ColoredPoint3D
255  template <>
256  void convertPointFromPCL(const pcl::PointXYZRGBA& source, visionx::ColoredPoint3D& target);
257  template <>
258  void convertPointToPCL(const visionx::ColoredPoint3D& source, pcl::PointXYZRGBA& target);
259 
260  // pcl::PointXYZL <-> visionx::LabeledPoint3D
261  template <>
262  void convertPointFromPCL(const pcl::PointXYZL& source, visionx::LabeledPoint3D& target);
263  template <>
264  void convertPointToPCL(const visionx::LabeledPoint3D& source, pcl::PointXYZL& target);
265 
266  // pcl::PointXYZRGBL <-> visionx::ColoredLabeledPoint3D
267  template <>
268  void convertPointFromPCL(const pcl::PointXYZRGBL& source,
269  visionx::ColoredLabeledPoint3D& target);
270  template <>
271  void convertPointToPCL(const visionx::ColoredLabeledPoint3D& source, pcl::PointXYZRGBL& target);
272 
273  // IMPLEMENTATION (depends on convertPointFromPcl)
274 
275  // Convert a PCL point cloud to a VisionX point cloud.
276  template <typename VisionXPointT, typename PclPointT>
277  void
278  convertFromPCL(const pcl::PointCloud<PclPointT>& source, std::vector<VisionXPointT>& target)
279  {
280  target.resize(source.size());
281  for (std::size_t i = 0; i < source.size(); ++i)
282  {
283  convertPointFromPCL(source[i], target[i]);
284  }
285  }
286 
287  // Convert a PCL point cloud to a VisionX point cloud.
288  template <typename VisionXPointT, typename PclPointT>
289  std::vector<VisionXPointT>
290  convertFromPCL(const pcl::PointCloud<PclPointT>& source)
291  {
292  std::vector<VisionXPointT> target;
293  convertFromPCL(source, target);
294  return target;
295  }
296 
297  // Convert a VisionX point cloud to a PCL point cloud.
298  template <typename PclPointT, typename VisionXPointT>
299  void
300  convertToPCL(const std::vector<VisionXPointT>& source, pcl::PointCloud<PclPointT>& target)
301  {
302  target.resize(source.size());
303  for (std::size_t i = 0; i < source.size(); ++i)
304  {
305  convertPointToPCL(source[i], target[i]);
306  }
307  }
308 
309  /// Convert a VisionX point cloud to a PCL point cloud.
310  template <typename PclPointT, typename VisionXPointT>
311  typename pcl::PointCloud<PclPointT>::Ptr
312  convertToPCL(const std::vector<VisionXPointT>& source)
313  {
314  typename pcl::PointCloud<PclPointT>::Ptr target(new pcl::PointCloud<PclPointT>);
315  convertToPCL(source, *target);
316  return target;
317  }
318 
319 } // namespace visionx::tools
visionx::tools::PCLToDebugDrawerConversionMode
PCLToDebugDrawerConversionMode
Definition: PointCloudConversions.h:125
visionx::tools
Definition: PCLUtilities.cpp:3
visionx::tools::detail::convertXYZ
void convertXYZ(const SourceT &source, TargetT &target)
Convert x, y, z attributes. Use with visionx::Point3D and PCL type with XYZ.
Definition: PointCloudConversions.h:216
boost::target
Vertex target(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:668
visionx::tools::eConvertAsColors
@ eConvertAsColors
Definition: PointCloudConversions.h:128
visionx::tools::isLabeled
bool isLabeled(PointContentType type)
Definition: PointCloudConversions.cpp:131
visionx::tools::detail::convertRGBA
void convertRGBA(const SourceT &source, TargetT &target)
Convert r, g, b attributes. Use with visionx::RGBA and PCL type with RGBA.
Definition: PointCloudConversions.h:226
visionx::tools::detail::convertLabel
void convertLabel(const SourceT &source, TargetT &target)
Convert label attribute. Use with visionx point type and PCL type with labels.
Definition: PointCloudConversions.h:237
visionx::tools::convertFromPCL
void convertFromPCL(pcl::PointCloud< pcl::PointXYZRGBA >::Ptr sourcePtr, void **target)
Definition: PointCloudConversions.cpp:144
visionx::tools::convertPointFromPCL
void convertPointFromPCL(const pcl::PointXYZ &source, visionx::Point3D &target)
Definition: PointCloudConversions.cpp:759
visionx::tools::convertPointToPCL
void convertPointToPCL(const visionx::Point3D &source, pcl::PointXYZ &target)
Definition: PointCloudConversions.cpp:766
visionx::tools::isColored
bool isColored(PointContentType type)
Definition: PointCloudConversions.cpp:117
visionx::tools::eConvertAsPoints
@ eConvertAsPoints
Definition: PointCloudConversions.h:127
visionx::tools::convertFromPCLToDebugDrawer
void convertFromPCLToDebugDrawer(const pcl::PointCloud< pcl::PointXYZRGBA >::Ptr &sourcePtr, armarx::DebugDrawer24BitColoredPointCloud &target, PCLToDebugDrawerConversionMode mode, float pointSize)
Definition: PointCloudConversions.cpp:576
visionx::tools::toString
std::string toString(PointContentType pointContent)
Definition: PointCloudConversions.cpp:839
visionx::tools::eConvertAsLabels
@ eConvertAsLabels
Definition: PointCloudConversions.h:129
boost::source
Vertex source(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:661
visionx::tools::convertToPCL
void convertToPCL(void **source, visionx::MetaPointCloudFormatPtr pointCloudFormat, pcl::PointCloud< pcl::PointXYZRGBA >::Ptr targetPtr)
Definition: PointCloudConversions.cpp:169
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
visionx::tools::getBytesPerPoint
size_t getBytesPerPoint(visionx::PointContentType pointContent)
Definition: PointCloudConversions.cpp:85
visionx::tools::getPointContentType
visionx::PointContentType getPointContentType()
Definition: PointCloudConversions.h:41
visionx::tools::detail
Definition: PCLUtilities.h:87