125 lines
4.8 KiB
C++
125 lines
4.8 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_PCL_H
|
|
#define VISIONTRANSFER_RECONSTRUCT3D_PCL_H
|
|
|
|
#ifdef PCL_MAJOR_VERSION
|
|
|
|
namespace visiontransfer {
|
|
|
|
/*
|
|
* PCL-specific implementations that need to be inlined in order to avoid
|
|
* dependencies for projects that do not make use of PCL
|
|
*/
|
|
|
|
template <typename T>
|
|
typename pcl::PointCloud<T>::Ptr Reconstruct3D::initPointCloud(const ImageSet& imageSet, const char* frameId) {
|
|
int sec, microsec;
|
|
imageSet.getTimestamp(sec, microsec);
|
|
|
|
typename pcl::PointCloud<T>::Ptr ret(
|
|
new pcl::PointCloud<T>(imageSet.getWidth(), imageSet.getHeight()));
|
|
|
|
ret->header.frame_id = frameId;
|
|
ret->header.seq = imageSet.getSequenceNumber();
|
|
ret->header.stamp = sec * 1000000LL + microsec;
|
|
ret->width = imageSet.getWidth();
|
|
ret->height = imageSet.getHeight();
|
|
ret->is_dense = true;
|
|
return ret;
|
|
}
|
|
|
|
inline pcl::PointCloud<pcl::PointXYZ>::Ptr Reconstruct3D::createXYZCloud(const ImageSet& imageSet,
|
|
const char* frameId, unsigned short minDisparity) {
|
|
float* pointMap = createPointMap(imageSet, minDisparity);
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr ret = initPointCloud<pcl::PointXYZ>(imageSet, frameId);
|
|
memcpy(&ret->points[0].x, pointMap, ret->width*ret->height*sizeof(float)*4);
|
|
return ret;
|
|
}
|
|
|
|
inline pcl::PointCloud<pcl::PointXYZI>::Ptr Reconstruct3D::createXYZICloud(const ImageSet& imageSet,
|
|
const char* frameId, unsigned short minDisparity) {
|
|
float* pointMap = createPointMap(imageSet, minDisparity);
|
|
pcl::PointCloud<pcl::PointXYZI>::Ptr ret = initPointCloud<pcl::PointXYZI>(imageSet, frameId);
|
|
|
|
pcl::PointXYZI* dstPtr = &ret->points[0];
|
|
if(imageSet.getPixelFormat(0) == ImageSet::FORMAT_8_BIT_MONO) {
|
|
for(int y = 0; y < imageSet.getHeight(); y++) {
|
|
unsigned char* rowPtr = imageSet.getPixelData(0) + y*imageSet.getRowStride(0);
|
|
unsigned char* endPtr = rowPtr + imageSet.getWidth();
|
|
for(; rowPtr < endPtr; rowPtr++) {
|
|
dstPtr->intensity = static_cast<float>(*rowPtr)/255.0F;
|
|
dstPtr->x = *pointMap++;
|
|
dstPtr->y = *pointMap++;
|
|
dstPtr->z = *pointMap++;
|
|
|
|
pointMap++;
|
|
dstPtr++;
|
|
}
|
|
}
|
|
} else if(imageSet.getPixelFormat(0) == ImageSet::FORMAT_12_BIT_MONO) {
|
|
for(int y = 0; y < imageSet.getHeight(); y++) {
|
|
unsigned short* rowPtr = reinterpret_cast<unsigned short*>(imageSet.getPixelData(0) + y*imageSet.getRowStride(0));
|
|
unsigned short* endPtr = rowPtr + imageSet.getWidth();
|
|
for(; rowPtr < endPtr; rowPtr++) {
|
|
dstPtr->intensity = static_cast<float>(*rowPtr)/4095.0F;
|
|
dstPtr->x = *pointMap++;
|
|
dstPtr->y = *pointMap++;
|
|
dstPtr->z = *pointMap++;
|
|
|
|
pointMap++;
|
|
dstPtr++;
|
|
}
|
|
}
|
|
} else {
|
|
throw std::runtime_error("Left image does not have a valid greyscale format");
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
inline pcl::PointCloud<pcl::PointXYZRGB>::Ptr Reconstruct3D::createXYZRGBCloud(const ImageSet& imageSet,
|
|
const char* frameId, unsigned short minDisparity) {
|
|
float* pointMap = createPointMap(imageSet, minDisparity);
|
|
pcl::PointCloud<pcl::PointXYZRGB>::Ptr ret = initPointCloud<pcl::PointXYZRGB>(imageSet, frameId);
|
|
|
|
pcl::PointXYZRGB* dstPtr = &ret->points[0];
|
|
if(imageSet.getPixelFormat(0) != ImageSet::FORMAT_8_BIT_RGB) {
|
|
throw std::runtime_error("Left image is not an RGB image");
|
|
}
|
|
|
|
for(int y = 0; y < imageSet.getHeight(); y++) {
|
|
unsigned char* rowPtr = imageSet.getPixelData(0) + y*imageSet.getRowStride(0);
|
|
unsigned char* endPtr = rowPtr + 3*imageSet.getWidth();
|
|
for(; rowPtr < endPtr;rowPtr +=3) {
|
|
dstPtr->r = rowPtr[0];
|
|
dstPtr->g = rowPtr[1];
|
|
dstPtr->b = rowPtr[2];
|
|
dstPtr->x = *pointMap++;
|
|
dstPtr->y = *pointMap++;
|
|
dstPtr->z = *pointMap++;
|
|
|
|
pointMap++;
|
|
dstPtr++;
|
|
}
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
#endif
|
|
#endif
|