From 3cd9c195f3a0730c759931b070e54efdcade791a Mon Sep 17 00:00:00 2001 From: Aymeric Dujardin Date: Wed, 11 May 2022 14:44:23 +0200 Subject: [PATCH] 3.7 sample upgrade (#483) --- CMakeLists.txt | 6 +- body tracking/cpp/CMakeLists.txt | 36 +- body tracking/cpp/src/main.cpp | 3 +- body tracking/csharp/CMakeLists.txt | 2 +- body tracking/csharp/MainWindow.cs | 2 +- body tracking/python/body_tracking.py | 2 +- camera control/cpp/CMakeLists.txt | 32 +- camera control/cpp/src/main.cpp | 11 +- camera control/csharp/CMakeLists.txt | 2 +- camera control/csharp/Program.cs | 3 +- camera control/python/camera_control.py | 2 +- camera streaming/receiver/README.md | 2 +- camera streaming/receiver/cpp/CMakeLists.txt | 20 +- camera streaming/receiver/cpp/src/main.cpp | 4 +- .../receiver/python/streaming_receiver.py | 2 +- camera streaming/sender/cpp/CMakeLists.txt | 22 +- camera streaming/sender/cpp/include/utils.hpp | 2 +- camera streaming/sender/cpp/src/main.cpp | 4 +- .../sender/python/streaming_sender.py | 2 +- depth sensing/cpp/CMakeLists.txt | 24 +- depth sensing/cpp/src/GLViewer.cpp | 2 +- depth sensing/cpp/src/main.cpp | 8 +- depth sensing/csharp/CMakeLists.txt | 2 +- depth sensing/csharp/MainWindow.cs | 2 +- depth sensing/python/depth_sensing.py | 2 +- .../birds eye viewer/cpp/CMakeLists.txt | 32 +- .../cpp/src/TrackingViewer.cpp | 50 +- .../birds eye viewer/cpp/src/main.cpp | 14 +- .../birds eye viewer/csharp/CMakeLists.txt | 2 +- .../birds eye viewer/csharp/MainWindow.cs | 2 +- .../python/object_detection_birds_view.py | 2 +- .../cpp/opencv_dnn_yolov4/CMakeLists.txt | 23 +- .../cpp/tensorrt_yolov5_v5.0/CMakeLists.txt | 73 ++ .../LICENSE | 0 .../README.md | 0 .../gen_wts.py | 0 .../include/GLViewer.hpp | 0 .../include/calibrator.h | 0 .../include/common.hpp | 0 .../include/cuda_utils.h | 0 .../include/logging.h | 0 .../include/utils.h | 0 .../include/utils.hpp | 0 .../include/yololayer.h | 0 .../src/GLViewer.cpp | 0 .../src/calibrator.cpp | 0 .../src/yololayer.cu | 0 .../src/yolov5.cpp | 0 .../CMakeLists.txt | 26 +- .../cpp/tensorrt_yolov5_v6.0/LICENSE | 21 + .../cpp/tensorrt_yolov5_v6.0/README.md | 78 ++ .../cpp/tensorrt_yolov5_v6.0/gen_wts.py | 49 + .../tensorrt_yolov5_v6.0/include/GLViewer.hpp | 303 +++++ .../tensorrt_yolov5_v6.0/include/calibrator.h | 48 + .../tensorrt_yolov5_v6.0/include/common.hpp | 328 ++++++ .../tensorrt_yolov5_v6.0/include/cuda_utils.h | 18 + .../tensorrt_yolov5_v6.0/include/logging.h | 509 ++++++++ .../cpp/tensorrt_yolov5_v6.0/include/utils.h | 52 + .../tensorrt_yolov5_v6.0/include/utils.hpp | 109 ++ .../tensorrt_yolov5_v6.0/include/yololayer.h | 146 +++ .../cpp/tensorrt_yolov5_v6.0/src/GLViewer.cpp | 1038 +++++++++++++++++ .../tensorrt_yolov5_v6.0/src/calibrator.cpp | 83 ++ .../cpp/tensorrt_yolov5_v6.0/src/yololayer.cu | 313 +++++ .../cpp/tensorrt_yolov5_v6.0/src/yolov5.cpp | 530 +++++++++ .../image viewer/cpp/CMakeLists.txt | 44 +- .../image viewer/cpp/src/main.cpp | 2 +- .../image viewer/csharp/CMakeLists.txt | 2 +- .../image viewer/csharp/MainWindow.cs | 2 +- .../python/object_detection_image_viewer.py | 2 +- other/cuda refocus/CMakeLists.txt | 24 +- other/cuda refocus/src/main.cpp | 2 +- other/multi camera/cpp/CMakeLists.txt | 20 +- other/multi camera/cpp/src/main.cpp | 2 +- other/multi camera/python/multi_camera.py | 2 +- other/opengl gpu interop/CMakeLists.txt | 22 +- other/opengl gpu interop/src/main.cpp | 2 +- plane detection/cpp/CMakeLists.txt | 25 +- plane detection/cpp/src/main.cpp | 2 +- plane detection/csharp/CMakeLists.txt | 2 +- plane detection/csharp/MainWindow.cs | 1 - plane detection/python/plane_detection.py | 2 +- positional tracking/cpp/CMakeLists.txt | 27 +- positional tracking/cpp/src/main.cpp | 2 +- positional tracking/csharp/CMakeLists.txt | 2 +- positional tracking/csharp/MainWindow.cs | 4 +- .../python/positional_tracking.py | 2 +- .../cpp/CMakeLists.txt | 28 +- .../cpp/src/main.cpp | 2 +- spatial mapping/basic/cpp/CMakeLists.txt | 27 +- spatial mapping/basic/cpp/src/main.cpp | 2 +- spatial mapping/basic/csharp/CMakeLists.txt | 2 +- spatial mapping/basic/csharp/MainWindow.cs | 9 +- .../basic/python/ogl_viewer/viewer.py | 81 +- .../basic/python/spatial_mapping.py | 44 +- svo recording/export/cpp/CMakeLists.txt | 24 +- svo recording/export/cpp/include/utils.hpp | 2 +- svo recording/export/cpp/src/main.cpp | 2 +- svo recording/export/csharp/CMakeLists.txt | 2 +- svo recording/export/csharp/Program.cs | 2 +- svo recording/export/python/svo_export.py | 2 +- svo recording/playback/cpp/CMakeLists.txt | 24 +- svo recording/playback/cpp/include/utils.hpp | 2 +- svo recording/playback/cpp/src/main.cpp | 2 +- svo recording/playback/csharp/CMakeLists.txt | 2 +- svo recording/playback/csharp/Program.cs | 2 +- svo recording/playback/python/svo_playback.py | 2 +- svo recording/recording/cpp/CMakeLists.txt | 28 +- svo recording/recording/cpp/include/utils.hpp | 2 +- svo recording/recording/cpp/src/main.cpp | 4 +- svo recording/recording/csharp/CMakeLists.txt | 2 +- svo recording/recording/csharp/Program.cs | 2 +- .../recording/python/svo_recording.py | 2 +- tutorials/tutorial 1 - hello ZED/c/main.c | 2 +- .../tutorial 1 - hello ZED/cpp/CMakeLists.txt | 19 +- tutorials/tutorial 1 - hello ZED/cpp/main.cpp | 2 +- .../csharp/CMakeLists.txt | 7 +- .../python/hello_zed.py | 2 +- tutorials/tutorial 2 - image capture/c/main.c | 2 +- .../cpp/CMakeLists.txt | 19 +- .../tutorial 2 - image capture/cpp/main.cpp | 2 +- .../csharp/CMakeLists.txt | 2 +- .../python/image_capture.py | 2 +- tutorials/tutorial 3 - depth sensing/c/main.c | 2 +- .../cpp/CMakeLists.txt | 21 +- .../tutorial 3 - depth sensing/cpp/main.cpp | 2 +- .../csharp/CMakeLists.txt | 2 +- .../python/depth_sensing.py | 2 +- .../tutorial 4 - positional tracking/c/main.c | 2 +- .../cpp/CMakeLists.txt | 19 +- .../cpp/main.cpp | 2 +- .../csharp/CMakeLists.txt | 2 +- .../python/positional_tracking.py | 2 +- .../tutorial 5 - spatial mapping/c/main.c | 2 +- .../cpp/CMakeLists.txt | 21 +- .../tutorial 5 - spatial mapping/cpp/main.cpp | 2 +- .../csharp/CMakeLists.txt | 2 +- .../python/spatial_mapping.py | 4 +- .../tutorial 6 - object detection/c/main.c | 2 +- .../cpp/CMakeLists.txt | 21 +- .../cpp/main.cpp | 4 +- .../csharp/CMakeLists.txt | 2 +- .../csharp/Program.cs | 1 - .../python/object_detection.py | 2 +- tutorials/tutorial 7 - sensor data/c/main.c | 2 +- .../cpp/CMakeLists.txt | 19 +- .../tutorial 7 - sensor data/cpp/main.cpp | 2 +- .../csharp/CMakeLists.txt | 2 +- .../python/sensor_data.py | 2 +- tutorials/tutorial 8 - body tracking/c/main.c | 2 +- .../cpp/CMakeLists.txt | 21 +- .../tutorial 8 - body tracking/cpp/main.cpp | 2 +- .../csharp/CMakeLists.txt | 2 +- .../csharp/Program.cs | 6 +- .../python/body_tracking.py | 2 +- 154 files changed, 4111 insertions(+), 675 deletions(-) create mode 100644 object detection/custom detector/cpp/tensorrt_yolov5_v5.0/CMakeLists.txt rename object detection/custom detector/cpp/{tensorrt_yolov5 => tensorrt_yolov5_v5.0}/LICENSE (100%) rename object detection/custom detector/cpp/{tensorrt_yolov5 => tensorrt_yolov5_v5.0}/README.md (100%) rename object detection/custom detector/cpp/{tensorrt_yolov5 => tensorrt_yolov5_v5.0}/gen_wts.py (100%) rename object detection/custom detector/cpp/{tensorrt_yolov5 => tensorrt_yolov5_v5.0}/include/GLViewer.hpp (100%) rename object detection/custom detector/cpp/{tensorrt_yolov5 => tensorrt_yolov5_v5.0}/include/calibrator.h (100%) rename object detection/custom detector/cpp/{tensorrt_yolov5 => tensorrt_yolov5_v5.0}/include/common.hpp (100%) rename object detection/custom detector/cpp/{tensorrt_yolov5 => tensorrt_yolov5_v5.0}/include/cuda_utils.h (100%) rename object detection/custom detector/cpp/{tensorrt_yolov5 => tensorrt_yolov5_v5.0}/include/logging.h (100%) rename object detection/custom detector/cpp/{tensorrt_yolov5 => tensorrt_yolov5_v5.0}/include/utils.h (100%) rename object detection/custom detector/cpp/{tensorrt_yolov5 => tensorrt_yolov5_v5.0}/include/utils.hpp (100%) rename object detection/custom detector/cpp/{tensorrt_yolov5 => tensorrt_yolov5_v5.0}/include/yololayer.h (100%) rename object detection/custom detector/cpp/{tensorrt_yolov5 => tensorrt_yolov5_v5.0}/src/GLViewer.cpp (100%) rename object detection/custom detector/cpp/{tensorrt_yolov5 => tensorrt_yolov5_v5.0}/src/calibrator.cpp (100%) rename object detection/custom detector/cpp/{tensorrt_yolov5 => tensorrt_yolov5_v5.0}/src/yololayer.cu (100%) rename object detection/custom detector/cpp/{tensorrt_yolov5 => tensorrt_yolov5_v5.0}/src/yolov5.cpp (100%) rename object detection/custom detector/cpp/{tensorrt_yolov5 => tensorrt_yolov5_v6.0}/CMakeLists.txt (77%) create mode 100644 object detection/custom detector/cpp/tensorrt_yolov5_v6.0/LICENSE create mode 100644 object detection/custom detector/cpp/tensorrt_yolov5_v6.0/README.md create mode 100644 object detection/custom detector/cpp/tensorrt_yolov5_v6.0/gen_wts.py create mode 100644 object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/GLViewer.hpp create mode 100644 object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/calibrator.h create mode 100644 object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/common.hpp create mode 100644 object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/cuda_utils.h create mode 100644 object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/logging.h create mode 100644 object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/utils.h create mode 100644 object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/utils.hpp create mode 100644 object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/yololayer.h create mode 100644 object detection/custom detector/cpp/tensorrt_yolov5_v6.0/src/GLViewer.cpp create mode 100644 object detection/custom detector/cpp/tensorrt_yolov5_v6.0/src/calibrator.cpp create mode 100644 object detection/custom detector/cpp/tensorrt_yolov5_v6.0/src/yololayer.cu create mode 100644 object detection/custom detector/cpp/tensorrt_yolov5_v6.0/src/yolov5.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 4d94c99d..5c9e0e61 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.1) +cmake_minimum_required(VERSION 3.5) PROJECT(SamplesAndTutos) OPTION(INSTALL_SAMPLES "Once build, deploy samples into bin" ON) @@ -10,6 +10,8 @@ else() SET(TYPE "csharp") endif() +SET(CMAKE_BUILD_TYPE "Release") + SET(SAMPLE_LIST "") add_subdirectory("camera control/${TYPE}") add_subdirectory("depth sensing/${TYPE}") @@ -34,4 +36,4 @@ add_subdirectory("tutorials") if(${INSTALL_SAMPLES} AND ${BUILD_CPP}) INSTALL(TARGETS ${SAMPLE_LIST} RUNTIME DESTINATION ${CMAKE_SOURCE_DIR}/bin) -endif() \ No newline at end of file +endif() diff --git a/body tracking/cpp/CMakeLists.txt b/body tracking/cpp/CMakeLists.txt index 583e8547..4ffbc961 100644 --- a/body tracking/cpp/CMakeLists.txt +++ b/body tracking/cpp/CMakeLists.txt @@ -1,32 +1,23 @@ -cmake_minimum_required(VERSION 2.4) +cmake_minimum_required(VERSION 3.5) PROJECT(ZED_Body_Tracking_Viewer) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +SET(CMAKE_BUILD_TYPE "Release") + 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() -if(COMMAND cmake_policy) - cmake_policy(SET CMP0003 OLD) - cmake_policy(SET CMP0004 OLD) - cmake_policy(SET CMP0015 OLD) -endif(COMMAND cmake_policy) - -SET(EXECUTABLE_OUTPUT_PATH ".") -SET(SPECIAL_OS_LIBS "") - find_package(ZED 3 REQUIRED) +find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED) find_package(OpenCV REQUIRED) find_package(GLUT REQUIRED) find_package(GLEW REQUIRED) +SET(OpenGL_GL_PREFERENCE GLVND) find_package(OpenGL REQUIRED) -find_package(CUDA REQUIRED) - -IF(NOT WIN32) - SET(SPECIAL_OS_LIBS "pthread" "X11") - add_definitions(-Wno-write-strings) -ENDIF() include_directories(${OpenCV_INCLUDE_DIRS}) include_directories(${ZED_INCLUDE_DIRS}) @@ -46,7 +37,6 @@ FILE(GLOB_RECURSE SRC_FILES src/*.c*) FILE(GLOB_RECURSE HDR_FILES include/*.h*) add_executable(${PROJECT_NAME} ${HDR_FILES} ${SRC_FILES}) -add_definitions(-std=c++14) if (LINK_SHARED_ZED) SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY} ${CUDA_DEP_LIBRARIES_ZED}) @@ -55,12 +45,12 @@ else() endif() target_link_libraries(${PROJECT_NAME} - ${SPECIAL_OS_LIBS} - ${ZED_LIBS} - ${OPENGL_LIBRARIES} - ${GLUT_LIBRARY} - ${OpenCV_LIBRARIES} - ${GLEW_LIBRARIES}) + ${ZED_LIBS} + ${OPENGL_LIBRARIES} + ${GLUT_LIBRARY} + ${OpenCV_LIBRARIES} + ${GLEW_LIBRARIES} +) if(INSTALL_SAMPLES) LIST(APPEND SAMPLE_LIST ${PROJECT_NAME}) diff --git a/body tracking/cpp/src/main.cpp b/body tracking/cpp/src/main.cpp index 2fc4e509..d21b4e34 100644 --- a/body tracking/cpp/src/main.cpp +++ b/body tracking/cpp/src/main.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // @@ -53,6 +53,7 @@ int main(int argc, char **argv) { // On Jetson the object detection combined with an heavy depth mode could reduce the frame rate too much init_parameters.depth_mode = isJetson ? DEPTH_MODE::PERFORMANCE : DEPTH_MODE::ULTRA; init_parameters.coordinate_system = COORDINATE_SYSTEM::RIGHT_HANDED_Y_UP; + init_parameters.sdk_verbose=1; parseArgs(argc, argv, init_parameters); diff --git a/body tracking/csharp/CMakeLists.txt b/body tracking/csharp/CMakeLists.txt index e7854d41..324e8aca 100644 --- a/body tracking/csharp/CMakeLists.txt +++ b/body tracking/csharp/CMakeLists.txt @@ -38,7 +38,7 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES set(CMAKE_SUPPRESS_REGENERATION true) set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES - "Stereolabs.zed_3.6.0" + "Stereolabs.zed_3.7.*" "OpenGL.Net_0.8.4" "OpenGL.Net.CoreUI_0.8.4" "OpenCvSharp4.Windows_4.5.0.20201013" diff --git a/body tracking/csharp/MainWindow.cs b/body tracking/csharp/MainWindow.cs index 2232d905..cf155bb0 100644 --- a/body tracking/csharp/MainWindow.cs +++ b/body tracking/csharp/MainWindow.cs @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/body tracking/python/body_tracking.py b/body tracking/python/body_tracking.py index 505aa674..b55008af 100644 --- a/body tracking/python/body_tracking.py +++ b/body tracking/python/body_tracking.py @@ -1,6 +1,6 @@ ######################################################################## # -# Copyright (c) 2021, STEREOLABS. +# Copyright (c) 2022, STEREOLABS. # # All rights reserved. # diff --git a/camera control/cpp/CMakeLists.txt b/camera control/cpp/CMakeLists.txt index 81c4afd4..7461b2a2 100644 --- a/camera control/cpp/CMakeLists.txt +++ b/camera control/cpp/CMakeLists.txt @@ -1,22 +1,19 @@ -CMAKE_MINIMUM_REQUIRED(VERSION 2.4) +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) PROJECT(ZED_Camera_Control) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +SET(CMAKE_BUILD_TYPE "Release") + 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() -if(COMMAND cmake_policy) - cmake_policy(SET CMP0003 OLD) - cmake_policy(SET CMP0015 OLD) -endif(COMMAND cmake_policy) - -SET(EXECUTABLE_OUTPUT_PATH ".") - find_package(ZED 3 REQUIRED) find_package(OpenCV REQUIRED) -find_package(CUDA ${ZED_CUDA_VERSION} EXACT REQUIRED) +find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED) include_directories(${CUDA_INCLUDE_DIRS}) include_directories(${ZED_INCLUDE_DIRS}) @@ -27,23 +24,6 @@ link_directories(${OpenCV_LIBRARY_DIRS}) link_directories(${CUDA_LIBRARY_DIRS}) ADD_EXECUTABLE(${PROJECT_NAME} src/main.cpp) -add_definitions(-std=c++14 -O3) - -## DEBUG/ SANITIZER options -IF(NOT WIN32) - add_definitions(-Werror=return-type) - IF (BUILD_WITH_SANITIZER) - message("!! Building with address sanitizer and -g !!") - set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fno-omit-frame-pointer -fsanitize=address -Wall -Wextra -g") - set (CMAKE_LINKER_FLAGS "${CMAKE_LINKER_FLAGS} -fno-omit-frame-pointer -fsanitize=address -Wall -Wextra -g") - add_definitions(-g -fsanitize=address -fno-omit-frame-pointer -static-libasan -Wall -Wextra) - ENDIF() - - IF (BUILD_WITH_DEBUGINFOS) - message("!! Building with -g !!") - add_definitions(-g) - ENDIF() -ENDIF() if (LINK_SHARED_ZED) SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY}) diff --git a/camera control/cpp/src/main.cpp b/camera control/cpp/src/main.cpp index 6a91bedc..12a4d6f6 100644 --- a/camera control/cpp/src/main.cpp +++ b/camera control/cpp/src/main.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // @@ -91,21 +91,16 @@ static void onMouse(int event, int x, int y, int, void*) } int main(int argc, char **argv) { + // Create a ZED Camera object - - std::cout<<" Reboot CAMERA "< "< &objects, bool render_mask, bool isTrackingON) { cv::Mat overlay = left_display.clone(); cv::Rect roi_render(0, 0, left_display.size().width, left_display.size().height); + const int line_thickness = 2; - // render skeleton joints and bones - for (auto i = objects.rbegin(); i != objects.rend(); ++i) { - sl::ObjectData& obj = (*i); - if(renderObject(obj, isTrackingON)) { + // render skeleton joints and bones if available + for (auto& obj : objects) { + if (renderObject(obj, isTrackingON)) { if (obj.keypoint_2d.size()) { cv::Scalar color = generateColorID_u(obj.id); // skeleton joints @@ -30,22 +30,18 @@ void render_2D(cv::Mat &left_display, sl::float2 img_scale, std::vectorf' ,float(vv)).hex()) + f.write('\n') diff --git a/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/GLViewer.hpp b/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/GLViewer.hpp new file mode 100644 index 00000000..0a1394fc --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/GLViewer.hpp @@ -0,0 +1,303 @@ +#ifndef __VIEWER_INCLUDE__ +#define __VIEWER_INCLUDE__ + +#include +#include + +#include + +#include +#include + +#include +#include + +#include + +#include "utils.hpp" + + +#ifndef M_PI +#define M_PI 3.141592653f +#endif + +#define MOUSE_R_SENSITIVITY 0.03f +#define MOUSE_UZ_SENSITIVITY 0.5f +#define MOUSE_DZ_SENSITIVITY 1.25f +#define MOUSE_T_SENSITIVITY 0.05f +#define KEY_T_SENSITIVITY 0.1f + +// Utils +std::vector getBBoxOnFloorPlane(std::vector &bbox, sl::Pose cam_pose); + +// 3D + +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 Shader { +public: + + Shader() { + } + Shader(GLchar* vs, GLchar* fs); + ~Shader(); + GLuint getProgramId(); + + static const GLint ATTRIB_VERTICES_POS = 0; + static const GLint ATTRIB_COLOR_POS = 1; +private: + bool compile(GLuint &shaderId, GLenum type, GLchar* src); + GLuint verterxId_; + GLuint fragmentId_; + GLuint programId_; +}; + +struct ShaderData { + Shader it; + GLuint MVP_Mat; +}; + +class Simple3DObject { +public: + + Simple3DObject() { + } + Simple3DObject(sl::Translation position, bool isStatic); + ~Simple3DObject(); + + void addPt(sl::float3 pt); + void addClr(sl::float4 clr); + + void addBBox(std::vector &pts, sl::float4 clr); + void addPoint(sl::float3 pt, sl::float4 clr); + void addTriangle(sl::float3 p1, sl::float3 p2, sl::float3 p3, sl::float4 clr); + void addLine(sl::float3 p1, sl::float3 p2, sl::float4 clr); + + // New 3D rendering + void addFullEdges(std::vector &pts, sl::float4 clr); + void addVerticalEdges(std::vector &pts, sl::float4 clr); + void addTopFace(std::vector &pts, sl::float4 clr); + void addVerticalFaces(std::vector &pts, sl::float4 clr); + + void pushToGPU(); + void clear(); + + void setDrawingType(GLenum type); + + void draw(); + + void translate(const sl::Translation& t); + void setPosition(const sl::Translation& p); + + void setRT(const sl::Transform& mRT); + + 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; + + sl::Transform getModelMatrix() const; +private: + std::vector vertices_; + std::vector colors_; + std::vector indices_; + + bool isStatic_; + + GLenum drawingType_; + + GLuint vaoID_; + /* + Vertex buffer IDs: + - [0]: Vertices coordinates; + - [1]: Indices; + */ + GLuint vboID_[2]; + + sl::Translation position_; + sl::Orientation rotation_; +}; + +class PointCloud { +public: + PointCloud(); + ~PointCloud(); + + // Initialize Opengl and Cuda buffers + // Warning: must be called in the Opengl thread + void initialize(sl::Resolution res); + // Push a new point cloud + // Warning: can be called from any thread but the mutex "mutexData" must be locked + void pushNewPC(sl::Mat &matXYZRGBA); + // Update the Opengl buffer + // Warning: must be called in the Opengl thread + void update(); + // Draw the point cloud + // Warning: must be called in the Opengl thread + void draw(const sl::Transform& vp); + // Close (disable update) + void close(); + +private: + sl::Mat matGPU_; + bool hasNewPCL_ = false; + ShaderData shader; + size_t numBytes_; + float* xyzrgbaMappedBuf_; + GLuint bufferGLID_; + cudaGraphicsResource* bufferCudaID_; +}; + +struct ObjectClassName { + sl::float3 position; + std::string name; + sl::float4 color; +}; + +// This class manages input events, window and Opengl rendering pipeline + +class GLViewer { +public: + GLViewer(); + ~GLViewer(); + bool isAvailable(); + + void init(int argc, char **argv, sl::CameraParameters& param, bool isTrackingON); + void updateData(sl::Mat& matXYZRGBA, std::vector& objs, sl::Transform &cam_pose); + + void exit(); +private: + // Rendering loop method called each frame by glutDisplayFunc + void render(); + // Everything that needs to be updated before rendering must be done in this method + void update(); + // Once everything is updated, every renderable objects must be drawn in this method + void draw(); + // Clear and refresh inputs' data + void clearInputs(); + + void printText(); + void createBboxRendering(std::vector &bbox, sl::float4 bbox_clr); + void createIDRendering(sl::float3 ¢er, sl::float4 clr, unsigned int id); + + // 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(); + + bool available; + + enum MOUSE_BUTTON { + LEFT = 0, + MIDDLE = 1, + RIGHT = 2, + WHEEL_UP = 3, + WHEEL_DOWN = 4 + }; + + enum KEY_STATE { + UP = 'u', + DOWN = 'd', + FREE = 'f' + }; + + bool isTrackingON_ = false; + + bool mouseButton_[3]; + int mouseWheelPosition_; + int mouseCurrentPosition_[2]; + int mouseMotion_[2]; + int previousMouseMotion_[2]; + KEY_STATE keyStates_[256]; + + std::mutex mtx; + + Simple3DObject frustum; + PointCloud pointCloud_; + CameraGL camera_; + ShaderData shaderLine; + ShaderData shader; + sl::float4 bckgrnd_clr; + sl::Transform cam_pose; + + std::vector objectsName; + Simple3DObject BBox_edges; + Simple3DObject BBox_faces; + Simple3DObject skeletons; + Simple3DObject floor_grid; +}; + +#endif /* __VIEWER_INCLUDE__ */ diff --git a/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/calibrator.h b/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/calibrator.h new file mode 100644 index 00000000..81e873b1 --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/calibrator.h @@ -0,0 +1,48 @@ +#ifdef ENABLE_INT8_CALIBRATOR + +#ifndef ENTROPY_CALIBRATOR_H +#define ENTROPY_CALIBRATOR_H + +#include "NvInfer.h" +#include +#include + +#if NV_TENSORRT_MAJOR >= 8 +#define TRT_NOEXCEPT noexcept +#else +#define TRT_NOEXCEPT +#endif + +//! \class Int8EntropyCalibrator2 +//! +//! \brief Implements Entropy calibrator 2. +//! CalibrationAlgoType is kENTROPY_CALIBRATION_2. +//! +class Int8EntropyCalibrator2 : public nvinfer1::IInt8EntropyCalibrator2 +{ +public: + Int8EntropyCalibrator2(int batchsize, int input_w, int input_h, const char* img_dir, const char* calib_table_name, const char* input_blob_name, bool read_cache = true); + + virtual ~Int8EntropyCalibrator2(); + int getBatchSize() const TRT_NOEXCEPT override; + bool getBatch(void* bindings[], const char* names[], int nbBindings) TRT_NOEXCEPT override; + const void* readCalibrationCache(size_t& length) TRT_NOEXCEPT override; + void writeCalibrationCache(const void* cache, size_t length) TRT_NOEXCEPT override; + +private: + int batchsize_; + int input_w_; + int input_h_; + int img_idx_; + std::string img_dir_; + std::vector img_files_; + size_t input_count_; + std::string calib_table_name_; + const char* input_blob_name_; + bool read_cache_; + void* device_input_; + std::vector calib_cache_; +}; + +#endif // ENTROPY_CALIBRATOR_H +#endif \ No newline at end of file diff --git a/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/common.hpp b/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/common.hpp new file mode 100644 index 00000000..ed84f0bf --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/common.hpp @@ -0,0 +1,328 @@ +#ifndef YOLOV5_COMMON_H_ +#define YOLOV5_COMMON_H_ + +#include +#include +#include +#include +#include +#include "NvInfer.h" +#include "yololayer.h" + +using namespace nvinfer1; + +cv::Rect get_rect(cv::Mat& img, float bbox[4]) { + float l, r, t, b; + float r_w = Yolo::INPUT_W / (img.cols * 1.0); + float r_h = Yolo::INPUT_H / (img.rows * 1.0); + if (r_h > r_w) { + l = bbox[0] - bbox[2] / 2.f; + r = bbox[0] + bbox[2] / 2.f; + t = bbox[1] - bbox[3] / 2.f - (Yolo::INPUT_H - r_w * img.rows) / 2; + b = bbox[1] + bbox[3] / 2.f - (Yolo::INPUT_H - r_w * img.rows) / 2; + l = l / r_w; + r = r / r_w; + t = t / r_w; + b = b / r_w; + } else { + l = bbox[0] - bbox[2] / 2.f - (Yolo::INPUT_W - r_h * img.cols) / 2; + r = bbox[0] + bbox[2] / 2.f - (Yolo::INPUT_W - r_h * img.cols) / 2; + t = bbox[1] - bbox[3] / 2.f; + b = bbox[1] + bbox[3] / 2.f; + l = l / r_h; + r = r / r_h; + t = t / r_h; + b = b / r_h; + } + return cv::Rect(round(l), round(t), round(r - l), round(b - t)); +} + +float iou(float lbox[4], float rbox[4]) { + float interBox[] = { + (std::max)(lbox[0] - lbox[2] / 2.f , rbox[0] - rbox[2] / 2.f), //left + (std::min)(lbox[0] + lbox[2] / 2.f , rbox[0] + rbox[2] / 2.f), //right + (std::max)(lbox[1] - lbox[3] / 2.f , rbox[1] - rbox[3] / 2.f), //top + (std::min)(lbox[1] + lbox[3] / 2.f , rbox[1] + rbox[3] / 2.f), //bottom + }; + + if (interBox[2] > interBox[3] || interBox[0] > interBox[1]) + return 0.0f; + + float interBoxS = (interBox[1] - interBox[0])*(interBox[3] - interBox[2]); + return interBoxS / (lbox[2] * lbox[3] + rbox[2] * rbox[3] - interBoxS); +} + +bool cmp(const Yolo::Detection& a, const Yolo::Detection& b) { + return a.conf > b.conf; +} + +void nms(std::vector& res, float *output, float conf_thresh, float nms_thresh = 0.5) { + int det_size = sizeof(Yolo::Detection) / sizeof(float); + std::map> m; + for (int i = 0; i < output[0] && i < Yolo::MAX_OUTPUT_BBOX_COUNT; i++) { + if (output[1 + det_size * i + 4] <= conf_thresh) continue; + Yolo::Detection det; + memcpy(&det, &output[1 + det_size * i], det_size * sizeof(float)); + if (m.count(det.class_id) == 0) m.emplace(det.class_id, std::vector()); + m[det.class_id].push_back(det); + } + for (auto it = m.begin(); it != m.end(); it++) { + //std::cout << it->second[0].class_id << " --- " << std::endl; + auto& dets = it->second; + std::sort(dets.begin(), dets.end(), cmp); + for (size_t m = 0; m < dets.size(); ++m) { + auto& item = dets[m]; + res.push_back(item); + for (size_t n = m + 1; n < dets.size(); ++n) { + if (iou(item.bbox, dets[n].bbox) > nms_thresh) { + dets.erase(dets.begin() + n); + --n; + } + } + } + } +} + +// TensorRT weight files have a simple space delimited format: +// [type] [size] +std::map loadWeights(const std::string file) { + std::cout << "Loading weights: " << file << std::endl; + std::map weightMap; + + // Open weights file + std::ifstream input(file); + assert(input.is_open() && "Unable to load weight file. please check if the .wts file path is right!!!!!!"); + + // Read number of weight blobs + int32_t count; + input >> count; + assert(count > 0 && "Invalid weight map file."); + + while (count--) + { + Weights wt{ DataType::kFLOAT, nullptr, 0 }; + uint32_t size; + + // Read name and type of blob + std::string name; + input >> name >> std::dec >> size; + wt.type = DataType::kFLOAT; + + // Load blob + uint32_t* val = reinterpret_cast(malloc(sizeof(val) * size)); + for (uint32_t x = 0, y = size; x < y; ++x) + { + input >> std::hex >> val[x]; + } + wt.values = val; + + wt.count = size; + weightMap[name] = wt; + } + + return weightMap; +} + +IScaleLayer* addBatchNorm2d(INetworkDefinition *network, std::map& weightMap, ITensor& input, std::string lname, float eps) { + float *gamma = (float*)weightMap[lname + ".weight"].values; + float *beta = (float*)weightMap[lname + ".bias"].values; + float *mean = (float*)weightMap[lname + ".running_mean"].values; + float *var = (float*)weightMap[lname + ".running_var"].values; + int len = weightMap[lname + ".running_var"].count; + + float *scval = reinterpret_cast(malloc(sizeof(float) * len)); + for (int i = 0; i < len; i++) { + scval[i] = gamma[i] / sqrt(var[i] + eps); + } + Weights scale{ DataType::kFLOAT, scval, len }; + + float *shval = reinterpret_cast(malloc(sizeof(float) * len)); + for (int i = 0; i < len; i++) { + shval[i] = beta[i] - mean[i] * gamma[i] / sqrt(var[i] + eps); + } + Weights shift{ DataType::kFLOAT, shval, len }; + + float *pval = reinterpret_cast(malloc(sizeof(float) * len)); + for (int i = 0; i < len; i++) { + pval[i] = 1.0; + } + Weights power{ DataType::kFLOAT, pval, len }; + + weightMap[lname + ".scale"] = scale; + weightMap[lname + ".shift"] = shift; + weightMap[lname + ".power"] = power; + IScaleLayer* scale_1 = network->addScale(input, ScaleMode::kCHANNEL, shift, scale, power); + assert(scale_1); + return scale_1; +} + +ILayer* convBlock(INetworkDefinition *network, std::map& weightMap, ITensor& input, int outch, int ksize, int s, int g, std::string lname) { + Weights emptywts{ DataType::kFLOAT, nullptr, 0 }; + int p = ksize / 3; + IConvolutionLayer* conv1 = network->addConvolutionNd(input, outch, DimsHW{ ksize, ksize }, weightMap[lname + ".conv.weight"], emptywts); + assert(conv1); + conv1->setStrideNd(DimsHW{ s, s }); + conv1->setPaddingNd(DimsHW{ p, p }); + conv1->setNbGroups(g); + IScaleLayer* bn1 = addBatchNorm2d(network, weightMap, *conv1->getOutput(0), lname + ".bn", 1e-3); + + // silu = x * sigmoid + auto sig = network->addActivation(*bn1->getOutput(0), ActivationType::kSIGMOID); + assert(sig); + auto ew = network->addElementWise(*bn1->getOutput(0), *sig->getOutput(0), ElementWiseOperation::kPROD); + assert(ew); + return ew; +} + +ILayer* focus(INetworkDefinition *network, std::map& weightMap, ITensor& input, int inch, int outch, int ksize, std::string lname) { + ISliceLayer *s1 = network->addSlice(input, Dims3{ 0, 0, 0 }, Dims3{ inch, Yolo::INPUT_H / 2, Yolo::INPUT_W / 2 }, Dims3{ 1, 2, 2 }); + ISliceLayer *s2 = network->addSlice(input, Dims3{ 0, 1, 0 }, Dims3{ inch, Yolo::INPUT_H / 2, Yolo::INPUT_W / 2 }, Dims3{ 1, 2, 2 }); + ISliceLayer *s3 = network->addSlice(input, Dims3{ 0, 0, 1 }, Dims3{ inch, Yolo::INPUT_H / 2, Yolo::INPUT_W / 2 }, Dims3{ 1, 2, 2 }); + ISliceLayer *s4 = network->addSlice(input, Dims3{ 0, 1, 1 }, Dims3{ inch, Yolo::INPUT_H / 2, Yolo::INPUT_W / 2 }, Dims3{ 1, 2, 2 }); + ITensor* inputTensors[] = { s1->getOutput(0), s2->getOutput(0), s3->getOutput(0), s4->getOutput(0) }; + auto cat = network->addConcatenation(inputTensors, 4); + auto conv = convBlock(network, weightMap, *cat->getOutput(0), outch, ksize, 1, 1, lname + ".conv"); + return conv; +} + +ILayer* bottleneck(INetworkDefinition *network, std::map& weightMap, ITensor& input, int c1, int c2, bool shortcut, int g, float e, std::string lname) { + auto cv1 = convBlock(network, weightMap, input, (int)((float)c2 * e), 1, 1, 1, lname + ".cv1"); + auto cv2 = convBlock(network, weightMap, *cv1->getOutput(0), c2, 3, 1, g, lname + ".cv2"); + if (shortcut && c1 == c2) { + auto ew = network->addElementWise(input, *cv2->getOutput(0), ElementWiseOperation::kSUM); + return ew; + } + return cv2; +} + +ILayer* bottleneckCSP(INetworkDefinition *network, std::map& weightMap, ITensor& input, int c1, int c2, int n, bool shortcut, int g, float e, std::string lname) { + Weights emptywts{ DataType::kFLOAT, nullptr, 0 }; + int c_ = (int)((float)c2 * e); + auto cv1 = convBlock(network, weightMap, input, c_, 1, 1, 1, lname + ".cv1"); + auto cv2 = network->addConvolutionNd(input, c_, DimsHW{ 1, 1 }, weightMap[lname + ".cv2.weight"], emptywts); + ITensor *y1 = cv1->getOutput(0); + for (int i = 0; i < n; i++) { + auto b = bottleneck(network, weightMap, *y1, c_, c_, shortcut, g, 1.0, lname + ".m." + std::to_string(i)); + y1 = b->getOutput(0); + } + auto cv3 = network->addConvolutionNd(*y1, c_, DimsHW{ 1, 1 }, weightMap[lname + ".cv3.weight"], emptywts); + + ITensor* inputTensors[] = { cv3->getOutput(0), cv2->getOutput(0) }; + auto cat = network->addConcatenation(inputTensors, 2); + + IScaleLayer* bn = addBatchNorm2d(network, weightMap, *cat->getOutput(0), lname + ".bn", 1e-4); + auto lr = network->addActivation(*bn->getOutput(0), ActivationType::kLEAKY_RELU); + lr->setAlpha(0.1); + + auto cv4 = convBlock(network, weightMap, *lr->getOutput(0), c2, 1, 1, 1, lname + ".cv4"); + return cv4; +} + +ILayer* C3(INetworkDefinition *network, std::map& weightMap, ITensor& input, int c1, int c2, int n, bool shortcut, int g, float e, std::string lname) { + int c_ = (int)((float)c2 * e); + auto cv1 = convBlock(network, weightMap, input, c_, 1, 1, 1, lname + ".cv1"); + auto cv2 = convBlock(network, weightMap, input, c_, 1, 1, 1, lname + ".cv2"); + ITensor *y1 = cv1->getOutput(0); + for (int i = 0; i < n; i++) { + auto b = bottleneck(network, weightMap, *y1, c_, c_, shortcut, g, 1.0, lname + ".m." + std::to_string(i)); + y1 = b->getOutput(0); + } + + ITensor* inputTensors[] = { y1, cv2->getOutput(0) }; + auto cat = network->addConcatenation(inputTensors, 2); + + auto cv3 = convBlock(network, weightMap, *cat->getOutput(0), c2, 1, 1, 1, lname + ".cv3"); + return cv3; +} + +ILayer* SPP(INetworkDefinition *network, std::map& weightMap, ITensor& input, int c1, int c2, int k1, int k2, int k3, std::string lname) { + int c_ = c1 / 2; + auto cv1 = convBlock(network, weightMap, input, c_, 1, 1, 1, lname + ".cv1"); + + auto pool1 = network->addPoolingNd(*cv1->getOutput(0), PoolingType::kMAX, DimsHW{ k1, k1 }); + pool1->setPaddingNd(DimsHW{ k1 / 2, k1 / 2 }); + pool1->setStrideNd(DimsHW{ 1, 1 }); + auto pool2 = network->addPoolingNd(*cv1->getOutput(0), PoolingType::kMAX, DimsHW{ k2, k2 }); + pool2->setPaddingNd(DimsHW{ k2 / 2, k2 / 2 }); + pool2->setStrideNd(DimsHW{ 1, 1 }); + auto pool3 = network->addPoolingNd(*cv1->getOutput(0), PoolingType::kMAX, DimsHW{ k3, k3 }); + pool3->setPaddingNd(DimsHW{ k3 / 2, k3 / 2 }); + pool3->setStrideNd(DimsHW{ 1, 1 }); + + ITensor* inputTensors[] = { cv1->getOutput(0), pool1->getOutput(0), pool2->getOutput(0), pool3->getOutput(0) }; + auto cat = network->addConcatenation(inputTensors, 4); + + auto cv2 = convBlock(network, weightMap, *cat->getOutput(0), c2, 1, 1, 1, lname + ".cv2"); + return cv2; +} + +// SPPF +ILayer* SPPF(INetworkDefinition *network, std::map& weightMap, ITensor& input, int c1, int c2, int k, std::string lname) { + int c_ = c1 / 2; + auto cv1 = convBlock(network, weightMap, input, c_, 1, 1, 1, lname + ".cv1"); + + auto pool1 = network->addPoolingNd(*cv1->getOutput(0), PoolingType::kMAX, DimsHW{ k, k }); + pool1->setPaddingNd(DimsHW{ k / 2, k / 2 }); + pool1->setStrideNd(DimsHW{ 1, 1 }); + auto pool2 = network->addPoolingNd(*pool1->getOutput(0), PoolingType::kMAX, DimsHW{ k, k }); + pool2->setPaddingNd(DimsHW{ k / 2, k / 2 }); + pool2->setStrideNd(DimsHW{ 1, 1 }); + auto pool3 = network->addPoolingNd(*pool2->getOutput(0), PoolingType::kMAX, DimsHW{ k, k }); + pool3->setPaddingNd(DimsHW{ k / 2, k / 2 }); + pool3->setStrideNd(DimsHW{ 1, 1 }); + ITensor* inputTensors[] = { cv1->getOutput(0), pool1->getOutput(0), pool2->getOutput(0), pool3->getOutput(0) }; + auto cat = network->addConcatenation(inputTensors, 4); + auto cv2 = convBlock(network, weightMap, *cat->getOutput(0), c2, 1, 1, 1, lname + ".cv2"); + return cv2; +} + +// + +std::vector> getAnchors(std::map& weightMap, std::string lname) { + std::vector> anchors; + Weights wts = weightMap[lname + ".anchor_grid"]; + int anchor_len = Yolo::CHECK_COUNT * 2; + for (int i = 0; i < wts.count / anchor_len; i++) { + auto *p = (const float*)wts.values + i * anchor_len; + std::vector anchor(p, p + anchor_len); + anchors.push_back(anchor); + } + return anchors; +} + +IPluginV2Layer* addYoLoLayer(INetworkDefinition *network, std::map& weightMap, std::string lname, std::vector dets) { + auto creator = getPluginRegistry()->getPluginCreator("YoloLayer_TRT", "1"); + auto anchors = getAnchors(weightMap, lname); + PluginField plugin_fields[2]; + int netinfo[4] = {Yolo::CLASS_NUM, Yolo::INPUT_W, Yolo::INPUT_H, Yolo::MAX_OUTPUT_BBOX_COUNT}; + plugin_fields[0].data = netinfo; + plugin_fields[0].length = 4; + plugin_fields[0].name = "netinfo"; + plugin_fields[0].type = PluginFieldType::kFLOAT32; + int scale = 8; + std::vector kernels; + for (size_t i = 0; i < anchors.size(); i++) { + Yolo::YoloKernel kernel; + kernel.width = Yolo::INPUT_W / scale; + kernel.height = Yolo::INPUT_H / scale; + memcpy(kernel.anchors, &anchors[i][0], anchors[i].size() * sizeof(float)); + kernels.push_back(kernel); + scale *= 2; + } + plugin_fields[1].data = &kernels[0]; + plugin_fields[1].length = kernels.size(); + plugin_fields[1].name = "kernels"; + plugin_fields[1].type = PluginFieldType::kFLOAT32; + PluginFieldCollection plugin_data; + plugin_data.nbFields = 2; + plugin_data.fields = plugin_fields; + IPluginV2 *plugin_obj = creator->createPlugin("yololayer", &plugin_data); + std::vector input_tensors; + for (auto det: dets) { + input_tensors.push_back(det->getOutput(0)); + } + auto yolo = network->addPluginV2(&input_tensors[0], input_tensors.size(), *plugin_obj); + return yolo; +} +#endif + diff --git a/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/cuda_utils.h b/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/cuda_utils.h new file mode 100644 index 00000000..8fbd3199 --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/cuda_utils.h @@ -0,0 +1,18 @@ +#ifndef TRTX_CUDA_UTILS_H_ +#define TRTX_CUDA_UTILS_H_ + +#include + +#ifndef CUDA_CHECK +#define CUDA_CHECK(callstr)\ + {\ + cudaError_t error_code = callstr;\ + if (error_code != cudaSuccess) {\ + std::cerr << "CUDA error " << error_code << " at " << __FILE__ << ":" << __LINE__;\ + assert(0);\ + }\ + } +#endif // CUDA_CHECK + +#endif // TRTX_CUDA_UTILS_H_ + diff --git a/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/logging.h b/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/logging.h new file mode 100644 index 00000000..1339ee2f --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/logging.h @@ -0,0 +1,509 @@ +/* + * Copyright (c) 2019, NVIDIA CORPORATION. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef TENSORRT_LOGGING_H +#define TENSORRT_LOGGING_H + +#include "NvInferRuntimeCommon.h" +#include +#include +#include +#include +#include +#include +#include + +#if NV_TENSORRT_MAJOR >= 8 +#define TRT_NOEXCEPT noexcept +#else +#define TRT_NOEXCEPT +#endif + +using Severity = nvinfer1::ILogger::Severity; + +class LogStreamConsumerBuffer : public std::stringbuf +{ +public: + LogStreamConsumerBuffer(std::ostream& stream, const std::string& prefix, bool shouldLog) + : mOutput(stream) + , mPrefix(prefix) + , mShouldLog(shouldLog) + { + } + + LogStreamConsumerBuffer(LogStreamConsumerBuffer&& other) + : mOutput(other.mOutput) + { + } + + ~LogStreamConsumerBuffer() + { + // std::streambuf::pbase() gives a pointer to the beginning of the buffered part of the output sequence + // std::streambuf::pptr() gives a pointer to the current position of the output sequence + // if the pointer to the beginning is not equal to the pointer to the current position, + // call putOutput() to log the output to the stream + if (pbase() != pptr()) + { + putOutput(); + } + } + + // synchronizes the stream buffer and returns 0 on success + // synchronizing the stream buffer consists of inserting the buffer contents into the stream, + // resetting the buffer and flushing the stream + virtual int sync() + { + putOutput(); + return 0; + } + + void putOutput() + { + if (mShouldLog) + { + // prepend timestamp + std::time_t timestamp = std::time(nullptr); + tm* tm_local = std::localtime(×tamp); + std::cout << "["; + std::cout << std::setw(2) << std::setfill('0') << 1 + tm_local->tm_mon << "/"; + std::cout << std::setw(2) << std::setfill('0') << tm_local->tm_mday << "/"; + std::cout << std::setw(4) << std::setfill('0') << 1900 + tm_local->tm_year << "-"; + std::cout << std::setw(2) << std::setfill('0') << tm_local->tm_hour << ":"; + std::cout << std::setw(2) << std::setfill('0') << tm_local->tm_min << ":"; + std::cout << std::setw(2) << std::setfill('0') << tm_local->tm_sec << "] "; + // std::stringbuf::str() gets the string contents of the buffer + // insert the buffer contents pre-appended by the appropriate prefix into the stream + mOutput << mPrefix << str(); + // set the buffer to empty + str(""); + // flush the stream + mOutput.flush(); + } + } + + void setShouldLog(bool shouldLog) + { + mShouldLog = shouldLog; + } + +private: + std::ostream& mOutput; + std::string mPrefix; + bool mShouldLog; +}; + +//! +//! \class LogStreamConsumerBase +//! \brief Convenience object used to initialize LogStreamConsumerBuffer before std::ostream in LogStreamConsumer +//! +class LogStreamConsumerBase +{ +public: + LogStreamConsumerBase(std::ostream& stream, const std::string& prefix, bool shouldLog) + : mBuffer(stream, prefix, shouldLog) + { + } + +protected: + LogStreamConsumerBuffer mBuffer; +}; + +//! +//! \class LogStreamConsumer +//! \brief Convenience object used to facilitate use of C++ stream syntax when logging messages. +//! Order of base classes is LogStreamConsumerBase and then std::ostream. +//! This is because the LogStreamConsumerBase class is used to initialize the LogStreamConsumerBuffer member field +//! in LogStreamConsumer and then the address of the buffer is passed to std::ostream. +//! This is necessary to prevent the address of an uninitialized buffer from being passed to std::ostream. +//! Please do not change the order of the parent classes. +//! +class LogStreamConsumer : protected LogStreamConsumerBase, public std::ostream +{ +public: + //! \brief Creates a LogStreamConsumer which logs messages with level severity. + //! Reportable severity determines if the messages are severe enough to be logged. + LogStreamConsumer(Severity reportableSeverity, Severity severity) + : LogStreamConsumerBase(severityOstream(severity), severityPrefix(severity), severity <= reportableSeverity) + , std::ostream(&mBuffer) // links the stream buffer with the stream + , mShouldLog(severity <= reportableSeverity) + , mSeverity(severity) + { + } + + LogStreamConsumer(LogStreamConsumer&& other) + : LogStreamConsumerBase(severityOstream(other.mSeverity), severityPrefix(other.mSeverity), other.mShouldLog) + , std::ostream(&mBuffer) // links the stream buffer with the stream + , mShouldLog(other.mShouldLog) + , mSeverity(other.mSeverity) + { + } + + void setReportableSeverity(Severity reportableSeverity) + { + mShouldLog = mSeverity <= reportableSeverity; + mBuffer.setShouldLog(mShouldLog); + } + +private: + static std::ostream& severityOstream(Severity severity) + { + return severity >= Severity::kINFO ? std::cout : std::cerr; + } + + static std::string severityPrefix(Severity severity) + { + switch (severity) + { + case Severity::kINTERNAL_ERROR: return "[F] "; + case Severity::kERROR: return "[E] "; + case Severity::kWARNING: return "[W] "; + case Severity::kINFO: return "[I] "; + case Severity::kVERBOSE: return "[V] "; + default: assert(0); return ""; + } + } + + bool mShouldLog; + Severity mSeverity; +}; + +//! \class Logger +//! +//! \brief Class which manages logging of TensorRT tools and samples +//! +//! \details This class provides a common interface for TensorRT tools and samples to log information to the console, +//! and supports logging two types of messages: +//! +//! - Debugging messages with an associated severity (info, warning, error, or internal error/fatal) +//! - Test pass/fail messages +//! +//! The advantage of having all samples use this class for logging as opposed to emitting directly to stdout/stderr is +//! that the logic for controlling the verbosity and formatting of sample output is centralized in one location. +//! +//! In the future, this class could be extended to support dumping test results to a file in some standard format +//! (for example, JUnit XML), and providing additional metadata (e.g. timing the duration of a test run). +//! +//! TODO: For backwards compatibility with existing samples, this class inherits directly from the nvinfer1::ILogger +//! interface, which is problematic since there isn't a clean separation between messages coming from the TensorRT +//! library and messages coming from the sample. +//! +//! In the future (once all samples are updated to use Logger::getTRTLogger() to access the ILogger) we can refactor the +//! class to eliminate the inheritance and instead make the nvinfer1::ILogger implementation a member of the Logger +//! object. + +class Logger : public nvinfer1::ILogger +{ +public: + Logger(Severity severity = Severity::kWARNING) + : mReportableSeverity(severity) + { + } + + //! + //! \enum TestResult + //! \brief Represents the state of a given test + //! + enum class TestResult + { + kRUNNING, //!< The test is running + kPASSED, //!< The test passed + kFAILED, //!< The test failed + kWAIVED //!< The test was waived + }; + + //! + //! \brief Forward-compatible method for retrieving the nvinfer::ILogger associated with this Logger + //! \return The nvinfer1::ILogger associated with this Logger + //! + //! TODO Once all samples are updated to use this method to register the logger with TensorRT, + //! we can eliminate the inheritance of Logger from ILogger + //! + nvinfer1::ILogger& getTRTLogger() + { + return *this; + } + + //! + //! \brief Implementation of the nvinfer1::ILogger::log() virtual method + //! + //! Note samples should not be calling this function directly; it will eventually go away once we eliminate the + //! inheritance from nvinfer1::ILogger + //! + void log(Severity severity, const char* msg) TRT_NOEXCEPT override + { + LogStreamConsumer(mReportableSeverity, severity) << "[TRT] " << std::string(msg) << std::endl; + } + + //! + //! \brief Method for controlling the verbosity of logging output + //! + //! \param severity The logger will only emit messages that have severity of this level or higher. + //! + void setReportableSeverity(Severity severity) + { + mReportableSeverity = severity; + } + + //! + //! \brief Opaque handle that holds logging information for a particular test + //! + //! This object is an opaque handle to information used by the Logger to print test results. + //! The sample must call Logger::defineTest() in order to obtain a TestAtom that can be used + //! with Logger::reportTest{Start,End}(). + //! + class TestAtom + { + public: + TestAtom(TestAtom&&) = default; + + private: + friend class Logger; + + TestAtom(bool started, const std::string& name, const std::string& cmdline) + : mStarted(started) + , mName(name) + , mCmdline(cmdline) + { + } + + bool mStarted; + std::string mName; + std::string mCmdline; + }; + + //! + //! \brief Define a test for logging + //! + //! \param[in] name The name of the test. This should be a string starting with + //! "TensorRT" and containing dot-separated strings containing + //! the characters [A-Za-z0-9_]. + //! For example, "TensorRT.sample_googlenet" + //! \param[in] cmdline The command line used to reproduce the test + // + //! \return a TestAtom that can be used in Logger::reportTest{Start,End}(). + //! + static TestAtom defineTest(const std::string& name, const std::string& cmdline) + { + return TestAtom(false, name, cmdline); + } + + //! + //! \brief A convenience overloaded version of defineTest() that accepts an array of command-line arguments + //! as input + //! + //! \param[in] name The name of the test + //! \param[in] argc The number of command-line arguments + //! \param[in] argv The array of command-line arguments (given as C strings) + //! + //! \return a TestAtom that can be used in Logger::reportTest{Start,End}(). + static TestAtom defineTest(const std::string& name, int argc, char const* const* argv) + { + auto cmdline = genCmdlineString(argc, argv); + return defineTest(name, cmdline); + } + + //! + //! \brief Report that a test has started. + //! + //! \pre reportTestStart() has not been called yet for the given testAtom + //! + //! \param[in] testAtom The handle to the test that has started + //! + static void reportTestStart(TestAtom& testAtom) + { + reportTestResult(testAtom, TestResult::kRUNNING); + assert(!testAtom.mStarted); + testAtom.mStarted = true; + } + + //! + //! \brief Report that a test has ended. + //! + //! \pre reportTestStart() has been called for the given testAtom + //! + //! \param[in] testAtom The handle to the test that has ended + //! \param[in] result The result of the test. Should be one of TestResult::kPASSED, + //! TestResult::kFAILED, TestResult::kWAIVED + //! + static void reportTestEnd(const TestAtom& testAtom, TestResult result) + { + assert(result != TestResult::kRUNNING); + assert(testAtom.mStarted); + reportTestResult(testAtom, result); + } + + static int reportPass(const TestAtom& testAtom) + { + reportTestEnd(testAtom, TestResult::kPASSED); + return EXIT_SUCCESS; + } + + static int reportFail(const TestAtom& testAtom) + { + reportTestEnd(testAtom, TestResult::kFAILED); + return EXIT_FAILURE; + } + + static int reportWaive(const TestAtom& testAtom) + { + reportTestEnd(testAtom, TestResult::kWAIVED); + return EXIT_SUCCESS; + } + + static int reportTest(const TestAtom& testAtom, bool pass) + { + return pass ? reportPass(testAtom) : reportFail(testAtom); + } + + Severity getReportableSeverity() const + { + return mReportableSeverity; + } + +private: + //! + //! \brief returns an appropriate string for prefixing a log message with the given severity + //! + static const char* severityPrefix(Severity severity) + { + switch (severity) + { + case Severity::kINTERNAL_ERROR: return "[F] "; + case Severity::kERROR: return "[E] "; + case Severity::kWARNING: return "[W] "; + case Severity::kINFO: return "[I] "; + case Severity::kVERBOSE: return "[V] "; + default: assert(0); return ""; + } + } + + //! + //! \brief returns an appropriate string for prefixing a test result message with the given result + //! + static const char* testResultString(TestResult result) + { + switch (result) + { + case TestResult::kRUNNING: return "RUNNING"; + case TestResult::kPASSED: return "PASSED"; + case TestResult::kFAILED: return "FAILED"; + case TestResult::kWAIVED: return "WAIVED"; + default: assert(0); return ""; + } + } + + //! + //! \brief returns an appropriate output stream (cout or cerr) to use with the given severity + //! + static std::ostream& severityOstream(Severity severity) + { + return severity >= Severity::kINFO ? std::cout : std::cerr; + } + + //! + //! \brief method that implements logging test results + //! + static void reportTestResult(const TestAtom& testAtom, TestResult result) + { + severityOstream(Severity::kINFO) << "&&&& " << testResultString(result) << " " << testAtom.mName << " # " + << testAtom.mCmdline << std::endl; + } + + //! + //! \brief generate a command line string from the given (argc, argv) values + //! + static std::string genCmdlineString(int argc, char const* const* argv) + { + std::stringstream ss; + for (int i = 0; i < argc; i++) + { + if (i > 0) + ss << " "; + ss << argv[i]; + } + return ss.str(); + } + + Severity mReportableSeverity; +}; + +namespace +{ + +//! +//! \brief produces a LogStreamConsumer object that can be used to log messages of severity kVERBOSE +//! +//! Example usage: +//! +//! LOG_VERBOSE(logger) << "hello world" << std::endl; +//! +inline LogStreamConsumer LOG_VERBOSE(const Logger& logger) +{ + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kVERBOSE); +} + +//! +//! \brief produces a LogStreamConsumer object that can be used to log messages of severity kINFO +//! +//! Example usage: +//! +//! LOG_INFO(logger) << "hello world" << std::endl; +//! +inline LogStreamConsumer LOG_INFO(const Logger& logger) +{ + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINFO); +} + +//! +//! \brief produces a LogStreamConsumer object that can be used to log messages of severity kWARNING +//! +//! Example usage: +//! +//! LOG_WARN(logger) << "hello world" << std::endl; +//! +inline LogStreamConsumer LOG_WARN(const Logger& logger) +{ + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kWARNING); +} + +//! +//! \brief produces a LogStreamConsumer object that can be used to log messages of severity kERROR +//! +//! Example usage: +//! +//! LOG_ERROR(logger) << "hello world" << std::endl; +//! +inline LogStreamConsumer LOG_ERROR(const Logger& logger) +{ + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kERROR); +} + +//! +//! \brief produces a LogStreamConsumer object that can be used to log messages of severity kINTERNAL_ERROR +// ("fatal" severity) +//! +//! Example usage: +//! +//! LOG_FATAL(logger) << "hello world" << std::endl; +//! +inline LogStreamConsumer LOG_FATAL(const Logger& logger) +{ + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINTERNAL_ERROR); +} + +} // anonymous namespace + +#endif // TENSORRT_LOGGING_H diff --git a/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/utils.h b/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/utils.h new file mode 100644 index 00000000..fe50d039 --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/utils.h @@ -0,0 +1,52 @@ +#ifndef TRTX_YOLOV5_UTILS_H_ +#define TRTX_YOLOV5_UTILS_H_ + +#include +#include + +static inline cv::Mat preprocess_img(cv::Mat& img, int input_w, int input_h) { + int w, h, x, y; + float r_w = input_w / (img.cols*1.0); + float r_h = input_h / (img.rows*1.0); + if (r_h > r_w) { + w = input_w; + h = r_w * img.rows; + x = 0; + y = (input_h - h) / 2; + } else { + w = r_h * img.cols; + h = input_h; + x = (input_w - w) / 2; + y = 0; + } + cv::Mat re(h, w, CV_8UC3); + cv::resize(img, re, re.size(), 0, 0, cv::INTER_LINEAR); + cv::Mat out(input_h, input_w, CV_8UC3, cv::Scalar(128, 128, 128)); + re.copyTo(out(cv::Rect(x, y, re.cols, re.rows))); + return out; +} + +static inline int read_files_in_dir(const char *p_dir_name, std::vector &file_names) { + DIR *p_dir = opendir(p_dir_name); + if (p_dir == nullptr) { + return -1; + } + + struct dirent* p_file = nullptr; + while ((p_file = readdir(p_dir)) != nullptr) { + if (strcmp(p_file->d_name, ".") != 0 && + strcmp(p_file->d_name, "..") != 0) { + //std::string cur_file_name(p_dir_name); + //cur_file_name += "/"; + //cur_file_name += p_file->d_name; + std::string cur_file_name(p_file->d_name); + file_names.push_back(cur_file_name); + } + } + + closedir(p_dir); + return 0; +} + +#endif // TRTX_YOLOV5_UTILS_H_ + diff --git a/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/utils.hpp b/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/utils.hpp new file mode 100644 index 00000000..a96504f8 --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/utils.hpp @@ -0,0 +1,109 @@ +#ifndef DRAWING_UTILS_HPP +#define DRAWING_UTILS_HPP + +#include + +#include +#include + +float const id_colors[5][3] = { + { 232.0f, 176.0f, 59.0f}, + { 175.0f, 208.0f, 25.0f}, + { 102.0f, 205.0f, 105.0f}, + { 185.0f, 0.0f, 255.0f}, + { 99.0f, 107.0f, 252.0f} +}; + +inline cv::Scalar generateColorID_u(int idx) { + if (idx < 0) return cv::Scalar(236, 184, 36, 255); + int color_idx = idx % 5; + return cv::Scalar(id_colors[color_idx][0], id_colors[color_idx][1], id_colors[color_idx][2], 255); +} + +inline sl::float4 generateColorID_f(int idx) { + auto clr_u = generateColorID_u(idx); + return sl::float4(static_cast (clr_u.val[0]) / 255.f, static_cast (clr_u.val[1]) / 255.f, static_cast (clr_u.val[2]) / 255.f, 1.f); +} + +inline bool renderObject(const sl::ObjectData& 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); +} + +float const class_colors[6][3] = { + { 44.0f, 117.0f, 255.0f}, // PEOPLE + { 255.0f, 0.0f, 255.0f}, // VEHICLE + { 0.0f, 0.0f, 255.0f}, + { 0.0f, 255.0f, 255.0f}, + { 0.0f, 255.0f, 0.0f}, + { 255.0f, 255.0f, 255.0f} +}; + +inline sl::float4 getColorClass(int idx) { + idx = std::min(5, idx); + sl::float4 clr(class_colors[idx][0], class_colors[idx][1], class_colors[idx][2], 1.f); + return clr / 255.f; +} + +template +inline uchar _applyFading(T val, float current_alpha, double current_clr) { + return static_cast (current_alpha * current_clr + (1.0 - current_alpha) * val); +} + +inline cv::Vec4b applyFading(cv::Scalar val, float current_alpha, cv::Scalar current_clr) { + cv::Vec4b out; + out[0] = _applyFading(val.val[0], current_alpha, current_clr.val[0]); + out[1] = _applyFading(val.val[1], current_alpha, current_clr.val[1]); + out[2] = _applyFading(val.val[2], current_alpha, current_clr.val[2]); + out[3] = 255; + return out; +} + +inline void drawVerticalLine( + cv::Mat &left_display, + cv::Point2i start_pt, + cv::Point2i end_pt, + cv::Scalar clr, + int thickness) { + int n_steps = 7; + cv::Point2i pt1, pt4; + pt1.x = ((n_steps - 1) * start_pt.x + end_pt.x) / n_steps; + pt1.y = ((n_steps - 1) * start_pt.y + end_pt.y) / n_steps; + + pt4.x = (start_pt.x + (n_steps - 1) * end_pt.x) / n_steps; + pt4.y = (start_pt.y + (n_steps - 1) * end_pt.y) / n_steps; + + cv::line(left_display, start_pt, pt1, clr, thickness); + cv::line(left_display, pt4, end_pt, clr, thickness); +} + +inline cv::Mat slMat2cvMat(sl::Mat& input) { + // Mapping between MAT_TYPE and CV_TYPE + int cv_type = -1; + switch (input.getDataType()) { + case sl::MAT_TYPE::F32_C1: cv_type = CV_32FC1; + break; + case sl::MAT_TYPE::F32_C2: cv_type = CV_32FC2; + break; + case sl::MAT_TYPE::F32_C3: cv_type = CV_32FC3; + break; + case sl::MAT_TYPE::F32_C4: cv_type = CV_32FC4; + break; + case sl::MAT_TYPE::U8_C1: cv_type = CV_8UC1; + break; + case sl::MAT_TYPE::U8_C2: cv_type = CV_8UC2; + break; + case sl::MAT_TYPE::U8_C3: cv_type = CV_8UC3; + break; + case sl::MAT_TYPE::U8_C4: cv_type = CV_8UC4; + break; + default: break; + } + + return cv::Mat(input.getHeight(), input.getWidth(), cv_type, input.getPtr(sl::MEM::CPU)); +} + + +#endif diff --git a/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/yololayer.h b/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/yololayer.h new file mode 100644 index 00000000..7224bde4 --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/include/yololayer.h @@ -0,0 +1,146 @@ +#ifndef _YOLO_LAYER_H +#define _YOLO_LAYER_H + +#include +#include +#include "NvInfer.h" + + +#if NV_TENSORRT_MAJOR >= 8 +#define TRT_NOEXCEPT noexcept +#define TRT_CONST_ENQUEUE const +#else +#define TRT_NOEXCEPT +#define TRT_CONST_ENQUEUE +#endif + +namespace Yolo +{ + static constexpr int CHECK_COUNT = 3; + static constexpr float IGNORE_THRESH = 0.1f; + struct YoloKernel + { + int width; + int height; + float anchors[CHECK_COUNT * 2]; + }; + static constexpr int MAX_OUTPUT_BBOX_COUNT = 1000; + static constexpr int CLASS_NUM = 80; + static constexpr int INPUT_H = 640; // yolov5's input height and width must be divisible by 32. + static constexpr int INPUT_W = 640; + + static constexpr int LOCATIONS = 4; + struct alignas(float) Detection { + //center_x center_y w h + float bbox[LOCATIONS]; + float conf; // bbox_conf * cls_conf + float class_id; + }; +} + +namespace nvinfer1 +{ + class YoloLayerPlugin : public IPluginV2IOExt + { + public: + YoloLayerPlugin(int classCount, int netWidth, int netHeight, int maxOut, const std::vector& vYoloKernel); + YoloLayerPlugin(const void* data, size_t length); + ~YoloLayerPlugin(); + + int getNbOutputs() const TRT_NOEXCEPT override + { + return 1; + } + + Dims getOutputDimensions(int index, const Dims* inputs, int nbInputDims) TRT_NOEXCEPT override; + + int initialize() TRT_NOEXCEPT override; + + virtual void terminate() TRT_NOEXCEPT override {}; + + virtual size_t getWorkspaceSize(int maxBatchSize) const TRT_NOEXCEPT override { return 0; } + + virtual int enqueue(int batchSize, const void* const* inputs, void*TRT_CONST_ENQUEUE* outputs, void* workspace, cudaStream_t stream) TRT_NOEXCEPT override; + + virtual size_t getSerializationSize() const TRT_NOEXCEPT override; + + virtual void serialize(void* buffer) const TRT_NOEXCEPT override; + + bool supportsFormatCombination(int pos, const PluginTensorDesc* inOut, int nbInputs, int nbOutputs) const TRT_NOEXCEPT override { + return inOut[pos].format == TensorFormat::kLINEAR && inOut[pos].type == DataType::kFLOAT; + } + + const char* getPluginType() const TRT_NOEXCEPT override; + + const char* getPluginVersion() const TRT_NOEXCEPT override; + + void destroy() TRT_NOEXCEPT override; + + IPluginV2IOExt* clone() const TRT_NOEXCEPT override; + + void setPluginNamespace(const char* pluginNamespace) TRT_NOEXCEPT override; + + const char* getPluginNamespace() const TRT_NOEXCEPT override; + + DataType getOutputDataType(int index, const nvinfer1::DataType* inputTypes, int nbInputs) const TRT_NOEXCEPT override; + + bool isOutputBroadcastAcrossBatch(int outputIndex, const bool* inputIsBroadcasted, int nbInputs) const TRT_NOEXCEPT override; + + bool canBroadcastInputAcrossBatch(int inputIndex) const TRT_NOEXCEPT override; + + void attachToContext( + cudnnContext* cudnnContext, cublasContext* cublasContext, IGpuAllocator* gpuAllocator) TRT_NOEXCEPT override; + + void configurePlugin(const PluginTensorDesc* in, int nbInput, const PluginTensorDesc* out, int nbOutput) TRT_NOEXCEPT override; + + void detachFromContext() TRT_NOEXCEPT override; + + private: + void forwardGpu(const float* const* inputs, float *output, cudaStream_t stream, int batchSize = 1); + int mThreadCount = 256; + const char* mPluginNamespace; + int mKernelCount; + int mClassCount; + int mYoloV5NetWidth; + int mYoloV5NetHeight; + int mMaxOutObject; + std::vector mYoloKernel; + void** mAnchor; + }; + + class YoloPluginCreator : public IPluginCreator + { + public: + YoloPluginCreator(); + + ~YoloPluginCreator() override = default; + + const char* getPluginName() const TRT_NOEXCEPT override; + + const char* getPluginVersion() const TRT_NOEXCEPT override; + + const PluginFieldCollection* getFieldNames() TRT_NOEXCEPT override; + + IPluginV2IOExt* createPlugin(const char* name, const PluginFieldCollection* fc) TRT_NOEXCEPT override; + + IPluginV2IOExt* deserializePlugin(const char* name, const void* serialData, size_t serialLength) TRT_NOEXCEPT override; + + void setPluginNamespace(const char* libNamespace) TRT_NOEXCEPT override + { + mNamespace = libNamespace; + } + + const char* getPluginNamespace() const TRT_NOEXCEPT override + { + return mNamespace.c_str(); + } + + private: + std::string mNamespace; + static PluginFieldCollection mFC; + static std::vector mPluginAttributes; + }; + REGISTER_TENSORRT_PLUGIN(YoloPluginCreator); +}; + +#endif diff --git a/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/src/GLViewer.cpp b/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/src/GLViewer.cpp new file mode 100644 index 00000000..51265df0 --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/src/GLViewer.cpp @@ -0,0 +1,1038 @@ +#include "GLViewer.hpp" +#include + + +#if defined(_DEBUG) && defined(_WIN32) +//#error "This sample should not be built in Debug mode, use RelWithDebInfo if you want to do step by step." +#endif + +#define FADED_RENDERING +const float grid_size = 10.0f; + +GLchar* VERTEX_SHADER = + "#version 330 core\n" + "layout(location = 0) in vec3 in_Vertex;\n" + "layout(location = 1) in vec4 in_Color;\n" + "uniform mat4 u_mvpMatrix;\n" + "out vec4 b_color;\n" + "void main() {\n" + " b_color = in_Color;\n" + " gl_Position = u_mvpMatrix * vec4(in_Vertex, 1);\n" + "}"; + +GLchar* 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" + "}"; + +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); +} + +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 (available) { + available = false; + pointCloud_.close(); + } +} + +bool GLViewer::isAvailable() { + glutMainLoopEvent(); + return available; +} + +Simple3DObject createFrustum(sl::CameraParameters param) { + + // Create 3D axis + Simple3DObject it(sl::Translation(0, 0, 0), true); + + float Z_ = -150; + sl::float3 cam_0(0, 0, 0); + sl::float3 cam_1, cam_2, cam_3, cam_4; + + float fx_ = 1.f / param.fx; + float fy_ = 1.f / param.fy; + + cam_1.z = Z_; + cam_1.x = (0 - param.cx) * Z_ *fx_; + cam_1.y = (0 - param.cy) * Z_ *fy_; + + cam_2.z = Z_; + cam_2.x = (param.image_size.width - param.cx) * Z_ *fx_; + cam_2.y = (0 - param.cy) * Z_ *fy_; + + cam_3.z = Z_; + cam_3.x = (param.image_size.width - param.cx) * Z_ *fx_; + cam_3.y = (param.image_size.height - param.cy) * Z_ *fy_; + + cam_4.z = Z_; + cam_4.x = (0 - param.cx) * Z_ *fx_; + cam_4.y = (param.image_size.height - param.cy) * Z_ *fy_; + + sl::float4 clr(0.8f, 0.5f, 0.2f, 1.0f); + it.addTriangle(cam_0, cam_1, cam_2, clr); + it.addTriangle(cam_0, cam_2, cam_3, clr); + it.addTriangle(cam_0, cam_3, cam_4, clr); + it.addTriangle(cam_0, cam_4, cam_1, clr); + + it.setDrawingType(GL_TRIANGLES); + return it; +} + +void CloseFunc(void) { + if (currentInstance_) + currentInstance_->exit(); +} + +void GLViewer::init(int argc, char **argv, sl::CameraParameters ¶m, bool isTrackingON) { + 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 Object Detection"); + + 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_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + glEnable(GL_LINE_SMOOTH); + glHint(GL_LINE_SMOOTH_HINT, GL_NICEST); + + pointCloud_.initialize(param.image_size); + + isTrackingON_ = isTrackingON; + + // Compile and create the shader + shader.it = Shader(VERTEX_SHADER, FRAGMENT_SHADER); + shader.MVP_Mat = glGetUniformLocation(shader.it.getProgramId(), "u_mvpMatrix"); + + shaderLine.it = Shader(VERTEX_SHADER, FRAGMENT_SHADER); + shaderLine.MVP_Mat = glGetUniformLocation(shaderLine.it.getProgramId(), "u_mvpMatrix"); + + // Create the camera + camera_ = CameraGL(sl::Translation(0, 0, 1000), sl::Translation(0, 0, -100)); + + frustum = createFrustum(param); + frustum.pushToGPU(); + + BBox_edges = Simple3DObject(sl::Translation(0, 0, 0), false); + BBox_edges.setDrawingType(GL_LINES); + + BBox_faces = Simple3DObject(sl::Translation(0, 0, 0), false); + BBox_faces.setDrawingType(GL_QUADS); + + skeletons = Simple3DObject(sl::Translation(0, 0, 0), false); + skeletons.setDrawingType(GL_LINES); + + bckgrnd_clr = sl::float4(0.2f, 0.19f, 0.2f, 1.0f); + + floor_grid = Simple3DObject(sl::Translation(0, 0, 0), true); + floor_grid.setDrawingType(GL_LINES); + + float limit = 20.f; + sl::float4 clr_grid(80, 80, 80, 255); + clr_grid /= 255.f; + float height = -3; + for (int i = (int) (-limit); i <= (int) (limit); i++) + addVert(floor_grid, i * 1000, limit * 1000, height * 1000, clr_grid); + + floor_grid.pushToGPU(); + + // 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; +} + +void GLViewer::render() { + if (available) { + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glClearColor(bckgrnd_clr.b, bckgrnd_clr.g, bckgrnd_clr.r, bckgrnd_clr.a); + update(); + draw(); + glutSwapBuffers(); + glutPostRedisplay(); + } +} + +void GLViewer::updateData(sl::Mat &matXYZRGBA, std::vector &objs, sl::Transform& pose) { + mtx.lock(); + pointCloud_.pushNewPC(matXYZRGBA); + BBox_edges.clear(); + BBox_faces.clear(); + objectsName.clear(); + skeletons.clear(); + cam_pose = pose; + sl::float3 tr_0(0, 0, 0); + cam_pose.setTranslation(tr_0); + + for (unsigned int i = 0; i < objs.size(); i++) { + if (renderObject(objs[i], isTrackingON_)) + { + auto bb_ = objs[i].bounding_box; + if (!bb_.empty()) { + auto clr_class = getColorClass((int) objs[i].raw_label); + auto clr_id = generateColorID_f(objs[i].id); + + if (objs[i].tracking_state != sl::OBJECT_TRACKING_STATE::OK) + clr_id = clr_class; + else + createIDRendering(objs[i].position, clr_id, objs[i].id); + + if (0) { // display centroid as a cross + sl::float3 centroid = objs[i].position; + const float size_cendtroid = 50; // mm + sl::float3 centroid1 = centroid; + centroid1.y += size_cendtroid; + sl::float3 centroid2 = objs[i].position; + centroid2.y -= size_cendtroid; + sl::float3 centroid3 = objs[i].position; + centroid3.x += size_cendtroid; + sl::float3 centroid4 = objs[i].position; + centroid4.x -= size_cendtroid; + + BBox_edges.addLine(centroid1, centroid2, sl::float4(1.f, 0.5f, 0.5f, 1.f)); + BBox_edges.addLine(centroid3, centroid4, sl::float4(1.f, 0.5f, 0.5f, 1.f)); + } + + //Display sekeleton if available + auto clr_bones = generateColorID_f(objs[i].id); + auto keypoints = objs[i].keypoint; + if (keypoints.size() > 0) { + for (auto &limb : sl::BODY_BONES) { + sl::float3 kp_1 = keypoints[(int) limb.first]; + sl::float3 kp_2 = keypoints[(int) limb.second]; + if (std::isfinite(kp_1.x) && std::isfinite(kp_2.x)) + skeletons.addLine(kp_1, kp_2, clr_bones); + } + } + createBboxRendering(bb_, clr_id); + } + } + } + mtx.unlock(); +} + +void GLViewer::createBboxRendering(std::vector &bbox, sl::float4 bbox_clr) { + // First create top and bottom full edges + BBox_edges.addFullEdges(bbox, bbox_clr); + // Add faded vertical edges + BBox_edges.addVerticalEdges(bbox, bbox_clr); + // Add faces + BBox_faces.addVerticalFaces(bbox, bbox_clr); + // Add top face + BBox_faces.addTopFace(bbox, bbox_clr); +} + +void GLViewer::createIDRendering(sl::float3 & center, sl::float4 clr, unsigned int id) { + ObjectClassName tmp; + tmp.name = "ID: " + std::to_string(id); + tmp.color = clr; + tmp.position = center; // Reference point + objectsName.push_back(tmp); +} + +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 || keyStates_['R'] == KEY_STATE::UP) { + camera_.setPosition(sl::Translation(0.0f, 0.0f, 1500.0f)); + //camera_.setOffsetFromPosition(sl::Translation(0.0f, 0.0f, 1500.0f)); + camera_.setDirection(sl::Translation(0.0f, 0.0f, 1.0f), sl::Translation(0.0f, 1.0f, 0.0f)); + } + + if (keyStates_['t'] == KEY_STATE::UP || keyStates_['T'] == KEY_STATE::UP) { + camera_.setPosition(sl::Translation(0.0f, 0.0f, 1500.0f)); + camera_.setOffsetFromPosition(sl::Translation(0.0f, 0.0f, 6000.0f)); + camera_.translate(sl::Translation(0.0f, 1500.0f, -4000.0f)); + camera_.setDirection(sl::Translation(0.0f, -1.0f, 0.0f), sl::Translation(0.0f, 1.0f, 0.0f)); + } + + // 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 * 1000); + camera_.translate(camera_.getRight() * (float) mouseMotion_[0] * MOUSE_T_SENSITIVITY * 1000); + } + + // 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 * 1000 * -1); + } else if (/*distance < camera_.getZFar()*/ mouseWheelPosition_ < 0) {// unzoom + //camera_.setOffsetFromPosition(camera_.getOffsetFromPosition() * MOUSE_DZ_SENSITIVITY); + camera_.translate(camera_.getForward() * MOUSE_UZ_SENSITIVITY * 1000); + } + } + + camera_.update(); + mtx.lock(); + // Update point cloud buffers + BBox_edges.pushToGPU(); + BBox_faces.pushToGPU(); + skeletons.pushToGPU(); + pointCloud_.update(); + mtx.unlock(); + clearInputs(); +} + +void GLViewer::draw() { + sl::Transform vpMatrix = camera_.getViewProjectionMatrix(); + + glUseProgram(shaderLine.it.getProgramId()); + glUniformMatrix4fv(shaderLine.MVP_Mat, 1, GL_TRUE, vpMatrix.m); + glLineWidth(1.f); + floor_grid.draw(); + glUseProgram(0); + + glPointSize(1.f); + + // Apply IMU Rotation compensation + vpMatrix = vpMatrix * cam_pose; + pointCloud_.draw(vpMatrix); + + glUseProgram(shader.it.getProgramId()); + glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); + glUniformMatrix4fv(shader.MVP_Mat, 1, GL_TRUE, vpMatrix.m); + glLineWidth(2.f); + frustum.draw(); + glLineWidth(1.5f); + BBox_edges.draw(); + glLineWidth(4.f); + skeletons.draw(); + glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); + BBox_faces.draw(); + glUseProgram(0); +} + +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; + proj3D_cam.y += 1000.f; + 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() { + const sl::Transform vpMatrix = camera_.getViewProjectionMatrix() * cam_pose; + sl::Resolution wnd_size(glutGet(GLUT_WINDOW_WIDTH), glutGet(GLUT_WINDOW_HEIGHT)); + for (auto it : objectsName) { + auto pt2d = compute3Dprojection(it.position, vpMatrix, wnd_size); + glColor4f(it.color.b, it.color.g, it.color.r, it.color.a); + const auto *string = it.name.c_str(); + glWindowPos2f(pt2d.x, pt2d.y); + int 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::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; +} + +void GLViewer::keyReleasedCallback(unsigned char c, int x, int y) { + currentInstance_->keyStates_[c] = KEY_STATE::UP; +} + +void GLViewer::idle() { + glutPostRedisplay(); +} + +Simple3DObject::Simple3DObject(sl::Translation position, bool isStatic) : isStatic_(isStatic) { + vaoID_ = 0; + drawingType_ = GL_TRIANGLES; + position_ = position; + rotation_.setIdentity(); +} + +Simple3DObject::~Simple3DObject() { + if (vaoID_ != 0) { + glDeleteBuffers(3, vboID_); + glDeleteVertexArrays(1, &vaoID_); + } +} + +void Simple3DObject::addBBox(std::vector &pts, sl::float4 clr) { + int start_id = vertices_.size() / 3; + + float transparency_top = 0.05f, transparency_bottom = 0.75f; + for (unsigned int i = 0; i < pts.size(); i++) { + addPt(pts[i]); + clr.a = (i < 4 ? transparency_top : transparency_bottom); + addClr(clr); + } + + const std::vector boxLinks = {4, 5, 5, 6, 6, 7, 7, 4, 0, 4, 1, 5, 2, 6, 3, 7}; + + for (unsigned int i = 0; i < boxLinks.size(); i += 2) { + indices_.push_back(start_id + boxLinks[i]); + indices_.push_back(start_id + boxLinks[i + 1]); + } +} + +void Simple3DObject::addPt(sl::float3 pt) { + vertices_.push_back(pt.x); + vertices_.push_back(pt.y); + vertices_.push_back(pt.z); +} + +void Simple3DObject::addClr(sl::float4 clr) { + colors_.push_back(clr.b); + colors_.push_back(clr.g); + colors_.push_back(clr.r); + colors_.push_back(clr.a); +} + +void Simple3DObject::addPoint(sl::float3 pt, sl::float4 clr) { + addPt(pt); + addClr(clr); + indices_.push_back((int) indices_.size()); +} + +void Simple3DObject::addLine(sl::float3 p1, sl::float3 p2, sl::float4 clr) { + addPoint(p1, clr); + addPoint(p2, clr); +} + +void Simple3DObject::addTriangle(sl::float3 p1, sl::float3 p2, sl::float3 p3, sl::float4 clr) { + addPoint(p1, clr); + addPoint(p2, clr); + addPoint(p3, clr); +} + +void Simple3DObject::addFullEdges(std::vector &pts, sl::float4 clr) { + clr.w = 0.4f; + + int start_id = vertices_.size() / 3; + + for (unsigned int i = 0; i < pts.size(); i++) { + addPt(pts[i]); + addClr(clr); + } + + const std::vector boxLinksTop = {0, 1, 1, 2, 2, 3, 3, 0}; + for (unsigned int i = 0; i < boxLinksTop.size(); i += 2) { + indices_.push_back(start_id + boxLinksTop[i]); + indices_.push_back(start_id + boxLinksTop[i + 1]); + } + + const std::vector boxLinksBottom = {4, 5, 5, 6, 6, 7, 7, 4}; + for (unsigned int i = 0; i < boxLinksBottom.size(); i += 2) { + indices_.push_back(start_id + boxLinksBottom[i]); + indices_.push_back(start_id + boxLinksBottom[i + 1]); + } +} + +void Simple3DObject::addVerticalEdges(std::vector &pts, sl::float4 clr) { + auto addSingleVerticalLine = [&](sl::float3 top_pt, sl::float3 bot_pt) { + std::vector current_pts{ + top_pt, + ((grid_size - 1.0f) * top_pt + bot_pt) / grid_size, + ((grid_size - 2.0f) * top_pt + bot_pt * 2.0f) / grid_size, + (2.0f * top_pt + bot_pt * (grid_size - 2.0f)) / grid_size, + (top_pt + bot_pt * (grid_size - 1.0f)) / grid_size, + bot_pt}; + + int start_id = vertices_.size() / 3; + for (unsigned int i = 0; i < current_pts.size(); i++) { + addPt(current_pts[i]); + clr.a = (i == 2 || i == 3) ? 0.0f : 0.4f; + addClr(clr); + } + + const std::vector boxLinks = {0, 1, 1, 2, 2, 3, 3, 4, 4, 5}; + for (unsigned int i = 0; i < boxLinks.size(); i += 2) { + indices_.push_back(start_id + boxLinks[i]); + indices_.push_back(start_id + boxLinks[i + 1]); + } + }; + + addSingleVerticalLine(pts[0], pts[4]); + addSingleVerticalLine(pts[1], pts[5]); + addSingleVerticalLine(pts[2], pts[6]); + addSingleVerticalLine(pts[3], pts[7]); +} + +void Simple3DObject::addTopFace(std::vector &pts, sl::float4 clr) { + clr.a = 0.3f; + for (auto it : pts) + addPoint(it, clr); +} + +void Simple3DObject::addVerticalFaces(std::vector &pts, sl::float4 clr) { + auto addQuad = [&](std::vector quad_pts, float alpha1, float alpha2) { // To use only with 4 points + for (unsigned int i = 0; i < quad_pts.size(); ++i) { + addPt(quad_pts[i]); + clr.a = (i < 2 ? alpha1 : alpha2); + addClr(clr); + } + + indices_.push_back((int) indices_.size()); + indices_.push_back((int) indices_.size()); + indices_.push_back((int) indices_.size()); + indices_.push_back((int) indices_.size()); + }; + + // For each face, we need to add 4 quads (the first 2 indexes are always the top points of the quad) + std::vector> quads + { + { + 0, 3, 7, 4 + }, // front face + { + 3, 2, 6, 7 + }, // right face + { + 2, 1, 5, 6 + }, // back face + { + 1, 0, 4, 5 + } // left face + }; + float alpha = 0.5f; + + for (const auto quad : quads) { + + // Top quads + std::vector quad_pts_1{ + pts[quad[0]], + pts[quad[1]], + ((grid_size - 0.5f) * pts[quad[1]] + 0.5f * pts[quad[2]]) / grid_size, + ((grid_size - 0.5f) * pts[quad[0]] + 0.5f * pts[quad[3]]) / grid_size}; + addQuad(quad_pts_1, alpha, alpha); + + std::vector quad_pts_2{ + ((grid_size - 0.5f) * pts[quad[0]] + 0.5f * pts[quad[3]]) / grid_size, + ((grid_size - 0.5f) * pts[quad[1]] + 0.5f * pts[quad[2]]) / grid_size, + ((grid_size - 1.0f) * pts[quad[1]] + pts[quad[2]]) / grid_size, + ((grid_size - 1.0f) * pts[quad[0]] + pts[quad[3]]) / grid_size}; + addQuad(quad_pts_2, alpha, 2 * alpha / 3); + + std::vector quad_pts_3{ + ((grid_size - 1.0f) * pts[quad[0]] + pts[quad[3]]) / grid_size, + ((grid_size - 1.0f) * pts[quad[1]] + pts[quad[2]]) / grid_size, + ((grid_size - 1.5f) * pts[quad[1]] + 1.5f * pts[quad[2]]) / grid_size, + ((grid_size - 1.5f) * pts[quad[0]] + 1.5f * pts[quad[3]]) / grid_size}; + addQuad(quad_pts_3, 2 * alpha / 3, alpha / 3); + + std::vector quad_pts_4{ + ((grid_size - 1.5f) * pts[quad[0]] + 1.5f * pts[quad[3]]) / grid_size, + ((grid_size - 1.5f) * pts[quad[1]] + 1.5f * pts[quad[2]]) / grid_size, + ((grid_size - 2.0f) * pts[quad[1]] + 2.0f * pts[quad[2]]) / grid_size, + ((grid_size - 2.0f) * pts[quad[0]] + 2.0f * pts[quad[3]]) / grid_size}; + addQuad(quad_pts_4, alpha / 3, 0.0f); + + // Bottom quads + std::vector quad_pts_5{ + (pts[quad[1]] * 2.0f + (grid_size - 2.0f) * pts[quad[2]]) / grid_size, + (pts[quad[0]] * 2.0f + (grid_size - 2.0f) * pts[quad[3]]) / grid_size, + (pts[quad[0]] * 1.5f + (grid_size - 1.5f) * pts[quad[3]]) / grid_size, + (pts[quad[1]] * 1.5f + (grid_size - 1.5f) * pts[quad[2]]) / grid_size}; + addQuad(quad_pts_5, 0.0f, alpha / 3); + + std::vector quad_pts_6{ + (pts[quad[1]] * 1.5f + (grid_size - 1.5f) * pts[quad[2]]) / grid_size, + (pts[quad[0]] * 1.5f + (grid_size - 1.5f) * pts[quad[3]]) / grid_size, + (pts[quad[0]] + (grid_size - 1.0f) * pts[quad[3]]) / grid_size, + (pts[quad[1]] + (grid_size - 1.0f) * pts[quad[2]]) / grid_size}; + addQuad(quad_pts_6, alpha / 3, 2 * alpha / 3); + + std::vector quad_pts_7{ + (pts[quad[1]] + (grid_size - 1.0f) * pts[quad[2]]) / grid_size, + (pts[quad[0]] + (grid_size - 1.0f) * pts[quad[3]]) / grid_size, + (pts[quad[0]] * 0.5f + (grid_size - 0.5f) * pts[quad[3]]) / grid_size, + (pts[quad[1]] * 0.5f + (grid_size - 0.5f) * pts[quad[2]]) / grid_size}; + addQuad(quad_pts_7, 2 * alpha / 3, alpha); + + std::vector quad_pts_8{ + (pts[quad[0]] * 0.5f + (grid_size - 0.5f) * pts[quad[3]]) / grid_size, + (pts[quad[1]] * 0.5f + (grid_size - 0.5f) * pts[quad[2]]) / grid_size, + pts[quad[2]], + pts[quad[3]]}; + addQuad(quad_pts_8, alpha, alpha); + } +} + +void Simple3DObject::pushToGPU() { + if (!isStatic_ || vaoID_ == 0) { + if (vaoID_ == 0) { + glGenVertexArrays(1, &vaoID_); + glGenBuffers(3, vboID_); + } + + if (vertices_.size() > 0) { + glBindVertexArray(vaoID_); + glBindBuffer(GL_ARRAY_BUFFER, vboID_[0]); + glBufferData(GL_ARRAY_BUFFER, vertices_.size() * sizeof (float), &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); + } + + if (colors_.size() > 0) { + glBindBuffer(GL_ARRAY_BUFFER, vboID_[1]); + glBufferData(GL_ARRAY_BUFFER, colors_.size() * sizeof (float), &colors_[0], isStatic_ ? GL_STATIC_DRAW : GL_DYNAMIC_DRAW); + glVertexAttribPointer(Shader::ATTRIB_COLOR_POS, 4, GL_FLOAT, GL_FALSE, 0, 0); + glEnableVertexAttribArray(Shader::ATTRIB_COLOR_POS); + } + + if (indices_.size() > 0) { + 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); + } +} + +void Simple3DObject::clear() { + vertices_.clear(); + colors_.clear(); + indices_.clear(); +} + +void Simple3DObject::setDrawingType(GLenum type) { + drawingType_ = type; +} + +void Simple3DObject::draw() { + if (indices_.size() && vaoID_) { + glBindVertexArray(vaoID_); + glDrawElements(drawingType_, (GLsizei) indices_.size(), GL_UNSIGNED_INT, 0); + glBindVertexArray(0); + } +} + +void Simple3DObject::translate(const sl::Translation& t) { + position_ = position_ + t; +} + +void Simple3DObject::setPosition(const sl::Translation& p) { + position_ = p; +} + +void Simple3DObject::setRT(const sl::Transform& mRT) { + position_ = mRT.getTranslation(); + rotation_ = mRT.getOrientation(); +} + +void Simple3DObject::rotate(const sl::Orientation& rot) { + rotation_ = rot * rotation_; +} + +void Simple3DObject::rotate(const sl::Rotation& m) { + this->rotate(sl::Orientation(m)); +} + +void Simple3DObject::setRotation(const sl::Orientation& rot) { + rotation_ = rot; +} + +void Simple3DObject::setRotation(const sl::Rotation& m) { + this->setRotation(sl::Orientation(m)); +} + +const sl::Translation& Simple3DObject::getPosition() const { + return position_; +} + +sl::Transform Simple3DObject::getModelMatrix() const { + sl::Transform tmp; + tmp.setOrientation(rotation_); + tmp.setTranslation(position_); + return tmp; +} + +Shader::Shader(GLchar* vs, 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) + glDeleteShader(verterxId_); + if (fragmentId_ != 0) + glDeleteShader(fragmentId_); + if (programId_ != 0) + glDeleteShader(programId_); +} + +GLuint Shader::getProgramId() { + return programId_; +} + +bool Shader::compile(GLuint &shaderId, GLenum type, 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; +} + +GLchar* POINTCLOUD_VERTEX_SHADER = + "#version 330 core\n" + "layout(location = 0) in vec4 in_VertexRGBA;\n" + "uniform mat4 u_mvpMatrix;\n" + "out vec4 b_color;\n" + "void main() {\n" + // 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.r / 255.0f, clr_int.g / 255.0f, clr_int.b / 255.0f, 1.f);" + " gl_Position = u_mvpMatrix * vec4(in_VertexRGBA.xyz, 1);\n" + "}"; + +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" + "}"; + +PointCloud::PointCloud() : hasNewPCL_(false) { +} + +PointCloud::~PointCloud() { + close(); +} + +void checkError(cudaError_t err) { + if (err != cudaSuccess) + std::cerr << "Error: (" << err << "): " << cudaGetErrorString(err) << std::endl; +} + +void PointCloud::close() { + if (matGPU_.isInit()) { + matGPU_.free(); + checkError(cudaGraphicsUnmapResources(1, &bufferCudaID_, 0)); + glDeleteBuffers(1, &bufferGLID_); + } +} + +void PointCloud::initialize(sl::Resolution res) { + glGenBuffers(1, &bufferGLID_); + glBindBuffer(GL_ARRAY_BUFFER, bufferGLID_); + glBufferData(GL_ARRAY_BUFFER, res.area() * 4 * sizeof (float), 0, GL_STATIC_DRAW); + glBindBuffer(GL_ARRAY_BUFFER, 0); + + checkError(cudaGraphicsGLRegisterBuffer(&bufferCudaID_, bufferGLID_, cudaGraphicsRegisterFlagsWriteDiscard)); + + shader.it = Shader(POINTCLOUD_VERTEX_SHADER, POINTCLOUD_FRAGMENT_SHADER); + shader.MVP_Mat = glGetUniformLocation(shader.it.getProgramId(), "u_mvpMatrix"); + + matGPU_.alloc(res, sl::MAT_TYPE::F32_C4, sl::MEM::GPU); + + checkError(cudaGraphicsMapResources(1, &bufferCudaID_, 0)); + checkError(cudaGraphicsResourceGetMappedPointer((void**) &xyzrgbaMappedBuf_, &numBytes_, bufferCudaID_)); +} + +void PointCloud::pushNewPC(sl::Mat &matXYZRGBA) { + if (matGPU_.isInit()) { + matGPU_.setFrom(matXYZRGBA, sl::COPY_TYPE::GPU_GPU); + hasNewPCL_ = true; + } +} + +void PointCloud::update() { + if (hasNewPCL_ && matGPU_.isInit()) { + checkError(cudaMemcpy(xyzrgbaMappedBuf_, matGPU_.getPtr(sl::MEM::GPU), numBytes_, cudaMemcpyDeviceToDevice)); + hasNewPCL_ = false; + } +} + +void PointCloud::draw(const sl::Transform& vp) { + if (matGPU_.isInit()) { + glUseProgram(shader.it.getProgramId()); + glUniformMatrix4fv(shader.MVP_Mat, 1, GL_TRUE, vp.m); + + 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, matGPU_.getResolution().area()); + glBindBuffer(GL_ARRAY_BUFFER, 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, 200.f, 50000.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_; +} \ No newline at end of file diff --git a/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/src/calibrator.cpp b/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/src/calibrator.cpp new file mode 100644 index 00000000..6fae5d66 --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/src/calibrator.cpp @@ -0,0 +1,83 @@ +#ifdef ENABLE_INT8_CALIBRATOR + +#include +#include +#include +#include +#include "calibrator.h" +#include "cuda_utils.h" +#include "utils.h" + +Int8EntropyCalibrator2::Int8EntropyCalibrator2(int batchsize, int input_w, int input_h, const char* img_dir, const char* calib_table_name, const char* input_blob_name, bool read_cache) + : batchsize_(batchsize) + , input_w_(input_w) + , input_h_(input_h) + , img_idx_(0) + , img_dir_(img_dir) + , calib_table_name_(calib_table_name) + , input_blob_name_(input_blob_name) + , read_cache_(read_cache) +{ + input_count_ = 3 * input_w * input_h * batchsize; + CUDA_CHECK(cudaMalloc(&device_input_, input_count_ * sizeof(float))); + read_files_in_dir(img_dir, img_files_); +} + +Int8EntropyCalibrator2::~Int8EntropyCalibrator2() +{ + CUDA_CHECK(cudaFree(device_input_)); +} + +int Int8EntropyCalibrator2::getBatchSize() const TRT_NOEXCEPT +{ + return batchsize_; +} + +bool Int8EntropyCalibrator2::getBatch(void* bindings[], const char* names[], int nbBindings) TRT_NOEXCEPT +{ + if (img_idx_ + batchsize_ > (int)img_files_.size()) { + return false; + } + + std::vector input_imgs_; + for (int i = img_idx_; i < img_idx_ + batchsize_; i++) { + std::cout << img_files_[i] << " " << i << std::endl; + cv::Mat temp = cv::imread(img_dir_ + img_files_[i]); + if (temp.empty()){ + std::cerr << "Fatal error: image cannot open!" << std::endl; + return false; + } + cv::Mat pr_img = preprocess_img(temp, input_w_, input_h_); + input_imgs_.push_back(pr_img); + } + img_idx_ += batchsize_; + cv::Mat blob = cv::dnn::blobFromImages(input_imgs_, 1.0 / 255.0, cv::Size(input_w_, input_h_), cv::Scalar(0, 0, 0), true, false); + + CUDA_CHECK(cudaMemcpy(device_input_, blob.ptr(0), input_count_ * sizeof(float), cudaMemcpyHostToDevice)); + assert(!strcmp(names[0], input_blob_name_)); + bindings[0] = device_input_; + return true; +} + +const void* Int8EntropyCalibrator2::readCalibrationCache(size_t& length) TRT_NOEXCEPT +{ + std::cout << "reading calib cache: " << calib_table_name_ << std::endl; + calib_cache_.clear(); + std::ifstream input(calib_table_name_, std::ios::binary); + input >> std::noskipws; + if (read_cache_ && input.good()) + { + std::copy(std::istream_iterator(input), std::istream_iterator(), std::back_inserter(calib_cache_)); + } + length = calib_cache_.size(); + return length ? calib_cache_.data() : nullptr; +} + +void Int8EntropyCalibrator2::writeCalibrationCache(const void* cache, size_t length) TRT_NOEXCEPT +{ + std::cout << "writing calib cache: " << calib_table_name_ << " size: " << length << std::endl; + std::ofstream output(calib_table_name_, std::ios::binary); + output.write(reinterpret_cast(cache), length); +} + +#endif \ No newline at end of file diff --git a/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/src/yololayer.cu b/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/src/yololayer.cu new file mode 100644 index 00000000..4c042a66 --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/src/yololayer.cu @@ -0,0 +1,313 @@ +#include +#include +#include +#include "yololayer.h" +#include "cuda_utils.h" + +namespace Tn +{ + template + void write(char*& buffer, const T& val) + { + *reinterpret_cast(buffer) = val; + buffer += sizeof(T); + } + + template + void read(const char*& buffer, T& val) + { + val = *reinterpret_cast(buffer); + buffer += sizeof(T); + } +} + +using namespace Yolo; + +namespace nvinfer1 +{ + YoloLayerPlugin::YoloLayerPlugin(int classCount, int netWidth, int netHeight, int maxOut, const std::vector& vYoloKernel) + { + mClassCount = classCount; + mYoloV5NetWidth = netWidth; + mYoloV5NetHeight = netHeight; + mMaxOutObject = maxOut; + mYoloKernel = vYoloKernel; + mKernelCount = vYoloKernel.size(); + + CUDA_CHECK(cudaMallocHost(&mAnchor, mKernelCount * sizeof(void*))); + size_t AnchorLen = sizeof(float)* CHECK_COUNT * 2; + for (int ii = 0; ii < mKernelCount; ii++) + { + CUDA_CHECK(cudaMalloc(&mAnchor[ii], AnchorLen)); + const auto& yolo = mYoloKernel[ii]; + CUDA_CHECK(cudaMemcpy(mAnchor[ii], yolo.anchors, AnchorLen, cudaMemcpyHostToDevice)); + } + } + YoloLayerPlugin::~YoloLayerPlugin() + { + for (int ii = 0; ii < mKernelCount; ii++) + { + CUDA_CHECK(cudaFree(mAnchor[ii])); + } + CUDA_CHECK(cudaFreeHost(mAnchor)); + } + + // create the plugin at runtime from a byte stream + YoloLayerPlugin::YoloLayerPlugin(const void* data, size_t length) + { + using namespace Tn; + const char *d = reinterpret_cast(data), *a = d; + read(d, mClassCount); + read(d, mThreadCount); + read(d, mKernelCount); + read(d, mYoloV5NetWidth); + read(d, mYoloV5NetHeight); + read(d, mMaxOutObject); + mYoloKernel.resize(mKernelCount); + auto kernelSize = mKernelCount * sizeof(YoloKernel); + memcpy(mYoloKernel.data(), d, kernelSize); + d += kernelSize; + CUDA_CHECK(cudaMallocHost(&mAnchor, mKernelCount * sizeof(void*))); + size_t AnchorLen = sizeof(float)* CHECK_COUNT * 2; + for (int ii = 0; ii < mKernelCount; ii++) + { + CUDA_CHECK(cudaMalloc(&mAnchor[ii], AnchorLen)); + const auto& yolo = mYoloKernel[ii]; + CUDA_CHECK(cudaMemcpy(mAnchor[ii], yolo.anchors, AnchorLen, cudaMemcpyHostToDevice)); + } + assert(d == a + length); + } + + void YoloLayerPlugin::serialize(void* buffer) const TRT_NOEXCEPT + { + using namespace Tn; + char* d = static_cast(buffer), *a = d; + write(d, mClassCount); + write(d, mThreadCount); + write(d, mKernelCount); + write(d, mYoloV5NetWidth); + write(d, mYoloV5NetHeight); + write(d, mMaxOutObject); + auto kernelSize = mKernelCount * sizeof(YoloKernel); + memcpy(d, mYoloKernel.data(), kernelSize); + d += kernelSize; + + assert(d == a + getSerializationSize()); + } + + size_t YoloLayerPlugin::getSerializationSize() const TRT_NOEXCEPT + { + return sizeof(mClassCount) + sizeof(mThreadCount) + sizeof(mKernelCount) + sizeof(Yolo::YoloKernel) * mYoloKernel.size() + sizeof(mYoloV5NetWidth) + sizeof(mYoloV5NetHeight) + sizeof(mMaxOutObject); + } + + int YoloLayerPlugin::initialize() TRT_NOEXCEPT + { + return 0; + } + + Dims YoloLayerPlugin::getOutputDimensions(int index, const Dims* inputs, int nbInputDims) TRT_NOEXCEPT + { + //output the result to channel + int totalsize = mMaxOutObject * sizeof(Detection) / sizeof(float); + + return Dims3(totalsize + 1, 1, 1); + } + + // Set plugin namespace + void YoloLayerPlugin::setPluginNamespace(const char* pluginNamespace) TRT_NOEXCEPT + { + mPluginNamespace = pluginNamespace; + } + + const char* YoloLayerPlugin::getPluginNamespace() const TRT_NOEXCEPT + { + return mPluginNamespace; + } + + // Return the DataType of the plugin output at the requested index + DataType YoloLayerPlugin::getOutputDataType(int index, const nvinfer1::DataType* inputTypes, int nbInputs) const TRT_NOEXCEPT + { + return DataType::kFLOAT; + } + + // Return true if output tensor is broadcast across a batch. + bool YoloLayerPlugin::isOutputBroadcastAcrossBatch(int outputIndex, const bool* inputIsBroadcasted, int nbInputs) const TRT_NOEXCEPT + { + return false; + } + + // Return true if plugin can use input that is broadcast across batch without replication. + bool YoloLayerPlugin::canBroadcastInputAcrossBatch(int inputIndex) const TRT_NOEXCEPT + { + return false; + } + + void YoloLayerPlugin::configurePlugin(const PluginTensorDesc* in, int nbInput, const PluginTensorDesc* out, int nbOutput) TRT_NOEXCEPT + { + } + + // Attach the plugin object to an execution context and grant the plugin the access to some context resource. + void YoloLayerPlugin::attachToContext(cudnnContext* cudnnContext, cublasContext* cublasContext, IGpuAllocator* gpuAllocator) TRT_NOEXCEPT + { + } + + // Detach the plugin object from its execution context. + void YoloLayerPlugin::detachFromContext() TRT_NOEXCEPT {} + + const char* YoloLayerPlugin::getPluginType() const TRT_NOEXCEPT + { + return "YoloLayer_TRT"; + } + + const char* YoloLayerPlugin::getPluginVersion() const TRT_NOEXCEPT + { + return "1"; + } + + void YoloLayerPlugin::destroy() TRT_NOEXCEPT + { + delete this; + } + + // Clone the plugin + IPluginV2IOExt* YoloLayerPlugin::clone() const TRT_NOEXCEPT + { + YoloLayerPlugin* p = new YoloLayerPlugin(mClassCount, mYoloV5NetWidth, mYoloV5NetHeight, mMaxOutObject, mYoloKernel); + p->setPluginNamespace(mPluginNamespace); + return p; + } + + __device__ float Logist(float data) { return 1.0f / (1.0f + expf(-data)); }; + + __global__ void CalDetection(const float *input, float *output, int noElements, + const int netwidth, const int netheight, int maxoutobject, int yoloWidth, int yoloHeight, const float anchors[CHECK_COUNT * 2], int classes, int outputElem) + { + + int idx = threadIdx.x + blockDim.x * blockIdx.x; + if (idx >= noElements) return; + + int total_grid = yoloWidth * yoloHeight; + int bnIdx = idx / total_grid; + idx = idx - total_grid * bnIdx; + int info_len_i = 5 + classes; + const float* curInput = input + bnIdx * (info_len_i * total_grid * CHECK_COUNT); + + for (int k = 0; k < CHECK_COUNT; ++k) { + float box_prob = Logist(curInput[idx + k * info_len_i * total_grid + 4 * total_grid]); + if (box_prob < IGNORE_THRESH) continue; + int class_id = 0; + float max_cls_prob = 0.0; + for (int i = 5; i < info_len_i; ++i) { + float p = Logist(curInput[idx + k * info_len_i * total_grid + i * total_grid]); + if (p > max_cls_prob) { + max_cls_prob = p; + class_id = i - 5; + } + } + float *res_count = output + bnIdx * outputElem; + int count = (int)atomicAdd(res_count, 1); + if (count >= maxoutobject) return; + char *data = (char*)res_count + sizeof(float) + count * sizeof(Detection); + Detection *det = (Detection*)(data); + + int row = idx / yoloWidth; + int col = idx % yoloWidth; + + //Location + // pytorch: + // y = x[i].sigmoid() + // y[..., 0:2] = (y[..., 0:2] * 2. - 0.5 + self.grid[i].to(x[i].device)) * self.stride[i] # xy + // y[..., 2:4] = (y[..., 2:4] * 2) ** 2 * self.anchor_grid[i] # wh + // X: (sigmoid(tx) + cx)/FeaturemapW * netwidth + det->bbox[0] = (col - 0.5f + 2.0f * Logist(curInput[idx + k * info_len_i * total_grid + 0 * total_grid])) * netwidth / yoloWidth; + det->bbox[1] = (row - 0.5f + 2.0f * Logist(curInput[idx + k * info_len_i * total_grid + 1 * total_grid])) * netheight / yoloHeight; + + // W: (Pw * e^tw) / FeaturemapW * netwidth + // v5: https://github.com/ultralytics/yolov5/issues/471 + det->bbox[2] = 2.0f * Logist(curInput[idx + k * info_len_i * total_grid + 2 * total_grid]); + det->bbox[2] = det->bbox[2] * det->bbox[2] * anchors[2 * k]; + det->bbox[3] = 2.0f * Logist(curInput[idx + k * info_len_i * total_grid + 3 * total_grid]); + det->bbox[3] = det->bbox[3] * det->bbox[3] * anchors[2 * k + 1]; + det->conf = box_prob * max_cls_prob; + det->class_id = class_id; + } + } + + void YoloLayerPlugin::forwardGpu(const float* const* inputs, float *output, cudaStream_t stream, int batchSize) + { + int outputElem = 1 + mMaxOutObject * sizeof(Detection) / sizeof(float); + for (int idx = 0; idx < batchSize; ++idx) { + CUDA_CHECK(cudaMemset(output + idx * outputElem, 0, sizeof(float))); + } + int numElem = 0; + for (unsigned int i = 0; i < mYoloKernel.size(); ++i) { + const auto& yolo = mYoloKernel[i]; + numElem = yolo.width * yolo.height * batchSize; + if (numElem < mThreadCount) mThreadCount = numElem; + + //printf("Net: %d %d \n", mYoloV5NetWidth, mYoloV5NetHeight); + CalDetection << < (numElem + mThreadCount - 1) / mThreadCount, mThreadCount, 0, stream >> > + (inputs[i], output, numElem, mYoloV5NetWidth, mYoloV5NetHeight, mMaxOutObject, yolo.width, yolo.height, (float*)mAnchor[i], mClassCount, outputElem); + } + } + + + int YoloLayerPlugin::enqueue(int batchSize, const void* const* inputs, void* TRT_CONST_ENQUEUE* outputs, void* workspace, cudaStream_t stream) TRT_NOEXCEPT + { + forwardGpu((const float* const*)inputs, (float*)outputs[0], stream, batchSize); + return 0; + } + + PluginFieldCollection YoloPluginCreator::mFC{}; + std::vector YoloPluginCreator::mPluginAttributes; + + YoloPluginCreator::YoloPluginCreator() + { + mPluginAttributes.clear(); + + mFC.nbFields = mPluginAttributes.size(); + mFC.fields = mPluginAttributes.data(); + } + + const char* YoloPluginCreator::getPluginName() const TRT_NOEXCEPT + { + return "YoloLayer_TRT"; + } + + const char* YoloPluginCreator::getPluginVersion() const TRT_NOEXCEPT + { + return "1"; + } + + const PluginFieldCollection* YoloPluginCreator::getFieldNames() TRT_NOEXCEPT + { + return &mFC; + } + + IPluginV2IOExt* YoloPluginCreator::createPlugin(const char* name, const PluginFieldCollection* fc) TRT_NOEXCEPT + { + assert(fc->nbFields == 2); + assert(strcmp(fc->fields[0].name, "netinfo") == 0); + assert(strcmp(fc->fields[1].name, "kernels") == 0); + int *p_netinfo = (int*)(fc->fields[0].data); + int class_count = p_netinfo[0]; + int input_w = p_netinfo[1]; + int input_h = p_netinfo[2]; + int max_output_object_count = p_netinfo[3]; + std::vector kernels(fc->fields[1].length); + memcpy(&kernels[0], fc->fields[1].data, kernels.size() * sizeof(Yolo::YoloKernel)); + YoloLayerPlugin* obj = new YoloLayerPlugin(class_count, input_w, input_h, max_output_object_count, kernels); + obj->setPluginNamespace(mNamespace.c_str()); + return obj; + } + + IPluginV2IOExt* YoloPluginCreator::deserializePlugin(const char* name, const void* serialData, size_t serialLength) TRT_NOEXCEPT + { + // This object will be deleted when the network is destroyed, which will + // call YoloLayerPlugin::destroy() + YoloLayerPlugin* obj = new YoloLayerPlugin(serialData, serialLength); + obj->setPluginNamespace(mNamespace.c_str()); + return obj; + } +} + diff --git a/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/src/yolov5.cpp b/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/src/yolov5.cpp new file mode 100644 index 00000000..525cc611 --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5_v6.0/src/yolov5.cpp @@ -0,0 +1,530 @@ +#include +#include +#include +#include "cuda_utils.h" +#include "logging.h" +#include "common.hpp" +#include "utils.h" +#include "calibrator.h" +#include "GLViewer.hpp" + +#include + +#define USE_FP16 // set USE_INT8 or USE_FP16 or USE_FP32 +#define DEVICE 0 // GPU id +#define NMS_THRESH 0.4 +#define CONF_THRESH 0.5 +#define BATCH_SIZE 1 + +// stuff we know about the network and the input/output blobs +static const int INPUT_H = Yolo::INPUT_H; +static const int INPUT_W = Yolo::INPUT_W; +static const int CLASS_NUM = Yolo::CLASS_NUM; +static const int OUTPUT_SIZE = Yolo::MAX_OUTPUT_BBOX_COUNT * sizeof (Yolo::Detection) / sizeof (float) + 1; // we assume the yololayer outputs no more than MAX_OUTPUT_BBOX_COUNT boxes that conf >= 0.1 +const char* INPUT_BLOB_NAME = "data"; +const char* OUTPUT_BLOB_NAME = "prob"; +static Logger gLogger; + +static int get_width(int x, float gw, int divisor = 8) { + return int(ceil((x * gw) / divisor)) * divisor; +} + +static int get_depth(int x, float gd) { + if (x == 1) return 1; + int r = round(x * gd); + if (x * gd - int(x * gd) == 0.5 && (int(x * gd) % 2) == 0) { + --r; + } + return std::max(r, 1); +} + +ICudaEngine* build_engine(unsigned int maxBatchSize, IBuilder* builder, IBuilderConfig* config, DataType dt, float& gd, float& gw, std::string& wts_name) { + INetworkDefinition* network = builder->createNetworkV2(0U); + + // Create input tensor of shape {3, INPUT_H, INPUT_W} with name INPUT_BLOB_NAME + ITensor* data = network->addInput(INPUT_BLOB_NAME, dt, Dims3{3, INPUT_H, INPUT_W}); + assert(data); + + std::map weightMap = loadWeights(wts_name); + + /* ------ yolov5 backbone------ */ + auto conv0 = convBlock(network, weightMap, *data, get_width(64, gw), 6, 2, 1, "model.0"); + assert(conv0); + auto conv1 = convBlock(network, weightMap, *conv0->getOutput(0), get_width(128, gw), 3, 2, 1, "model.1"); + auto bottleneck_CSP2 = C3(network, weightMap, *conv1->getOutput(0), get_width(128, gw), get_width(128, gw), get_depth(3, gd), true, 1, 0.5, "model.2"); + auto conv3 = convBlock(network, weightMap, *bottleneck_CSP2->getOutput(0), get_width(256, gw), 3, 2, 1, "model.3"); + auto bottleneck_csp4 = C3(network, weightMap, *conv3->getOutput(0), get_width(256, gw), get_width(256, gw), get_depth(6, gd), true, 1, 0.5, "model.4"); + auto conv5 = convBlock(network, weightMap, *bottleneck_csp4->getOutput(0), get_width(512, gw), 3, 2, 1, "model.5"); + auto bottleneck_csp6 = C3(network, weightMap, *conv5->getOutput(0), get_width(512, gw), get_width(512, gw), get_depth(9, gd), true, 1, 0.5, "model.6"); + auto conv7 = convBlock(network, weightMap, *bottleneck_csp6->getOutput(0), get_width(1024, gw), 3, 2, 1, "model.7"); + auto bottleneck_csp8 = C3(network, weightMap, *conv7->getOutput(0), get_width(1024, gw), get_width(1024, gw), get_depth(3, gd), true, 1, 0.5, "model.8"); + auto spp9 = SPPF(network, weightMap, *bottleneck_csp8->getOutput(0), get_width(1024, gw), get_width(1024, gw), 5, "model.9"); + + /* ------ yolov5 head ------ */ + auto conv10 = convBlock(network, weightMap, *spp9->getOutput(0), get_width(512, gw), 1, 1, 1, "model.10"); + + auto upsample11 = network->addResize(*conv10->getOutput(0)); + assert(upsample11); + upsample11->setResizeMode(ResizeMode::kNEAREST); + upsample11->setOutputDimensions(bottleneck_csp6->getOutput(0)->getDimensions()); + + ITensor * inputTensors12[] = {upsample11->getOutput(0), bottleneck_csp6->getOutput(0)}; + auto cat12 = network->addConcatenation(inputTensors12, 2); + auto bottleneck_csp13 = C3(network, weightMap, *cat12->getOutput(0), get_width(1024, gw), get_width(512, gw), get_depth(3, gd), false, 1, 0.5, "model.13"); + auto conv14 = convBlock(network, weightMap, *bottleneck_csp13->getOutput(0), get_width(256, gw), 1, 1, 1, "model.14"); + + auto upsample15 = network->addResize(*conv14->getOutput(0)); + assert(upsample15); + upsample15->setResizeMode(ResizeMode::kNEAREST); + upsample15->setOutputDimensions(bottleneck_csp4->getOutput(0)->getDimensions()); + + ITensor * inputTensors16[] = {upsample15->getOutput(0), bottleneck_csp4->getOutput(0)}; + auto cat16 = network->addConcatenation(inputTensors16, 2); + + auto bottleneck_csp17 = C3(network, weightMap, *cat16->getOutput(0), get_width(512, gw), get_width(256, gw), get_depth(3, gd), false, 1, 0.5, "model.17"); + + /* ------ detect ------ */ + IConvolutionLayer* det0 = network->addConvolutionNd(*bottleneck_csp17->getOutput(0), 3 * (Yolo::CLASS_NUM + 5), DimsHW { + 1, 1 }, weightMap["model.24.m.0.weight"], weightMap["model.24.m.0.bias"]); + auto conv18 = convBlock(network, weightMap, *bottleneck_csp17->getOutput(0), get_width(256, gw), 3, 2, 1, "model.18"); + ITensor * inputTensors19[] = {conv18->getOutput(0), conv14->getOutput(0)}; + auto cat19 = network->addConcatenation(inputTensors19, 2); + auto bottleneck_csp20 = C3(network, weightMap, *cat19->getOutput(0), get_width(512, gw), get_width(512, gw), get_depth(3, gd), false, 1, 0.5, "model.20"); + IConvolutionLayer* det1 = network->addConvolutionNd(*bottleneck_csp20->getOutput(0), 3 * (Yolo::CLASS_NUM + 5), DimsHW { + 1, 1 }, weightMap["model.24.m.1.weight"], weightMap["model.24.m.1.bias"]); + auto conv21 = convBlock(network, weightMap, *bottleneck_csp20->getOutput(0), get_width(512, gw), 3, 2, 1, "model.21"); + ITensor * inputTensors22[] = {conv21->getOutput(0), conv10->getOutput(0)}; + auto cat22 = network->addConcatenation(inputTensors22, 2); + auto bottleneck_csp23 = C3(network, weightMap, *cat22->getOutput(0), get_width(1024, gw), get_width(1024, gw), get_depth(3, gd), false, 1, 0.5, "model.23"); + IConvolutionLayer* det2 = network->addConvolutionNd(*bottleneck_csp23->getOutput(0), 3 * (Yolo::CLASS_NUM + 5), DimsHW { + 1, 1 }, weightMap["model.24.m.2.weight"], weightMap["model.24.m.2.bias"]); + + auto yolo = addYoLoLayer(network, weightMap, "model.24", std::vector{det0, det1, det2}); + yolo->getOutput(0)->setName(OUTPUT_BLOB_NAME); + network->markOutput(*yolo->getOutput(0)); + + // Build engine + builder->setMaxBatchSize(maxBatchSize); + config->setMaxWorkspaceSize(16 * (1 << 20)); // 16MB +#if defined(USE_FP16) + config->setFlag(BuilderFlag::kFP16); +#elif defined(USE_INT8) + std::cout << "Your platform support int8: " << (builder->platformHasFastInt8() ? "true" : "false") << std::endl; + assert(builder->platformHasFastInt8()); + config->setFlag(BuilderFlag::kINT8); + Int8EntropyCalibrator2* calibrator = new Int8EntropyCalibrator2(1, INPUT_W, INPUT_H, "./coco_calib/", "int8calib.table", INPUT_BLOB_NAME); + config->setInt8Calibrator(calibrator); +#endif + + std::cout << "Building engine, please wait for a while..." << std::endl; + ICudaEngine* engine = builder->buildEngineWithConfig(*network, *config); + std::cout << "Build engine successfully!" << std::endl; + + // Don't need the network any more + network->destroy(); + + // Release host memory + for (auto& mem : weightMap) { + free((void*) (mem.second.values)); + } + + return engine; +} + +//v6.0 +ICudaEngine* build_engine_p6(unsigned int maxBatchSize, IBuilder* builder, IBuilderConfig* config, DataType dt, float& gd, float& gw, std::string& wts_name) { + INetworkDefinition* network = builder->createNetworkV2(0U); + + // Create input tensor of shape {3, INPUT_H, INPUT_W} with name INPUT_BLOB_NAME + ITensor* data = network->addInput(INPUT_BLOB_NAME, dt, Dims3{3, INPUT_H, INPUT_W}); + assert(data); + + std::map weightMap = loadWeights(wts_name); + + /* ------ yolov5 backbone------ */ + auto conv0 = convBlock(network, weightMap, *data, get_width(64, gw), 6, 2, 1, "model.0"); + auto conv1 = convBlock(network, weightMap, *conv0->getOutput(0), get_width(128, gw), 3, 2, 1, "model.1"); + auto c3_2 = C3(network, weightMap, *conv1->getOutput(0), get_width(128, gw), get_width(128, gw), get_depth(3, gd), true, 1, 0.5, "model.2"); + auto conv3 = convBlock(network, weightMap, *c3_2->getOutput(0), get_width(256, gw), 3, 2, 1, "model.3"); + auto c3_4 = C3(network, weightMap, *conv3->getOutput(0), get_width(256, gw), get_width(256, gw), get_depth(6, gd), true, 1, 0.5, "model.4"); + auto conv5 = convBlock(network, weightMap, *c3_4->getOutput(0), get_width(512, gw), 3, 2, 1, "model.5"); + auto c3_6 = C3(network, weightMap, *conv5->getOutput(0), get_width(512, gw), get_width(512, gw), get_depth(9, gd), true, 1, 0.5, "model.6"); + auto conv7 = convBlock(network, weightMap, *c3_6->getOutput(0), get_width(768, gw), 3, 2, 1, "model.7"); + auto c3_8 = C3(network, weightMap, *conv7->getOutput(0), get_width(768, gw), get_width(768, gw), get_depth(3, gd), true, 1, 0.5, "model.8"); + auto conv9 = convBlock(network, weightMap, *c3_8->getOutput(0), get_width(1024, gw), 3, 2, 1, "model.9"); + auto c3_10 = C3(network, weightMap, *conv9->getOutput(0), get_width(1024, gw), get_width(1024, gw), get_depth(3, gd), true, 1, 0.5, "model.10"); + auto sppf11 = SPPF(network, weightMap, *c3_10->getOutput(0), get_width(1024, gw), get_width(1024, gw), 5, "model.11"); + + /* ------ yolov5 head ------ */ + auto conv12 = convBlock(network, weightMap, *sppf11->getOutput(0), get_width(768, gw), 1, 1, 1, "model.12"); + auto upsample13 = network->addResize(*conv12->getOutput(0)); + assert(upsample13); + upsample13->setResizeMode(ResizeMode::kNEAREST); + upsample13->setOutputDimensions(c3_8->getOutput(0)->getDimensions()); + ITensor * inputTensors14[] = {upsample13->getOutput(0), c3_8->getOutput(0)}; + auto cat14 = network->addConcatenation(inputTensors14, 2); + auto c3_15 = C3(network, weightMap, *cat14->getOutput(0), get_width(1536, gw), get_width(768, gw), get_depth(3, gd), false, 1, 0.5, "model.15"); + + auto conv16 = convBlock(network, weightMap, *c3_15->getOutput(0), get_width(512, gw), 1, 1, 1, "model.16"); + auto upsample17 = network->addResize(*conv16->getOutput(0)); + assert(upsample17); + upsample17->setResizeMode(ResizeMode::kNEAREST); + upsample17->setOutputDimensions(c3_6->getOutput(0)->getDimensions()); + ITensor * inputTensors18[] = {upsample17->getOutput(0), c3_6->getOutput(0)}; + auto cat18 = network->addConcatenation(inputTensors18, 2); + auto c3_19 = C3(network, weightMap, *cat18->getOutput(0), get_width(1024, gw), get_width(512, gw), get_depth(3, gd), false, 1, 0.5, "model.19"); + + auto conv20 = convBlock(network, weightMap, *c3_19->getOutput(0), get_width(256, gw), 1, 1, 1, "model.20"); + auto upsample21 = network->addResize(*conv20->getOutput(0)); + assert(upsample21); + upsample21->setResizeMode(ResizeMode::kNEAREST); + upsample21->setOutputDimensions(c3_4->getOutput(0)->getDimensions()); + ITensor * inputTensors21[] = {upsample21->getOutput(0), c3_4->getOutput(0)}; + auto cat22 = network->addConcatenation(inputTensors21, 2); + auto c3_23 = C3(network, weightMap, *cat22->getOutput(0), get_width(512, gw), get_width(256, gw), get_depth(3, gd), false, 1, 0.5, "model.23"); + + auto conv24 = convBlock(network, weightMap, *c3_23->getOutput(0), get_width(256, gw), 3, 2, 1, "model.24"); + ITensor * inputTensors25[] = {conv24->getOutput(0), conv20->getOutput(0)}; + auto cat25 = network->addConcatenation(inputTensors25, 2); + auto c3_26 = C3(network, weightMap, *cat25->getOutput(0), get_width(1024, gw), get_width(512, gw), get_depth(3, gd), false, 1, 0.5, "model.26"); + + auto conv27 = convBlock(network, weightMap, *c3_26->getOutput(0), get_width(512, gw), 3, 2, 1, "model.27"); + ITensor * inputTensors28[] = {conv27->getOutput(0), conv16->getOutput(0)}; + auto cat28 = network->addConcatenation(inputTensors28, 2); + auto c3_29 = C3(network, weightMap, *cat28->getOutput(0), get_width(1536, gw), get_width(768, gw), get_depth(3, gd), false, 1, 0.5, "model.29"); + + auto conv30 = convBlock(network, weightMap, *c3_29->getOutput(0), get_width(768, gw), 3, 2, 1, "model.30"); + ITensor * inputTensors31[] = {conv30->getOutput(0), conv12->getOutput(0)}; + auto cat31 = network->addConcatenation(inputTensors31, 2); + auto c3_32 = C3(network, weightMap, *cat31->getOutput(0), get_width(2048, gw), get_width(1024, gw), get_depth(3, gd), false, 1, 0.5, "model.32"); + + /* ------ detect ------ */ + IConvolutionLayer* det0 = network->addConvolutionNd(*c3_23->getOutput(0), 3 * (Yolo::CLASS_NUM + 5), DimsHW { + 1, 1 }, weightMap["model.33.m.0.weight"], weightMap["model.33.m.0.bias"]); + IConvolutionLayer* det1 = network->addConvolutionNd(*c3_26->getOutput(0), 3 * (Yolo::CLASS_NUM + 5), DimsHW { + 1, 1 }, weightMap["model.33.m.1.weight"], weightMap["model.33.m.1.bias"]); + IConvolutionLayer* det2 = network->addConvolutionNd(*c3_29->getOutput(0), 3 * (Yolo::CLASS_NUM + 5), DimsHW { + 1, 1 }, weightMap["model.33.m.2.weight"], weightMap["model.33.m.2.bias"]); + IConvolutionLayer* det3 = network->addConvolutionNd(*c3_32->getOutput(0), 3 * (Yolo::CLASS_NUM + 5), DimsHW { + 1, 1 }, weightMap["model.33.m.3.weight"], weightMap["model.33.m.3.bias"]); + + auto yolo = addYoLoLayer(network, weightMap, "model.33", std::vector{det0, det1, det2, det3}); + yolo->getOutput(0)->setName(OUTPUT_BLOB_NAME); + network->markOutput(*yolo->getOutput(0)); + + // Build engine + builder->setMaxBatchSize(maxBatchSize); + config->setMaxWorkspaceSize(16 * (1 << 20)); // 16MB +#if defined(USE_FP16) + config->setFlag(BuilderFlag::kFP16); +#elif defined(USE_INT8) + std::cout << "Your platform support int8: " << (builder->platformHasFastInt8() ? "true" : "false") << std::endl; + assert(builder->platformHasFastInt8()); + config->setFlag(BuilderFlag::kINT8); + Int8EntropyCalibrator2* calibrator = new Int8EntropyCalibrator2(1, INPUT_W, INPUT_H, "./coco_calib/", "int8calib.table", INPUT_BLOB_NAME); + config->setInt8Calibrator(calibrator); +#endif + + std::cout << "Building engine, please wait for a while..." << std::endl; + ICudaEngine* engine = builder->buildEngineWithConfig(*network, *config); + std::cout << "Build engine successfully!" << std::endl; + + // Don't need the network any more + network->destroy(); + + // Release host memory + for (auto& mem : weightMap) { + free((void*) (mem.second.values)); + } + + return engine; +} + +void APIToModel(unsigned int maxBatchSize, IHostMemory** modelStream, bool& is_p6, float& gd, float& gw, std::string& wts_name) { + // Create builder + IBuilder* builder = createInferBuilder(gLogger); + IBuilderConfig* config = builder->createBuilderConfig(); + + // Create model to populate the network, then set the outputs and create an engine + ICudaEngine *engine = nullptr; + if (is_p6) { + engine = build_engine_p6(maxBatchSize, builder, config, DataType::kFLOAT, gd, gw, wts_name); + } else { + engine = build_engine(maxBatchSize, builder, config, DataType::kFLOAT, gd, gw, wts_name); + } + assert(engine != nullptr); + + // Serialize the engine + (*modelStream) = engine->serialize(); + + // Close everything down + engine->destroy(); + builder->destroy(); + config->destroy(); +} + +void doInference(IExecutionContext& context, cudaStream_t& stream, void **buffers, float* input, float* output, int batchSize) { + // DMA input batch data to device, infer on the batch asynchronously, and DMA output back to host + CUDA_CHECK(cudaMemcpyAsync(buffers[0], input, batchSize * 3 * INPUT_H * INPUT_W * sizeof (float), cudaMemcpyHostToDevice, stream)); + context.enqueue(batchSize, buffers, stream, nullptr); + CUDA_CHECK(cudaMemcpyAsync(output, buffers[1], batchSize * OUTPUT_SIZE * sizeof (float), cudaMemcpyDeviceToHost, stream)); + cudaStreamSynchronize(stream); +} + +bool parse_args(int argc, char** argv, std::string& wts, std::string& engine, bool& is_p6, float& gd, float& gw) { + if (argc < 4) return false; + if (std::string(argv[1]) == "-s" && (argc == 5 || argc == 7)) { + wts = std::string(argv[2]); + engine = std::string(argv[3]); + auto net = std::string(argv[4]); + if (net[0] == 'n') { + gd = 0.33; + gw = 0.25; + } else if (net[0] == 's') { + gd = 0.33; + gw = 0.50; + } else if (net[0] == 'm') { + gd = 0.67; + gw = 0.75; + } else if (net[0] == 'l') { + gd = 1.0; + gw = 1.0; + } else if (net[0] == 'x') { + gd = 1.33; + gw = 1.25; + } else if (net[0] == 'c' && argc == 7) { + gd = atof(argv[5]); + gw = atof(argv[6]); + } else { + return false; + } + if (net.size() == 2 && net[1] == '6') { + is_p6 = true; + } + } else if (std::string(argv[1]) == "-d") { + engine = std::string(argv[2]); + } else { + return false; + } + return true; +} + +void print(std::string msg_prefix, sl::ERROR_CODE err_code, std::string msg_suffix) { + std::cout << "[Sample] "; + if (err_code != sl::ERROR_CODE::SUCCESS) + std::cout << "[Error] "; + std::cout << msg_prefix << " "; + if (err_code != sl::ERROR_CODE::SUCCESS) { + std::cout << " | " << toString(err_code) << " : "; + std::cout << toVerbose(err_code); + } + if (!msg_suffix.empty()) + std::cout << " " << msg_suffix; + std::cout << std::endl; +} + +std::vector cvt(const cv::Rect &bbox_in){ + std::vector bbox_out(4); + bbox_out[0] = sl::uint2(bbox_in.x, bbox_in.y); + bbox_out[1] = sl::uint2(bbox_in.x + bbox_in.width, bbox_in.y); + bbox_out[2] = sl::uint2(bbox_in.x + bbox_in.width, bbox_in.y + bbox_in.height); + bbox_out[3] = sl::uint2(bbox_in.x, bbox_in.y + bbox_in.height); + return bbox_out; +} + +int main(int argc, char** argv) { + + std::string wts_name = ""; + std::string engine_name = ""; + bool is_p6 = false; + float gd = 0.0f, gw = 0.0f; + if (!parse_args(argc, argv, wts_name, engine_name, is_p6, gd, gw)) { + std::cerr << "arguments not right!" << std::endl; + std::cerr << "./yolov5 -s [.wts] [.engine] [n/s/m/l/x/n6/s6/m6/l6/x6 or c/c6 gd gw] // serialize model to plan file" << std::endl; + std::cerr << "./yolov5 -d [.engine] ZED_input_option // deserialize plan file and run inference" << std::endl; + return -1; + } + + // create a model using the API directly and serialize it to a stream + if (!wts_name.empty()) { + cudaSetDevice(DEVICE); + IHostMemory * modelStream{ nullptr}; + APIToModel(BATCH_SIZE, &modelStream, is_p6, gd, gw, wts_name); + assert(modelStream != nullptr); + std::ofstream p(engine_name, std::ios::binary); + if (!p) { + std::cerr << "could not open plan output file" << std::endl; + return -1; + } + p.write(reinterpret_cast (modelStream->data()), modelStream->size()); + modelStream->destroy(); + return 0; + } + + /// Opening the ZED camera before the model deserialization to avoid cuda context issue + sl::Camera zed; + sl::InitParameters init_parameters; + init_parameters.camera_resolution = sl::RESOLUTION::HD1080; + init_parameters.sdk_verbose = true; + init_parameters.depth_mode = sl::DEPTH_MODE::ULTRA; + init_parameters.coordinate_system = sl::COORDINATE_SYSTEM::RIGHT_HANDED_Y_UP; // OpenGL's coordinate system is right_handed + + if (argc > 3) { + std::string zed_opt = argv[3]; + if (zed_opt.find(".svo") != std::string::npos) + init_parameters.input.setFromSVOFile(zed_opt.c_str()); + } + // Open the camera + auto returned_state = zed.open(init_parameters); + if (returned_state != sl::ERROR_CODE::SUCCESS) { + print("Camera Open", returned_state, "Exit program."); + return EXIT_FAILURE; + } + zed.enablePositionalTracking(); + // Custom OD + sl::ObjectDetectionParameters detection_parameters; + detection_parameters.enable_tracking = true; + detection_parameters.enable_mask_output = false; // designed to give person pixel mask + detection_parameters.detection_model = sl::DETECTION_MODEL::CUSTOM_BOX_OBJECTS; + returned_state = zed.enableObjectDetection(detection_parameters); + if (returned_state != sl::ERROR_CODE::SUCCESS) { + print("enableObjectDetection", returned_state, "\nExit program."); + zed.close(); + return EXIT_FAILURE; + } + auto camera_config = zed.getCameraInformation().camera_configuration; + sl::Resolution pc_resolution(std::min((int) camera_config.resolution.width, 720), std::min((int) camera_config.resolution.height, 404)); + auto camera_info = zed.getCameraInformation(pc_resolution).camera_configuration; + // Create OpenGL Viewer + GLViewer viewer; + viewer.init(argc, argv, camera_info.calibration_parameters.left_cam, true); + // --------- + + + // deserialize the .engine and run inference + std::ifstream file(engine_name, std::ios::binary); + if (!file.good()) { + std::cerr << "read " << engine_name << " error!" << std::endl; + return -1; + } + char *trtModelStream = nullptr; + size_t size = 0; + file.seekg(0, file.end); + size = file.tellg(); + file.seekg(0, file.beg); + trtModelStream = new char[size]; + assert(trtModelStream); + file.read(trtModelStream, size); + file.close(); + + // prepare input data --------------------------- + static float data[BATCH_SIZE * 3 * INPUT_H * INPUT_W]; + static float prob[BATCH_SIZE * OUTPUT_SIZE]; + IRuntime* runtime = createInferRuntime(gLogger); + assert(runtime != nullptr); + ICudaEngine* engine = runtime->deserializeCudaEngine(trtModelStream, size); + assert(engine != nullptr); + IExecutionContext* context = engine->createExecutionContext(); + assert(context != nullptr); + delete[] trtModelStream; + assert(engine->getNbBindings() == 2); + void* buffers[2]; + // In order to bind the buffers, we need to know the names of the input and output tensors. + // Note that indices are guaranteed to be less than IEngine::getNbBindings() + const int inputIndex = engine->getBindingIndex(INPUT_BLOB_NAME); + const int outputIndex = engine->getBindingIndex(OUTPUT_BLOB_NAME); + assert(inputIndex == 0); + assert(outputIndex == 1); + // Create GPU buffers on device + CUDA_CHECK(cudaMalloc(&buffers[inputIndex], BATCH_SIZE * 3 * INPUT_H * INPUT_W * sizeof (float))); + CUDA_CHECK(cudaMalloc(&buffers[outputIndex], BATCH_SIZE * OUTPUT_SIZE * sizeof (float))); + // Create stream + cudaStream_t stream; + CUDA_CHECK(cudaStreamCreate(&stream)); + + assert(BATCH_SIZE == 1); // This sample only support batch 1 for now + + sl::Mat left_sl, point_cloud; + cv::Mat left_cv_rgb; + sl::ObjectDetectionRuntimeParameters objectTracker_parameters_rt; + sl::Objects objects; + sl::Pose cam_w_pose; + cam_w_pose.pose_data.setIdentity(); + + while (viewer.isAvailable()) { + if (zed.grab() == sl::ERROR_CODE::SUCCESS) { + + zed.retrieveImage(left_sl, sl::VIEW::LEFT); + + // Preparing inference + cv::Mat left_cv_rgba = slMat2cvMat(left_sl); + cv::cvtColor(left_cv_rgba, left_cv_rgb, cv::COLOR_BGRA2BGR); + if (left_cv_rgb.empty()) continue; + cv::Mat pr_img = preprocess_img(left_cv_rgb, INPUT_W, INPUT_H); // letterbox BGR to RGB + int i = 0; + int batch = 0; + for (int row = 0; row < INPUT_H; ++row) { + uchar* uc_pixel = pr_img.data + row * pr_img.step; + for (int col = 0; col < INPUT_W; ++col) { + data[batch * 3 * INPUT_H * INPUT_W + i] = (float) uc_pixel[2] / 255.0; + data[batch * 3 * INPUT_H * INPUT_W + i + INPUT_H * INPUT_W] = (float) uc_pixel[1] / 255.0; + data[batch * 3 * INPUT_H * INPUT_W + i + 2 * INPUT_H * INPUT_W] = (float) uc_pixel[0] / 255.0; + uc_pixel += 3; + ++i; + } + } + + // Running inference + doInference(*context, stream, buffers, data, prob, BATCH_SIZE); + std::vector> batch_res(BATCH_SIZE); + auto& res = batch_res[batch]; + nms(res, &prob[batch * OUTPUT_SIZE], CONF_THRESH, NMS_THRESH); + + // Preparing for ZED SDK ingesting + std::vector objects_in; + for (auto &it : res) { + sl::CustomBoxObjectData tmp; + cv::Rect r = get_rect(left_cv_rgb, it.bbox); + // Fill the detections into the correct format + tmp.unique_object_id = sl::generate_unique_id(); + tmp.probability = it.conf; + tmp.label = (int) it.class_id; + tmp.bounding_box_2d = cvt(r); + tmp.is_grounded = ((int) it.class_id == 0); // Only the first class (person) is grounded, that is moving on the floor plane + // others are tracked in full 3D space + objects_in.push_back(tmp); + } + // Send the custom detected boxes to the ZED + zed.ingestCustomBoxObjects(objects_in); + + + // Displaying 'raw' objects + for (size_t j = 0; j < res.size(); j++) { + cv::Rect r = get_rect(left_cv_rgb, res[j].bbox); + cv::rectangle(left_cv_rgb, r, cv::Scalar(0x27, 0xC1, 0x36), 2); + cv::putText(left_cv_rgb, std::to_string((int) res[j].class_id), cv::Point(r.x, r.y - 1), cv::FONT_HERSHEY_PLAIN, 1.2, cv::Scalar(0xFF, 0xFF, 0xFF), 2); + } + cv::imshow("Objects", left_cv_rgb); + cv::waitKey(10); + + // Retrieve the tracked objects, with 2D and 3D attributes + zed.retrieveObjects(objects, objectTracker_parameters_rt); + // GL Viewer + zed.retrieveMeasure(point_cloud, sl::MEASURE::XYZRGBA, sl::MEM::GPU, pc_resolution); + zed.getPosition(cam_w_pose, sl::REFERENCE_FRAME::WORLD); + viewer.updateData(point_cloud, objects.object_list, cam_w_pose.pose_data); + } + } + + // Release stream and buffers + cudaStreamDestroy(stream); + CUDA_CHECK(cudaFree(buffers[inputIndex])); + CUDA_CHECK(cudaFree(buffers[outputIndex])); + // Destroy the engine + context->destroy(); + engine->destroy(); + runtime->destroy(); + viewer.exit(); + + return 0; +} diff --git a/object detection/image viewer/cpp/CMakeLists.txt b/object detection/image viewer/cpp/CMakeLists.txt index 262712c1..c1109ed8 100644 --- a/object detection/image viewer/cpp/CMakeLists.txt +++ b/object detection/image viewer/cpp/CMakeLists.txt @@ -1,31 +1,22 @@ -cmake_minimum_required(VERSION 2.4) +cmake_minimum_required(VERSION 3.5) PROJECT(ZED_Object_detection_image_viewer) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +SET(CMAKE_BUILD_TYPE "Release") + 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() -if(COMMAND cmake_policy) - cmake_policy(SET CMP0003 OLD) - cmake_policy(SET CMP0004 OLD) - cmake_policy(SET CMP0015 OLD) -endif(COMMAND cmake_policy) - -SET(EXECUTABLE_OUTPUT_PATH ".") -SET(SPECIAL_OS_LIBS "") - find_package(ZED 3 REQUIRED) +find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED) find_package(GLUT REQUIRED) find_package(GLEW REQUIRED) +SET(OpenGL_GL_PREFERENCE GLVND) find_package(OpenGL REQUIRED) -find_package(CUDA REQUIRED) - -IF(NOT WIN32) - SET(SPECIAL_OS_LIBS "pthread" "X11") - add_definitions(-Wno-write-strings) -ENDIF() include_directories(${ZED_INCLUDE_DIRS}) include_directories(${GLEW_INCLUDE_DIRS}) @@ -43,33 +34,14 @@ FILE(GLOB_RECURSE SRC_FILES src/*.c*) FILE(GLOB_RECURSE HDR_FILES include/*.h*) add_executable(${PROJECT_NAME} ${HDR_FILES} ${SRC_FILES}) -add_definitions(-std=c++14) - -## DEBUG/ SANITIZER options -IF(NOT WIN32) - add_definitions(-Werror=return-type) - IF (BUILD_WITH_SANITIZER) - message("!! Building with address sanitizer and -g !!") - set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fno-omit-frame-pointer -fsanitize=address -Wall -Wextra -g") - set (CMAKE_LINKER_FLAGS "${CMAKE_LINKER_FLAGS} -fno-omit-frame-pointer -fsanitize=address -Wall -Wextra -g") - add_definitions(-g -fsanitize=address -fno-omit-frame-pointer -static-libasan -Wall -Wextra) - ENDIF() - - IF (BUILD_WITH_DEBUGINFOS) - message("!! Building with -g !!") - add_definitions(-g) - ENDIF() -ENDIF() - if (LINK_SHARED_ZED) - SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY} ${CUDA_DEP_LIBRARIES_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} - ${SPECIAL_OS_LIBS} ${ZED_LIBS} ${OPENGL_LIBRARIES} ${GLUT_LIBRARY} diff --git a/object detection/image viewer/cpp/src/main.cpp b/object detection/image viewer/cpp/src/main.cpp index b96336cd..f0a16372 100644 --- a/object detection/image viewer/cpp/src/main.cpp +++ b/object detection/image viewer/cpp/src/main.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/object detection/image viewer/csharp/CMakeLists.txt b/object detection/image viewer/csharp/CMakeLists.txt index 3426a640..9e8c3f7e 100644 --- a/object detection/image viewer/csharp/CMakeLists.txt +++ b/object detection/image viewer/csharp/CMakeLists.txt @@ -41,6 +41,6 @@ set(CMAKE_SUPPRESS_REGENERATION true) set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES "OpenGL.Net_0.8.4" "OpenGL.Net.CoreUI_0.8.4" - "Stereolabs.zed_3.6.0" + "Stereolabs.zed_3.7.*" ) \ No newline at end of file diff --git a/object detection/image viewer/csharp/MainWindow.cs b/object detection/image viewer/csharp/MainWindow.cs index e15b5496..fe231f32 100644 --- a/object detection/image viewer/csharp/MainWindow.cs +++ b/object detection/image viewer/csharp/MainWindow.cs @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/object detection/image viewer/python/object_detection_image_viewer.py b/object detection/image viewer/python/object_detection_image_viewer.py index b10c1959..ae14df3f 100644 --- a/object detection/image viewer/python/object_detection_image_viewer.py +++ b/object detection/image viewer/python/object_detection_image_viewer.py @@ -1,6 +1,6 @@ ######################################################################## # -# Copyright (c) 2021, STEREOLABS. +# Copyright (c) 2022, STEREOLABS. # # All rights reserved. # diff --git a/other/cuda refocus/CMakeLists.txt b/other/cuda refocus/CMakeLists.txt index 1c8a3a52..a68da747 100644 --- a/other/cuda refocus/CMakeLists.txt +++ b/other/cuda refocus/CMakeLists.txt @@ -1,30 +1,23 @@ -CMAKE_MINIMUM_REQUIRED(VERSION 2.4) +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) PROJECT(ZED_CUDA_Refocus) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +SET(CMAKE_BUILD_TYPE "Release") + 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() -if(COMMAND cmake_policy) - cmake_policy(SET CMP0003 OLD) - cmake_policy(SET CMP0002 OLD) - cmake_policy(SET CMP0015 OLD) -endif(COMMAND cmake_policy) - -SET(EXECUTABLE_OUTPUT_PATH ".") - find_package(ZED 3 REQUIRED) -find_package(CUDA ${ZED_CUDA_VERSION} EXACT REQUIRED) +find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED) +SET(OpenGL_GL_PREFERENCE GLVND) find_package(OpenGL REQUIRED) find_package(GLUT REQUIRED) find_package(GLEW REQUIRED) -IF(NOT WIN32) - SET(SPECIAL_OS_LIBS "X11" "pthread") -ENDIF() - include_directories(${ZED_INCLUDE_DIRS}) include_directories(${GLUT_INCLUDE_PATH}) include_directories(${GLEW_INCLUDE_DIRS}) @@ -41,9 +34,7 @@ link_directories(${OpenGL_LIBRARY_DIRS}) FILE(GLOB_RECURSE SRC_FILES src/*.c*) FILE(GLOB_RECURSE HRD_FILES include/*.h) -set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} -std=c++11) cuda_add_executable(${PROJECT_NAME} ${HRD_FILES} ${SRC_FILES}) -add_definitions(-std=c++14 -O3) if (LINK_SHARED_ZED) SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY}) @@ -52,7 +43,6 @@ else() endif() TARGET_LINK_LIBRARIES(${PROJECT_NAME} - ${SPECIAL_OS_LIBS} ${ZED_LIBS} ${OPENGL_LIBRARIES} ${GLUT_LIBRARY} diff --git a/other/cuda refocus/src/main.cpp b/other/cuda refocus/src/main.cpp index a1c242b4..aaf6a3af 100644 --- a/other/cuda refocus/src/main.cpp +++ b/other/cuda refocus/src/main.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/other/multi camera/cpp/CMakeLists.txt b/other/multi camera/cpp/CMakeLists.txt index 6436c034..efc60366 100644 --- a/other/multi camera/cpp/CMakeLists.txt +++ b/other/multi camera/cpp/CMakeLists.txt @@ -1,21 +1,18 @@ -CMAKE_MINIMUM_REQUIRED(VERSION 2.4) +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) PROJECT(ZED_Multi_Camera) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +SET(CMAKE_BUILD_TYPE "Release") + 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() -if(COMMAND cmake_policy) - cmake_policy(SET CMP0003 OLD) - cmake_policy(SET CMP0015 OLD) -endif(COMMAND cmake_policy) - -SET(EXECUTABLE_OUTPUT_PATH ".") - IF(NOT MSVC) - SET(SPECIAL_OS_LIBS "pthread" "X11") + SET(SPECIAL_OS_LIBS "pthread") ENDIF() find_package(ZED 3 REQUIRED) @@ -31,12 +28,11 @@ link_directories(${OpenCV_LIBRARY_DIRS}) link_directories(${CUDA_LIBRARY_DIRS}) ADD_EXECUTABLE(${PROJECT_NAME} src/main.cpp) -add_definitions(-std=c++14 -O3) if (LINK_SHARED_ZED) - SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY} ${SPECIAL_OS_LIBS}) + SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY}) else() - SET(ZED_LIBS ${ZED_STATIC_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_LIBRARY} ${SPECIAL_OS_LIBS}) + SET(ZED_LIBS ${ZED_STATIC_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_LIBRARY}) endif() TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${ZED_LIBS} ${SPECIAL_OS_LIBS} ${OpenCV_LIBRARIES}) diff --git a/other/multi camera/cpp/src/main.cpp b/other/multi camera/cpp/src/main.cpp index 0b6a6c3e..5789c1e7 100644 --- a/other/multi camera/cpp/src/main.cpp +++ b/other/multi camera/cpp/src/main.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/other/multi camera/python/multi_camera.py b/other/multi camera/python/multi_camera.py index d161d0ca..d03f9354 100644 --- a/other/multi camera/python/multi_camera.py +++ b/other/multi camera/python/multi_camera.py @@ -1,6 +1,6 @@ ######################################################################## # -# Copyright (c) 2021, STEREOLABS. +# Copyright (c) 2022, STEREOLABS. # # All rights reserved. # diff --git a/other/opengl gpu interop/CMakeLists.txt b/other/opengl gpu interop/CMakeLists.txt index 215120b5..06e87fd1 100644 --- a/other/opengl gpu interop/CMakeLists.txt +++ b/other/opengl gpu interop/CMakeLists.txt @@ -1,29 +1,23 @@ -CMAKE_MINIMUM_REQUIRED(VERSION 2.4) +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) PROJECT(ZED_openGL) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +SET(CMAKE_BUILD_TYPE "Release") + 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() -if(COMMAND cmake_policy) - cmake_policy(SET CMP0003 OLD) - cmake_policy(SET CMP0015 OLD) -endif(COMMAND cmake_policy) - -SET(EXECUTABLE_OUTPUT_PATH ".") - find_package(ZED 3 REQUIRED) -find_package(CUDA ${ZED_CUDA_VERSION} EXACT REQUIRED) +find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED) +SET(OpenGL_GL_PREFERENCE GLVND) find_package(OpenGL REQUIRED) find_package(GLUT REQUIRED) find_package(GLEW REQUIRED) -IF(NOT WIN32) - SET(SPECIAL_OS_LIBS "X11" "pthread") -ENDIF() - include_directories(${ZED_INCLUDE_DIRS}) include_directories(${GLUT_INCLUDE_PATH}) include_directories(${GLEW_INCLUDE_DIRS}) @@ -37,7 +31,6 @@ link_directories(${GLEW_LIBRARY_DIRS}) link_directories(${OpenGL_LIBRARY_DIRS}) ADD_EXECUTABLE(${PROJECT_NAME} src/main.cpp) -add_definitions(-std=c++14 -O3) if (LINK_SHARED_ZED) SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY}) @@ -46,7 +39,6 @@ else() endif() TARGET_LINK_LIBRARIES(${PROJECT_NAME} - ${SPECIAL_OS_LIBS} ${ZED_LIBS} ${OPENGL_LIBRARIES} ${GLUT_LIBRARY} diff --git a/other/opengl gpu interop/src/main.cpp b/other/opengl gpu interop/src/main.cpp index 067d58bd..eb3cd210 100644 --- a/other/opengl gpu interop/src/main.cpp +++ b/other/opengl gpu interop/src/main.cpp @@ -1,7 +1,7 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/plane detection/cpp/CMakeLists.txt b/plane detection/cpp/CMakeLists.txt index 3cb54ab1..797a3ad6 100644 --- a/plane detection/cpp/CMakeLists.txt +++ b/plane detection/cpp/CMakeLists.txt @@ -1,30 +1,23 @@ -CMAKE_MINIMUM_REQUIRED(VERSION 2.4) +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) PROJECT(ZED_Plane_Detection) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +SET(CMAKE_BUILD_TYPE "Release") + 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() -if(COMMAND cmake_policy) - cmake_policy(SET CMP0003 OLD) - cmake_policy(SET CMP0015 OLD) -endif(COMMAND cmake_policy) - -SET(EXECUTABLE_OUTPUT_PATH ".") - find_package(ZED 3 REQUIRED) -find_package(CUDA ${ZED_CUDA_VERSION} EXACT REQUIRED) +find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED) find_package(GLUT REQUIRED) find_package(GLEW REQUIRED) +SET(OpenGL_GL_PREFERENCE GLVND) find_package(OpenGL REQUIRED) -IF(NOT WIN32) - SET(SPECIAL_OS_LIBS "pthread" "X11") - add_definitions(-Wno-write-strings) -ENDIF() - include_directories(${ZED_INCLUDE_DIRS}) include_directories(${GLUT_INCLUDE_PATH}) include_directories(${GLEW_INCLUDE_DIRS}) @@ -42,7 +35,6 @@ FILE(GLOB_RECURSE SRC_FILES src/*.cpp) FILE(GLOB_RECURSE HDR_FILES include/*.hpp) ADD_EXECUTABLE(${PROJECT_NAME} ${HDR_FILES} ${SRC_FILES}) -add_definitions(-std=c++14 -O3) if (LINK_SHARED_ZED) SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY}) @@ -50,8 +42,7 @@ else() SET(ZED_LIBS ${ZED_STATIC_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_LIBRARY}) endif() -TARGET_LINK_LIBRARIES(${PROJECT_NAME} - ${SPECIAL_OS_LIBS} +TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${ZED_LIBS} ${OPENGL_LIBRARIES} ${GLUT_LIBRARY} diff --git a/plane detection/cpp/src/main.cpp b/plane detection/cpp/src/main.cpp index 2e830f82..3521a7a6 100644 --- a/plane detection/cpp/src/main.cpp +++ b/plane detection/cpp/src/main.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/plane detection/csharp/CMakeLists.txt b/plane detection/csharp/CMakeLists.txt index 2c5b4eb1..99299a47 100644 --- a/plane detection/csharp/CMakeLists.txt +++ b/plane detection/csharp/CMakeLists.txt @@ -41,5 +41,5 @@ set(CMAKE_SUPPRESS_REGENERATION true) set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES "OpenGL.Net_0.8.4" "OpenGL.Net.CoreUI_0.8.4" - "Stereolabs.zed_3.6.0" + "Stereolabs.zed_3.7.*" ) \ No newline at end of file diff --git a/plane detection/csharp/MainWindow.cs b/plane detection/csharp/MainWindow.cs index 9cbb8187..709e1755 100644 --- a/plane detection/csharp/MainWindow.cs +++ b/plane detection/csharp/MainWindow.cs @@ -46,7 +46,6 @@ public MainWindow(string[] args) init_params.depthMode = DEPTH_MODE.ULTRA; init_params.coordinateUnits = UNIT.METER; init_params.coordinateSystem = COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP; - init_params.sdkVerbose = true; parseArgs(args, ref init_params); // Open the camera diff --git a/plane detection/python/plane_detection.py b/plane detection/python/plane_detection.py index cb68fe32..5be29555 100644 --- a/plane detection/python/plane_detection.py +++ b/plane detection/python/plane_detection.py @@ -1,6 +1,6 @@ ######################################################################## # -# Copyright (c) 2021, STEREOLABS. +# Copyright (c) 2022, STEREOLABS. # # All rights reserved. # diff --git a/positional tracking/cpp/CMakeLists.txt b/positional tracking/cpp/CMakeLists.txt index 073c5a26..2a02f4e5 100644 --- a/positional tracking/cpp/CMakeLists.txt +++ b/positional tracking/cpp/CMakeLists.txt @@ -1,33 +1,22 @@ -cmake_minimum_required(VERSION 2.4) +cmake_minimum_required(VERSION 3.5) PROJECT(ZED_Positional_Tracking) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +SET(CMAKE_BUILD_TYPE "Release") + 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() -if(COMMAND cmake_policy) - cmake_policy(SET CMP0003 OLD) - cmake_policy(SET CMP0015 OLD) -endif(COMMAND cmake_policy) - -if (NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "") -SET(CMAKE_BUILD_TYPE "RelWithDebInfo") -endif() - -SET(EXECUTABLE_OUTPUT_PATH ".") - find_package(ZED 3 REQUIRED) +find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED) find_package(GLUT REQUIRED) find_package(GLEW REQUIRED) +SET(OpenGL_GL_PREFERENCE GLVND) find_package(OpenGL REQUIRED) -find_package(CUDA ${ZED_CUDA_VERSION} EXACT REQUIRED) - -IF(NOT WIN32) - SET(SPECIAL_OS_LIBS "pthread" "X11") - add_definitions(-Wno-write-strings) -ENDIF() include_directories(${ZED_INCLUDE_DIRS}) include_directories(${GLEW_INCLUDE_DIRS}) @@ -45,7 +34,6 @@ FILE(GLOB_RECURSE SRC_FILES src/*.cpp) FILE(GLOB_RECURSE HDR_FILES include/*.hpp) ADD_EXECUTABLE(${PROJECT_NAME} ${HDR_FILES} ${SRC_FILES}) -add_definitions(-std=c++14 -O3) if (LINK_SHARED_ZED) SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY}) @@ -54,7 +42,6 @@ else() endif() TARGET_LINK_LIBRARIES(${PROJECT_NAME} - ${SPECIAL_OS_LIBS} ${ZED_LIBS} ${OPENGL_LIBRARIES} ${GLUT_LIBRARY} diff --git a/positional tracking/cpp/src/main.cpp b/positional tracking/cpp/src/main.cpp index f38ea755..4f14e5e2 100644 --- a/positional tracking/cpp/src/main.cpp +++ b/positional tracking/cpp/src/main.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/positional tracking/csharp/CMakeLists.txt b/positional tracking/csharp/CMakeLists.txt index 4dfca95a..42feb822 100644 --- a/positional tracking/csharp/CMakeLists.txt +++ b/positional tracking/csharp/CMakeLists.txt @@ -42,5 +42,5 @@ set(CMAKE_SUPPRESS_REGENERATION true) set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES "OpenGL.Net_0.8.4" "OpenGL.Net.CoreUI_0.8.4" - "Stereolabs.zed_3.6.0" + "Stereolabs.zed_3.7.*" ) \ No newline at end of file diff --git a/positional tracking/csharp/MainWindow.cs b/positional tracking/csharp/MainWindow.cs index 7d6cdf37..53fd20e1 100644 --- a/positional tracking/csharp/MainWindow.cs +++ b/positional tracking/csharp/MainWindow.cs @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // @@ -57,7 +57,6 @@ public MainWindow(string[] args) init_params.depthMode = DEPTH_MODE.ULTRA; init_params.coordinateUnits = UNIT.METER; init_params.coordinateSystem = COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP; - init_params.sdkVerbose = true; parseArgs(args, ref init_params); // Open the camera @@ -198,6 +197,7 @@ private void NativeWindow_Render(object sender, NativeWindowEventArgs e) private void close() { + zedCamera.SaveAreaMap("toto.area"); zedCamera.DisablePositionalTracking(); zedCamera.Close(); viewer.exit(); diff --git a/positional tracking/python/positional_tracking.py b/positional tracking/python/positional_tracking.py index 33710873..a186432b 100644 --- a/positional tracking/python/positional_tracking.py +++ b/positional tracking/python/positional_tracking.py @@ -1,6 +1,6 @@ ######################################################################## # -# Copyright (c) 2021, STEREOLABS. +# Copyright (c) 2022, STEREOLABS. # # All rights reserved. # diff --git a/spatial mapping/advanced point cloud mapping/cpp/CMakeLists.txt b/spatial mapping/advanced point cloud mapping/cpp/CMakeLists.txt index 0b60dfd7..73e91986 100644 --- a/spatial mapping/advanced point cloud mapping/cpp/CMakeLists.txt +++ b/spatial mapping/advanced point cloud mapping/cpp/CMakeLists.txt @@ -1,33 +1,24 @@ -CMAKE_MINIMUM_REQUIRED(VERSION 2.4) +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) PROJECT(ZED_Point_Cloud_Mapping) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +SET(CMAKE_BUILD_TYPE "Release") + 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() -if(COMMAND cmake_policy) - cmake_policy(SET CMP0003 OLD) - cmake_policy(SET CMP0015 OLD) -endif(COMMAND cmake_policy) - -SET(EXECUTABLE_OUTPUT_PATH ".") -SET(SPECIAL_OS_LIBS "") - find_package(ZED 3 REQUIRED) +find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED) find_package(OpenCV REQUIRED) find_package(GLUT REQUIRED) find_package(GLEW REQUIRED) +SET(OpenGL_GL_PREFERENCE GLVND) find_package(OpenGL REQUIRED) -IF(NOT WIN32) - SET(SPECIAL_OS_LIBS "pthread" "X11") - add_definitions(-Wno-write-strings -fpermissive) -ENDIF() - -find_package(CUDA ${ZED_CUDA_VERSION} EXACT REQUIRED) - include_directories(${ZED_INCLUDE_DIRS}) include_directories(${OpenCV_INCLUDE_DIRS}) include_directories(${GLEW_INCLUDE_DIRS}) @@ -41,22 +32,19 @@ link_directories(${GLEW_LIBRARY_DIRS}) link_directories(${GLUT_LIBRARY_DIRS}) link_directories(${OpenGL_LIBRARY_DIRS}) link_directories(${CUDA_LIBRARY_DIRS}) -link_directories(${CMAKE_CURRENT_SOURCE_DIR}/lib) FILE(GLOB_RECURSE SRC_FILES src/*.cpp) FILE(GLOB_RECURSE HDR_FILES include/*.hpp) ADD_EXECUTABLE(${PROJECT_NAME} ${HDR_FILES} ${SRC_FILES}) -add_definitions(-std=c++14 -O3) if (LINK_SHARED_ZED) - SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY} ${CUDA_DEP_LIBRARIES_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} - ${SPECIAL_OS_LIBS} ${ZED_LIBS} ${OpenCV_LIBRARIES} ${OPENGL_LIBRARIES} diff --git a/spatial mapping/advanced point cloud mapping/cpp/src/main.cpp b/spatial mapping/advanced point cloud mapping/cpp/src/main.cpp index 45716e31..0756ea8b 100644 --- a/spatial mapping/advanced point cloud mapping/cpp/src/main.cpp +++ b/spatial mapping/advanced point cloud mapping/cpp/src/main.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/spatial mapping/basic/cpp/CMakeLists.txt b/spatial mapping/basic/cpp/CMakeLists.txt index 8449e55b..2404e7c4 100644 --- a/spatial mapping/basic/cpp/CMakeLists.txt +++ b/spatial mapping/basic/cpp/CMakeLists.txt @@ -1,34 +1,23 @@ -CMAKE_MINIMUM_REQUIRED(VERSION 2.4) +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) PROJECT(ZED_Spatial_Mapping) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +SET(CMAKE_BUILD_TYPE "Release") + 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() -if(COMMAND cmake_policy) - cmake_policy(SET CMP0003 OLD) - cmake_policy(SET CMP0015 OLD) -endif(COMMAND cmake_policy) - -if (NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "") -SET(CMAKE_BUILD_TYPE "RelWithDebInfo") -endif() - -SET(EXECUTABLE_OUTPUT_PATH ".") - find_package(ZED 3 REQUIRED) -find_package(CUDA ${ZED_CUDA_VERSION} EXACT REQUIRED) +find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED) find_package(GLUT REQUIRED) find_package(GLEW REQUIRED) +SET(OpenGL_GL_PREFERENCE GLVND) find_package(OpenGL REQUIRED) -IF(NOT WIN32) - SET(SPECIAL_OS_LIBS "pthread" "X11") - add_definitions(-Wno-write-strings -fpermissive) -ENDIF() - include_directories(${ZED_INCLUDE_DIRS}) include_directories(${GLUT_INCLUDE_PATH}) include_directories(${GLEW_INCLUDE_DIRS}) @@ -46,7 +35,6 @@ FILE(GLOB_RECURSE SRC_FILES src/*) FILE(GLOB_RECURSE HDR_FILES include/*) ADD_EXECUTABLE(${PROJECT_NAME} ${HDR_FILES} ${SRC_FILES}) -add_definitions(-std=c++14 -O3) if (LINK_SHARED_ZED) SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY}) @@ -55,7 +43,6 @@ else() endif() TARGET_LINK_LIBRARIES(${PROJECT_NAME} - ${SPECIAL_OS_LIBS} ${ZED_LIBS} ${OPENGL_LIBRARIES} ${GLUT_LIBRARY} diff --git a/spatial mapping/basic/cpp/src/main.cpp b/spatial mapping/basic/cpp/src/main.cpp index 538bfa8e..deae0967 100644 --- a/spatial mapping/basic/cpp/src/main.cpp +++ b/spatial mapping/basic/cpp/src/main.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/spatial mapping/basic/csharp/CMakeLists.txt b/spatial mapping/basic/csharp/CMakeLists.txt index dd9f6c76..928d41a5 100644 --- a/spatial mapping/basic/csharp/CMakeLists.txt +++ b/spatial mapping/basic/csharp/CMakeLists.txt @@ -40,5 +40,5 @@ set(CMAKE_SUPPRESS_REGENERATION true) set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES "OpenGL.Net_0.8.4" "OpenGL.Net.CoreUI_0.8.4" - "Stereolabs.zed_3.6.0" + "Stereolabs.zed_3.7.*" ) \ No newline at end of file diff --git a/spatial mapping/basic/csharp/MainWindow.cs b/spatial mapping/basic/csharp/MainWindow.cs index ada54987..d840d94c 100644 --- a/spatial mapping/basic/csharp/MainWindow.cs +++ b/spatial mapping/basic/csharp/MainWindow.cs @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // @@ -67,7 +67,6 @@ public MainWindow(string[] args) init_params.coordinateUnits = UNIT.METER; init_params.coordinateSystem = COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP; init_params.depthMaximumDistance = 15f; - init_params.sdkVerbose = true; parseArgs(args, ref init_params); // Open the camera @@ -77,12 +76,6 @@ public MainWindow(string[] args) if (err != ERROR_CODE.SUCCESS) Environment.Exit(-1); - if (zedCamera.CameraModel != sl.MODEL.ZED2) - { - Console.WriteLine(" ERROR : Use ZED2 Camera only"); - return; - } - tracking_state = POSITIONAL_TRACKING_STATE.OFF; mapping_state = SPATIAL_MAPPING_STATE.NOT_ENABLED; mapping_activated = false; diff --git a/spatial mapping/basic/python/ogl_viewer/viewer.py b/spatial mapping/basic/python/ogl_viewer/viewer.py index 2f2cb091..a1e805de 100644 --- a/spatial mapping/basic/python/ogl_viewer/viewer.py +++ b/spatial mapping/basic/python/ogl_viewer/viewer.py @@ -12,7 +12,7 @@ M_PI = 3.1415926 -MESH_VERTEX_SHADER =""" +MESH_VERTEX_SHADER = """ #version 330 core layout(location = 0) in vec3 in_Vertex; uniform mat4 u_mvpMatrix; @@ -24,6 +24,18 @@ } """ +FPC_VERTEX_SHADER = """ +#version 330 core +layout(location = 0) in vec4 in_Vertex; +uniform mat4 u_mvpMatrix; +uniform vec3 u_color; +out vec3 b_color; +void main() { + b_color = u_color; + gl_Position = u_mvpMatrix * vec4(in_Vertex.xyz, 1); +} +""" + VERTEX_SHADER = """ # version 330 core layout(location = 0) in vec3 in_Vertex; @@ -207,7 +219,7 @@ def __init__(self): self.tracking_state = sl.POSITIONAL_TRACKING_STATE.OFF self.mapping_state = sl.SPATIAL_MAPPING_STATE.NOT_ENABLED - def init(self, _params, _mesh): + def init(self, _params, _mesh, _create_mesh): glutInit() wnd_w = glutGet(GLUT_SCREEN_WIDTH) wnd_h = glutGet(GLUT_SCREEN_HEIGHT) @@ -233,12 +245,13 @@ def init(self, _params, _mesh): # Initialize image renderer self.image_handler.initialize(_params.image_size) - # Init mesh (TODO fused point cloud) - self.init_mesh(_mesh) + self.init_mesh(_mesh, _create_mesh) # Compile and create the shader if(self.draw_mesh): self.shader_image = Shader(MESH_VERTEX_SHADER, FRAGMENT_SHADER) + else: + self.shader_image = Shader(FPC_VERTEX_SHADER, FRAGMENT_SHADER) self.shader_MVP = glGetUniformLocation(self.shader_image.get_program_id(), "u_mvpMatrix") self.shader_color_loc = glGetUniformLocation(self.shader_image.get_program_id(), "u_color") @@ -267,8 +280,8 @@ def init(self, _params, _mesh): # Ready to start self.chunks_pushed = True - def init_mesh(self, _mesh): - self.draw_mesh = True + def init_mesh(self, _mesh, _create_mesh): + self.draw_mesh = _create_mesh self.mesh = _mesh def set_render_camera_projection(self, _params): @@ -357,24 +370,26 @@ def draw_callback(self): glutSwapBuffers() glutPostRedisplay() + # Update both Mesh and FPC def update(self): if self.new_chunks: if self.ask_clear: self.sub_maps = [] self.ask_clear = False - nb_c = 0 - if self.draw_mesh: - nb_c = len(self.mesh.chunks) + nb_c = len(self.mesh.chunks) if nb_c > len(self.sub_maps): for n in range(len(self.sub_maps),nb_c): self.sub_maps.append(SubMapObj()) - if self.draw_mesh: - for m in range(len(self.sub_maps)): - if (m < nb_c) and self.mesh.chunks[m].has_been_updated: - self.sub_maps[m].update(self.mesh.chunks[m]) + # For both Mesh and FPC + for m in range(len(self.sub_maps)): + if (m < nb_c) and self.mesh.chunks[m].has_been_updated: + if self.draw_mesh: + self.sub_maps[m].update_mesh(self.mesh.chunks[m]) + else: + self.sub_maps[m].update_fpc(self.mesh.chunks[m]) self.new_chunks = False self.chunks_pushed = True @@ -399,7 +414,7 @@ def draw(self): glUniform3fv(self.shader_color_loc, 1, (GLfloat * len(self.vertices_color))(*self.vertices_color)) for m in range(len(self.sub_maps)): - self.sub_maps[m].draw() + self.sub_maps[m].draw(self.draw_mesh) glUseProgram(0) glPolygonMode(GL_FRONT_AND_BACK, GL_FILL) @@ -440,10 +455,11 @@ class SubMapObj: def __init__(self): self.current_fc = 0 self.vboID = None + self.index = [] # For FPC only self.vert = [] self.tri = [] - def update(self, _chunk): + def update_mesh(self, _chunk): if(self.vboID is None): self.vboID = glGenBuffers(2) @@ -451,20 +467,43 @@ def update(self, _chunk): self.vert = _chunk.vertices.flatten() # transform _chunk.vertices into 1D array glBindBuffer(GL_ARRAY_BUFFER, self.vboID[0]) glBufferData(GL_ARRAY_BUFFER, len(self.vert) * self.vert.itemsize, (GLfloat * len(self.vert))(*self.vert), GL_DYNAMIC_DRAW) - + if len(_chunk.triangles): self.tri = _chunk.triangles.flatten() # transform _chunk.triangles into 1D array glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, self.vboID[1]) glBufferData(GL_ELEMENT_ARRAY_BUFFER, len(self.tri) * self.tri.itemsize , (GLuint * len(self.tri))(*self.tri), GL_DYNAMIC_DRAW) self.current_fc = len(self.tri) - - def draw(self): + + def update_fpc(self, _chunk): + if(self.vboID is None): + self.vboID = glGenBuffers(2) + + if len(_chunk.vertices): + self.vert = _chunk.vertices.flatten() # transform _chunk.vertices into 1D array + glBindBuffer(GL_ARRAY_BUFFER, self.vboID[0]) + glBufferData(GL_ARRAY_BUFFER, len(self.vert) * self.vert.itemsize, (GLfloat * len(self.vert))(*self.vert), GL_DYNAMIC_DRAW) + + for i in range(len(_chunk.vertices)): + self.index.append(i) + + index_np = np.array(self.index) + glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, self.vboID[1]) + glBufferData(GL_ELEMENT_ARRAY_BUFFER, len(index_np) * index_np.itemsize, (GLuint * len(index_np))(*index_np), GL_DYNAMIC_DRAW) + self.current_fc = len(index_np) + + def draw(self, _draw_mesh): if self.current_fc: glEnableVertexAttribArray(0) glBindBuffer(GL_ARRAY_BUFFER, self.vboID[0]) - glVertexAttribPointer(0,3,GL_FLOAT,GL_FALSE,0,None) + if _draw_mesh == True: + glVertexAttribPointer(0,3,GL_FLOAT,GL_FALSE,0,None) + else: + glVertexAttribPointer(0,4,GL_FLOAT,GL_FALSE,0,None) glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, self.vboID[1]) - glDrawElements(GL_TRIANGLES, self.current_fc, GL_UNSIGNED_INT, None) - + if len(self.index) > 0: + glDrawElements(GL_POINTS, self.current_fc, GL_UNSIGNED_INT, None) + else: + glDrawElements(GL_TRIANGLES, self.current_fc, GL_UNSIGNED_INT, None) + glDisableVertexAttribArray(0) diff --git a/spatial mapping/basic/python/spatial_mapping.py b/spatial mapping/basic/python/spatial_mapping.py index bbb50865..022f8783 100644 --- a/spatial mapping/basic/python/spatial_mapping.py +++ b/spatial mapping/basic/python/spatial_mapping.py @@ -1,6 +1,6 @@ ######################################################################## # -# Copyright (c) 2021, STEREOLABS. +# Copyright (c) 2022, STEREOLABS. # # All rights reserved. # @@ -29,6 +29,9 @@ import pyzed.sl as sl import ogl_viewer.viewer as gl +# Set this variable to 'True' for a Mesh viewer +# or to 'False' for a Point Cloud viewer +CREATE_MESH = False def main(): print("Running Spatial Mapping sample ... Press 'q' to quit") @@ -58,12 +61,15 @@ def main(): # Get camera parameters camera_parameters = zed.get_camera_information().camera_configuration.calibration_parameters.left_cam - pymesh = sl.Mesh() # Current incremental mesh - image = sl.Mat() # Left image from camera - pose = sl.Pose() # Camera pose tracking data + if CREATE_MESH: + pymesh = sl.Mesh() # Current incremental mesh + else: + pymesh = sl.FusedPointCloud() # Current incremental FusedPointCloud + image = sl.Mat() # Left image from camera + pose = sl.Pose() # Camera pose tracking data viewer = gl.GLViewer() - viewer.init(camera_parameters, pymesh) + viewer.init(camera_parameters, pymesh, CREATE_MESH) spatial_mapping_parameters = sl.SpatialMappingParameters() tracking_state = sl.POSITIONAL_TRACKING_STATE.OFF @@ -112,10 +118,13 @@ def main(): spatial_mapping_parameters.resolution_meter = sl.SpatialMappingParameters().get_resolution_preset(sl.MAPPING_RESOLUTION.MEDIUM) spatial_mapping_parameters.use_chunk_only = True spatial_mapping_parameters.save_texture = False # Set to True to apply texture over the created mesh - spatial_mapping_parameters.map_type = sl.SPATIAL_MAP_TYPE.MESH + if CREATE_MESH: + spatial_mapping_parameters.map_type = sl.SPATIAL_MAP_TYPE.MESH + else: + spatial_mapping_parameters.map_type = sl.SPATIAL_MAP_TYPE.FUSED_POINT_CLOUD # Enable spatial mapping - zed.enable_spatial_mapping() + zed.enable_spatial_mapping(spatial_mapping_parameters) # Clear previous mesh data pymesh.clear() @@ -129,16 +138,17 @@ def main(): # Extract whole mesh zed.extract_whole_spatial_map(pymesh) - filter_params = sl.MeshFilterParameters() - filter_params.set(sl.MESH_FILTER.MEDIUM) - # Filter the extracted mesh - pymesh.filter(filter_params, True) - viewer.clear_current_mesh() - - # If textures have been saved during spatial mapping, apply them to the mesh - if(spatial_mapping_parameters.save_texture): - print("Save texture set to : {}".format(spatial_mapping_parameters.save_texture)) - pymesh.apply_texture(sl.MESH_TEXTURE_FORMAT.RGBA) + if CREATE_MESH: + filter_params = sl.MeshFilterParameters() + filter_params.set(sl.MESH_FILTER.MEDIUM) + # Filter the extracted mesh + pymesh.filter(filter_params, True) + viewer.clear_current_mesh() + + # If textures have been saved during spatial mapping, apply them to the mesh + if(spatial_mapping_parameters.save_texture): + print("Save texture set to : {}".format(spatial_mapping_parameters.save_texture)) + pymesh.apply_texture(sl.MESH_TEXTURE_FORMAT.RGBA) # Save mesh as an obj file filepath = "mesh_gen.obj" diff --git a/svo recording/export/cpp/CMakeLists.txt b/svo recording/export/cpp/CMakeLists.txt index 23c72485..0f27b08b 100644 --- a/svo recording/export/cpp/CMakeLists.txt +++ b/svo recording/export/cpp/CMakeLists.txt @@ -1,30 +1,19 @@ -CMAKE_MINIMUM_REQUIRED(VERSION 2.4) +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) PROJECT(ZED_SVO_Export) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +SET(CMAKE_BUILD_TYPE "Release") + 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() -if(COMMAND cmake_policy) - cmake_policy(SET CMP0003 OLD) - cmake_policy(SET CMP0015 OLD) -endif(COMMAND cmake_policy) - -if (NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "") -SET(CMAKE_BUILD_TYPE "RelWithDebInfo") -endif() - -SET(EXECUTABLE_OUTPUT_PATH ".") - find_package(ZED 3 REQUIRED) find_package(OpenCV REQUIRED) -find_package(CUDA ${ZED_CUDA_VERSION} EXACT REQUIRED) - -IF(NOT WIN32) - SET(SPECIAL_OS_LIBS "pthread" "X11") -ENDIF() +find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED) include_directories(${CUDA_INCLUDE_DIRS}) include_directories(${ZED_INCLUDE_DIRS}) @@ -36,7 +25,6 @@ link_directories(${CUDA_LIBRARY_DIRS}) link_directories(${OpenCV_LIBRARY_DIRS}) ADD_EXECUTABLE(${PROJECT_NAME} include/utils.hpp src/main.cpp) -add_definitions(-std=c++14 -O3) if (LINK_SHARED_ZED) SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY}) diff --git a/svo recording/export/cpp/include/utils.hpp b/svo recording/export/cpp/include/utils.hpp index deb12301..521ca29f 100644 --- a/svo recording/export/cpp/include/utils.hpp +++ b/svo recording/export/cpp/include/utils.hpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/svo recording/export/cpp/src/main.cpp b/svo recording/export/cpp/src/main.cpp index e50296ba..22b62465 100644 --- a/svo recording/export/cpp/src/main.cpp +++ b/svo recording/export/cpp/src/main.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/svo recording/export/csharp/CMakeLists.txt b/svo recording/export/csharp/CMakeLists.txt index 8f06823d..6bba660c 100644 --- a/svo recording/export/csharp/CMakeLists.txt +++ b/svo recording/export/csharp/CMakeLists.txt @@ -30,6 +30,6 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES set(CMAKE_SUPPRESS_REGENERATION true) set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES - "Stereolabs.zed_3.6.0" + "Stereolabs.zed_3.7.*" "OpenCvSharp4.Windows_4.5.0.20201013" ) \ No newline at end of file diff --git a/svo recording/export/csharp/Program.cs b/svo recording/export/csharp/Program.cs index a5112fe7..12d4f4f4 100644 --- a/svo recording/export/csharp/Program.cs +++ b/svo recording/export/csharp/Program.cs @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/svo recording/export/python/svo_export.py b/svo recording/export/python/svo_export.py index 3a9d0417..e76de90a 100644 --- a/svo recording/export/python/svo_export.py +++ b/svo recording/export/python/svo_export.py @@ -1,6 +1,6 @@ ######################################################################## # -# Copyright (c) 2021, STEREOLABS. +# Copyright (c) 2022, STEREOLABS. # # All rights reserved. # diff --git a/svo recording/playback/cpp/CMakeLists.txt b/svo recording/playback/cpp/CMakeLists.txt index 9ac050b6..8230f152 100644 --- a/svo recording/playback/cpp/CMakeLists.txt +++ b/svo recording/playback/cpp/CMakeLists.txt @@ -1,31 +1,20 @@ -CMAKE_MINIMUM_REQUIRED(VERSION 2.4) +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) PROJECT(ZED_SVO_Playback) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +SET(CMAKE_BUILD_TYPE "Release") + 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() -if(COMMAND cmake_policy) - cmake_policy(SET CMP0003 OLD) - cmake_policy(SET CMP0015 OLD) -endif(COMMAND cmake_policy) - -if (NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "") -SET(CMAKE_BUILD_TYPE "RelWithDebInfo") -endif() - -SET(EXECUTABLE_OUTPUT_PATH ".") - find_package(ZED 3 REQUIRED) find_package(OpenCV REQUIRED) -find_package(CUDA ${ZED_CUDA_VERSION} EXACT REQUIRED) +find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED) -IF(NOT WIN32) - SET(SPECIAL_OS_LIBS "pthread" "X11") -ENDIF() - include_directories(${CUDA_INCLUDE_DIRS}) include_directories(${ZED_INCLUDE_DIRS}) include_directories(${OpenCV_INCLUDE_DIRS}) @@ -36,7 +25,6 @@ link_directories(${CUDA_LIBRARY_DIRS}) link_directories(${OpenCV_LIBRARY_DIRS}) ADD_EXECUTABLE(${PROJECT_NAME} include/utils.hpp src/main.cpp) -add_definitions(-std=c++14 -O3) if (LINK_SHARED_ZED) SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY}) diff --git a/svo recording/playback/cpp/include/utils.hpp b/svo recording/playback/cpp/include/utils.hpp index 0aeefcf0..f9cf0c9c 100644 --- a/svo recording/playback/cpp/include/utils.hpp +++ b/svo recording/playback/cpp/include/utils.hpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/svo recording/playback/cpp/src/main.cpp b/svo recording/playback/cpp/src/main.cpp index 0b5af64f..285e9a3f 100644 --- a/svo recording/playback/cpp/src/main.cpp +++ b/svo recording/playback/cpp/src/main.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/svo recording/playback/csharp/CMakeLists.txt b/svo recording/playback/csharp/CMakeLists.txt index 1354ac81..c7e7d055 100644 --- a/svo recording/playback/csharp/CMakeLists.txt +++ b/svo recording/playback/csharp/CMakeLists.txt @@ -29,6 +29,6 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES set(CMAKE_SUPPRESS_REGENERATION true) set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES - "Stereolabs.zed_3.6.0" + "Stereolabs.zed_3.7.*" "OpenCvSharp4.Windows_4.5.0.20201013" ) diff --git a/svo recording/playback/csharp/Program.cs b/svo recording/playback/csharp/Program.cs index a2250932..3af5dec1 100644 --- a/svo recording/playback/csharp/Program.cs +++ b/svo recording/playback/csharp/Program.cs @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/svo recording/playback/python/svo_playback.py b/svo recording/playback/python/svo_playback.py index 3d75ec03..45ab57d4 100644 --- a/svo recording/playback/python/svo_playback.py +++ b/svo recording/playback/python/svo_playback.py @@ -1,6 +1,6 @@ ######################################################################## # -# Copyright (c) 2021, STEREOLABS. +# Copyright (c) 2022, STEREOLABS. # # All rights reserved. # diff --git a/svo recording/recording/cpp/CMakeLists.txt b/svo recording/recording/cpp/CMakeLists.txt index b34b1af0..d0fbeecd 100644 --- a/svo recording/recording/cpp/CMakeLists.txt +++ b/svo recording/recording/cpp/CMakeLists.txt @@ -1,30 +1,19 @@ -CMAKE_MINIMUM_REQUIRED(VERSION 2.4) +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) PROJECT(ZED_SVO_Recording) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +SET(CMAKE_BUILD_TYPE "Release") + 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() -if(COMMAND cmake_policy) - cmake_policy(SET CMP0003 OLD) - cmake_policy(SET CMP0015 OLD) -endif(COMMAND cmake_policy) - -if (NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "") -SET(CMAKE_BUILD_TYPE "RelWithDebInfo") -endif() - -SET(EXECUTABLE_OUTPUT_PATH ".") - -find_package(ZED 2 REQUIRED) -find_package(CUDA ${ZED_CUDA_VERSION} EXACT REQUIRED) +find_package(ZED 3 REQUIRED) +find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED) -IF(NOT WIN32) - SET(SPECIAL_OS_LIBS "pthread" "X11") -ENDIF() - include_directories(${CUDA_INCLUDE_DIRS}) include_directories(${ZED_INCLUDE_DIRS}) include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) @@ -33,10 +22,9 @@ link_directories(${ZED_LIBRARY_DIR}) link_directories(${CUDA_LIBRARY_DIRS}) ADD_EXECUTABLE(${PROJECT_NAME} include/utils.hpp src/main.cpp) -add_definitions(-std=c++14 -O3) if (LINK_SHARED_ZED) - SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY} ${CUDA_NPP_LIBRARIES_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() diff --git a/svo recording/recording/cpp/include/utils.hpp b/svo recording/recording/cpp/include/utils.hpp index 08a86ed6..bf1c28d8 100644 --- a/svo recording/recording/cpp/include/utils.hpp +++ b/svo recording/recording/cpp/include/utils.hpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/svo recording/recording/cpp/src/main.cpp b/svo recording/recording/cpp/src/main.cpp index 91cda62e..9330e74f 100644 --- a/svo recording/recording/cpp/src/main.cpp +++ b/svo recording/recording/cpp/src/main.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // @@ -61,7 +61,7 @@ int main(int argc, char **argv) { // Enable recording with the filename specified in argument String path_output(argv[1]); - returned_state = zed.enableRecording(RecordingParameters(path_output, SVO_COMPRESSION_MODE::H264)); + returned_state = zed.enableRecording(RecordingParameters(path_output, SVO_COMPRESSION_MODE::H264_LOSSLESS)); if (returned_state != ERROR_CODE::SUCCESS) { print("Recording ZED : ", returned_state); zed.close(); diff --git a/svo recording/recording/csharp/CMakeLists.txt b/svo recording/recording/csharp/CMakeLists.txt index 771f1b34..3be7d198 100644 --- a/svo recording/recording/csharp/CMakeLists.txt +++ b/svo recording/recording/csharp/CMakeLists.txt @@ -30,5 +30,5 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES set(CMAKE_SUPPRESS_REGENERATION true) set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES - "Stereolabs.zed_3.6.0" + "Stereolabs.zed_3.7.*" ) \ No newline at end of file diff --git a/svo recording/recording/csharp/Program.cs b/svo recording/recording/csharp/Program.cs index 4f4cad2e..0e9007a9 100644 --- a/svo recording/recording/csharp/Program.cs +++ b/svo recording/recording/csharp/Program.cs @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/svo recording/recording/python/svo_recording.py b/svo recording/recording/python/svo_recording.py index a7ef0a2b..7465a28e 100644 --- a/svo recording/recording/python/svo_recording.py +++ b/svo recording/recording/python/svo_recording.py @@ -1,6 +1,6 @@ ######################################################################## # -# Copyright (c) 2021, STEREOLABS. +# Copyright (c) 2022, STEREOLABS. # # All rights reserved. # diff --git a/tutorials/tutorial 1 - hello ZED/c/main.c b/tutorials/tutorial 1 - hello ZED/c/main.c index c9a22839..bbcc4b4c 100644 --- a/tutorials/tutorial 1 - hello ZED/c/main.c +++ b/tutorials/tutorial 1 - hello ZED/c/main.c @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/tutorials/tutorial 1 - hello ZED/cpp/CMakeLists.txt b/tutorials/tutorial 1 - hello ZED/cpp/CMakeLists.txt index ff23a439..73e6672e 100644 --- a/tutorials/tutorial 1 - hello ZED/cpp/CMakeLists.txt +++ b/tutorials/tutorial 1 - hello ZED/cpp/CMakeLists.txt @@ -1,25 +1,17 @@ -CMAKE_MINIMUM_REQUIRED(VERSION 2.4) +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) PROJECT(ZED_Tutorial_1) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED 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() -if(COMMAND cmake_policy) - cmake_policy(SET CMP0003 OLD) - cmake_policy(SET CMP0015 OLD) -endif(COMMAND cmake_policy) - -if (NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "") -SET(CMAKE_BUILD_TYPE "RelWithDebInfo") -endif() - -SET(EXECUTABLE_OUTPUT_PATH ".") - find_package(ZED 3 REQUIRED) -find_package(CUDA ${ZED_CUDA_VERSION} EXACT REQUIRED) +find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED) include_directories(${CUDA_INCLUDE_DIRS}) include_directories(${ZED_INCLUDE_DIRS}) @@ -28,7 +20,6 @@ link_directories(${ZED_LIBRARY_DIR}) link_directories(${CUDA_LIBRARY_DIRS}) ADD_EXECUTABLE(${PROJECT_NAME} main.cpp) -add_definitions(-std=c++14 -O3) if (LINK_SHARED_ZED) SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY}) diff --git a/tutorials/tutorial 1 - hello ZED/cpp/main.cpp b/tutorials/tutorial 1 - hello ZED/cpp/main.cpp index b5de162c..d59d2c0a 100644 --- a/tutorials/tutorial 1 - hello ZED/cpp/main.cpp +++ b/tutorials/tutorial 1 - hello ZED/cpp/main.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/tutorials/tutorial 1 - hello ZED/csharp/CMakeLists.txt b/tutorials/tutorial 1 - hello ZED/csharp/CMakeLists.txt index cc730039..c82ab6df 100644 --- a/tutorials/tutorial 1 - hello ZED/csharp/CMakeLists.txt +++ b/tutorials/tutorial 1 - hello ZED/csharp/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required( VERSION 3.8.0 ) project(Hello_ZED CSharp) + add_executable(${PROJECT_NAME} Program.cs App.config @@ -28,9 +29,5 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES set(CMAKE_SUPPRESS_REGENERATION true) set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES - "Stereolabs.zed_3.6.0" -<<<<<<< HEAD -) -======= + "Stereolabs.zed_3.7.*" ) ->>>>>>> release_3.6 diff --git a/tutorials/tutorial 1 - hello ZED/python/hello_zed.py b/tutorials/tutorial 1 - hello ZED/python/hello_zed.py index b9fe11ca..f10a8972 100644 --- a/tutorials/tutorial 1 - hello ZED/python/hello_zed.py +++ b/tutorials/tutorial 1 - hello ZED/python/hello_zed.py @@ -1,6 +1,6 @@ ######################################################################## # -# Copyright (c) 2021, STEREOLABS. +# Copyright (c) 2022, STEREOLABS. # # All rights reserved. # diff --git a/tutorials/tutorial 2 - image capture/c/main.c b/tutorials/tutorial 2 - image capture/c/main.c index 79e123fb..ddf59505 100644 --- a/tutorials/tutorial 2 - image capture/c/main.c +++ b/tutorials/tutorial 2 - image capture/c/main.c @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/tutorials/tutorial 2 - image capture/cpp/CMakeLists.txt b/tutorials/tutorial 2 - image capture/cpp/CMakeLists.txt index 2e190c21..9376d1e9 100644 --- a/tutorials/tutorial 2 - image capture/cpp/CMakeLists.txt +++ b/tutorials/tutorial 2 - image capture/cpp/CMakeLists.txt @@ -1,25 +1,17 @@ -CMAKE_MINIMUM_REQUIRED(VERSION 2.4) +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) PROJECT(ZED_Tutorial_2) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED 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() -if(COMMAND cmake_policy) - cmake_policy(SET CMP0003 OLD) - cmake_policy(SET CMP0015 OLD) -endif(COMMAND cmake_policy) - -if (NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "") -SET(CMAKE_BUILD_TYPE "RelWithDebInfo") -endif() - -SET(EXECUTABLE_OUTPUT_PATH ".") - find_package(ZED 3 REQUIRED) -find_package(CUDA ${ZED_CUDA_VERSION} EXACT REQUIRED) +find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED) include_directories(${CUDA_INCLUDE_DIRS}) include_directories(${ZED_INCLUDE_DIRS}) @@ -28,7 +20,6 @@ link_directories(${ZED_LIBRARY_DIR}) link_directories(${CUDA_LIBRARY_DIRS}) ADD_EXECUTABLE(${PROJECT_NAME} main.cpp) -add_definitions(-std=c++14 -O3) if (LINK_SHARED_ZED) SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY}) diff --git a/tutorials/tutorial 2 - image capture/cpp/main.cpp b/tutorials/tutorial 2 - image capture/cpp/main.cpp index f7fc26c5..84a18f88 100644 --- a/tutorials/tutorial 2 - image capture/cpp/main.cpp +++ b/tutorials/tutorial 2 - image capture/cpp/main.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/tutorials/tutorial 2 - image capture/csharp/CMakeLists.txt b/tutorials/tutorial 2 - image capture/csharp/CMakeLists.txt index a67cc934..68a62147 100644 --- a/tutorials/tutorial 2 - image capture/csharp/CMakeLists.txt +++ b/tutorials/tutorial 2 - image capture/csharp/CMakeLists.txt @@ -28,5 +28,5 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES set(CMAKE_SUPPRESS_REGENERATION true) set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES - "Stereolabs.zed_3.6.0" + "Stereolabs.zed_3.7.*" ) \ No newline at end of file diff --git a/tutorials/tutorial 2 - image capture/python/image_capture.py b/tutorials/tutorial 2 - image capture/python/image_capture.py index c24a5f30..18df903e 100644 --- a/tutorials/tutorial 2 - image capture/python/image_capture.py +++ b/tutorials/tutorial 2 - image capture/python/image_capture.py @@ -1,6 +1,6 @@ ######################################################################## # -# Copyright (c) 2021, STEREOLABS. +# Copyright (c) 2022, STEREOLABS. # # All rights reserved. # diff --git a/tutorials/tutorial 3 - depth sensing/c/main.c b/tutorials/tutorial 3 - depth sensing/c/main.c index 8de8b0ca..ce3d2154 100644 --- a/tutorials/tutorial 3 - depth sensing/c/main.c +++ b/tutorials/tutorial 3 - depth sensing/c/main.c @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/tutorials/tutorial 3 - depth sensing/cpp/CMakeLists.txt b/tutorials/tutorial 3 - depth sensing/cpp/CMakeLists.txt index abab5f0b..90e70c93 100644 --- a/tutorials/tutorial 3 - depth sensing/cpp/CMakeLists.txt +++ b/tutorials/tutorial 3 - depth sensing/cpp/CMakeLists.txt @@ -1,25 +1,17 @@ -CMAKE_MINIMUM_REQUIRED(VERSION 2.4) +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) PROJECT(ZED_Tutorial_3) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED 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() -if(COMMAND cmake_policy) - cmake_policy(SET CMP0003 OLD) - cmake_policy(SET CMP0015 OLD) -endif(COMMAND cmake_policy) - -if (NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "") -SET(CMAKE_BUILD_TYPE "RelWithDebInfo") -endif() - -SET(EXECUTABLE_OUTPUT_PATH ".") - find_package(ZED 3 REQUIRED) -find_package(CUDA ${ZED_CUDA_VERSION} EXACT REQUIRED) +find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED) include_directories(${CUDA_INCLUDE_DIRS}) include_directories(${ZED_INCLUDE_DIRS}) @@ -28,7 +20,6 @@ link_directories(${ZED_LIBRARY_DIR}) link_directories(${CUDA_LIBRARY_DIRS}) ADD_EXECUTABLE(${PROJECT_NAME} main.cpp) -add_definitions(-std=c++14 -O3) if (LINK_SHARED_ZED) SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY}) @@ -36,4 +27,4 @@ else() SET(ZED_LIBS ${ZED_STATIC_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_LIBRARY}) endif() -TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${ZED_LIBS}) +TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${ZED_LIBS}) \ No newline at end of file diff --git a/tutorials/tutorial 3 - depth sensing/cpp/main.cpp b/tutorials/tutorial 3 - depth sensing/cpp/main.cpp index 4f0e59ec..89f8fe2e 100644 --- a/tutorials/tutorial 3 - depth sensing/cpp/main.cpp +++ b/tutorials/tutorial 3 - depth sensing/cpp/main.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/tutorials/tutorial 3 - depth sensing/csharp/CMakeLists.txt b/tutorials/tutorial 3 - depth sensing/csharp/CMakeLists.txt index cae5249e..e4aadfb0 100644 --- a/tutorials/tutorial 3 - depth sensing/csharp/CMakeLists.txt +++ b/tutorials/tutorial 3 - depth sensing/csharp/CMakeLists.txt @@ -28,5 +28,5 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES set(CMAKE_SUPPRESS_REGENERATION true) set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES - "Stereolabs.zed_3.6.0" + "Stereolabs.zed_3.7.*" ) \ No newline at end of file diff --git a/tutorials/tutorial 3 - depth sensing/python/depth_sensing.py b/tutorials/tutorial 3 - depth sensing/python/depth_sensing.py index 799bd86d..8399a758 100644 --- a/tutorials/tutorial 3 - depth sensing/python/depth_sensing.py +++ b/tutorials/tutorial 3 - depth sensing/python/depth_sensing.py @@ -1,6 +1,6 @@ ######################################################################## # -# Copyright (c) 2021, STEREOLABS. +# Copyright (c) 2022, STEREOLABS. # # All rights reserved. # diff --git a/tutorials/tutorial 4 - positional tracking/c/main.c b/tutorials/tutorial 4 - positional tracking/c/main.c index d0e11b75..2e880505 100644 --- a/tutorials/tutorial 4 - positional tracking/c/main.c +++ b/tutorials/tutorial 4 - positional tracking/c/main.c @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/tutorials/tutorial 4 - positional tracking/cpp/CMakeLists.txt b/tutorials/tutorial 4 - positional tracking/cpp/CMakeLists.txt index 74de80a8..fe278ce2 100644 --- a/tutorials/tutorial 4 - positional tracking/cpp/CMakeLists.txt +++ b/tutorials/tutorial 4 - positional tracking/cpp/CMakeLists.txt @@ -1,25 +1,17 @@ -CMAKE_MINIMUM_REQUIRED(VERSION 2.4) +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) PROJECT(ZED_Tutorial_4) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED 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() -if(COMMAND cmake_policy) - cmake_policy(SET CMP0003 OLD) - cmake_policy(SET CMP0015 OLD) -endif(COMMAND cmake_policy) - -if (NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "") -SET(CMAKE_BUILD_TYPE "RelWithDebInfo") -endif() - -SET(EXECUTABLE_OUTPUT_PATH ".") - find_package(ZED 3 REQUIRED) -find_package(CUDA ${ZED_CUDA_VERSION} EXACT REQUIRED) +find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED) include_directories(${CUDA_INCLUDE_DIRS}) include_directories(${ZED_INCLUDE_DIRS}) @@ -28,7 +20,6 @@ link_directories(${ZED_LIBRARY_DIR}) link_directories(${CUDA_LIBRARY_DIRS}) ADD_EXECUTABLE(${PROJECT_NAME} main.cpp) -add_definitions(-std=c++14 -O3) if (LINK_SHARED_ZED) SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY}) diff --git a/tutorials/tutorial 4 - positional tracking/cpp/main.cpp b/tutorials/tutorial 4 - positional tracking/cpp/main.cpp index 1918269f..c9b8d2f1 100644 --- a/tutorials/tutorial 4 - positional tracking/cpp/main.cpp +++ b/tutorials/tutorial 4 - positional tracking/cpp/main.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/tutorials/tutorial 4 - positional tracking/csharp/CMakeLists.txt b/tutorials/tutorial 4 - positional tracking/csharp/CMakeLists.txt index af942a0a..cc1b94e4 100644 --- a/tutorials/tutorial 4 - positional tracking/csharp/CMakeLists.txt +++ b/tutorials/tutorial 4 - positional tracking/csharp/CMakeLists.txt @@ -28,5 +28,5 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES set(CMAKE_SUPPRESS_REGENERATION true) set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES - "Stereolabs.zed_3.6.0" + "Stereolabs.zed_3.7.*" ) \ No newline at end of file diff --git a/tutorials/tutorial 4 - positional tracking/python/positional_tracking.py b/tutorials/tutorial 4 - positional tracking/python/positional_tracking.py index 0430b7a8..024a34d2 100644 --- a/tutorials/tutorial 4 - positional tracking/python/positional_tracking.py +++ b/tutorials/tutorial 4 - positional tracking/python/positional_tracking.py @@ -1,6 +1,6 @@ ######################################################################## # -# Copyright (c) 2021, STEREOLABS. +# Copyright (c) 2022, STEREOLABS. # # All rights reserved. # diff --git a/tutorials/tutorial 5 - spatial mapping/c/main.c b/tutorials/tutorial 5 - spatial mapping/c/main.c index c402b561..e4596eef 100644 --- a/tutorials/tutorial 5 - spatial mapping/c/main.c +++ b/tutorials/tutorial 5 - spatial mapping/c/main.c @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/tutorials/tutorial 5 - spatial mapping/cpp/CMakeLists.txt b/tutorials/tutorial 5 - spatial mapping/cpp/CMakeLists.txt index 0f302b4a..e088f71c 100644 --- a/tutorials/tutorial 5 - spatial mapping/cpp/CMakeLists.txt +++ b/tutorials/tutorial 5 - spatial mapping/cpp/CMakeLists.txt @@ -1,25 +1,17 @@ -CMAKE_MINIMUM_REQUIRED(VERSION 2.4) +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) PROJECT(ZED_Tutorial_5) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED 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() -if(COMMAND cmake_policy) - cmake_policy(SET CMP0003 OLD) - cmake_policy(SET CMP0015 OLD) -endif(COMMAND cmake_policy) - -if (NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "") -SET(CMAKE_BUILD_TYPE "RelWithDebInfo") -endif() - -SET(EXECUTABLE_OUTPUT_PATH ".") - find_package(ZED 3 REQUIRED) -find_package(CUDA ${ZED_CUDA_VERSION} EXACT REQUIRED) +find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED) include_directories(${CUDA_INCLUDE_DIRS}) include_directories(${ZED_INCLUDE_DIRS}) @@ -28,7 +20,6 @@ link_directories(${ZED_LIBRARY_DIR}) link_directories(${CUDA_LIBRARY_DIRS}) ADD_EXECUTABLE(${PROJECT_NAME} main.cpp) -add_definitions(-std=c++14 -O3) if (LINK_SHARED_ZED) SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY}) @@ -36,4 +27,4 @@ else() SET(ZED_LIBS ${ZED_STATIC_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_LIBRARY}) endif() -TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${ZED_LIBS}) +TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${ZED_LIBS}) \ No newline at end of file diff --git a/tutorials/tutorial 5 - spatial mapping/cpp/main.cpp b/tutorials/tutorial 5 - spatial mapping/cpp/main.cpp index bfc2b007..9954c3c7 100644 --- a/tutorials/tutorial 5 - spatial mapping/cpp/main.cpp +++ b/tutorials/tutorial 5 - spatial mapping/cpp/main.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/tutorials/tutorial 5 - spatial mapping/csharp/CMakeLists.txt b/tutorials/tutorial 5 - spatial mapping/csharp/CMakeLists.txt index 222c9e25..7c99992b 100644 --- a/tutorials/tutorial 5 - spatial mapping/csharp/CMakeLists.txt +++ b/tutorials/tutorial 5 - spatial mapping/csharp/CMakeLists.txt @@ -28,6 +28,6 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES set(CMAKE_SUPPRESS_REGENERATION true) set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES - "Stereolabs.zed_3.6.0" + "Stereolabs.zed_3.7.*" ) \ No newline at end of file diff --git a/tutorials/tutorial 5 - spatial mapping/python/spatial_mapping.py b/tutorials/tutorial 5 - spatial mapping/python/spatial_mapping.py index 199c7db4..28f4e772 100644 --- a/tutorials/tutorial 5 - spatial mapping/python/spatial_mapping.py +++ b/tutorials/tutorial 5 - spatial mapping/python/spatial_mapping.py @@ -1,6 +1,6 @@ ######################################################################## # -# Copyright (c) 2021, STEREOLABS. +# Copyright (c) 2022, STEREOLABS. # # All rights reserved. # @@ -40,7 +40,7 @@ def main(): # Enable positional tracking with default parameters. # Positional tracking needs to be enabled before using spatial mapping py_transform = sl.Transform() - tracking_parameters = sl.PositionalTrackingParameters(init_pos=py_transform) + tracking_parameters = sl.PositionalTrackingParameters(_init_pos=py_transform) err = zed.enable_positional_tracking(tracking_parameters) if err != sl.ERROR_CODE.SUCCESS: exit(1) diff --git a/tutorials/tutorial 6 - object detection/c/main.c b/tutorials/tutorial 6 - object detection/c/main.c index 55c34503..360592e8 100644 --- a/tutorials/tutorial 6 - object detection/c/main.c +++ b/tutorials/tutorial 6 - object detection/c/main.c @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/tutorials/tutorial 6 - object detection/cpp/CMakeLists.txt b/tutorials/tutorial 6 - object detection/cpp/CMakeLists.txt index 53dd1ae7..758e4c6d 100644 --- a/tutorials/tutorial 6 - object detection/cpp/CMakeLists.txt +++ b/tutorials/tutorial 6 - object detection/cpp/CMakeLists.txt @@ -1,25 +1,17 @@ -CMAKE_MINIMUM_REQUIRED(VERSION 2.4) +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) PROJECT(ZED_Tutorial_6) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED 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() -if(COMMAND cmake_policy) - cmake_policy(SET CMP0003 OLD) - cmake_policy(SET CMP0015 OLD) -endif(COMMAND cmake_policy) - -if (NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "") -SET(CMAKE_BUILD_TYPE "RelWithDebInfo") -endif() - -SET(EXECUTABLE_OUTPUT_PATH ".") - find_package(ZED 3 REQUIRED) -find_package(CUDA ${ZED_CUDA_VERSION} EXACT REQUIRED) +find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED) include_directories(${CUDA_INCLUDE_DIRS}) include_directories(${ZED_INCLUDE_DIRS}) @@ -28,7 +20,6 @@ link_directories(${ZED_LIBRARY_DIR}) link_directories(${CUDA_LIBRARY_DIRS}) ADD_EXECUTABLE(${PROJECT_NAME} main.cpp) -add_definitions(-std=c++14 -O3) if (LINK_SHARED_ZED) SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY}) @@ -36,4 +27,4 @@ else() SET(ZED_LIBS ${ZED_STATIC_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_LIBRARY}) endif() -TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${ZED_LIBS}) +TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${ZED_LIBS}) \ No newline at end of file diff --git a/tutorials/tutorial 6 - object detection/cpp/main.cpp b/tutorials/tutorial 6 - object detection/cpp/main.cpp index 2620342b..e8c04ead 100644 --- a/tutorials/tutorial 6 - object detection/cpp/main.cpp +++ b/tutorials/tutorial 6 - object detection/cpp/main.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // @@ -121,4 +121,4 @@ int main(int argc, char** argv) { } zed.close(); return EXIT_SUCCESS; -} \ No newline at end of file +} diff --git a/tutorials/tutorial 6 - object detection/csharp/CMakeLists.txt b/tutorials/tutorial 6 - object detection/csharp/CMakeLists.txt index 2a215f86..9e1f2b3f 100644 --- a/tutorials/tutorial 6 - object detection/csharp/CMakeLists.txt +++ b/tutorials/tutorial 6 - object detection/csharp/CMakeLists.txt @@ -29,5 +29,5 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES set(CMAKE_SUPPRESS_REGENERATION true) set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES - "Stereolabs.zed_3.6.0" + "Stereolabs.zed_3.7.*" ) \ No newline at end of file diff --git a/tutorials/tutorial 6 - object detection/csharp/Program.cs b/tutorials/tutorial 6 - object detection/csharp/Program.cs index f6289d5c..06af82f3 100644 --- a/tutorials/tutorial 6 - object detection/csharp/Program.cs +++ b/tutorials/tutorial 6 - object detection/csharp/Program.cs @@ -15,7 +15,6 @@ static void Main(string[] args) init_params.coordinateUnits = UNIT.METER; init_params.coordinateSystem = COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP; init_params.depthMode = DEPTH_MODE.PERFORMANCE; - init_params.sdkVerbose = true; Camera zedCamera = new Camera(0); // Open the camera diff --git a/tutorials/tutorial 6 - object detection/python/object_detection.py b/tutorials/tutorial 6 - object detection/python/object_detection.py index b7e6ee65..a6c03def 100644 --- a/tutorials/tutorial 6 - object detection/python/object_detection.py +++ b/tutorials/tutorial 6 - object detection/python/object_detection.py @@ -1,6 +1,6 @@ ######################################################################## # -# Copyright (c) 2021, STEREOLABS. +# Copyright (c) 2022, STEREOLABS. # # All rights reserved. # diff --git a/tutorials/tutorial 7 - sensor data/c/main.c b/tutorials/tutorial 7 - sensor data/c/main.c index f3c13c43..74934f26 100644 --- a/tutorials/tutorial 7 - sensor data/c/main.c +++ b/tutorials/tutorial 7 - sensor data/c/main.c @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/tutorials/tutorial 7 - sensor data/cpp/CMakeLists.txt b/tutorials/tutorial 7 - sensor data/cpp/CMakeLists.txt index dcebcffa..f4dfc830 100644 --- a/tutorials/tutorial 7 - sensor data/cpp/CMakeLists.txt +++ b/tutorials/tutorial 7 - sensor data/cpp/CMakeLists.txt @@ -1,25 +1,17 @@ -CMAKE_MINIMUM_REQUIRED(VERSION 2.4) +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) PROJECT(ZED_Tutorial_7) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED 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() -if(COMMAND cmake_policy) - cmake_policy(SET CMP0003 OLD) - cmake_policy(SET CMP0015 OLD) -endif(COMMAND cmake_policy) - -if (NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "") -SET(CMAKE_BUILD_TYPE "RelWithDebInfo") -endif() - -SET(EXECUTABLE_OUTPUT_PATH ".") - find_package(ZED 3 REQUIRED) -find_package(CUDA ${ZED_CUDA_VERSION} EXACT REQUIRED) +find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED) include_directories(${CUDA_INCLUDE_DIRS}) include_directories(${ZED_INCLUDE_DIRS}) @@ -28,7 +20,6 @@ link_directories(${ZED_LIBRARY_DIR}) link_directories(${CUDA_LIBRARY_DIRS}) ADD_EXECUTABLE(${PROJECT_NAME} main.cpp) -add_definitions(-std=c++14 -O3) if (LINK_SHARED_ZED) SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY}) diff --git a/tutorials/tutorial 7 - sensor data/cpp/main.cpp b/tutorials/tutorial 7 - sensor data/cpp/main.cpp index eb088358..e927369a 100644 --- a/tutorials/tutorial 7 - sensor data/cpp/main.cpp +++ b/tutorials/tutorial 7 - sensor data/cpp/main.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/tutorials/tutorial 7 - sensor data/csharp/CMakeLists.txt b/tutorials/tutorial 7 - sensor data/csharp/CMakeLists.txt index 2af5a4ef..f0a3b10c 100644 --- a/tutorials/tutorial 7 - sensor data/csharp/CMakeLists.txt +++ b/tutorials/tutorial 7 - sensor data/csharp/CMakeLists.txt @@ -28,5 +28,5 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES set(CMAKE_SUPPRESS_REGENERATION true) set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES - "Stereolabs.zed_3.6.0" + "Stereolabs.zed_3.7.*" ) \ No newline at end of file diff --git a/tutorials/tutorial 7 - sensor data/python/sensor_data.py b/tutorials/tutorial 7 - sensor data/python/sensor_data.py index 5e3548b7..5fb30421 100644 --- a/tutorials/tutorial 7 - sensor data/python/sensor_data.py +++ b/tutorials/tutorial 7 - sensor data/python/sensor_data.py @@ -1,6 +1,6 @@ ######################################################################## # -# Copyright (c) 2021, STEREOLABS. +# Copyright (c) 2022, STEREOLABS. # # All rights reserved. # diff --git a/tutorials/tutorial 8 - body tracking/c/main.c b/tutorials/tutorial 8 - body tracking/c/main.c index e8c777a0..51146532 100644 --- a/tutorials/tutorial 8 - body tracking/c/main.c +++ b/tutorials/tutorial 8 - body tracking/c/main.c @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/tutorials/tutorial 8 - body tracking/cpp/CMakeLists.txt b/tutorials/tutorial 8 - body tracking/cpp/CMakeLists.txt index 51fb1dda..fa5e1b12 100644 --- a/tutorials/tutorial 8 - body tracking/cpp/CMakeLists.txt +++ b/tutorials/tutorial 8 - body tracking/cpp/CMakeLists.txt @@ -1,25 +1,17 @@ -CMAKE_MINIMUM_REQUIRED(VERSION 2.4) +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) PROJECT(ZED_Tutorial_8) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED 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() -if(COMMAND cmake_policy) - cmake_policy(SET CMP0003 OLD) - cmake_policy(SET CMP0015 OLD) -endif(COMMAND cmake_policy) - -if (NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "") -SET(CMAKE_BUILD_TYPE "RelWithDebInfo") -endif() - -SET(EXECUTABLE_OUTPUT_PATH ".") - find_package(ZED 3 REQUIRED) -find_package(CUDA ${ZED_CUDA_VERSION} EXACT REQUIRED) +find_package(CUDA ${ZED_CUDA_VERSION} REQUIRED) include_directories(${CUDA_INCLUDE_DIRS}) include_directories(${ZED_INCLUDE_DIRS}) @@ -28,7 +20,6 @@ link_directories(${ZED_LIBRARY_DIR}) link_directories(${CUDA_LIBRARY_DIRS}) ADD_EXECUTABLE(${PROJECT_NAME} main.cpp) -add_definitions(-std=c++14 -O3) if (LINK_SHARED_ZED) SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY}) @@ -36,4 +27,4 @@ else() SET(ZED_LIBS ${ZED_STATIC_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_LIBRARY}) endif() -TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${ZED_LIBS}) +TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${ZED_LIBS}) \ No newline at end of file diff --git a/tutorials/tutorial 8 - body tracking/cpp/main.cpp b/tutorials/tutorial 8 - body tracking/cpp/main.cpp index 01375262..cacd575f 100644 --- a/tutorials/tutorial 8 - body tracking/cpp/main.cpp +++ b/tutorials/tutorial 8 - body tracking/cpp/main.cpp @@ -1,6 +1,6 @@ /////////////////////////////////////////////////////////////////////////// // -// Copyright (c) 2021, STEREOLABS. +// Copyright (c) 2022, STEREOLABS. // // All rights reserved. // diff --git a/tutorials/tutorial 8 - body tracking/csharp/CMakeLists.txt b/tutorials/tutorial 8 - body tracking/csharp/CMakeLists.txt index 32ff0841..1964fe28 100644 --- a/tutorials/tutorial 8 - body tracking/csharp/CMakeLists.txt +++ b/tutorials/tutorial 8 - body tracking/csharp/CMakeLists.txt @@ -30,5 +30,5 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES - "Stereolabs.zed_3.6.0" + "Stereolabs.zed_3.7.*" ) \ No newline at end of file diff --git a/tutorials/tutorial 8 - body tracking/csharp/Program.cs b/tutorials/tutorial 8 - body tracking/csharp/Program.cs index a830ef54..6b6141ae 100644 --- a/tutorials/tutorial 8 - body tracking/csharp/Program.cs +++ b/tutorials/tutorial 8 - body tracking/csharp/Program.cs @@ -15,7 +15,7 @@ static void Main(string[] args) InitParameters init_params = new InitParameters(); init_params.resolution = RESOLUTION.HD720; init_params.coordinateUnits = UNIT.METER; - init_params.sdkVerbose = true; + init_params.sdkVerbose = 1; Camera zedCamera = new Camera(0); // Open the camera @@ -99,12 +99,12 @@ static void Main(string[] args) for (int i = 0; i < (int)sl.BODY_PARTS.LAST; i++) { var kp = firstObject.keypoints2D[i]; - Console.WriteLine(" " + (sl.BODY_PART)i + " " + kp.X + ", " + kp.Y); + Console.WriteLine(" " + (sl.BODY_PARTS)i + " " + kp.X + ", " + kp.Y); } // The BODY_PARTS can be link as bones, using sl::BODY_BONES which gives the BODY_PARTS pair for each Console.WriteLine(" Keypoints 3D "); - for (int i = 0; i < (int)sl.BODY_PARTS_POSE_18.LAST; i++) + for (int i = 0; i < (int)sl.BODY_PARTS.LAST; i++) { var kp = firstObject.keypoints[i]; Console.WriteLine(" " + (sl.BODY_PARTS)i + " " + kp.X + ", " + kp.Y + ", " + kp.Z); diff --git a/tutorials/tutorial 8 - body tracking/python/body_tracking.py b/tutorials/tutorial 8 - body tracking/python/body_tracking.py index 558cd789..22dedb8c 100644 --- a/tutorials/tutorial 8 - body tracking/python/body_tracking.py +++ b/tutorials/tutorial 8 - body tracking/python/body_tracking.py @@ -1,6 +1,6 @@ ######################################################################## # -# Copyright (c) 2021, STEREOLABS. +# Copyright (c) 2022, STEREOLABS. # # All rights reserved. #