Add rudimentary pcloud dumper and meshing with OFM & GP3

The OFM algo runs in fractions of a millisecond. GP3 runs in
fractions of a second. I think if we can get more input data to
the OFM or something akin to it, we will have a winner.

stimBuffApis/livoxGen1/CMakeLists.txt

@@ -15,6 +15,7 @@ if(ENABLE_STIMBUFFAPI_livoxGen1)
 	add_library(livoxGen1 SHARED
 		livoxGen1.cpp
 		pcloudStimulusProducer.cpp
+		livoxPcloudFrameDumper.cpp
 		ioUringAssemblyEngine.cpp
 		openClCollatingAndMeshingEngine.cpp
 		openClKernels.cl.S

stimBuffApis/livoxGen1/livoxPcloudFrameDumper.cpp

@@ -0,0 +1,238 @@
+#include "livoxPcloudFrameDumper.h"
+
+#include <chrono>
+#include <cmath>
+#include <ctime>
+#include <fstream>
+#include <iomanip>
+#include <limits>
+#include <sstream>
+#include <stdexcept>
+
+namespace smo {
+namespace stim_buff {
+
+namespace {
+
+constexpr const char* kDumpDirParamName = "pcloud-dump-dir";
+constexpr const char* kPcdExtension = ".pcd";
+constexpr std::size_t kXyzComponentsPerPoint = 3;
+
+std::string makePcdHeader(std::size_t width, std::size_t height)
+{
+	std::ostringstream oss;
+	oss << "# .PCD v0.7 - Point Cloud Data file format\n";
+	oss << "VERSION 0.7\n";
+	oss << "FIELDS x y z\n";
+	oss << "SIZE 4 4 4\n";
+	oss << "TYPE F F F\n";
+	oss << "COUNT 1 1 1\n";
+	oss << "WIDTH " << width << "\n";
+	oss << "HEIGHT " << height << "\n";
+	oss << "VIEWPOINT 0 0 0 1 0 0 0\n";
+	oss << "POINTS " << (width * height) << "\n";
+	oss << "DATA binary\n";
+	return oss.str();
+}
+
+float sanitizeCoordinate(float coordinate)
+{
+	if (coordinate == 0.0f)
+	{
+		return std::numeric_limits<float>::quiet_NaN();
+	}
+
+	return coordinate;
+}
+
+bool pointIsZero(float x, float y, float z)
+{
+	return x == 0.0f && y == 0.0f && z == 0.0f;
+}
+
+} // namespace
+
+LivoxPcloudFrameDumper::LivoxPcloudFrameDumper(
+	const std::shared_ptr<device::DeviceAttachmentSpec>& deviceAttachmentSpec)
+: enabled(false),
+deviceSelector(deviceAttachmentSpec ? deviceAttachmentSpec->deviceSelector : ""),
+dumpDirectory()
+{
+	if (!deviceAttachmentSpec)
+	{
+		throw std::runtime_error(
+			std::string(__func__) + ": deviceAttachmentSpec is null");
+	}
+
+	std::string dumpDirParam = parseDumpDirParam(
+		deviceAttachmentSpec->stimBuffApiParams);
+	if (dumpDirParam.empty())
+	{
+		return;
+	}
+
+	enabled = true;
+	dumpDirectory = resolveDumpDirectory(dumpDirParam);
+}
+
+void LivoxPcloudFrameDumper::prepareForRun() const
+{
+	if (!enabled)
+	{
+		return;
+	}
+
+	std::filesystem::create_directories(dumpDirectory);
+}
+
+void LivoxPcloudFrameDumper::dumpProducedFrame(
+	const livoxProto1::Device& device,
+	const StagingBuffer& collationBuffer,
+	const sscl::AsynchronousLoop& frameAssemblyResult) const
+{
+	if (!enabled)
+	{
+		return;
+	}
+
+	std::size_t nSucceeded = frameAssemblyResult.nSucceeded.load();
+	if (nSucceeded == 0)
+	{
+		return;
+	}
+
+	std::size_t pointsPerDgram = livoxProto1::Device::getNPointsPerDgram(
+		static_cast<int>(device.currentReturnMode));
+	if (pointsPerDgram == 0)
+	{
+		throw std::runtime_error(
+			std::string(__func__) + ": pointsPerDgram resolved to 0");
+	}
+
+	std::filesystem::path outputPath = makeUniqueFramePath(
+		dumpDirectory, deviceSelector);
+	writeBinaryPcdFile(
+		outputPath, collationBuffer, nSucceeded, pointsPerDgram);
+}
+
+std::string LivoxPcloudFrameDumper::parseDumpDirParam(
+	const std::vector<std::pair<std::string, std::string>>& params)
+{
+	for (auto it = params.rbegin(); it != params.rend(); ++it)
+	{
+		if (it->first == kDumpDirParamName)
+		{
+			return it->second;
+		}
+	}
+
+	return "";
+}
+
+std::filesystem::path LivoxPcloudFrameDumper::resolveDumpDirectory(
+	const std::string& dumpDirParam)
+{
+	std::filesystem::path dumpDirPath(dumpDirParam);
+	if (dumpDirPath.is_absolute())
+	{
+		return dumpDirPath;
+	}
+
+	return std::filesystem::current_path() / dumpDirPath;
+}
+
+std::string LivoxPcloudFrameDumper::makeTimestampStem(
+	const std::string& deviceSelector)
+{
+	auto now = std::chrono::system_clock::now();
+	std::time_t nowTime = std::chrono::system_clock::to_time_t(now);
+	std::tm nowTm = *std::localtime(&nowTime);
+
+	std::ostringstream oss;
+	oss << deviceSelector << "-"
+		<< std::put_time(&nowTm, "%Y-%m-%d--%H:%M:%S");
+	return oss.str();
+}
+
+std::filesystem::path LivoxPcloudFrameDumper::makeUniqueFramePath(
+	const std::filesystem::path& dumpDir,
+	const std::string& deviceSelector)
+{
+	std::string stem = makeTimestampStem(deviceSelector);
+	std::filesystem::path candidate = dumpDir / (stem + kPcdExtension);
+	if (!std::filesystem::exists(candidate))
+	{
+		return candidate;
+	}
+
+	for (std::size_t suffix = 1; ; ++suffix)
+	{
+		std::ostringstream oss;
+		oss << stem << "--" << std::setw(3) << std::setfill('0') << suffix
+			<< kPcdExtension;
+		candidate = dumpDir / oss.str();
+		if (!std::filesystem::exists(candidate))
+		{
+			return candidate;
+		}
+	}
+}
+
+void LivoxPcloudFrameDumper::writeBinaryPcdFile(
+	const std::filesystem::path& outputPath,
+	const StagingBuffer& collationBuffer,
+	std::size_t nSucceeded,
+	std::size_t pointsPerDgram)
+{
+	std::ofstream output(outputPath, std::ios::binary);
+	if (!output)
+	{
+		throw std::runtime_error(
+			std::string(__func__) + ": failed to open " + outputPath.string());
+	}
+
+	output << makePcdHeader(pointsPerDgram, nSucceeded);
+
+	struct iovec collationIov = collationBuffer.getClEngineIovec();
+	const auto* bufferBase = static_cast<const std::uint8_t*>(collationIov.iov_base);
+	const std::size_t rowNBytes =
+		pointsPerDgram * kXyzComponentsPerPoint * sizeof(float);
+	const std::size_t slotStrideNBytes = collationBuffer.slotStrideNBytes;
+
+	for (std::size_t rowIndex = 0; rowIndex < nSucceeded; ++rowIndex)
+	{
+		const auto* rowBase = reinterpret_cast<const float*>(
+			bufferBase + (rowIndex * slotStrideNBytes));
+
+		for (std::size_t pointIndex = 0; pointIndex < pointsPerDgram; ++pointIndex)
+		{
+			const std::size_t pointOffset = pointIndex * kXyzComponentsPerPoint;
+			float x = rowBase[pointOffset + 0];
+			float y = rowBase[pointOffset + 1];
+			float z = rowBase[pointOffset + 2];
+
+			if (pointIsZero(x, y, z))
+			{
+				x = sanitizeCoordinate(x);
+				y = sanitizeCoordinate(y);
+				z = sanitizeCoordinate(z);
+			}
+
+			output.write(reinterpret_cast<const char*>(&x), sizeof(float));
+			output.write(reinterpret_cast<const char*>(&y), sizeof(float));
+			output.write(reinterpret_cast<const char*>(&z), sizeof(float));
+		}
+
+		(void)rowNBytes;
+	}
+
+	if (!output)
+	{
+		throw std::runtime_error(
+			std::string(__func__) + ": failed while writing "
+			+ outputPath.string());
+	}
+}
+
+} // namespace stim_buff
+} // namespace smo

stimBuffApis/livoxGen1/livoxPcloudFrameDumper.h

@@ -0,0 +1,58 @@
+#ifndef _LIVOX_GEN1_PCLOUD_FRAME_DUMPER_H
+#define _LIVOX_GEN1_PCLOUD_FRAME_DUMPER_H
+
+#include <filesystem>
+#include <memory>
+#include <string>
+#include <livoxProto1/device.h>
+#include <spinscale/asynchronousLoop.h>
+#include <user/deviceAttachmentSpec.h>
+#include <user/stagingBuffer.h>
+
+namespace smo {
+namespace stim_buff {
+
+class LivoxPcloudFrameDumper
+{
+public:
+	explicit LivoxPcloudFrameDumper(
+		const std::shared_ptr<device::DeviceAttachmentSpec>& deviceAttachmentSpec);
+	~LivoxPcloudFrameDumper() = default;
+
+	LivoxPcloudFrameDumper(const LivoxPcloudFrameDumper&) = delete;
+	LivoxPcloudFrameDumper& operator=(const LivoxPcloudFrameDumper&) = delete;
+	LivoxPcloudFrameDumper(LivoxPcloudFrameDumper&&) = delete;
+	LivoxPcloudFrameDumper& operator=(LivoxPcloudFrameDumper&&) = delete;
+
+	bool isEnabled() const { return enabled; }
+	void prepareForRun() const;
+	void dumpProducedFrame(
+		const livoxProto1::Device& device,
+		const StagingBuffer& collationBuffer,
+		const sscl::AsynchronousLoop& frameAssemblyResult) const;
+
+private:
+	static std::string parseDumpDirParam(
+		const std::vector<std::pair<std::string, std::string>>& params);
+	static std::filesystem::path resolveDumpDirectory(
+		const std::string& dumpDirParam);
+	static std::string makeTimestampStem(const std::string& deviceSelector);
+	static std::filesystem::path makeUniqueFramePath(
+		const std::filesystem::path& dumpDir,
+		const std::string& deviceSelector);
+	static void writeBinaryPcdFile(
+		const std::filesystem::path& outputPath,
+		const StagingBuffer& collationBuffer,
+		std::size_t nSucceeded,
+		std::size_t pointsPerDgram);
+
+private:
+	bool enabled;
+	std::string deviceSelector;
+	std::filesystem::path dumpDirectory;
+};
+
+} // namespace stim_buff
+} // namespace smo
+
+#endif // _LIVOX_GEN1_PCLOUD_FRAME_DUMPER_H

stimBuffApis/livoxGen1/pcloudStimulusProducer.cpp

@@ -102,6 +102,7 @@ averageIntensityBuffer(
 	nDgramsPerStagingBufferFrame),
 averageIntensityBufferMlockPinner(
 	averageIntensityBuffer.makeMlockPinner()),
+pcloudFrameDumper(deviceAttachmentSpec),
 tempStimulusFrameMem(0),
 tempStimulusFrame(
 	FrameAssemblyDesc::SlotDesc{
@@ -154,6 +155,8 @@ void PcloudStimulusProducer::start()
 	std::cout << __func__ << ": Starting PcloudStimulusProducer for device "
 		<< device->discoveredDevice.deviceIdentifier << std::endl;
 
+	pcloudFrameDumper.prepareForRun();
+
 	// Call ioUringAssemblyEngine setup() as the first step
 	if (!ioUringAssemblyEngine.setup())
 	{
@@ -513,6 +516,23 @@ public:
 			std::cerr << __func__ << ": Failed to compact and collate frame" << std::endl;
 		} else
 		{
+			lock.unlockPrematurely();
+			if (pcloudProducer.pcloudFrameDumper.isEnabled())
+			{
+				try
+				{
+					pcloudProducer.pcloudFrameDumper.dumpProducedFrame(
+						*pcloudProducer.device,
+						pcloudProducer.collationBuffer,
+						context->frameAssemblyResult);
+				}
+				catch (const std::exception& e)
+				{
+					std::cerr << __func__ << ": Failed to dump pcloud frame: "
+						<< e.what() << std::endl;
+				}
+			}
+
 #if 0
 			// Print execution durations
 			auto assemblyDuration = pcloudProducer.ioUringAssemblyEngine.getAssemblyDuration();

stimBuffApis/livoxGen1/pcloudStimulusProducer.h

@@ -10,6 +10,7 @@
 #include <spinscale/callback.h>
 #include <user/stagingBuffer.h>
 #include "ioUringAssemblyEngine.h"
+#include "livoxPcloudFrameDumper.h"
 #include "openClCollatingAndMeshingEngine.h"
 #include "meshStimulusBuffer.h"
 #include "pcloudIntensityStimulusBuffer.h"
@@ -98,6 +99,7 @@ public:
 	StagingBuffer averageIntensityBuffer;
 	std::unique_ptr<StagingBuffer::MlockPinner>
 		averageIntensityBufferMlockPinner;
+	LivoxPcloudFrameDumper pcloudFrameDumper;
 	size_t tempStimulusFrameMem;
 	StimulusFrame tempStimulusFrame;
 	std::atomic<std::shared_ptr<MeshStimulusBuffer>> meshStimulusBuffer;