Skip to content

Commit 6c810ad

Browse files
author
kaatrasa
committed
WIP: cpp recording tools
1 parent 0d802a7 commit 6c810ad

File tree

9 files changed

+746
-0
lines changed

9 files changed

+746
-0
lines changed

‎python/.gitignore‎ renamed to ‎.gitignore‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,2 +1,4 @@
11
__pycache__
22
*.pyc
3+
target/
4+
venv

‎cpp/k4a/CMakeLists.txt‎

Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
if(MSVC)
2+
# Windows build uses newer features
3+
cmake_minimum_required(VERSION 3.21)
4+
else()
5+
cmake_minimum_required(VERSION 3.3)
6+
endif()
7+
8+
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake")
9+
10+
project(spectacularAI_k4a_tools)
11+
12+
if(MSVC)
13+
set(CMAKE_CXX_STANDARD 20)
14+
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP")
15+
else()
16+
set(CMAKE_CXX_STANDARD 14)
17+
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra")
18+
endif()
19+
20+
find_package(Threads REQUIRED)
21+
find_package(spectacularAI_k4aPlugin REQUIRED)
22+
23+
if(MSVC) # Must be after project() is called
24+
set(CMAKE_CXX_STANDARD 20)
25+
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP /Gy")
26+
# ./cmake/Findk4a.cmake is only tested on Windows and ideally we would rely on system dependency
27+
find_package(k4a MODULE REQUIRED)
28+
else()
29+
set(CMAKE_CXX_STANDARD 14)
30+
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra")
31+
find_package(k4a REQUIRED PATHS "${k4a_DIR}")
32+
SET(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--exclude-libs=ALL")
33+
endif()
34+
35+
set(TOOL_LIBS
36+
k4a::k4a
37+
spectacularAI::k4aPlugin)
38+
39+
# enables searching for dynamic libraries from the relative path ../lib
40+
if(NOT MSVC)
41+
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,-rpath='$ORIGIN/../lib:$ORIGIN/../lib/3rdparty'")
42+
endif()
43+
44+
add_executable(sai-k4a-record record.cpp)
45+
target_link_libraries(sai-k4a-record ${TOOL_LIBS})
46+
47+
if(MSVC)
48+
add_custom_command(TARGET sai-k4a-record POST_BUILD
49+
COMMAND ${CMAKE_COMMAND} -E copy $<TARGET_RUNTIME_DLLS:sai-k4a-record> $<TARGET_FILE_DIR:sai-k4a-record>
50+
COMMAND_EXPAND_LISTS
51+
)
52+
endif()

‎cpp/k4a/cmake/Findk4a.cmake‎

Lines changed: 88 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,88 @@
1+
#.rst:
2+
# Findk4a
3+
# -------
4+
#
5+
# Find Azure Kinect Sensor SDK include dirs, and libraries.
6+
#
7+
# IMPORTED Targets
8+
# ^^^^^^^^^^^^^^^^
9+
#
10+
# This module defines the :prop_tgt:`IMPORTED` targets:
11+
#
12+
# ``k4a::k4a``
13+
# Defined if the system has Azure Kinect Sensor SDK.
14+
#
15+
# Result Variables
16+
# ^^^^^^^^^^^^^^^^
17+
#
18+
# This module sets the following variables:
19+
#
20+
# ::
21+
#
22+
# k4a_FOUND True in case Azure Kinect Sensor SDK is found, otherwise false
23+
# k4a_ROOT Path to the root of found Azure Kinect Sensor SDK installation
24+
#
25+
# Example usage
26+
# ^^^^^^^^^^^^^
27+
#
28+
# ::
29+
#
30+
# find_package(k4a REQUIRED)
31+
#
32+
# add_executable(foo foo.cc)
33+
# target_link_libraries(foo k4a::k4a)
34+
#
35+
# License
36+
# ^^^^^^^
37+
#
38+
# Copyright (c) 2019 Tsukasa SUGIURA
39+
# Distributed under the MIT License.
40+
#
41+
# Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
42+
# The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
43+
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
44+
#
45+
46+
find_path(k4a_INCLUDE_DIR
47+
NAMES
48+
k4a/k4a.h
49+
HINTS
50+
${k4a_DIR}/sdk/
51+
PATHS
52+
"${k4a_PATH_DIR}/sdk/"
53+
PATH_SUFFIXES
54+
include
55+
)
56+
57+
find_library(k4a_LIBRARY
58+
NAMES
59+
k4a.lib
60+
HINTS
61+
${k4a_DIR}/sdk/windows-desktop/amd64/release
62+
PATHS
63+
"${k4a_PATH_DIR}/sdk/windows-desktop/amd64/release"
64+
PATH_SUFFIXES
65+
lib
66+
)
67+
68+
include(FindPackageHandleStandardArgs)
69+
find_package_handle_standard_args(
70+
k4a DEFAULT_MSG
71+
k4a_LIBRARY k4a_INCLUDE_DIR
72+
)
73+
74+
if(k4a_FOUND)
75+
add_library(k4a::k4a SHARED IMPORTED)
76+
77+
set_target_properties(k4a::k4a PROPERTIES INTERFACE_INCLUDE_DIRECTORIES "${k4a_INCLUDE_DIR}")
78+
set_property(TARGET k4a::k4a APPEND PROPERTY IMPORTED_CONFIGURATIONS "RELEASE")
79+
set_target_properties(k4a::k4a PROPERTIES IMPORTED_LINK_INTERFACE_LANGUAGES_RELEASE "CXX")
80+
81+
# TODO: Ugly "hack" to find the DLL. Couldn't find any better way of doing this
82+
string(REPLACE "lib/k4a.lib" "bin/k4a.dll" k4a_RUNTIME_LIBRARY "${k4a_LIBRARY}")
83+
84+
set_target_properties(k4a::k4a PROPERTIES IMPORTED_IMPLIB "${k4a_LIBRARY}")
85+
set_target_properties(k4a::k4a PROPERTIES IMPORTED_LOCATION "${k4a_RUNTIME_LIBRARY}")
86+
87+
get_filename_component(K4a_ROOT "${k4a_INCLUDE_DIR}" PATH)
88+
endif()

‎cpp/k4a/cmake/spectacularAI_k4aPluginConfig.cmake‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
include("${CMAKE_CURRENT_LIST_DIR}/spectacularAI_k4aPluginTargets.cmake")

‎cpp/k4a/record.cpp‎

Lines changed: 195 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,195 @@
1+
#include <atomic>
2+
#include <iostream>
3+
#include <thread>
4+
#include <vector>
5+
#include <k4a/k4a.h>
6+
#include <k4a/k4atypes.h>
7+
#include <spectacularAI/k4a/plugin.hpp>
8+
9+
void showUsage() {
10+
std::cout << "Supported arguments:" << std::endl
11+
<< " -h, --help Help" << std::endl
12+
<< " --output <recording_folder>, recorded output" << std::endl
13+
<< " --recording_only, disables Vio" << std::endl
14+
<< " --color_res <720p, 1080p, 1440p, 1536p, 2160p, 3070p>" << std::endl
15+
<< " --depth_mode <1 (NVOF_2X2BINNED), 2 (NVOF_UNBINNED), 3 (WFOV_2X2BINNED), 4 (WFOV_UNBINNED)>" << std::endl
16+
<< " --frame_rate <5, 15, 30> " << std::endl
17+
<< " --align, use k4a sdk to align depth images to color images (note depth image fov!)" << std::endl
18+
<< " --mono (no depth images)" << std::endl
19+
<< " --fast" << std::endl
20+
<< " --vio_only (-useSlam=false)" << std::endl
21+
<< " --exposure <microseconds>" << std::endl
22+
<< " --whitebalance <kelvins>" << std::endl
23+
<< " --gain [0-255]" << std::endl
24+
<< " --brightness [0-255]" << std::endl
25+
<< " --print" << std::endl
26+
<< std::endl;
27+
}
28+
29+
int main(int argc, char *argv[]) {
30+
std::vector<std::string> arguments(argv, argv + argc);
31+
std::string colorResolution = "720p";
32+
int depthMode = K4A_DEPTH_MODE_WFOV_2X2BINNED;
33+
int frameRate = 30;
34+
int32_t exposureTimeMicroseconds = -1;
35+
int32_t whiteBalanceKelvins = -1;
36+
int32_t gain = -1;
37+
int32_t brightness = -1;
38+
spectacularAI::k4aPlugin::Configuration config;
39+
bool print = false;
40+
41+
for (size_t i = 1; i < arguments.size(); ++i) {
42+
const std::string &argument = arguments.at(i);
43+
if (argument == "--output")
44+
config.recordingFolder = arguments.at(++i);
45+
else if (argument == "--recording_only")
46+
config.recordingOnly = true;
47+
else if (argument == "--color_res")
48+
colorResolution = arguments.at(++i);
49+
else if (argument == "--depth_mode")
50+
depthMode = std::stoi(arguments.at(++i));
51+
else if (argument == "--frame_rate")
52+
frameRate = std::stoi(arguments.at(++i));
53+
else if (argument == "--align")
54+
config.alignedDepth = true;
55+
else if (argument == "--mono")
56+
config.useStereo = false;
57+
else if (argument == "--fast")
58+
config.fastVio = true;
59+
else if (argument == "--vio_only")
60+
config.useSlam = false;
61+
else if (argument == "--print")
62+
print = true;
63+
else if (argument == "--exposure")
64+
exposureTimeMicroseconds = std::stoi(arguments.at(++i));
65+
else if (argument == "--whitebalance")
66+
whiteBalanceKelvins = std::stoi(arguments.at(++i));
67+
else if (argument == "--gain")
68+
gain = std::stoi(arguments.at(++i));
69+
else if (argument == "--brightness")
70+
brightness = std::stoi(arguments.at(++i));
71+
else if (argument == "--help" || argument == "-h") {
72+
showUsage();
73+
return EXIT_SUCCESS;
74+
} else {
75+
showUsage();
76+
std::cerr << "Unknown argument: " + argument << std::endl;
77+
return EXIT_FAILURE;
78+
}
79+
}
80+
81+
// Require recording folder when using recording only mode.
82+
if (config.recordingOnly && config.recordingFolder.empty())
83+
fatal_error("Record only but recording folder is not set!");
84+
85+
// In monocular mode, disable depth camera.
86+
if (!config.useStereo) depthMode = K4A_DEPTH_MODE_OFF;
87+
88+
// Get configuration for k4a device.
89+
config.k4aConfig = spectacularAI::k4aPlugin::getK4AConfiguration(colorResolution, depthMode, frameRate, config.useStereo);
90+
91+
// Create vio pipeline using the config
92+
spectacularAI::k4aPlugin::Pipeline vioPipeline(config);
93+
94+
k4a_device_t deviceHandle = vioPipeline.getDeviceHandle();
95+
if (exposureTimeMicroseconds > 0) {
96+
/*
97+
Very limited number of options are actually supported;
98+
And the options depend on K4A_COLOR_CONTROL_POWERLINE_FREQUENCY.
99+
Mode: 50Hz, 60Hz
100+
{ 500, 500},
101+
{ 1250, 1250},
102+
{ 2500, 2500},
103+
{ 10000, 8330},
104+
{ 20000, 16670},
105+
{ 30000, 33330},
106+
{ 40000, 41670},
107+
{ 50000, 50000},
108+
{ 60000, 66670},
109+
{ 80000, 83330},
110+
{ 100000, 100000},
111+
{ 120000, 116670},
112+
{ 130000, 133330}
113+
The exposure time is also limited by camera FPS and some of the options did not seem to work on Ubuntu 20.04.
114+
*/
115+
116+
// https://github.com/microsoft/Azure-Kinect-Sensor-SDK/blob/develop/src/color/color_priv.h
117+
k4a_result_t res = k4a_device_set_color_control(deviceHandle,
118+
k4a_color_control_command_t::K4A_COLOR_CONTROL_EXPOSURE_TIME_ABSOLUTE,
119+
k4a_color_control_mode_t::K4A_COLOR_CONTROL_MODE_MANUAL,
120+
exposureTimeMicroseconds
121+
);
122+
if (res != K4A_RESULT_SUCCEEDED) {
123+
std::cerr << "Failed to set exposure time to " + std::to_string(exposureTimeMicroseconds) << std::endl;
124+
return EXIT_FAILURE;
125+
}
126+
}
127+
128+
if (whiteBalanceKelvins > 0) {
129+
// The unit is degrees Kelvin. The setting must be set to a value evenly divisible by 10 degrees.
130+
int32_t kelvins = whiteBalanceKelvins;
131+
kelvins = kelvins - kelvins % 10; // floor to nearest 10
132+
k4a_result_t res = k4a_device_set_color_control(deviceHandle,
133+
k4a_color_control_command_t::K4A_COLOR_CONTROL_WHITEBALANCE,
134+
k4a_color_control_mode_t::K4A_COLOR_CONTROL_MODE_MANUAL,
135+
kelvins
136+
);
137+
if (res != K4A_RESULT_SUCCEEDED) {
138+
std::cerr << "Failed to set white balance to " << std::to_string(kelvins) << " (might be out of valid range, try 2500)" << std::endl;
139+
return EXIT_FAILURE;
140+
}
141+
}
142+
143+
if (brightness >= 0) {
144+
k4a_result_t res = k4a_device_set_color_control(deviceHandle,
145+
k4a_color_control_command_t::K4A_COLOR_CONTROL_BRIGHTNESS,
146+
k4a_color_control_mode_t::K4A_COLOR_CONTROL_MODE_MANUAL,
147+
brightness
148+
);
149+
if (res != K4A_RESULT_SUCCEEDED) {
150+
std::cerr << "Failed to set brightness to " << std::to_string(brightness) << " (try 255)" << std::endl;
151+
return EXIT_FAILURE;
152+
}
153+
}
154+
155+
if (gain >= 0) {
156+
k4a_result_t res = k4a_device_set_color_control(deviceHandle,
157+
k4a_color_control_command_t::K4A_COLOR_CONTROL_GAIN,
158+
k4a_color_control_mode_t::K4A_COLOR_CONTROL_MODE_MANUAL,
159+
gain
160+
);
161+
if (res != K4A_RESULT_SUCCEEDED) {
162+
std::cerr << "Failed to set gain to " << std::to_string(gain) << " (try 255)" << std::endl;
163+
return EXIT_FAILURE;
164+
}
165+
}
166+
167+
// Start k4a device and vio
168+
auto session = vioPipeline.startSession();
169+
170+
std::atomic<bool> shouldQuit(false);
171+
std::thread inputThread([&]() {
172+
std::cout << "Press Enter to quit." << std::endl << std::endl;
173+
getchar();
174+
shouldQuit = true;
175+
});
176+
177+
while (!shouldQuit) {
178+
if (session->hasOutput()) {
179+
auto out = session->getOutput();
180+
if (print) {
181+
std::cout << "Vio API pose: " << out->pose.time << ", " << out->pose.position.x
182+
<< ", " << out->pose.position.y << ", " << out->pose.position.z << ", "
183+
<< out->pose.orientation.x << ", " << out->pose.orientation.y << ", "
184+
<< out->pose.orientation.z << ", " << out->pose.orientation.w
185+
<< std::endl;
186+
}
187+
}
188+
std::this_thread::sleep_for(std::chrono::milliseconds(1));
189+
}
190+
191+
std::cout << "Exiting." << std::endl;
192+
if (shouldQuit) inputThread.join();
193+
194+
return EXIT_SUCCESS;
195+
}

‎cpp/orbbec/CMakeLists.txt‎

Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
if(MSVC)
2+
# Windows build uses newer features
3+
cmake_minimum_required(VERSION 3.21)
4+
else()
5+
cmake_minimum_required(VERSION 3.3)
6+
endif()
7+
8+
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake")
9+
10+
project(spectacularAI_orbbec_tools)
11+
12+
if(MSVC)
13+
set(CMAKE_CXX_STANDARD 20)
14+
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP")
15+
else()
16+
set(CMAKE_CXX_STANDARD 14)
17+
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra")
18+
endif()
19+
20+
find_package(Threads REQUIRED)
21+
find_package(spectacularAI_orbbecPlugin REQUIRED)
22+
23+
if(MSVC) # Must be after project() is called
24+
set(CMAKE_CXX_STANDARD 20)
25+
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP /Gy")
26+
find_package(OrbbecSDK CONFIG REQUIRED PATHS "${OrbbecSDK_DIR}")
27+
else()
28+
set(CMAKE_CXX_STANDARD 14)
29+
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra")
30+
find_package(OrbbecSDK CONFIG REQUIRED PATHS "${OrbbecSDK_DIR}")
31+
SET(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--exclude-libs=ALL")
32+
endif()
33+
34+
set(TOOL_LIBS
35+
${OrbbecSDK_LIBS}
36+
spectacularAI::orbbecPlugin)
37+
38+
# enables searching for dynamic libraries from the relative path ../lib
39+
if(NOT MSVC)
40+
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,-rpath='$ORIGIN/../lib:$ORIGIN/../lib/3rdparty'")
41+
endif()
42+
43+
add_executable(sai-record-orbbec record.cpp)
44+
target_link_libraries(sai-record-orbbec ${TOOL_LIBS})
45+
target_include_directories(sai-record-orbbec PRIVATE ${OrbbecSDK_INCLUDE_DIRS})
46+
47+
if(MSVC)
48+
add_custom_command(TARGET sai-record-orbbec POST_BUILD
49+
COMMAND ${CMAKE_COMMAND} -E copy $<TARGET_RUNTIME_DLLS:sai-record-orbbec> $<TARGET_FILE_DIR:sai-record-orbbec>
50+
COMMAND_EXPAND_LISTS
51+
)
52+
endif()

0 commit comments

Comments
 (0)