├── .github └── workflows │ └── CI.yml ├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── LICENSE ├── MANIFEST.in ├── README.md ├── examples ├── main.cpp └── python.py ├── include └── libmotioncapture │ ├── fzmotion.h │ ├── mock.h │ ├── motionanalysis.h │ ├── motioncapture.h │ ├── nokov.h │ ├── optitrack.h │ ├── optitrack_closed_source.h │ ├── qualisys.h │ ├── vicon.h │ └── vrpn.h ├── pyproject.toml ├── setup.py ├── src ├── fzmotion.cpp ├── mock.cpp ├── motionanalysis.cpp ├── motioncapture.cpp ├── nokov.cpp ├── optitrack.cpp ├── optitrack_closed_source.cpp ├── python_bindings.cpp ├── qualisys.cpp ├── vicon.cpp └── vrpn.cpp ├── tools └── build │ └── build-wheels.sh └── version /.github/workflows/CI.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: 4 | push: 5 | branches: [ main ] 6 | tags: 7 | - '*' 8 | pull_request: 9 | branches: [ main ] 10 | # schedule: 11 | # # Weekly build to make sure dependencies are OK 12 | # - cron: '30 14 * * 3' 13 | 14 | jobs: 15 | Source: 16 | runs-on: ubuntu-latest 17 | 18 | steps: 19 | - uses: actions/checkout@v3 20 | with: 21 | submodules: true 22 | 23 | - name: Install sdist dependencies 24 | run: pip3 install setuptools 25 | 26 | - name: Generate source distribution tarball 27 | run: python3 setup.py sdist 28 | 29 | - name: Build source distribution 30 | run: | 31 | sudo apt-get install -y libboost-system-dev libboost-thread-dev libeigen3-dev ninja-build 32 | pip3 install wheel 33 | pip3 wheel dist/*.tar.gz 34 | 35 | - name: Upload Build Artifact 36 | uses: actions/upload-artifact@v4 37 | with: 38 | name: source-build 39 | path: "dist/*.tar.gz" 40 | 41 | Linux: 42 | runs-on: ubuntu-22.04 43 | needs: Source 44 | 45 | strategy: 46 | matrix: 47 | # active versions: https://devguide.python.org/versions/ 48 | python: ['3.9', '3.10', '3.11', '3.12', '3.13'] 49 | 50 | steps: 51 | - uses: actions/checkout@v3 52 | with: 53 | submodules: true 54 | 55 | - name: Pull builder image 56 | run: docker pull quay.io/pypa/manylinux_2_28_x86_64 57 | 58 | - name: Python version 59 | run: python --version 60 | 61 | - name: Build wheels 62 | run: tools/build/build-wheels.sh ${{ matrix.python }} 63 | 64 | - name: Upload Build Artifact 65 | uses: actions/upload-artifact@v4 66 | with: 67 | name: linux-build-${{ strategy.job-index }} 68 | path: "dist/*.whl" 69 | 70 | MacOS: 71 | runs-on: macos-14 72 | needs: Source 73 | 74 | strategy: 75 | matrix: 76 | # active versions: https://devguide.python.org/versions/ 77 | python: ['3.9', '3.10', '3.11', '3.12', '3.13'] 78 | 79 | steps: 80 | - uses: actions/checkout@v3 81 | with: 82 | submodules: true 83 | 84 | - name: Set up Python ${{ matrix.python }} 85 | uses: actions/setup-python@v4 86 | with: 87 | python-version: ${{ matrix.python }} 88 | 89 | # Using older boost version to avoid compiler errors with io_service 90 | - name: Install dependencies 91 | run: | 92 | brew install eigen boost 93 | pip install cmake ninja wheel setuptools 94 | 95 | - name: Build 96 | run: | 97 | python setup.py bdist_wheel 98 | 99 | - name: Upload Build Artifact 100 | uses: actions/upload-artifact@v4 101 | with: 102 | name: macos-build-${{ strategy.job-index }} 103 | path: "dist/*.whl" 104 | 105 | Windows: 106 | runs-on: windows-2022 107 | needs: Source 108 | 109 | strategy: 110 | matrix: 111 | # active versions: https://devguide.python.org/versions/ 112 | python: ['3.9', '3.10', '3.11', '3.12', '3.13'] 113 | 114 | steps: 115 | - uses: actions/checkout@v3 116 | with: 117 | submodules: true 118 | 119 | - name: Set up Python ${{ matrix.python }} 120 | uses: actions/setup-python@v4 121 | with: 122 | python-version: ${{ matrix.python }} 123 | 124 | - name: Install dependencies 125 | run: | 126 | pip install wheel setuptools 127 | vcpkg install eigen3:x64-windows 128 | 129 | - name: Install boost 130 | uses: MarkusJx/install-boost@v2 131 | id: install-boost 132 | with: 133 | boost_version: 1.87.0 134 | platform_version: 2022 135 | toolset: msvc 136 | link: static 137 | 138 | - name: Build 139 | run: python setup.py bdist_wheel 140 | env: 141 | BOOST_ROOT: ${{ steps.install-boost.outputs.BOOST_ROOT }} 142 | CMAKE_TOOLCHAIN_FILE: "C:/vcpkg/scripts/buildsystems/vcpkg.cmake" 143 | 144 | - name: Upload Build Artifact 145 | uses: actions/upload-artifact@v4 146 | with: 147 | name: win-build-${{ strategy.job-index }} 148 | path: "dist/*.whl" 149 | 150 | Publish: 151 | runs-on: ubuntu-22.04 152 | environment: 153 | name: pypi 154 | url: https://pypi.org/p/motioncapture 155 | permissions: 156 | id-token: write # IMPORTANT: this permission is mandatory for trusted publishing 157 | needs: [Source, Linux, MacOS, Windows] 158 | # Only run publish if we have tagged the version 159 | if: github.event_name == 'push' && startsWith(github.ref, 'refs/tags') 160 | steps: 161 | # download all the artifacts 162 | - uses: actions/download-artifact@v4 163 | 164 | - name: Move all files into the dist folder 165 | run: | 166 | mkdir dist 167 | mv source-build/* dist 168 | mv linux-build-*/* dist 169 | mv macos-build-*/* dist 170 | mv win-build-*/* dist 171 | ls -R dist 172 | 173 | - uses: actions/setup-python@v4 174 | with: 175 | python-version: '3.8' 176 | 177 | - name: Check match between the build version and the current git tag 178 | run: | 179 | sudo apt-get install -y jq 180 | pip3 install wheel-inspect 181 | 182 | export TAG=$(echo ${{ github.ref }} | cut -d/ -f3) 183 | python3 -m wheel_inspect dist/*.whl | jq .version | grep ^\\\"$TAG\\\"$ 184 | 185 | - name: Publish on Pypi 186 | uses: pypa/gh-action-pypi-publish@release/v1 187 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build/* 2 | .vscode/ 3 | dist/ 4 | 5 | *.so 6 | *.egg-info/ 7 | *.whl -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "externalDependencies/vicon-datastream-sdk"] 2 | path = deps/vicon-datastream-sdk 3 | url = https://github.com/whoenig/vicon-datastream-sdk.git 4 | [submodule "externalDependencies/qualisys_cpp_sdk"] 5 | path = deps/qualisys_cpp_sdk 6 | url = https://github.com/qualisys/qualisys_cpp_sdk.git 7 | shallow = true 8 | [submodule "externalDependencies/pybind11"] 9 | path = deps/pybind11 10 | url = https://github.com/pybind/pybind11.git 11 | [submodule "externalDependencies/vrpn"] 12 | path = deps/vrpn 13 | url = https://github.com/vrpn/vrpn.git 14 | [submodule "externalDependencies/NatNetSDKCrossplatform"] 15 | path = deps/NatNetSDKCrossplatform 16 | url = https://github.com/whoenig/NatNetSDKCrossplatform.git 17 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(libmotioncapture) 3 | 4 | # define some options 5 | option(LIBMOTIONCAPTURE_BUILD_PYTHON_BINDINGS "Generate Python Bindings" ON) 6 | option(LIBMOTIONCAPTURE_ENABLE_QUALISYS "Enable Qualisys" ON) 7 | option(LIBMOTIONCAPTURE_ENABLE_OPTITRACK "Enable Optitrack" ON) 8 | option(LIBMOTIONCAPTURE_ENABLE_OPTITRACK_CLOSED_SOURCE "Enable Optitrack (Closed Source)" ON) 9 | option(LIBMOTIONCAPTURE_ENABLE_VICON "Enable Vicon" ON) 10 | option(LIBMOTIONCAPTURE_ENABLE_NOKOV "Enable Nokov" OFF) 11 | option(LIBMOTIONCAPTURE_ENABLE_VRPN "Enable VRPN" ON) 12 | option(LIBMOTIONCAPTURE_ENABLE_FZMOTION "Enable FZMOTION" ON) 13 | option(LIBMOTIONCAPTURE_ENABLE_MOTIONANALYSIS "Enable MotionAnalysis" OFF) 14 | option(LIBMOTIONCAPTURE_BUILD_EXAMPLE "Enable Example application" ON) 15 | 16 | # Enable C++14 17 | set(CMAKE_CXX_STANDARD 14) 18 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 19 | set(CMAKE_CXX_EXTENSIONS OFF) 20 | 21 | find_package(Boost 1.5 COMPONENTS system REQUIRED) # for optitrack 22 | add_definitions( 23 | -DBOOST_DATE_TIME_NO_LIB 24 | -DBOOST_REGEX_NO_LIB 25 | -D_BUILD_STATIC_LIB 26 | ) 27 | 28 | find_package(Eigen3 REQUIRED) 29 | 30 | set(VICON_SDK_DIR ${CMAKE_CURRENT_SOURCE_DIR}/deps/vicon-datastream-sdk/) 31 | set(QUALISYS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/deps/qualisys_cpp_sdk/) 32 | set(NATNET_SDK_DIR ${CMAKE_CURRENT_SOURCE_DIR}/deps/NatNetSDKCrossplatform/) 33 | set(NOKOV_SDK_DIR ${CMAKE_CURRENT_SOURCE_DIR}/deps/nokov_sdk/) 34 | set(VRPN_DIR ${CMAKE_CURRENT_SOURCE_DIR}/deps/vrpn/) 35 | set(MOTIONANALYSIS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/deps/cortex_sdk_linux/) 36 | 37 | 38 | set(CMAKE_POSITION_INDEPENDENT_CODE ON) 39 | 40 | ########### 41 | ## Build ## 42 | ########### 43 | 44 | ## Additional include folders 45 | set(my_include_directories 46 | include 47 | ${Boost_INCLUDE_DIRS} 48 | ${EIGEN3_INCLUDE_DIRS} 49 | ) 50 | set(my_link_directories 51 | ${Boost_LIBRARY_DIRS} 52 | ) 53 | set(my_files 54 | src/motioncapture.cpp 55 | src/mock.cpp 56 | ) 57 | 58 | if (LIBMOTIONCAPTURE_ENABLE_VICON) 59 | message("including vicon") 60 | 61 | add_definitions(-DENABLE_VICON) 62 | add_subdirectory(deps/vicon-datastream-sdk EXCLUDE_FROM_ALL) 63 | set(my_include_directories 64 | ${my_include_directories} 65 | ${VICON_SDK_DIR}/Vicon/CrossMarket/DataStream 66 | ) 67 | set(my_files 68 | ${my_files} 69 | src/vicon.cpp 70 | ) 71 | set(my_libraries 72 | ${my_libraries} 73 | ViconDataStreamSDK_CPP 74 | ) 75 | endif() 76 | 77 | if (LIBMOTIONCAPTURE_ENABLE_OPTITRACK) 78 | message("including optitrack") 79 | 80 | add_definitions(-DENABLE_OPTITRACK) 81 | set(my_include_directories 82 | ${my_include_directories} 83 | ) 84 | set(my_files 85 | ${my_files} 86 | src/optitrack.cpp 87 | ) 88 | set(my_libraries 89 | ${my_libraries} 90 | Boost::system 91 | ) 92 | endif() 93 | 94 | if (LIBMOTIONCAPTURE_ENABLE_OPTITRACK_CLOSED_SOURCE) 95 | if ((CMAKE_SYSTEM_NAME STREQUAL "Linux") AND (CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64")) 96 | message("Including optitrack_closed_source") 97 | 98 | add_definitions(-DENABLE_OPTITRACK_CLOSED_SOURCE) 99 | add_subdirectory(deps/NatNetSDKCrossplatform EXCLUDE_FROM_ALL) 100 | set(my_include_directories 101 | ${my_include_directories} 102 | ${NATNET_SDK_DIR}/include 103 | ) 104 | set(my_files 105 | ${my_files} 106 | src/optitrack_closed_source.cpp 107 | ) 108 | set(my_link_directories 109 | ${my_link_directories} 110 | ${NATNET_SDK_DIR}/lib/ubuntu 111 | ) 112 | set(my_libraries 113 | ${my_libraries} 114 | NatNet 115 | ) 116 | else() 117 | message(WARNING "Can not include optitrack_closed_source due to missing SDK. (${CMAKE_SYSTEM_NAME}, ${CMAKE_SYSTEM_PROCESSOR})") 118 | endif() 119 | endif() 120 | 121 | if (LIBMOTIONCAPTURE_ENABLE_QUALISYS) 122 | message("including qualisys") 123 | 124 | add_definitions(-DENABLE_QUALISYS) 125 | add_subdirectory(deps/qualisys_cpp_sdk EXCLUDE_FROM_ALL) 126 | set(my_include_directories 127 | ${my_include_directories} 128 | ${QUALISYS_DIR} 129 | ) 130 | set(my_files 131 | ${my_files} 132 | src/qualisys.cpp 133 | ) 134 | set(my_libraries 135 | ${my_libraries} 136 | qualisys_cpp_sdk 137 | ) 138 | endif() 139 | 140 | if (LIBMOTIONCAPTURE_ENABLE_NOKOV) 141 | message("including nokov") 142 | 143 | EXECUTE_PROCESS( COMMAND uname -m COMMAND tr -d '\n' OUTPUT_VARIABLE ARCHITECTURE ) 144 | message( STATUS "Architecture: ${ARCHITECTURE}" ) 145 | 146 | add_definitions(-DENABLE_NOKOV) 147 | set(my_include_directories 148 | ${my_include_directories} 149 | ${NOKOV_SDK_DIR}/include 150 | ) 151 | set(my_files 152 | ${my_files} 153 | src/nokov.cpp 154 | ) 155 | 156 | if (${ARCHITECTURE} MATCHES "arm") 157 | set(my_link_directories ${my_link_directories} ${NOKOV_SDK_DIR}/lib/armhf) 158 | elseif (${ARCHITECTURE} MATCHES "aarch") 159 | set(my_link_directories ${my_link_directories} ${NOKOV_SDK_DIR}/lib/aarch64) 160 | else() 161 | set(my_link_directories ${my_link_directories} ${NOKOV_SDK_DIR}/lib/x64) 162 | endif() 163 | 164 | message(STATUS "Link to nokov library directory:${my_link_directories}") 165 | 166 | set(my_libraries 167 | ${my_libraries} 168 | nokov_sdk 169 | ) 170 | endif() 171 | 172 | if (LIBMOTIONCAPTURE_ENABLE_VRPN) 173 | message("including vrpn") 174 | 175 | add_definitions(-DENABLE_VRPN) 176 | option(VRPN_BUILD_CLIENT_LIBRARY "" ON) 177 | add_subdirectory(deps/vrpn EXCLUDE_FROM_ALL) 178 | set(my_include_directories 179 | ${my_include_directories} 180 | ${VRPN_DIR} 181 | ) 182 | set(my_files 183 | ${my_files} 184 | src/vrpn.cpp 185 | ) 186 | set(my_libraries 187 | ${my_libraries} 188 | ${VRPN_CLIENT_LIBRARY} 189 | ) 190 | endif() 191 | 192 | if (LIBMOTIONCAPTURE_ENABLE_FZMOTION) 193 | message("including fzmotion") 194 | 195 | add_definitions(-DENABLE_FZMOTION) 196 | set(my_files 197 | ${my_files} 198 | src/fzmotion.cpp 199 | ) 200 | endif() 201 | 202 | if (LIBMOTIONCAPTURE_ENABLE_MOTIONANALYSIS) 203 | message("including motionanalysis") 204 | 205 | add_definitions(-DENABLE_MOTIONANALYSIS) 206 | add_subdirectory(deps/cortex_sdk_linux EXCLUDE_FROM_ALL) 207 | set(my_include_directories 208 | ${my_include_directories} 209 | ${MOTIONANALYSIS_DIR} 210 | ) 211 | set(my_files 212 | ${my_files} 213 | src/motionanalysis.cpp 214 | ) 215 | set(my_libraries 216 | ${my_libraries} 217 | cortex_sdk pthread m 218 | ) 219 | endif() 220 | 221 | include_directories( 222 | ${my_include_directories} 223 | ) 224 | 225 | ## Declare a cpp library 226 | add_library(libmotioncapture 227 | ${my_files} 228 | ) 229 | 230 | ## Specify libraries to link a library or executable target against 231 | target_link_directories(libmotioncapture PUBLIC 232 | ${my_link_directories} 233 | ) 234 | target_link_libraries(libmotioncapture 235 | ${my_libraries} 236 | ) 237 | set_property(TARGET libmotioncapture PROPERTY POSITION_INDEPENDENT_CODE ON) 238 | 239 | if (LIBMOTIONCAPTURE_BUILD_PYTHON_BINDINGS) 240 | # Python bindings 241 | find_package (Python3 COMPONENTS Interpreter Development) 242 | 243 | add_subdirectory(deps/pybind11 EXCLUDE_FROM_ALL) 244 | # find_package(Python COMPONENTS Interpreter Development) 245 | # find_package(pybind11 CONFIG) 246 | 247 | pybind11_add_module(motioncapture 248 | src/python_bindings.cpp 249 | ) 250 | 251 | target_link_libraries(motioncapture 252 | PRIVATE 253 | libmotioncapture 254 | ) 255 | endif() 256 | 257 | if (LIBMOTIONCAPTURE_BUILD_EXAMPLE) 258 | add_executable(motioncapture_example 259 | examples/main.cpp 260 | ) 261 | target_link_libraries(motioncapture_example 262 | libmotioncapture 263 | ) 264 | 265 | endif() 266 | 267 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2016 USC-ACTLab 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /MANIFEST.in: -------------------------------------------------------------------------------- 1 | include README.md LICENSE version 2 | recursive-include deps * 3 | graft src 4 | graft examples 5 | graft include/libmotioncapture 6 | global-include CMakeLists.txt *.cmake -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![CI](https://github.com/IMRCLab/libmotioncapture/actions/workflows/CI.yml/badge.svg)](https://github.com/IMRCLab/libmotioncapture/actions/workflows/CI.yml) 2 | 3 | # libmotioncapture 4 | Interface Abstraction for Motion Capture System APIs such as VICON, OptiTrack, Qualisys, Nokov, FZMotion, or VRPN. 5 | 6 | This can be used as C++ library or Python package. For Python, use 7 | 8 | ``` 9 | pip install motioncapture 10 | ``` 11 | 12 | For C++, follow the instructions below. 13 | 14 | This is a fork of https://github.com/USC-ACTLab/libmotioncapture/ with the following changes: 15 | 16 | - Python bindings 17 | - Factory method 18 | - Refactored API 19 | - Support for VRPN by default 20 | 21 | ## Compile options 22 | By default, `libmotioncapture` supports the following hardware: 23 | 24 | - VICON - SDK git submodule 25 | - Qualisys - SDK git submodule 26 | - OptiTrack - binary parsing over network (no dependency) 27 | - VRPN - SDK git submodule 28 | - NOKOV - manually obtain SDK and copy to deps/nokov_sdk/ 29 | - FZMotion - no dependency 30 | - Motion Analysis - manually obtain SDK and copy to deps/cortex_sdk_linux/ 31 | 32 | CMake flags can be used to disable individual systems in `CMakeLists.txt`. 33 | 34 | ## Prerequisites 35 | 36 | ``` 37 | sudo apt install libboost-system-dev libboost-thread-dev libeigen3-dev ninja-build 38 | ``` 39 | 40 | ## C++ 41 | 42 | ``` 43 | git submodule init 44 | git submodule update 45 | mkdir build 46 | cd build 47 | cmake .. 48 | make 49 | ``` 50 | 51 | An example application is in `examples/main.cpp`. Run it using 52 | 53 | ``` 54 | ./motioncapture_example 55 | ``` 56 | 57 | ## Python (Development) 58 | 59 | ``` 60 | git submodule init 61 | git submodule update 62 | python3 setup.py develop --user 63 | python3 examples/python.py 64 | ``` 65 | -------------------------------------------------------------------------------- /examples/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | // Motion Capture 4 | #include 5 | 6 | int main(int argc, char **argv) 7 | { 8 | if (argc < 3) { 9 | std::cerr << "Use ./motioncapture_example [option1] [value1] ..." << std::endl; 10 | return 1; 11 | } 12 | 13 | // Make a new client 14 | std::map cfg; 15 | cfg["hostname"] = argv[2]; 16 | for (int i = 3; i+1 < argc; i+=2) { 17 | cfg[argv[i]] = argv[i+1]; 18 | } 19 | 20 | libmotioncapture::MotionCapture *mocap = libmotioncapture::MotionCapture::connect(argv[1], cfg); 21 | 22 | std::cout << "supportsRigidBodyTracking: " << mocap->supportsRigidBodyTracking() << std::endl; 23 | std::cout << "supportsLatencyEstimate: " << mocap->supportsLatencyEstimate() << std::endl; 24 | std::cout << "supportsPointCloud: " << mocap->supportsPointCloud() << std::endl; 25 | std::cout << "supportsTimeStamp: " << mocap->supportsTimeStamp() << std::endl; 26 | 27 | for (size_t frameId = 0;; ++frameId) 28 | { 29 | // Get a frame 30 | mocap->waitForNextFrame(); 31 | 32 | std::cout << "frame " << frameId << std::endl; 33 | if (mocap->supportsTimeStamp()) { 34 | std::cout << " timestamp: " << mocap->timeStamp() << " us" << std::endl; 35 | } 36 | if (mocap->supportsLatencyEstimate()) { 37 | std::cout << " latency: " << std::endl; 38 | for (const auto& latency : mocap->latency()) { 39 | std::cout << " " << latency.name() << " " << latency.value() << " s" << std::endl; 40 | } 41 | } 42 | 43 | if (mocap->supportsPointCloud()) { 44 | std::cout << " pointcloud:" << std::endl; 45 | auto pointcloud = mocap->pointCloud(); 46 | for (size_t i = 0; i < pointcloud.rows(); ++i) { 47 | const auto& point = pointcloud.row(i); 48 | std::cout << " \"" << i << "\": [" << point(0) << "," << point(1) << "," << point(2) << "]" << std::endl; 49 | } 50 | } 51 | 52 | if (mocap->supportsRigidBodyTracking()) { 53 | auto rigidBodies = mocap->rigidBodies(); 54 | 55 | std::cout << " rigid bodies:" << std::endl; 56 | 57 | for (auto const& item: rigidBodies) { 58 | const auto& rigidBody = item.second; 59 | 60 | std::cout << " \"" << rigidBody.name() << "\":" << std::endl; 61 | 62 | const auto& position = rigidBody.position(); 63 | const auto& rotation = rigidBody.rotation(); 64 | std::cout << " position: [" << position(0) << ", " << position(1) << ", " << position(2) << "]" << std::endl; 65 | std::cout << " rotation: [" << rotation.w() << ", " << rotation.vec()(0) << ", " 66 | << rotation.vec()(1) << ", " << rotation.vec()(2) << "]" << std::endl; 67 | } 68 | } 69 | } 70 | 71 | return 0; 72 | } 73 | -------------------------------------------------------------------------------- /examples/python.py: -------------------------------------------------------------------------------- 1 | import motioncapture 2 | import argparse 3 | 4 | if __name__ == "__main__": 5 | 6 | parser = argparse.ArgumentParser() 7 | parser.add_argument("type") 8 | parser.add_argument("hostname") 9 | args = parser.parse_args() 10 | 11 | mc = motioncapture.connect(args.type, {"hostname": args.hostname}) 12 | while True: 13 | mc.waitForNextFrame() 14 | for name, obj in mc.rigidBodies.items(): 15 | print(name, obj.position, obj.rotation.z) 16 | -------------------------------------------------------------------------------- /include/libmotioncapture/fzmotion.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "libmotioncapture/motioncapture.h" 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #define MAX_PACKET_SIZE 65535 13 | #define MAX_FRAME_SIZE 65535 14 | using namespace boost::asio::ip; 15 | using namespace boost::asio; 16 | using namespace boost; 17 | using namespace std; 18 | 19 | typedef unsigned char byte; 20 | typedef byte octet; 21 | 22 | typedef char char8; 23 | typedef wchar_t char16; 24 | 25 | typedef char int8; 26 | typedef short int16; 27 | typedef int int32; 28 | typedef long int64; 29 | 30 | typedef unsigned char uint8; 31 | typedef unsigned short uint16; 32 | typedef unsigned int uint32; 33 | typedef unsigned long uint64; 34 | 35 | typedef float real32; 36 | typedef double real64; 37 | 38 | typedef uint8 uuid[16]; 39 | 40 | namespace libmotioncapture { 41 | //get an empty byte buffer of specified size 42 | inline byte* GetEmptyBuffer(const uint32 uSize) { 43 | byte* pBuffer = new byte[uSize]; 44 | memset(pBuffer, 0, uSize); 45 | return pBuffer; 46 | } 47 | 48 | //clear the specified buffer 49 | inline void EmptyBuffer(byte* const pBuffer, const uint32 uSize) { 50 | memset(pBuffer, 0, uSize); 51 | } 52 | 53 | //copy data from one buffer to the other buffer 54 | inline void CopyBuffer(byte* const pDst, const byte* const pSrc, const uint32 uSize) { 55 | memcpy(pDst, pSrc, uSize); 56 | } 57 | 58 | //release allocated buffer 59 | inline void ReleaseBuffer(byte* const pBuffer) { 60 | delete[] pBuffer; 61 | } 62 | // Marker 63 | typedef struct 64 | { 65 | uint32 ID; // Marker ID: 66 | struct { 67 | real32 x; 68 | real32 y; 69 | real32 z; 70 | }sPosition; 71 | }LMarker; 72 | 73 | // Rigidbody Data 74 | typedef struct 75 | { 76 | uint32 ID; 77 | struct { 78 | real32 x; 79 | real32 y; 80 | real32 z; 81 | }sPosition; //Position 82 | struct { 83 | real32 qx; 84 | real32 qy; 85 | real32 qz; 86 | real32 qw; 87 | }sOrientation; // Orientation 88 | uint32 uTrack; // tracking flags 89 | }LRigidBody; 90 | 91 | // Rigidbody Tag 92 | typedef struct { 93 | char8 szName[256]; 94 | uint32 uRigidbodyID; 95 | struct { 96 | real32 x; 97 | real32 y; 98 | real32 z; 99 | }sCenteroidTransform; 100 | }LRigidbodyTag; 101 | 102 | // Message 103 | typedef enum { 104 | Connect, //connection request 105 | Disconnect, //disconnection request 106 | 107 | Connected, //connected status 108 | Disconnected, //dosconnected status 109 | 110 | RequestTagList, //request model definition 111 | RequestData, //request motion capture data 112 | 113 | TagListData, //model definitino 114 | MotionCaptureData, //motion capture data 115 | 116 | Ready, //ready status 117 | Busy //busy status 118 | }Message; 119 | 120 | //constexpr uuid id = { 219, 49, 232, 58, 66, 199, 72, 92, 167, 100, 237, 202, 8, 78, 168, 29 }; //FZMotion UUID:DB31E83A-42C7-485C-A764-EDCA084EA81D 121 | 122 | typedef struct { 123 | Message iMessage; 124 | }SimpleMessage; 125 | 126 | typedef struct { 127 | //uuid uid; //uuid - universally unique identifier - 16 bytes 128 | Message eMessage; //status of data sending end 129 | uint16 uDataBytes; //bytes of transmitted data - 2 bytes 130 | char8 szSoftware[256]; //name of sent software - 16 bytes 131 | 132 | union { 133 | struct { uint8 v1; uint8 v2; uint8 v3; uint8 v4; }; 134 | uint8 version[4]; 135 | }uVersion; //software version of sending end - 4 bytes 136 | 137 | union { 138 | struct { uint8 v1; uint8 v2; uint8 v3; uint8 v4; }; 139 | uint8 version[4]; 140 | }uSdkVersion; //network module version - 4 bytes 141 | 142 | uint16 uDataPort; //data transmission port - 2 bytes 143 | 144 | union { 145 | struct { octet h1; octet h2; octet l1; octet l2; }; 146 | octet ipv4[4]; 147 | }uMulticastGroup; //ip address of multicast group - 4 bytes 148 | //uint8 uOptions; //options for the data transmission - 1 byte 149 | }SimpleConfirmMessage; 150 | 151 | class MotionCaptureFZMotion : public MotionCapture { 152 | private: 153 | MotionCaptureFZMotion() = delete; 154 | MotionCaptureFZMotion(const MotionCaptureFZMotion& mcl) = delete; 155 | MotionCaptureFZMotion& operator=(const MotionCaptureFZMotion& mcl) = delete; 156 | 157 | MotionCaptureFZMotion(const string& strLocalIP, 158 | const int iLocalPort, const string& strRemoteIP, const int iRemotePort); 159 | 160 | static MotionCaptureFZMotion* s_pInstance; 161 | static recursive_mutex s_mutex; 162 | 163 | boost::asio::io_context m_IOContext; 164 | udp::socket m_TransmissionSocket; 165 | udp::socket m_ConnectionSocket; 166 | udp::resolver m_Resolver; 167 | 168 | udp::endpoint m_localCEndpoint; //local connection endpoint 169 | udp::endpoint m_remoteCEndpoint; //remote connection endpoint 170 | udp::endpoint m_localMEndpoint; //local multicast endpoint - data transmisson endpoint 171 | udp::endpoint m_remoteMEndpoint; //remote multicast endpoint - data transmission endpoint 172 | 173 | int32 m_iLocalCPort; 174 | int32 m_iRemoteCPort; 175 | int32 m_iDataReceivePort; 176 | 177 | mutable int32 m_uPreviousFrame; 178 | int32 m_uFrameNumber; 179 | 180 | uint32 m_uPagkageSize; 181 | 182 | atomic m_bIsConnected; 183 | atomic m_bFirstFrame; 184 | 185 | string m_strLocalIP; 186 | string m_strRemoteIP; 187 | 188 | string m_strSoftware; 189 | string m_strSDKVersion; 190 | string m_strSoftwareVersion; 191 | string m_strMulticastGroup; 192 | 193 | vector m_vctMarkData; 194 | vector m_vctRigidbodyData; 195 | mutable map m_mapRigidbodyTagList; 196 | 197 | //initailzie the instance 198 | void init(); 199 | 200 | //parse message received form the server 201 | void parseMessage(const SimpleConfirmMessage& scm); 202 | 203 | //parse rigidbody tag list 204 | void parseRigidbodyTagList(const byte* const pData, map& mapTagList); 205 | 206 | //parse marker and rigibody data 207 | void parseData(const byte* const pData, vector& allMarkers, vector& allRigidBodys); 208 | 209 | //receive and parse each frame data 210 | void receiveFrameData(); 211 | 212 | //set the connection flag 213 | inline void setConnected(const bool bIsConnected) { this->m_bIsConnected = bIsConnected; } 214 | 215 | //set the first frame flag 216 | inline void setFirstFrame(const bool bFirstFrame) { this->m_bFirstFrame = bFirstFrame; } 217 | protected: 218 | public: 219 | //get current unque instance 220 | inline static MotionCaptureFZMotion* getInstance() { 221 | return s_pInstance; 222 | } 223 | 224 | //get unique instance with specified parameter 225 | static MotionCaptureFZMotion* getInstance( 226 | const string& strLocalIP, const int iLocalPort, const string& strRemoteIP, const int iRemotePort) { 227 | s_mutex.lock(); 228 | 229 | if (s_pInstance == nullptr) { 230 | s_pInstance = new MotionCaptureFZMotion(strLocalIP, iLocalPort, strRemoteIP, iRemotePort); 231 | } 232 | else { 233 | s_pInstance->disconnect(); 234 | s_pInstance->setConnectionInfo(strLocalIP, iLocalPort, strRemoteIP, iRemotePort); 235 | s_pInstance->connect(); 236 | } 237 | 238 | s_mutex.unlock(); 239 | return s_pInstance; 240 | } 241 | 242 | virtual ~MotionCaptureFZMotion() { 243 | s_mutex.lock(); 244 | 245 | this->disconnect(); 246 | 247 | if (s_pInstance != nullptr) 248 | delete(s_pInstance); 249 | 250 | s_pInstance = nullptr; 251 | 252 | s_mutex.unlock(); 253 | } 254 | 255 | //set both local and remote host ip and port 256 | void setConnectionInfo(const string& strLocalIP, const int iLocalPort, const string& strRemoteIP, const int iRemotePort); 257 | 258 | //connect with the server 259 | bool connect(); 260 | 261 | //disconnect with the server and clean all data 262 | void disconnect(); 263 | 264 | inline bool isConnected() const { return this->m_bIsConnected; } 265 | 266 | //overload virtual functions 267 | void waitForNextFrame(); 268 | const std::map& rigidBodies() const; 269 | const PointCloud& pointCloud() const; 270 | inline bool supportsRigidBodyTracking() const { return true; } 271 | inline bool supportsPointCloud() const { return true; } 272 | }; 273 | } 274 | -------------------------------------------------------------------------------- /include/libmotioncapture/mock.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "libmotioncapture/motioncapture.h" 3 | 4 | namespace libmotioncapture { 5 | class MotionCaptureMockImpl; 6 | 7 | class MotionCaptureMock : public MotionCapture{ 8 | public: 9 | MotionCaptureMock( 10 | float dt, 11 | const std::vector& objects, 12 | const PointCloud& pointCloud); 13 | 14 | virtual ~MotionCaptureMock(); 15 | 16 | // implementations for MotionCapture interface 17 | virtual void waitForNextFrame(); 18 | 19 | const std::map& rigidBodies() const; 20 | 21 | const PointCloud& pointCloud() const; 22 | 23 | virtual bool supportsRigidBodyTracking() const 24 | { 25 | return true; 26 | } 27 | 28 | virtual bool supportsPointCloud() const 29 | { 30 | return true; 31 | } 32 | 33 | private: 34 | MotionCaptureMockImpl * pImpl; 35 | }; 36 | } 37 | 38 | -------------------------------------------------------------------------------- /include/libmotioncapture/motionanalysis.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "libmotioncapture/motioncapture.h" 4 | 5 | namespace libmotioncapture { 6 | 7 | class MotionCaptureMotionAnalysisImpl; 8 | 9 | class MotionCaptureMotionAnalysis : public MotionCapture { 10 | public: 11 | MotionCaptureMotionAnalysis( 12 | const std::string &hostname, 13 | int updateFrequency = 100); 14 | 15 | virtual ~MotionCaptureMotionAnalysis(); 16 | 17 | const std::string &version() const; 18 | 19 | virtual void waitForNextFrame() override; 20 | 21 | virtual const std::map &rigidBodies() const override; 22 | 23 | virtual RigidBody rigidBodyByName(const std::string &name) const override; 24 | 25 | virtual bool supportsRigidBodyTracking() const override; 26 | 27 | private: 28 | MotionCaptureMotionAnalysisImpl *pImpl; 29 | }; 30 | 31 | } // namespace libmotioncapture -------------------------------------------------------------------------------- /include/libmotioncapture/motioncapture.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | // Eigen 9 | #include 10 | 11 | namespace libmotioncapture { 12 | 13 | typedef Eigen::Matrix PointCloud; 14 | 15 | const char* version(); 16 | 17 | class RigidBody 18 | { 19 | public: 20 | RigidBody( 21 | const std::string& name, 22 | const Eigen::Vector3f& position, 23 | Eigen::Quaternionf& rotation) 24 | : m_name(name) 25 | , m_position(position) 26 | , m_rotation(rotation) 27 | { 28 | } 29 | 30 | const std::string& name() const { 31 | return m_name; 32 | } 33 | 34 | const Eigen::Vector3f& position() const { 35 | return m_position; 36 | } 37 | 38 | const Eigen::Quaternionf& rotation() const { 39 | return m_rotation; 40 | } 41 | 42 | private: 43 | std::string m_name; 44 | Eigen::Vector3f m_position; 45 | Eigen::Quaternionf m_rotation; 46 | }; 47 | 48 | class LatencyInfo 49 | { 50 | public: 51 | LatencyInfo( 52 | const std::string& name, 53 | double value) 54 | : m_name(name) 55 | , m_value(value) 56 | { 57 | } 58 | 59 | const std::string& name() const { 60 | return m_name; 61 | } 62 | 63 | // seconds 64 | double value() const { 65 | return m_value; 66 | } 67 | private: 68 | std::string m_name; 69 | double m_value; 70 | }; 71 | 72 | class MotionCapture 73 | { 74 | public: 75 | static MotionCapture *connect( 76 | const std::string &type, 77 | const std::map &cfg); 78 | 79 | virtual ~MotionCapture() 80 | { 81 | } 82 | 83 | // waits until a new frame is available 84 | virtual void waitForNextFrame() = 0; 85 | 86 | // Query data 87 | 88 | // returns reference to rigid bodies available in the current frame 89 | virtual const std::map& rigidBodies() const 90 | { 91 | rigidBodies_.clear(); 92 | return rigidBodies_; 93 | } 94 | 95 | // returns copy of rigid body with a specified name 96 | virtual RigidBody rigidBodyByName( 97 | const std::string& name) const; 98 | 99 | // returns pointer to point cloud (all unlabled markers) 100 | virtual const PointCloud& pointCloud() const 101 | { 102 | pointcloud_.resize(0, Eigen::NoChange); 103 | return pointcloud_; 104 | } 105 | 106 | // return latency information 107 | virtual const std::vector& latency() const 108 | { 109 | latencies_.clear(); 110 | return latencies_; 111 | } 112 | 113 | // returns timestamp in microseconds 114 | virtual uint64_t timeStamp() const 115 | { 116 | return 0; 117 | } 118 | 119 | // Query API capabilities 120 | 121 | // return true, if tracking of objects is supported 122 | virtual bool supportsRigidBodyTracking() const 123 | { 124 | return false; 125 | } 126 | // returns true, if latency can be estimated 127 | virtual bool supportsLatencyEstimate() const 128 | { 129 | return false; 130 | } 131 | // returns true if raw point cloud is available 132 | virtual bool supportsPointCloud() const 133 | { 134 | return false; 135 | } 136 | // returns true if timestamp is available 137 | virtual bool supportsTimeStamp() const 138 | { 139 | return false; 140 | } 141 | 142 | protected: 143 | mutable std::map rigidBodies_; 144 | mutable PointCloud pointcloud_; 145 | mutable std::vector latencies_; 146 | mutable uint64_t timestamp_; 147 | }; 148 | 149 | } // namespace libobjecttracker 150 | 151 | 152 | -------------------------------------------------------------------------------- /include/libmotioncapture/nokov.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "libmotioncapture/motioncapture.h" 4 | 5 | namespace libmotioncapture { 6 | 7 | class MotionCaptureNokovImpl; 8 | 9 | class MotionCaptureNokov: public MotionCapture 10 | { 11 | public: 12 | MotionCaptureNokov( 13 | const std::string& hostname, 14 | bool enableFrequency = false, 15 | int updateFrequency = 100); 16 | 17 | virtual ~MotionCaptureNokov(); 18 | 19 | const std::string& version() const; 20 | 21 | // implementations for MotionCapture interface 22 | virtual void waitForNextFrame(); 23 | virtual const std::map& rigidBodies() const override; 24 | virtual RigidBody rigidBodyByName(const std::string& name) const override; 25 | virtual const PointCloud& pointCloud() const override; 26 | virtual bool supportsRigidBodyTracking() const override; 27 | virtual bool supportsPointCloud() const; 28 | 29 | private: 30 | MotionCaptureNokovImpl* pImpl; 31 | }; 32 | 33 | } // namespace libobjecttracker 34 | 35 | 36 | -------------------------------------------------------------------------------- /include/libmotioncapture/optitrack.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "libmotioncapture/motioncapture.h" 3 | 4 | namespace libmotioncapture { 5 | class MotionCaptureOptitrackImpl; 6 | 7 | class MotionCaptureOptitrack : public MotionCapture{ 8 | public: 9 | MotionCaptureOptitrack( 10 | const std::string &hostname, 11 | const std::string& interface_ip = "0.0.0.0", 12 | int port_command = 1510); 13 | 14 | virtual ~MotionCaptureOptitrack(); 15 | 16 | const std::string& version() const; 17 | 18 | // implementations for MotionCapture interface 19 | virtual void waitForNextFrame(); 20 | virtual const std::map& rigidBodies() const; 21 | virtual const PointCloud& pointCloud() const; 22 | virtual const std::vector &latency() const; 23 | virtual uint64_t timeStamp() const; 24 | 25 | virtual bool supportsRigidBodyTracking() const 26 | { 27 | return true; 28 | } 29 | virtual bool supportsPointCloud() const 30 | { 31 | return true; 32 | } 33 | 34 | virtual bool supportsLatencyEstimate() const 35 | { 36 | return true; 37 | } 38 | 39 | virtual bool supportsTimeStamp() const 40 | { 41 | return true; 42 | } 43 | 44 | private: 45 | MotionCaptureOptitrackImpl * pImpl; 46 | }; 47 | } 48 | 49 | -------------------------------------------------------------------------------- /include/libmotioncapture/optitrack_closed_source.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "libmotioncapture/motioncapture.h" 3 | 4 | namespace libmotioncapture { 5 | class MotionCaptureOptitrackClosedSourceImpl; 6 | 7 | class MotionCaptureOptitrackClosedSource : public MotionCapture{ 8 | public: 9 | MotionCaptureOptitrackClosedSource( 10 | const std::string &hostname, 11 | int port_command = 1510); 12 | 13 | virtual ~MotionCaptureOptitrackClosedSource(); 14 | 15 | const std::string& version() const; 16 | 17 | // implementations for MotionCapture interface 18 | virtual void waitForNextFrame(); 19 | virtual const std::map& rigidBodies() const; 20 | virtual const PointCloud& pointCloud() const; 21 | virtual const std::vector &latency() const; 22 | virtual uint64_t timeStamp() const; 23 | 24 | virtual bool supportsRigidBodyTracking() const 25 | { 26 | return true; 27 | } 28 | virtual bool supportsPointCloud() const 29 | { 30 | return true; 31 | } 32 | 33 | virtual bool supportsLatencyEstimate() const 34 | { 35 | return true; 36 | } 37 | 38 | virtual bool supportsTimeStamp() const 39 | { 40 | return true; 41 | } 42 | 43 | private: 44 | MotionCaptureOptitrackClosedSourceImpl * pImpl; 45 | }; 46 | } 47 | 48 | -------------------------------------------------------------------------------- /include/libmotioncapture/qualisys.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "libmotioncapture/motioncapture.h" 3 | 4 | namespace libmotioncapture { 5 | 6 | class MotionCaptureQualisysImpl; 7 | 8 | class MotionCaptureQualisys 9 | : public MotionCapture 10 | { 11 | public: 12 | MotionCaptureQualisys( 13 | const std::string& hostname, 14 | int basePort, 15 | bool enableObjects, 16 | bool enablePointcloud); 17 | 18 | virtual ~MotionCaptureQualisys(); 19 | 20 | const std::string& version() const; 21 | 22 | // implementations for MotionCapture interface 23 | virtual void waitForNextFrame(); 24 | virtual const std::map& rigidBodies() const; 25 | virtual RigidBody rigidBodyByName(const std::string &name) const; 26 | virtual const PointCloud& pointCloud() const; 27 | virtual uint64_t timeStamp() const; 28 | 29 | virtual bool supportsRigidBodyTracking() const 30 | { 31 | return true; 32 | } 33 | 34 | virtual bool supportsPointCloud() const 35 | { 36 | return true; 37 | } 38 | 39 | virtual bool supportsTimeStamp() const 40 | { 41 | return true; 42 | } 43 | 44 | private: 45 | MotionCaptureQualisysImpl* pImpl; 46 | }; 47 | 48 | } // namespace libobjecttracker 49 | 50 | 51 | -------------------------------------------------------------------------------- /include/libmotioncapture/vicon.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "libmotioncapture/motioncapture.h" 3 | 4 | // GetSegmentGlobalTranslation 5 | // GetSegmentGlobalRotationQuaternion 6 | // Connect 7 | // IsConnected 8 | // EnableSegmentData 9 | // EnableUnlabeledMarkerData 10 | // EnableMarkerData 11 | // GetVersion 12 | // GetFrame 13 | // GetLatencyTotal, 14 | // GetUnlabeledMarkerCount 15 | // GetUnlabeledMarkerGlobalTranslation 16 | 17 | namespace libmotioncapture { 18 | 19 | class MotionCaptureViconImpl; 20 | 21 | class MotionCaptureVicon 22 | : public MotionCapture 23 | { 24 | public: 25 | MotionCaptureVicon( 26 | const std::string& hostname, 27 | bool enableObjects, 28 | bool enablePointcloud); 29 | 30 | virtual ~MotionCaptureVicon(); 31 | 32 | const std::string& version() const; 33 | 34 | // implementations for MotionCapture interface 35 | virtual void waitForNextFrame(); 36 | virtual const std::map& rigidBodies() const; 37 | virtual RigidBody rigidBodyByName(const std::string& name) const; 38 | virtual const PointCloud& pointCloud() const; 39 | virtual const std::vector& latency() const; 40 | 41 | virtual bool supportsRigidBodyTracking() const 42 | { 43 | return true; 44 | } 45 | 46 | virtual bool supportsLatencyEstimate() const 47 | { 48 | return true; 49 | } 50 | 51 | virtual bool supportsPointCloud() const 52 | { 53 | return true; 54 | } 55 | 56 | private: 57 | MotionCaptureViconImpl* pImpl; 58 | }; 59 | 60 | } // namespace libobjecttracker 61 | 62 | 63 | -------------------------------------------------------------------------------- /include/libmotioncapture/vrpn.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "libmotioncapture/motioncapture.h" 3 | 4 | namespace libmotioncapture { 5 | 6 | class MotionCaptureVrpnImpl; 7 | 8 | class MotionCaptureVrpn 9 | : public MotionCapture 10 | { 11 | public: 12 | MotionCaptureVrpn( 13 | const std::string& hostname, 14 | int updateFrequency = 100); 15 | 16 | virtual ~MotionCaptureVrpn(); 17 | 18 | // implementations for MotionCapture interface 19 | virtual void waitForNextFrame(); 20 | virtual const std::map &rigidBodies() const; 21 | virtual RigidBody rigidBodyByName(const std::string &name) const; 22 | 23 | virtual bool supportsRigidBodyTracking() const 24 | { 25 | return true; 26 | } 27 | 28 | private: 29 | MotionCaptureVrpnImpl* pImpl; 30 | }; 31 | 32 | } // namespace libobjecttracker 33 | 34 | 35 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = [ 3 | "setuptools>=42", 4 | "wheel", 5 | "ninja; sys_platform != 'win32'", 6 | "cmake>=3.12", 7 | ] 8 | build-backend = "setuptools.build_meta" 9 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | import os 3 | import sys 4 | import subprocess 5 | 6 | from setuptools import setup, Extension 7 | from setuptools.command.build_ext import build_ext 8 | 9 | # Convert distutils Windows platform specifiers to CMake -A arguments 10 | PLAT_TO_CMAKE = { 11 | "win32": "Win32", 12 | "win-amd64": "x64", 13 | "win-arm32": "ARM", 14 | "win-arm64": "ARM64", 15 | } 16 | 17 | 18 | # A CMakeExtension needs a sourcedir instead of a file list. 19 | # The name must be the _single_ output extension from the CMake build. 20 | # If you need multiple extensions, see scikit-build. 21 | class CMakeExtension(Extension): 22 | def __init__(self, name, sourcedir=""): 23 | Extension.__init__(self, name, sources=[]) 24 | self.sourcedir = os.path.abspath(sourcedir) 25 | 26 | 27 | class CMakeBuild(build_ext): 28 | def build_extension(self, ext): 29 | extdir = os.path.abspath(os.path.dirname(self.get_ext_fullpath(ext.name))) 30 | 31 | # required for auto-detection of auxiliary "native" libs 32 | if not extdir.endswith(os.path.sep): 33 | extdir += os.path.sep 34 | 35 | cfg = "Debug" if self.debug else "Release" 36 | 37 | # CMake lets you override the generator - we need to check this. 38 | # Can be set with Conda-Build, for example. 39 | cmake_generator = os.environ.get("CMAKE_GENERATOR", "") 40 | 41 | # Set Python_EXECUTABLE instead if you use PYBIND11_FINDPYTHON 42 | cmake_args = [ 43 | "-DCMAKE_LIBRARY_OUTPUT_DIRECTORY={}".format(extdir), 44 | "-DPYTHON_EXECUTABLE={}".format(sys.executable), 45 | "-DCMAKE_BUILD_TYPE={}".format(cfg), # not used on MSVC, but no harm 46 | "-DCMAKE_POLICY_VERSION_MINIMUM=3.5" # to make sure vrpn compiles 47 | ] 48 | build_args = [] 49 | 50 | if self.compiler.compiler_type != "msvc": 51 | # Using Ninja-build since it a) is available as a wheel and b) 52 | # multithreads automatically. MSVC would require all variables be 53 | # exported for Ninja to pick it up, which is a little tricky to do. 54 | # Users can override the generator with CMAKE_GENERATOR in CMake 55 | # 3.15+. 56 | if not cmake_generator: 57 | cmake_args += ["-GNinja"] 58 | 59 | else: 60 | 61 | # Single config generators are handled "normally" 62 | single_config = any(x in cmake_generator for x in {"NMake", "Ninja"}) 63 | 64 | # CMake allows an arch-in-generator style for backward compatibility 65 | contains_arch = any(x in cmake_generator for x in {"ARM", "Win64"}) 66 | 67 | # Specify the arch if using MSVC generator, but only if it doesn't 68 | # contain a backward-compatibility arch spec already in the 69 | # generator name. 70 | if not single_config and not contains_arch: 71 | cmake_args += ["-A", PLAT_TO_CMAKE[self.plat_name]] 72 | 73 | # Multi-config generators have a different way to specify configs 74 | if not single_config: 75 | cmake_args += [ 76 | "-DCMAKE_LIBRARY_OUTPUT_DIRECTORY_{}={}".format(cfg.upper(), extdir) 77 | ] 78 | build_args += ["--config", cfg] 79 | 80 | # Set CMAKE_BUILD_PARALLEL_LEVEL to control the parallel build level 81 | # across all generators. 82 | if "CMAKE_BUILD_PARALLEL_LEVEL" not in os.environ: 83 | # self.parallel is a Python 3 only way to set parallel jobs by hand 84 | # using -j in the build_ext call, not supported by pip or PyPA-build. 85 | if hasattr(self, "parallel") and self.parallel: 86 | # CMake 3.12+ only. 87 | build_args += ["-j{}".format(self.parallel)] 88 | 89 | if not os.path.exists(self.build_temp): 90 | os.makedirs(self.build_temp) 91 | 92 | subprocess.check_call( 93 | ["cmake", ext.sourcedir] + cmake_args, cwd=self.build_temp 94 | ) 95 | subprocess.check_call( 96 | ["cmake", "--build", "."] + build_args, cwd=self.build_temp 97 | ) 98 | 99 | 100 | with open('version', 'r') as file: 101 | version = file.read().replace('\"', '') 102 | 103 | # The information here can also be placed in setup.cfg - better separation of 104 | # logic and declaration, and simpler if you include description/version in a file. 105 | setup( 106 | name="motioncapture", 107 | version=version, 108 | description="Interface for common motion capture systems", 109 | long_description="Interface for common motion capture systems", 110 | ext_modules=[CMakeExtension("motioncapture")], 111 | cmdclass={"build_ext": CMakeBuild}, 112 | zip_safe=False, 113 | ) 114 | -------------------------------------------------------------------------------- /src/fzmotion.cpp: -------------------------------------------------------------------------------- 1 | #include "libmotioncapture/fzmotion.h" 2 | namespace libmotioncapture { 3 | recursive_mutex MotionCaptureFZMotion::s_mutex; 4 | MotionCaptureFZMotion* MotionCaptureFZMotion::s_pInstance = nullptr; 5 | MotionCaptureFZMotion::MotionCaptureFZMotion( 6 | const string& strLocalIP, 7 | const int iLocalPort, 8 | const string& strRemoteIP, 9 | const int iRemotePort 10 | ) : m_ConnectionSocket(m_IOContext), 11 | m_TransmissionSocket(m_IOContext), 12 | m_Resolver(m_IOContext){ 13 | this->init(); 14 | this->setConnectionInfo(strLocalIP, iLocalPort, strRemoteIP, iRemotePort); 15 | this->connect(); 16 | } 17 | //initailzie the instance 18 | void MotionCaptureFZMotion::init() { 19 | this->m_mapRigidbodyTagList.clear(); 20 | this->m_vctMarkData.clear(); 21 | this->m_vctRigidbodyData.clear(); 22 | 23 | this->setConnected(false); 24 | this->setFirstFrame(true); 25 | 26 | this->m_strLocalIP = ""; 27 | this->m_strRemoteIP = ""; 28 | 29 | this->m_strSoftware = ""; 30 | this->m_strSDKVersion = ""; 31 | this->m_strSoftwareVersion = ""; 32 | this->m_strMulticastGroup = ""; 33 | 34 | this->m_iLocalCPort = 0; 35 | this->m_iRemoteCPort = 0; 36 | this->m_iDataReceivePort = 0; 37 | this->m_uPagkageSize = 0; 38 | this->m_uPreviousFrame = -1; 39 | this->m_uFrameNumber = 0; 40 | 41 | this->m_localCEndpoint = udp::endpoint(); //local connection endpoint 42 | this->m_remoteCEndpoint = udp::endpoint(); //remote connection endpoint 43 | this->m_localMEndpoint = udp::endpoint(); //local multicast endpoint - data transmisson endpoint 44 | this->m_remoteMEndpoint = udp::endpoint(); //remote multicast endpoint - data transmisson endpoint 45 | } 46 | //set both local and remote host ip and port 47 | void MotionCaptureFZMotion::setConnectionInfo(const string& strLocalIP, const int iLocalPort, const string& strRemoteIP, const int iRemotePort) { 48 | s_mutex.lock(); 49 | 50 | this->disconnect(); 51 | 52 | this->m_strLocalIP = strLocalIP; 53 | this->m_iLocalCPort = iLocalPort; 54 | this->m_strRemoteIP = strRemoteIP; 55 | this->m_iRemoteCPort = iRemotePort; 56 | 57 | this->m_localCEndpoint = udp::endpoint(make_address_v4(this->m_strLocalIP), this->m_iLocalCPort); 58 | this->m_remoteCEndpoint = udp::endpoint(make_address(this->m_strRemoteIP), this->m_iRemoteCPort); 59 | 60 | s_mutex.unlock(); 61 | } 62 | //connect with the server 63 | bool MotionCaptureFZMotion::connect() { 64 | s_mutex.lock(); 65 | 66 | if (this->m_ConnectionSocket.is_open() == false) { 67 | this->m_ConnectionSocket.open(udp::v4()); 68 | this->m_ConnectionSocket.set_option(udp::socket::reuse_address(true)); 69 | } 70 | 71 | SimpleConfirmMessage scm; 72 | udp::endpoint remoteEndpoint; 73 | boost::system::error_code ec; 74 | SimpleMessage sm = { Message::Connect }; 75 | 76 | cout << "Connecting..." << endl; 77 | 78 | //create connection request thread 79 | bool bReceivedConfirmMessage = false; 80 | thread requestConnection([&]{ 81 | do{ 82 | //connect to data sending end 83 | if(this->m_ConnectionSocket.send_to(boost::asio::buffer(&sm, sizeof(sm)), this->m_remoteCEndpoint, 0, ec) <= 0){ 84 | cout << "Sending connection request failed. Error code: " << ec.value() << endl; 85 | continue; 86 | } 87 | cout << "Waiting for response..." << endl; 88 | #ifdef _WIN32 89 | Sleep(1000); 90 | #else 91 | sleep(1); 92 | #endif 93 | }while(bReceivedConfirmMessage == false); 94 | }); 95 | 96 | while(true){ 97 | //receive confirmation data from the data sending end 98 | if (this->m_ConnectionSocket.receive_from(boost::asio::buffer(&scm, sizeof(scm)), remoteEndpoint, 0, ec) <= 0) { 99 | cout << "Failed to receive confirm message. Error code: " << ec.value() << endl; 100 | continue; 101 | } 102 | 103 | if (scm.eMessage != Message::Ready) { 104 | cout << "Data sending host is not ready or receved package is not confirm message." << endl; 105 | continue; 106 | } 107 | bReceivedConfirmMessage = true; 108 | requestConnection.detach(); 109 | break; 110 | } 111 | 112 | this->m_remoteCEndpoint = remoteEndpoint; 113 | 114 | //parse received data 115 | parseMessage(scm); 116 | 117 | //create tag list request thread 118 | bool bTagListReceived = false; 119 | sm = { Message::RequestTagList }; 120 | byte* pBuffer = GetEmptyBuffer(MAX_PACKET_SIZE); 121 | 122 | thread requestTagList([&]{ 123 | do{ 124 | //request rigidbody tag list 125 | if(this->m_ConnectionSocket.send_to(boost::asio::buffer(&sm, sizeof(sm)), this->m_remoteCEndpoint, 0, ec) <= 0){ 126 | cout << "Failed to send tag list request. Error code: " << ec.value() << endl; 127 | continue; 128 | }; 129 | cout << "Waiting for response..." << endl; 130 | #ifdef _WIN32 131 | Sleep(1000); 132 | #else 133 | sleep(1); 134 | #endif 135 | }while(bTagListReceived == false); 136 | }); 137 | 138 | while(true){ 139 | //receive rigidbody tag list 140 | if (this->m_ConnectionSocket.receive_from(boost::asio::buffer(pBuffer, MAX_PACKET_SIZE), this->m_remoteCEndpoint, 0, ec) <= 0) { 141 | cout << "Failed to receive tag list package. Error code: " << ec.value() << endl; 142 | continue; 143 | } 144 | 145 | Message eMessage; 146 | CopyBuffer((byte*)&eMessage, pBuffer, sizeof(eMessage)); 147 | 148 | if (eMessage != Message::TagListData) { 149 | cout << "Receved package is not tag list." << endl; 150 | continue; 151 | } 152 | //parse received data 153 | parseRigidbodyTagList(pBuffer, this->m_mapRigidbodyTagList); 154 | bTagListReceived = true; 155 | requestTagList.detach(); 156 | EmptyBuffer(pBuffer, MAX_PACKET_SIZE); 157 | break; 158 | }; 159 | 160 | ReleaseBuffer(pBuffer); 161 | 162 | //joint multicast group 163 | this->m_localMEndpoint = udp::endpoint(make_address_v4(this->m_strMulticastGroup), this->m_iDataReceivePort); 164 | if (this->m_TransmissionSocket.is_open() == false) { 165 | this->m_TransmissionSocket.open(this->m_localMEndpoint.protocol()); 166 | this->m_TransmissionSocket.set_option(udp::socket::reuse_address(true)); 167 | this->m_TransmissionSocket.set_option(ip::multicast::hops(5)); 168 | this->m_TransmissionSocket.set_option(ip::multicast::enable_loopback(true)); 169 | this->m_TransmissionSocket.set_option(ip::multicast::join_group(make_address_v4(this->m_strMulticastGroup), make_address_v4(this->m_strLocalIP))); 170 | this->m_TransmissionSocket.bind(this->m_localMEndpoint); 171 | } 172 | 173 | this->setConnected(true); 174 | cout << "Connected successfully." << endl; 175 | s_mutex.unlock(); 176 | return true; 177 | } 178 | //disconnect with the server and clean all data 179 | void MotionCaptureFZMotion::disconnect() { 180 | s_mutex.lock(); 181 | 182 | this->setConnected(false); 183 | this->setFirstFrame(true); 184 | 185 | if (this->m_ConnectionSocket.is_open() == true) { 186 | this->m_ConnectionSocket.close(); 187 | } 188 | 189 | if (this->m_TransmissionSocket.is_open() == true) { 190 | this->m_TransmissionSocket.set_option(ip::multicast::leave_group(make_address_v4(this->m_strMulticastGroup), make_address_v4(this->m_strLocalIP))); 191 | this->m_TransmissionSocket.close(); 192 | } 193 | 194 | this->m_mapRigidbodyTagList.clear(); 195 | this->m_vctMarkData.clear(); 196 | this->m_vctRigidbodyData.clear(); 197 | 198 | this->m_strLocalIP = ""; 199 | this->m_strRemoteIP = ""; 200 | 201 | this->m_strSoftware = ""; 202 | this->m_strSDKVersion = ""; 203 | this->m_strSoftwareVersion = ""; 204 | this->m_strMulticastGroup = ""; 205 | 206 | this->m_iLocalCPort = 0; 207 | this->m_iRemoteCPort = 0; 208 | this->m_iDataReceivePort = 0; 209 | this->m_uPagkageSize = 0; 210 | this->m_uPreviousFrame = -1; 211 | this->m_uFrameNumber = 0; 212 | 213 | this->m_localCEndpoint = udp::endpoint(); //local connection endpoint 214 | this->m_remoteCEndpoint = udp::endpoint(); //remote connection endpoint 215 | this->m_localMEndpoint = udp::endpoint(); //local multicast endpoint - data transmisson endpoint 216 | this->m_remoteMEndpoint = udp::endpoint(); //remote multicast endpoint - data transmisson endpoint 217 | 218 | s_mutex.unlock(); 219 | } 220 | //parse message received form the server 221 | void MotionCaptureFZMotion::parseMessage(const SimpleConfirmMessage& scm) { 222 | //software name 223 | this->m_strSoftware = scm.szSoftware; 224 | 225 | //buffer size 226 | this->m_uPagkageSize = scm.uDataBytes; 227 | 228 | //data receive port 229 | this->m_iDataReceivePort = scm.uDataPort; 230 | 231 | //parse sdk version 232 | stringstream sstream; 233 | sstream << (uint32)scm.uSdkVersion.version[0] << "." << (uint32)scm.uSdkVersion.version[1] << 234 | "." << (uint32)scm.uSdkVersion.version[2] << "." << (uint32)scm.uSdkVersion.version[3]; 235 | 236 | this->m_strSDKVersion = sstream.str(); 237 | 238 | //parse sdk version 239 | sstream.str(""); 240 | sstream << (uint32)scm.uVersion.version[0] << "." << (uint32)scm.uVersion.version[1] << 241 | "." << (uint32)scm.uVersion.version[2] << "." << (uint32)scm.uVersion.version[3]; 242 | 243 | this->m_strSoftwareVersion = sstream.str(); 244 | 245 | //parse multicast ip group 246 | sstream.str(this->m_strMulticastGroup); 247 | sstream << (uint32)scm.uMulticastGroup.ipv4[0] << "." << (uint32)scm.uMulticastGroup.ipv4[1] << "." << 248 | (uint32)scm.uMulticastGroup.ipv4[2] << "." << (uint32)scm.uMulticastGroup.ipv4[3]; 249 | 250 | this->m_strMulticastGroup = sstream.str(); 251 | } 252 | //parse rigidbody tag list 253 | void MotionCaptureFZMotion::parseRigidbodyTagList(const byte* const pData, map& mapTagList) { 254 | //parse rigidboy tag list 255 | byte* ptr = const_cast(pData); 256 | 257 | //get message 258 | Message eMessage; 259 | CopyBuffer((byte*)&eMessage, ptr, sizeof(Message)); 260 | 261 | //validate message 262 | if (eMessage != Message::TagListData) 263 | return; 264 | 265 | ptr += sizeof(Message); 266 | 267 | //get byte count of sent data 268 | uint16 uDataBytes; 269 | CopyBuffer((byte*)&uDataBytes, ptr, sizeof(uint16)); 270 | this->m_uPagkageSize = uDataBytes; 271 | ptr += sizeof(uint16); 272 | 273 | //get set number 274 | int32 iDataSetNumber = int32(*ptr); 275 | CopyBuffer((byte*)&iDataSetNumber, ptr, sizeof(int32)); 276 | ptr += sizeof(int32); 277 | 278 | //get tag list data 279 | mapTagList.clear(); 280 | for (int i = 0; i < iDataSetNumber; i++) { 281 | LRigidbodyTag tag; 282 | strcpy(tag.szName, (char*)ptr); 283 | int iOffset = static_cast(strlen(tag.szName) + 1); 284 | ptr += iOffset; 285 | CopyBuffer((byte*)&tag + 256, ptr, sizeof(uint32) + sizeof(real32) * 3); 286 | mapTagList[tag.uRigidbodyID] = tag; 287 | ptr += sizeof(uint32) + sizeof(real32) * 3; 288 | } 289 | } 290 | //receive and parse each frame data 291 | void MotionCaptureFZMotion::receiveFrameData() { 292 | byte* pBuffer = GetEmptyBuffer(MAX_FRAME_SIZE); 293 | boost:system::error_code ec; 294 | udp::endpoint multicastEndpoint; 295 | cout << "receiveiFrameData" << endl; 296 | do{ 297 | //receive motion capture data 298 | if (this->m_TransmissionSocket.receive_from(boost::asio::buffer(pBuffer, MAX_FRAME_SIZE), multicastEndpoint,0, ec) <= 0) { 299 | cout << "Failed to receve data frame." << endl; 300 | continue; 301 | } 302 | //get message 303 | Message eMessage; 304 | CopyBuffer((byte*)&eMessage, pBuffer, sizeof(Message)); 305 | 306 | if (eMessage != Message::MotionCaptureData) { 307 | continue; 308 | } 309 | 310 | if (this->m_bFirstFrame == true) { 311 | this->setFirstFrame(false); 312 | this->m_remoteCEndpoint = multicastEndpoint; 313 | } 314 | 315 | //parse data 316 | parseData(pBuffer, this->m_vctMarkData, this->m_vctRigidbodyData); 317 | 318 | //clear buffer 319 | EmptyBuffer(pBuffer, MAX_FRAME_SIZE); 320 | }while (this->m_TransmissionSocket.available() > 0); 321 | 322 | ReleaseBuffer(pBuffer); 323 | } 324 | //parse marker and rigibody data 325 | void MotionCaptureFZMotion::parseData(const byte* const pData, vector& allMarkers, vector& allRigidBodys) { 326 | //parse received data 327 | byte* ptr = const_cast(pData); 328 | 329 | Message eMessage; 330 | CopyBuffer((byte*)&eMessage, ptr, sizeof(Message)); 331 | 332 | if (eMessage != Message::MotionCaptureData) 333 | return; 334 | 335 | ptr += sizeof(Message); 336 | uint16 uDataBytes; 337 | CopyBuffer((byte*)&uDataBytes, ptr, sizeof(uint16)); 338 | ptr += sizeof(uint16); 339 | CopyBuffer((byte*)&this->m_uFrameNumber, ptr, sizeof(uint32)); 340 | ptr += sizeof(int32); 341 | uint32 uMarkerSets; 342 | CopyBuffer((byte*)&uMarkerSets, ptr, sizeof(uint32)); 343 | ptr += sizeof(uint32); 344 | int32 iMarkerNumber; 345 | CopyBuffer((byte*)&iMarkerNumber, ptr, sizeof(int32)); 346 | ptr += sizeof(int32); 347 | 348 | allMarkers.clear(); 349 | if (iMarkerNumber > 0) { 350 | uint32 uCopySize = sizeof(LMarker) * iMarkerNumber; 351 | allMarkers.resize(iMarkerNumber); 352 | CopyBuffer((byte*)allMarkers.data(), ptr, uCopySize); 353 | ptr += uCopySize; 354 | } 355 | 356 | allRigidBodys.clear(); 357 | uint32 uRigidSets; 358 | CopyBuffer((byte*)&uRigidSets, ptr, sizeof(uint32)); 359 | ptr += 4; 360 | if (uRigidSets > 0) { 361 | uint32 uCopySize = sizeof(LRigidBody) * uRigidSets; 362 | allRigidBodys.resize(uRigidSets); 363 | CopyBuffer((byte*)allRigidBodys.data(), ptr, uCopySize); 364 | ptr += uCopySize; 365 | } 366 | } 367 | void MotionCaptureFZMotion::waitForNextFrame() { 368 | s_mutex.lock(); 369 | 370 | if (this->isConnected() == true) { 371 | receiveFrameData(); 372 | } 373 | s_mutex.unlock(); 374 | } 375 | const std::map& MotionCaptureFZMotion::rigidBodies() const { 376 | s_mutex.lock(); 377 | if (this->m_uPreviousFrame == this->m_uFrameNumber) { 378 | s_mutex.unlock(); 379 | return rigidBodies_; 380 | } 381 | 382 | rigidBodies_.clear(); 383 | 384 | for (auto& lrb : this->m_vctRigidbodyData) { 385 | auto& tag = m_mapRigidbodyTagList[lrb.ID]; 386 | Eigen::Vector3f position( 387 | lrb.sPosition.x + tag.sCenteroidTransform.x, 388 | lrb.sPosition.y + tag.sCenteroidTransform.y, 389 | lrb.sPosition.z + tag.sCenteroidTransform.z 390 | ); 391 | Eigen::Quaternionf rotation(Eigen::Quaternionf( 392 | lrb.sOrientation.qw, 393 | lrb.sOrientation.qx, 394 | lrb.sOrientation.qy, 395 | lrb.sOrientation.qz 396 | )); 397 | RigidBody rigidbody(tag.szName, position, rotation); 398 | rigidBodies_.emplace(tag.szName, rigidbody); 399 | } 400 | 401 | this->m_uPreviousFrame = this->m_uFrameNumber; 402 | s_mutex.unlock(); 403 | return rigidBodies_; 404 | } 405 | const PointCloud& MotionCaptureFZMotion::pointCloud() const { 406 | s_mutex.lock(); 407 | if (this->m_uPreviousFrame == this->m_uFrameNumber) { 408 | s_mutex.unlock(); 409 | return pointcloud_; 410 | } 411 | 412 | if (pointcloud_.size() != this->m_vctMarkData.size()) { 413 | pointcloud_.resize(this->m_vctMarkData.size(), Eigen::NoChange); 414 | } 415 | 416 | for (uint32 row = 0; row < this->m_vctMarkData.size(); row++) { 417 | auto& marker = this->m_vctMarkData[row]; 418 | pointcloud_.row(row) << marker.sPosition.x, marker.sPosition.y, marker.sPosition.z; 419 | } 420 | s_mutex.unlock(); 421 | return pointcloud_; 422 | } 423 | } 424 | -------------------------------------------------------------------------------- /src/mock.cpp: -------------------------------------------------------------------------------- 1 | #include "libmotioncapture/mock.h" 2 | 3 | #include 4 | 5 | namespace libmotioncapture { 6 | 7 | class MotionCaptureMockImpl{ 8 | public: 9 | MotionCaptureMockImpl() 10 | { 11 | } 12 | 13 | public: 14 | float dt; 15 | }; 16 | 17 | MotionCaptureMock::MotionCaptureMock( 18 | float dt, 19 | const std::vector& objects, 20 | const PointCloud& pointCloud) 21 | { 22 | pImpl = new MotionCaptureMockImpl; 23 | pImpl->dt = dt; 24 | for (const auto& obj : objects) { 25 | rigidBodies_.insert(std::make_pair(obj.name(), obj)); 26 | } 27 | pointcloud_ = pointCloud; 28 | } 29 | 30 | void MotionCaptureMock::waitForNextFrame() 31 | { 32 | std::this_thread::sleep_for(std::chrono::milliseconds((int)(pImpl->dt * 1000))); 33 | } 34 | 35 | const std::map& MotionCaptureMock::rigidBodies() const 36 | { 37 | return rigidBodies_; 38 | } 39 | 40 | const PointCloud& MotionCaptureMock::pointCloud() const 41 | { 42 | return pointcloud_; 43 | } 44 | 45 | MotionCaptureMock::~MotionCaptureMock() 46 | { 47 | delete pImpl; 48 | } 49 | } 50 | 51 | -------------------------------------------------------------------------------- /src/motionanalysis.cpp: -------------------------------------------------------------------------------- 1 | #include "libmotioncapture/motionanalysis.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include "cortex.h" 7 | 8 | namespace libmotioncapture { 9 | 10 | std::mutex mtx; 11 | sFrameOfData frameOfData = {0}; 12 | 13 | const sFrameOfData &GetCurrentFrame() { 14 | static sFrameOfData frame = {0}; 15 | { 16 | std::lock_guard lck(mtx); 17 | frame = frameOfData; 18 | } 19 | return frame; 20 | } 21 | 22 | void ErrorMsgHandler(int iLevel, const char *szMsg) { 23 | const char *szLevel = nullptr; 24 | 25 | if (iLevel == VL_Debug) { 26 | szLevel = "Debug"; 27 | } else if (iLevel == VL_Info) { 28 | szLevel = "Info"; 29 | } else if (iLevel == VL_Warning) { 30 | szLevel = "Warning"; 31 | } else if (iLevel == VL_Error) { 32 | szLevel = "Error"; 33 | } 34 | 35 | printf(" %s: %s\n", szLevel, szMsg); 36 | } 37 | 38 | void DataHandler(sFrameOfData *pFrameOfData) { 39 | std::lock_guard lck(mtx); 40 | frameOfData = *pFrameOfData; 41 | } 42 | 43 | class MotionCaptureMotionAnalysisImpl { 44 | public: 45 | std::string version = "0.0.0"; 46 | sBodyDefs *pBodyDefs = nullptr; 47 | 48 | int lastFrame = 0; 49 | 50 | ~MotionCaptureMotionAnalysisImpl() { 51 | if (nullptr != pBodyDefs) { 52 | pBodyDefs = nullptr; 53 | } 54 | } 55 | }; 56 | 57 | MotionCaptureMotionAnalysis::MotionCaptureMotionAnalysis( 58 | const std::string &hostname, 59 | int updateFrequency) { 60 | pImpl = new MotionCaptureMotionAnalysisImpl(); 61 | 62 | unsigned char SDK_Version[4]; 63 | int retval; 64 | 65 | Cortex_SetVerbosityLevel(VL_Info); 66 | Cortex_GetSdkVersion(SDK_Version); 67 | pImpl->version = std::to_string(SDK_Version[1]) + "." + std::to_string(SDK_Version[2]) + "." + 68 | std::to_string(SDK_Version[3]); 69 | 70 | Cortex_SetErrorMsgHandlerFunc(ErrorMsgHandler); 71 | Cortex_SetDataHandlerFunc(DataHandler); 72 | 73 | retval = Cortex_Initialize(hostname.c_str(), nullptr); 74 | 75 | if (retval != RC_Okay) { 76 | std::stringstream sstr; 77 | sstr << "Error: Unable to initialize ethernet communication"; 78 | throw std::runtime_error(sstr.str()); 79 | } 80 | } 81 | 82 | MotionCaptureMotionAnalysis::~MotionCaptureMotionAnalysis() { 83 | if (nullptr != pImpl) { 84 | delete pImpl; 85 | pImpl = nullptr; 86 | } 87 | } 88 | 89 | const std::string &MotionCaptureMotionAnalysis::version() const { 90 | return pImpl->version; 91 | } 92 | 93 | void MotionCaptureMotionAnalysis::waitForNextFrame() { 94 | static auto lastTime = std::chrono::high_resolution_clock::now(); 95 | auto now = std::chrono::high_resolution_clock::now(); 96 | 97 | int frameNo; 98 | do { 99 | std::this_thread::sleep_for(std::chrono::milliseconds(1)); 100 | frameNo = GetCurrentFrame().iFrame; 101 | } while (frameNo == pImpl->lastFrame); 102 | pImpl->lastFrame = frameNo; 103 | lastTime = now; 104 | } 105 | 106 | const std::map &MotionCaptureMotionAnalysis::rigidBodies() const { 107 | rigidBodies_.clear(); 108 | 109 | sFrameOfData pFrameOfData = GetCurrentFrame(); 110 | 111 | for (int iBody = 0; iBody < pFrameOfData.nBodies; iBody++) { 112 | sBodyData *Body = &pFrameOfData.BodyData[iBody]; 113 | 114 | float centroid[3] = {0, 0, 0}; 115 | 116 | for (int iMarker = 0; iMarker < Body->nMarkers; iMarker++) { 117 | centroid[0] += Body->Markers[iMarker][0]; 118 | centroid[1] += Body->Markers[iMarker][1]; 119 | centroid[2] += Body->Markers[iMarker][2]; 120 | } 121 | 122 | centroid[0] /= (float) Body->nMarkers; 123 | centroid[1] /= (float) Body->nMarkers; 124 | centroid[2] /= (float) Body->nMarkers; 125 | 126 | Eigen::Vector3f position(centroid[0], centroid[1], centroid[2]); 127 | Eigen::Quaternionf rotation = Eigen::Quaternionf::Identity(); 128 | 129 | char *bodyName = Body->szName; 130 | 131 | rigidBodies_.emplace(bodyName, RigidBody(bodyName, position, rotation)); 132 | } 133 | 134 | return rigidBodies_; 135 | } 136 | 137 | RigidBody MotionCaptureMotionAnalysis::rigidBodyByName(const std::string &name) const { 138 | sFrameOfData pFrameOfData = GetCurrentFrame(); 139 | 140 | for (int iBody = 0; iBody < pFrameOfData.nBodies; iBody++) { 141 | sBodyData *Body = &pFrameOfData.BodyData[iBody]; 142 | 143 | if (Body->szName == name) { 144 | float centroid[3] = {0, 0, 0}; 145 | 146 | for (int iMarker = 0; iMarker < Body->nMarkers; iMarker++) { 147 | centroid[0] += Body->Markers[iMarker][0]; 148 | centroid[1] += Body->Markers[iMarker][1]; 149 | centroid[2] += Body->Markers[iMarker][2]; 150 | } 151 | 152 | centroid[0] /= (float) Body->nMarkers; 153 | centroid[1] /= (float) Body->nMarkers; 154 | centroid[2] /= (float) Body->nMarkers; 155 | 156 | Eigen::Vector3f position(centroid[0], centroid[1], centroid[2]); 157 | Eigen::Quaternionf rotation = Eigen::Quaternionf::Identity(); 158 | 159 | return RigidBody(name, position, rotation); 160 | } 161 | } 162 | throw std::runtime_error("Rigid body not found"); 163 | } 164 | 165 | bool MotionCaptureMotionAnalysis::supportsRigidBodyTracking() const { 166 | return true; 167 | } 168 | 169 | } // namespace libmotioncapture -------------------------------------------------------------------------------- /src/motioncapture.cpp: -------------------------------------------------------------------------------- 1 | #include "libmotioncapture/motioncapture.h" 2 | #include "libmotioncapture/mock.h" 3 | #ifdef ENABLE_VICON 4 | #include "libmotioncapture/vicon.h" 5 | #endif 6 | #ifdef ENABLE_OPTITRACK 7 | #include "libmotioncapture/optitrack.h" 8 | #endif 9 | #ifdef ENABLE_OPTITRACK_CLOSED_SOURCE 10 | #include "libmotioncapture/optitrack_closed_source.h" 11 | #endif 12 | #ifdef ENABLE_QUALISYS 13 | #include "libmotioncapture/qualisys.h" 14 | #endif 15 | #ifdef ENABLE_NOKOV 16 | #include "libmotioncapture/nokov.h" 17 | #endif 18 | #ifdef ENABLE_VRPN 19 | #include "libmotioncapture/vrpn.h" 20 | #endif 21 | #ifdef ENABLE_MOTIONANALYSIS 22 | #include "libmotioncapture/motionanalysis.h" 23 | #endif 24 | #ifdef ENABLE_FZMOTION 25 | #include "libmotioncapture/fzmotion.h" 26 | #endif 27 | 28 | namespace libmotioncapture { 29 | 30 | const char *version_string = 31 | #include "../version" 32 | ; 33 | 34 | const char *version() 35 | { 36 | return version_string; 37 | } 38 | 39 | RigidBody MotionCapture::rigidBodyByName( 40 | const std::string& name) const 41 | { 42 | const auto& obj = rigidBodies(); 43 | const auto iter = obj.find(name); 44 | if (iter != obj.end()) { 45 | return iter->second; 46 | } 47 | throw std::runtime_error("Rigid body not found!"); 48 | } 49 | 50 | std::string getString( 51 | const std::map &cfg, 52 | const std::string& key, 53 | const std::string& default_value) 54 | { 55 | const auto iter = cfg.find(key); 56 | if (iter != cfg.end()) { 57 | return iter->second; 58 | } 59 | return default_value; 60 | } 61 | 62 | bool getBool( 63 | const std::map &cfg, 64 | const std::string& key, 65 | bool default_value) 66 | { 67 | const auto iter = cfg.find(key); 68 | if (iter != cfg.end()) { 69 | if (iter->second == "1" || iter->second == "true") { 70 | return true; 71 | } 72 | return false; 73 | } 74 | return default_value; 75 | } 76 | 77 | int getInt( 78 | const std::map &cfg, 79 | const std::string &key, 80 | int default_value) 81 | { 82 | const auto iter = cfg.find(key); 83 | if (iter != cfg.end()) 84 | { 85 | return std::stoi(iter->second); 86 | } 87 | return default_value; 88 | } 89 | 90 | MotionCapture *MotionCapture::connect( 91 | const std::string &type, 92 | const std::map &cfg) 93 | { 94 | MotionCapture* mocap = nullptr; 95 | 96 | if (false) 97 | { 98 | } 99 | else if (type == "mock") 100 | { 101 | 102 | // read rigid bodies from string 103 | // e.g., "rb1(x,y,z,qw,qx,qy,qz);rb2(x,y,z,qw,qx,qy,qz)" 104 | 105 | std::vector rigidBodies; 106 | auto rbstring = getString(cfg, "rigid_bodies", ""); 107 | size_t pos1 = 0, pos2 = 0; 108 | while (pos1 <= rbstring.size()) { 109 | pos2 = rbstring.find(';', pos1); 110 | if (pos2 == std::string::npos) { 111 | pos2 = rbstring.size(); 112 | } 113 | auto rbstr = rbstring.substr(pos1, pos2-pos1); 114 | float x, y, z, qw, qx, qy, qz; 115 | char name[100]; 116 | int scanned = std::sscanf(rbstr.c_str(), "%[^(](%f,%f,%f,%f,%f,%f,%f)", name, &x, &y, &z, &qw, &qx, &qy, &qz); 117 | if (scanned == 8) { 118 | Eigen::Vector3f pos(x,y,z); 119 | Eigen::Quaternionf rot(qw, qx, qy, qz); 120 | rigidBodies.emplace_back(libmotioncapture::RigidBody(std::string(name), pos, rot)); 121 | } else { 122 | break; 123 | } 124 | pos1 = pos2 + 1; 125 | } 126 | 127 | // read pointcloud from string 128 | // e.g., "x,y,z;x,y,z" 129 | 130 | PointCloud pc; 131 | auto pcstring = getString(cfg, "pointcloud", ""); 132 | pos1 = 0, pos2 = 0; 133 | while (pos1 <= pcstring.size()) { 134 | pos2 = pcstring.find(';', pos1); 135 | if (pos2 == std::string::npos) { 136 | pos2 = pcstring.size(); 137 | } 138 | auto pcstr = pcstring.substr(pos1, pos2-pos1); 139 | float x, y, z; 140 | int scanned = std::sscanf(pcstr.c_str(), "%f,%f,%f", &x, &y, &z); 141 | if (scanned == 3) { 142 | pc.conservativeResize(pc.rows()+1, Eigen::NoChange); 143 | pc.row(pc.rows()-1) << x, y, z; 144 | } else { 145 | break; 146 | } 147 | pos1 = pos2 + 1; 148 | } 149 | // pc.resize(4, Eigen::NoChange); 150 | // pc.row(0) << 0, 0, 0; 151 | // pc.row(1) << 0, 0.5, 0; 152 | // pc.row(2) << 0, -0.5, 0; 153 | // pc.row(3) << 0.5, 0, 0; 154 | 155 | mocap = new libmotioncapture::MotionCaptureMock( 156 | 1.0f / getInt(cfg, "frequency", 100), 157 | rigidBodies, pc); 158 | } 159 | #ifdef ENABLE_VICON 160 | else if (type == "vicon") 161 | { 162 | mocap = new libmotioncapture::MotionCaptureVicon( 163 | getString(cfg, "hostname", "localhost"), 164 | getBool(cfg, "enable_objects", true), 165 | getBool(cfg, "enable_pointclout", true)); 166 | } 167 | #endif 168 | #ifdef ENABLE_OPTITRACK 169 | else if (type == "optitrack") 170 | { 171 | mocap = new libmotioncapture::MotionCaptureOptitrack( 172 | getString(cfg, "hostname", "localhost"), 173 | getString(cfg, "interface_ip", "0.0.0.0"), 174 | getInt(cfg, "port_command", 1510)); 175 | } 176 | #endif 177 | #ifdef ENABLE_OPTITRACK_CLOSED_SOURCE 178 | else if (type == "optitrack_closed_source") 179 | { 180 | mocap = new libmotioncapture::MotionCaptureOptitrackClosedSource( 181 | getString(cfg, "hostname", "localhost"), 182 | getInt(cfg, "port_command", 1510)); 183 | } 184 | #endif 185 | #ifdef ENABLE_QUALISYS 186 | else if (type == "qualisys") 187 | { 188 | mocap = new libmotioncapture::MotionCaptureQualisys( 189 | getString(cfg, "hostname", "localhost"), 190 | getInt(cfg, "port", 22222), 191 | getBool(cfg, "enable_objects", true), 192 | getBool(cfg, "enable_pointcloud", true)); 193 | } 194 | #endif 195 | #ifdef ENABLE_NOKOV 196 | else if (type == "nokov") 197 | { 198 | mocap = new libmotioncapture::MotionCaptureNokov( 199 | getString(cfg, "hostname", "localhost"), 200 | getBool(cfg, "enableFrequency", false), 201 | getInt(cfg, "updateFrequency", 100)); 202 | } 203 | #endif 204 | #ifdef ENABLE_VRPN 205 | else if (type == "vrpn") 206 | { 207 | mocap = new libmotioncapture::MotionCaptureVrpn( 208 | getString(cfg, "hostname", "localhost")); 209 | } 210 | #endif 211 | #ifdef ENABLE_MOTIONANALYSIS 212 | else if (type == "motionanalysis") 213 | { 214 | mocap = new libmotioncapture::MotionCaptureMotionAnalysis( 215 | getString(cfg, "hostname", "localhost")); 216 | } 217 | #endif 218 | #ifdef ENABLE_FZMOTION 219 | else if (type == "fzmotion") 220 | { 221 | mocap = libmotioncapture::MotionCaptureFZMotion::getInstance( 222 | getString(cfg, "local_IP", "0.0.0.0"), 223 | getInt(cfg, "local_port", 9762), 224 | getString(cfg, "hostname", "fzmotion"), 225 | getInt(cfg, "remote_port", 9761)); 226 | } 227 | #endif 228 | else 229 | { 230 | throw std::runtime_error("Unknown motion capture type!"); 231 | } 232 | 233 | return mocap; 234 | } 235 | 236 | } 237 | -------------------------------------------------------------------------------- /src/nokov.cpp: -------------------------------------------------------------------------------- 1 | #include "libmotioncapture/nokov.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | //#include 9 | #include "NokovSDKCAPI.h" 10 | // NOKOV 11 | #include "NokovSDKClient.h" 12 | 13 | namespace libmotioncapture { 14 | 15 | typedef struct 16 | { 17 | int ID; // RigidBody identifier 18 | float x, y, z; // Position 19 | float qx, qy, qz, qw; // Orientation 20 | } sBodyData; 21 | 22 | typedef struct 23 | { 24 | int iFrame; // host defined frame number 25 | int nOtherMarkers; // # of undefined markers 26 | MarkerData OtherMarkers[MAX_MARKERS]; // undefined marker data 27 | int nRigidBodies; // # of rigid bodies 28 | sBodyData RigidBodies[MAX_RIGIDBODIES]; // Rigid body data 29 | float fLatency; // host defined time delta between capture and send 30 | unsigned int Timecode; // SMPTE timecode (if available) 31 | unsigned int TimecodeSubframe; // timecode sub-frame data 32 | long long iTimeStamp; // FrameGroup timestamp 33 | short params; // host defined parameters 34 | 35 | } sFrameOfObjData; 36 | 37 | // Global Var 38 | sFrameOfObjData frameObjData = {0}; 39 | std::mutex mtx; 40 | 41 | const sFrameOfObjData& GetCurrentFrame() 42 | { 43 | static sFrameOfObjData frame = {0}; 44 | { 45 | std::lock_guard lck (mtx); 46 | frame = frameObjData; 47 | } 48 | return frame; 49 | } 50 | 51 | void DataHandler(sFrameOfMocapData* pFrameOfData, void* pUserData) 52 | { 53 | if (nullptr == pFrameOfData) 54 | return; 55 | 56 | // Store the frame 57 | std::lock_guard lck (mtx); 58 | 59 | int nmaker = (pFrameOfData->nOtherMarkers < MAX_MARKERS)?pFrameOfData->nOtherMarkers:MAX_MARKERS; 60 | 61 | frameObjData.iFrame = pFrameOfData->iFrame; 62 | frameObjData.nOtherMarkers = nmaker; 63 | frameObjData.nRigidBodies = pFrameOfData->nRigidBodies; 64 | frameObjData.fLatency = pFrameOfData->fLatency; 65 | frameObjData.Timecode = pFrameOfData->Timecode; 66 | frameObjData.TimecodeSubframe = pFrameOfData->TimecodeSubframe; 67 | frameObjData.iTimeStamp = pFrameOfData->iTimeStamp; 68 | frameObjData.params = pFrameOfData->params; 69 | 70 | for(int i = 0; i< nmaker; ++i) 71 | { 72 | frameObjData.OtherMarkers[i][0] = pFrameOfData->OtherMarkers[i][0] * 0.001; 73 | frameObjData.OtherMarkers[i][1] = pFrameOfData->OtherMarkers[i][1] * 0.001; 74 | frameObjData.OtherMarkers[i][2] = pFrameOfData->OtherMarkers[i][2] * 0.001; 75 | } 76 | 77 | for(int i = 0; i< pFrameOfData->nRigidBodies; ++i) 78 | { 79 | frameObjData.RigidBodies[i].ID = pFrameOfData->RigidBodies[i].ID; 80 | frameObjData.RigidBodies[i].x = pFrameOfData->RigidBodies[i].x * 0.001; 81 | frameObjData.RigidBodies[i].y = pFrameOfData->RigidBodies[i].y * 0.001; 82 | frameObjData.RigidBodies[i].z = pFrameOfData->RigidBodies[i].z * 0.001; 83 | frameObjData.RigidBodies[i].qx = pFrameOfData->RigidBodies[i].qx; 84 | frameObjData.RigidBodies[i].qy = pFrameOfData->RigidBodies[i].qy; 85 | frameObjData.RigidBodies[i].qz = pFrameOfData->RigidBodies[i].qz; 86 | frameObjData.RigidBodies[i].qw = pFrameOfData->RigidBodies[i].qw; 87 | } 88 | } 89 | 90 | class MotionCaptureNokovImpl 91 | { 92 | public: 93 | std::string version = "0.0.0.0"; 94 | int updateFrequency = 100; 95 | bool enableFixedUpdate = false; 96 | sDataDescriptions* pBodyDefs = nullptr; 97 | NokovSDKClient* pClient = nullptr; 98 | int lastFrame = 0; 99 | std::unordered_map bodyMap; 100 | 101 | size_t GetBodyIdByName(const std::string& name) const { 102 | if (bodyMap.find(name) != bodyMap.end()) 103 | { 104 | return bodyMap.at(name); 105 | } 106 | 107 | return -1; 108 | } 109 | 110 | const std::string GetBodyNameById(size_t id) const { 111 | for (auto pair : bodyMap) 112 | { 113 | if (pair.second == id) 114 | { 115 | return pair.first; 116 | } 117 | } 118 | 119 | return std::string(); 120 | } 121 | 122 | ~MotionCaptureNokovImpl() 123 | { 124 | if (nullptr != pClient) 125 | { 126 | pClient->Uninitialize(); 127 | delete pClient; 128 | pClient = nullptr; 129 | } 130 | 131 | if (nullptr != pBodyDefs) 132 | { 133 | XingYing_FreeDescriptions(pBodyDefs); 134 | pBodyDefs= nullptr; 135 | } 136 | } 137 | }; 138 | 139 | MotionCaptureNokov::MotionCaptureNokov( 140 | const std::string& hostname, 141 | bool enableFrequency, 142 | int updateFrequency) 143 | { 144 | pImpl = new MotionCaptureNokovImpl; 145 | 146 | NokovSDKClient* theClient = new NokovSDKClient(); 147 | unsigned char version[4] = {0}; 148 | theClient->NokovSDKVersion(version); 149 | { 150 | std::stringstream sstr; 151 | sstr << (int)version[0] << "." << (int)version[1] << "." << (int)version[2] << "." << (int)version[3]; 152 | pImpl->version = sstr.str(); 153 | } 154 | 155 | theClient->SetDataCallback(DataHandler); 156 | 157 | // Check the ret value 158 | int retValue = theClient->Initialize((char*)hostname.c_str()); 159 | if (ErrorCode_OK != retValue) 160 | { 161 | std::stringstream sstr; 162 | sstr << "Error connecting XINGYING on address: " << hostname << " Code:" << retValue; 163 | throw std::runtime_error(sstr.str()); 164 | } 165 | 166 | if (ErrorCode_OK != theClient->GetDataDescriptions(&pImpl->pBodyDefs) || nullptr == pImpl->pBodyDefs) 167 | { 168 | std::stringstream sstr; 169 | sstr << "Error Request BodyDefs on address: " << hostname << " Code:" << retValue; 170 | throw std::runtime_error(sstr.str()); 171 | } 172 | 173 | for (int iDataDef = 0; iDataDef < pImpl->pBodyDefs->nDataDescriptions; ++iDataDef) 174 | { 175 | if (pImpl->pBodyDefs->arrDataDescriptions[iDataDef].type == Descriptor_RigidBody) 176 | { 177 | auto bodeDef = pImpl->pBodyDefs->arrDataDescriptions[iDataDef].Data.RigidBodyDescription; 178 | pImpl->bodyMap[bodeDef->szName] = bodeDef->ID; 179 | } 180 | } 181 | 182 | pImpl->pClient = theClient; 183 | pImpl->enableFixedUpdate = enableFrequency; 184 | pImpl->updateFrequency = updateFrequency; 185 | } 186 | 187 | MotionCaptureNokov::~MotionCaptureNokov() 188 | { 189 | if (pImpl) 190 | { 191 | delete pImpl; 192 | } 193 | } 194 | 195 | const std::string& MotionCaptureNokov::version() const 196 | { 197 | return pImpl->version; 198 | } 199 | 200 | void MotionCaptureNokov::waitForNextFrame() 201 | { 202 | static auto lastTime = std::chrono::high_resolution_clock::now(); 203 | auto now = std::chrono::high_resolution_clock::now(); 204 | 205 | if (pImpl->enableFixedUpdate) 206 | { 207 | auto elapsed = now - lastTime; 208 | auto desiredPeriod = std::chrono::milliseconds(1000 / pImpl->updateFrequency); 209 | //std::cout <<"elapsed: " << std::chrono::duration(elapsed).count() << "\tdesired:" << std::chrono::duration(desiredPeriod).count() << std::endl; 210 | if (elapsed < desiredPeriod) { 211 | //std::cout << "Sleep Done" << std::endl; 212 | std::this_thread::sleep_for(desiredPeriod - elapsed); 213 | } 214 | } 215 | 216 | int frameNo; 217 | do 218 | { 219 | frameNo = GetCurrentFrame().iFrame; 220 | } 221 | while (frameNo == pImpl->lastFrame); 222 | pImpl->lastFrame = frameNo; 223 | lastTime = std::chrono::high_resolution_clock::now(); 224 | } 225 | 226 | bool MotionCaptureNokov::supportsPointCloud() const 227 | { 228 | return true; 229 | } 230 | 231 | const std::map& MotionCaptureNokov::rigidBodies() const 232 | { 233 | rigidBodies_.clear(); 234 | 235 | auto frameData = GetCurrentFrame(); 236 | for (int iBody = 0; iBody < frameData.nRigidBodies; ++iBody) { 237 | 238 | const auto& rb = frameData.RigidBodies[iBody]; 239 | Eigen::Vector3f position( 240 | rb.x, 241 | rb.y, 242 | rb.z); 243 | 244 | // Convention 245 | Eigen::Quaternionf rotation(rb.qw, rb.qx, rb.qy, rb.qz); 246 | auto bodyName = pImpl->GetBodyNameById(rb.ID); 247 | 248 | rigidBodies_.emplace(bodyName, RigidBody(bodyName,position, rotation)); 249 | } 250 | 251 | return rigidBodies_; 252 | } 253 | 254 | 255 | libmotioncapture::RigidBody MotionCaptureNokov::rigidBodyByName(const std::string& name) const 256 | { 257 | size_t bodyId = pImpl->GetBodyIdByName(name); 258 | if (bodyId < 0) 259 | { 260 | throw std::runtime_error("Unknown rigid body!"); 261 | } 262 | 263 | auto frameData = GetCurrentFrame(); 264 | for (size_t iBody = 0; iBody < frameData.nRigidBodies; ++iBody) { 265 | 266 | const auto& rb = frameData.RigidBodies[iBody]; 267 | 268 | if (bodyId == rb.ID) 269 | { 270 | Eigen::Vector3f position( 271 | rb.x, 272 | rb.y, 273 | rb.z); 274 | 275 | Eigen::Quaternionf rotation(rb.qw, rb.qx, rb.qy, rb.qz); 276 | 277 | return RigidBody(pImpl->GetBodyNameById(bodyId), position, rotation); 278 | } 279 | } 280 | 281 | throw std::runtime_error("Unknown rigid body!"); 282 | } 283 | 284 | const libmotioncapture::PointCloud& MotionCaptureNokov::pointCloud() const 285 | { 286 | auto frameData = GetCurrentFrame(); 287 | size_t count = frameData.nOtherMarkers; 288 | pointcloud_.resize(count, Eigen::NoChange); 289 | for (size_t iMarkerIdx = 0; iMarkerIdx < count; ++iMarkerIdx) { 290 | 291 | pointcloud_.row(iMarkerIdx) << frameData.OtherMarkers[iMarkerIdx][0], 292 | frameData.OtherMarkers[iMarkerIdx][1], 293 | frameData.OtherMarkers[iMarkerIdx][2]; 294 | } 295 | return pointcloud_; 296 | } 297 | 298 | 299 | bool MotionCaptureNokov::supportsRigidBodyTracking() const 300 | { 301 | return true; 302 | } 303 | 304 | } 305 | -------------------------------------------------------------------------------- /src/optitrack.cpp: -------------------------------------------------------------------------------- 1 | #include "libmotioncapture/optitrack.h" 2 | 3 | #include 4 | 5 | using boost::asio::ip::udp; 6 | 7 | namespace libmotioncapture { 8 | 9 | constexpr int MAX_PACKETSIZE = 65503; // max size of packet (actual packet size is dynamic) 10 | constexpr int MAX_NAMELENGTH = 256; 11 | 12 | constexpr int NAT_CONNECT = 0; 13 | constexpr int NAT_SERVERINFO = 1; 14 | constexpr int NAT_REQUEST_MODELDEF = 4; 15 | constexpr int NAT_MODELDEF = 5; 16 | 17 | /** 18 | * \brief Unpack number of bytes of data for a given data type. 19 | * Useful if you want to skip this type of data. 20 | * \param ptr - input data stream pointer 21 | * \param major - NatNet major version 22 | * \param minor - NatNet minor version 23 | * \return - pointer after decoded object 24 | */ 25 | char* UnpackDataSize(char* ptr, int major, int minor, int& nBytes, bool skip = false ) 26 | { 27 | nBytes = 0; 28 | 29 | // size of all data for this data type (in bytes); 30 | if (((major == 4) && (minor > 0)) || (major > 4)) 31 | { 32 | memcpy(&nBytes, ptr, 4); ptr += 4; 33 | // printf("Byte Count: %d\n", nBytes); 34 | if (skip) 35 | { 36 | ptr += nBytes; 37 | } 38 | } 39 | return ptr; 40 | } 41 | 42 | class MotionCaptureOptitrackImpl{ 43 | public: 44 | MotionCaptureOptitrackImpl() 45 | : version() 46 | , versionMajor(0) 47 | , versionMinor(0) 48 | , io_context() 49 | , socket(io_context) 50 | , sender_endpoint() 51 | , data(MAX_PACKETSIZE) 52 | { 53 | } 54 | // void getObjectByRigidbody( 55 | // const RigidBody& rb, 56 | // Object& result) const 57 | // { 58 | // std::stringstream sstr; 59 | // sstr << rb.id(); 60 | // const std::string name_number = sstr.str(); 61 | // std::string name_cf = "cf"; 62 | // const std::string name = name_cf + name_number; 63 | 64 | // auto const translation = rb.location(); 65 | // auto const quaternion = rb.orientation(); 66 | 67 | // if(rb.trackingValid()) { 68 | // Eigen::Vector3f position( 69 | // -translation.y, 70 | // translation.x, 71 | // translation.z); 72 | 73 | // Eigen::Quaternionf rotation( 74 | // quaternion.qw, 75 | // -quaternion.qy, 76 | // quaternion.qx, 77 | // quaternion.qz 78 | // ); 79 | 80 | // result = Object(name, position, rotation); 81 | 82 | // } else { 83 | // result = Object(name); 84 | // } 85 | // } 86 | 87 | void parseModelDef(const char* data) 88 | { 89 | const char *ptr = data; 90 | int major = versionMajor; 91 | int minor = versionMinor; 92 | 93 | // First 2 Bytes is message ID 94 | int MessageID = 0; 95 | memcpy(&MessageID, ptr, 2); ptr += 2; 96 | // printf("Message ID : %d\n", MessageID); 97 | 98 | // Second 2 Bytes is the size of the packet 99 | int nBytes = 0; 100 | memcpy(&nBytes, ptr, 2); ptr += 2; 101 | // printf("Byte count : %d\n", nBytes); 102 | 103 | if(MessageID == NAT_MODELDEF) // Data Descriptions 104 | { 105 | // number of datasets 106 | int nDatasets = 0; memcpy(&nDatasets, ptr, 4); ptr += 4; 107 | // printf("Dataset Count : %d\n", nDatasets); 108 | 109 | for(int i=0; i < nDatasets; i++) 110 | { 111 | // printf("Dataset %d\n", i); 112 | 113 | int type = 0; memcpy(&type, ptr, 4); ptr += 4; 114 | int description_size = 0; 115 | // printf("Type : %d\n", i, type); 116 | 117 | if ((major == 4 && minor >= 1) || major > 4) 118 | { 119 | // If the NatNet version is 4.1 or greater, next four bytes represent 120 | // the number of bytes in the dataset. Just skip them. 121 | memcpy(&description_size, ptr, 4); ptr += 4; 122 | } 123 | 124 | if(type == 0) // markerset 125 | { 126 | ptr += strlen(ptr) + 1; // name 127 | 128 | // marker data 129 | int nMarkers = 0; memcpy(&nMarkers, ptr, 4); ptr += 4; 130 | // printf("Marker Count : %d\n", nMarkers); 131 | 132 | for(int j=0; j < nMarkers; j++) 133 | { 134 | ptr += strlen(ptr) + 1; 135 | } 136 | } 137 | else if(type ==1) // rigid body 138 | { 139 | char szName[MAX_NAMELENGTH]; 140 | if(major >= 2) 141 | { 142 | // name 143 | strcpy(szName, ptr); 144 | ptr += strlen(ptr) + 1; 145 | // printf("Name: %s\n", szName); 146 | } 147 | 148 | int ID = 0; memcpy(&ID, ptr, 4); ptr +=4; 149 | // printf("ID : %d\n", ID); 150 | 151 | rigidBodyDefinitions[ID].name = szName; 152 | rigidBodyDefinitions[ID].ID = ID; 153 | 154 | memcpy(&rigidBodyDefinitions[ID].parentID, ptr, 4); ptr +=4; 155 | memcpy(&rigidBodyDefinitions[ID].xoffset, ptr, 4); ptr +=4; 156 | memcpy(&rigidBodyDefinitions[ID].yoffset, ptr, 4); ptr +=4; 157 | memcpy(&rigidBodyDefinitions[ID].zoffset, ptr, 4); ptr +=4; 158 | 159 | // Per-marker data (NatNet 3.0 and later) 160 | if ( major >= 3 ) 161 | { 162 | int nMarkers = 0; memcpy( &nMarkers, ptr, 4 ); ptr += 4; 163 | // Marker positions 164 | nBytes = nMarkers * 3 * sizeof(float); 165 | ptr += nBytes; 166 | // Marker required active labels 167 | nBytes = nMarkers * sizeof(int); 168 | ptr += nBytes; 169 | // Marker Name 170 | if (major >= 4) { 171 | for (int markerIdx = 0; markerIdx < nMarkers; ++markerIdx) { 172 | ptr += strlen(ptr) + 1; 173 | } 174 | } 175 | } 176 | } 177 | else if ((major == 4 && minor >= 1) || major > 4) 178 | { 179 | // We got a description_size for > 4.1, which is simpler to discard 180 | // for unsuported datatypes 181 | ptr += description_size; 182 | } 183 | else if(type ==2) // skeleton 184 | { 185 | // char szName[MAX_NAMELENGTH]; 186 | // strcpy(szName, ptr); 187 | ptr += strlen(ptr) + 1; 188 | // printf("Name: %s\n", szName); 189 | 190 | // int ID = 0; memcpy(&ID, ptr, 4); 191 | ptr +=4; 192 | // printf("ID : %d\n", ID); 193 | 194 | int nRigidBodies = 0; memcpy(&nRigidBodies, ptr, 4); ptr +=4; 195 | // printf("RigidBody (Bone) Count : %d\n", nRigidBodies); 196 | 197 | for(int i=0; i< nRigidBodies; i++) 198 | { 199 | if(major >= 2) 200 | { 201 | // RB name 202 | // char szName[MAX_NAMELENGTH]; 203 | // strcpy(szName, ptr); 204 | ptr += strlen(ptr) + 1; 205 | // printf("Rigid Body Name: %s\n", szName); 206 | } 207 | 208 | // int ID = 0; memcpy(&ID, ptr, 4); 209 | ptr +=4; 210 | // printf("RigidBody ID : %d\n", ID); 211 | 212 | // int parentID = 0; memcpy(&parentID, ptr, 4); 213 | ptr +=4; 214 | // printf("Parent ID : %d\n", parentID); 215 | 216 | // float xoffset = 0; memcpy(&xoffset, ptr, 4); 217 | ptr +=4; 218 | // printf("X Offset : %3.2f\n", xoffset); 219 | 220 | // float yoffset = 0; memcpy(&yoffset, ptr, 4); 221 | ptr +=4; 222 | // printf("Y Offset : %3.2f\n", yoffset); 223 | 224 | // float zoffset = 0; memcpy(&zoffset, ptr, 4); 225 | ptr +=4; 226 | // printf("Z Offset : %3.2f\n", zoffset); 227 | } 228 | } 229 | } // next dataset 230 | 231 | // printf("End Packet\n-------------\n"); 232 | 233 | } 234 | } 235 | 236 | public: 237 | // NatNetClient client; 238 | std::string version; 239 | int versionMajor; 240 | int versionMinor; 241 | uint64_t clockFrequency; // ticks/second for timestamps 242 | 243 | boost::asio::io_context io_context; 244 | boost::asio::ip::udp::socket socket; 245 | boost::asio::ip::udp::endpoint sender_endpoint; 246 | std::vector data; 247 | 248 | struct rigidBody { 249 | int ID; 250 | float x; 251 | float y; 252 | float z; 253 | float qx; 254 | float qy; 255 | float qz; 256 | float qw; 257 | float fError; // mean marker error 258 | bool bTrackingValid; 259 | }; 260 | std::vector rigidBodies; 261 | 262 | struct marker { 263 | float x; 264 | float y; 265 | float z; 266 | }; 267 | std::vector markers; 268 | 269 | struct rigidBodyDefinition { 270 | std::string name; 271 | int ID; 272 | int parentID; 273 | float xoffset; 274 | float yoffset; 275 | float zoffset; 276 | }; 277 | std::map rigidBodyDefinitions; 278 | }; 279 | 280 | MotionCaptureOptitrack::MotionCaptureOptitrack( 281 | const std::string &hostname, 282 | const std::string& interface_ip, 283 | int port_command) 284 | { 285 | pImpl = new MotionCaptureOptitrackImpl; 286 | 287 | // Connect to command port to query version 288 | boost::asio::io_context io_context_cmd; 289 | udp::socket socket_cmd(io_context_cmd, udp::endpoint(udp::v4(), 0)); 290 | udp::resolver resolver_cmd(io_context_cmd); 291 | udp::endpoint endpoint_cmd(boost::asio::ip::make_address(hostname), port_command); 292 | 293 | typedef struct 294 | { 295 | unsigned short iMessage; // message ID (e.g. NAT_FRAMEOFDATA) 296 | unsigned short nDataBytes; // Num bytes in payload 297 | } sRequest; 298 | 299 | typedef struct 300 | { 301 | unsigned short iMessage; 302 | unsigned short nDataBytes; 303 | char szName[MAX_NAMELENGTH]; // host app's name 304 | unsigned char Version[4]; // host app's version [major.minor.build.revision] 305 | unsigned char NatNetVersion[4]; // host app's NatNet version [major.minor.build.revision] 306 | uint8_t HighResClockFrequency[8]; // host's high resolution clock frequency (ticks per second) 307 | uint16_t DataPort; 308 | bool IsMulticast; 309 | uint8_t MulticastGroupAddress[4]; 310 | } sResponse; 311 | 312 | sRequest connectCmd = {NAT_CONNECT, 0}; 313 | socket_cmd.send_to(boost::asio::buffer(&connectCmd, sizeof(connectCmd)), endpoint_cmd); 314 | 315 | sResponse response; 316 | udp::endpoint sender_endpoint; 317 | size_t reply_length = socket_cmd.receive_from( 318 | boost::asio::buffer(&response, sizeof(response)), sender_endpoint); 319 | 320 | if (response.iMessage != NAT_SERVERINFO) { 321 | throw std::runtime_error("Could not query NatNet version!"); 322 | } 323 | 324 | std::ostringstream stringStream; 325 | stringStream << (int)response.NatNetVersion[0] << "." 326 | << (int)response.NatNetVersion[1] << "." 327 | << (int)response.NatNetVersion[2] << "." 328 | << (int)response.NatNetVersion[3]; 329 | pImpl->version = stringStream.str(); 330 | 331 | pImpl->versionMajor = response.NatNetVersion[0]; 332 | pImpl->versionMinor = response.NatNetVersion[1]; 333 | memcpy(&pImpl->clockFrequency, response.HighResClockFrequency, sizeof(uint64_t)); 334 | 335 | if (!response.IsMulticast) { 336 | throw std::runtime_error("Motive does not use multicast. Please update your streaming settings!"); 337 | } 338 | 339 | std::stringstream sstr; 340 | sstr << (int)response.MulticastGroupAddress[0] << "." 341 | << (int)response.MulticastGroupAddress[1] << "." 342 | << (int)response.MulticastGroupAddress[2] << "." 343 | << (int)response.MulticastGroupAddress[3]; 344 | std::string multicast_address = sstr.str(); 345 | uint16_t port_data = response.DataPort; 346 | 347 | // query model def 348 | sRequest modelDefCmd = {NAT_REQUEST_MODELDEF, 0}; 349 | socket_cmd.send_to(boost::asio::buffer(&modelDefCmd, sizeof(modelDefCmd)), endpoint_cmd); 350 | std::vector modelDef(MAX_PACKETSIZE); 351 | reply_length = socket_cmd.receive_from( 352 | boost::asio::buffer(modelDef.data(), modelDef.size()), sender_endpoint); 353 | modelDef.resize(reply_length); 354 | pImpl->parseModelDef(modelDef.data()); 355 | 356 | // connect to data port to receive mocap data 357 | auto listen_address_boost = boost::asio::ip::make_address_v4(interface_ip); 358 | auto multicast_address_boost = boost::asio::ip::make_address_v4(multicast_address); 359 | 360 | // Create the socket so that multiple may be bound to the same address. 361 | boost::asio::ip::udp::endpoint listen_endpoint( 362 | listen_address_boost, port_data); 363 | pImpl->socket.open(listen_endpoint.protocol()); 364 | pImpl->socket.set_option(boost::asio::ip::udp::socket::reuse_address(true)); 365 | pImpl->socket.bind(listen_endpoint); 366 | 367 | // Join the multicast group on a specific interface 368 | pImpl->socket.set_option(boost::asio::ip::multicast::join_group(multicast_address_boost, listen_address_boost)); 369 | } 370 | 371 | const std::string & MotionCaptureOptitrack::version() const 372 | { 373 | return pImpl->version; 374 | } 375 | 376 | void MotionCaptureOptitrack::waitForNextFrame() 377 | { 378 | // use a loop to get latest data 379 | do { 380 | pImpl->data.resize(MAX_PACKETSIZE); 381 | size_t length = pImpl->socket.receive_from(boost::asio::buffer(pImpl->data.data(), pImpl->data.size()), pImpl->sender_endpoint); 382 | pImpl->data.resize(length); 383 | } while (pImpl->socket.available() > 0); 384 | 385 | if (pImpl->data.size() > 4) { 386 | char *ptr = pImpl->data.data(); 387 | int major = pImpl->versionMajor; 388 | int minor = pImpl->versionMinor; 389 | 390 | // First 2 Bytes is message ID 391 | int MessageID = 0; 392 | memcpy(&MessageID, ptr, 2); ptr += 2; 393 | // printf("Message ID : %d\n", MessageID); 394 | 395 | // Second 2 Bytes is the size of the packet 396 | int nBytes = 0; 397 | memcpy(&nBytes, ptr, 2); ptr += 2; 398 | // printf("Byte count : %d\n", nBytes); 399 | 400 | if(MessageID == 7) // FRAME OF MOCAP DATA packet 401 | { 402 | // Next 4 Bytes is the frame number 403 | int frameNumber = 0; memcpy(&frameNumber, ptr, 4); ptr += 4; 404 | // printf("Frame # : %d\n", frameNumber); 405 | 406 | // Next 4 Bytes is the number of data sets (markersets, rigidbodies, etc) 407 | int nMarkerSets = 0; memcpy(&nMarkerSets, ptr, 4); ptr += 4; 408 | // printf("Marker Set Count : %d\n", nMarkerSets); 409 | 410 | int nBytes=0; 411 | ptr = UnpackDataSize(ptr, major, minor,nBytes); 412 | 413 | // Loop through number of marker sets and get name and data 414 | for (int i=0; i < nMarkerSets; i++) 415 | { 416 | ptr += strlen(ptr) + 1; 417 | int nMarkers = 0; memcpy(&nMarkers, ptr, 4); ptr += 4; 418 | ptr += nMarkers * 12; 419 | } 420 | 421 | // Loop through unlabeled markers 422 | // OtherMarker list is Deprecated 423 | int nOtherMarkers = 0; memcpy(&nOtherMarkers, ptr, 4); ptr += 4; 424 | ptr = UnpackDataSize(ptr, major, minor,nBytes); 425 | pImpl->markers.resize(nOtherMarkers); 426 | for (int j = 0; j < nOtherMarkers; j++) 427 | { 428 | memcpy(&pImpl->markers[j].x, ptr, 4); ptr += 4; 429 | memcpy(&pImpl->markers[j].y, ptr, 4); ptr += 4; 430 | memcpy(&pImpl->markers[j].z, ptr, 4); ptr += 4; 431 | } 432 | 433 | // Loop through rigidbodies 434 | int nRigidBodies = 0; memcpy(&nRigidBodies, ptr, 4); ptr += 4; 435 | ptr = UnpackDataSize(ptr, major, minor,nBytes); 436 | pImpl->rigidBodies.resize(nRigidBodies); 437 | // printf("Rigid Body Count : %d\n", nRigidBodies); 438 | for (int j=0; j < nRigidBodies; j++) 439 | { 440 | // Rigid body position and orientation 441 | memcpy(&pImpl->rigidBodies[j].ID, ptr, 4); ptr += 4; 442 | memcpy(&pImpl->rigidBodies[j].x, ptr, 4); ptr += 4; 443 | memcpy(&pImpl->rigidBodies[j].y, ptr, 4); ptr += 4; 444 | memcpy(&pImpl->rigidBodies[j].z, ptr, 4); ptr += 4; 445 | memcpy(&pImpl->rigidBodies[j].qx, ptr, 4); ptr += 4; 446 | memcpy(&pImpl->rigidBodies[j].qy, ptr, 4); ptr += 4; 447 | memcpy(&pImpl->rigidBodies[j].qz, ptr, 4); ptr += 4; 448 | memcpy(&pImpl->rigidBodies[j].qw, ptr, 4); ptr += 4; 449 | 450 | // NatNet version 2.0 and later 451 | if(major >= 2) 452 | { 453 | // Mean marker error 454 | memcpy(&pImpl->rigidBodies[j].fError, ptr, 4); ptr += 4; 455 | } 456 | 457 | // NatNet version 2.6 and later 458 | if( ((major == 2)&&(minor >= 6)) || (major > 2) || (major == 0) ) 459 | { 460 | // params 461 | short params = 0; memcpy(¶ms, ptr, 2); ptr += 2; 462 | pImpl->rigidBodies[j].bTrackingValid = params & 0x01; // 0x01 : rigid body was successfully tracked in this frame 463 | } 464 | } // Go to next rigid body 465 | 466 | // Skeletons (NatNet version 2.1 and later) 467 | // (we do not support skeletons) 468 | if( ((major == 2)&&(minor>0)) || (major>2)) 469 | { 470 | int nSkeletons = 0; memcpy(&nSkeletons, ptr, 4); ptr += 4; 471 | // printf("Skeleton Count : %d\n", nSkeletons); 472 | ptr = UnpackDataSize(ptr, major, minor,nBytes); 473 | 474 | // Loop through skeletons 475 | for (int j=0; j < nSkeletons; j++) 476 | { 477 | // skeleton id 478 | // int skeletonID = 0; 479 | // memcpy(&skeletonID, ptr, 4); 480 | ptr += 4; 481 | 482 | // Number of rigid bodies (bones) in skeleton 483 | int nRigidBodies = 0; 484 | memcpy(&nRigidBodies, ptr, 4); ptr += 4; 485 | // printf("Rigid Body Count : %d\n", nRigidBodies); 486 | 487 | // Loop through rigid bodies (bones) in skeleton 488 | for (int j=0; j < nRigidBodies; j++) 489 | { 490 | // Rigid body position and orientation 491 | ptr += 8*4; 492 | 493 | // Mean marker error (NatNet version 2.0 and later) 494 | if(major >= 2) 495 | { 496 | ptr += 4; 497 | } 498 | 499 | // Tracking flags (NatNet version 2.6 and later) 500 | if( ((major == 2)&&(minor >= 6)) || (major > 2) || (major == 0) ) 501 | { 502 | ptr += 2; 503 | } 504 | } // next rigid body 505 | } // next skeleton 506 | } 507 | 508 | // Assets ( Motive 3.1 / NatNet 4.1 and greater) 509 | if (((major == 4) && (minor > 0)) || (major > 4)) 510 | { 511 | int nAssets = 0; 512 | memcpy(&nAssets, ptr, 4); ptr += 4; 513 | // printf("Asset Count : %d\n", nAssets); 514 | 515 | int nBytes=0; 516 | ptr = UnpackDataSize(ptr, major, minor,nBytes); 517 | ptr += nBytes; 518 | } 519 | 520 | // labeled markers (NatNet version 2.3 and later) 521 | // labeled markers - this includes all markers: Active, Passive, and 'unlabeled' (markers with no asset but a PointCloud ID) 522 | if( ((major == 2)&&(minor>=3)) || (major>2)) 523 | { 524 | int nLabeledMarkers = 0; 525 | memcpy(&nLabeledMarkers, ptr, 4); ptr += 4; 526 | ptr = UnpackDataSize(ptr, major, minor,nBytes); 527 | pImpl->markers.resize(nOtherMarkers + nLabeledMarkers); 528 | // printf("Labeled Marker Count : %d\n", nLabeledMarkers); 529 | 530 | // Loop through labeled markers 531 | for (int j=0; j < nLabeledMarkers; j++) 532 | { 533 | // id 534 | // Marker ID Scheme: 535 | // Active Markers: 536 | // ID = ActiveID, correlates to RB ActiveLabels list 537 | // Passive Markers: 538 | // If Asset with Legacy Labels 539 | // AssetID (Hi Word) 540 | // MemberID (Lo Word) 541 | // Else 542 | // PointCloud ID 543 | // int ID = 0; memcpy(&ID, ptr, 4); 544 | ptr += 4; 545 | // int modelID, markerID; 546 | // DecodeMarkerID(ID, &modelID, &markerID); 547 | 548 | memcpy(&pImpl->markers[nOtherMarkers + j].x, ptr, 4); ptr += 4; 549 | memcpy(&pImpl->markers[nOtherMarkers + j].y, ptr, 4); ptr += 4; 550 | memcpy(&pImpl->markers[nOtherMarkers + j].z, ptr, 4); ptr += 4; 551 | // size 552 | //float size = 0.0f; memcpy(&size, ptr, 4); 553 | ptr += 4; 554 | 555 | // NatNet version 2.6 and later 556 | if( ((major == 2)&&(minor >= 6)) || (major > 2) || (major == 0) ) 557 | { 558 | // marker params 559 | // short params = 0; memcpy(¶ms, ptr, 2); 560 | ptr += 2; 561 | // bool bOccluded = (params & 0x01) != 0; // marker was not visible (occluded) in this frame 562 | // bool bPCSolved = (params & 0x02) != 0; // position provided by point cloud solve 563 | // bool bModelSolved = (params & 0x04) != 0; // position provided by model solve 564 | // if ((major >= 3) || (major == 0)) 565 | // { 566 | // bool bHasModel = (params & 0x08) != 0; // marker has an associated asset in the data stream 567 | // bool bUnlabeled = (params & 0x10) != 0; // marker is 'unlabeled', but has a point cloud ID 568 | // bool bActiveMarker = (params & 0x20) != 0; // marker is an actively labeled LED marker 569 | // } 570 | } 571 | 572 | // NatNet version 3.0 and later 573 | // float residual = 0.0f; 574 | if ((major >= 3) || (major == 0)) 575 | { 576 | // Marker residual 577 | // memcpy(&residual, ptr, 4); 578 | ptr += 4; 579 | } 580 | } 581 | } 582 | 583 | // Force Plate data (NatNet version 2.9 and later) 584 | if (((major == 2) && (minor >= 9)) || (major > 2)) 585 | { 586 | int nForcePlates; 587 | memcpy(&nForcePlates, ptr, 4); ptr += 4; 588 | ptr = UnpackDataSize(ptr, major, minor,nBytes); 589 | for (int iForcePlate = 0; iForcePlate < nForcePlates; iForcePlate++) 590 | { 591 | // ID 592 | // int ID = 0; memcpy(&ID, ptr, 4); 593 | ptr += 4; 594 | // printf("Force Plate : %d\n", ID); 595 | 596 | // Channel Count 597 | int nChannels = 0; memcpy(&nChannels, ptr, 4); ptr += 4; 598 | 599 | // Channel Data 600 | for (int i = 0; i < nChannels; i++) 601 | { 602 | // printf(" Channel %d : ", i); 603 | int nFrames = 0; memcpy(&nFrames, ptr, 4); ptr += 4; 604 | for (int j = 0; j < nFrames; j++) 605 | { 606 | // float val = 0.0f; memcpy(&val, ptr, 4); 607 | ptr += 4; 608 | // printf("%3.2f ", val); 609 | } 610 | // printf("\n"); 611 | } 612 | } 613 | } 614 | 615 | // Device data (NatNet version 3.0 and later) 616 | if (((major == 2) && (minor >= 11)) || (major > 2)) 617 | { 618 | int nDevices; 619 | memcpy(&nDevices, ptr, 4); ptr += 4; 620 | ptr = UnpackDataSize(ptr, major, minor,nBytes); 621 | for (int iDevice = 0; iDevice < nDevices; iDevice++) 622 | { 623 | // ID 624 | // int ID = 0; memcpy(&ID, ptr, 4); 625 | ptr += 4; 626 | // printf("Device : %d\n", ID); 627 | 628 | // Channel Count 629 | int nChannels = 0; memcpy(&nChannels, ptr, 4); ptr += 4; 630 | 631 | // Channel Data 632 | for (int i = 0; i < nChannels; i++) 633 | { 634 | // printf(" Channel %d : ", i); 635 | int nFrames = 0; memcpy(&nFrames, ptr, 4); ptr += 4; 636 | for (int j = 0; j < nFrames; j++) 637 | { 638 | // float val = 0.0f; memcpy(&val, ptr, 4); 639 | ptr += 4; 640 | // printf("%3.2f ", val); 641 | } 642 | // printf("\n"); 643 | } 644 | } 645 | } 646 | 647 | // software latency (removed in version 3.0) 648 | if ( major < 3 ) 649 | { 650 | // float softwareLatency = 0.0f; memcpy(&softwareLatency, ptr, 4); 651 | ptr += 4; 652 | // printf("software latency : %3.3f\n", softwareLatency); 653 | } 654 | 655 | // timecode 656 | // unsigned int timecode = 0; memcpy(&timecode, ptr, 4); 657 | ptr += 4; 658 | // unsigned int timecodeSub = 0; memcpy(&timecodeSub, ptr, 4); 659 | ptr += 4; 660 | // char szTimecode[128] = ""; 661 | // TimecodeStringify(timecode, timecodeSub, szTimecode, 128); 662 | 663 | // timestamp 664 | // double timestamp = 0.0f; 665 | 666 | // NatNet version 2.7 and later - increased from single to double precision 667 | if( ((major == 2)&&(minor>=7)) || (major>2)) 668 | { 669 | // memcpy(×tamp, ptr, 8); 670 | ptr += 8; 671 | } 672 | else 673 | { 674 | // float fTemp = 0.0f; 675 | // memcpy(&fTemp, ptr, 4); 676 | ptr += 4; 677 | // timestamp = (double)fTemp; 678 | } 679 | // printf("Timestamp : %3.3f\n", timestamp); 680 | 681 | // high res timestamps (version 3.0 and later) 682 | latencies_.clear(); 683 | if ( (major >= 3) || (major == 0) ) 684 | { 685 | uint64_t cameraMidExposureTimestamp = 0; 686 | memcpy( &cameraMidExposureTimestamp, ptr, 8 ); 687 | ptr += 8; 688 | 689 | uint64_t cameraDataReceivedTimestamp = 0; 690 | memcpy( &cameraDataReceivedTimestamp, ptr, 8 ); 691 | ptr += 8; 692 | 693 | uint64_t transmitTimestamp = 0; 694 | memcpy( &transmitTimestamp, ptr, 8 ); 695 | ptr += 8; 696 | 697 | const uint64_t cameraLatencyTicks = cameraDataReceivedTimestamp - cameraMidExposureTimestamp; 698 | const double cameraLatencySeconds = cameraLatencyTicks / (double)pImpl->clockFrequency; 699 | latencies_.emplace_back(LatencyInfo("Camera", cameraLatencySeconds)); 700 | 701 | const uint64_t swLatencyTicks = transmitTimestamp - cameraDataReceivedTimestamp; 702 | const double swLatencySeconds = swLatencyTicks / (double)pImpl->clockFrequency; 703 | latencies_.emplace_back(LatencyInfo("Motive", swLatencySeconds)); 704 | 705 | // convert actual shutter timestamp to microseconds 706 | timestamp_ = cameraMidExposureTimestamp * 1e6 / pImpl->clockFrequency; 707 | } 708 | 709 | // frame params 710 | short params = 0; memcpy(¶ms, ptr, 2); 711 | ptr += 2; 712 | // bool bIsRecording = (params & 0x01) != 0; // 0x01 Motive is recording 713 | bool bTrackedModelsChanged = (params & 0x02) != 0; // 0x02 Actively tracked model list has changed 714 | 715 | // end of data tag 716 | // int eod = 0; memcpy(&eod, ptr, 4); ptr += 4; 717 | // printf("End Packet\n-------------\n"); 718 | } 719 | else 720 | { 721 | printf("Unrecognized Packet Type.\n"); 722 | } 723 | } 724 | 725 | } 726 | 727 | const std::map& MotionCaptureOptitrack::rigidBodies() const 728 | { 729 | // TODO: avoid copies here... 730 | rigidBodies_.clear(); 731 | for (const auto& rb : pImpl->rigidBodies) { 732 | if (rb.bTrackingValid) { 733 | const auto& def = pImpl->rigidBodyDefinitions[rb.ID]; 734 | 735 | Eigen::Vector3f position( 736 | rb.x + def.xoffset, 737 | rb.y + def.yoffset, 738 | rb.z + def.zoffset); 739 | 740 | Eigen::Quaternionf rotation( 741 | rb.qw, // w 742 | rb.qx, // x 743 | rb.qy, // y 744 | rb.qz // z 745 | ); 746 | rigidBodies_.emplace(def.name, RigidBody(def.name, position, rotation)); 747 | } 748 | } 749 | return rigidBodies_; 750 | } 751 | 752 | const PointCloud& MotionCaptureOptitrack::pointCloud() const 753 | { 754 | // TODO: avoid copies here... 755 | pointcloud_.resize(pImpl->markers.size(), Eigen::NoChange); 756 | for (size_t r = 0; r < pImpl->markers.size(); ++r) { 757 | const auto& marker = pImpl->markers[r]; 758 | pointcloud_.row(r) << marker.x, marker.y, marker.z; 759 | } 760 | return pointcloud_; 761 | } 762 | 763 | const std::vector &MotionCaptureOptitrack::latency() const 764 | { 765 | return latencies_; 766 | } 767 | 768 | uint64_t MotionCaptureOptitrack::timeStamp() const 769 | { 770 | return timestamp_; 771 | } 772 | 773 | MotionCaptureOptitrack::~MotionCaptureOptitrack() 774 | { 775 | delete pImpl; 776 | } 777 | 778 | } 779 | 780 | -------------------------------------------------------------------------------- /src/optitrack_closed_source.cpp: -------------------------------------------------------------------------------- 1 | #include "libmotioncapture/optitrack_closed_source.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | // #include 8 | #include 9 | #include 10 | 11 | namespace libmotioncapture { 12 | 13 | class MotionCaptureOptitrackClosedSourceImpl{ 14 | public: 15 | MotionCaptureOptitrackClosedSourceImpl() 16 | : received(false) 17 | { 18 | } 19 | 20 | void FrameReceivedCallback(sFrameOfMocapData *data) 21 | { 22 | // We can only receive data, if we can hold onto the data mutex to update the data structure 23 | std::lock_guard lk(data_m); 24 | 25 | // update state 26 | rigidBodies->clear(); 27 | for (size_t i = 0; i < data->nRigidBodies; ++i) { 28 | const auto& rb = data->RigidBodies[i]; 29 | if (rb.params & 0x01) { 30 | const auto &def = rigidBodyDefinitions[rb.ID]; 31 | 32 | Eigen::Vector3f position( 33 | rb.x + def.xoffset, 34 | rb.y + def.yoffset, 35 | rb.z + def.zoffset); 36 | 37 | Eigen::Quaternionf rotation( 38 | rb.qw, // w 39 | rb.qx, // x 40 | rb.qy, // y 41 | rb.qz // z 42 | ); 43 | rigidBodies->emplace(def.name, RigidBody(def.name, position, rotation)); 44 | } 45 | } 46 | 47 | pointcloud->resize(data->nOtherMarkers, Eigen::NoChange); 48 | for (size_t r = 0; r < data->nOtherMarkers; ++r) 49 | { 50 | pointcloud->row(r) << 51 | data->OtherMarkers[r][0], 52 | data->OtherMarkers[r][1], 53 | data->OtherMarkers[r][2]; 54 | } 55 | 56 | latencies->clear(); 57 | const uint64_t cameraLatencyTicks = data->CameraDataReceivedTimestamp - data->CameraMidExposureTimestamp; 58 | const double cameraLatencySeconds = cameraLatencyTicks / (double)clockFrequency; 59 | latencies->emplace_back(LatencyInfo("Camera", cameraLatencySeconds)); 60 | 61 | const uint64_t swLatencyTicks = data->TransmitTimestamp - data->CameraDataReceivedTimestamp; 62 | const double swLatencySeconds = swLatencyTicks / (double)clockFrequency; 63 | latencies->emplace_back(LatencyInfo("Motive", swLatencySeconds)); 64 | 65 | // convert actual shutter timestamp to microseconds 66 | *timestamp = data->CameraMidExposureTimestamp * 1e6 / clockFrequency; 67 | 68 | // notify main thread of update 69 | { 70 | std::lock_guard lk(cv_m); 71 | received = true; 72 | } 73 | cv.notify_all(); 74 | } 75 | 76 | public: 77 | NatNetClient client; 78 | std::condition_variable cv; 79 | std::mutex cv_m; 80 | std::mutex data_m; 81 | bool received; 82 | 83 | uint64_t clockFrequency; // ticks/second for timestamps 84 | struct rigidBodyDefinition 85 | { 86 | std::string name; 87 | int ID; 88 | int parentID; 89 | float xoffset; 90 | float yoffset; 91 | float zoffset; 92 | }; 93 | std::map rigidBodyDefinitions; 94 | 95 | std::map* rigidBodies; 96 | PointCloud* pointcloud; 97 | std::vector* latencies; 98 | uint64_t* timestamp; 99 | }; 100 | 101 | static void FrameReceivedCallback(sFrameOfMocapData *data, void *pUserData) 102 | { 103 | auto client = reinterpret_cast(pUserData); 104 | client->FrameReceivedCallback(data); 105 | } 106 | 107 | MotionCaptureOptitrackClosedSource::MotionCaptureOptitrackClosedSource( 108 | const std::string &hostname, 109 | int port_command) 110 | { 111 | pImpl = new MotionCaptureOptitrackClosedSourceImpl; 112 | pImpl->rigidBodies = &rigidBodies_; 113 | pImpl->pointcloud = &pointcloud_; 114 | pImpl->latencies = &latencies_; 115 | pImpl->timestamp = ×tamp_; 116 | 117 | ErrorCode err; 118 | 119 | err = pImpl->client.SetFrameReceivedCallback(FrameReceivedCallback, pImpl); 120 | if (err != ErrorCode_OK) { 121 | throw std::runtime_error("NatNetSDK Error " + std::to_string(err)); 122 | } 123 | 124 | // Connect first to find out server configuration 125 | sNatNetClientConnectParams connectParams; 126 | connectParams.serverAddress = hostname.c_str(); 127 | connectParams.serverCommandPort = port_command; 128 | err = pImpl->client.Connect(connectParams); 129 | if (err != ErrorCode_OK) { 130 | throw std::runtime_error("NatNetSDK Error " + std::to_string(err)); 131 | } 132 | 133 | // Find server configuration and update connection settings 134 | sServerDescription serverDesc; 135 | err = pImpl->client.GetServerDescription(&serverDesc); 136 | if (err != ErrorCode_OK) { 137 | throw std::runtime_error("NatNetSDK Error " + std::to_string(err)); 138 | } 139 | pImpl->clockFrequency = serverDesc.HighResClockFrequency; 140 | 141 | connectParams.serverDataPort = serverDesc.ConnectionDataPort; 142 | connectParams.connectionType = serverDesc.ConnectionMulticast ? ConnectionType_Multicast : ConnectionType_Unicast; 143 | 144 | std::string multicastAddress = 145 | std::to_string(serverDesc.ConnectionMulticastAddress[0]) + "." + 146 | std::to_string(serverDesc.ConnectionMulticastAddress[1]) + "." + 147 | std::to_string(serverDesc.ConnectionMulticastAddress[2]) + "." + 148 | std::to_string(serverDesc.ConnectionMulticastAddress[3]); 149 | connectParams.multicastAddress = multicastAddress.c_str(); 150 | 151 | // Reconnect 152 | pImpl->client.Disconnect(); 153 | err = pImpl->client.Connect(connectParams); 154 | if (err != ErrorCode_OK) 155 | { 156 | throw std::runtime_error("NatNetSDK Error " + std::to_string(err)); 157 | } 158 | 159 | // get data description 160 | sDataDescriptions *pDataDefs = NULL; 161 | err = pImpl->client.GetDataDescriptionList(&pDataDefs, 1 << Descriptor_RigidBody); 162 | if (err != ErrorCode_OK || pDataDefs == NULL) 163 | { 164 | throw std::runtime_error("NatNetSDK Error " + std::to_string(err)); 165 | } 166 | for (int i = 0; i < pDataDefs->nDataDescriptions; i++) 167 | { 168 | if (pDataDefs->arrDataDescriptions[i].type == Descriptor_RigidBody) { 169 | const auto pRB = pDataDefs->arrDataDescriptions[i].Data.RigidBodyDescription; 170 | auto& def = pImpl->rigidBodyDefinitions[pRB->ID]; 171 | def.ID = pRB->ID; 172 | def.name = pRB->szName; 173 | def.parentID = pRB->parentID; 174 | def.xoffset = pRB->offsetx; 175 | def.yoffset = pRB->offsety; 176 | def.zoffset = pRB->offsety; 177 | } 178 | } 179 | } 180 | 181 | const std::string & MotionCaptureOptitrackClosedSource::version() const 182 | { 183 | // return pImpl->version; 184 | } 185 | 186 | void MotionCaptureOptitrackClosedSource::waitForNextFrame() 187 | { 188 | // unlock data mutex to allow asynchronous callback to update 189 | pImpl->received = false; 190 | pImpl->data_m.unlock(); 191 | 192 | // wait for update 193 | std::unique_lock lk(pImpl->cv_m); 194 | pImpl->cv.wait(lk, [this] { return pImpl->received; }); 195 | 196 | // lock data mutex for user to use data safely 197 | pImpl->data_m.lock(); 198 | } 199 | 200 | const std::map& MotionCaptureOptitrackClosedSource::rigidBodies() const 201 | { 202 | return rigidBodies_; 203 | } 204 | 205 | const PointCloud& MotionCaptureOptitrackClosedSource::pointCloud() const 206 | { 207 | return pointcloud_; 208 | } 209 | 210 | const std::vector &MotionCaptureOptitrackClosedSource::latency() const 211 | { 212 | return latencies_; 213 | } 214 | 215 | uint64_t MotionCaptureOptitrackClosedSource::timeStamp() const 216 | { 217 | return timestamp_; 218 | } 219 | 220 | MotionCaptureOptitrackClosedSource::~MotionCaptureOptitrackClosedSource() 221 | { 222 | delete pImpl; 223 | } 224 | 225 | } 226 | 227 | -------------------------------------------------------------------------------- /src/python_bindings.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "libmotioncapture/motioncapture.h" 10 | 11 | using namespace libmotioncapture; 12 | 13 | namespace py = pybind11; 14 | using namespace pybind11::literals; 15 | 16 | PYBIND11_MODULE(motioncapture, m) { 17 | 18 | m.attr("__version__") = version(); 19 | 20 | m.def("connect", &MotionCapture::connect); 21 | 22 | // Quaternions 23 | py::class_(m, "Quaternion") 24 | .def_property_readonly("x", py::overload_cast<>(&Eigen::Quaternionf::x, py::const_)) 25 | .def_property_readonly("y", py::overload_cast<>(&Eigen::Quaternionf::y, py::const_)) 26 | .def_property_readonly("z", py::overload_cast<>(&Eigen::Quaternionf::z, py::const_)) 27 | .def_property_readonly("w", py::overload_cast<>(&Eigen::Quaternionf::w, py::const_)); 28 | 29 | // RigidBody 30 | py::class_(m, "RigidBody") 31 | .def_property_readonly("name", &RigidBody::name) 32 | .def_property_readonly("position", &RigidBody::position) 33 | .def_property_readonly("rotation", &RigidBody::rotation); 34 | 35 | // 36 | py::class_(m, "MotionCapture") 37 | .def("waitForNextFrame", &MotionCapture::waitForNextFrame, py::call_guard()) 38 | .def_property_readonly("rigidBodies", &MotionCapture::rigidBodies); 39 | 40 | } 41 | -------------------------------------------------------------------------------- /src/qualisys.cpp: -------------------------------------------------------------------------------- 1 | // For windows, make sure that M_PI will be defined 2 | #define _USE_MATH_DEFINES 3 | #include 4 | 5 | #include "libmotioncapture/qualisys.h" 6 | 7 | // Qualisys 8 | #include "RTProtocol.h" 9 | 10 | #include 11 | #include 12 | 13 | namespace libmotioncapture { 14 | 15 | class MotionCaptureQualisysImpl 16 | { 17 | public: 18 | CRTProtocol poRTProtocol; 19 | CRTPacket* pRTPacket; 20 | CRTPacket::EComponentType componentType; 21 | std::string version; 22 | }; 23 | 24 | MotionCaptureQualisys::MotionCaptureQualisys( 25 | const std::string& hostname, 26 | int basePort, 27 | bool enableObjects, 28 | bool enablePointcloud) 29 | { 30 | pImpl = new MotionCaptureQualisysImpl; 31 | unsigned short udpPort = 6734; 32 | 33 | // Connecting ... 34 | if (!pImpl->poRTProtocol.Connect((char*)hostname.c_str(), basePort, &udpPort, 1, 19)) { 35 | std::stringstream sstr; 36 | sstr << "Error connecting QTM on address: " << hostname << ":" << basePort; 37 | throw std::runtime_error(sstr.str()); 38 | } 39 | pImpl->pRTPacket = nullptr; 40 | 41 | // Setting component flag 42 | pImpl->componentType = static_cast(0); 43 | if (enableObjects) { 44 | pImpl->componentType = static_cast(pImpl->componentType | CRTProtocol::cComponent6dEuler); 45 | } 46 | if (enablePointcloud) { 47 | pImpl->componentType = static_cast(pImpl->componentType | CRTProtocol::cComponent3dNoLabels); 48 | } 49 | 50 | // Get 6DOF settings 51 | bool dataAvailable; 52 | pImpl->poRTProtocol.Read6DOFSettings(dataAvailable); 53 | 54 | // Enable UDP streaming of selected component 55 | if (!pImpl->poRTProtocol.StreamFrames(CRTProtocol::RateAllFrames, 0, udpPort, NULL, pImpl->componentType)) { 56 | std::stringstream sstr; 57 | sstr << "Error streaming on port " << udpPort; 58 | throw std::runtime_error(sstr.str()); 59 | } 60 | 61 | // Getting version 62 | char qtmVersion[255]; 63 | unsigned int major, minor; 64 | pImpl->poRTProtocol.GetQTMVersion(qtmVersion, 255); 65 | pImpl->poRTProtocol.GetVersion(major, minor); 66 | std::stringstream sstr; 67 | sstr << qtmVersion << " (Protocol: " << major << "." << minor <<")"; 68 | pImpl->version = sstr.str(); 69 | } 70 | 71 | MotionCaptureQualisys::~MotionCaptureQualisys() 72 | { 73 | delete pImpl; 74 | } 75 | 76 | const std::string& MotionCaptureQualisys::version() const 77 | { 78 | return pImpl->version; 79 | } 80 | 81 | void MotionCaptureQualisys::waitForNextFrame() 82 | { 83 | CRTPacket::EPacketType packetType; 84 | do { 85 | auto result = pImpl->poRTProtocol.Receive(packetType, true, -1); 86 | if (result == CNetwork::ResponseType::success 87 | && packetType == CRTPacket::PacketData) { 88 | pImpl->pRTPacket = pImpl->poRTProtocol.GetRTPacket(); 89 | break; 90 | } 91 | } while(true); 92 | } 93 | 94 | const std::map& MotionCaptureQualisys::rigidBodies() const 95 | { 96 | float pos[3], rx, ry, rz; 97 | 98 | rigidBodies_.clear(); 99 | size_t count = pImpl->pRTPacket->Get6DOFEulerBodyCount(); 100 | 101 | for(size_t i = 0; i < count; ++i) { 102 | std::string name = std::string(pImpl->poRTProtocol.Get6DOFBodyName(i)); 103 | pImpl->pRTPacket->Get6DOFEulerBody(i, pos[0], pos[1], pos[2], rx, ry, rz); 104 | if (!std::isnan(pos[0])) { 105 | Eigen::Vector3f position = Eigen::Vector3f(pos) / 1000.0; 106 | 107 | Eigen::Matrix3f rotation; 108 | rotation = Eigen::AngleAxisf((rx/180.0f)*M_PI, Eigen::Vector3f::UnitX()) 109 | * Eigen::AngleAxisf((ry/180.0f)*M_PI, Eigen::Vector3f::UnitY()) 110 | * Eigen::AngleAxisf((rz/180.0f)*M_PI, Eigen::Vector3f::UnitZ()); 111 | Eigen::Quaternionf quaternion = Eigen::Quaternionf(rotation); 112 | 113 | rigidBodies_.emplace(name, RigidBody(name, position, quaternion)); 114 | } 115 | } 116 | return rigidBodies_; 117 | } 118 | 119 | RigidBody MotionCaptureQualisys::rigidBodyByName(const std::string &name) const 120 | { 121 | // Find object index 122 | size_t bodyCount = pImpl->pRTPacket->Get6DOFEulerBodyCount(); 123 | size_t bodyId; 124 | for (bodyId = 0; bodyId < bodyCount; ++bodyId) { 125 | if (!strcmp(name.c_str(), pImpl->poRTProtocol.Get6DOFBodyName(bodyId))) { 126 | break; 127 | } 128 | } 129 | 130 | // If found, get object position 131 | if (bodyId < bodyCount) { 132 | float pos[3], rx, ry, rz; 133 | 134 | pImpl->pRTPacket->Get6DOFEulerBody(bodyId, pos[0], pos[1], pos[2], rx, ry, rz); 135 | 136 | if (!std::isnan(pos[0])) { 137 | Eigen::Vector3f position = Eigen::Vector3f(pos) / 1000.0; 138 | 139 | Eigen::Matrix3f rotation; 140 | rotation = Eigen::AngleAxisf((rx/180.0f)*M_PI, Eigen::Vector3f::UnitX()) 141 | * Eigen::AngleAxisf((ry/180.0f)*M_PI, Eigen::Vector3f::UnitY()) 142 | * Eigen::AngleAxisf((rz/180.0f)*M_PI, Eigen::Vector3f::UnitZ()); 143 | Eigen::Quaternionf quaternion = Eigen::Quaternionf(rotation); 144 | 145 | return RigidBody(name, position, quaternion); 146 | } 147 | } 148 | throw std::runtime_error("Unknown rigid body!"); 149 | } 150 | 151 | const PointCloud& MotionCaptureQualisys::pointCloud() const 152 | { 153 | size_t count = pImpl->pRTPacket->Get3DNoLabelsMarkerCount(); 154 | pointcloud_.resize(count, Eigen::NoChange); 155 | for(size_t i = 0; i < count; ++i) { 156 | float x, y, z; 157 | unsigned int nId; 158 | pImpl->pRTPacket->Get3DNoLabelsMarker(i, x, y, z, nId); 159 | pointcloud_.row(i) << x / 1000.0, y / 1000.0, z / 1000.0; 160 | } 161 | return pointcloud_; 162 | } 163 | 164 | uint64_t MotionCaptureQualisys::timeStamp() const 165 | { 166 | return pImpl->pRTPacket->GetTimeStamp(); 167 | } 168 | 169 | } 170 | -------------------------------------------------------------------------------- /src/vicon.cpp: -------------------------------------------------------------------------------- 1 | #include "libmotioncapture/vicon.h" 2 | 3 | // VICON 4 | #include "ViconDataStreamSDK_CPP/DataStreamClient.h" 5 | 6 | using namespace ViconDataStreamSDK::CPP; 7 | 8 | namespace libmotioncapture { 9 | 10 | class MotionCaptureViconImpl 11 | { 12 | public: 13 | Client client; 14 | std::string version; 15 | }; 16 | 17 | MotionCaptureVicon::MotionCaptureVicon( 18 | const std::string& hostname, 19 | bool enableObjects, 20 | bool enablePointcloud) 21 | { 22 | pImpl = new MotionCaptureViconImpl; 23 | 24 | // Try connecting... 25 | while (!pImpl->client.IsConnected().Connected) { 26 | pImpl->client.Connect(hostname); 27 | } 28 | 29 | if (enableObjects) { 30 | pImpl->client.EnableSegmentData(); 31 | } 32 | if (enablePointcloud) { 33 | pImpl->client.EnableUnlabeledMarkerData(); 34 | } 35 | 36 | // This is the lowest latency option 37 | pImpl->client.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ServerPush); 38 | 39 | // Set the global up axis 40 | pImpl->client.SetAxisMapping(Direction::Forward, 41 | Direction::Left, 42 | Direction::Up); // Z-up 43 | 44 | // Discover the version number 45 | Output_GetVersion version = pImpl->client.GetVersion(); 46 | std::stringstream sstr; 47 | sstr << version.Major << "." << version.Minor << "." << version.Point; 48 | pImpl->version = sstr.str(); 49 | } 50 | 51 | MotionCaptureVicon::~MotionCaptureVicon() 52 | { 53 | delete pImpl; 54 | } 55 | 56 | const std::string& MotionCaptureVicon::version() const 57 | { 58 | return pImpl->version; 59 | } 60 | 61 | void MotionCaptureVicon::waitForNextFrame() 62 | { 63 | while (pImpl->client.GetFrame().Result != Result::Success) { 64 | } 65 | } 66 | 67 | const std::map& MotionCaptureVicon::rigidBodies() const 68 | { 69 | rigidBodies_.clear(); 70 | size_t count = pImpl->client.GetSubjectCount().SubjectCount; 71 | for (size_t i = 0; i < count; ++i) { 72 | auto const name = pImpl->client.GetSubjectName(i).SubjectName; 73 | auto const translation = pImpl->client.GetSegmentGlobalTranslation(name, name); 74 | auto const quaternion = pImpl->client.GetSegmentGlobalRotationQuaternion(name, name); 75 | if ( translation.Result == Result::Success 76 | && quaternion.Result == Result::Success 77 | && !translation.Occluded 78 | && !quaternion.Occluded) { 79 | 80 | Eigen::Vector3f position( 81 | translation.Translation[0] / 1000.0, 82 | translation.Translation[1] / 1000.0, 83 | translation.Translation[2] / 1000.0); 84 | 85 | Eigen::Quaternionf rotation( 86 | quaternion.Rotation[3], // w 87 | quaternion.Rotation[0], // x 88 | quaternion.Rotation[1], // y 89 | quaternion.Rotation[2] // z 90 | ); 91 | 92 | rigidBodies_.emplace(name, RigidBody(name, position, rotation)); 93 | } 94 | } 95 | return rigidBodies_; 96 | } 97 | 98 | RigidBody MotionCaptureVicon::rigidBodyByName( 99 | const std::string& name) const 100 | { 101 | auto const translation = pImpl->client.GetSegmentGlobalTranslation(name, name); 102 | auto const quaternion = pImpl->client.GetSegmentGlobalRotationQuaternion(name, name); 103 | if ( translation.Result == Result::Success 104 | && quaternion.Result == Result::Success 105 | && !translation.Occluded 106 | && !quaternion.Occluded) { 107 | 108 | Eigen::Vector3f position( 109 | translation.Translation[0] / 1000.0, 110 | translation.Translation[1] / 1000.0, 111 | translation.Translation[2] / 1000.0); 112 | 113 | Eigen::Quaternionf rotation( 114 | quaternion.Rotation[3], // w 115 | quaternion.Rotation[0], // x 116 | quaternion.Rotation[1], // y 117 | quaternion.Rotation[2] // z 118 | ); 119 | 120 | return RigidBody(name, position, rotation); 121 | } 122 | throw std::runtime_error("Unknown rigid body!"); 123 | } 124 | 125 | const PointCloud& MotionCaptureVicon::pointCloud() const 126 | { 127 | size_t count = pImpl->client.GetUnlabeledMarkerCount().MarkerCount; 128 | pointcloud_.resize(count, Eigen::NoChange); 129 | for(size_t i = 0; i < count; ++i) { 130 | Output_GetUnlabeledMarkerGlobalTranslation translation = 131 | pImpl->client.GetUnlabeledMarkerGlobalTranslation(i); 132 | pointcloud_.row(i) << 133 | translation.Translation[0] / 1000.0, 134 | translation.Translation[1] / 1000.0, 135 | translation.Translation[2] / 1000.0; 136 | } 137 | return pointcloud_; 138 | } 139 | 140 | const std::vector& MotionCaptureVicon::latency() const 141 | { 142 | latencies_.clear(); 143 | size_t latencyCount = pImpl->client.GetLatencySampleCount().Count; 144 | for(size_t i = 0; i < latencyCount; ++i) { 145 | std::string sampleName = pImpl->client.GetLatencySampleName(i).Name; 146 | double sampleValue = pImpl->client.GetLatencySampleValue(sampleName).Value; 147 | latencies_.emplace_back(LatencyInfo(sampleName, sampleValue)); 148 | } 149 | return latencies_; 150 | } 151 | 152 | } 153 | -------------------------------------------------------------------------------- /src/vrpn.cpp: -------------------------------------------------------------------------------- 1 | #include "libmotioncapture/vrpn.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | // VRPN 11 | #include 12 | #include 13 | 14 | namespace 15 | { 16 | std::unordered_set name_blacklist_({"VRPN Control"}); 17 | } 18 | 19 | namespace libmotioncapture { 20 | 21 | class MotionCaptureVrpnImpl 22 | { 23 | public: 24 | static MotionCaptureVrpnImpl* instance; 25 | std::shared_ptr connection; 26 | std::unordered_map > trackers; 27 | std::unordered_map trackerData; 28 | int updateFrequency; 29 | 30 | void updateTrackers() 31 | { 32 | const char* name = nullptr; 33 | for (int i = 0; (name = connection->sender_name(i)) != NULL; ++i) { 34 | if (trackers.count(name) == 0 && name_blacklist_.count(name) == 0) 35 | { 36 | std::cerr << "tracker: " << name << std::endl; 37 | trackers.insert(std::make_pair(name, 38 | std::make_shared(name, connection.get()))); 39 | 40 | trackers[name]->register_change_handler((void*)name, &MotionCaptureVrpnImpl::handle_pose); 41 | } 42 | } 43 | } 44 | 45 | static void VRPN_CALLBACK handle_pose(void *userData, const vrpn_TRACKERCB tracker_pose) 46 | { 47 | // std::cerr << "pos " << (const char*)userData << tracker_pose.pos[0] << std::endl; 48 | std::string name((const char*)userData); 49 | instance->trackerData[name] = tracker_pose; 50 | } 51 | }; 52 | 53 | MotionCaptureVrpnImpl* MotionCaptureVrpnImpl::instance; 54 | 55 | MotionCaptureVrpn::MotionCaptureVrpn( 56 | const std::string& hostname, 57 | int updateFrequency) 58 | { 59 | pImpl = new MotionCaptureVrpnImpl; 60 | pImpl->instance = pImpl; 61 | pImpl->updateFrequency = updateFrequency; 62 | 63 | pImpl->connection = std::shared_ptr(vrpn_get_connection_by_name(hostname.c_str())); 64 | } 65 | 66 | MotionCaptureVrpn::~MotionCaptureVrpn() 67 | { 68 | delete pImpl; 69 | } 70 | 71 | void MotionCaptureVrpn::waitForNextFrame() 72 | { 73 | // We use a fixed update frequency here, because VRPN is stateless 74 | // with respect to the active trackers. Since users might enable/disable 75 | // trackers at any time, this approach keeps the active trackers updated. 76 | // Disadvantage: higher latency, since we do not attempt to synchronize 77 | 78 | static auto lastTime = std::chrono::high_resolution_clock::now(); 79 | 80 | auto now = std::chrono::high_resolution_clock::now(); 81 | auto elapsed = now - lastTime; 82 | auto desiredPeriod = std::chrono::milliseconds(1000 / pImpl->updateFrequency); 83 | if (elapsed < desiredPeriod) { 84 | std::this_thread::sleep_for(desiredPeriod - elapsed); 85 | } 86 | 87 | 88 | pImpl->updateTrackers(); 89 | pImpl->trackerData.clear(); 90 | // do { 91 | pImpl->connection->mainloop(); 92 | for (auto tracker : pImpl->trackers) { 93 | tracker.second->mainloop(); 94 | } 95 | // std::this_thread::sleep_for(std::chrono::microseconds(1)); 96 | // } while(pImpl->trackerData.size() < pImpl->trackers.size()); 97 | lastTime = now; 98 | } 99 | 100 | const std::map& MotionCaptureVrpn::rigidBodies() const 101 | { 102 | rigidBodies_.clear(); 103 | for (const auto& data : pImpl->trackerData) { 104 | Eigen::Vector3f position( 105 | data.second.pos[0], 106 | data.second.pos[1], 107 | data.second.pos[2]); 108 | 109 | Eigen::Quaternionf rotation( 110 | data.second.quat[3], // w 111 | data.second.quat[0], // x 112 | data.second.quat[1], // y 113 | data.second.quat[2] // z 114 | ); 115 | 116 | rigidBodies_.emplace(data.first, RigidBody(data.first, position, rotation)); 117 | } 118 | return rigidBodies_; 119 | } 120 | 121 | RigidBody MotionCaptureVrpn::rigidBodyByName(const std::string &name) const 122 | { 123 | const auto data = pImpl->trackerData.find(name); 124 | if (data != pImpl->trackerData.end()) { 125 | 126 | Eigen::Vector3f position( 127 | data->second.pos[0], 128 | data->second.pos[1], 129 | data->second.pos[2]); 130 | 131 | Eigen::Quaternionf rotation( 132 | data->second.quat[3], // w 133 | data->second.quat[0], // x 134 | data->second.quat[1], // y 135 | data->second.quat[2] // z 136 | ); 137 | 138 | return RigidBody(name, position, rotation); 139 | } 140 | throw std::runtime_error("Unknown rigid body!"); 141 | } 142 | 143 | } 144 | -------------------------------------------------------------------------------- /tools/build/build-wheels.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -ex 3 | 4 | PYVERSION=${1?"Error: expected the python version"} 5 | 6 | # Prefer running with podman 7 | DOCKER=docker 8 | if which podman 9 | then 10 | DOCKER=podman 11 | fi 12 | 13 | # Test if we are currently in the builder image 14 | if [ ! -d /opt/python ] 15 | then 16 | # We are not in the image, start the image with the build script 17 | $DOCKER run --rm -v $(realpath $(dirname $0)/../..):/io quay.io/pypa/manylinux_2_28_x86_64 /io/tools/build/build-wheels.sh $PYVERSION 18 | else 19 | # We are in the image, building! 20 | PYVERCL=${PYVERSION//./} 21 | # Bins located in paths similar to /opt/python/cp38-cp38/bin 22 | PYBIN="/opt/python/cp${PYVERCL}-cp${PYVERCL}/bin" 23 | echo "Using python path $PYBIN" 24 | 25 | yum install -y boost-devel eigen3-devel 26 | 27 | cd /io 28 | 29 | PATH=${PYBIN}:$PATH 30 | "${PYBIN}/pip" install -U "setuptools>=42" wheel ninja "cmake>=3.12" 31 | "${PYBIN}/python" setup.py bdist_wheel 32 | 33 | WHEELS=$(echo dist/*.whl) 34 | 35 | for whl in $WHEELS; do 36 | auditwheel repair "$whl" -w dist/ 37 | done 38 | 39 | rm $WHEELS 40 | fi 41 | -------------------------------------------------------------------------------- /version: -------------------------------------------------------------------------------- 1 | "1.0a4" --------------------------------------------------------------------------------