Skip to content

Commit dbdead2

Browse files
kaatrasakaatrasa
committed
Initial version
1 parent 52e6278 commit dbdead2

File tree

232 files changed

+1164111
-0
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

232 files changed

+1164111
-0
lines changed

‎plugin/CMakeLists.txt‎

Lines changed: 56 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,56 @@
1+
cmake_minimum_required(VERSION 3.21)
2+
project(spectacularAI_unity)
3+
4+
if(MSVC)
5+
set(CMAKE_CXX_STANDARD 20)
6+
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP")
7+
else()
8+
set(CMAKE_CXX_STANDARD 14)
9+
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wl,--no-as-needed")
10+
endif()
11+
12+
find_package(depthai REQUIRED)
13+
find_package(spectacularAI_depthaiPlugin REQUIRED)
14+
15+
set(PLUGIN_SRC
16+
src/replay.cpp
17+
src/output.cpp
18+
src/util.cpp
19+
src/depthai.cpp
20+
src/mapping.cpp
21+
)
22+
23+
set(PLUGIN_LIBS
24+
depthai::core
25+
spectacularAI::depthaiPlugin)
26+
27+
if(MSVC)
28+
# Depthai-core needs this and cmake can't find it otherwise
29+
find_package(usb-1.0 REQUIRED)
30+
list(APPEND PLUGIN_LIBS usb-1.0)
31+
endif()
32+
33+
# enables searching for dynamic libraries from the relative path ../lib
34+
if(NOT MSVC)
35+
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,-rpath='$ORIGIN/../lib:$ORIGIN/../lib/3rdparty'")
36+
endif()
37+
38+
add_library(${CMAKE_PROJECT_NAME} SHARED ${PLUGIN_SRC})
39+
target_link_libraries(${CMAKE_PROJECT_NAME} PRIVATE ${PLUGIN_LIBS})
40+
41+
# C++ Replay example
42+
add_executable(main_replay ${PLUGIN_SRC} examples/main_replay.cpp)
43+
target_link_libraries(main_replay PRIVATE ${PLUGIN_LIBS})
44+
target_include_directories(main_replay PRIVATE "include/spectacularAI/unity")
45+
46+
# C++ DepthAI example
47+
add_executable(main_depthai ${PLUGIN_SRC} examples/main_depthai.cpp)
48+
target_link_libraries(main_depthai PRIVATE ${PLUGIN_LIBS})
49+
target_include_directories(main_depthai PRIVATE "include/spectacularAI/unity")
50+
51+
if(MSVC)
52+
add_custom_command(TARGET ${CMAKE_PROJECT_NAME} POST_BUILD
53+
COMMAND ${CMAKE_COMMAND} -E copy $<TARGET_RUNTIME_DLLS:${CMAKE_PROJECT_NAME}> $<TARGET_FILE_DIR:${CMAKE_PROJECT_NAME}>
54+
COMMAND_EXPAND_LISTS
55+
)
56+
endif()

