libvisiontransfer  9.0.3
reconstruct3d-open3d.h
1 /*******************************************************************************
2  * Copyright (c) 2021 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_OPEN3D_H
16 #define VISIONTRANSFER_RECONSTRUCT3D_OPEN3D_H
17 
18 #ifdef OPEN3D_VERSION
19 
20 namespace visiontransfer {
21 
22 /*
23  * Open3d-specific implementations that need to be inlined in order to avoid
24  * dependencies for projects that do not make use of Open3D
25  */
26 
27 inline std::shared_ptr<open3d::geometry::PointCloud> Reconstruct3D::createOpen3DCloud(
28  const ImageSet& imageSet, bool withColor, unsigned short minDisparity) {
29 
30  int numPoints = imageSet.getWidth() * imageSet.getHeight();
31  std::shared_ptr<open3d::geometry::PointCloud> ret(new open3d::geometry::PointCloud());
32 
33  // Convert the 3D point cloud
34  ret->points_.resize(numPoints);
35 
36  float* points = createPointMap(imageSet, minDisparity);
37  float* end = &points[4*numPoints];
38  Eigen::Vector3d* dest = &ret->points_[0];
39 
40  while(points != end) {
41  float x = *(points++);
42  float y = *(points++);
43  float z = *(points++);
44  points++;
45 
46  *dest = Eigen::Vector3d(x, y, z);
47  dest++;
48  }
49 
50  // Convert the color information if enabled
51  if(withColor && imageSet.hasImageType(ImageSet::IMAGE_LEFT)) {
52  ret->colors_.resize(numPoints);
53  unsigned char* pixel = imageSet.getPixelData(ImageSet::IMAGE_LEFT);
54  Eigen::Vector3d* color = &ret->colors_[0];
55  Eigen::Vector3d* colorEnd = &ret->colors_[numPoints];
56 
57  switch(imageSet.getPixelFormat(ImageSet::IMAGE_LEFT)) {
59  while(color != colorEnd) {
60  double col = double(*(pixel++))/0xFF;
61  *(color++) = Eigen::Vector3d(col, col, col);
62  }
63  break;
65  while(color != colorEnd) {
66  double col = double(*reinterpret_cast<unsigned short*>(pixel))/0xFFF;
67  pixel+=2;
68  *(color++) = Eigen::Vector3d(col, col, col);
69  }
70  break;
72  while(color != colorEnd) {
73  double r = double(*(pixel++))/0xFF;
74  double g = double(*(pixel++))/0xFF;
75  double b = double(*(pixel++))/0xFF;
76  *(color++) = Eigen::Vector3d(r, g, b);
77  }
78  break;
79  default: throw std::runtime_error("Illegal pixel format");
80  }
81  }
82 
83  return ret;
84 }
85 
86 inline std::shared_ptr<open3d::geometry::RGBDImage> Reconstruct3D::createOpen3DImageRGBD(const ImageSet& imageSet,
87  unsigned short minDisparity) {
88 
89  std::shared_ptr<open3d::geometry::RGBDImage> ret(new open3d::geometry::RGBDImage);
90 
91  // Convert depth map
92  ret->depth_.width_ = imageSet.getWidth();
93  ret->depth_.height_ = imageSet.getHeight();
94  ret->depth_.num_of_channels_ = 1;
95  ret->depth_.bytes_per_channel_ = sizeof(float);
96  ret->depth_.data_.resize(ret->depth_.width_*ret->depth_.height_*ret->depth_.bytes_per_channel_);
97 
98  float* zMap = createZMap(imageSet, minDisparity);
99  memcpy(&ret->depth_.data_[0], zMap, ret->depth_.data_.size());
100 
101  // Convert color
102  ret->color_.width_ = imageSet.getWidth();
103  ret->color_.height_ = imageSet.getHeight();
104  ret->color_.num_of_channels_ = 3;
105  ret->color_.bytes_per_channel_ = 1;
106  ret->color_.data_.resize(ret->color_.width_ * ret->color_.height_ *
107  ret->color_.num_of_channels_ * ret->color_.bytes_per_channel_);
108 
109  unsigned char* srcPixel = imageSet.getPixelData(ImageSet::IMAGE_LEFT);
110  unsigned char* dstPixel = &ret->color_.data_[0];
111  unsigned char* dstEnd = &ret->color_.data_[ret->color_.data_.size()];
112 
113  switch(imageSet.getPixelFormat(ImageSet::IMAGE_LEFT)) {
115  while(dstPixel != dstEnd) {
116  *(dstPixel++) = *srcPixel;
117  *(dstPixel++) = *srcPixel;
118  *(dstPixel++) = *(srcPixel++);
119  }
120  break;
122  while(dstPixel != dstEnd) {
123  unsigned short pixel16Bit = *reinterpret_cast<unsigned short*>(srcPixel);
124  unsigned char pixel8Bit = pixel16Bit / 0xF;
125  srcPixel += 2;
126 
127  *(dstPixel++) = pixel8Bit;
128  *(dstPixel++) = pixel8Bit;
129  *(dstPixel++) = pixel8Bit;
130  }
131  break;
133  memcpy(&ret->color_.data_[0], srcPixel, ret->color_.data_.size());
134  break;
135  default: throw std::runtime_error("Illegal pixel format");
136  }
137 
138  return ret;
139 }
140 
141 } // namespace
142 
143 #endif
144 #endif
float * createPointMap(const ImageSet &imageSet, unsigned short minDisparity=1)
Reconstructs the 3D location of each pixel in the given disparity map.
float * createZMap(const ImageSet &imageSet, unsigned short minDisparity=1, unsigned short maxDisparity=0xFFF)
Converts the disparit in an image set to a depth map.
Nerian Vision Technologies