144 lines
5.4 KiB
C++
144 lines
5.4 KiB
C++
/*******************************************************************************
|
|
* Copyright (c) 2021 Nerian Vision GmbH
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in
|
|
* all copies or substantial portions of the Software.
|
|
*******************************************************************************/
|
|
|
|
#ifndef VISIONTRANSFER_RECONSTRUCT3D_OPEN3D_H
|
|
#define VISIONTRANSFER_RECONSTRUCT3D_OPEN3D_H
|
|
|
|
#ifdef OPEN3D_VERSION
|
|
|
|
namespace visiontransfer {
|
|
|
|
/*
|
|
* Open3d-specific implementations that need to be inlined in order to avoid
|
|
* dependencies for projects that do not make use of Open3D
|
|
*/
|
|
|
|
inline std::shared_ptr<open3d::geometry::PointCloud> Reconstruct3D::createOpen3DCloud(
|
|
const ImageSet& imageSet, bool withColor, unsigned short minDisparity) {
|
|
|
|
int numPoints = imageSet.getWidth() * imageSet.getHeight();
|
|
std::shared_ptr<open3d::geometry::PointCloud> ret(new open3d::geometry::PointCloud());
|
|
|
|
// Convert the 3D point cloud
|
|
ret->points_.resize(numPoints);
|
|
|
|
float* points = createPointMap(imageSet, minDisparity);
|
|
float* end = &points[4*numPoints];
|
|
Eigen::Vector3d* dest = &ret->points_[0];
|
|
|
|
while(points != end) {
|
|
float x = *(points++);
|
|
float y = *(points++);
|
|
float z = *(points++);
|
|
points++;
|
|
|
|
*dest = Eigen::Vector3d(x, y, z);
|
|
dest++;
|
|
}
|
|
|
|
// Convert the color information if enabled
|
|
if(withColor && imageSet.hasImageType(ImageSet::IMAGE_LEFT)) {
|
|
ret->colors_.resize(numPoints);
|
|
unsigned char* pixel = imageSet.getPixelData(ImageSet::IMAGE_LEFT);
|
|
Eigen::Vector3d* color = &ret->colors_[0];
|
|
Eigen::Vector3d* colorEnd = &ret->colors_[numPoints];
|
|
|
|
switch(imageSet.getPixelFormat(ImageSet::IMAGE_LEFT)) {
|
|
case ImageSet::FORMAT_8_BIT_MONO:
|
|
while(color != colorEnd) {
|
|
double col = double(*(pixel++))/0xFF;
|
|
*(color++) = Eigen::Vector3d(col, col, col);
|
|
}
|
|
break;
|
|
case ImageSet::FORMAT_12_BIT_MONO:
|
|
while(color != colorEnd) {
|
|
double col = double(*reinterpret_cast<unsigned short*>(pixel))/0xFFF;
|
|
pixel+=2;
|
|
*(color++) = Eigen::Vector3d(col, col, col);
|
|
}
|
|
break;
|
|
case ImageSet::FORMAT_8_BIT_RGB:
|
|
while(color != colorEnd) {
|
|
double r = double(*(pixel++))/0xFF;
|
|
double g = double(*(pixel++))/0xFF;
|
|
double b = double(*(pixel++))/0xFF;
|
|
*(color++) = Eigen::Vector3d(r, g, b);
|
|
}
|
|
break;
|
|
default: throw std::runtime_error("Illegal pixel format");
|
|
}
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
inline std::shared_ptr<open3d::geometry::RGBDImage> Reconstruct3D::createOpen3DImageRGBD(const ImageSet& imageSet,
|
|
unsigned short minDisparity) {
|
|
|
|
std::shared_ptr<open3d::geometry::RGBDImage> ret(new open3d::geometry::RGBDImage);
|
|
|
|
// Convert depth map
|
|
ret->depth_.width_ = imageSet.getWidth();
|
|
ret->depth_.height_ = imageSet.getHeight();
|
|
ret->depth_.num_of_channels_ = 1;
|
|
ret->depth_.bytes_per_channel_ = sizeof(float);
|
|
ret->depth_.data_.resize(ret->depth_.width_*ret->depth_.height_*ret->depth_.bytes_per_channel_);
|
|
|
|
float* zMap = createZMap(imageSet, minDisparity);
|
|
memcpy(&ret->depth_.data_[0], zMap, ret->depth_.data_.size());
|
|
|
|
// Convert color
|
|
ret->color_.width_ = imageSet.getWidth();
|
|
ret->color_.height_ = imageSet.getHeight();
|
|
ret->color_.num_of_channels_ = 3;
|
|
ret->color_.bytes_per_channel_ = 1;
|
|
ret->color_.data_.resize(ret->color_.width_ * ret->color_.height_ *
|
|
ret->color_.num_of_channels_ * ret->color_.bytes_per_channel_);
|
|
|
|
unsigned char* srcPixel = imageSet.getPixelData(ImageSet::IMAGE_LEFT);
|
|
unsigned char* dstPixel = &ret->color_.data_[0];
|
|
unsigned char* dstEnd = &ret->color_.data_[ret->color_.data_.size()];
|
|
|
|
switch(imageSet.getPixelFormat(ImageSet::IMAGE_LEFT)) {
|
|
case ImageSet::FORMAT_8_BIT_MONO:
|
|
while(dstPixel != dstEnd) {
|
|
*(dstPixel++) = *srcPixel;
|
|
*(dstPixel++) = *srcPixel;
|
|
*(dstPixel++) = *(srcPixel++);
|
|
}
|
|
break;
|
|
case ImageSet::FORMAT_12_BIT_MONO:
|
|
while(dstPixel != dstEnd) {
|
|
unsigned short pixel16Bit = *reinterpret_cast<unsigned short*>(srcPixel);
|
|
unsigned char pixel8Bit = pixel16Bit / 0xF;
|
|
srcPixel += 2;
|
|
|
|
*(dstPixel++) = pixel8Bit;
|
|
*(dstPixel++) = pixel8Bit;
|
|
*(dstPixel++) = pixel8Bit;
|
|
}
|
|
break;
|
|
case ImageSet::FORMAT_8_BIT_RGB:
|
|
memcpy(&ret->color_.data_[0], srcPixel, ret->color_.data_.size());
|
|
break;
|
|
default: throw std::runtime_error("Illegal pixel format");
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
#endif
|
|
#endif
|