Add visualizations to cpp recording tools (#19)

* Add visualization to cpp record tools (WIP)

* Add --no_record option

* Add --preview_fps option

* Remove std::filesystem usage

* Fix typo

* Fix voxel size parsing

* Serialization improvements

* Cleanup OpenGL resources

* Improve PoseTrailRenderer performance

* Fix crash on Linux

* Restructure cpp visualization code

* Rename resolution -> preview_resolution

Author: kaatrasa

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "cpp/common/3rdparty/json"]
+	path = cpp/common/3rdparty/json
+	url = https://github.com/nlohmann/json

cpp/common/3rdparty/json

@@ -0,0 +1,67 @@
+#pragma once
+
+#include <string>
+#include <chrono>
+#include <sys/stat.h>
+#include <sstream>
+#include <iomanip>
+
+#ifdef _MSC_VER
+#include <direct.h>
+#endif
+
+inline int makeDir(const std::string &dir) {
+#ifdef _MSC_VER
+    return _mkdir(dir.c_str());
+#else
+    mode_t mode = 0755;
+    return mkdir(dir.c_str(), mode);
+#endif
+}
+
+inline bool folderExists(const std::string &folder) {
+#ifdef _MSC_VER
+    struct _stat info;
+    if (_stat(folder.c_str(), &info) != 0) return false;
+    return (info.st_mode & _S_IFDIR) != 0;
+#else
+    struct stat info;
+    if (stat(folder.c_str(), &info) != 0) return false;
+    return (info.st_mode & S_IFDIR) != 0;
+#endif
+}
+
+inline bool createFolders(const std::string &folder) {
+    int ret = makeDir(folder);
+    if (ret == 0) return true;
+
+    switch (errno) {
+        case ENOENT: {
+            size_t pos = folder.find_last_of('/');
+            if (pos == std::string::npos)
+#ifdef _MSC_VER
+                pos = folder.find_last_of('\\');
+            if (pos == std::string::npos)
+#endif
+                return false;
+            if (!createFolders(folder.substr(0, pos)))
+                return false;
+            return 0 == makeDir(folder);
+        }
+        case EEXIST:
+            return folderExists(folder);
+
+        default:
+            return false;
+    }
+}
+
+inline void setAutoSubfolder(std::string &recordingFolder) {
+    auto now = std::chrono::system_clock::now();
+    auto timePoint = std::chrono::system_clock::to_time_t(now);
+    std::tm localTime = *std::localtime(&timePoint);
+    std::ostringstream oss;
+    oss << recordingFolder;
+    oss << std::put_time(&localTime, "/%Y-%m-%d_%H-%M-%S");
+    recordingFolder = oss.str();
+}

cpp/common/visualization/helpers.hpp

