diff --git a/compile/CMakeLists.txt b/compile/CMakeLists.txt index 56ec87e..05e99b5 100644 --- a/compile/CMakeLists.txt +++ b/compile/CMakeLists.txt @@ -1,4 +1,5 @@ option(COMPILE_CL_CHECKS "Compile CL checks" OFF) +option(COMPILE_PCL_TOOLS "Compile PCL-based validation tools" ON) if(COMPILE_CL_CHECKS) # Find OpenCL: try find_package first, fall back to pkg-config @@ -55,3 +56,18 @@ if(COMPILE_CL_CHECKS) target_link_libraries(clzerocopycheck ${OPENCL_LIBRARIES}) endif() + +if(COMPILE_PCL_TOOLS) + enable_language(C) + find_package(MPI REQUIRED COMPONENTS C) + find_package(PCL QUIET COMPONENTS common io surface features search kdtree) + if(PCL_FOUND) + add_executable(meshFromPcd meshFromPcd.cpp) + target_include_directories(meshFromPcd PUBLIC ${PCL_INCLUDE_DIRS}) + target_link_directories(meshFromPcd PUBLIC ${PCL_LIBRARY_DIRS}) + target_link_libraries(meshFromPcd ${PCL_LIBRARIES}) + target_compile_options(meshFromPcd PRIVATE ${PCL_DEFINITIONS}) + else() + message(WARNING "PCL not found; skipping meshFromPcd build") + endif() +endif() diff --git a/compile/meshFromPcd.cpp b/compile/meshFromPcd.cpp new file mode 100644 index 0000000..dfb9eff --- /dev/null +++ b/compile/meshFromPcd.cpp @@ -0,0 +1,700 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { + +enum class MeshAlgorithm +{ + Ofm, + Gp3, +}; + +struct ToolOptions +{ + std::vector inputPaths; + std::filesystem::path outputDirectory; + bool hasOutputDirectory = false; + MeshAlgorithm algorithm = MeshAlgorithm::Ofm; + int ofmTrianglePixelSize = 1; + float ofmMaxEdgeLength = 0.25f; + pcl::OrganizedFastMesh::TriangulationType ofmTriangulationType = + pcl::OrganizedFastMesh::TRIANGLE_ADAPTIVE_CUT; + double gp3SearchRadius = 0.05; + double gp3Mu = 2.5; + int gp3MaxNeighbors = 100; + double gp3MaxSurfaceAngleDeg = 45.0; + double gp3MinAngleDeg = 10.0; + double gp3MaxAngleDeg = 120.0; + int gp3NormalK = 20; + bool gp3NormalConsistency = false; +}; + +struct ConversionStats +{ + std::filesystem::path inputPath; + std::filesystem::path outputPath; + MeshAlgorithm algorithm = MeshAlgorithm::Ofm; + std::size_t pointCount = 0; + std::size_t finitePointCount = 0; + std::size_t polygonCount = 0; + double normalDurationMs = 0.0; + double meshDurationMs = 0.0; + double totalDurationMs = 0.0; + bool success = false; + std::string errorMessage; +}; + +constexpr double kPi = 3.14159265358979323846; +constexpr std::size_t kMinimumTriangulationPoints = 3; + +double degreesToRadians(double degrees) +{ + return degrees * kPi / 180.0; +} + +std::string algorithmToString(MeshAlgorithm algorithm) +{ + switch (algorithm) + { + case MeshAlgorithm::Ofm: + return "ofm"; + case MeshAlgorithm::Gp3: + return "gp3"; + } + + throw std::runtime_error("Unsupported mesh algorithm enum"); +} + +void printUsage(const char* argv0) +{ + std::cerr + << "Usage: " << argv0 + << " [--algorithm ofm|gp3]" + << " [--output-dir DIR]" + << " [--ofm-triangle-pixel-size N]" + << " [--ofm-max-edge-length F]" + << " [--ofm-triangulation adaptive|left|right|quad]" + << " [--gp3-search-radius F]" + << " [--gp3-mu F]" + << " [--gp3-max-neighbors N]" + << " [--gp3-max-surface-angle-deg F]" + << " [--gp3-min-angle-deg F]" + << " [--gp3-max-angle-deg F]" + << " [--gp3-normal-k N]" + << " [--gp3-normal-consistency true|false]" + << " input1.pcd [input2.pcd ...]\n"; +} + +bool parseBooleanValue(const std::string& value) +{ + if (value == "true" || value == "1") + { + return true; + } + if (value == "false" || value == "0") + { + return false; + } + + throw std::runtime_error( + "Expected boolean value true|false|1|0, got: " + value); +} + +MeshAlgorithm parseAlgorithm(const std::string& value) +{ + if (value == "ofm") + { + return MeshAlgorithm::Ofm; + } + if (value == "gp3") + { + return MeshAlgorithm::Gp3; + } + + throw std::runtime_error("Unsupported algorithm: " + value); +} + +bool parseTriangulationType( + const std::string& value, + pcl::OrganizedFastMesh::TriangulationType& ofmTriangulationType) +{ + if (value == "adaptive") + { + ofmTriangulationType = + pcl::OrganizedFastMesh::TRIANGLE_ADAPTIVE_CUT; + return true; + } + if (value == "left") + { + ofmTriangulationType = + pcl::OrganizedFastMesh::TRIANGLE_LEFT_CUT; + return true; + } + if (value == "right") + { + ofmTriangulationType = + pcl::OrganizedFastMesh::TRIANGLE_RIGHT_CUT; + return true; + } + if (value == "quad") + { + ofmTriangulationType = + pcl::OrganizedFastMesh::QUAD_MESH; + return true; + } + + return false; +} + +void parseToolOption( + ToolOptions& options, + const std::string& arg, + int& i, + int argc, + char** argv) +{ + auto requireValue = [&](const char* optionName) -> std::string { + if (i + 1 >= argc) + { + throw std::runtime_error(std::string(optionName) + " requires a value"); + } + return argv[++i]; + }; + + if (arg == "--algorithm") + { + options.algorithm = parseAlgorithm(requireValue("--algorithm")); + return; + } + if (arg == "--output-dir") + { + options.outputDirectory = requireValue("--output-dir"); + options.hasOutputDirectory = true; + return; + } + if (arg == "--ofm-triangle-pixel-size") + { + options.ofmTrianglePixelSize = std::stoi( + requireValue("--ofm-triangle-pixel-size")); + return; + } + if (arg == "--ofm-max-edge-length") + { + options.ofmMaxEdgeLength = std::stof( + requireValue("--ofm-max-edge-length")); + return; + } + if (arg == "--ofm-triangulation") + { + std::string triangulationValue = requireValue("--ofm-triangulation"); + if (!parseTriangulationType( + triangulationValue, options.ofmTriangulationType)) + { + throw std::runtime_error( + "Unsupported triangulation type: " + triangulationValue); + } + return; + } + if (arg == "--gp3-search-radius") + { + options.gp3SearchRadius = std::stod( + requireValue("--gp3-search-radius")); + return; + } + if (arg == "--gp3-mu") + { + options.gp3Mu = std::stod(requireValue("--gp3-mu")); + return; + } + if (arg == "--gp3-max-neighbors") + { + options.gp3MaxNeighbors = std::stoi( + requireValue("--gp3-max-neighbors")); + return; + } + if (arg == "--gp3-max-surface-angle-deg") + { + options.gp3MaxSurfaceAngleDeg = std::stod( + requireValue("--gp3-max-surface-angle-deg")); + return; + } + if (arg == "--gp3-min-angle-deg") + { + options.gp3MinAngleDeg = std::stod( + requireValue("--gp3-min-angle-deg")); + return; + } + if (arg == "--gp3-max-angle-deg") + { + options.gp3MaxAngleDeg = std::stod( + requireValue("--gp3-max-angle-deg")); + return; + } + if (arg == "--gp3-normal-k") + { + options.gp3NormalK = std::stoi(requireValue("--gp3-normal-k")); + return; + } + if (arg == "--gp3-normal-consistency") + { + options.gp3NormalConsistency = parseBooleanValue( + requireValue("--gp3-normal-consistency")); + return; + } + + throw std::runtime_error("Unknown option: " + arg); +} + +ToolOptions parseArgs(int argc, char** argv) +{ + ToolOptions options; + + for (int i = 1; i < argc; ++i) + { + std::string arg = argv[i]; + if (!arg.empty() && arg[0] == '-') + { + parseToolOption(options, arg, i, argc, argv); + continue; + } + + options.inputPaths.emplace_back(arg); + } + + if (options.inputPaths.empty()) + { + throw std::runtime_error("At least one input .pcd file is required"); + } + + return options; +} + +std::filesystem::path computeOutputPath( + const ToolOptions& options, + const std::filesystem::path& inputPath) +{ + std::filesystem::path outputDirectory = options.hasOutputDirectory + ? options.outputDirectory + : inputPath.parent_path(); + std::filesystem::path outputFileName = inputPath.filename(); + outputFileName.replace_extension(".vtk"); + return outputDirectory / outputFileName; +} + +pcl::PointCloud::Ptr loadInputCloud( + const std::filesystem::path& inputPath) +{ + pcl::PointCloud::Ptr cloud( + new pcl::PointCloud()); + if (pcl::io::loadPCDFile(inputPath.string(), *cloud) != 0) + { + throw std::runtime_error("Failed to load input PCD"); + } + + return cloud; +} + +void ensureOutputDirectoryExists(const std::filesystem::path& outputPath) +{ + std::filesystem::create_directories(outputPath.parent_path()); +} + +void ensureOrganizedCloudForOfm(const pcl::PointCloud& cloud) +{ + if (!cloud.isOrganized()) + { + throw std::runtime_error("Input point cloud is not organized"); + } +} + +pcl::PointCloud::Ptr buildFinitePointCloud( + const pcl::PointCloud& cloud) +{ + pcl::PointCloud::Ptr finiteCloud( + new pcl::PointCloud()); + finiteCloud->reserve(cloud.size()); + + for (const auto& point : cloud.points) + { + if (!std::isfinite(point.x) || !std::isfinite(point.y) || + !std::isfinite(point.z)) + { + continue; + } + + finiteCloud->push_back(point); + } + + finiteCloud->width = static_cast(finiteCloud->size()); + finiteCloud->height = 1; + finiteCloud->is_dense = false; + return finiteCloud; +} + +void ensureEnoughFinitePoints( + const pcl::PointCloud& finiteCloud, + int normalK) +{ + if (finiteCloud.size() < kMinimumTriangulationPoints) + { + throw std::runtime_error("Too few finite points to triangulate"); + } + + if (finiteCloud.size() <= static_cast(normalK)) + { + throw std::runtime_error( + "Too few finite points for requested GP3 normal-k"); + } +} + +pcl::PointCloud::Ptr estimateNormals( + const pcl::PointCloud::Ptr& finiteCloud, + int normalK) +{ + pcl::search::KdTree::Ptr searchTree( + new pcl::search::KdTree()); + searchTree->setInputCloud(finiteCloud); + + pcl::NormalEstimation normalEstimation; + normalEstimation.setInputCloud(finiteCloud); + normalEstimation.setSearchMethod(searchTree); + normalEstimation.setKSearch(normalK); + normalEstimation.setViewPoint(0.0f, 0.0f, 0.0f); + + pcl::PointCloud::Ptr normals(new pcl::PointCloud()); + normalEstimation.compute(*normals); + return normals; +} + +pcl::PointCloud::Ptr buildPointNormalsCloud( + const pcl::PointCloud& finiteCloud, + const pcl::PointCloud& normals) +{ + pcl::PointCloud::Ptr pointNormals( + new pcl::PointCloud()); + pcl::concatenateFields(finiteCloud, normals, *pointNormals); + return pointNormals; +} + +void ensureGp3NormalsAreFinite( + const pcl::PointCloud& pointNormals) +{ + for (const auto& point : pointNormals.points) + { + if (std::isfinite(point.normal_x) && std::isfinite(point.normal_y) && + std::isfinite(point.normal_z)) + { + return; + } + } + + throw std::runtime_error("Normal estimation produced no finite normals"); +} + +void saveMeshOrThrow( + const std::filesystem::path& outputPath, + const pcl::PolygonMesh& mesh) +{ + if (mesh.polygons.empty()) + { + throw std::runtime_error("Reconstruction produced no polygons"); + } + + if (pcl::io::saveVTKFile(outputPath.string(), mesh) != 0) + { + throw std::runtime_error("Failed to save output VTK"); + } +} + +void configureOfm( + pcl::OrganizedFastMesh& ofm, + const ToolOptions& options, + const pcl::PointCloud::Ptr& cloud) +{ + ofm.setInputCloud(cloud); + ofm.setTriangulationType(options.ofmTriangulationType); + ofm.setTrianglePixelSize(options.ofmTrianglePixelSize); + ofm.setViewpoint(Eigen::Vector3f::Zero()); + ofm.setMaxEdgeLength(options.ofmMaxEdgeLength); +} + +void configureGp3( + pcl::GreedyProjectionTriangulation& gp3, + const ToolOptions& options, + const pcl::PointCloud::Ptr& pointNormals, + const pcl::search::KdTree::Ptr& searchTree) +{ + gp3.setInputCloud(pointNormals); + gp3.setSearchMethod(searchTree); + gp3.setSearchRadius(options.gp3SearchRadius); + gp3.setMu(options.gp3Mu); + gp3.setMaximumNearestNeighbors(options.gp3MaxNeighbors); + gp3.setMaximumSurfaceAngle( + degreesToRadians(options.gp3MaxSurfaceAngleDeg)); + gp3.setMinimumAngle(degreesToRadians(options.gp3MinAngleDeg)); + gp3.setMaximumAngle(degreesToRadians(options.gp3MaxAngleDeg)); + gp3.setNormalConsistency(options.gp3NormalConsistency); +} + +ConversionStats convertWithOfm( + const ToolOptions& options, + const std::filesystem::path& inputPath) +{ + ConversionStats stats; + stats.algorithm = MeshAlgorithm::Ofm; + stats.inputPath = inputPath; + stats.outputPath = computeOutputPath(options, inputPath); + + auto cloud = loadInputCloud(inputPath); + ensureOrganizedCloudForOfm(*cloud); + ensureOutputDirectoryExists(stats.outputPath); + + pcl::OrganizedFastMesh ofm; + configureOfm(ofm, options, cloud); + + pcl::PolygonMesh mesh; + auto meshStart = std::chrono::steady_clock::now(); + ofm.reconstruct(mesh); + auto meshEnd = std::chrono::steady_clock::now(); + + saveMeshOrThrow(stats.outputPath, mesh); + + stats.pointCount = cloud->size(); + stats.polygonCount = mesh.polygons.size(); + stats.meshDurationMs = std::chrono::duration( + meshEnd - meshStart).count(); + stats.totalDurationMs = stats.meshDurationMs; + stats.success = true; + return stats; +} + +ConversionStats convertWithGp3( + const ToolOptions& options, + const std::filesystem::path& inputPath) +{ + ConversionStats stats; + stats.algorithm = MeshAlgorithm::Gp3; + stats.inputPath = inputPath; + stats.outputPath = computeOutputPath(options, inputPath); + + auto cloud = loadInputCloud(inputPath); + auto finiteCloud = buildFinitePointCloud(*cloud); + ensureEnoughFinitePoints(*finiteCloud, options.gp3NormalK); + ensureOutputDirectoryExists(stats.outputPath); + + auto normalStart = std::chrono::steady_clock::now(); + auto normals = estimateNormals(finiteCloud, options.gp3NormalK); + auto normalEnd = std::chrono::steady_clock::now(); + + auto pointNormals = buildPointNormalsCloud(*finiteCloud, *normals); + ensureGp3NormalsAreFinite(*pointNormals); + + pcl::search::KdTree::Ptr searchTree( + new pcl::search::KdTree()); + searchTree->setInputCloud(pointNormals); + + pcl::GreedyProjectionTriangulation gp3; + configureGp3(gp3, options, pointNormals, searchTree); + + pcl::PolygonMesh mesh; + auto meshStart = std::chrono::steady_clock::now(); + gp3.reconstruct(mesh); + auto meshEnd = std::chrono::steady_clock::now(); + + saveMeshOrThrow(stats.outputPath, mesh); + + stats.pointCount = cloud->size(); + stats.finitePointCount = finiteCloud->size(); + stats.polygonCount = mesh.polygons.size(); + stats.normalDurationMs = std::chrono::duration( + normalEnd - normalStart).count(); + stats.meshDurationMs = std::chrono::duration( + meshEnd - meshStart).count(); + stats.totalDurationMs = stats.normalDurationMs + stats.meshDurationMs; + stats.success = true; + return stats; +} + +ConversionStats convertSingleFile( + const ToolOptions& options, + const std::filesystem::path& inputPath) +{ + try + { + if (options.algorithm == MeshAlgorithm::Ofm) + { + return convertWithOfm(options, inputPath); + } + + return convertWithGp3(options, inputPath); + } + catch (const std::exception& e) + { + ConversionStats stats; + stats.algorithm = options.algorithm; + stats.inputPath = inputPath; + stats.outputPath = computeOutputPath(options, inputPath); + stats.errorMessage = e.what(); + return stats; + } +} + +void printOfmStats(const ConversionStats& stats) +{ + std::cout << std::fixed << std::setprecision(3) + << "OK" + << " algorithm=ofm" + << " input=" << stats.inputPath + << " output=" << stats.outputPath + << " points=" << stats.pointCount + << " polygons=" << stats.polygonCount + << " mesh_ms=" << stats.meshDurationMs + << "\n"; +} + +void printGp3Stats(const ConversionStats& stats) +{ + std::cout << std::fixed << std::setprecision(3) + << "OK" + << " algorithm=gp3" + << " input=" << stats.inputPath + << " output=" << stats.outputPath + << " points=" << stats.pointCount + << " finite_points=" << stats.finitePointCount + << " polygons=" << stats.polygonCount + << " normal_ms=" << stats.normalDurationMs + << " mesh_ms=" << stats.meshDurationMs + << " total_ms=" << stats.totalDurationMs + << "\n"; +} + +void printPerFileStats(const ConversionStats& stats) +{ + if (!stats.success) + { + std::cout << "FAIL" + << " algorithm=" << algorithmToString(stats.algorithm) + << " input=" << stats.inputPath + << " error=\"" << stats.errorMessage << "\"\n"; + return; + } + + if (stats.algorithm == MeshAlgorithm::Ofm) + { + printOfmStats(stats); + return; + } + + printGp3Stats(stats); +} + +void printAggregateStats(const ToolOptions& options, + const std::vector& statsList) +{ + std::size_t successCount = 0; + std::size_t failureCount = 0; + double totalNormalMs = 0.0; + double totalMeshMs = 0.0; + double totalConversionMs = 0.0; + double minTotalMs = std::numeric_limits::max(); + double maxTotalMs = 0.0; + + for (const auto& stats : statsList) + { + if (!stats.success) + { + ++failureCount; + continue; + } + + ++successCount; + totalNormalMs += stats.normalDurationMs; + totalMeshMs += stats.meshDurationMs; + totalConversionMs += stats.totalDurationMs; + minTotalMs = std::min(minTotalMs, stats.totalDurationMs); + maxTotalMs = std::max(maxTotalMs, stats.totalDurationMs); + } + + double averageTotalMs = successCount == 0 + ? 0.0 + : totalConversionMs / static_cast(successCount); + if (successCount == 0) + { + minTotalMs = 0.0; + } + + std::cout << std::fixed << std::setprecision(3) + << "SUMMARY" + << " algorithm=" << algorithmToString(options.algorithm) + << " processed=" << statsList.size() + << " succeeded=" << successCount + << " failed=" << failureCount; + + if (options.algorithm == MeshAlgorithm::Gp3) + { + std::cout + << " total_normal_ms=" << totalNormalMs + << " total_mesh_ms=" << totalMeshMs + << " total_conversion_ms=" << totalConversionMs + << " avg_total_ms=" << averageTotalMs + << " min_total_ms=" << minTotalMs + << " max_total_ms=" << maxTotalMs + << "\n"; + return; + } + + std::cout + << " total_mesh_ms=" << totalMeshMs + << " avg_mesh_ms=" << averageTotalMs + << " min_mesh_ms=" << minTotalMs + << " max_mesh_ms=" << maxTotalMs + << "\n"; +} + +} // namespace + +int main(int argc, char** argv) +{ + try + { + ToolOptions options = parseArgs(argc, argv); + std::vector statsList; + statsList.reserve(options.inputPaths.size()); + + for (const auto& inputPath : options.inputPaths) + { + ConversionStats stats = convertSingleFile(options, inputPath); + printPerFileStats(stats); + statsList.push_back(std::move(stats)); + } + + printAggregateStats(options, statsList); + return 0; + } + catch (const std::exception& e) + { + printUsage(argv[0]); + std::cerr << e.what() << std::endl; + return 1; + } +} diff --git a/stimBuffApis/livoxGen1/CMakeLists.txt b/stimBuffApis/livoxGen1/CMakeLists.txt index 166d9f8..a2819cb 100644 --- a/stimBuffApis/livoxGen1/CMakeLists.txt +++ b/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 diff --git a/stimBuffApis/livoxGen1/livoxPcloudFrameDumper.cpp b/stimBuffApis/livoxGen1/livoxPcloudFrameDumper.cpp new file mode 100644 index 0000000..b6ab6cc --- /dev/null +++ b/stimBuffApis/livoxGen1/livoxPcloudFrameDumper.cpp @@ -0,0 +1,238 @@ +#include "livoxPcloudFrameDumper.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +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::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& 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(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>& 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(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( + 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(&x), sizeof(float)); + output.write(reinterpret_cast(&y), sizeof(float)); + output.write(reinterpret_cast(&z), sizeof(float)); + } + + (void)rowNBytes; + } + + if (!output) + { + throw std::runtime_error( + std::string(__func__) + ": failed while writing " + + outputPath.string()); + } +} + +} // namespace stim_buff +} // namespace smo diff --git a/stimBuffApis/livoxGen1/livoxPcloudFrameDumper.h b/stimBuffApis/livoxGen1/livoxPcloudFrameDumper.h new file mode 100644 index 0000000..0575ab6 --- /dev/null +++ b/stimBuffApis/livoxGen1/livoxPcloudFrameDumper.h @@ -0,0 +1,58 @@ +#ifndef _LIVOX_GEN1_PCLOUD_FRAME_DUMPER_H +#define _LIVOX_GEN1_PCLOUD_FRAME_DUMPER_H + +#include +#include +#include +#include +#include +#include +#include + +namespace smo { +namespace stim_buff { + +class LivoxPcloudFrameDumper +{ +public: + explicit LivoxPcloudFrameDumper( + const std::shared_ptr& 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>& 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 diff --git a/stimBuffApis/livoxGen1/pcloudStimulusProducer.cpp b/stimBuffApis/livoxGen1/pcloudStimulusProducer.cpp index 61c1195..1da12bf 100644 --- a/stimBuffApis/livoxGen1/pcloudStimulusProducer.cpp +++ b/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(); diff --git a/stimBuffApis/livoxGen1/pcloudStimulusProducer.h b/stimBuffApis/livoxGen1/pcloudStimulusProducer.h index 5977b8b..d6ea2c3 100644 --- a/stimBuffApis/livoxGen1/pcloudStimulusProducer.h +++ b/stimBuffApis/livoxGen1/pcloudStimulusProducer.h @@ -10,6 +10,7 @@ #include #include #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 averageIntensityBufferMlockPinner; + LivoxPcloudFrameDumper pcloudFrameDumper; size_t tempStimulusFrameMem; StimulusFrame tempStimulusFrame; std::atomic> meshStimulusBuffer;