Initial version

plugin/CMakeLists.txt

@@ -0,0 +1,56 @@
+cmake_minimum_required(VERSION 3.21)
+project(spectacularAI_unity)
+
+if(MSVC)
+  set(CMAKE_CXX_STANDARD 20)
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP")
+else()
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wl,--no-as-needed")
+endif()
+
+find_package(depthai REQUIRED)
+find_package(spectacularAI_depthaiPlugin REQUIRED)
+
+set(PLUGIN_SRC
+  src/replay.cpp
+  src/output.cpp
+  src/util.cpp
+  src/depthai.cpp
+  src/mapping.cpp
+)
+
+set(PLUGIN_LIBS
+  depthai::core
+  spectacularAI::depthaiPlugin)
+
+if(MSVC)
+  # Depthai-core needs this and cmake can't find it otherwise
+  find_package(usb-1.0 REQUIRED)
+  list(APPEND PLUGIN_LIBS usb-1.0)
+endif()
+
+# enables searching for dynamic libraries from the relative path ../lib
+if(NOT MSVC)
+    set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,-rpath='$ORIGIN/../lib:$ORIGIN/../lib/3rdparty'")
+endif()
+
+add_library(${CMAKE_PROJECT_NAME} SHARED ${PLUGIN_SRC})
+target_link_libraries(${CMAKE_PROJECT_NAME} PRIVATE ${PLUGIN_LIBS})
+
+# C++ Replay example
+add_executable(main_replay ${PLUGIN_SRC} examples/main_replay.cpp)
+target_link_libraries(main_replay PRIVATE ${PLUGIN_LIBS})
+target_include_directories(main_replay PRIVATE "include/spectacularAI/unity")
+
+# C++ DepthAI example
+add_executable(main_depthai ${PLUGIN_SRC} examples/main_depthai.cpp)
+target_link_libraries(main_depthai PRIVATE ${PLUGIN_LIBS})
+target_include_directories(main_depthai PRIVATE "include/spectacularAI/unity")
+
+if(MSVC)
+  add_custom_command(TARGET ${CMAKE_PROJECT_NAME} POST_BUILD
+    COMMAND ${CMAKE_COMMAND} -E copy $<TARGET_RUNTIME_DLLS:${CMAKE_PROJECT_NAME}> $<TARGET_FILE_DIR:${CMAKE_PROJECT_NAME}>
+    COMMAND_EXPAND_LISTS
+  )
+endif()

plugin/README.md