@@ -0,0 +1,221 @@
+#include "serialization.hpp"
+#include "helpers.hpp"
+
+#include <iostream>
+#include <nlohmann/json.hpp>
+
+namespace spectacularAI {
+namespace visualization {
+namespace {
+
+// Deleting directory in C++ without <filesystem> is painful...
+// Use python instead...
+const char* pythonScript =
+R"(
+import os
+
+def deleteDirectory(folder_path):
+    # Check if the folder exists
+    if os.path.exists(folder_path) and os.path.isdir(folder_path):
+        # Iterate over files in the folder
+        for file_name in os.listdir(folder_path):
+            if file_name.startswith("pointCloud") or file_name.startswith("json"):
+                file_path = os.path.join(folder_path, file_name)
+                try:
+                    # Delete the file
+                    os.remove(file_path)
+                except Exception as e:
+                    print(f"Error deleting file {file_path}: {e}")
+
+        # Check if the folder is empty after deleting files
+        if not os.listdir(folder_path):
+            try:
+                # Delete the folder if it is empty
+                os.rmdir(folder_path)
+            except Exception as e:
+                print(f"Error deleting folder {folder_path}: {e}")
+        else:
+            print(f"Folder {folder_path} is not empty.")
+    else:
+        print(f"Folder {folder_path} does not exist or is not a directory.")
+
+if __name__ == '__main__':
+    import argparse
+    p = argparse.ArgumentParser(__doc__)
+    p.add_argument("directory", help="Directory to delete")
+    args = p.parse_args()
+    deleteDirectory(args.directory)
+)";
+
+// Runs Python visualization using system()
+bool cleanUpTempDirectoryPython(const std::string &directory) {
+    // Create a temporary Python script
+    std::ofstream tempFile("spectacularAI_serializer_cleanup.py");
+    tempFile << pythonScript;
+    tempFile.close();
+
+    // Execute the Python script using system()
+    std::string pythonCommand = "python spectacularAI_serializer_cleanup.py " + directory;
+    int result = system(pythonCommand.c_str());
+
+    // Delete the temporary Python script
+    std::remove("spectacularAI_serializer_cleanup.py");
+
+    return result == 0;
+}
+
+nlohmann::json serializeCamera(const spectacularAI::Camera &camera) {
+    float near = 0.01f, far = 100.0f;
+    const Matrix3d &intrinsics = camera.getIntrinsicMatrix();
+    const Matrix4d &projectionMatrixOpenGL = camera.getProjectionMatrixOpenGL(near, far);
+
+    nlohmann::json json;
+    json["intrinsics"] = intrinsics;
+    json["projectionMatrixOpenGL"] = projectionMatrixOpenGL;
+    return json;
+}
+
+void serializePointCloud(const std::string &filename, std::shared_ptr<spectacularAI::mapping::PointCloud> pointCloud) {
+    std::ofstream outputStream(filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
+    if (!outputStream.is_open()) {
+        throw std::runtime_error("Failed to create temporary file <" + filename + "> for pointcloud serialization.");
+    }
+
+    std::size_t points = pointCloud->size();
+    if (points > 0) {
+        outputStream.write(
+            reinterpret_cast<const char*>(pointCloud->getPositionData()),
+            sizeof(spectacularAI::Vector3f) * points);
+
+        if (pointCloud->hasNormals()) {
+            outputStream.write(
+                reinterpret_cast<const char*>(pointCloud->getNormalData()),
+                sizeof(spectacularAI::Vector3f) * points);
+        }
+
+        if (pointCloud->hasColors()) {
+            outputStream.write(
+                reinterpret_cast<const char*>(pointCloud->getRGB24Data()),
+                sizeof(std::uint8_t) * 3 * points);
+        }
+    }
+
+    outputStream.close();
+}
+
+} // anonymous namespace
+
+Serializer::Serializer(const std::string &folder) : folder(folder) {
+    if (!folderExists(folder)) createFolders(folder);
+
+    std::string filename = folder + "/json";
+    outputStream = std::ofstream(filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
+    if (!outputStream.is_open()) {
+        throw std::runtime_error("Failed to create temporary file <" + filename + "> for vio output serialization.");
+    }
+}
+
+Serializer::~Serializer() {
+    outputStream.close();
+
+    if (!cleanUpTempDirectoryPython(folder)) {
+        std::cerr << "Failed to cleanup temporary serialization directory: " << folder << std::endl;
+    }
+}
+
+void Serializer::serializeVioOutput(spectacularAI::VioOutputPtr vioOutput) {
+    const spectacularAI::Camera &camera = *vioOutput->getCameraPose(0).camera;
+    const Matrix4d &cameraToWorld = vioOutput->getCameraPose(0).getCameraToWorldMatrix();
+
+    // Only properties used in current visualization are serialized, i.e. add more stuff if needed.
+    nlohmann::json json;
+    json["cameraPoses"] = {
+        {
+            {"camera", serializeCamera(camera)},
+            {"cameraToWorld", cameraToWorld}
+        }
+    };
+    json["trackingStatus"] = static_cast<int32_t>(vioOutput->status);
+
+    std::string jsonStr = json.dump();
+    uint32_t jsonLength = jsonStr.length();
+
+    MessageHeader header = {
+        .magicBytes = MAGIC_BYTES,
+        .messageId = messageIdCounter,
+        .jsonSize = jsonLength,
+        .binarySize = 0
+    };
+
+    std::lock_guard<std::mutex> lock(outputMutex);
+    outputStream.write(reinterpret_cast<char*>(&header), sizeof(MessageHeader));
+    outputStream.write(jsonStr.c_str(), jsonStr.size());
+    outputStream.flush();
+    messageIdCounter++;
+}
+
+void Serializer::serializeMappingOutput(spectacularAI::mapping::MapperOutputPtr mapperOutput) {
+    std::map<std::string, nlohmann::json> jsonKeyFrames;
+    std::size_t binaryLength = 0;
+
+    for (auto keyFrameId : mapperOutput->updatedKeyFrames) {
+        auto search = mapperOutput->map->keyFrames.find(keyFrameId);
+        if (search == mapperOutput->map->keyFrames.end()) continue; // deleted frame, skip
+        auto& frameSet = search->second->frameSet;
+        auto& pointCloud = search->second->pointCloud;
+        const spectacularAI::Camera &camera = *frameSet->primaryFrame->cameraPose.camera;
+        const Matrix4d &cameraToWorld = frameSet->primaryFrame->cameraPose.getCameraToWorldMatrix();
+        nlohmann::json keyFrameJson;
+        keyFrameJson["id"] = std::to_string(keyFrameId);
+        keyFrameJson["frameSet"] = {
+            {"primaryFrame", {
+                {"cameraPose", {
+                    {"camera", serializeCamera(camera)},
+                    {"cameraToWorld", cameraToWorld}
+                }}
+            }}
+        };
+        std::size_t points = pointCloud->size();
+        if (points > 0) {
+            keyFrameJson["pointCloud"] = {
+                {"size", points },
+                {"hasNormals", pointCloud->hasNormals() },
+                {"hasColors", pointCloud->hasColors() }
+            };
+            binaryLength += points * sizeof(spectacularAI::Vector3f);
+            if (pointCloud->hasNormals()) binaryLength += points * sizeof(spectacularAI::Vector3f);
+            if (pointCloud->hasColors()) binaryLength += points * sizeof(std::uint8_t) * 3;
+
+            if (serializedKeyFrameIds.find(keyFrameId) == serializedKeyFrameIds.end()) {
+                std::stringstream filename;
+                filename << folder << "/pointCloud" << keyFrameId;
+                serializedKeyFrameIds.insert(keyFrameId);
+                serializePointCloud(filename.str(), pointCloud);
+            }
+        }
+        jsonKeyFrames[keyFrameJson["id"]] = keyFrameJson;
+    }
+
+    nlohmann::json json;
+    json["updatedKeyFrames"] = mapperOutput->updatedKeyFrames;
+    json["map"] = {{"keyFrames", jsonKeyFrames}};
+    json["finalMap"] = mapperOutput->finalMap;
+
+    std::string jsonStr = json.dump();
+    uint32_t jsonLength = jsonStr.length();
+    MessageHeader header = {
+        .magicBytes = MAGIC_BYTES,
+        .messageId = messageIdCounter,
+        .jsonSize = jsonLength,
+        .binarySize = (uint32_t)binaryLength
+    };
+
+    std::lock_guard<std::mutex> lock(outputMutex);
+    outputStream.write(reinterpret_cast<char*>(&header), sizeof(MessageHeader));
+    outputStream.write(jsonStr.c_str(), jsonStr.size());
+    outputStream.flush();
+    messageIdCounter++;
+}
+
+} // namespace visualization
+} // namespace spectacularAI

cpp/common/visualization/serialization.cpp

@@ -0,0 +1,45 @@
+#pragma once
+
+#include <array>
+#include <fstream>
+#include <mutex>
+#include <set>
+
+#include <spectacularAI/output.hpp>
+#include <spectacularAI/mapping.hpp>
+
+namespace spectacularAI {
+namespace visualization {
+
+// Random number indicating start of a MessageHeader
+#define MAGIC_BYTES 2727221974
+
+using Matrix3d = std::array<std::array<double, 3>, 3>;
+using Matrix4d = std::array<std::array<double, 4>, 4>;
+using SAI_BOOL = uint8_t;
+
+struct MessageHeader {
+    uint32_t magicBytes;
+    uint32_t messageId; // Counter for debugging
+    uint32_t jsonSize;
+    uint32_t binarySize;
+};
+
+class Serializer {
+public:
+    Serializer(const std::string &folder);
+    ~Serializer();
+
+    void serializeVioOutput(spectacularAI::VioOutputPtr vioOutput);
+    void serializeMappingOutput(spectacularAI::mapping::MapperOutputPtr mapperOutput);
+
+private:
+    const std::string folder;
+    std::mutex outputMutex;
+    std::ofstream outputStream;
+    uint32_t messageIdCounter = 0;
+    std::set<int> serializedKeyFrameIds;
+};
+
+} // namespace visualization
+} // namespace spectacularAI