PointcloudUtilities.cpp

Go to the documentation of this file.

// Copyright (c) 2023 FrostBit Software Lab at the Lapland University of Applied Sciences
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
 
#include "PointcloudUtilities.h"
 
#include "CoreMinimal.h"
#include "Misc/Paths.h"
#include "Misc/FileHelper.h"
#include "Async/Async.h"
#include "HAL/PlatformFileManager.h"
 
#include <fstream>
#include <vector>
#include <string>
#include <functional>
 
void UPointcloudUtilities::SaveTArrayAsPlyAsync(FString FullFileName, const TArray<FVector> points)
{
    if (points.Num() == 0)
    {
        return;
    }
 
    if (!FullFileName.EndsWith(TEXT(".ply")))
    {
        UE_LOG(LogTemp, Warning, TEXT("PointcloudUtilities.cpp: Invalid file format. Expecting filename to end with '.ply'!"));
        return;
    }
 
    FString Directory = FPaths::GetPath(FullFileName);
    IPlatformFile& PlatformFile = FPlatformFileManager::Get().GetPlatformFile();
    if (!PlatformFile.DirectoryExists(*Directory))
    {
        if (!PlatformFile.CreateDirectoryTree(*Directory))
        {
            return;
        }
    }
 
    AsyncTask(ENamedThreads::AnyBackgroundThreadNormalTask, [FullFileName, points]()
        {
            // Create PLY header in binary format
            std::string header = "ply\nformat binary_little_endian 1.0\n"
                "element vertex " + std::to_string(points.Num()) + "\n"
                "property double x\nproperty double y\nproperty double z\n"
                "end_header\n";
 
            // Calculate the size of the buffer
            size_t numPoints = points.Num();
            size_t bufferSize = numPoints * sizeof(FVector);
 
            // Open file
            std::ofstream plyFile(TCHAR_TO_UTF8(*FullFileName), std::ios::out | std::ios::binary);
            if (!plyFile.is_open())
            {
                return;
            }
 
            // Write header
            plyFile.write(header.c_str(), header.size());
 
            // Write point cloud buffer to the file
            plyFile.write(reinterpret_cast<const char*>(points.GetData()), bufferSize);
 
            // Close file
            plyFile.close();
        });
}
 
void UPointcloudUtilities::SaveVectorArrayAsPlyAsync(FString FullFileName, const std::vector<FPointData> points)
{
    if (points.empty())
    {
        return;
    }
 
    if (!FullFileName.EndsWith(TEXT(".ply")))
    {
        UE_LOG(LogTemp, Warning, TEXT("PointcloudUtilities.cpp: Invalid file format. Expecting filename to end with '.ply'!"));
        return;
    }
 
    FString Directory = FPaths::GetPath(FullFileName);
    IPlatformFile& PlatformFile = FPlatformFileManager::Get().GetPlatformFile();
    if (!PlatformFile.DirectoryExists(*Directory))
    {
        if (!PlatformFile.CreateDirectoryTree(*Directory))
        {
            return;
        }
    }
 
    AsyncTask(ENamedThreads::AnyBackgroundThreadNormalTask, [FullFileName, points]()
        {
            // Create PLY header in binary format
            std::string header = "ply\nformat binary_little_endian 1.0\n"
                "element vertex " + std::to_string(points.size()) + "\n"
                "property float x\nproperty float y\nproperty float z\n"
                "property uchar red\nproperty uchar green\nproperty uchar blue\n"
                "end_header\n";
 
            // Prepare the buffer size
            size_t numPoints = points.size();
            size_t bufferSize = numPoints * (sizeof(float) * 3 + sizeof(uint8_t) * 3);
            std::vector<char> buffer;
            buffer.reserve(bufferSize);
 
            // Prepare point data
            for (const auto& point : points)
            {
                // Write coordinates (X, Y, Z) as binary floats
                buffer.insert(buffer.end(), reinterpret_cast<const char*>(&point.X), reinterpret_cast<const char*>(&point.X) + sizeof(float));
                buffer.insert(buffer.end(), reinterpret_cast<const char*>(&point.Y), reinterpret_cast<const char*>(&point.Y) + sizeof(float));
                buffer.insert(buffer.end(), reinterpret_cast<const char*>(&point.Z), reinterpret_cast<const char*>(&point.Z) + sizeof(float));
 
                // Extract RGB components and write them as unsigned chars
                uint8_t red = (point.RGB >> 16) & 0xFF;
                uint8_t green = (point.RGB >> 8) & 0xFF;
                uint8_t blue = point.RGB & 0xFF;
                buffer.insert(buffer.end(), reinterpret_cast<const char*>(&red), reinterpret_cast<const char*>(&red) + sizeof(uint8_t));
                buffer.insert(buffer.end(), reinterpret_cast<const char*>(&green), reinterpret_cast<const char*>(&green) + sizeof(uint8_t));
                buffer.insert(buffer.end(), reinterpret_cast<const char*>(&blue), reinterpret_cast<const char*>(&blue) + sizeof(uint8_t));
            }
 
            // Open file
            std::ofstream plyFile(TCHAR_TO_UTF8(*FullFileName), std::ios::out | std::ios::binary);
            if (!plyFile.is_open())
            {
                UE_LOG(LogTemp, Warning, TEXT("Failed to open file for writing: %s"), *FullFileName);
                return;
            }
 
            // Write header
            plyFile.write(header.c_str(), header.size());
 
            // Write point cloud buffer to the file
            plyFile.write(buffer.data(), buffer.size());
 
            // Close file
            plyFile.close();
        });
}