‎plugin/README.md‎

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
# spectacularAI_unity plugin
2+
3+
## Dependencies
4+
5+
The `spectacularAI_depthaiPlugin` package is required. For non-commercial purposes you can find one here: https://github.com/SpectacularAI/sdk/releases
6+
7+
## Building (Windows)
8+
```
9+
mkdir target && cd target
10+
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 ..
11+
cmake --build . --config Release
12+
```
13+
14+
Replace the existing `spectacularAI_unity.dll` [here](https://github.com/SpectacularAI/unity-wrapper/tree/main/unity-examples/Assets/SpectacularAI/Plugins/Windows).
15+
16+
## Building (Linux)
17+
```
18+
mkdir target && cd target
19+
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 ..
20+
make
21+
```
22+
Replace the existing `spectacularAI_unity.so` [here](https://github.com/SpectacularAI/unity-wrapper/tree/main/unity-examples/Assets/SpectacularAI/Plugins/Linux_Ubuntu_x86-64).
23+
24+
## Run C++ examples (for debugging/testing)
25+
1. Live example with DepthAI devices. Connect DepthAI device and then run
26+
```
27+
.\Release\main_depthai.exe
28+
```
29+
The position of the device should be printed in your terminal.
30+
31+
2. If you have recorded datasets, then you can replay them using
32+
```
33+
.\Release\main_replay.exe path\to\recording
34+
```
35+
The position of the device should be printed in your terminal.

‎plugin/examples/main_depthai.cpp‎

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
#include "../include/spectacularAI/unity/depthai.hpp"
2+
3+
#include <spectacularAI/output.hpp>
4+
#include <iostream>
5+
#include <sstream>
6+
#include <vector>
7+
8+
int main(int argc, char *argv[]) {
9+
ConfigurationWrapper config;
10+
config.lowLatency = true;
11+
12+
// SLAM callback
13+
callback_t_mapper_output onMapperOutput = [](const MapperOutputWrapper* mapperOutput) {
14+
const int64_t* updatedKeyFrames;
15+
int32_t nUpdatedKeyFrames = sai_mapper_output_get_updated_key_frames(mapperOutput, &updatedKeyFrames);
16+
if (sai_mapper_output_get_final_map(mapperOutput)) {
17+
std::cout << "SLAM: final map: " << nUpdatedKeyFrames << std::endl;
18+
} else {
19+
std::cout << "SLAM: update map: " << nUpdatedKeyFrames << std::endl;
20+
}
21+
sai_mapper_output_release(mapperOutput); // must release memory!
22+
};
23+
24+
PipelineWrapper* pipeline = sai_depthai_pipeline_build(&config, nullptr, 0, onMapperOutput);
25+
spectacularAI::daiPlugin::Session* session = sai_depthai_pipeline_start_session(pipeline);
26+
27+
int counter = 0;
28+
while (counter < 1000) {
29+
if (sai_depthai_session_has_output(session)) {
30+
++counter;
31+
VioOutputWrapper* output = sai_depthai_session_get_output(session);
32+
spectacularAI::Pose pose = sai_vio_output_get_pose(output);
33+
std::cout << counter << ". position = " << pose.position.x << ", " << pose.position.y << ", " << pose.position.z << std::endl;
34+
sai_vio_output_release(output); // must release memory!
35+
}
36+
}
37+
38+
sai_depthai_session_release(session); // must release memory!
39+
sai_depthai_pipeline_release(pipeline); // must release memory!
40+
41+
return 0;
42+
}

‎plugin/examples/main_replay.cpp‎

Lines changed: 40 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
#include "../include/spectacularAI/unity/replay.hpp"
2+
3+
#include <iostream>
4+
#include <sstream>
5+
#include <vector>
6+
7+
int main(int argc, char *argv[]) {
8+
if (argc < 2) {
9+
std::cout << "Usage: ./main_replay path/to/recording" << std::endl;
10+
return 1;
11+
}
12+
13+
// Recording folder
14+
std::string dataFolder = argv[1];
15+
16+
// VIO callback
17+
callback_t_vio_output onVioOutput = [](const VioOutputWrapper* output) {
18+
spectacularAI::Pose pose = sai_vio_output_get_pose(output);
19+
std::cout << "position = " << pose.position.x << ", " << pose.position.y << ", " << pose.position.z << std::endl;
20+
sai_vio_output_release(output); // must release memory!
21+
};
22+
23+
// SLAM callback
24+
callback_t_mapper_output onMapperOutput = [](const MapperOutputWrapper* mapperOutput) {
25+
if (sai_mapper_output_get_final_map(mapperOutput)) {
26+
std::cout << "SLAM: final map" << std::endl;
27+
} else {
28+
std::cout << "SLAM: update map" << std::endl;
29+
}
30+
sai_mapper_output_release(mapperOutput); // must release memory!
31+
};
32+
33+
spectacularAI::Replay* replayHandle = sai_replay_build(dataFolder.c_str(), "", onMapperOutput);
34+
sai_replay_set_output_callback(replayHandle, onVioOutput);
35+
36+
while (sai_replay_one_line(replayHandle));
37+
sai_replay_release(replayHandle); // must release memory!
38+
39+
return 0;
40+
}

‎plugin/include/spectacularAI/unity/depthai.hpp‎

Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,69 @@
1+
#pragma once
2+
3+
#include <spectacularAI/depthai/plugin.hpp>
4+
#include "types.hpp"
5+
#include "mapping.hpp"
6+
#include "output.hpp"
7+
8+
struct ConfigurationWrapper {
9+
bool useStereo=true;
10+
bool useSlam=false;
11+
bool useFeatureTracker=true;
12+
bool fastVio=false;
13+
bool useColorStereoCameras=false;
14+
const char* mapSavePath="";
15+
const char* mapLoadPath="";
16+
const char* aprilTagPath="";
17+
uint32_t accFrequencyHz=500;
18+
uint32_t gyroFrequencyHz=400;
19+
int keyframeCandidateEveryNthFrame=6;
20+
const char* inputResolution="400p";
21+
const char* recordingFolder="";
22+
bool recordingOnly=false;
23+
bool fastImu=false;
24+
bool lowLatency=false;
25+
};
26+
27+
struct PipelineWrapper {
28+
PipelineWrapper(
29+
std::shared_ptr<spectacularAI::daiPlugin::Pipeline> handle,
30+
std::shared_ptr<dai::Pipeline> pipeline,
31+
std::shared_ptr<dai::Device> device) : _handle(handle), _pipeline(pipeline), _device(device) {};
32+
const std::shared_ptr<spectacularAI::daiPlugin::Pipeline> getHandle() const { return _handle; }
33+
const std::shared_ptr<dai::Device> getDevice() const { return _device; }
34+
35+
private:
36+
const std::shared_ptr<spectacularAI::daiPlugin::Pipeline> _handle;
37+
const std::shared_ptr<dai::Pipeline> _pipeline;
38+
const std::shared_ptr<dai::Device> _device;
39+
};
40+
41+
extern "C"
42+
{
43+
/** Pipeline API */
44+
EXPORT_API PipelineWrapper* sai_depthai_pipeline_build(
45+
ConfigurationWrapper* configuration,
46+
const char** internalParameters,
47+
int internalParametersCount,
48+
callback_t_mapper_output onMapperOutput);
49+
EXPORT_API spectacularAI::daiPlugin::Session* sai_depthai_pipeline_start_session(PipelineWrapper* pipelineHandle);
50+
EXPORT_API void sai_depthai_pipeline_release(PipelineWrapper* pipelineHandle);
51+
52+
/** Session API */
53+
EXPORT_API bool sai_depthai_session_has_output(const spectacularAI::daiPlugin::Session* sessionHandle);
54+
EXPORT_API VioOutputWrapper* sai_depthai_session_get_output(spectacularAI::daiPlugin::Session* sessionHandle);
55+
EXPORT_API VioOutputWrapper* sai_depthai_session_wait_for_output(spectacularAI::daiPlugin::Session* sessionHandle);
56+
EXPORT_API void sai_depthai_session_add_trigger(
57+
spectacularAI::daiPlugin::Session* sessionHandle,
58+
double t,
59+
int tag);
60+
EXPORT_API void sai_depthai_session_add_absolute_pose(
61+
spectacularAI::daiPlugin::Session* sessionHandle,
62+
spectacularAI::Pose pose,
63+
Matrix3dWrapper positionCovariance,
64+
double orientationVariance);
65+
EXPORT_API spectacularAI::CameraPose* sai_depthai_session_get_rgb_camera_pose(
66+
spectacularAI::daiPlugin::Session* sessionHandle,
67+
const VioOutputWrapper* vioOutputHandle);
68+
EXPORT_API void sai_depthai_session_release(spectacularAI::daiPlugin::Session* sessionHandle);
69+
}

‎plugin/include/spectacularAI/unity/mapping.hpp‎

Lines changed: 59 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,59 @@
1+
#pragma once
2+
3+
#include <spectacularAI/mapping.hpp>
4+
#include "types.hpp"
5+
6+
using MapperOutputWrapper = Wrapper<const spectacularAI::mapping::MapperOutput>;
7+
using MapWrapper = Wrapper<const spectacularAI::mapping::Map>;
8+
using KeyFrameWrapper = Wrapper<const spectacularAI::mapping::KeyFrame>;
9+
using FrameSetWrapper = Wrapper<spectacularAI::mapping::FrameSet>;
10+
using FrameWrapper = Wrapper<spectacularAI::mapping::Frame>;
11+
using PointCloudWrapper = Wrapper<spectacularAI::mapping::PointCloud>;
12+
13+
typedef void (*callback_t_mapper_output)(const MapperOutputWrapper*);
14+
15+
extern "C" {
16+
/** MapperOutput API */
17+
EXPORT_API MapWrapper* sai_mapper_output_get_map(const MapperOutputWrapper* mapperOutputHandle);
18+
EXPORT_API int32_t sai_mapper_output_get_updated_key_frames(
19+
const MapperOutputWrapper* mapperOutputHandle,
20+
const int64_t** updatedKeyFramesHandle);
21+
EXPORT_API bool sai_mapper_output_get_final_map(const MapperOutputWrapper* mapperOutputHandle);
22+
EXPORT_API void sai_mapper_output_release(const MapperOutputWrapper* mapperOutputHandle);
23+
24+
/** Map API */
25+
EXPORT_API int32_t sai_map_get_key_frame_count(const MapWrapper* mapHandle);
26+
EXPORT_API void sai_map_get_key_frames(
27+
const MapWrapper* mapHandle,
28+
const KeyFrameWrapper** keyFramesHandles);
29+
EXPORT_API void sai_map_release(const MapWrapper* mapHandle);
30+
31+
/** KeyFrame API */
32+
EXPORT_API int64_t sai_key_frame_get_id(const KeyFrameWrapper* keyFrameHandle);
33+
EXPORT_API FrameSetWrapper* sai_key_frame_get_frame_set(const KeyFrameWrapper* keyFrameHandle);
34+
EXPORT_API PointCloudWrapper* sai_key_frame_get_point_cloud(const KeyFrameWrapper* keyFrameHandle);
35+
EXPORT_API spectacularAI::Vector3d sai_key_frame_get_angular_velocity(const KeyFrameWrapper* keyFrameHandle);
36+
EXPORT_API void sai_key_frame_release(const KeyFrameWrapper* keyFrameHandle);
37+
38+
/** FrameSet API */
39+
EXPORT_API FrameWrapper* sai_point_cloud_get_primary_frame(FrameSetWrapper* frameSetHandle);
40+
EXPORT_API FrameWrapper* sai_point_cloud_get_secondary_frame(FrameSetWrapper* frameSetHandle);
41+
EXPORT_API FrameWrapper* sai_point_cloud_get_rgb_frame(FrameSetWrapper* frameSetHandle);
42+
EXPORT_API FrameWrapper* sai_point_cloud_get_depth_frame(FrameSetWrapper* frameSetHandle);
43+
EXPORT_API void sai_frame_set_release(FrameSetWrapper* frameSetHandle);
44+
45+
/** Frame API */
46+
EXPORT_API spectacularAI::CameraPose* sai_frame_get_camera_pose(FrameWrapper* frameHandle);
47+
EXPORT_API double sai_frame_get_depth_scale(FrameWrapper* frameHandle);
48+
EXPORT_API void sai_frame_release(FrameWrapper* frameHandle);
49+
50+
/** PointCloud API */
51+
EXPORT_API int sai_point_cloud_get_size(const PointCloudWrapper* pointCloudHandle);
52+
EXPORT_API bool sai_point_cloud_empty(const PointCloudWrapper* pointCloudHandle);
53+
EXPORT_API bool sai_point_cloud_has_normals(const PointCloudWrapper* pointCloudHandle);
54+
EXPORT_API bool sai_point_cloud_has_colors(const PointCloudWrapper* pointCloudHandle);
55+
EXPORT_API const spectacularAI::Vector3f* sai_point_cloud_get_position_data(PointCloudWrapper* pointCloudHandle);
56+
EXPORT_API const spectacularAI::Vector3f* sai_point_cloud_get_normal_data(PointCloudWrapper* pointCloudHandle);
57+
EXPORT_API const std::uint8_t* sai_point_cloud_get_rgb24_data(PointCloudWrapper* pointCloudHandle);
58+
EXPORT_API void sai_point_cloud_release(PointCloudWrapper* pointCloudHandle);
59+
}

‎plugin/include/spectacularAI/unity/output.hpp‎

Lines changed: 56 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,56 @@
1+
#pragma once
2+
3+
#include <memory>
4+
#include <spectacularAI/output.hpp>
5+
#include <spectacularAI/types.hpp>
6+
#include "types.hpp"
7+
8+
// Specializations for VioOutput and Camera
9+
using VioOutputWrapper = Wrapper<const spectacularAI::VioOutput>;
10+
using CameraWrapper = Wrapper<const spectacularAI::Camera>;
11+
12+
typedef void (*callback_t_vio_output)(const VioOutputWrapper*);
13+
14+
extern "C" {
15+
/** VioOutput API */
16+
EXPORT_API spectacularAI::TrackingStatus sai_vio_output_get_tracking_status(const VioOutputWrapper* vioOutputHandle);
17+
EXPORT_API spectacularAI::Pose sai_vio_output_get_pose(const VioOutputWrapper* vioOutputHandle);
18+
EXPORT_API spectacularAI::Vector3d sai_vio_output_get_velocity(const VioOutputWrapper* vioOutputHandle);
19+
EXPORT_API spectacularAI::Vector3d sai_vio_output_get_angular_velocity(const VioOutputWrapper* vioOutputHandle);
20+
EXPORT_API spectacularAI::Vector3d sai_vio_output_get_acceleration(const VioOutputWrapper* vioOutputHandle);
21+
EXPORT_API Matrix3dWrapper sai_vio_output_get_position_covariance(const VioOutputWrapper* vioOutputHandle);
22+
EXPORT_API Matrix3dWrapper sai_vio_output_get_velocity_covariance(const VioOutputWrapper* vioOutputHandle);
23+
EXPORT_API spectacularAI::CameraPose* sai_vio_output_get_camera_pose(const VioOutputWrapper* vioOutputHandle, int cameraId);
24+
EXPORT_API int32_t sai_vio_output_get_tag(const VioOutputWrapper* vioOutputHandle);
25+
EXPORT_API void sai_vio_output_release(const VioOutputWrapper* vioOutputHandle);
26+
27+
/** CameraPose API */
28+
EXPORT_API spectacularAI::Pose sai_camera_pose_get_pose(spectacularAI::CameraPose* cameraPoseHandle);
29+
EXPORT_API spectacularAI::Vector3d sai_camera_pose_get_velocity(spectacularAI::CameraPose* cameraPoseHandle);
30+
EXPORT_API const CameraWrapper* sai_camera_pose_get_camera(spectacularAI::CameraPose* cameraPoseHandle);
31+
EXPORT_API Matrix4dWrapper sai_camera_pose_get_world_to_camera_matrix(const spectacularAI::CameraPose* cameraPoseHandle);
32+
EXPORT_API Matrix4dWrapper sai_camera_pose_get_camera_to_world_matrix(const spectacularAI::CameraPose* cameraPoseHandle);
33+
EXPORT_API spectacularAI::Vector3d sai_camera_pose_get_position(const spectacularAI::CameraPose* cameraPoseHandle);
34+
EXPORT_API void sai_camera_pose_release(spectacularAI::CameraPose* cameraPoseHandle);
35+
36+
/** Camera API */
37+
EXPORT_API bool sai_camera_pixel_to_ray(
38+
const CameraWrapper* cameraHandle,
39+
const spectacularAI::PixelCoordinates* pixel,
40+
spectacularAI::Vector3d* ray);
41+
EXPORT_API bool sai_camera_ray_to_pixel(
42+
const CameraWrapper* cameraHandle,
43+
const spectacularAI::Vector3d* ray,
44+
spectacularAI::PixelCoordinates *pixel);
45+
EXPORT_API Matrix3dWrapper sai_camera_get_intrinsic_matrix(
46+
const CameraWrapper* cameraHandle);
47+
EXPORT_API Matrix4dWrapper sai_camera_get_projection_matrix_opengl(
48+
const CameraWrapper* cameraHandle,
49+
double nearClip,
50+
double farClip);
51+
EXPORT_API CameraWrapper* sai_camera_build_pinhole(
52+
Matrix3dWrapper intrinsicMatrix,
53+
int width,
54+
int height);
55+
EXPORT_API void sai_camera_release(const CameraWrapper* cameraHandle);
56+
}

‎plugin/include/spectacularAI/unity/replay.hpp‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
#pragma once
2+
3+
#include <spectacularAI/replay.hpp>
4+
#include "types.hpp"
5+
#include "output.hpp"
6+
#include "mapping.hpp"
7+
8+
typedef void(*callback_t_string)(const char*);
9+
10+
extern "C" {
11+
12+
EXPORT_API spectacularAI::Replay* sai_replay_build(
13+
const char* folder,
14+
const char* configurationYAML="",
15+
callback_t_mapper_output onMapperOutput=nullptr);
16+
EXPORT_API void sai_replay_start(spectacularAI::Replay* replayHandle);
17+
EXPORT_API void sai_replay_run(spectacularAI::Replay* replayHandle);
18+
EXPORT_API bool sai_replay_one_line(spectacularAI::Replay* replayHandle);
19+
EXPORT_API void sai_replay_set_playback_speed(spectacularAI::Replay* replayHandle, double speed);
20+
EXPORT_API void sai_replay_set_dry_run(spectacularAI::Replay* replayHandle, bool isDryRun);
21+
EXPORT_API void sai_replay_set_output_callback(spectacularAI::Replay* replayHandle, callback_t_vio_output onOutput);
22+
EXPORT_API void sai_replay_release(spectacularAI::Replay* replayHandle);
23+
}

0 commit comments

Comments
 (0)