@@ -0,0 +1,35 @@
+# spectacularAI_unity plugin
+
+## Dependencies
+
+The `spectacularAI_depthaiPlugin` package is required. For non-commercial purposes you can find one here: https://github.com/SpectacularAI/sdk/releases
+
+## Building (Windows)
+```
+mkdir target && cd target
+cmake -Ddepthai_DIR=path\to\spectacularAI_depthaiPlugin_cpp_non-commercial_1.32.0\Windows\lib\cmake\depthai -DspectacularAI_depthaiPlugin_DIR=path\to\spectacularAI_depthaiPlugin_cpp_non-commercial_1.32.0\Windows\lib\cmake\spectacularAI ..
+cmake --build . --config Release
+```
+
+Replace the existing `spectacularAI_unity.dll` [here](https://github.com/SpectacularAI/unity-wrapper/tree/main/unity-examples/Assets/SpectacularAI/Plugins/Windows).
+
+## Building (Linux)
+```
+mkdir target && cd target
+cmake -Ddepthai_DIR=path\to\spectacularAI_depthaiPlugin_cpp_non-commercial_1.32.0\Linux_Ubuntu_x86-64\lib\cmake\depthai -DspectacularAI_depthaiPlugin_DIR=path\to\spectacularAI_depthaiPlugin_cpp_non-commercial_1.32.0\Linux_Ubuntu_x86-64\lib\cmake\spectacularAI ..
+make
+```
+Replace the existing `spectacularAI_unity.so` [here](https://github.com/SpectacularAI/unity-wrapper/tree/main/unity-examples/Assets/SpectacularAI/Plugins/Linux_Ubuntu_x86-64).
+
+## Run C++ examples (for debugging/testing)
+1. Live example with DepthAI devices. Connect DepthAI device and then run
+```
+.\Release\main_depthai.exe
+```
+The position of the device should be printed in your terminal.
+
+2. If you have recorded datasets, then you can replay them using
+```
+.\Release\main_replay.exe path\to\recording
+```
+The position of the device should be printed in your terminal.

plugin/examples/main_depthai.cpp

@@ -0,0 +1,42 @@
+#include "../include/spectacularAI/unity/depthai.hpp"
+
+#include <spectacularAI/output.hpp>
+#include <iostream>
+#include <sstream>
+#include <vector>
+
+int main(int argc, char *argv[]) {
+    ConfigurationWrapper config;
+    config.lowLatency = true;
+
+    // SLAM callback
+    callback_t_mapper_output onMapperOutput = [](const MapperOutputWrapper* mapperOutput) {
+        const int64_t* updatedKeyFrames;
+        int32_t nUpdatedKeyFrames = sai_mapper_output_get_updated_key_frames(mapperOutput, &updatedKeyFrames);
+        if (sai_mapper_output_get_final_map(mapperOutput)) {
+            std::cout << "SLAM: final map: " << nUpdatedKeyFrames << std::endl;
+        } else {
+            std::cout << "SLAM: update map: " << nUpdatedKeyFrames << std::endl;
+        }
+        sai_mapper_output_release(mapperOutput); // must release memory!
+    };
+
+    PipelineWrapper* pipeline = sai_depthai_pipeline_build(&config, nullptr, 0, onMapperOutput);
+    spectacularAI::daiPlugin::Session* session = sai_depthai_pipeline_start_session(pipeline);
+
+    int counter = 0;
+    while (counter < 1000) {
+        if (sai_depthai_session_has_output(session)) {
+            ++counter;
+            VioOutputWrapper* output = sai_depthai_session_get_output(session);
+            spectacularAI::Pose pose = sai_vio_output_get_pose(output);
+            std::cout << counter << ". position = " << pose.position.x << ", " << pose.position.y << ", " << pose.position.z << std::endl;
+            sai_vio_output_release(output); // must release memory!
+        }
+    }
+
+    sai_depthai_session_release(session); // must release memory!
+    sai_depthai_pipeline_release(pipeline); // must release memory!
+
+    return 0;
+}

plugin/examples/main_replay.cpp

@@ -0,0 +1,40 @@
+#include "../include/spectacularAI/unity/replay.hpp"
+
+#include <iostream>
+#include <sstream>
+#include <vector>
+
+int main(int argc, char *argv[]) {
+    if (argc < 2) {
+        std::cout << "Usage: ./main_replay path/to/recording" << std::endl;
+        return 1;
+    }
+
+    // Recording folder
+    std::string dataFolder = argv[1];
+
+    // VIO callback
+    callback_t_vio_output onVioOutput = [](const VioOutputWrapper* output) { 
+        spectacularAI::Pose pose = sai_vio_output_get_pose(output);
+        std::cout << "position = " << pose.position.x << ", " << pose.position.y << ", " << pose.position.z << std::endl;
+        sai_vio_output_release(output); // must release memory!
+    };
+
+    // SLAM callback
+    callback_t_mapper_output onMapperOutput = [](const MapperOutputWrapper* mapperOutput) {
+        if (sai_mapper_output_get_final_map(mapperOutput)) {
+            std::cout << "SLAM: final map" << std::endl;
+        } else {
+            std::cout << "SLAM: update map" << std::endl;
+        }
+        sai_mapper_output_release(mapperOutput); // must release memory!
+    };
+
+    spectacularAI::Replay* replayHandle = sai_replay_build(dataFolder.c_str(), "", onMapperOutput);
+    sai_replay_set_output_callback(replayHandle, onVioOutput);
+
+    while (sai_replay_one_line(replayHandle));
+    sai_replay_release(replayHandle); // must release memory!
+
+    return 0;
+}

plugin/include/spectacularAI/unity/depthai.hpp

@@ -0,0 +1,69 @@
+#pragma once
+
+#include <spectacularAI/depthai/plugin.hpp>
+#include "types.hpp"
+#include "mapping.hpp"
+#include "output.hpp"
+
+struct ConfigurationWrapper {
+    bool useStereo=true;
+    bool useSlam=false;
+    bool useFeatureTracker=true;
+    bool fastVio=false;
+    bool useColorStereoCameras=false;
+    const char* mapSavePath="";
+    const char* mapLoadPath="";
+    const char* aprilTagPath="";
+    uint32_t accFrequencyHz=500;
+    uint32_t gyroFrequencyHz=400;
+    int keyframeCandidateEveryNthFrame=6;
+    const char* inputResolution="400p";
+    const char* recordingFolder="";
+    bool recordingOnly=false;
+    bool fastImu=false;
+    bool lowLatency=false;
+};
+
+struct PipelineWrapper {
+    PipelineWrapper(
+        std::shared_ptr<spectacularAI::daiPlugin::Pipeline> handle,
+        std::shared_ptr<dai::Pipeline> pipeline,
+        std::shared_ptr<dai::Device> device) : _handle(handle), _pipeline(pipeline), _device(device) {};
+    const std::shared_ptr<spectacularAI::daiPlugin::Pipeline> getHandle() const { return _handle; }
+    const std::shared_ptr<dai::Device> getDevice() const { return _device; }
+
+private:
+    const std::shared_ptr<spectacularAI::daiPlugin::Pipeline> _handle;
+    const std::shared_ptr<dai::Pipeline> _pipeline;
+    const std::shared_ptr<dai::Device> _device;
+};
+
+extern "C"
+{
+    /** Pipeline API */
+    EXPORT_API PipelineWrapper* sai_depthai_pipeline_build(
+        ConfigurationWrapper* configuration,
+        const char** internalParameters,
+        int internalParametersCount,
+        callback_t_mapper_output onMapperOutput);
+    EXPORT_API spectacularAI::daiPlugin::Session* sai_depthai_pipeline_start_session(PipelineWrapper* pipelineHandle);
+    EXPORT_API void sai_depthai_pipeline_release(PipelineWrapper* pipelineHandle);
+
+    /** Session API */
+    EXPORT_API bool sai_depthai_session_has_output(const spectacularAI::daiPlugin::Session* sessionHandle);
+    EXPORT_API VioOutputWrapper* sai_depthai_session_get_output(spectacularAI::daiPlugin::Session* sessionHandle);
+    EXPORT_API VioOutputWrapper* sai_depthai_session_wait_for_output(spectacularAI::daiPlugin::Session* sessionHandle);
+    EXPORT_API void sai_depthai_session_add_trigger(
+        spectacularAI::daiPlugin::Session* sessionHandle,
+        double t,
+        int tag);
+    EXPORT_API void sai_depthai_session_add_absolute_pose(
+        spectacularAI::daiPlugin::Session* sessionHandle,
+        spectacularAI::Pose pose,
+        Matrix3dWrapper positionCovariance,
+        double orientationVariance);
+    EXPORT_API spectacularAI::CameraPose* sai_depthai_session_get_rgb_camera_pose(
+        spectacularAI::daiPlugin::Session* sessionHandle,
+        const VioOutputWrapper* vioOutputHandle);
+    EXPORT_API void sai_depthai_session_release(spectacularAI::daiPlugin::Session* sessionHandle);
+}

plugin/include/spectacularAI/unity/mapping.hpp

@@ -0,0 +1,59 @@
+#pragma once
+
+#include <spectacularAI/mapping.hpp>
+#include "types.hpp"
+
+using MapperOutputWrapper = Wrapper<const spectacularAI::mapping::MapperOutput>;
+using MapWrapper = Wrapper<const spectacularAI::mapping::Map>;
+using KeyFrameWrapper = Wrapper<const spectacularAI::mapping::KeyFrame>;
+using FrameSetWrapper = Wrapper<spectacularAI::mapping::FrameSet>;
+using FrameWrapper = Wrapper<spectacularAI::mapping::Frame>;
+using PointCloudWrapper = Wrapper<spectacularAI::mapping::PointCloud>;
+
+typedef void (*callback_t_mapper_output)(const MapperOutputWrapper*);
+
+extern "C" {
+    /** MapperOutput API */
+    EXPORT_API MapWrapper* sai_mapper_output_get_map(const MapperOutputWrapper* mapperOutputHandle);
+    EXPORT_API int32_t sai_mapper_output_get_updated_key_frames(
+        const MapperOutputWrapper* mapperOutputHandle,
+        const int64_t** updatedKeyFramesHandle);
+    EXPORT_API bool sai_mapper_output_get_final_map(const MapperOutputWrapper* mapperOutputHandle);
+    EXPORT_API void sai_mapper_output_release(const MapperOutputWrapper* mapperOutputHandle);
+
+    /** Map API */
+    EXPORT_API int32_t sai_map_get_key_frame_count(const MapWrapper* mapHandle);
+    EXPORT_API void sai_map_get_key_frames(
+        const MapWrapper* mapHandle,
+        const KeyFrameWrapper** keyFramesHandles);
+    EXPORT_API void sai_map_release(const MapWrapper* mapHandle);
+
+    /** KeyFrame API */
+    EXPORT_API int64_t sai_key_frame_get_id(const KeyFrameWrapper* keyFrameHandle);
+    EXPORT_API FrameSetWrapper* sai_key_frame_get_frame_set(const KeyFrameWrapper* keyFrameHandle);
+    EXPORT_API PointCloudWrapper* sai_key_frame_get_point_cloud(const KeyFrameWrapper* keyFrameHandle);
+    EXPORT_API spectacularAI::Vector3d sai_key_frame_get_angular_velocity(const KeyFrameWrapper* keyFrameHandle);
+    EXPORT_API void sai_key_frame_release(const KeyFrameWrapper* keyFrameHandle);
+
+    /** FrameSet API */
+    EXPORT_API FrameWrapper* sai_point_cloud_get_primary_frame(FrameSetWrapper* frameSetHandle);
+    EXPORT_API FrameWrapper* sai_point_cloud_get_secondary_frame(FrameSetWrapper* frameSetHandle);
+    EXPORT_API FrameWrapper* sai_point_cloud_get_rgb_frame(FrameSetWrapper* frameSetHandle);
+    EXPORT_API FrameWrapper* sai_point_cloud_get_depth_frame(FrameSetWrapper* frameSetHandle);
+    EXPORT_API void sai_frame_set_release(FrameSetWrapper* frameSetHandle);
+
+    /** Frame API */
+    EXPORT_API spectacularAI::CameraPose* sai_frame_get_camera_pose(FrameWrapper* frameHandle);
+    EXPORT_API double sai_frame_get_depth_scale(FrameWrapper* frameHandle);
+    EXPORT_API void sai_frame_release(FrameWrapper* frameHandle);
+
+    /** PointCloud API */
+    EXPORT_API int sai_point_cloud_get_size(const PointCloudWrapper* pointCloudHandle);
+    EXPORT_API bool sai_point_cloud_empty(const PointCloudWrapper* pointCloudHandle);
+    EXPORT_API bool sai_point_cloud_has_normals(const PointCloudWrapper* pointCloudHandle);
+    EXPORT_API bool sai_point_cloud_has_colors(const PointCloudWrapper* pointCloudHandle);
+    EXPORT_API const spectacularAI::Vector3f* sai_point_cloud_get_position_data(PointCloudWrapper* pointCloudHandle);
+    EXPORT_API const spectacularAI::Vector3f* sai_point_cloud_get_normal_data(PointCloudWrapper* pointCloudHandle);
+    EXPORT_API const std::uint8_t* sai_point_cloud_get_rgb24_data(PointCloudWrapper* pointCloudHandle);
+    EXPORT_API void sai_point_cloud_release(PointCloudWrapper* pointCloudHandle);
+}

plugin/include/spectacularAI/unity/output.hpp

@@ -0,0 +1,56 @@
+#pragma once
+
+#include <memory>
+#include <spectacularAI/output.hpp>
+#include <spectacularAI/types.hpp>
+#include "types.hpp"
+
+// Specializations for VioOutput and Camera
+using VioOutputWrapper = Wrapper<const spectacularAI::VioOutput>;
+using CameraWrapper = Wrapper<const spectacularAI::Camera>;
+
+typedef void (*callback_t_vio_output)(const VioOutputWrapper*);
+
+extern "C" {
+    /** VioOutput API */
+    EXPORT_API spectacularAI::TrackingStatus sai_vio_output_get_tracking_status(const VioOutputWrapper* vioOutputHandle);
+    EXPORT_API spectacularAI::Pose sai_vio_output_get_pose(const VioOutputWrapper* vioOutputHandle);
+    EXPORT_API spectacularAI::Vector3d sai_vio_output_get_velocity(const VioOutputWrapper* vioOutputHandle);
+    EXPORT_API spectacularAI::Vector3d sai_vio_output_get_angular_velocity(const VioOutputWrapper* vioOutputHandle);
+    EXPORT_API spectacularAI::Vector3d sai_vio_output_get_acceleration(const VioOutputWrapper* vioOutputHandle);
+    EXPORT_API Matrix3dWrapper sai_vio_output_get_position_covariance(const VioOutputWrapper* vioOutputHandle);
+    EXPORT_API Matrix3dWrapper sai_vio_output_get_velocity_covariance(const VioOutputWrapper* vioOutputHandle);
+    EXPORT_API spectacularAI::CameraPose* sai_vio_output_get_camera_pose(const VioOutputWrapper* vioOutputHandle, int cameraId);
+    EXPORT_API int32_t sai_vio_output_get_tag(const VioOutputWrapper* vioOutputHandle);
+    EXPORT_API void sai_vio_output_release(const VioOutputWrapper* vioOutputHandle);
+
+    /** CameraPose API */
+    EXPORT_API spectacularAI::Pose sai_camera_pose_get_pose(spectacularAI::CameraPose* cameraPoseHandle);
+    EXPORT_API spectacularAI::Vector3d sai_camera_pose_get_velocity(spectacularAI::CameraPose* cameraPoseHandle);
+    EXPORT_API const CameraWrapper* sai_camera_pose_get_camera(spectacularAI::CameraPose* cameraPoseHandle);
+    EXPORT_API Matrix4dWrapper sai_camera_pose_get_world_to_camera_matrix(const spectacularAI::CameraPose* cameraPoseHandle);
+    EXPORT_API Matrix4dWrapper sai_camera_pose_get_camera_to_world_matrix(const spectacularAI::CameraPose* cameraPoseHandle);
+    EXPORT_API spectacularAI::Vector3d sai_camera_pose_get_position(const spectacularAI::CameraPose* cameraPoseHandle);
+    EXPORT_API void sai_camera_pose_release(spectacularAI::CameraPose* cameraPoseHandle);
+
+    /** Camera API */
+    EXPORT_API bool sai_camera_pixel_to_ray(
+        const CameraWrapper* cameraHandle,
+        const spectacularAI::PixelCoordinates* pixel,
+        spectacularAI::Vector3d* ray);
+    EXPORT_API bool sai_camera_ray_to_pixel(
+        const CameraWrapper* cameraHandle,
+        const spectacularAI::Vector3d* ray,
+        spectacularAI::PixelCoordinates *pixel);
+    EXPORT_API Matrix3dWrapper sai_camera_get_intrinsic_matrix(
+        const CameraWrapper* cameraHandle);
+    EXPORT_API Matrix4dWrapper sai_camera_get_projection_matrix_opengl(
+        const CameraWrapper* cameraHandle,
+        double nearClip, 
+        double farClip);
+    EXPORT_API CameraWrapper* sai_camera_build_pinhole(
+        Matrix3dWrapper intrinsicMatrix, 
+        int width,
+        int height);
+    EXPORT_API void sai_camera_release(const CameraWrapper* cameraHandle);
+}

plugin/include/spectacularAI/unity/replay.hpp

@@ -0,0 +1,23 @@
+#pragma once
+
+#include <spectacularAI/replay.hpp>
+#include "types.hpp"
+#include "output.hpp"
+#include "mapping.hpp"
+
+typedef void(*callback_t_string)(const char*);
+
+extern "C" {
+
+    EXPORT_API spectacularAI::Replay* sai_replay_build(
+        const char* folder,
+        const char* configurationYAML="",
+        callback_t_mapper_output onMapperOutput=nullptr);
+    EXPORT_API void sai_replay_start(spectacularAI::Replay* replayHandle);
+    EXPORT_API void sai_replay_run(spectacularAI::Replay* replayHandle);
+    EXPORT_API bool sai_replay_one_line(spectacularAI::Replay* replayHandle);
+    EXPORT_API void sai_replay_set_playback_speed(spectacularAI::Replay* replayHandle, double speed);
+    EXPORT_API void sai_replay_set_dry_run(spectacularAI::Replay* replayHandle, bool isDryRun);
+    EXPORT_API void sai_replay_set_output_callback(spectacularAI::Replay* replayHandle, callback_t_vio_output onOutput);
+    EXPORT_API void sai_replay_release(spectacularAI::Replay* replayHandle);
+}