point_cloud_utilities.cpp
Go to the documentation of this file.
2 
3 #include <pcl/point_types.h>
4 
5 #include <limits>
6 #include <string>
7 #include <vector>
8 #include <ros/ros.h>
9 
10 double const NXLIB_TIMESTAMP_OFFSET = 11644473600;
11 
12 pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloudFromNxLib(NxLibItem const& node, std::string const& frame,
13  PointCloudROI const* roi)
14 {
15  int width, height;
16  double timestamp;
17  std::vector<float> data;
18 
19  node.getBinaryDataInfo(&width, &height, 0, 0, 0, &timestamp);
20  node.getBinaryData(data, 0);
21 
22  auto cloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
23 
24  // PCL timestamp is in microseconds and Unix time.
25  cloud->header.stamp = (timestamp - NXLIB_TIMESTAMP_OFFSET) * 1e6;
26  cloud->header.frame_id = frame;
27 
28  cloud->width = width;
29  cloud->height = height;
30  cloud->is_dense = false;
31  cloud->points.resize(width * height);
32  for (int i = 0; i < width * height; i++)
33  {
34  cloud->points[i].x = data[3 * i] / 1000.0f;
35  cloud->points[i].y = data[3 * i + 1] / 1000.0f;
36  cloud->points[i].z = data[3 * i + 2] / 1000.0f;
37 
38  if (roi != nullptr && !roi->contains(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z))
39  {
40  cloud->points[i].x = std::numeric_limits<float>::quiet_NaN();
41  cloud->points[i].y = std::numeric_limits<float>::quiet_NaN();
42  cloud->points[i].z = std::numeric_limits<float>::quiet_NaN();
43  }
44  }
45 
46  return cloud;
47 }
48 
49 pcl::PointCloud<pcl::PointNormal>::Ptr pointCloudWithNormalsFromNxLib(NxLibItem const& pointMapNode,
50  NxLibItem const& normalNode,
51  std::string const& frame,
52  PointCloudROI const* roi)
53 {
54  int width, height;
55  double timestamp;
56  std::vector<float> pointData;
57  std::vector<float> normalData;
58 
59  pointMapNode.getBinaryDataInfo(&width, &height, 0, 0, 0, &timestamp);
60  pointMapNode.getBinaryData(pointData, 0);
61  normalNode.getBinaryData(normalData, 0);
62 
63  auto cloud = boost::make_shared<pcl::PointCloud<pcl::PointNormal>>();
64 
65  // PCL timestamp is in microseconds and Unix time.
66  cloud->header.stamp = (timestamp - NXLIB_TIMESTAMP_OFFSET) * 1e6;
67  cloud->header.frame_id = frame;
68 
69  cloud->width = width;
70  cloud->height = height;
71  cloud->is_dense = false;
72  cloud->points.resize(width * height);
73  for (int i = 0; i < width * height; i++)
74  {
75  // The NxLib point cloud is in millimeters, ROS needs everything in meters.
76  cloud->points[i].x = pointData[3 * i] / 1000.0f;
77  cloud->points[i].y = pointData[3 * i + 1] / 1000.0f;
78  cloud->points[i].z = pointData[3 * i + 2] / 1000.0f;
79 
80  if (roi != nullptr && !roi->contains(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z))
81  {
82  cloud->points[i].x = std::numeric_limits<float>::quiet_NaN();
83  cloud->points[i].y = std::numeric_limits<float>::quiet_NaN();
84  cloud->points[i].z = std::numeric_limits<float>::quiet_NaN();
85  continue;
86  }
87 
88  cloud->points[i].normal_x = normalData[3 * i];
89  cloud->points[i].normal_y = normalData[3 * i + 1];
90  cloud->points[i].normal_z = normalData[3 * i + 2];
91  }
92 
93  return cloud;
94 }
95 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudTexturedFromNxLib(NxLibItem const& imageNode,
96  NxLibItem const& pointsNode,
97  std::string const& frame, PointCloudROI const* roi)
98 {
99  double timestamp;
100 
101  int width, height;
102  std::vector<float> data;
103  std::vector<unsigned char> imageData;
104 
105  pointsNode.getBinaryDataInfo(&width, &height, 0, 0, 0, 0);
106  pointsNode.getBinaryData(data, &timestamp);
107  imageNode.getBinaryData(imageData, 0);
108 
109  auto cloud_colored = boost::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>();
110 
111  // PCL timestamp is in microseconds and Unix time.
112  cloud_colored->header.stamp = (timestamp - NXLIB_TIMESTAMP_OFFSET) * 1e6;
113  cloud_colored->header.frame_id = frame;
114 
115  cloud_colored->width = width;
116  cloud_colored->height = height;
117  cloud_colored->is_dense = false;
118  cloud_colored->points.resize(width * height);
119 
120  for (int i = 0; i < width * height; i++)
121  {
122  cloud_colored->points[i].x = data[3 * i] / 1000.0f;
123  cloud_colored->points[i].y = data[3 * i + 1] / 1000.0f;
124  cloud_colored->points[i].z = data[3 * i + 2] / 1000.0f;
125 
126  cloud_colored->points[i].r = imageData[4 * i];
127  cloud_colored->points[i].g = imageData[4 * i + 1];
128  cloud_colored->points[i].b = imageData[4 * i + 2];
129  cloud_colored->points[i].a = imageData[4 * i + 3];
130 
131  if (roi != nullptr &&
132  !roi->contains(cloud_colored->points[i].x, cloud_colored->points[i].y, cloud_colored->points[i].z))
133  {
134  cloud_colored->points[i].x = std::numeric_limits<float>::quiet_NaN();
135  cloud_colored->points[i].y = std::numeric_limits<float>::quiet_NaN();
136  cloud_colored->points[i].z = std::numeric_limits<float>::quiet_NaN();
137  }
138  }
139 
140  return cloud_colored;
141 }
pcl::PointCloud< pcl::PointXYZ >::Ptr pointCloudFromNxLib(NxLibItem const &node, std::string const &frame, PointCloudROI const *roi)
double const NXLIB_TIMESTAMP_OFFSET
pcl::PointCloud< pcl::PointXYZRGB >::Ptr pointCloudTexturedFromNxLib(NxLibItem const &imageNode, NxLibItem const &pointsNode, std::string const &frame, PointCloudROI const *roi)
pcl::PointCloud< pcl::PointNormal >::Ptr pointCloudWithNormalsFromNxLib(NxLibItem const &pointMapNode, NxLibItem const &normalNode, std::string const &frame, PointCloudROI const *roi)
bool contains(float x, float y, float z) const


ensenso_camera
Author(s): Ensenso
autogenerated on Sat Jul 27 2019 03:51:24