From 53a443d12044adc1fa02ff204d8adaa7d5de43d7 Mon Sep 17 00:00:00 2001 From: crosstyan Date: Wed, 4 Feb 2026 03:03:16 +0000 Subject: [PATCH] x --- .gitignore | 1 + CMakeLists.txt | 61 ++ ZED_SDK_ARCHITECTURE.md | 349 +++++++++++ include/ClientPublisher.hpp | 74 +++ include/GLViewer.hpp | 336 ++++++++++ include/utils.hpp | 56 ++ src/ClientPublisher.cpp | 104 ++++ src/GLViewer.cpp | 1161 +++++++++++++++++++++++++++++++++++ src/main.cpp | 211 +++++++ 9 files changed, 2353 insertions(+) create mode 100644 .gitignore create mode 100755 CMakeLists.txt create mode 100644 ZED_SDK_ARCHITECTURE.md create mode 100755 include/ClientPublisher.hpp create mode 100755 include/GLViewer.hpp create mode 100755 include/utils.hpp create mode 100755 src/ClientPublisher.cpp create mode 100755 src/GLViewer.cpp create mode 100755 src/main.cpp diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..378eac2 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +build diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100755 index 0000000..b82ee2b --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,61 @@ +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) +PROJECT(ZED_BodyFusion) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +SET(CMAKE_BUILD_TYPE "Release") +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +option(LINK_SHARED_ZED "Link with the ZED SDK shared executable" ON) + +if (NOT LINK_SHARED_ZED AND MSVC) + message(FATAL_ERROR "LINK_SHARED_ZED OFF : ZED SDK static libraries not available on Windows") +endif() + +find_package(ZED REQUIRED) +find_package(CUDA REQUIRED) +find_package(GLUT REQUIRED) +find_package(GLEW REQUIRED) +SET(OpenGL_GL_PREFERENCE GLVND) +find_package(OpenGL REQUIRED) +find_package(OpenCV) + +include_directories(${CUDA_INCLUDE_DIRS}) +include_directories(${ZED_INCLUDE_DIRS}) +include_directories(${GLEW_INCLUDE_DIRS}) +include_directories(${GLUT_INCLUDE_DIR}) +include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) +include_directories(${OpenCV_INCLUDE_DIRS}) + +link_directories(${ZED_LIBRARY_DIR}) +link_directories(${CUDA_LIBRARY_DIRS}) +link_directories(${GLEW_LIBRARY_DIRS}) +link_directories(${GLUT_LIBRARY_DIRS}) +link_directories(${OpenGL_LIBRARY_DIRS}) +link_directories(${OpenCV_LIBRARY_DIRS}) + +IF(NOT WIN32) + SET(SPECIAL_OS_LIBS "pthread") + + IF (CMAKE_SYSTEM_PROCESSOR MATCHES aarch64) + add_definitions(-DJETSON_STYLE) + ENDIF() +ENDIF() + +FILE(GLOB_RECURSE SRC_FILES src/*.c*) +FILE(GLOB_RECURSE HDR_FILES include/*.h*) + +add_executable(${PROJECT_NAME} ${HDR_FILES} ${SRC_FILES}) + +if (LINK_SHARED_ZED) + SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY}) +else() + SET(ZED_LIBS ${ZED_STATIC_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_LIBRARY}) +endif() + +TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${ZED_LIBS} ${UTILS_LIB} ${SPECIAL_OS_LIBS} ${OpenCV_LIBRARIES} ${OPENGL_LIBRARIES} ${GLUT_LIBRARIES} ${GLEW_LIBRARIES}) + +if(INSTALL_SAMPLES) + LIST(APPEND SAMPLE_LIST ${PROJECT_NAME}) + SET(SAMPLE_LIST "${SAMPLE_LIST}" PARENT_SCOPE) +endif() diff --git a/ZED_SDK_ARCHITECTURE.md b/ZED_SDK_ARCHITECTURE.md new file mode 100644 index 0000000..b7c6a34 --- /dev/null +++ b/ZED_SDK_ARCHITECTURE.md @@ -0,0 +1,349 @@ +# ZED SDK Architecture: Streaming vs Fusion API + +## Overview + +The ZED SDK provides two distinct APIs for transmitting camera data over a network: + +1. **Streaming API** (`enableStreaming`) - Video streaming +2. **Fusion API** (`startPublishing`) - Metadata publishing + +These serve fundamentally different use cases and have different compute/bandwidth tradeoffs. + +## API Comparison + +| Feature | Streaming API | Fusion API | +|---------|---------------|------------| +| **Primary Use Case** | Remote camera access | Multi-camera data fusion | +| **Data Transmitted** | Compressed video (H264/H265) | Metadata only (bodies, objects, poses) | +| **Bandwidth per Camera** | 10-40 Mbps | <100 Kbps | +| **Edge Compute** | Video encoding only (NVENC) | Full depth NN + tracking + detection | +| **Host Compute** | Full depth NN + tracking + detection | Lightweight fusion only | +| **Synchronization** | None | Time-synced + geometric calibration | +| **360° Coverage** | No | Yes (fuses overlapping views) | +| **Receiver API** | `zed.open()` with `INPUT_TYPE::STREAM` | `fusion.subscribe()` | + +## Architecture Diagrams + +### Streaming API (Single Camera Remote Access) + +``` +┌─────────────────────┐ ┌─────────────────────┐ +│ Edge (Jetson) │ │ Host (Server) │ +│ │ │ │ +│ ┌───────────────┐ │ H264/H265 RTP │ ┌───────────────┐ │ +│ │ ZED Camera │ │ (10-40 Mbps) │ │ Decode │ │ +│ └───────┬───────┘ │ ───────────────────► │ │ (NVENC) │ │ +│ │ │ │ └───────┬───────┘ │ +│ ┌───────▼───────┐ │ │ │ │ +│ │ NVENC │ │ │ ┌───────▼───────┐ │ +│ │ Encode │──┘ │ │ Neural Depth │ │ +│ │ (hardware) │ │ │ (NN on GPU) │ │ +│ └───────────────┘ │ └───────┬───────┘ │ +│ │ │ │ +│ │ ┌───────▼───────┐ │ +│ │ │ Tracking / │ │ +│ │ │ Detection │ │ +│ │ └───────┬───────┘ │ +│ │ │ │ +│ │ ┌───────▼───────┐ │ +│ │ │ Point Cloud │ │ +│ │ └───────────────┘ │ +└─────────────────────┘ └─────────────────────┘ + +Edge: Lightweight (encode only) +Host: Heavy (NN depth + all processing) +``` + +### Fusion API (Multi-Camera 360° Coverage) + +``` +┌─────────────────────┐ +│ Edge #1 (Jetson) │ +│ ┌───────────────┐ │ +│ │ ZED Camera │ │ +│ └───────┬───────┘ │ Metadata Only +│ ┌───────▼───────┐ │ (bodies, poses) +│ │ Neural Depth │ │ (<100 Kbps) ┌─────────────────────┐ +│ │ (NN on GPU) │ │ ──────────────────────►│ │ +│ └───────┬───────┘ │ │ Fusion Server │ +│ ┌───────▼───────┐ │ │ │ +│ │ Body Track │──┘ │ ┌───────────────┐ │ +│ └───────────────┘ │ │ Subscribe │ │ +└─────────────────────┘ │ │ to all │ │ + │ │ cameras │ │ +┌─────────────────────┐ │ └───────┬───────┘ │ +│ Edge #2 (Jetson) │ │ │ │ +│ ┌───────────────┐ │ Metadata Only │ ┌───────▼───────┐ │ +│ │ ZED Camera │ │ ──────────────────────►│ │ Time Sync │ │ +│ └───────┬───────┘ │ │ │ + Geometric │ │ +│ ┌───────▼───────┐ │ │ │ Calibration │ │ +│ │ Neural Depth │ │ │ └───────┬───────┘ │ +│ └───────┬───────┘ │ │ │ │ +│ ┌───────▼───────┐ │ │ ┌───────▼───────┐ │ +│ │ Body Track │──┘ │ │ 360° Fusion │ │ +│ └───────────────┘ │ │ (merge views)│ │ +└─────────────────────┘ │ └───────────────┘ │ + │ │ +┌─────────────────────┐ │ Lightweight GPU │ +│ Edge #3 (Jetson) │ Metadata Only │ requirements │ +│ ... │ ──────────────────────►│ │ +└─────────────────────┘ └─────────────────────┘ + +Each Edge: Heavy (NN depth + tracking) +Fusion Server: Lightweight (data fusion only) +``` + +## Communication Modes + +### Streaming API + +| Mode | Description | +|------|-------------| +| **H264** | AVCHD encoding, wider GPU support | +| **H265** | HEVC encoding, better compression, requires Pascal+ GPU | + +Port: Even number (default 30000), uses RTP protocol. + +### Fusion API + +| Mode | Description | +|------|-------------| +| **INTRA_PROCESS** | Same machine, shared memory (zero-copy) | +| **LOCAL_NETWORK** | Different machines, RTP over network | + +Port: Default 30000, configurable per camera. + +## Bandwidth Requirements + +### Streaming (H265 Compressed Video) + +| Resolution | FPS | Bitrate per Camera | 4 Cameras | +|------------|-----|-------------------|-----------| +| 2K | 15 | 7 Mbps | 28 Mbps | +| HD1080 | 30 | 11 Mbps | 44 Mbps | +| HD720 | 60 | 6 Mbps | 24 Mbps | +| HD1200 | 30 | ~12 Mbps | ~48 Mbps | + +### Fusion (Metadata Only) + +| Data Type | Size per Frame | @ 30 FPS | 4 Cameras | +|-----------|---------------|----------|-----------| +| Body (18 keypoints) | ~2 KB | ~60 KB/s | ~240 KB/s | +| Object detection | ~1 KB | ~30 KB/s | ~120 KB/s | +| Pose/Transform | ~100 B | ~3 KB/s | ~12 KB/s | + +**Fusion uses 100-1000x less bandwidth than Streaming.** + +## The Architectural Gap + +### What You CAN Do + +| Scenario | API | Edge Computes | Host Receives | +|----------|-----|---------------|---------------| +| Remote camera access | Streaming | Video encoding | Video → computes depth/tracking | +| Multi-camera fusion | Fusion | Depth + tracking | Metadata only (bodies, poses) | +| Local processing | Direct | Everything | N/A (same machine) | + +### What You CANNOT Do + +**There is no ZED SDK mode for:** + +``` +┌─────────────────────┐ ┌─────────────────────┐ +│ Edge (Jetson) │ │ Host (Server) │ +│ │ │ │ +│ ┌───────────────┐ │ Depth Map / │ ┌───────────────┐ │ +│ │ ZED Camera │ │ Point Cloud │ │ Receive │ │ +│ └───────┬───────┘ │ │ │ Depth/PC │ │ +│ │ │ ??? │ └───────┬───────┘ │ +│ ┌───────▼───────┐ │ ─────────────────X─► │ │ │ +│ │ Neural Depth │ │ NOT SUPPORTED │ ┌───────▼───────┐ │ +│ │ (NN on GPU) │ │ │ │ Further │ │ +│ └───────┬───────┘ │ │ │ Processing │ │ +│ │ │ │ └───────────────┘ │ +│ ┌───────▼───────┐ │ │ │ +│ │ Point Cloud │──┘ │ │ +│ └───────────────┘ │ │ +└─────────────────────┘ └─────────────────────┘ + +❌ Edge computes depth → streams depth map → Host receives depth +❌ Edge computes point cloud → streams point cloud → Host receives point cloud +``` + +## Why This Architecture? + +### 1. Bandwidth Economics + +Point cloud streaming would require significantly more bandwidth than video: + +| Data Type | Size per Frame (HD1080) | @ 30 FPS | +|-----------|------------------------|----------| +| Raw stereo video | ~12 MB | 360 MB/s | +| H265 compressed | ~46 KB | 11 Mbps | +| Depth map (16-bit) | ~4 MB | 120 MB/s | +| Point cloud (XYZ float) | ~12 MB | 360 MB/s | + +Compressed depth/point cloud is lossy and still large (~50-100 Mbps). + +### 2. Compute Distribution Philosophy + +ZED SDK follows: **"Compute entirely at edge OR entirely at host, not split"** + +| Scenario | Solution | +|----------|----------| +| Low bandwidth, multi-camera | Fusion (edge computes all, sends metadata) | +| High bandwidth, single camera | Streaming (host computes all) | +| Same machine | INTRA_PROCESS (shared memory) | + +### 3. Fusion API Design Goals + +From Stereolabs documentation: + +> "The Fusion module is **lightweight** (in computation resources requirements) compared to the requirements for camera publishers." + +The Fusion receiver is intentionally lightweight because: +- It only needs to fuse pre-computed metadata +- It handles time synchronization and geometric calibration +- It can run on modest hardware while edges do heavy compute + +### 4. Product Strategy + +Stereolabs sells: +- **ZED cameras** (hardware) +- **ZED Box** (edge compute appliances) +- **ZED Hub** (cloud management) + +The Fusion API encourages purchasing ZED Boxes for edge compute rather than building custom streaming solutions. + +## Workarounds for Custom Point Cloud Streaming + +If you need to stream point clouds from edge to host (outside ZED SDK): + +### Option 1: Custom Compression + Streaming + +```cpp +// On edge: compute point cloud, compress, send +sl::Mat point_cloud; +zed.retrieveMeasure(point_cloud, MEASURE::XYZRGBA); + +// Compress with Draco/PCL octree +std::vector compressed = draco_compress(point_cloud); + +// Send via ZeroMQ/gRPC/raw UDP +socket.send(compressed); +``` + +### Option 2: Depth Map Streaming + +```cpp +// On edge: get depth, compress as 16-bit PNG, send +sl::Mat depth; +zed.retrieveMeasure(depth, MEASURE::DEPTH); + +// Compress as lossless PNG +cv::Mat depth_cv = slMat2cvMat(depth); +std::vector png; +cv::imencode(".png", depth_cv, png); + +// Send via network +socket.send(png); +``` + +### Bandwidth Estimate for Custom Streaming + +| Method | Compression | Bandwidth (HD1080@30fps) | +|--------|-------------|-------------------------| +| Depth PNG (lossless) | ~4:1 | ~240 Mbps | +| Depth JPEG (lossy) | ~20:1 | ~48 Mbps | +| Point cloud Draco | ~10:1 | ~100 Mbps | + +**10 Gbps Ethernet could handle 4 cameras with custom depth streaming.** + +## Recommendations + +| Use Case | Recommended API | +|----------|-----------------| +| Single camera, remote development | Streaming | +| Multi-camera body tracking | Fusion | +| Multi-camera 360° coverage | Fusion | +| Custom point cloud pipeline | Manual (ZeroMQ + Draco) | +| Low latency, same machine | INTRA_PROCESS | + +## Code Examples + +### Streaming Sender (Edge) + +```cpp +sl::StreamingParameters stream_params; +stream_params.codec = sl::STREAMING_CODEC::H265; +stream_params.bitrate = 12000; +stream_params.port = 30000; + +zed.enableStreaming(stream_params); + +while (running) { + zed.grab(); // Encodes and sends frame +} +``` + +### Streaming Receiver (Host) + +```cpp +sl::InitParameters init_params; +init_params.input.setFromStream("192.168.1.100", 30000); + +zed.open(init_params); + +while (running) { + if (zed.grab() == ERROR_CODE::SUCCESS) { + // Full ZED SDK available - depth, tracking, etc. + zed.retrieveMeasure(depth, MEASURE::DEPTH); + zed.retrieveMeasure(point_cloud, MEASURE::XYZRGBA); + } +} +``` + +### Fusion Sender (Edge) + +```cpp +// Enable body tracking +zed.enableBodyTracking(body_params); + +// Start publishing metadata +sl::CommunicationParameters comm_params; +comm_params.setForLocalNetwork(30000); +zed.startPublishing(comm_params); + +while (running) { + if (zed.grab() == ERROR_CODE::SUCCESS) { + zed.retrieveBodies(bodies); // Computes and publishes + } +} +``` + +### Fusion Receiver (Host) + +```cpp +sl::Fusion fusion; +fusion.init(init_fusion_params); + +sl::CameraIdentifier cam1(serial_number); +fusion.subscribe(cam1, comm_params, pose); + +while (running) { + if (fusion.process() == FUSION_ERROR_CODE::SUCCESS) { + fusion.retrieveBodies(fused_bodies); // Already computed by edges + } +} +``` + +## Summary + +The ZED SDK architecture forces a choice: + +1. **Streaming**: Edge sends video → Host computes depth (NN inference on host) +2. **Fusion**: Edge computes depth → Sends metadata only (no point cloud) + +There is **no built-in support** for streaming computed depth maps or point clouds from edge to host. This is by design for bandwidth efficiency and to encourage use of ZED Box edge compute products. + +For custom depth/point cloud streaming, you must implement your own compression and network layer outside the ZED SDK. diff --git a/include/ClientPublisher.hpp b/include/ClientPublisher.hpp new file mode 100755 index 0000000..feeb5dd --- /dev/null +++ b/include/ClientPublisher.hpp @@ -0,0 +1,74 @@ +#ifndef __SENDER_RUNNER_HDR__ +#define __SENDER_RUNNER_HDR__ + +#include +#include + +#include +#include + +// Hard-coded camera stream configuration from inside_network.json +struct CameraStreamConfig { + int serial_number; + std::string ip_address; + int port; +}; + +// Predefined camera stream configurations +const std::vector CAMERA_STREAM_CONFIGS = { + {44289123, "192.168.128.2", 30000}, + {44435674, "192.168.128.2", 30002}, + {41831756, "192.168.128.2", 30004}, + {46195029, "192.168.128.2", 30006} +}; + +struct Trigger{ + + void notifyZED(){ + + cv.notify_all(); + + if(running){ + bool wait_for_zed = true; + const int nb_zed = states.size(); + while(wait_for_zed){ + int count_r = 0; + for(auto &it:states) + count_r += it.second; + wait_for_zed = count_r != nb_zed; + sl::sleep_ms(1); + } + for(auto &it:states) + it.second = false; + } + } + + std::condition_variable cv; + bool running = true; + std::map states; +}; + +class ClientPublisher{ + +public: + ClientPublisher(); + ~ClientPublisher(); + + // Open camera from stream (IP:port) instead of local input + bool open(const std::string& ip, int port, Trigger* ref, int sdk_gpu_id); + void start(); + void stop(); + void setStartSVOPosition(unsigned pos); + + int getSerialNumber() const { return serial; } + +private: + sl::Camera zed; + void work(); + std::thread runner; + int serial; + std::mutex mtx; + Trigger *p_trigger; +}; + +#endif // ! __SENDER_RUNNER_HDR__ diff --git a/include/GLViewer.hpp b/include/GLViewer.hpp new file mode 100755 index 0000000..f3727b5 --- /dev/null +++ b/include/GLViewer.hpp @@ -0,0 +1,336 @@ +#ifndef __VIEWER_INCLUDE__ +#define __VIEWER_INCLUDE__ + +#include + +#include +#include + +#include +#include + +#include +#include + +#ifndef M_PI +#define M_PI 3.141592653f +#endif + +#define MOUSE_R_SENSITIVITY 0.03f +#define MOUSE_UZ_SENSITIVITY 0.75f +#define MOUSE_DZ_SENSITIVITY 1.25f +#define MOUSE_T_SENSITIVITY 0.05f +#define KEY_T_SENSITIVITY 0.1f + + +/////////////////////////////////////////////////////////////////////////////////////////////// + +class Shader { +public: + + Shader() : verterxId_(0), fragmentId_(0), programId_(0) {} + Shader(const GLchar* vs, const GLchar* fs); + ~Shader(); + + // Delete the move constructor and move assignment operator + Shader(Shader&&) = delete; + Shader& operator=(Shader&&) = delete; + + // Delete the copy constructor and copy assignment operator + Shader(const Shader&) = delete; + Shader& operator=(const Shader&) = delete; + + void set(const GLchar* vs, const GLchar* fs); + GLuint getProgramId(); + + static const GLint ATTRIB_VERTICES_POS = 0; + static const GLint ATTRIB_COLOR_POS = 1; + static const GLint ATTRIB_NORMAL = 2; +private: + bool compile(GLuint &shaderId, GLenum type, const GLchar* src); + GLuint verterxId_; + GLuint fragmentId_; + GLuint programId_; +}; + +struct ShaderData { + Shader it; + GLuint MVP_Mat; +}; + +class Simple3DObject { +public: + + Simple3DObject(); + + ~Simple3DObject(); + + void addPoint(sl::float3 pt, sl::float3 clr); + void addLine(sl::float3 pt1, sl::float3 pt2, sl::float3 clr); + void addFace(sl::float3 p1, sl::float3 p2, sl::float3 p3, sl::float3 clr); + void pushToGPU(); + void clear(); + + void setStatic(bool _static) { + isStatic_ = _static; + } + + void setDrawingType(GLenum type); + + void draw(); + +private: + std::vector vertices_; + std::vector colors_; + std::vector indices_; + + bool isStatic_; + bool need_update; + GLenum drawingType_; + GLuint vaoID_; + GLuint vboID_[3]; +}; + +class CameraGL { +public: + + CameraGL() { + } + + enum DIRECTION { + UP, DOWN, LEFT, RIGHT, FORWARD, BACK + }; + CameraGL(sl::Translation position, sl::Translation direction, sl::Translation vertical = sl::Translation(0, 1, 0)); // vertical = Eigen::Vector3f(0, 1, 0) + ~CameraGL(); + + void update(); + void setProjection(float horizontalFOV, float verticalFOV, float znear, float zfar); + const sl::Transform& getViewProjectionMatrix() const; + + float getHorizontalFOV() const; + float getVerticalFOV() const; + + // Set an offset between the eye of the camera and its position + // Note: Useful to use the camera as a trackball camera with z>0 and x = 0, y = 0 + // Note: coordinates are in local space + void setOffsetFromPosition(const sl::Translation& offset); + const sl::Translation& getOffsetFromPosition() const; + + void setDirection(const sl::Translation& direction, const sl::Translation &vertical); + void translate(const sl::Translation& t); + void setPosition(const sl::Translation& p); + void rotate(const sl::Orientation& rot); + void rotate(const sl::Rotation& m); + void setRotation(const sl::Orientation& rot); + void setRotation(const sl::Rotation& m); + + const sl::Translation& getPosition() const; + const sl::Translation& getForward() const; + const sl::Translation& getRight() const; + const sl::Translation& getUp() const; + const sl::Translation& getVertical() const; + float getZNear() const; + float getZFar() const; + + static const sl::Translation ORIGINAL_FORWARD; + static const sl::Translation ORIGINAL_UP; + static const sl::Translation ORIGINAL_RIGHT; + + sl::Transform projection_; + bool usePerspective_; +private: + void updateVectors(); + void updateView(); + void updateVPMatrix(); + + sl::Translation offset_; + sl::Translation position_; + sl::Translation forward_; + sl::Translation up_; + sl::Translation right_; + sl::Translation vertical_; + + sl::Orientation rotation_; + + sl::Transform view_; + sl::Transform vpMatrix_; + float horizontalFieldOfView_; + float verticalFieldOfView_; + float znear_; + float zfar_; +}; + + +class PointCloud { +public: + PointCloud(); + ~PointCloud(); + + // Initialize Opengl and Cuda buffers + // Warning: must be called in the Opengl thread + void initialize(sl::Mat&, sl::float3 clr); + // Push a new point cloud + // Warning: can be called from any thread but the mutex "mutexData" must be locked + void pushNewPC(); + // Draw the point cloud + // Warning: must be called in the Opengl thread + void draw(const sl::Transform& vp, bool draw_clr); + // Close (disable update) + void close(); + +private: + sl::Mat refMat; + sl::float3 clr; + + Shader shader_; + GLuint shMVPMatrixLoc_; + GLuint shDrawColor; + GLuint shColor; + + size_t numBytes_; + float* xyzrgbaMappedBuf_; + GLuint bufferGLID_; + cudaGraphicsResource* bufferCudaID_; +}; + +class CameraViewer { +public: + CameraViewer(); + ~CameraViewer(); + + // Initialize Opengl and Cuda buffers + bool initialize(sl::Mat& image, sl::float3 clr); + // Push a new Image + Z buffer and transform into a point cloud + void pushNewImage(); + // Draw the Image + void draw(sl::Transform vpMatrix); + // Close (disable update) + void close(); + + Simple3DObject frustum; +private: + sl::Mat ref; + cudaArray_t ArrIm; + cudaGraphicsResource* cuda_gl_ressource;//cuda GL resource + Shader shader; + GLuint shMVPMatrixLocTex_; + + GLuint texture; + GLuint vaoID_; + GLuint vboID_[3]; + + std::vector faces; + std::vector vert; + std::vector uv; +}; + +struct ObjectClassName { + sl::float3 position; + std::string name_lineA; + std::string name_lineB; + sl::float3 color; +}; + +// This class manages input events, window and Opengl rendering pipeline + +class GLViewer { +public: + GLViewer(); + ~GLViewer(); + bool isAvailable(); + + void init(int argc, char **argv); + + void updateCamera(int, sl::Mat &, sl::Mat &); + void updateCamera(sl::Mat &); + + void updateBodies(sl::Bodies &objs,std::map& singledata, sl::FusionMetrics& metrics); + + void setCameraPose(int, sl::Transform); + + int getKey() { + const int key = last_key; + last_key = -1; + return key; + } + + void exit(); +private: + void render(); + void update(); + void draw(); + void clearInputs(); + void setRenderCameraProjection(sl::CameraParameters params, float znear, float zfar); + + void printText(); + + // Glut functions callbacks + static void drawCallback(); + static void mouseButtonCallback(int button, int state, int x, int y); + static void mouseMotionCallback(int x, int y); + static void reshapeCallback(int width, int height); + static void keyPressedCallback(unsigned char c, int x, int y); + static void keyReleasedCallback(unsigned char c, int x, int y); + static void idle(); + + sl::float3 getColor(int, bool); + + bool available; + bool drawBbox = false; + + enum MOUSE_BUTTON { + LEFT = 0, + MIDDLE = 1, + RIGHT = 2, + WHEEL_UP = 3, + WHEEL_DOWN = 4 + }; + + enum KEY_STATE { + UP = 'u', + DOWN = 'd', + FREE = 'f' + }; + + unsigned char lastPressedKey; + + bool mouseButton_[3]; + int mouseWheelPosition_; + int mouseCurrentPosition_[2]; + int mouseMotion_[2]; + int previousMouseMotion_[2]; + KEY_STATE keyStates_[256]; + + std::mutex mtx; + std::mutex mtx_clr; + + ShaderData shader; + + sl::Transform projection_; + sl::float3 bckgrnd_clr; + + std::map point_clouds; + std::map viewers; + std::map poses; + + std::map skeletons_raw; + std::map colors; + std::map colors_sk; + + std::vector fusionStats; + + CameraGL camera_; + Simple3DObject skeletons; + Simple3DObject floor_grid; + + bool show_pc = true; + bool show_raw = false; + bool draw_flat_color = false; + + std::uniform_int_distribution uint_dist360; + std::mt19937 rng; + + int last_key = -1; +}; + +#endif /* __VIEWER_INCLUDE__ */ diff --git a/include/utils.hpp b/include/utils.hpp new file mode 100755 index 0000000..776cf1f --- /dev/null +++ b/include/utils.hpp @@ -0,0 +1,56 @@ +#pragma once + +#include + +/** +* @brief Compute the start frame of each SVO for playback to be synced +* +* @param svo_files Map camera index to SVO file path +* @return Map camera index to starting SVO frame for synced playback +*/ +std::map syncDATA(std::map svo_files) { + std::map output; // map of camera index and frame index of the starting point for each + + // Open all SVO + std::map> p_zeds; + + for (auto &it : svo_files) { + auto p_zed = std::make_shared(); + + sl::InitParameters init_param; + init_param.depth_mode = sl::DEPTH_MODE::NONE; + init_param.camera_disable_self_calib = true; + init_param.input.setFromSVOFile(it.second.c_str()); + + auto error = p_zed->open(init_param); + if (error == sl::ERROR_CODE::SUCCESS) + p_zeds.insert(std::make_pair(it.first, p_zed)); + else { + std::cerr << "Could not open file " << it.second.c_str() << ": " << sl::toString(error) << ". Skipping" << std::endl; + } + } + + // Compute the starting point, we have to take the latest one + sl::Timestamp start_ts = 0; + for (auto &it : p_zeds) { + it.second->grab(); + auto ts = it.second->getTimestamp(sl::TIME_REFERENCE::IMAGE); + + if (ts > start_ts) + start_ts = ts; + } + + std::cout << "Found SVOs common starting time: " << start_ts << std::endl; + + // The starting point is now known, let's find the frame idx for all corresponding + for (auto &it : p_zeds) { + auto frame_position_at_ts = it.second->getSVOPositionAtTimestamp(start_ts); + + if (frame_position_at_ts != -1) + output.insert(std::make_pair(it.first, frame_position_at_ts)); + } + + for (auto &it : p_zeds) it.second->close(); + + return output; +} diff --git a/src/ClientPublisher.cpp b/src/ClientPublisher.cpp new file mode 100755 index 0000000..b8dd964 --- /dev/null +++ b/src/ClientPublisher.cpp @@ -0,0 +1,104 @@ +#include "ClientPublisher.hpp" + +ClientPublisher::ClientPublisher() { } + +ClientPublisher::~ClientPublisher() +{ + zed.close(); +} + +bool ClientPublisher::open(const std::string& ip, int port, Trigger* ref, int sdk_gpu_id) { + + p_trigger = ref; + + sl::InitParameters init_parameters; + init_parameters.depth_mode = sl::DEPTH_MODE::NEURAL_PLUS; + // Use stream input instead of local camera + init_parameters.input.setFromStream(sl::String(ip.c_str()), port); + init_parameters.coordinate_units = sl::UNIT::METER; + init_parameters.depth_stabilization = 30; + init_parameters.sdk_gpu_id = sdk_gpu_id; + + auto state = zed.open(init_parameters); + if (state != sl::ERROR_CODE::SUCCESS) + { + std::cout << "Error opening stream " << ip << ":" << port << " - " << state << std::endl; + return false; + } + + serial = zed.getCameraInformation().serial_number; + p_trigger->states[serial] = false; + + // in most cases in body tracking setup, the cameras are static + sl::PositionalTrackingParameters positional_tracking_parameters; + // in most cases for body detection application the camera is static: + positional_tracking_parameters.set_as_static = true; + + state = zed.enablePositionalTracking(positional_tracking_parameters); + if (state != sl::ERROR_CODE::SUCCESS) + { + std::cout << "Error enabling positional tracking: " << state << std::endl; + return false; + } + + // define the body tracking parameters, as the fusion can does the tracking and fitting you don't need to enable them here, unless you need it for your app + sl::BodyTrackingParameters body_tracking_parameters; + body_tracking_parameters.detection_model = sl::BODY_TRACKING_MODEL::HUMAN_BODY_ACCURATE; + body_tracking_parameters.body_format = sl::BODY_FORMAT::BODY_18; + body_tracking_parameters.enable_body_fitting = false; + body_tracking_parameters.enable_tracking = false; + state = zed.enableBodyTracking(body_tracking_parameters); + if (state != sl::ERROR_CODE::SUCCESS) + { + std::cout << "Error enabling body tracking: " << state << std::endl; + return false; + } + + return true; +} + +void ClientPublisher::start() +{ + if (zed.isOpened()) { + // the camera should stream its data so the fusion can subscibe to it to gather the detected body and others metadata needed for the process. + zed.startPublishing(); + // the thread can start to process the camera grab in background + runner = std::thread(&ClientPublisher::work, this); + } +} + +void ClientPublisher::stop() +{ + if (runner.joinable()) + runner.join(); + zed.close(); +} + +void ClientPublisher::work() +{ + sl::Bodies bodies; + sl::BodyTrackingRuntimeParameters body_runtime_parameters; + body_runtime_parameters.detection_confidence_threshold = 40; + zed.setBodyTrackingRuntimeParameters(body_runtime_parameters); + + sl::RuntimeParameters rt; + rt.confidence_threshold = 50; + + // In this sample we use a dummy thread to process the ZED data. + // you can replace it by your own application and use the ZED like you use to, retrieve its images, depth, sensors data and so on. + // As long as you call the grab method, since the camera is subscribed to fusion it will run the detection and + // the camera will be able to seamlessly transmit the data to the fusion module. + while (p_trigger->running) { + std::unique_lock lk(mtx); + p_trigger->cv.wait(lk); + if (p_trigger->running) { + if (zed.grab(rt) == sl::ERROR_CODE::SUCCESS) { + } + } + p_trigger->states[serial] = true; + } +} + +void ClientPublisher::setStartSVOPosition(unsigned pos) { + zed.setSVOPosition(pos); +} diff --git a/src/GLViewer.cpp b/src/GLViewer.cpp new file mode 100755 index 0000000..66a7526 --- /dev/null +++ b/src/GLViewer.cpp @@ -0,0 +1,1161 @@ +#include "GLViewer.hpp" + +const GLchar* VERTEX_SHADER = + "#version 330 core\n" + "layout(location = 0) in vec3 in_Vertex;\n" + "layout(location = 1) in vec3 in_Color;\n" + "uniform mat4 u_mvpMatrix;\n" + "out vec3 b_color;\n" + "void main() {\n" + " b_color = in_Color.bgr;\n" + " gl_Position = u_mvpMatrix * vec4(in_Vertex, 1);\n" + "}"; + +const GLchar* FRAGMENT_SHADER = + "#version 330 core\n" + "in vec3 b_color;\n" + "layout(location = 0) out vec4 color;\n" + "void main() {\n" + " color = vec4(b_color, 0.95);\n" + "}"; + + +const GLchar* POINTCLOUD_VERTEX_SHADER = + "#version 330 core\n" + "layout(location = 0) in vec4 in_VertexRGBA;\n" + "out vec4 b_color;\n" + "uniform mat4 u_mvpMatrix;\n" + "uniform vec3 u_color;\n" + "uniform bool u_drawFlat;\n" + "void main() {\n" + " if(u_drawFlat)\n" + " b_color = vec4(u_color.bgr, .85f);\n" + "else{" + // Decompose the 4th channel of the XYZRGBA buffer to retrieve the color of the point (1float to 4uint) + " uint vertexColor = floatBitsToUint(in_VertexRGBA.w); \n" + " vec3 clr_int = vec3((vertexColor & uint(0x000000FF)), (vertexColor & uint(0x0000FF00)) >> 8, (vertexColor & uint(0x00FF0000)) >> 16);\n" + " b_color = vec4(clr_int.b / 255.0f, clr_int.g / 255.0f, clr_int.r / 255.0f, .85f);\n" + " }" + " gl_Position = u_mvpMatrix * vec4(in_VertexRGBA.xyz, 1);\n" + "}"; + +const GLchar* POINTCLOUD_FRAGMENT_SHADER = + "#version 330 core\n" + "in vec4 b_color;\n" + "layout(location = 0) out vec4 out_Color;\n" + "void main() {\n" + " out_Color = b_color;\n" + "}"; + +const GLchar* VERTEX_SHADER_TEXTURE = + "#version 330 core\n" + "layout(location = 0) in vec3 in_Vertex;\n" + "layout(location = 1) in vec2 in_UVs;\n" + "uniform mat4 u_mvpMatrix;\n" + "out vec2 UV;\n" + "void main() {\n" + " gl_Position = u_mvpMatrix * vec4(in_Vertex, 1);\n" + " UV = in_UVs;\n" + "}\n"; + +const GLchar* FRAGMENT_SHADER_TEXTURE = + "#version 330 core\n" + "in vec2 UV;\n" + "uniform sampler2D texture_sampler;\n" + "void main() {\n" + " gl_FragColor = vec4(texture(texture_sampler, UV).bgr, 1.0);\n" + "}\n"; + + +GLViewer* currentInstance_ = nullptr; + +GLViewer::GLViewer() : available(false) { + currentInstance_ = this; + mouseButton_[0] = mouseButton_[1] = mouseButton_[2] = false; + clearInputs(); + previousMouseMotion_[0] = previousMouseMotion_[1] = 0; +} + +GLViewer::~GLViewer() { +} + +void GLViewer::exit() { + if (currentInstance_ && available) { + available = false; + for (auto& pc : point_clouds) + pc.second.close(); + for(auto& v : viewers) + v.second.close(); + } +} + +bool GLViewer::isAvailable() { + if(available) + glutMainLoopEvent(); + return available; +} + +void CloseFunc(void) { + if (currentInstance_) currentInstance_->exit(); +} + +void addVert(Simple3DObject &obj, float i_f, float limit, float height, sl::float4 &clr) { + auto p1 = sl::float3(i_f, height, -limit); + auto p2 = sl::float3(i_f, height, limit); + auto p3 = sl::float3(-limit, height, i_f); + auto p4 = sl::float3(limit, height, i_f); + + obj.addLine(p1, p2, clr); + obj.addLine(p3, p4, clr); +} + +void GLViewer::init(int argc, char **argv) { + + glutInit(&argc, argv); + int wnd_w = glutGet(GLUT_SCREEN_WIDTH); + int wnd_h = glutGet(GLUT_SCREEN_HEIGHT); + + glutInitWindowSize(1200, 700); + glutInitWindowPosition(wnd_w * 0.05, wnd_h * 0.05); + glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH); + + glutCreateWindow("ZED| 3D View"); + + GLenum err = glewInit(); + if (GLEW_OK != err) + std::cout << "ERROR: glewInit failed: " << glewGetErrorString(err) << "\n"; + + glutSetOption(GLUT_ACTION_ON_WINDOW_CLOSE, GLUT_ACTION_CONTINUE_EXECUTION); + + glEnable(GL_DEPTH_TEST); + glEnable(GL_TEXTURE_2D); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + +#ifndef JETSON_STYLE + glEnable(GL_LINE_SMOOTH); +#endif + + // Compile and create the shader for 3D objects + shader.it.set(VERTEX_SHADER, FRAGMENT_SHADER); + shader.MVP_Mat = glGetUniformLocation(shader.it.getProgramId(), "u_mvpMatrix"); + + // Create the camera + camera_ = CameraGL(sl::Translation(0, 3, 3), sl::Translation(0, 0, -1)); + sl::float3 ea(-30,0,0); + sl::Rotation rot; + rot.setEulerAngles(ea, false); + camera_.rotate(rot); + + // Create the skeletons objects + skeletons.setDrawingType(GL_LINES); + floor_grid.setDrawingType(GL_LINES); + + // Set background color (black) + bckgrnd_clr = sl::float4(0.2f, 0.19f, 0.2f, 1.0f); + + + float limit = 20.0f; + sl::float4 clr_grid(80, 80, 80, 255); + clr_grid /= 255.f; + + float grid_height = -0; + for (int i = (int) (-limit); i <= (int) (limit); i++) + addVert(floor_grid, i, limit, grid_height, clr_grid); + + floor_grid.pushToGPU(); + + std::random_device dev; + rng = std::mt19937(dev()); + rng.seed(42); + uint_dist360 = std::uniform_int_distribution(0, 360); + + // Map glut function on this class methods + glutDisplayFunc(GLViewer::drawCallback); + glutMouseFunc(GLViewer::mouseButtonCallback); + glutMotionFunc(GLViewer::mouseMotionCallback); + glutReshapeFunc(GLViewer::reshapeCallback); + glutKeyboardFunc(GLViewer::keyPressedCallback); + glutKeyboardUpFunc(GLViewer::keyReleasedCallback); + glutCloseFunc(CloseFunc); + + available = true; +} + +sl::float3 newColor(float hh) { + float s = 0.75f; + float v = 0.9f; + + sl::float3 clr; + int i = (int)hh; + float ff = hh - i; + float p = v * (1.f - s); + float q = v * (1.f - (s * ff)); + float t = v * (1.f - (s * (1.f - ff))); + switch (i) { + case 0: + clr.r = v; + clr.g = t; + clr.b = p; + break; + case 1: + clr.r = q; + clr.g = v; + clr.b = p; + break; + case 2: + clr.r = p; + clr.g = v; + clr.b = t; + break; + + case 3: + clr.r = p; + clr.g = q; + clr.b = v; + break; + case 4: + clr.r = t; + clr.g = p; + clr.b = v; + break; + case 5: + default: + clr.r = v; + clr.g = p; + clr.b = q; + break; + } + return clr; +} + +sl::float3 GLViewer::getColor(int id, bool for_skeleton){ + const std::lock_guard lock(mtx_clr); + if(for_skeleton){ + if (colors_sk.find(id) == colors_sk.end()) { + float hh = uint_dist360(rng) / 60.f; + colors_sk[id] = newColor(hh); + } + return colors_sk[id]; + }else{ + if (colors.find(id) == colors.end()) { + int h_ = uint_dist360(rng); + float hh = h_ / 60.f; + colors[id] = newColor(hh); + } + return colors[id]; + } +} + +void GLViewer::updateCamera(int id, sl::Mat &view, sl::Mat &pc){ + const std::lock_guard lock(mtx); + auto clr = getColor(id, false); + if(view.isInit() && viewers.find(id) == viewers.end()) + viewers[id].initialize(view, clr); + + if(pc.isInit() && point_clouds.find(id) == point_clouds.end()) + point_clouds[id].initialize(pc, clr); + +} + +void GLViewer::updateCamera(sl::Mat &pc){ + const std::lock_guard lock(mtx); + int id = 0; + auto clr = getColor(id, false); + + // we need to release old pc and initialize new one because fused point cloud don't have the same number of points for each process + // I used close but it crashed in draw. Not yet investigated + point_clouds[id].initialize(pc, clr); +} + + +void GLViewer::setRenderCameraProjection(sl::CameraParameters params, float znear, float zfar) { + // Just slightly up the ZED camera FOV to make a small black border + float fov_y = (params.v_fov + 0.5f) * M_PI / 180.f; + float fov_x = (params.h_fov + 0.5f) * M_PI / 180.f; + + projection_(0, 0) = 1.0f / tanf(fov_x * 0.5f); + projection_(1, 1) = 1.0f / tanf(fov_y * 0.5f); + projection_(2, 2) = -(zfar + znear) / (zfar - znear); + projection_(3, 2) = -1; + projection_(2, 3) = -(2.f * zfar * znear) / (zfar - znear); + projection_(3, 3) = 0; + + projection_(0, 0) = 1.0f / tanf(fov_x * 0.5f); //Horizontal FoV. + projection_(0, 1) = 0; + projection_(0, 2) = 2.0f * ((params.image_size.width - 1.0f * params.cx) / params.image_size.width) - 1.0f; //Horizontal offset. + projection_(0, 3) = 0; + + projection_(1, 0) = 0; + projection_(1, 1) = 1.0f / tanf(fov_y * 0.5f); //Vertical FoV. + projection_(1, 2) = -(2.0f * ((params.image_size.height - 1.0f * params.cy) / params.image_size.height) - 1.0f); //Vertical offset. + projection_(1, 3) = 0; + + projection_(2, 0) = 0; + projection_(2, 1) = 0; + projection_(2, 2) = -(zfar + znear) / (zfar - znear); //Near and far planes. + projection_(2, 3) = -(2.0f * zfar * znear) / (zfar - znear); //Near and far planes. + + projection_(3, 0) = 0; + projection_(3, 1) = 0; + projection_(3, 2) = -1; + projection_(3, 3) = 0.0f; +} + +void GLViewer::render() { + if (available) { + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glClearColor(bckgrnd_clr.r, bckgrnd_clr.g, bckgrnd_clr.b, 1.f); + update(); + draw(); + printText(); + glutSwapBuffers(); + glutPostRedisplay(); + } +} + +void GLViewer::setCameraPose(int id, sl::Transform pose) { + const std::lock_guard lock(mtx); + auto clr = getColor(id, false); + poses[id] = pose; + + if(viewers.find(id) == viewers.end()){ + sl::Mat view; + viewers[id].initialize(view, clr); + } +} + +inline bool renderBody(const sl::BodyData& i, const bool isTrackingON) { + if (isTrackingON) + return (i.tracking_state == sl::OBJECT_TRACKING_STATE::OK); + else + return (i.tracking_state == sl::OBJECT_TRACKING_STATE::OK || i.tracking_state == sl::OBJECT_TRACKING_STATE::OFF); +} + +template +void createSKPrimitive(sl::BodyData& body, const std::vector>& map, Simple3DObject& skp, sl::float3 clr_id) { + for (auto& limb : map) { + sl::float3 kp_1 = body.keypoint[getIdx(limb.first)]; + sl::float3 kp_2 = body.keypoint[getIdx(limb.second)]; + if (std::isfinite(kp_1.norm()) && std::isfinite(kp_2.norm())) + skp.addLine(kp_1, kp_2, clr_id); + } +} + +void addSKeleton(sl::BodyData& obj, Simple3DObject& simpleObj, sl::float3 clr_id) { + switch (obj.keypoint.size()) { + case 18: + createSKPrimitive(obj, sl::BODY_18_BONES, simpleObj, clr_id); + break; + case 34: + createSKPrimitive(obj, sl::BODY_34_BONES, simpleObj, clr_id); + break; + case 38: + createSKPrimitive(obj, sl::BODY_38_BONES, simpleObj, clr_id); + break; + } +} + +void GLViewer::updateBodies(sl::Bodies &bodies, std::map& singledata, sl::FusionMetrics& metrics) { + const std::lock_guard lock(mtx); + + if (bodies.is_new) { + skeletons.clear(); + for(auto &it:bodies.body_list) { + auto clr = getColor(it.id, true); + if (renderBody(it, bodies.is_tracked)) + addSKeleton(it, skeletons, clr); + } + } + + fusionStats.clear(); + int id = 0; + + ObjectClassName obj_str; + obj_str.name_lineA = "Publishers :" + std::to_string(metrics.mean_camera_fused); + obj_str.name_lineB = "Sync :" + std::to_string(metrics.mean_stdev_between_camera * 1000.f); + obj_str.color = sl::float4(0.9,0.9,0.9,1); + obj_str.position = sl::float3(10, (id * 30), 0); + fusionStats.push_back(obj_str); + + for (auto &it : singledata) { + auto clr = getColor(it.first.sn, false); + id++; + if (it.second.is_new) + { + auto& sk_r = skeletons_raw[it.first.sn]; + sk_r.clear(); + sk_r.setDrawingType(GL_LINES); + + for (auto& sk : it.second.body_list) { + if(renderBody(sk, it.second.is_tracked)) + addSKeleton(sk, sk_r, clr); + } + } + + ObjectClassName obj_str; + obj_str.name_lineA = "CAM: " + std::to_string(it.first.sn) + " FPS: " + std::to_string(metrics.camera_individual_stats[it.first].received_fps); + obj_str.name_lineB = "Ratio Detection :" + std::to_string(metrics.camera_individual_stats[it.first].ratio_detection) + " Delta " + std::to_string(metrics.camera_individual_stats[it.first].delta_ts * 1000.f); + obj_str.color = clr; + obj_str.position = sl::float3(10, (id * 30), 0); + fusionStats.push_back(obj_str); + } +} + +void GLViewer::update() { + + if (keyStates_['q'] == KEY_STATE::UP || keyStates_['Q'] == KEY_STATE::UP || keyStates_[27] == KEY_STATE::UP) { + currentInstance_->exit(); + return; + } + + if (keyStates_['r'] == KEY_STATE::UP) + currentInstance_->show_raw = !currentInstance_->show_raw; + + if (keyStates_['c'] == KEY_STATE::UP) + currentInstance_->draw_flat_color = !currentInstance_->draw_flat_color; + + if (keyStates_['p'] == KEY_STATE::UP) + currentInstance_->show_pc = !currentInstance_->show_pc; + + // Rotate camera with mouse + if (mouseButton_[MOUSE_BUTTON::LEFT]) { + camera_.rotate(sl::Rotation((float) mouseMotion_[1] * MOUSE_R_SENSITIVITY, camera_.getRight())); + camera_.rotate(sl::Rotation((float) mouseMotion_[0] * MOUSE_R_SENSITIVITY, camera_.getVertical() * -1.f)); + } + + // Translate camera with mouse + if (mouseButton_[MOUSE_BUTTON::RIGHT]) { + camera_.translate(camera_.getUp() * (float) mouseMotion_[1] * MOUSE_T_SENSITIVITY); + camera_.translate(camera_.getRight() * (float) mouseMotion_[0] * MOUSE_T_SENSITIVITY); + } + + // Zoom in with mouse wheel + if (mouseWheelPosition_ != 0) { + //float distance = sl::Translation(camera_.getOffsetFromPosition()).norm(); + if (mouseWheelPosition_ > 0 /* && distance > camera_.getZNear()*/) { // zoom + camera_.translate(camera_.getForward() * MOUSE_UZ_SENSITIVITY * 0.5f * -1); + } else if (/*distance < camera_.getZFar()*/ mouseWheelPosition_ < 0) {// unzoom + //camera_.setOffsetFromPosition(camera_.getOffsetFromPosition() * MOUSE_DZ_SENSITIVITY); + camera_.translate(camera_.getForward() * MOUSE_UZ_SENSITIVITY * 0.5f); + } + } + + camera_.update(); + const std::lock_guard lock(mtx); + // Update point cloud buffers + skeletons.pushToGPU(); + for(auto &it: skeletons_raw) + it.second.pushToGPU(); + + // Update point cloud buffers + for(auto &it: point_clouds) + it.second.pushNewPC(); + + for(auto &it: viewers) + it.second.pushNewImage(); + clearInputs(); +} + + +void GLViewer::draw() { + + glPolygonMode(GL_FRONT, GL_LINE); + glPolygonMode(GL_BACK, GL_LINE); + glLineWidth(2.f); + glPointSize(1.f); + + sl::Transform vpMatrix = camera_.getViewProjectionMatrix(); + glUseProgram(shader.it.getProgramId()); + glUniformMatrix4fv(shader.MVP_Mat, 1, GL_TRUE, vpMatrix.m); + + floor_grid.draw(); + + skeletons.draw(); + + if (show_raw){ + glLineWidth(1.f); + for (auto& it : skeletons_raw) + it.second.draw(); + glLineWidth(2.f); + } + + for (auto& it : viewers) { + sl::Transform pose_ = vpMatrix * poses[it.first]; + glUniformMatrix4fv(shader.MVP_Mat, 1, GL_FALSE, sl::Transform::transpose(pose_).m); + it.second.frustum.draw(); + } + + glUseProgram(0); + + for (auto& it : poses) { + sl::Transform vpMatrix_world = vpMatrix * it.second; + + if(show_pc) + if(point_clouds.find(it.first) != point_clouds.end()) + point_clouds[it.first].draw(vpMatrix_world, draw_flat_color); + + if (viewers.find(it.first) != viewers.end()) + viewers[it.first].draw(vpMatrix_world); + } +} + +sl::float2 compute3Dprojection(sl::float3 &pt, const sl::Transform &cam, sl::Resolution wnd_size) { + sl::float4 pt4d(pt.x, pt.y, pt.z, 1.); + auto proj3D_cam = pt4d * cam; + sl::float2 proj2D; + proj2D.x = ((proj3D_cam.x / pt4d.w) * wnd_size.width) / (2.f * proj3D_cam.w) + wnd_size.width / 2.f; + proj2D.y = ((proj3D_cam.y / pt4d.w) * wnd_size.height) / (2.f * proj3D_cam.w) + wnd_size.height / 2.f; + return proj2D; +} + +void GLViewer::printText() { + + sl::Resolution wnd_size(glutGet(GLUT_WINDOW_WIDTH), glutGet(GLUT_WINDOW_HEIGHT)); + for (auto &it : fusionStats) { +#if 0 + auto pt2d = compute3Dprojection(it.position, projection_, wnd_size); +#else + sl::float2 pt2d(it.position.x, it.position.y); +#endif + glColor4f(it.color.b, it.color.g, it.color.r, .85f); + const auto *string = it.name_lineA.c_str(); + glWindowPos2f(pt2d.x, pt2d.y + 15); + int len = (int) strlen(string); + for (int i = 0; i < len; i++) + glutBitmapCharacter(GLUT_BITMAP_HELVETICA_12, string[i]); + + string = it.name_lineB.c_str(); + glWindowPos2f(pt2d.x, pt2d.y); + len = (int) strlen(string); + for (int i = 0; i < len; i++) + glutBitmapCharacter(GLUT_BITMAP_HELVETICA_12, string[i]); + } +} + +void GLViewer::clearInputs() { + mouseMotion_[0] = mouseMotion_[1] = 0; + mouseWheelPosition_ = 0; + for (unsigned int i = 0; i < 256; ++i) { + if (keyStates_[i] == KEY_STATE::UP) + last_key = i; + if (keyStates_[i] != KEY_STATE::DOWN) + keyStates_[i] = KEY_STATE::FREE; + } +} + +void GLViewer::drawCallback() { + currentInstance_->render(); +} + +void GLViewer::mouseButtonCallback(int button, int state, int x, int y) { + if (button < 5) { + if (button < 3) { + currentInstance_->mouseButton_[button] = state == GLUT_DOWN; + } else { + currentInstance_->mouseWheelPosition_ += button == MOUSE_BUTTON::WHEEL_UP ? 1 : -1; + } + currentInstance_->mouseCurrentPosition_[0] = x; + currentInstance_->mouseCurrentPosition_[1] = y; + currentInstance_->previousMouseMotion_[0] = x; + currentInstance_->previousMouseMotion_[1] = y; + } +} + +void GLViewer::mouseMotionCallback(int x, int y) { + currentInstance_->mouseMotion_[0] = x - currentInstance_->previousMouseMotion_[0]; + currentInstance_->mouseMotion_[1] = y - currentInstance_->previousMouseMotion_[1]; + currentInstance_->previousMouseMotion_[0] = x; + currentInstance_->previousMouseMotion_[1] = y; +} + +void GLViewer::reshapeCallback(int width, int height) { + glViewport(0, 0, width, height); + float hfov = (180.0f / M_PI) * (2.0f * atan(width / (2.0f * 500))); + float vfov = (180.0f / M_PI) * (2.0f * atan(height / (2.0f * 500))); + currentInstance_->camera_.setProjection(hfov, vfov, currentInstance_->camera_.getZNear(), currentInstance_->camera_.getZFar()); +} + +void GLViewer::keyPressedCallback(unsigned char c, int x, int y) { + currentInstance_->keyStates_[c] = KEY_STATE::DOWN; + currentInstance_->lastPressedKey = c; + //glutPostRedisplay(); +} + +void GLViewer::keyReleasedCallback(unsigned char c, int x, int y) { + currentInstance_->keyStates_[c] = KEY_STATE::UP; +} + +void GLViewer::idle() { + glutPostRedisplay(); +} + +Simple3DObject::Simple3DObject() { + vaoID_ = 0; + drawingType_ = GL_TRIANGLES; + isStatic_ = need_update = false; +} + +Simple3DObject::~Simple3DObject() { + clear(); + if (vaoID_ != 0) { + glDeleteBuffers(3, vboID_); + glDeleteVertexArrays(1, &vaoID_); + } +} + +void Simple3DObject::addPoint(sl::float3 pt, sl::float3 clr){ + vertices_.push_back(pt); + colors_.push_back(clr); + indices_.push_back((int) indices_.size()); + need_update = true; +} + +void Simple3DObject::addLine(sl::float3 pt1, sl::float3 pt2, sl::float3 clr){ + addPoint(pt1, clr); + addPoint(pt2, clr); +} + +void Simple3DObject::addFace(sl::float3 p1, sl::float3 p2, sl::float3 p3, sl::float3 clr){ + addPoint(p1, clr); + addPoint(p2, clr); + addPoint(p3, clr); +} + +void Simple3DObject::pushToGPU() { + if(!need_update) return; + + if (!isStatic_ || vaoID_ == 0) { + if (vaoID_ == 0) { + glGenVertexArrays(1, &vaoID_); + glGenBuffers(3, vboID_); + } + glBindVertexArray(vaoID_); + glBindBuffer(GL_ARRAY_BUFFER, vboID_[0]); + glBufferData(GL_ARRAY_BUFFER, vertices_.size() * sizeof(sl::float3), &vertices_[0], isStatic_ ? GL_STATIC_DRAW : GL_DYNAMIC_DRAW); + glVertexAttribPointer(Shader::ATTRIB_VERTICES_POS, 3, GL_FLOAT, GL_FALSE, 0, 0); + glEnableVertexAttribArray(Shader::ATTRIB_VERTICES_POS); + + glBindBuffer(GL_ARRAY_BUFFER, vboID_[1]); + glBufferData(GL_ARRAY_BUFFER, colors_.size() * sizeof(sl::float3), &colors_[0], isStatic_ ? GL_STATIC_DRAW : GL_DYNAMIC_DRAW); + glVertexAttribPointer(Shader::ATTRIB_COLOR_POS, 3, GL_FLOAT, GL_FALSE, 0, 0); + glEnableVertexAttribArray(Shader::ATTRIB_COLOR_POS); + + glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, vboID_[2]); + glBufferData(GL_ELEMENT_ARRAY_BUFFER, indices_.size() * sizeof (unsigned int), &indices_[0], isStatic_ ? GL_STATIC_DRAW : GL_DYNAMIC_DRAW); + + glBindVertexArray(0); + glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0); + glBindBuffer(GL_ARRAY_BUFFER, 0); + need_update = false; + } +} + +void Simple3DObject::clear() { + vertices_.clear(); + colors_.clear(); + indices_.clear(); +} + +void Simple3DObject::setDrawingType(GLenum type) { + drawingType_ = type; +} + +void Simple3DObject::draw() { + glBindVertexArray(vaoID_); + glDrawElements(drawingType_, (GLsizei) indices_.size(), GL_UNSIGNED_INT, 0); + glBindVertexArray(0); +} + +Shader::Shader(const GLchar* vs, const GLchar* fs) { + set(vs, fs); +} + +void Shader::set(const GLchar* vs, const GLchar* fs) { + if (!compile(verterxId_, GL_VERTEX_SHADER, vs)) { + std::cout << "ERROR: while compiling vertex shader" << std::endl; + } + if (!compile(fragmentId_, GL_FRAGMENT_SHADER, fs)) { + std::cout << "ERROR: while compiling fragment shader" << std::endl; + } + + programId_ = glCreateProgram(); + + glAttachShader(programId_, verterxId_); + glAttachShader(programId_, fragmentId_); + + glBindAttribLocation(programId_, ATTRIB_VERTICES_POS, "in_vertex"); + glBindAttribLocation(programId_, ATTRIB_COLOR_POS, "in_texCoord"); + + glLinkProgram(programId_); + + GLint errorlk(0); + glGetProgramiv(programId_, GL_LINK_STATUS, &errorlk); + if (errorlk != GL_TRUE) { + std::cout << "ERROR: while linking Shader :" << std::endl; + GLint errorSize(0); + glGetProgramiv(programId_, GL_INFO_LOG_LENGTH, &errorSize); + + char *error = new char[errorSize + 1]; + glGetShaderInfoLog(programId_, errorSize, &errorSize, error); + error[errorSize] = '\0'; + std::cout << error << std::endl; + + delete[] error; + glDeleteProgram(programId_); + } +} + +Shader::~Shader() { + if (verterxId_ != 0 && glIsShader(verterxId_)) + glDeleteShader(verterxId_); + if (fragmentId_ != 0 && glIsShader(fragmentId_)) + glDeleteShader(fragmentId_); + if (programId_ != 0 && glIsProgram(programId_)) + glDeleteProgram(programId_); +} + +GLuint Shader::getProgramId() { + return programId_; +} + +bool Shader::compile(GLuint &shaderId, GLenum type, const GLchar* src) { + shaderId = glCreateShader(type); + if (shaderId == 0) { + std::cout << "ERROR: shader type (" << type << ") does not exist" << std::endl; + return false; + } + glShaderSource(shaderId, 1, (const char**) &src, 0); + glCompileShader(shaderId); + + GLint errorCp(0); + glGetShaderiv(shaderId, GL_COMPILE_STATUS, &errorCp); + if (errorCp != GL_TRUE) { + std::cout << "ERROR: while compiling Shader :" << std::endl; + GLint errorSize(0); + glGetShaderiv(shaderId, GL_INFO_LOG_LENGTH, &errorSize); + + char *error = new char[errorSize + 1]; + glGetShaderInfoLog(shaderId, errorSize, &errorSize, error); + error[errorSize] = '\0'; + std::cout << error << std::endl; + + delete[] error; + glDeleteShader(shaderId); + return false; + } + return true; +} + +const GLchar* IMAGE_FRAGMENT_SHADER = + "#version 330 core\n" + " in vec2 UV;\n" + " out vec4 color;\n" + " uniform sampler2D texImage;\n" + " void main() {\n" + " vec2 scaler =vec2(UV.x,1.f - UV.y);\n" + " vec3 rgbcolor = vec3(texture(texImage, scaler).zyx);\n" + " vec3 color_rgb = pow(rgbcolor, vec3(1.65f));\n" + " color = vec4(color_rgb,1);\n" + "}"; + +const GLchar* IMAGE_VERTEX_SHADER = + "#version 330\n" + "layout(location = 0) in vec3 vert;\n" + "out vec2 UV;" + "void main() {\n" + " UV = (vert.xy+vec2(1,1))* .5f;\n" + " gl_Position = vec4(vert, 1);\n" + "}\n"; + + +PointCloud::PointCloud(): numBytes_(0), xyzrgbaMappedBuf_(nullptr) { + +} + +PointCloud::~PointCloud() { + close(); +} + +void PointCloud::close() { + if (refMat.isInit()) { + auto err = cudaGraphicsUnmapResources(1, &bufferCudaID_, 0); + if (err) std::cerr << "Error CUDA " << cudaGetErrorString(err) << std::endl; + err = cudaGraphicsUnregisterResource(bufferCudaID_); + if (err) std::cerr << "Error CUDA " << cudaGetErrorString(err) << std::endl; + glDeleteBuffers(1, &bufferGLID_); + refMat.free(); + } +} + +void PointCloud::initialize(sl::Mat& ref, sl::float3 clr_) +{ + refMat = ref; + clr = clr_; +} + +void PointCloud::pushNewPC() { + if (refMat.isInit()) { + + int sizebytes = refMat.getResolution().area() * sizeof(sl::float4); + if (numBytes_ != sizebytes) { + + if (numBytes_ == 0) { + glGenBuffers(1, &bufferGLID_); + + shader_.set(POINTCLOUD_VERTEX_SHADER, POINTCLOUD_FRAGMENT_SHADER); + shMVPMatrixLoc_ = glGetUniformLocation(shader_.getProgramId(), "u_mvpMatrix"); + shColor = glGetUniformLocation(shader_.getProgramId(), "u_color"); + shDrawColor = glGetUniformLocation(shader_.getProgramId(), "u_drawFlat"); + } + else { + cudaGraphicsUnmapResources(1, &bufferCudaID_, 0); + cudaGraphicsUnregisterResource(bufferCudaID_); + } + + glBindBuffer(GL_ARRAY_BUFFER, bufferGLID_); + glBufferData(GL_ARRAY_BUFFER, sizebytes, 0, GL_STATIC_DRAW); + glBindBuffer(GL_ARRAY_BUFFER, 0); + + cudaError_t err = cudaGraphicsGLRegisterBuffer(&bufferCudaID_, bufferGLID_, cudaGraphicsRegisterFlagsNone); + if (err) std::cerr << "Error CUDA " << cudaGetErrorString(err) << std::endl; + + err = cudaGraphicsMapResources(1, &bufferCudaID_, 0); + if (err) std::cerr << "Error CUDA " << cudaGetErrorString(err) << std::endl; + + err = cudaGraphicsResourceGetMappedPointer((void**)&xyzrgbaMappedBuf_, &numBytes_, bufferCudaID_); + if (err) std::cerr << "Error CUDA " << cudaGetErrorString(err) << std::endl; + } + + cudaMemcpy(xyzrgbaMappedBuf_, refMat.getPtr(sl::MEM::CPU), numBytes_, cudaMemcpyHostToDevice); + } +} + +void PointCloud::draw(const sl::Transform& vp, bool draw_flat) { + if (refMat.isInit()) { +#ifndef JETSON_STYLE + glDisable(GL_BLEND); +#endif + + glUseProgram(shader_.getProgramId()); + glUniformMatrix4fv(shMVPMatrixLoc_, 1, GL_TRUE, vp.m); + + glUniform3fv(shColor, 1, clr.v); + glUniform1i(shDrawColor, draw_flat); + + glBindBuffer(GL_ARRAY_BUFFER, bufferGLID_); + glVertexAttribPointer(Shader::ATTRIB_VERTICES_POS, 4, GL_FLOAT, GL_FALSE, 0, 0); + glEnableVertexAttribArray(Shader::ATTRIB_VERTICES_POS); + + glDrawArrays(GL_POINTS, 0, refMat.getResolution().area()); + glBindBuffer(GL_ARRAY_BUFFER, 0); + glUseProgram(0); + +#ifndef JETSON_STYLE + glEnable(GL_BLEND); +#endif + } +} + + +CameraViewer::CameraViewer() { + +} + +CameraViewer::~CameraViewer() { + close(); +} + +void CameraViewer::close() { + if (ref.isInit()) { + + auto err = cudaGraphicsUnmapResources(1, &cuda_gl_ressource, 0); + if (err) std::cerr << "Error CUDA " << cudaGetErrorString(err) << std::endl; + err = cudaGraphicsUnregisterResource(cuda_gl_ressource); + if (err) std::cerr << "Error CUDA " << cudaGetErrorString(err) << std::endl; + + glDeleteTextures(1, &texture); + glDeleteBuffers(3, vboID_); + glDeleteVertexArrays(1, &vaoID_); + ref.free(); + } +} + +bool CameraViewer::initialize(sl::Mat &im, sl::float3 clr) { + + // Create 3D axis + float fx,fy,cx,cy; + fx = fy = 1400; + float width, height; + width = 2208; + height = 1242; + cx = width /2; + cy = height /2; + + float Z_ = .5f; + sl::float3 toOGL(1,-1,-1); + sl::float3 cam_0(0, 0, 0); + sl::float3 cam_1, cam_2, cam_3, cam_4; + + float fx_ = 1.f / fx; + float fy_ = 1.f / fy; + + cam_1.z = Z_; + cam_1.x = (0 - cx) * Z_ *fx_; + cam_1.y = (0 - cy) * Z_ *fy_ ; + cam_1 *= toOGL; + + cam_2.z = Z_; + cam_2.x = (width - cx) * Z_ *fx_; + cam_2.y = (0 - cy) * Z_ *fy_; + cam_2 *= toOGL; + + cam_3.z = Z_; + cam_3.x = (width - cx) * Z_ *fx_; + cam_3.y = (height - cy) * Z_ *fy_; + cam_3 *= toOGL; + + cam_4.z = Z_; + cam_4.x = (0 - cx) * Z_ *fx_; + cam_4.y = (height - cy) * Z_ *fy_; + cam_4 *= toOGL; + + frustum.addPoint(cam_0, clr); + frustum.addPoint(cam_1, clr); + + frustum.addPoint(cam_0, clr); + frustum.addPoint(cam_2, clr); + + frustum.addPoint(cam_0, clr); + frustum.addPoint(cam_3, clr); + + frustum.addPoint(cam_0, clr); + frustum.addPoint(cam_4, clr); + + frustum.setDrawingType(GL_LINES); + frustum.pushToGPU(); + + vert.push_back(cam_1); + vert.push_back(cam_2); + vert.push_back(cam_3); + vert.push_back(cam_4); + + uv.push_back(sl::float2(0,0)); + uv.push_back(sl::float2(1,0)); + uv.push_back(sl::float2(1,1)); + uv.push_back(sl::float2(0,1)); + + faces.push_back(sl::uint3(0,1,2)); + faces.push_back(sl::uint3(0,2,3)); + + ref = im; + if(!ref.isInit()) + return 1; + + shader.set(VERTEX_SHADER_TEXTURE, FRAGMENT_SHADER_TEXTURE); + shMVPMatrixLocTex_ = glGetUniformLocation(shader.getProgramId(), "u_mvpMatrix"); + + glGenVertexArrays(1, &vaoID_); + glGenBuffers(3, vboID_); + + glBindVertexArray(vaoID_); + glBindBuffer(GL_ARRAY_BUFFER, vboID_[0]); + glBufferData(GL_ARRAY_BUFFER, vert.size() * sizeof(sl::float3), &vert[0], GL_STATIC_DRAW); + glVertexAttribPointer(Shader::ATTRIB_VERTICES_POS, 3, GL_FLOAT, GL_FALSE, 0, 0); + glEnableVertexAttribArray(Shader::ATTRIB_VERTICES_POS); + + glBindBuffer(GL_ARRAY_BUFFER, vboID_[1]); + glBufferData(GL_ARRAY_BUFFER, uv.size() * sizeof(sl::float2), &uv[0], GL_STATIC_DRAW); + glVertexAttribPointer(Shader::ATTRIB_COLOR_POS, 2, GL_FLOAT, GL_FALSE, 0, 0); + glEnableVertexAttribArray(Shader::ATTRIB_COLOR_POS); + + glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, vboID_[2]); + glBufferData(GL_ELEMENT_ARRAY_BUFFER, faces.size() * sizeof(sl::uint3), &faces[0], GL_STATIC_DRAW); + + glBindVertexArray(0); + glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0); + glBindBuffer(GL_ARRAY_BUFFER, 0); + + auto res = ref.getResolution(); + glGenTextures(1, &texture); + glBindTexture(GL_TEXTURE_2D, texture); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, res.width, res.height, 0, GL_BGRA_EXT, GL_UNSIGNED_BYTE, NULL); + glBindTexture(GL_TEXTURE_2D, 0); + cudaError_t err = cudaGraphicsGLRegisterImage(&cuda_gl_ressource, texture, GL_TEXTURE_2D, cudaGraphicsRegisterFlagsWriteDiscard); + if (err) std::cout << "err alloc " << err << " " << cudaGetErrorString(err) << "\n"; + glDisable(GL_TEXTURE_2D); + + err = cudaGraphicsMapResources(1, &cuda_gl_ressource, 0); + if (err) std::cout << "err 0 " << err << " " << cudaGetErrorString(err) << "\n"; + err = cudaGraphicsSubResourceGetMappedArray(&ArrIm, cuda_gl_ressource, 0, 0); + if (err) std::cout << "err 1 " << err << " " << cudaGetErrorString(err) << "\n"; + + return (err == cudaSuccess); +} + +void CameraViewer::pushNewImage() { + if (!ref.isInit()) return; + auto err = cudaMemcpy2DToArray(ArrIm, 0, 0, ref.getPtr(sl::MEM::CPU), ref.getStepBytes(sl::MEM::CPU), ref.getPixelBytes() * ref.getWidth(), ref.getHeight(), cudaMemcpyHostToDevice); + if (err) std::cout << "err 2 " << err << " " << cudaGetErrorString(err) << "\n"; +} + +void CameraViewer::draw(sl::Transform vpMatrix) { + if (!ref.isInit()) return; + + glUseProgram(shader.getProgramId()); + glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); + + glUniformMatrix4fv(shMVPMatrixLocTex_, 1, GL_FALSE, sl::Transform::transpose(vpMatrix).m); + glBindTexture(GL_TEXTURE_2D, texture); + + glBindVertexArray(vaoID_); + glDrawElements(GL_TRIANGLES, (GLsizei)faces.size()*3, GL_UNSIGNED_INT, 0); + glBindVertexArray(0); + + glUseProgram(0); +} + + +const sl::Translation CameraGL::ORIGINAL_FORWARD = sl::Translation(0, 0, 1); +const sl::Translation CameraGL::ORIGINAL_UP = sl::Translation(0, 1, 0); +const sl::Translation CameraGL::ORIGINAL_RIGHT = sl::Translation(1, 0, 0); + +CameraGL::CameraGL(sl::Translation position, sl::Translation direction, sl::Translation vertical) { + this->position_ = position; + setDirection(direction, vertical); + + offset_ = sl::Translation(0, 0, 0); + view_.setIdentity(); + updateView(); + setProjection(70, 70, 0.200f, 50.f); + updateVPMatrix(); +} + +CameraGL::~CameraGL() { +} + +void CameraGL::update() { + if (sl::Translation::dot(vertical_, up_) < 0) + vertical_ = vertical_ * -1.f; + updateView(); + updateVPMatrix(); +} + +void CameraGL::setProjection(float horizontalFOV, float verticalFOV, float znear, float zfar) { + horizontalFieldOfView_ = horizontalFOV; + verticalFieldOfView_ = verticalFOV; + znear_ = znear; + zfar_ = zfar; + + float fov_y = verticalFOV * M_PI / 180.f; + float fov_x = horizontalFOV * M_PI / 180.f; + + projection_.setIdentity(); + projection_(0, 0) = 1.0f / tanf(fov_x * 0.5f); + projection_(1, 1) = 1.0f / tanf(fov_y * 0.5f); + projection_(2, 2) = -(zfar + znear) / (zfar - znear); + projection_(3, 2) = -1; + projection_(2, 3) = -(2.f * zfar * znear) / (zfar - znear); + projection_(3, 3) = 0; +} + +const sl::Transform& CameraGL::getViewProjectionMatrix() const { + return vpMatrix_; +} + +float CameraGL::getHorizontalFOV() const { + return horizontalFieldOfView_; +} + +float CameraGL::getVerticalFOV() const { + return verticalFieldOfView_; +} + +void CameraGL::setOffsetFromPosition(const sl::Translation& o) { + offset_ = o; +} + +const sl::Translation& CameraGL::getOffsetFromPosition() const { + return offset_; +} + +void CameraGL::setDirection(const sl::Translation& direction, const sl::Translation& vertical) { + sl::Translation dirNormalized = direction; + dirNormalized.normalize(); + this->rotation_ = sl::Orientation(ORIGINAL_FORWARD, dirNormalized * -1.f); + updateVectors(); + this->vertical_ = vertical; + if (sl::Translation::dot(vertical_, up_) < 0) + rotate(sl::Rotation(M_PI, ORIGINAL_FORWARD)); +} + +void CameraGL::translate(const sl::Translation& t) { + position_ = position_ + t; +} + +void CameraGL::setPosition(const sl::Translation& p) { + position_ = p; +} + +void CameraGL::rotate(const sl::Orientation& rot) { + rotation_ = rot * rotation_; + updateVectors(); +} + +void CameraGL::rotate(const sl::Rotation& m) { + this->rotate(sl::Orientation(m)); +} + +void CameraGL::setRotation(const sl::Orientation& rot) { + rotation_ = rot; + updateVectors(); +} + +void CameraGL::setRotation(const sl::Rotation& m) { + this->setRotation(sl::Orientation(m)); +} + +const sl::Translation& CameraGL::getPosition() const { + return position_; +} + +const sl::Translation& CameraGL::getForward() const { + return forward_; +} + +const sl::Translation& CameraGL::getRight() const { + return right_; +} + +const sl::Translation& CameraGL::getUp() const { + return up_; +} + +const sl::Translation& CameraGL::getVertical() const { + return vertical_; +} + +float CameraGL::getZNear() const { + return znear_; +} + +float CameraGL::getZFar() const { + return zfar_; +} + +void CameraGL::updateVectors() { + forward_ = ORIGINAL_FORWARD * rotation_; + up_ = ORIGINAL_UP * rotation_; + right_ = sl::Translation(ORIGINAL_RIGHT * -1.f) * rotation_; +} + +void CameraGL::updateView() { + sl::Transform transformation(rotation_, (offset_ * rotation_) + position_); + view_ = sl::Transform::inverse(transformation); +} + +void CameraGL::updateVPMatrix() { + vpMatrix_ = projection_ * view_; +} diff --git a/src/main.cpp b/src/main.cpp new file mode 100755 index 0000000..b64307c --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,211 @@ +/////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2025, STEREOLABS. +// +// All rights reserved. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +/////////////////////////////////////////////////////////////////////////// + +// ZED include +#include "ClientPublisher.hpp" +#include "GLViewer.hpp" + +#include + +int main(int argc, char **argv) { + +#ifdef _SL_JETSON_ + const bool isJetson = true; +#else + const bool isJetson = false; +#endif + + if (argc != 2) { + // this file should be generated by using the tool ZED360 + std::cout << "Need a Configuration file in input" << std::endl; + return 1; + } + + // Defines the Coordinate system and unit used in this sample + constexpr sl::COORDINATE_SYSTEM COORDINATE_SYSTEM = + sl::COORDINATE_SYSTEM::RIGHT_HANDED_Y_UP; + constexpr sl::UNIT UNIT = sl::UNIT::METER; + + // Read json file containing the configuration of your multicamera setup. + auto configurations = + sl::readFusionConfigurationFile(argv[1], COORDINATE_SYSTEM, UNIT); + + if (configurations.empty()) { + std::cout << "Empty configuration File." << std::endl; + return EXIT_FAILURE; + } + + Trigger trigger; + + // Open camera streams from hard-coded configuration + std::vector> clients; + int id_ = 0; + int gpu_id = 0; + + int nb_gpu = 0; + if (!isJetson) + cudaGetDeviceCount(&nb_gpu); + + // Use hard-coded camera stream configurations + for (const auto& cam_config : CAMERA_STREAM_CONFIGS) { + std::cout << "Try to open stream for ZED " << cam_config.serial_number + << " at " << cam_config.ip_address << ":" << cam_config.port << ".." + << std::flush; + + gpu_id = (nb_gpu == 0) ? 0 : id_ % nb_gpu; + + auto client = std::make_unique(); + auto state = client->open(cam_config.ip_address, cam_config.port, &trigger, gpu_id); + if (!state) { + std::cerr << "Could not open stream for ZED: " << cam_config.serial_number + << ". Skipping..." << std::endl; + continue; + } + + std::cout << ". ready ! (Serial: " << client->getSerialNumber() << ")" << std::endl; + + clients.push_back(std::move(client)); + id_++; + } + + // start camera threads + for (auto &it : clients) { + it->start(); + } + + // Now that the ZED camera are running, we need to initialize the fusion + // module + sl::InitFusionParameters init_params; + init_params.coordinate_units = UNIT; + init_params.coordinate_system = COORDINATE_SYSTEM; + + sl::Resolution low_res(512, 360); + init_params.maximum_working_resolution = low_res; + // create and initialize it + sl::Fusion fusion; + fusion.init(init_params); + + // subscribe to every cameras of the setup to internally gather their data + std::vector cameras; + for (auto &it : configurations) { + sl::CameraIdentifier uuid(it.serial_number); + // to subscribe to a camera you must give its serial number, the way to + // communicate with it (shared memory or local network), and its world pose + // in the setup. + auto state = fusion.subscribe(uuid, it.communication_parameters, it.pose, + it.override_gravity); + if (state != sl::FUSION_ERROR_CODE::SUCCESS) + std::cout << "Unable to subscribe to " << std::to_string(uuid.sn) << " . " + << state << std::endl; + else + cameras.push_back(uuid); + } + + // check that at least one camera is connected + if (cameras.empty()) { + std::cout << "no connections " << std::endl; + return EXIT_FAILURE; + } + + // as this sample shows how to fuse body detection from the multi camera setup + // we enable the Body Tracking module with its options + sl::BodyTrackingFusionParameters body_fusion_init_params; + body_fusion_init_params.enable_tracking = true; + body_fusion_init_params.enable_body_fitting = + !isJetson; // skeletons will looks more natural but requires more + // computations + fusion.enableBodyTracking(body_fusion_init_params); + + // define fusion behavior + sl::BodyTrackingFusionRuntimeParameters body_tracking_runtime_parameters; + // be sure that the detection skeleton is complete enough + body_tracking_runtime_parameters.skeleton_minimum_allowed_keypoints = 7; + // we can also want to retrieve skeleton seen by multiple camera, in this case + // at least half of them + body_tracking_runtime_parameters.skeleton_minimum_allowed_camera = + cameras.size() / 2.; + body_tracking_runtime_parameters.skeleton_smoothing = 0.1f; + + // creation of a 3D viewer + GLViewer viewer; + viewer.init(argc, argv); + + std::cout << "Viewer Shortcuts\n" + << "\t- 'q': quit the application\n" + << "\t- 'r': switch on/off for raw skeleton display\n" + << "\t- 'p': switch on/off for live point cloud display\n" + << "\t- 'c': switch on/off point cloud display with raw color\n" + << std::endl; + + // fusion outputs + sl::Bodies fused_bodies; + std::map camera_raw_data; + sl::FusionMetrics metrics; + std::map views; + std::map pointClouds; + sl::CameraIdentifier fused_camera(0); + + // run the fusion as long as the viewer is available. + while (viewer.isAvailable()) { + trigger.notifyZED(); + + // run the fusion process (which gather data from all camera, sync them and + // process them) + if (fusion.process() == sl::FUSION_ERROR_CODE::SUCCESS) { + + // Retrieve fused body + fusion.retrieveBodies(fused_bodies, body_tracking_runtime_parameters); + // for debug, you can retrieve the data sent by each camera + for (auto &id : cameras) { + fusion.retrieveBodies(camera_raw_data[id], + body_tracking_runtime_parameters, id); + + auto state_view = fusion.retrieveImage(views[id], id, low_res); + auto state_pc = fusion.retrieveMeasure(pointClouds[id], id, + sl::MEASURE::XYZBGRA, low_res); + + if (state_view == sl::FUSION_ERROR_CODE::SUCCESS && + state_pc == sl::FUSION_ERROR_CODE::SUCCESS) + viewer.updateCamera(id.sn, views[id], pointClouds[id]); + + sl::Pose pose; + if (fusion.getPosition(pose, sl::REFERENCE_FRAME::WORLD, id, + sl::POSITION_TYPE::RAW) == + sl::POSITIONAL_TRACKING_STATE::OK) + viewer.setCameraPose(id.sn, pose.pose_data); + } + + // get metrics about the fusion process for monitoring purposes + fusion.getProcessMetrics(metrics); + } + // update the 3D view + viewer.updateBodies(fused_bodies, camera_raw_data, metrics); + } + + trigger.running = false; + trigger.notifyZED(); + + for (auto &it : clients) + it->stop(); + + fusion.close(); + + return EXIT_SUCCESS; +}