libvisiontransfer  6.1.0
reconstruct3d-pcl.h
1 /*******************************************************************************
2  * Copyright (c) 2018 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 /*
21  * PCL-specific implementations that need to be inlined in order to avoid
22  * dependencies for projects that do not make use of PCL
23  */
24 
25 template <typename T>
26 typename pcl::PointCloud<T>::Ptr Reconstruct3D::initPointCloud(const ImagePair& imagePair, const char* frameId) {
27  int sec, microsec;
28  imagePair.getTimestamp(sec, microsec);
29 
30  typename pcl::PointCloud<T>::Ptr ret(
31  new pcl::PointCloud<T>(imagePair.getWidth(), imagePair.getHeight()));
32 
33  ret->header.frame_id = frameId;
34  ret->header.seq = imagePair.getSequenceNumber();
35  ret->header.stamp = sec * 1000000LL + microsec;
36  ret->width = imagePair.getWidth();
37  ret->height = imagePair.getHeight();
38  ret->is_dense = true;
39  return ret;
40 }
41 
42 inline pcl::PointCloud<pcl::PointXYZ>::Ptr Reconstruct3D::createXYZCloud(const ImagePair& imagePair,
43  const char* frameId, unsigned short minDisparity) {
44  float* pointMap = createPointMap(imagePair, minDisparity);
45  pcl::PointCloud<pcl::PointXYZ>::Ptr ret = initPointCloud<pcl::PointXYZ>(imagePair, frameId);
46  memcpy(&ret->points[0].x, pointMap, ret->width*ret->height*sizeof(float)*4);
47  return ret;
48 }
49 
50 inline pcl::PointCloud<pcl::PointXYZI>::Ptr Reconstruct3D::createXYZICloud(const ImagePair& imagePair,
51  const char* frameId, unsigned short minDisparity) {
52  float* pointMap = createPointMap(imagePair, minDisparity);
53  pcl::PointCloud<pcl::PointXYZI>::Ptr ret = initPointCloud<pcl::PointXYZI>(imagePair, frameId);
54 
55  pcl::PointXYZI* dstPtr = &ret->points[0];
56  if(imagePair.getPixelFormat(0) == ImagePair::FORMAT_8_BIT_MONO) {
57  for(int y = 0; y < imagePair.getHeight(); y++) {
58  unsigned char* rowPtr = imagePair.getPixelData(0) + y*imagePair.getRowStride(0);
59  unsigned char* endPtr = rowPtr + imagePair.getWidth();
60  for(; rowPtr < endPtr; rowPtr++) {
61  dstPtr->intensity = static_cast<float>(*rowPtr)/255.0F;
62  dstPtr->x = *pointMap++;
63  dstPtr->y = *pointMap++;
64  dstPtr->z = *pointMap++;
65 
66  pointMap++;
67  dstPtr++;
68  }
69  }
70  } else if(imagePair.getPixelFormat(0) == ImagePair::FORMAT_12_BIT_MONO) {
71  for(int y = 0; y < imagePair.getHeight(); y++) {
72  unsigned short* rowPtr = reinterpret_cast<unsigned short*>(imagePair.getPixelData(0) + y*imagePair.getRowStride(0));
73  unsigned short* endPtr = rowPtr + imagePair.getWidth();
74  for(; rowPtr < endPtr; rowPtr++) {
75  dstPtr->intensity = static_cast<float>(*rowPtr)/4095.0F;
76  dstPtr->x = *pointMap++;
77  dstPtr->y = *pointMap++;
78  dstPtr->z = *pointMap++;
79 
80  pointMap++;
81  dstPtr++;
82  }
83  }
84  } else {
85  throw std::runtime_error("Left image does not have a valid greyscale format");
86  }
87 
88  return ret;
89 }
90 
91 #endif
92 #endif
8-bit greyscale format
Definition: imagepair.h:38
int getHeight() const
Returns the height of each image.
Definition: imagepair.h:178
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::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.
unsigned int getSequenceNumber() const
Returns the sequence number for this image pair.
Definition: imagepair.h:223
int getWidth() const
Returns the width of each image.
Definition: imagepair.h:173
A set of two images, which are usually the left camera image and the disparity map.
Definition: imagepair.h:31
void getTimestamp(int &seconds, int &microsec) const
Returns the time at which this image pair has been captured.
Definition: imagepair.h:232
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.
Nerian Vision Technologies