libvisiontransfer  6.4.0
reconstruct3d-pcl.h
1 /*******************************************************************************
2  * Copyright (c) 2019 Nerian Vision GmbH
3  *
4  * Permission is hereby granted, free of charge, to any person obtaining a copy
5  * of this software and associated documentation files (the "Software"), to deal
6  * in the Software without restriction, including without limitation the rights
7  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
8  * copies of the Software, and to permit persons to whom the Software is
9  * furnished to do so, subject to the following conditions:
10  *
11  * The above copyright notice and this permission notice shall be included in
12  * all copies or substantial portions of the Software.
13  *******************************************************************************/
14 
15 #ifndef VISIONTRANSFER_RECONSTRUCT3D_PCL_H
16 #define VISIONTRANSFER_RECONSTRUCT3D_PCL_H
17 
18 #ifdef PCL_MAJOR_VERSION
19 
20 namespace visiontransfer {
21 
22 /*
23  * PCL-specific implementations that need to be inlined in order to avoid
24  * dependencies for projects that do not make use of PCL
25  */
26 
27 template <typename T>
28 typename pcl::PointCloud<T>::Ptr Reconstruct3D::initPointCloud(const ImagePair& imagePair, const char* frameId) {
29  int sec, microsec;
30  imagePair.getTimestamp(sec, microsec);
31 
32  typename pcl::PointCloud<T>::Ptr ret(
33  new pcl::PointCloud<T>(imagePair.getWidth(), imagePair.getHeight()));
34 
35  ret->header.frame_id = frameId;
36  ret->header.seq = imagePair.getSequenceNumber();
37  ret->header.stamp = sec * 1000000LL + microsec;
38  ret->width = imagePair.getWidth();
39  ret->height = imagePair.getHeight();
40  ret->is_dense = true;
41  return ret;
42 }
43 
44 inline pcl::PointCloud<pcl::PointXYZ>::Ptr Reconstruct3D::createXYZCloud(const ImagePair& imagePair,
45  const char* frameId, unsigned short minDisparity) {
46  float* pointMap = createPointMap(imagePair, minDisparity);
47  pcl::PointCloud<pcl::PointXYZ>::Ptr ret = initPointCloud<pcl::PointXYZ>(imagePair, frameId);
48  memcpy(&ret->points[0].x, pointMap, ret->width*ret->height*sizeof(float)*4);
49  return ret;
50 }
51 
52 inline pcl::PointCloud<pcl::PointXYZI>::Ptr Reconstruct3D::createXYZICloud(const ImagePair& imagePair,
53  const char* frameId, unsigned short minDisparity) {
54  float* pointMap = createPointMap(imagePair, minDisparity);
55  pcl::PointCloud<pcl::PointXYZI>::Ptr ret = initPointCloud<pcl::PointXYZI>(imagePair, frameId);
56 
57  pcl::PointXYZI* dstPtr = &ret->points[0];
58  if(imagePair.getPixelFormat(0) == ImagePair::FORMAT_8_BIT_MONO) {
59  for(int y = 0; y < imagePair.getHeight(); y++) {
60  unsigned char* rowPtr = imagePair.getPixelData(0) + y*imagePair.getRowStride(0);
61  unsigned char* endPtr = rowPtr + imagePair.getWidth();
62  for(; rowPtr < endPtr; rowPtr++) {
63  dstPtr->intensity = static_cast<float>(*rowPtr)/255.0F;
64  dstPtr->x = *pointMap++;
65  dstPtr->y = *pointMap++;
66  dstPtr->z = *pointMap++;
67 
68  pointMap++;
69  dstPtr++;
70  }
71  }
72  } else if(imagePair.getPixelFormat(0) == ImagePair::FORMAT_12_BIT_MONO) {
73  for(int y = 0; y < imagePair.getHeight(); y++) {
74  unsigned short* rowPtr = reinterpret_cast<unsigned short*>(imagePair.getPixelData(0) + y*imagePair.getRowStride(0));
75  unsigned short* endPtr = rowPtr + imagePair.getWidth();
76  for(; rowPtr < endPtr; rowPtr++) {
77  dstPtr->intensity = static_cast<float>(*rowPtr)/4095.0F;
78  dstPtr->x = *pointMap++;
79  dstPtr->y = *pointMap++;
80  dstPtr->z = *pointMap++;
81 
82  pointMap++;
83  dstPtr++;
84  }
85  }
86  } else {
87  throw std::runtime_error("Left image does not have a valid greyscale format");
88  }
89 
90  return ret;
91 }
92 
93 inline pcl::PointCloud<pcl::PointXYZRGB>::Ptr Reconstruct3D::createXYZRGBCloud(const ImagePair& imagePair,
94  const char* frameId, unsigned short minDisparity) {
95  float* pointMap = createPointMap(imagePair, minDisparity);
96  pcl::PointCloud<pcl::PointXYZRGB>::Ptr ret = initPointCloud<pcl::PointXYZRGB>(imagePair, frameId);
97 
98  pcl::PointXYZRGB* dstPtr = &ret->points[0];
99  if(imagePair.getPixelFormat(0) != ImagePair::FORMAT_8_BIT_RGB) {
100  throw std::runtime_error("Left image is not an RGB image");
101  }
102 
103  for(int y = 0; y < imagePair.getHeight(); y++) {
104  unsigned char* rowPtr = imagePair.getPixelData(0) + y*imagePair.getRowStride(0);
105  unsigned char* endPtr = rowPtr + 3*imagePair.getWidth();
106  for(; rowPtr < endPtr;rowPtr +=3) {
107  dstPtr->r = rowPtr[0];
108  dstPtr->g = rowPtr[1];
109  dstPtr->b = rowPtr[2];
110  dstPtr->x = *pointMap++;
111  dstPtr->y = *pointMap++;
112  dstPtr->z = *pointMap++;
113 
114  pointMap++;
115  dstPtr++;
116  }
117  }
118 
119  return ret;
120 }
121 
122 } // namespace
123 
124 #endif
125 #endif
pcl::PointCloud< pcl::PointXYZRGB >::Ptr createXYZRGBCloud(const ImagePair &imagePair, const char *frameId, unsigned short minDisparity=0)
Projects the given disparity map to a PCL point cloud, including pixel RGB data.
float * createPointMap(const unsigned short *dispMap, int width, int height, int rowStride, const float *q, unsigned short minDisparity=1)
Reconstructs the 3D location of each pixel in the given disparity map.
pcl::PointCloud< pcl::PointXYZI >::Ptr createXYZICloud(const ImagePair &imagePair, const char *frameId, unsigned short minDisparity=0)
Projects the given disparity map to a PCL point cloud, including pixel intensities.
pcl::PointCloud< pcl::PointXYZ >::Ptr createXYZCloud(const ImagePair &imagePair, const char *frameId, unsigned short minDisparity=0)
Projects the given disparity map to a PCL point cloud without pixel intensities.
A set of two images, which are usually the left camera image and the disparity map.
Definition: imagepair.h:33
Nerian Vision Technologies