15 #ifndef VISIONTRANSFER_RECONSTRUCT3D_OPEN3D_H 16 #define VISIONTRANSFER_RECONSTRUCT3D_OPEN3D_H 27 inline std::shared_ptr<open3d::geometry::PointCloud> Reconstruct3D::createOpen3DCloud(
28 const ImageSet& imageSet,
bool withColor,
unsigned short minDisparity) {
30 int numPoints = imageSet.getWidth() * imageSet.getHeight();
31 std::shared_ptr<open3d::geometry::PointCloud> ret(
new open3d::geometry::PointCloud());
34 ret->points_.resize(numPoints);
37 float* end = &points[4*numPoints];
38 Eigen::Vector3d* dest = &ret->points_[0];
40 while(points != end) {
41 float x = *(points++);
42 float y = *(points++);
43 float z = *(points++);
46 *dest = Eigen::Vector3d(x, y, z);
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];
57 switch(imageSet.getPixelFormat(ImageSet::IMAGE_LEFT)) {
59 while(color != colorEnd) {
60 double col = double(*(pixel++))/0xFF;
61 *(color++) = Eigen::Vector3d(col, col, col);
65 while(color != colorEnd) {
66 double col = double(*reinterpret_cast<unsigned short*>(pixel))/0xFFF;
68 *(color++) = Eigen::Vector3d(col, col, col);
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);
79 default:
throw std::runtime_error(
"Illegal pixel format");
86 inline std::shared_ptr<open3d::geometry::RGBDImage> Reconstruct3D::createOpen3DImageRGBD(
const ImageSet& imageSet,
87 unsigned short minDisparity) {
89 std::shared_ptr<open3d::geometry::RGBDImage> ret(
new open3d::geometry::RGBDImage);
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_);
98 float* zMap =
createZMap(imageSet, minDisparity);
99 memcpy(&ret->depth_.data_[0], zMap, ret->depth_.data_.size());
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_);
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()];
113 switch(imageSet.getPixelFormat(ImageSet::IMAGE_LEFT)) {
115 while(dstPixel != dstEnd) {
116 *(dstPixel++) = *srcPixel;
117 *(dstPixel++) = *srcPixel;
118 *(dstPixel++) = *(srcPixel++);
122 while(dstPixel != dstEnd) {
123 unsigned short pixel16Bit = *
reinterpret_cast<unsigned short*
>(srcPixel);
124 unsigned char pixel8Bit = pixel16Bit / 0xF;
127 *(dstPixel++) = pixel8Bit;
128 *(dstPixel++) = pixel8Bit;
129 *(dstPixel++) = pixel8Bit;
133 memcpy(&ret->color_.data_[0], srcPixel, ret->color_.data_.size());
135 default:
throw std::runtime_error(
"Illegal pixel format");
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.