demo version

demo/camera_client/dependencies/libvisiontransfer/visiontransfer/reconstruct3d-pcl.h

@@ -0,0 +1,125 @@
+/*******************************************************************************
+ * 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