diff --git a/body tracking/README.md b/body tracking/README.md index 2a1e3361..ff1d6224 100644 --- a/body tracking/README.md +++ b/body tracking/README.md @@ -10,4 +10,4 @@ ![](body_tracking.jpg) ->*NOTE: A [ZED 2](https://store.stereolabs.com/products/zed-2) is required to use this module.* \ No newline at end of file +>*NOTE: The ZED v1 is not compatible with this module* diff --git a/body tracking/cpp/README.md b/body tracking/cpp/README.md index 80913b6b..bbee5bca 100644 --- a/body tracking/cpp/README.md +++ b/body tracking/cpp/README.md @@ -11,7 +11,7 @@ This sample shows how to detect and track human bodies in space. - Build for [Linux/Jetson](https://www.stereolabs.com/docs/app-development/cpp/linux/) ## Run the program -*NOTE: A [ZED 2](https://store.stereolabs.com/products/zed-2) is required to use this module.* +*NOTE: The ZED v1 is not compatible with this module* - Navigate to the build directory and launch the executable - Or open a terminal in the build directory and run the sample : diff --git a/body tracking/cpp/include/GLViewer.hpp b/body tracking/cpp/include/GLViewer.hpp index b0e30bc6..1d97144b 100644 --- a/body tracking/cpp/include/GLViewer.hpp +++ b/body tracking/cpp/include/GLViewer.hpp @@ -301,7 +301,7 @@ class GLViewer { GLViewer(); ~GLViewer(); bool isAvailable(); - void init(int argc, char **argv, sl::CameraParameters& param, bool isTrackingON); + void init(int argc, char **argv, sl::CameraParameters& param, bool isTrackingON, sl::BODY_FORMAT body_format); void updateData(sl::Mat &matXYZRGBA, std::vector &objs, sl::Transform& pose); void exit(); void setFloorPlaneEquation(sl::float4 eq); @@ -370,6 +370,7 @@ class GLViewer { bool showPC = false; bool isTrackingON_ = false; + sl::BODY_FORMAT body_format_ = sl::BODY_FORMAT::POSE_18; }; #endif /* __VIEWER_INCLUDE__ */ diff --git a/body tracking/cpp/include/TrackingViewer.hpp b/body tracking/cpp/include/TrackingViewer.hpp index bfa7921e..f2206c35 100644 --- a/body tracking/cpp/include/TrackingViewer.hpp +++ b/body tracking/cpp/include/TrackingViewer.hpp @@ -21,7 +21,7 @@ inline sl::float2 getImagePosition(std::vector &bounding_box_image, s return position; } -void render_2D(cv::Mat &left, sl::float2 img_scale, std::vector &objects, bool isTrackingON); +void render_2D(cv::Mat &left, sl::float2 img_scale, std::vector &objects, bool isTrackingON, sl::BODY_FORMAT body_format); inline cv::Mat slMat2cvMat(sl::Mat& input) { // Mapping between MAT_TYPE and CV_TYPE diff --git a/body tracking/cpp/src/GLViewer.cpp b/body tracking/cpp/src/GLViewer.cpp index 093b0920..bb2b8a01 100644 --- a/body tracking/cpp/src/GLViewer.cpp +++ b/body tracking/cpp/src/GLViewer.cpp @@ -125,7 +125,7 @@ bool GLViewer::isAvailable() { void CloseFunc(void) { if (currentInstance_) currentInstance_->exit(); } -void GLViewer::init(int argc, char **argv, sl::CameraParameters ¶m, bool isTrackingON) { +void GLViewer::init(int argc, char **argv, sl::CameraParameters ¶m, bool isTrackingON, sl::BODY_FORMAT body_format) { glutInit(&argc, argv); int wnd_w = glutGet(GLUT_SCREEN_WIDTH); @@ -169,6 +169,7 @@ void GLViewer::init(int argc, char **argv, sl::CameraParameters ¶m, bool isT floor_plane_set = false; isTrackingON_ = isTrackingON; + body_format_ = body_format; // Set background color (black) bckgrnd_clr = sl::float4(0.2f, 0.19f, 0.2f, 1.0f); @@ -268,32 +269,50 @@ void GLViewer::updateData(sl::Mat &matXYZRGBA, std::vector &objs // draw skeletons auto clr_id = generateColorID(objs[i].id); if (objs[i].keypoint.size()) { - for (auto& limb : SKELETON_BONES) { - sl::float3 kp_1 = objs[i].keypoint[getIdx(limb.first)]; - sl::float3 kp_2 = objs[i].keypoint[getIdx(limb.second)]; - float norm_1 = kp_1.norm(); - float norm_2 = kp_2.norm(); + if (body_format_ == sl::BODY_FORMAT::POSE_18) { + for (auto& limb : SKELETON_BONES) { + sl::float3 kp_1 = objs[i].keypoint[getIdx(limb.first)]; + sl::float3 kp_2 = objs[i].keypoint[getIdx(limb.second)]; + float norm_1 = kp_1.norm(); + float norm_2 = kp_2.norm(); + // draw cylinder between two keypoints + if (std::isfinite(norm_1) && std::isfinite(norm_2)) { + skeletons.addCylinder(kp_1, kp_2, clr_id); + } + } + // Create bone between spine and neck (not existing in sl::BODY_BONES) + sl::float3 spine = (objs[i].keypoint[getIdx(sl::BODY_PARTS::LEFT_HIP)] + objs[i].keypoint[getIdx(sl::BODY_PARTS::RIGHT_HIP)]) / 2; // Create new KP (spine for rendering) + sl::float3 neck = objs[i].keypoint[getIdx(sl::BODY_PARTS::NECK)]; + float norm_1 = spine.norm(); + float norm_2 = neck.norm(); // draw cylinder between two keypoints if (std::isfinite(norm_1) && std::isfinite(norm_2)) { - skeletons.addCylinder(kp_1, kp_2, clr_id); + skeletons.addCylinder(spine, neck, clr_id); } - } - // Create bone between spine and neck (not existing in sl::BODY_BONES) - sl::float3 spine = (objs[i].keypoint[getIdx(sl::BODY_PARTS::LEFT_HIP)] + objs[i].keypoint[getIdx(sl::BODY_PARTS::RIGHT_HIP)]) / 2; // Create new KP (spine for rendering) - sl::float3 neck = objs[i].keypoint[getIdx(sl::BODY_PARTS::NECK)]; - float norm_1 = spine.norm(); - float norm_2 = neck.norm(); - // draw cylinder between two keypoints - if (std::isfinite(norm_1) && std::isfinite(norm_2)) { - skeletons.addCylinder(spine, neck, clr_id); - } - for (int j = 0; j < static_cast(sl::BODY_PARTS::LAST); j++) { - sl::float3 kp = objs[i].keypoint[j]; - if (std::isfinite(kp.norm()))skeletons.addSphere(kp, clr_id); + for (int j = 0; j < static_cast(sl::BODY_PARTS::LAST); j++) { + sl::float3 kp = objs[i].keypoint[j]; + if (std::isfinite(kp.norm()))skeletons.addSphere(kp, clr_id); + } + // Add Sphere at the Spine position + if (std::isfinite(spine.norm()))skeletons.addSphere(spine, clr_id); + } + else if (body_format_ == sl::BODY_FORMAT::POSE_34) { + for (auto& limb : sl::BODY_BONES_POSE_34) { + sl::float3 kp_1 = objs[i].keypoint[getIdx(limb.first)]; + sl::float3 kp_2 = objs[i].keypoint[getIdx(limb.second)]; + float norm_1 = kp_1.norm(); + float norm_2 = kp_2.norm(); + // draw cylinder between two keypoints + if (std::isfinite(norm_1) && std::isfinite(norm_2)) { + skeletons.addCylinder(kp_1, kp_2, clr_id); + } + } + for (int j = 0; j < static_cast(sl::BODY_PARTS_POSE_34::LAST); j++) { + sl::float3 kp = objs[i].keypoint[j]; + if (std::isfinite(kp.norm()))skeletons.addSphere(kp, clr_id); + } } - // Add Sphere at the Spine position - if (std::isfinite(spine.norm()))skeletons.addSphere(spine, clr_id); } } } diff --git a/body tracking/cpp/src/TrackingViewer.cpp b/body tracking/cpp/src/TrackingViewer.cpp index 622cacf2..f9d9949e 100644 --- a/body tracking/cpp/src/TrackingViewer.cpp +++ b/body tracking/cpp/src/TrackingViewer.cpp @@ -9,7 +9,7 @@ inline cv::Point2f cvt(T pt, sl::float2 scale) { return cv::Point2f(pt.x * scale.x, pt.y * scale.y); } -void render_2D(cv::Mat &left_display, sl::float2 img_scale, std::vector &objects, bool isTrackingON) { +void render_2D(cv::Mat &left_display, sl::float2 img_scale, std::vector &objects, bool isTrackingON, sl::BODY_FORMAT body_format) { cv::Mat overlay = left_display.clone(); cv::Rect roi_render(0, 0, left_display.size().width, left_display.size().height); @@ -19,49 +19,75 @@ void render_2D(cv::Mat &left_display, sl::float2 img_scale, std::vector 0 && hip_left.y > 0 && hip_right.x > 0 && hip_right.y > 0 && neck.x > 0 && neck.y > 0) { + if (hip_left.x > 0 && hip_left.y > 0 && hip_right.x > 0 && hip_right.y > 0 && neck.x > 0 && neck.y > 0) { - auto kp_a = cvt(spine, img_scale); - auto kp_b = cvt(obj.keypoint_2d[getIdx(sl::BODY_PARTS::NECK)], img_scale); - if (roi_render.contains(kp_a) && roi_render.contains(kp_b)) - { - #if (defined(CV_VERSION_EPOCH) && CV_VERSION_EPOCH == 2) - cv::line(left_display, kp_a, kp_b, color, 1); - #else - cv::line(left_display, kp_a, kp_b, color, 1, cv::LINE_AA); - #endif + auto kp_a = cvt(spine, img_scale); + auto kp_b = cvt(obj.keypoint_2d[getIdx(sl::BODY_PARTS::NECK)], img_scale); + if (roi_render.contains(kp_a) && roi_render.contains(kp_b)) + { +#if (defined(CV_VERSION_EPOCH) && CV_VERSION_EPOCH == 2) + cv::line(left_display, kp_a, kp_b, color, 1); +#else + cv::line(left_display, kp_a, kp_b, color, 1, cv::LINE_AA); +#endif + } } - } - // skeleton joints - for (auto& kp : obj.keypoint_2d) { - cv::Point2f cv_kp = cvt(kp, img_scale); - if (roi_render.contains(cv_kp)) + // skeleton joints + for (auto& kp : obj.keypoint_2d) { + cv::Point2f cv_kp = cvt(kp, img_scale); + if (roi_render.contains(cv_kp)) + cv::circle(left_display, cv_kp, 3, color, -1); + } + cv::Point2f cv_kp = cvt(spine, img_scale); + if (hip_left.x > 0 && hip_left.y > 0 && hip_right.x > 0 && hip_right.y > 0) cv::circle(left_display, cv_kp, 3, color, -1); } - cv::Point2f cv_kp = cvt(spine, img_scale); - if (hip_left.x > 0 && hip_left.y > 0 && hip_right.x > 0 && hip_right.y > 0) - cv::circle(left_display, cv_kp, 3, color, -1); + else if (body_format == sl::BODY_FORMAT::POSE_34) { + // skeleton bones + for (const auto& parts : sl::BODY_BONES_POSE_34) { + auto kp_a = cvt(obj.keypoint_2d[getIdx(parts.first)], img_scale); + auto kp_b = cvt(obj.keypoint_2d[getIdx(parts.second)], img_scale); + if (roi_render.contains(kp_a) && roi_render.contains(kp_b)) + { + +#if (defined(CV_VERSION_EPOCH) && CV_VERSION_EPOCH == 2) + cv::line(left_display, kp_a, kp_b, color, 1); +#else + cv::line(left_display, kp_a, kp_b, color, 1, cv::LINE_AA); +#endif + } + } + + // skeleton joints + for (auto& kp : obj.keypoint_2d) { + cv::Point2f cv_kp = cvt(kp, img_scale); + if (roi_render.contains(cv_kp)) + cv::circle(left_display, cv_kp, 3, color, -1); + } + } } + } } // Here, overlay is as the left image, but with opaque masks on each detected objects diff --git a/body tracking/cpp/src/main.cpp b/body tracking/cpp/src/main.cpp index 6f749be1..2fc4e509 100644 --- a/body tracking/cpp/src/main.cpp +++ b/body tracking/cpp/src/main.cpp @@ -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; + parseArgs(argc, argv, init_parameters); // Open the camera @@ -66,7 +67,7 @@ int main(int argc, char **argv) { // Enable Positional tracking (mandatory for object detection) PositionalTrackingParameters positional_tracking_parameters; //If the camera is static, uncomment the following line to have better performances and boxes sticked to the ground. - //positional_tracking_parameters.set_as_static = true; + returned_state = zed.enablePositionalTracking(positional_tracking_parameters); if (returned_state != ERROR_CODE::SUCCESS) { print("enable Positional Tracking", returned_state, "\nExit program."); @@ -78,8 +79,8 @@ int main(int argc, char **argv) { ObjectDetectionParameters obj_det_params; obj_det_params.enable_tracking = true; // track people across images flow obj_det_params.enable_body_fitting = false; // smooth skeletons moves + obj_det_params.body_format = sl::BODY_FORMAT::POSE_34; obj_det_params.detection_model = isJetson ? DETECTION_MODEL::HUMAN_BODY_FAST : DETECTION_MODEL::HUMAN_BODY_ACCURATE; - returned_state = zed.enableObjectDetection(obj_det_params); if (returned_state != ERROR_CODE::SUCCESS) { print("enable Object Detection", returned_state, "\nExit program."); @@ -102,7 +103,7 @@ int main(int argc, char **argv) { Mat point_cloud(pc_resolution, MAT_TYPE::F32_C4, MEM::GPU); // Create OpenGL Viewer GLViewer viewer; - viewer.init(argc, argv, camera_parameters, obj_det_params.enable_tracking); + viewer.init(argc, argv, camera_parameters, obj_det_params.enable_tracking, obj_det_params.body_format); Pose cam_pose; cam_pose.pose_data.setIdentity(); @@ -151,7 +152,7 @@ int main(int argc, char **argv) { if (is_playback && zed.getSVOPosition() == zed.getSVONumberOfFrames()) { quit = true; } - render_2D(image_left_ocv, img_scale, bodies.object_list, obj_det_params.enable_tracking); + render_2D(image_left_ocv, img_scale, bodies.object_list, obj_det_params.enable_tracking, obj_det_params.body_format); cv::imshow(window_name, image_left_ocv); key = cv::waitKey(10); } diff --git a/body tracking/csharp/CMakeLists.txt b/body tracking/csharp/CMakeLists.txt index 40d24610..e7854d41 100644 --- a/body tracking/csharp/CMakeLists.txt +++ b/body tracking/csharp/CMakeLists.txt @@ -32,14 +32,14 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES "System.Windows" "System.Windows.Forms" "System.Numerics" - "System.Drawing" "WindowsBase" + ) +set(CMAKE_SUPPRESS_REGENERATION true) set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES + "Stereolabs.zed_3.6.0" "OpenGL.Net_0.8.4" "OpenGL.Net.CoreUI_0.8.4" "OpenCvSharp4.Windows_4.5.0.20201013" - "Stereolabs.zed_3.5.0" - "sl_zed_interface_3.5.0" ) \ No newline at end of file diff --git a/body tracking/csharp/GLViewer.cs b/body tracking/csharp/GLViewer.cs index 3884ffab..f5077f3a 100644 --- a/body tracking/csharp/GLViewer.cs +++ b/body tracking/csharp/GLViewer.cs @@ -36,7 +36,7 @@ public bool isAvailable() } - public void init(CameraParameters param, bool showOnlyOk) + public void init(CameraParameters param, bool showOnlyOk, sl.BODY_FORMAT body_format) { Gl.Enable(EnableCap.FramebufferSrgb); @@ -52,6 +52,7 @@ public void init(CameraParameters param, bool showOnlyOk) camera_ = new CameraGL(new Vector3(0, 0, 0), new Vector3(0, 0, -1f), Vector3.UnitY); showOnlyOK_ = showOnlyOk; + body_format_ = body_format; pointCloud.initialize(param.resolution); @@ -111,45 +112,76 @@ public void update(Mat pointCloud_, Objects objects, sl.Pose pose) if (keypoints.Length > 0) { - foreach (var limb in BODY_BONES) + if (body_format_ == BODY_FORMAT.POSE_18) { - Vector3 kp_1 = keypoints[getIdx(limb.Item1)]; - Vector3 kp_2 = keypoints[getIdx(limb.Item2)]; + foreach (var limb in SKELETON_BONES) + { + Vector3 kp_1 = keypoints[getIdx(limb.Item1)]; + Vector3 kp_2 = keypoints[getIdx(limb.Item2)]; + + float norm_1 = kp_1.Length(); + float norm_2 = kp_2.Length(); + + if (!float.IsNaN(norm_1) && norm_1 > 0 && !float.IsNaN(norm_2) && norm_2 > 0) + { + skeleton.addCylinder(new float3(kp_1.X, kp_1.Y, kp_1.Z), new float3(kp_2.X, kp_2.Y, kp_2.Z), clr_id); + } + } - float norm_1 = kp_1.Length(); - float norm_2 = kp_2.Length(); + Vector3 spine = (obj.keypoints[getIdx(sl.BODY_PARTS.LEFT_HIP)] + obj.keypoints[getIdx(sl.BODY_PARTS.RIGHT_HIP)]) / 2; + Vector3 neck = obj.keypoints[getIdx(sl.BODY_PARTS.NECK)]; + float norm_spine = spine.Length(); + float norm_neck = neck.Length(); - if (!float.IsNaN(norm_1) && norm_1 > 0 && !float.IsNaN(norm_2) && norm_2 > 0) + if (!float.IsNaN(norm_spine) && norm_spine > 0 && !float.IsNaN(norm_neck) && norm_neck > 0) { - skeleton.addCylinder(new float3(kp_1.X, kp_1.Y, kp_1.Z), new float3(kp_2.X, kp_2.Y, kp_2.Z), clr_id); + skeleton.addCylinder(new float3(spine.X, spine.Y, spine.Z), new float3(neck.X, neck.Y, neck.Z), clr_id); } - } - Vector3 spine = (obj.keypoints[getIdx(sl.BODY_PARTS.LEFT_HIP)] + obj.keypoints[getIdx(sl.BODY_PARTS.RIGHT_HIP)]) / 2; - Vector3 neck = obj.keypoints[getIdx(sl.BODY_PARTS.NECK)]; - float norm_spine = spine.Length(); - float norm_neck = neck.Length(); + for (int i = 0; i < (int)BODY_PARTS.LAST; i++) + { + Vector3 kp = keypoints[i]; + float norm = kp.Length(); + if (!float.IsNaN(norm) && norm > 0) + { + skeleton.addSphere(sphere, new float3(kp.X, kp.Y, kp.Z), clr_id); + } + } - if (!float.IsNaN(norm_spine) && norm_spine > 0 && !float.IsNaN(norm_neck) && norm_neck > 0) - { - skeleton.addCylinder(new float3(spine.X, spine.Y, spine.Z), new float3(neck.X, neck.Y, neck.Z), clr_id); + if (!float.IsNaN(norm_spine) && norm_spine > 0) + { + skeleton.addSphere(sphere, new float3(spine.X, spine.Y, spine.Z), clr_id); + } } - - for (int i = 0; i < (int)BODY_PARTS.LAST; i++) + else if (body_format_ == BODY_FORMAT.POSE_34) { - Vector3 kp = keypoints[i]; - float norm = kp.Length(); - if (!float.IsNaN(norm) && norm > 0) + foreach (var limb in BODY_BONES_POSE_34) { - skeleton.addSphere(sphere, new float3(kp.X, kp.Y, kp.Z), clr_id); + Vector3 kp_1 = keypoints[getIdx(limb.Item1)]; + Vector3 kp_2 = keypoints[getIdx(limb.Item2)]; + + float norm_1 = kp_1.Length(); + float norm_2 = kp_2.Length(); + + if (!float.IsNaN(norm_1) && norm_1 > 0 && !float.IsNaN(norm_2) && norm_2 > 0) + { + skeleton.addCylinder(new float3(kp_1.X, kp_1.Y, kp_1.Z), new float3(kp_2.X, kp_2.Y, kp_2.Z), clr_id); + } } - } - if (!float.IsNaN(norm_spine) && norm_spine > 0) - { - skeleton.addSphere(sphere, new float3(spine.X, spine.Y, spine.Z), clr_id); + for (int i = 0; i < keypoints.Length; i++) + { + Vector3 kp = keypoints[i]; + float norm = kp.Length(); + + if (!float.IsNaN(norm) && norm > 0) + { + skeleton.addSphere(sphere, new float3(kp.X, kp.Y, kp.Z), clr_id); + } + } } } + } } } @@ -363,25 +395,69 @@ int getIdx(BODY_PARTS part) return (int)(part); } - public Tuple[] BODY_BONES = - { - Tuple.Create(BODY_PARTS.NOSE, BODY_PARTS.NECK), - Tuple.Create(BODY_PARTS.NECK, BODY_PARTS.RIGHT_SHOULDER), - Tuple.Create(BODY_PARTS.RIGHT_SHOULDER, BODY_PARTS.RIGHT_ELBOW), - Tuple.Create(BODY_PARTS.RIGHT_ELBOW, BODY_PARTS.RIGHT_WRIST), - Tuple.Create(BODY_PARTS.NECK, BODY_PARTS.LEFT_SHOULDER), - Tuple.Create(BODY_PARTS.LEFT_SHOULDER, BODY_PARTS.LEFT_ELBOW), - Tuple.Create(BODY_PARTS.LEFT_ELBOW, BODY_PARTS.LEFT_WRIST), - Tuple.Create(BODY_PARTS.RIGHT_HIP, BODY_PARTS.RIGHT_KNEE), - Tuple.Create(BODY_PARTS.RIGHT_KNEE, BODY_PARTS.RIGHT_ANKLE), - Tuple.Create(BODY_PARTS.LEFT_HIP, BODY_PARTS.LEFT_KNEE), - Tuple.Create(BODY_PARTS.LEFT_KNEE, BODY_PARTS.LEFT_ANKLE), - Tuple.Create(BODY_PARTS.RIGHT_SHOULDER, BODY_PARTS.LEFT_SHOULDER), - Tuple.Create(BODY_PARTS.RIGHT_HIP, BODY_PARTS.LEFT_HIP), - Tuple.Create(BODY_PARTS.NOSE, BODY_PARTS.RIGHT_EYE), - Tuple.Create(BODY_PARTS.RIGHT_EYE, BODY_PARTS.RIGHT_EAR), - Tuple.Create(BODY_PARTS.NOSE, BODY_PARTS.LEFT_EYE), - Tuple.Create(BODY_PARTS.LEFT_EYE, BODY_PARTS.LEFT_EAR) + int getIdx(BODY_PARTS_POSE_34 part) + { + return (int)(part); + } + + private static readonly Tuple[] SKELETON_BONES = + { + Tuple.Create(BODY_PARTS.NOSE, BODY_PARTS.NECK), + Tuple.Create(BODY_PARTS.NECK, BODY_PARTS.RIGHT_SHOULDER), + Tuple.Create(BODY_PARTS.RIGHT_SHOULDER, BODY_PARTS.RIGHT_ELBOW), + Tuple.Create(BODY_PARTS.RIGHT_ELBOW, BODY_PARTS.RIGHT_WRIST), + Tuple.Create(BODY_PARTS.NECK, BODY_PARTS.LEFT_SHOULDER), + Tuple.Create(BODY_PARTS.LEFT_SHOULDER, BODY_PARTS.LEFT_ELBOW), + Tuple.Create(BODY_PARTS.LEFT_ELBOW, BODY_PARTS.LEFT_WRIST), + Tuple.Create(BODY_PARTS.RIGHT_HIP, BODY_PARTS.RIGHT_KNEE), + Tuple.Create(BODY_PARTS.RIGHT_KNEE, BODY_PARTS.RIGHT_ANKLE), + Tuple.Create(BODY_PARTS.LEFT_HIP, BODY_PARTS.LEFT_KNEE), + Tuple.Create(BODY_PARTS.LEFT_KNEE, BODY_PARTS.LEFT_ANKLE), + Tuple.Create(BODY_PARTS.RIGHT_SHOULDER, BODY_PARTS.LEFT_SHOULDER), + Tuple.Create(BODY_PARTS.RIGHT_HIP, BODY_PARTS.LEFT_HIP), + Tuple.Create(BODY_PARTS.NOSE, BODY_PARTS.RIGHT_EYE), + Tuple.Create(BODY_PARTS.RIGHT_EYE, BODY_PARTS.RIGHT_EAR), + Tuple.Create(BODY_PARTS.NOSE, BODY_PARTS.LEFT_EYE), + Tuple.Create(BODY_PARTS.LEFT_EYE, BODY_PARTS.LEFT_EAR) + }; + + private static readonly Tuple[] BODY_BONES_POSE_34 = + { + Tuple.Create(BODY_PARTS_POSE_34.PELVIS, BODY_PARTS_POSE_34.NAVAL_SPINE), + Tuple.Create(BODY_PARTS_POSE_34.NAVAL_SPINE, BODY_PARTS_POSE_34.CHEST_SPINE), + Tuple.Create(BODY_PARTS_POSE_34.CHEST_SPINE, BODY_PARTS_POSE_34.LEFT_CLAVICLE), + Tuple.Create(BODY_PARTS_POSE_34.LEFT_CLAVICLE, BODY_PARTS_POSE_34.LEFT_SHOULDER), + Tuple.Create(BODY_PARTS_POSE_34.LEFT_SHOULDER, BODY_PARTS_POSE_34.LEFT_ELBOW), + Tuple.Create(BODY_PARTS_POSE_34.LEFT_ELBOW, BODY_PARTS_POSE_34.LEFT_WRIST), + Tuple.Create(BODY_PARTS_POSE_34.LEFT_WRIST, BODY_PARTS_POSE_34.LEFT_HAND), + Tuple.Create(BODY_PARTS_POSE_34.LEFT_HAND, BODY_PARTS_POSE_34.LEFT_HANDTIP), + Tuple.Create(BODY_PARTS_POSE_34.LEFT_WRIST, BODY_PARTS_POSE_34.LEFT_THUMB), + Tuple.Create(BODY_PARTS_POSE_34.CHEST_SPINE, BODY_PARTS_POSE_34.RIGHT_CLAVICLE), + Tuple.Create(BODY_PARTS_POSE_34.RIGHT_CLAVICLE, BODY_PARTS_POSE_34.RIGHT_SHOULDER), + Tuple.Create(BODY_PARTS_POSE_34.RIGHT_SHOULDER, BODY_PARTS_POSE_34.RIGHT_ELBOW), + Tuple.Create(BODY_PARTS_POSE_34.RIGHT_ELBOW, BODY_PARTS_POSE_34.RIGHT_WRIST), + Tuple.Create(BODY_PARTS_POSE_34.RIGHT_WRIST, BODY_PARTS_POSE_34.RIGHT_HAND), + Tuple.Create(BODY_PARTS_POSE_34.RIGHT_HAND, BODY_PARTS_POSE_34.RIGHT_HANDTIP), + Tuple.Create(BODY_PARTS_POSE_34.RIGHT_WRIST, BODY_PARTS_POSE_34.RIGHT_THUMB), + Tuple.Create(BODY_PARTS_POSE_34.PELVIS, BODY_PARTS_POSE_34.LEFT_HIP), + Tuple.Create(BODY_PARTS_POSE_34.LEFT_HIP, BODY_PARTS_POSE_34.LEFT_KNEE), + Tuple.Create(BODY_PARTS_POSE_34.LEFT_KNEE, BODY_PARTS_POSE_34.LEFT_ANKLE), + Tuple.Create(BODY_PARTS_POSE_34.LEFT_ANKLE, BODY_PARTS_POSE_34.LEFT_FOOT), + Tuple.Create(BODY_PARTS_POSE_34.PELVIS, BODY_PARTS_POSE_34.RIGHT_HIP), + Tuple.Create(BODY_PARTS_POSE_34.RIGHT_HIP, BODY_PARTS_POSE_34.RIGHT_KNEE), + Tuple.Create(BODY_PARTS_POSE_34.RIGHT_KNEE, BODY_PARTS_POSE_34.RIGHT_ANKLE), + Tuple.Create(BODY_PARTS_POSE_34.RIGHT_ANKLE, BODY_PARTS_POSE_34.RIGHT_FOOT), + Tuple.Create(BODY_PARTS_POSE_34.CHEST_SPINE, BODY_PARTS_POSE_34.NECK), + Tuple.Create(BODY_PARTS_POSE_34.NECK, BODY_PARTS_POSE_34.HEAD), + Tuple.Create(BODY_PARTS_POSE_34.HEAD, BODY_PARTS_POSE_34.NOSE), + Tuple.Create(BODY_PARTS_POSE_34.NOSE, BODY_PARTS_POSE_34.LEFT_EYE), + Tuple.Create(BODY_PARTS_POSE_34.LEFT_EYE, BODY_PARTS_POSE_34.LEFT_EAR), + Tuple.Create(BODY_PARTS_POSE_34.NOSE, BODY_PARTS_POSE_34.RIGHT_EYE), + Tuple.Create(BODY_PARTS_POSE_34.RIGHT_EYE, BODY_PARTS_POSE_34.RIGHT_EAR), + Tuple.Create(BODY_PARTS_POSE_34.LEFT_ANKLE, BODY_PARTS_POSE_34.LEFT_HEEL), + Tuple.Create(BODY_PARTS_POSE_34.RIGHT_ANKLE, BODY_PARTS_POSE_34.RIGHT_HEEL), + Tuple.Create(BODY_PARTS_POSE_34.LEFT_HEEL, BODY_PARTS_POSE_34.LEFT_FOOT), + Tuple.Create(BODY_PARTS_POSE_34.RIGHT_HEEL, BODY_PARTS_POSE_34.RIGHT_FOOT) }; public void exit() @@ -404,6 +480,7 @@ public void exit() bool available; bool showOnlyOK_ = false; + sl.BODY_FORMAT body_format_; Matrix4x4 projection_; Matrix4x4 cam_pose; diff --git a/body tracking/csharp/MainWindow.cs b/body tracking/csharp/MainWindow.cs index c0380eb2..2232d905 100644 --- a/body tracking/csharp/MainWindow.cs +++ b/body tracking/csharp/MainWindow.cs @@ -25,11 +25,8 @@ using System; using System.Collections.Generic; -using System.Linq; -using System.Text; using System.Numerics; using System.Net; -using System.Threading.Tasks; using System.Windows.Forms; using OpenGL; using OpenGL.CoreUI; @@ -43,6 +40,7 @@ class MainWindow Resolution displayRes; GLViewer viewer; Camera zedCamera; + ObjectDetectionParameters obj_det_params; ObjectDetectionRuntimeParameters obj_runtime_parameters; RuntimeParameters runtimeParameters; sl.Mat pointCloud; @@ -72,11 +70,15 @@ public MainWindow(string[] args) ERROR_CODE err = zedCamera.Open(ref init_params); if (err != ERROR_CODE.SUCCESS) + { + Console.WriteLine("ERROR while opening the camera. Exiting..."); Environment.Exit(-1); + } - if (!(zedCamera.CameraModel == sl.MODEL.ZED2 || zedCamera.CameraModel == sl.MODEL.ZED2i)) + + if (zedCamera.CameraModel == MODEL.ZED) { - Console.WriteLine(" ERROR : Use ZED2/ZED2i Camera only"); + Console.WriteLine(" ERROR : not compatible camera model"); return; } @@ -87,13 +89,14 @@ public MainWindow(string[] args) runtimeParameters = new RuntimeParameters(); // Enable the Objects detection module - ObjectDetectionParameters obj_det_params = new ObjectDetectionParameters(); + obj_det_params = new ObjectDetectionParameters(); obj_det_params.enableObjectTracking = true; // the object detection will track objects across multiple images, instead of an image-by-image basis isTrackingON = obj_det_params.enableObjectTracking; obj_det_params.enable2DMask = false; obj_det_params.enableBodyFitting = true; // smooth skeletons moves obj_det_params.imageSync = true; // the object detection is synchronized to the image obj_det_params.detectionModel = sl.DETECTION_MODEL.HUMAN_BODY_ACCURATE; + obj_det_params.bodyFormat = sl.BODY_FORMAT.POSE_34; zedCamera.EnableObjectDetection(ref obj_det_params); @@ -170,7 +173,14 @@ public void CreateWindow() nativeWindow.Create((int)(zedCamera.ImageWidth * 0.05f), (int)(zedCamera.ImageHeight * 0.05f), 1200, 700, NativeWindowStyle.Resizeable); nativeWindow.Show(); - nativeWindow.Run(); + try + { + nativeWindow.Run(); + } + catch(Exception e) + { + Console.WriteLine("Mouse wheel is broken in the current OPENGL .NET VERSION. Please do not use it."); + } } } @@ -201,7 +211,7 @@ private void NativeWindow_ContextCreated(object sender, NativeWindowEventArgs e) Gl.Enable(EnableCap.LineSmooth); Gl.Hint(HintTarget.LineSmoothHint, HintMode.Nicest); - viewer.init(zedCamera.GetCalibrationParameters().leftCam, isTrackingON); + viewer.init(zedCamera.GetCalibrationParameters().leftCam, isTrackingON, obj_det_params.bodyFormat); } // Render loop @@ -224,7 +234,7 @@ private void NativeWindow_Render(object sender, NativeWindowEventArgs e) // Retrieve Objects zedCamera.RetrieveObjects(ref objects, ref obj_runtime_parameters); - TrackingViewer.render_2D(ref imageLeftOcv, imgScale, ref objects, isTrackingON); + TrackingViewer.render_2D(ref imageLeftOcv, imgScale, ref objects, isTrackingON, obj_det_params.bodyFormat); //Update GL View viewer.update(pointCloud, objects, camPose); diff --git a/body tracking/csharp/README.md b/body tracking/csharp/README.md index a7fb2a48..c834b754 100644 --- a/body tracking/csharp/README.md +++ b/body tracking/csharp/README.md @@ -10,7 +10,7 @@ This sample shows how to detect and track human bodies in space. - Build for [Windows](https://www.stereolabs.com/docs/app-development/cpp/windows/) ## Run the program -*NOTE: A [ZED 2](https://store.stereolabs.com/products/zed-2) is required to use this module.* +*NOTE: The ZED v1 is not compatible with this module* - Navigate to the build directory and launch the executable - Or open a terminal in the build directory and run the sample : diff --git a/body tracking/csharp/TrackingViewer.cs b/body tracking/csharp/TrackingViewer.cs index 62da45dc..b41cc226 100644 --- a/body tracking/csharp/TrackingViewer.cs +++ b/body tracking/csharp/TrackingViewer.cs @@ -12,7 +12,7 @@ public class TrackingViewer { private static readonly Tuple[] SKELETON_BONES = - { + { Tuple.Create(BODY_PARTS.NOSE, BODY_PARTS.NECK), Tuple.Create(BODY_PARTS.NECK, BODY_PARTS.RIGHT_SHOULDER), Tuple.Create(BODY_PARTS.RIGHT_SHOULDER, BODY_PARTS.RIGHT_ELBOW), @@ -32,6 +32,45 @@ public class TrackingViewer Tuple.Create(BODY_PARTS.LEFT_EYE, BODY_PARTS.LEFT_EAR) }; + private static readonly Tuple[] BODY_BONES_POSE_34 = +{ + Tuple.Create(BODY_PARTS_POSE_34.PELVIS, BODY_PARTS_POSE_34.NAVAL_SPINE), + Tuple.Create(BODY_PARTS_POSE_34.NAVAL_SPINE, BODY_PARTS_POSE_34.CHEST_SPINE), + Tuple.Create(BODY_PARTS_POSE_34.CHEST_SPINE, BODY_PARTS_POSE_34.LEFT_CLAVICLE), + Tuple.Create(BODY_PARTS_POSE_34.LEFT_CLAVICLE, BODY_PARTS_POSE_34.LEFT_SHOULDER), + Tuple.Create(BODY_PARTS_POSE_34.LEFT_SHOULDER, BODY_PARTS_POSE_34.LEFT_ELBOW), + Tuple.Create(BODY_PARTS_POSE_34.LEFT_ELBOW, BODY_PARTS_POSE_34.LEFT_WRIST), + Tuple.Create(BODY_PARTS_POSE_34.LEFT_WRIST, BODY_PARTS_POSE_34.LEFT_HAND), + Tuple.Create(BODY_PARTS_POSE_34.LEFT_HAND, BODY_PARTS_POSE_34.LEFT_HANDTIP), + Tuple.Create(BODY_PARTS_POSE_34.LEFT_WRIST, BODY_PARTS_POSE_34.LEFT_THUMB), + Tuple.Create(BODY_PARTS_POSE_34.CHEST_SPINE, BODY_PARTS_POSE_34.RIGHT_CLAVICLE), + Tuple.Create(BODY_PARTS_POSE_34.RIGHT_CLAVICLE, BODY_PARTS_POSE_34.RIGHT_SHOULDER), + Tuple.Create(BODY_PARTS_POSE_34.RIGHT_SHOULDER, BODY_PARTS_POSE_34.RIGHT_ELBOW), + Tuple.Create(BODY_PARTS_POSE_34.RIGHT_ELBOW, BODY_PARTS_POSE_34.RIGHT_WRIST), + Tuple.Create(BODY_PARTS_POSE_34.RIGHT_WRIST, BODY_PARTS_POSE_34.RIGHT_HAND), + Tuple.Create(BODY_PARTS_POSE_34.RIGHT_HAND, BODY_PARTS_POSE_34.RIGHT_HANDTIP), + Tuple.Create(BODY_PARTS_POSE_34.RIGHT_WRIST, BODY_PARTS_POSE_34.RIGHT_THUMB), + Tuple.Create(BODY_PARTS_POSE_34.PELVIS, BODY_PARTS_POSE_34.LEFT_HIP), + Tuple.Create(BODY_PARTS_POSE_34.LEFT_HIP, BODY_PARTS_POSE_34.LEFT_KNEE), + Tuple.Create(BODY_PARTS_POSE_34.LEFT_KNEE, BODY_PARTS_POSE_34.LEFT_ANKLE), + Tuple.Create(BODY_PARTS_POSE_34.LEFT_ANKLE, BODY_PARTS_POSE_34.LEFT_FOOT), + Tuple.Create(BODY_PARTS_POSE_34.PELVIS, BODY_PARTS_POSE_34.RIGHT_HIP), + Tuple.Create(BODY_PARTS_POSE_34.RIGHT_HIP, BODY_PARTS_POSE_34.RIGHT_KNEE), + Tuple.Create(BODY_PARTS_POSE_34.RIGHT_KNEE, BODY_PARTS_POSE_34.RIGHT_ANKLE), + Tuple.Create(BODY_PARTS_POSE_34.RIGHT_ANKLE, BODY_PARTS_POSE_34.RIGHT_FOOT), + Tuple.Create(BODY_PARTS_POSE_34.CHEST_SPINE, BODY_PARTS_POSE_34.NECK), + Tuple.Create(BODY_PARTS_POSE_34.NECK, BODY_PARTS_POSE_34.HEAD), + Tuple.Create(BODY_PARTS_POSE_34.HEAD, BODY_PARTS_POSE_34.NOSE), + Tuple.Create(BODY_PARTS_POSE_34.NOSE, BODY_PARTS_POSE_34.LEFT_EYE), + Tuple.Create(BODY_PARTS_POSE_34.LEFT_EYE, BODY_PARTS_POSE_34.LEFT_EAR), + Tuple.Create(BODY_PARTS_POSE_34.NOSE, BODY_PARTS_POSE_34.RIGHT_EYE), + Tuple.Create(BODY_PARTS_POSE_34.RIGHT_EYE, BODY_PARTS_POSE_34.RIGHT_EAR), + Tuple.Create(BODY_PARTS_POSE_34.LEFT_ANKLE, BODY_PARTS_POSE_34.LEFT_HEEL), + Tuple.Create(BODY_PARTS_POSE_34.RIGHT_ANKLE, BODY_PARTS_POSE_34.RIGHT_HEEL), + Tuple.Create(BODY_PARTS_POSE_34.LEFT_HEEL, BODY_PARTS_POSE_34.LEFT_FOOT), + Tuple.Create(BODY_PARTS_POSE_34.RIGHT_HEEL, BODY_PARTS_POSE_34.RIGHT_FOOT) + }; + static float[,] id_colors = new float[8, 3]{ { 232.0f, 176.0f ,59.0f }, { 165.0f, 218.0f ,25.0f }, @@ -79,7 +118,7 @@ static sl.float2 getImagePosition(Vector2[] bounding_box_image, sl.float2 img_sc return position; } - public static void render_2D(ref OpenCvSharp.Mat left_display, sl.float2 img_scale, ref sl.Objects objects, bool showOnlyOK) + public static void render_2D(ref OpenCvSharp.Mat left_display, sl.float2 img_scale, ref sl.Objects objects, bool showOnlyOK, sl.BODY_FORMAT body_format) { OpenCvSharp.Mat overlay = left_display.Clone(); OpenCvSharp.Rect roi_render = new OpenCvSharp.Rect(1, 1, left_display.Size().Width, left_display.Size().Height); @@ -91,47 +130,72 @@ public static void render_2D(ref OpenCvSharp.Mat left_display, sl.float2 img_sca { // Draw Skeleton bones OpenCvSharp.Scalar base_color = generateColorID(obj.id); - foreach (var part in SKELETON_BONES) + if (body_format == BODY_FORMAT.POSE_18) { - var kp_a = cvt(obj.keypoints2D[(int)part.Item1], img_scale); - var kp_b = cvt(obj.keypoints2D[(int)part.Item2], img_scale); - if (roi_render.Contains(kp_a) && roi_render.Contains(kp_b)) + foreach (var part in SKELETON_BONES) { - Cv2.Line(left_display, kp_a, kp_b, base_color, 1, LineTypes.AntiAlias); + var kp_a = cvt(obj.keypoints2D[(int)part.Item1], img_scale); + var kp_b = cvt(obj.keypoints2D[(int)part.Item2], img_scale); + if (roi_render.Contains(kp_a) && roi_render.Contains(kp_b)) + { + Cv2.Line(left_display, kp_a, kp_b, base_color, 1, LineTypes.AntiAlias); + } } - } - var hip_left = obj.keypoints2D[(int)sl.BODY_PARTS.LEFT_HIP]; - var hip_right = obj.keypoints2D[(int)sl.BODY_PARTS.RIGHT_HIP]; - var spine = (hip_left + hip_right) / 2; - var neck = obj.keypoints2D[(int)sl.BODY_PARTS.NECK]; + var hip_left = obj.keypoints2D[(int)sl.BODY_PARTS.LEFT_HIP]; + var hip_right = obj.keypoints2D[(int)sl.BODY_PARTS.RIGHT_HIP]; + var spine = (hip_left + hip_right) / 2; + var neck = obj.keypoints2D[(int)sl.BODY_PARTS.NECK]; - if (hip_left.X > 0 && hip_left.Y > 0 && hip_right.X > 0 && hip_right.Y > 0 && neck.X > 0 && neck.Y > 0) - { - var spine_a = cvt(spine, img_scale); - var spine_b = cvt(neck, img_scale); - if (roi_render.Contains(spine_a) && roi_render.Contains(spine_b)) + if (hip_left.X > 0 && hip_left.Y > 0 && hip_right.X > 0 && hip_right.Y > 0 && neck.X > 0 && neck.Y > 0) { - Cv2.Line(left_display, spine_a, spine_b, base_color, 1, LineTypes.AntiAlias); + var spine_a = cvt(spine, img_scale); + var spine_b = cvt(neck, img_scale); + if (roi_render.Contains(spine_a) && roi_render.Contains(spine_b)) + { + Cv2.Line(left_display, spine_a, spine_b, base_color, 1, LineTypes.AntiAlias); + } } - } - // Draw Skeleton joints - foreach (var kp in obj.keypoints2D) - { - Point cv_kp = cvt(kp, img_scale); - if (roi_render.Contains(cv_kp)) + // Draw Skeleton joints + foreach (var kp in obj.keypoints2D) { - Cv2.Circle(left_display, cv_kp, 3, base_color, -1); + Point cv_kp = cvt(kp, img_scale); + if (roi_render.Contains(cv_kp)) + { + Cv2.Circle(left_display, cv_kp, 3, base_color, -1); + } + } + + if (hip_left.X > 0 && hip_left.Y > 0 && hip_right.X > 0 && hip_right.Y > 0) + { + Point cv_spine = cvt(spine, img_scale); + if (roi_render.Contains(cv_spine)) + { + Cv2.Circle(left_display, cv_spine, 3, base_color, -1); + } } } + else if (body_format == BODY_FORMAT.POSE_34){ - if (hip_left.X > 0 && hip_left.Y > 0 && hip_right.X > 0 && hip_right.Y > 0) - { - Point cv_spine = cvt(spine, img_scale); - if (roi_render.Contains(cv_spine)) + foreach (var part in BODY_BONES_POSE_34) + { + var kp_a = cvt(obj.keypoints2D[(int)part.Item1], img_scale); + var kp_b = cvt(obj.keypoints2D[(int)part.Item2], img_scale); + if (roi_render.Contains(kp_a) && roi_render.Contains(kp_b)) + { + Cv2.Line(left_display, kp_a, kp_b, base_color, 1, LineTypes.AntiAlias); + } + } + + // Draw Skeleton joints + foreach (var kp in obj.keypoints2D) { - Cv2.Circle(left_display, cv_spine, 3, base_color, -1); + Point cv_kp = cvt(kp, img_scale); + if (roi_render.Contains(cv_kp)) + { + Cv2.Circle(left_display, cv_kp, 3, base_color, -1); + } } } } diff --git a/body tracking/csharp/packages.config b/body tracking/csharp/packages.config index 3df83b03..00464fb1 100644 --- a/body tracking/csharp/packages.config +++ b/body tracking/csharp/packages.config @@ -1,9 +1,4 @@  - - - - - \ No newline at end of file diff --git a/body tracking/python/README.md b/body tracking/python/README.md index dd3898f0..45bc46d1 100644 --- a/body tracking/python/README.md +++ b/body tracking/python/README.md @@ -7,7 +7,7 @@ This sample shows how to detect and track human bodies in space. - Check the [Documentation](https://www.stereolabs.com/docs/) ## Run the program -*NOTE: A [ZED 2](https://store.stereolabs.com/products/zed-2) is required to use this module.* +*NOTE: The ZED v1 is not compatible with this module* python "body_tracking.py" diff --git a/body tracking/python/body_tracking.py b/body tracking/python/body_tracking.py index 4bd68a09..505aa674 100644 --- a/body tracking/python/body_tracking.py +++ b/body tracking/python/body_tracking.py @@ -62,9 +62,10 @@ zed.enable_positional_tracking(positional_tracking_parameters) obj_param = sl.ObjectDetectionParameters() - obj_param.enable_body_fitting = True # Smooth skeleton move - obj_param.enable_tracking = True # Track people across images flow + obj_param.enable_body_fitting = True # Smooth skeleton move + obj_param.enable_tracking = True # Track people across images flow obj_param.detection_model = sl.DETECTION_MODEL.HUMAN_BODY_FAST + obj_param.body_format = sl.BODY_FORMAT.POSE_18 # Choose the BODY_FORMAT you wish to use # Enable Object Detection module zed.enable_object_detection(obj_param) @@ -82,7 +83,7 @@ # Create OpenGL viewer viewer = gl.GLViewer() - viewer.init(camera_info.calibration_parameters.left_cam, obj_param.enable_tracking) + viewer.init(camera_info.calibration_parameters.left_cam, obj_param.enable_tracking,obj_param.body_format) # Create ZED objects filled in the main loop bodies = sl.Objects() @@ -100,7 +101,7 @@ viewer.update_view(image, bodies) # Update OCV view image_left_ocv = image.get_data() - cv_viewer.render_2D(image_left_ocv,image_scale,bodies.object_list, obj_param.enable_tracking) + cv_viewer.render_2D(image_left_ocv,image_scale,bodies.object_list, obj_param.enable_tracking, obj_param.body_format) cv2.imshow("ZED | 2D View", image_left_ocv) cv2.waitKey(10) diff --git a/body tracking/python/cv_viewer/tracking_viewer.py b/body tracking/python/cv_viewer/tracking_viewer.py index 5d2ac22e..cd7dcbc2 100644 --- a/body tracking/python/cv_viewer/tracking_viewer.py +++ b/body tracking/python/cv_viewer/tracking_viewer.py @@ -14,7 +14,7 @@ def cvt(pt, scale): out = [pt[0]*scale[0], pt[1]*scale[1]] return out -def render_2D(left_display, img_scale, objects, is_tracking_on): +def render_2D(left_display, img_scale, objects, is_tracking_on, body_format): ''' Parameters left_display (np.array): numpy array containing image data @@ -28,36 +28,51 @@ def render_2D(left_display, img_scale, objects, is_tracking_on): if render_object(obj, is_tracking_on): if len(obj.keypoint_2d) > 0: color = generate_color_id_u(obj.id) - # Draw skeleton bones - for part in SKELETON_BONES: - kp_a = cvt(obj.keypoint_2d[part[0].value], img_scale) - kp_b = cvt(obj.keypoint_2d[part[1].value], img_scale) + # POSE_18 + if body_format == sl.BODY_FORMAT.POSE_18: + # Draw skeleton bones + for part in SKELETON_BONES: + kp_a = cvt(obj.keypoint_2d[part[0].value], img_scale) + kp_b = cvt(obj.keypoint_2d[part[1].value], img_scale) + # Check that the keypoints are inside the image + if(kp_a[0] < left_display.shape[1] and kp_a[1] < left_display.shape[0] + and kp_b[0] < left_display.shape[1] and kp_b[1] < left_display.shape[0] + and kp_a[0] > 0 and kp_a[1] > 0 and kp_b[0] > 0 and kp_b[1] > 0 ): + cv2.line(left_display, (int(kp_a[0]), int(kp_a[1])), (int(kp_b[0]), int(kp_b[1])), color, 1, cv2.LINE_AA) + + # Get spine base coordinates to create backbone + left_hip = obj.keypoint_2d[sl.BODY_PARTS.LEFT_HIP.value] + right_hip = obj.keypoint_2d[sl.BODY_PARTS.RIGHT_HIP.value] + spine = (left_hip + right_hip) / 2 + kp_spine = cvt(spine, img_scale) + kp_neck = cvt(obj.keypoint_2d[sl.BODY_PARTS.NECK.value], img_scale) # Check that the keypoints are inside the image - if(kp_a[0] < left_display.shape[1] and kp_a[1] < left_display.shape[0] - and kp_b[0] < left_display.shape[1] and kp_b[1] < left_display.shape[0] - and kp_a[0] > 0 and kp_a[1] > 0 and kp_b[0] > 0 and kp_b[1] > 0 ): - cv2.line(left_display, (int(kp_a[0]), int(kp_a[1])), (int(kp_b[0]), int(kp_b[1])), color, 1, cv2.LINE_AA) - - # Get spine base coordinates to create backbone - left_hip = obj.keypoint_2d[sl.BODY_PARTS.LEFT_HIP.value] - right_hip = obj.keypoint_2d[sl.BODY_PARTS.RIGHT_HIP.value] - spine = (left_hip + right_hip) / 2 - kp_spine = cvt(spine, img_scale) - kp_neck = cvt(obj.keypoint_2d[sl.BODY_PARTS.NECK.value], img_scale) - # Check that the keypoints are inside the image - if(kp_spine[0] < left_display.shape[1] and kp_spine[1] < left_display.shape[0] - and kp_neck[0] < left_display.shape[1] and kp_neck[1] < left_display.shape[0] - and kp_spine[0] > 0 and kp_spine[1] > 0 and kp_neck[0] > 0 and kp_neck[1] > 0 - and left_hip[0] > 0 and left_hip[1] > 0 and right_hip[0] > 0 and right_hip[1] > 0 ): - cv2.line(left_display, (int(kp_spine[0]), int(kp_spine[1])), (int(kp_neck[0]), int(kp_neck[1])), color, 1, cv2.LINE_AA) + if(kp_spine[0] < left_display.shape[1] and kp_spine[1] < left_display.shape[0] + and kp_neck[0] < left_display.shape[1] and kp_neck[1] < left_display.shape[0] + and kp_spine[0] > 0 and kp_spine[1] > 0 and kp_neck[0] > 0 and kp_neck[1] > 0 + and left_hip[0] > 0 and left_hip[1] > 0 and right_hip[0] > 0 and right_hip[1] > 0 ): + cv2.line(left_display, (int(kp_spine[0]), int(kp_spine[1])), (int(kp_neck[0]), int(kp_neck[1])), color, 1, cv2.LINE_AA) + + # Skeleton joints for spine + if(kp_spine[0] < left_display.shape[1] and kp_spine[1] < left_display.shape[0] + and left_hip[0] > 0 and left_hip[1] > 0 and right_hip[0] > 0 and right_hip[1] > 0 ): + cv2.circle(left_display, (int(kp_spine[0]), int(kp_spine[1])), 3, color, -1) + + elif body_format == sl.BODY_FORMAT.POSE_34: + # Draw skeleton bones + for part in sl.BODY_BONES_POSE_34: + kp_a = cvt(obj.keypoint_2d[part[0].value], img_scale) + kp_b = cvt(obj.keypoint_2d[part[1].value], img_scale) + # Check that the keypoints are inside the image + if(kp_a[0] < left_display.shape[1] and kp_a[1] < left_display.shape[0] + and kp_b[0] < left_display.shape[1] and kp_b[1] < left_display.shape[0] + and kp_a[0] > 0 and kp_a[1] > 0 and kp_b[0] > 0 and kp_b[1] > 0 ): + cv2.line(left_display, (int(kp_a[0]), int(kp_a[1])), (int(kp_b[0]), int(kp_b[1])), color, 1, cv2.LINE_AA) # Skeleton joints for kp in obj.keypoint_2d: cv_kp = cvt(kp, img_scale) if(cv_kp[0] < left_display.shape[1] and cv_kp[1] < left_display.shape[0]): cv2.circle(left_display, (int(cv_kp[0]), int(cv_kp[1])), 3, color, -1) - if(kp_spine[0] < left_display.shape[1] and kp_spine[1] < left_display.shape[0] - and left_hip[0] > 0 and left_hip[1] > 0 and right_hip[0] > 0 and right_hip[1] > 0 ): - cv2.circle(left_display, (int(kp_spine[0]), int(kp_spine[1])), 3, color, -1) - + cv2.addWeighted(left_display, 0.9, overlay, 0.1, 0.0, left_display) \ No newline at end of file diff --git a/body tracking/python/ogl_viewer/viewer.py b/body tracking/python/ogl_viewer/viewer.py index f861b566..05efa983 100644 --- a/body tracking/python/ogl_viewer/viewer.py +++ b/body tracking/python/ogl_viewer/viewer.py @@ -267,11 +267,12 @@ def draw(self): glDisableVertexAttribArray(1) class Skeleton: - def __init__(self): + def __init__(self, _body_format = sl.BODY_FORMAT.POSE_18): self.clr = [0,0,0,1] self.kps = [] self.joints = Simple3DObject(False) self.Z = 1 + self.body_format = _body_format def set(self, obj): self.joints.set_drawing_type(GL_LINES) @@ -279,28 +280,47 @@ def set(self, obj): self.Z = abs(obj.position[2]) # Draw skeletons if obj.keypoint.size > 0: - # Bones - # Definition of SKELETON_BONES in cv_viewer.utils.py, which slightly differs from BODY_BONES - for bone in SKELETON_BONES: - kp_1 = obj.keypoint[bone[0].value] - kp_2 = obj.keypoint[bone[1].value] - if math.isfinite(kp_1[0]) and math.isfinite(kp_2[0]): - self.joints.add_line(kp_1, kp_2) - - for part in range(len(sl.BODY_PARTS)-1): # -1 to avoid LAST - kp = obj.keypoint[part] - norm = np.linalg.norm(kp) - if math.isfinite(norm): - self.kps.append(kp) + # POSE_18 -> 18 keypoints + if self.body_format == sl.BODY_FORMAT.POSE_18: + # Bones + # Definition of SKELETON_BONES in cv_viewer.utils.py, which slightly differs from BODY_BONES + for bone in SKELETON_BONES: + kp_1 = obj.keypoint[bone[0].value] + kp_2 = obj.keypoint[bone[1].value] + if math.isfinite(kp_1[0]) and math.isfinite(kp_2[0]): + self.joints.add_line(kp_1, kp_2) + + for part in range(len(sl.BODY_PARTS)-1): # -1 to avoid LAST + kp = obj.keypoint[part] + norm = np.linalg.norm(kp) + if math.isfinite(norm): + self.kps.append(kp) + + # Create backbone (not defined in sl.BODY_BONES) + spine = (obj.keypoint[sl.BODY_PARTS.LEFT_HIP.value] + obj.keypoint[sl.BODY_PARTS.RIGHT_HIP.value]) / 2 + neck = obj.keypoint[sl.BODY_PARTS.NECK.value] + self.joints.add_line(spine, neck) + + # Spine base joint + if math.isfinite(np.linalg.norm(spine)): + self.kps.append(spine) - # Create backbone (not defined in sl.BODY_BONES) - spine = (obj.keypoint[sl.BODY_PARTS.LEFT_HIP.value] + obj.keypoint[sl.BODY_PARTS.RIGHT_HIP.value]) / 2 - neck = obj.keypoint[sl.BODY_PARTS.NECK.value] - self.joints.add_line(spine, neck) + # POSE_34 -> 34 keypoints + elif self.body_format == sl.BODY_FORMAT.POSE_34: + for bone in sl.BODY_BONES_POSE_34: + kp_1 = obj.keypoint[bone[0].value] + kp_2 = obj.keypoint[bone[1].value] + if math.isfinite(kp_1[0]) and math.isfinite(kp_2[0]): + self.joints.add_line(kp_1, kp_2) + + for part in range(len(sl.BODY_PARTS_POSE_34)-1): + kp = obj.keypoint[part] + norm = np.linalg.norm(kp) + if math.isfinite(norm): + self.kps.append(kp) + + - # Spine base joint - if math.isfinite(np.linalg.norm(spine)): - self.kps.append(spine) def push_to_GPU(self): self.joints.push_to_GPU() @@ -424,8 +444,9 @@ def __init__(self): self.basic_sphere = Simple3DObject(True) # Show tracked objects only self.is_tracking_on = False + self.body_format = sl.BODY_FORMAT.POSE_18 - def init(self, _params, _is_tracking_on): + def init(self, _params, _is_tracking_on, _body_format): glutInit() wnd_w = glutGet(GLUT_SCREEN_WIDTH) wnd_h = glutGet(GLUT_SCREEN_HEIGHT) @@ -480,6 +501,7 @@ def init(self, _params, _is_tracking_on): glutCloseFunc(self.close_func) self.available = True + self.body_format = _body_format def set_floor_plane_equation(self, _eq): self.floor_plane_set = True @@ -534,7 +556,7 @@ def update_view(self, _image, _objs): # _objs of type sl.Objects # Only show tracked objects for obj in _objs.object_list: if self.render_object(obj): - current_sk = Skeleton() + current_sk = Skeleton(self.body_format) current_sk.set(obj) self.bodies.append(current_sk) self.mutex.release() diff --git a/camera control/cpp/CMakeLists.txt b/camera control/cpp/CMakeLists.txt index 9ff343b4..81c4afd4 100644 --- a/camera control/cpp/CMakeLists.txt +++ b/camera control/cpp/CMakeLists.txt @@ -29,6 +29,22 @@ 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}) else() diff --git a/camera control/cpp/src/main.cpp b/camera control/cpp/src/main.cpp index cc7f87b3..6a91bedc 100644 --- a/camera control/cpp/src/main.cpp +++ b/camera control/cpp/src/main.cpp @@ -92,6 +92,11 @@ 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 "< "< - - - \ No newline at end of file diff --git a/depth sensing/csharp/CMakeLists.txt b/depth sensing/csharp/CMakeLists.txt index b6980061..5f96602d 100644 --- a/depth sensing/csharp/CMakeLists.txt +++ b/depth sensing/csharp/CMakeLists.txt @@ -35,11 +35,12 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES "WindowsBase" ) +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.5.0" - "sl_zed_interface_3.5.0" + "Stereolabs.zed_3.6.0" ) \ No newline at end of file diff --git a/depth sensing/csharp/MainWindow.cs b/depth sensing/csharp/MainWindow.cs index cb2cad10..5d336b30 100644 --- a/depth sensing/csharp/MainWindow.cs +++ b/depth sensing/csharp/MainWindow.cs @@ -64,10 +64,9 @@ public MainWindow(string[] args) if (err != ERROR_CODE.SUCCESS) Environment.Exit(-1); - // Enable trackin - Quaternion quat = Quaternion.Identity; - Vector3 vec = Vector3.Zero; - zedCamera.EnablePositionalTracking(ref quat, ref vec); + // Enable tracking + PositionalTrackingParameters trackingParams = new PositionalTrackingParameters(); + zedCamera.EnablePositionalTracking(ref trackingParams); runtimeParameters = new RuntimeParameters(); @@ -129,7 +128,14 @@ public void CreateWindow() nativeWindow.Create((int)(zedCamera.ImageWidth * 0.05f), (int)(zedCamera.ImageHeight * 0.05f), 1200, 700, NativeWindowStyle.Resizeable); nativeWindow.Show(); - nativeWindow.Run(); + try + { + nativeWindow.Run(); + } + catch (Exception e) + { + Console.WriteLine("Mouse wheel is broken in the current OPENGL .NET VERSION. Please do not use it."); + } } } diff --git a/depth sensing/csharp/packages.config b/depth sensing/csharp/packages.config index 0f7ea7dc..8038b422 100644 --- a/depth sensing/csharp/packages.config +++ b/depth sensing/csharp/packages.config @@ -1,8 +1,4 @@  - - - - \ No newline at end of file diff --git a/object detection/README.md b/object detection/README.md index d230ad80..b3f2f77f 100644 --- a/object detection/README.md +++ b/object detection/README.md @@ -1,4 +1,5 @@ # ZED SDK - Object Detection - **Birds eye viewer**: Deteted objects are displayed in a **3D** view with the current point cloud. - - **Image viewer**: Detected objects are displayed in a **2D** view with the current image. \ No newline at end of file + - **Image viewer**: Detected objects are displayed in a **2D** view with the current image. + - **Custom detector**: Sample with external detectors to use the DETECTION_MODEL::CUSTOM_BOX_OBJECTS mode \ No newline at end of file diff --git a/object detection/birds eye viewer/README.md b/object detection/birds eye viewer/README.md index 22bc0904..c3446e94 100644 --- a/object detection/birds eye viewer/README.md +++ b/object detection/birds eye viewer/README.md @@ -10,4 +10,4 @@ ![](object_detection.jpg) ->*NOTE: A [ZED 2](https://store.stereolabs.com/products/zed-2) is required to use this module.* \ No newline at end of file +>*NOTE: The ZED v1 is not compatible with this module* diff --git a/object detection/birds eye viewer/cpp/README.md b/object detection/birds eye viewer/cpp/README.md index 941fe9b6..648cc8ce 100644 --- a/object detection/birds eye viewer/cpp/README.md +++ b/object detection/birds eye viewer/cpp/README.md @@ -11,7 +11,7 @@ This sample shows how to detect and track objects in space. - Build for [Linux/Jetson](https://www.stereolabs.com/docs/app-development/cpp/linux/) ## Run the program -*NOTE: A [ZED 2](https://store.stereolabs.com/products/zed-2) is required to use this module.* +*NOTE: The ZED v1 is not compatible with this module* - Navigate to the build directory and launch the executable - Or open a terminal in the build directory and run the sample : diff --git a/object detection/birds eye viewer/cpp/src/main.cpp b/object detection/birds eye viewer/cpp/src/main.cpp index 00c3a8c0..a9f954ed 100644 --- a/object detection/birds eye viewer/cpp/src/main.cpp +++ b/object detection/birds eye viewer/cpp/src/main.cpp @@ -76,6 +76,7 @@ int main(int argc, char **argv) { init_parameters.depth_mode = isJetson ? DEPTH_MODE::PERFORMANCE : DEPTH_MODE::ULTRA; init_parameters.depth_maximum_distance = 10.0f * 1000.0f; init_parameters.coordinate_system = COORDINATE_SYSTEM::RIGHT_HANDED_Y_UP; // OpenGL's coordinate system is right_handed + parseArgs(argc, argv, init_parameters); // Open the camera diff --git a/object detection/birds eye viewer/csharp/CMakeLists.txt b/object detection/birds eye viewer/csharp/CMakeLists.txt index 376b1b71..4ed6de7f 100644 --- a/object detection/birds eye viewer/csharp/CMakeLists.txt +++ b/object detection/birds eye viewer/csharp/CMakeLists.txt @@ -38,12 +38,14 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES "WindowsBase" ) +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" "OpenCvSharp4.Windows_4.5.0.20201013" - "Stereolabs.zed_3.5.0" - "sl_zed_interface_3.5.0" + "Stereolabs.zed_3.6.0" ) \ No newline at end of file diff --git a/object detection/birds eye viewer/csharp/MainWindow.cs b/object detection/birds eye viewer/csharp/MainWindow.cs index f82898c9..30eb162f 100644 --- a/object detection/birds eye viewer/csharp/MainWindow.cs +++ b/object detection/birds eye viewer/csharp/MainWindow.cs @@ -92,16 +92,15 @@ public MainWindow(string[] args) if (err != ERROR_CODE.SUCCESS) Environment.Exit(-1); - if (zedCamera.CameraModel != sl.MODEL.ZED2) + if (zedCamera.CameraModel == sl.MODEL.ZED) { - Console.WriteLine(" ERROR : Use ZED2 Camera only"); + Console.WriteLine(" ERROR : not compatible camera model"); return; } // Enable tracking (mandatory for object detection) - Quaternion quat = Quaternion.Identity; - Vector3 vec = Vector3.Zero; - zedCamera.EnablePositionalTracking(ref quat, ref vec); + PositionalTrackingParameters trackingParams = new PositionalTrackingParameters(); + zedCamera.EnablePositionalTracking(ref trackingParams); runtimeParameters = new RuntimeParameters(); @@ -219,7 +218,14 @@ public void CreateWindow() nativeWindow.Create((int)(zedCamera.ImageWidth * 0.05f), (int)(zedCamera.ImageHeight * 0.05f), 1200, 700, NativeWindowStyle.Resizeable); nativeWindow.Show(); - nativeWindow.Run(); + try + { + nativeWindow.Run(); + } + catch (Exception e) + { + Console.WriteLine("Mouse wheel is broken in the current OPENGL .NET VERSION. Please do not use it."); + } } } diff --git a/object detection/birds eye viewer/csharp/README.md b/object detection/birds eye viewer/csharp/README.md index 7a95ca93..4bb3d995 100644 --- a/object detection/birds eye viewer/csharp/README.md +++ b/object detection/birds eye viewer/csharp/README.md @@ -10,7 +10,7 @@ This sample shows how to detect and track objects in space. - Build for [Windows](https://www.stereolabs.com/docs/app-development/cpp/windows/) ## Run the program -*NOTE: A [ZED 2](https://store.stereolabs.com/products/zed-2) is required to use this module.* +*NOTE: The ZED v1 is not compatible with this module* - Navigate to the build directory and launch the executable - Or open a terminal in the build directory and run the sample : diff --git a/object detection/birds eye viewer/csharp/Utils.cs b/object detection/birds eye viewer/csharp/Utils.cs index abc15534..6250a92f 100644 --- a/object detection/birds eye viewer/csharp/Utils.cs +++ b/object detection/birds eye viewer/csharp/Utils.cs @@ -137,7 +137,7 @@ public static OpenCvSharp.Scalar generateColorClass_u(int idx) return color; } - bool renderObject(ObjectData i, bool showOnlyOK) + public static bool renderObject(ObjectData i, bool showOnlyOK) { if (showOnlyOK) return (i.objectTrackingState == sl.OBJECT_TRACKING_STATE.OK); diff --git a/object detection/birds eye viewer/csharp/packages.config b/object detection/birds eye viewer/csharp/packages.config index 452c3818..fa2bea01 100644 --- a/object detection/birds eye viewer/csharp/packages.config +++ b/object detection/birds eye viewer/csharp/packages.config @@ -1,9 +1,4 @@  - - - - - \ No newline at end of file diff --git a/object detection/birds eye viewer/python/README.md b/object detection/birds eye viewer/python/README.md index 167e5881..33a8a8fa 100644 --- a/object detection/birds eye viewer/python/README.md +++ b/object detection/birds eye viewer/python/README.md @@ -7,7 +7,7 @@ This sample shows how to detect and track objects in space. - Check the [Documentation](https://www.stereolabs.com/docs/) ## Run the program -*NOTE: A [ZED 2](https://store.stereolabs.com/products/zed-2) is required to use this module.* +*NOTE: The ZED v1 is not compatible with this module* python "object_detection_birds_view.py" diff --git a/object detection/custom detector/cpp/opencv_dnn_yolov4/CMakeLists.txt b/object detection/custom detector/cpp/opencv_dnn_yolov4/CMakeLists.txt new file mode 100644 index 00000000..8ab3c682 --- /dev/null +++ b/object detection/custom detector/cpp/opencv_dnn_yolov4/CMakeLists.txt @@ -0,0 +1,81 @@ +cmake_minimum_required(VERSION 2.4) +PROJECT(opencv_dnn_zed) + +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) + +if (NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "") +SET(CMAKE_BUILD_TYPE "RelWithDebInfo") +endif() + +SET(EXECUTABLE_OUTPUT_PATH ".") +SET(SPECIAL_OS_LIBS "") + +find_package(ZED 3 REQUIRED) +find_package(OpenCV REQUIRED) +find_package(GLUT REQUIRED) +find_package(GLEW REQUIRED) +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}) +include_directories(${GLEW_INCLUDE_DIRS}) +include_directories(${GLUT_INCLUDE_DIR}) +include_directories(${CUDA_INCLUDE_DIRS}) +include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) + +link_directories(${ZED_LIBRARY_DIR}) +link_directories(${GLEW_LIBRARY_DIRS}) +link_directories(${GLUT_LIBRARY_DIRS}) +link_directories(${OpenGL_LIBRARY_DIRS}) +link_directories(${CUDA_LIBRARY_DIRS}) +link_directories(${OpenCV_LIBRARY_DIRS}) +# cuda +include_directories(/usr/local/cuda/include) +link_directories(/usr/local/cuda/lib64) +# tensorrt +include_directories(/usr/include/x86_64-linux-gnu/) +link_directories(/usr/lib/x86_64-linux-gnu/) + +FILE(GLOB_RECURSE SRC_FILES src/*.c*) +FILE(GLOB_RECURSE HDR_FILES include/*.h*) + +set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} -std=c++11) +cuda_add_executable(${PROJECT_NAME} ${HDR_FILES} ${SRC_FILES}) +add_definitions(-std=c++14 -g -O3 -D_MWAITXINTRIN_H_INCLUDED -Wno-deprecated-declarations) + +if (LINK_SHARED_ZED) + SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY}) +else() + SET(ZED_LIBS ${ZED_STATIC_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_LIBRARY}) +endif() + + +target_link_libraries(${PROJECT_NAME} + ${SPECIAL_OS_LIBS} + ${ZED_LIBS} + ${OPENGL_LIBRARIES} + ${GLUT_LIBRARY} + ${GLEW_LIBRARIES} + ${OpenCV_LIBRARIES} + ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY}) + +if(INSTALL_SAMPLES) + LIST(APPEND SAMPLE_LIST ${PROJECT_NAME}) + SET(SAMPLE_LIST "${SAMPLE_LIST}" PARENT_SCOPE) +endif() diff --git a/object detection/custom detector/cpp/opencv_dnn_yolov4/README.md b/object detection/custom detector/cpp/opencv_dnn_yolov4/README.md new file mode 100644 index 00000000..94b7672f --- /dev/null +++ b/object detection/custom detector/cpp/opencv_dnn_yolov4/README.md @@ -0,0 +1,62 @@ +# OpenCV DNN with ZED Custom Box input + +This sample is designed to run a state of the art object detection model using OpenCV with the DNN module. The image are taken from the ZED SDK, and the 2D box detections are then ingested into the ZED SDK to extract 3D informations (localization, 3D bounding boxes) and tracking. + +The default model is [Yolov4 from the darknet framework](https://github.com/AlexeyAB/darknet). + +## Getting Started + - Get the latest [ZED SDK](https://www.stereolabs.com/developers/release/) + - Check the [Documentation](https://www.stereolabs.com/docs/) + - Install OpenCV with the DNN module + + +## Using the sample + +After installing OpenCV with DNN module (and preferably CUDA Backend support), this sample can be built. + +### Build the program + - Build for [Windows](https://www.stereolabs.com/docs/app-development/cpp/windows/) + - Build for [Linux/Jetson](https://www.stereolabs.com/docs/app-development/cpp/linux/) + +### Preparing the model + +The default model is running Yolov4 trained on COCO dataset, for a custom model the file containing the name and the `NUM_CLASSES` variable should be udpated accordingly. + + - Download the [Yolov4 weights](https://github.com/AlexeyAB/darknet/releases/download/darknet_yolo_v3_optimal/yolov4.weights) + +```sh +wget https://github.com/AlexeyAB/darknet/releases/download/darknet_yolo_v3_optimal/yolov4.weights +``` + +In the sample root folder should be present 3 files for the model : + +- `coco.names.txt` containing the class name for display, this file is optionnal +- `yolov4.cfg` containing the network definition +- `yolov4.weights` containing the network weights + +The sample should be run from this folder to avoid path issue when loading those files. + +For instance on Linux: + +```sh +mkdir build +cd build +cmake .. +make +cd ../ + +# ls +# build +# CMakeLists.txt +# coco.names.txt +# include +# README.md +# src +# yolov4.cfg +# yolov4.weights + +# Running the sample +./build/opencv_dnn_zed +``` + +The GUI is composed of 2 window, a 2D OpenCV view of the raw detections and a 3D OpenGL view of the ZED SDK output from the OpenCV DNN detection with 3D informations and tracking extracted. \ No newline at end of file diff --git a/object detection/custom detector/cpp/opencv_dnn_yolov4/coco.names.txt b/object detection/custom detector/cpp/opencv_dnn_yolov4/coco.names.txt new file mode 100644 index 00000000..ca76c80b --- /dev/null +++ b/object detection/custom detector/cpp/opencv_dnn_yolov4/coco.names.txt @@ -0,0 +1,80 @@ +person +bicycle +car +motorbike +aeroplane +bus +train +truck +boat +traffic light +fire hydrant +stop sign +parking meter +bench +bird +cat +dog +horse +sheep +cow +elephant +bear +zebra +giraffe +backpack +umbrella +handbag +tie +suitcase +frisbee +skis +snowboard +sports ball +kite +baseball bat +baseball glove +skateboard +surfboard +tennis racket +bottle +wine glass +cup +fork +knife +spoon +bowl +banana +apple +sandwich +orange +broccoli +carrot +hot dog +pizza +donut +cake +chair +sofa +pottedplant +bed +diningtable +toilet +tvmonitor +laptop +mouse +remote +keyboard +cell phone +microwave +oven +toaster +sink +refrigerator +book +clock +vase +scissors +teddy bear +hair drier +toothbrush diff --git a/object detection/custom detector/cpp/opencv_dnn_yolov4/include/GLViewer.hpp b/object detection/custom detector/cpp/opencv_dnn_yolov4/include/GLViewer.hpp new file mode 100644 index 00000000..0a1394fc --- /dev/null +++ b/object detection/custom detector/cpp/opencv_dnn_yolov4/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/opencv_dnn_yolov4/include/utils.hpp b/object detection/custom detector/cpp/opencv_dnn_yolov4/include/utils.hpp new file mode 100644 index 00000000..a96504f8 --- /dev/null +++ b/object detection/custom detector/cpp/opencv_dnn_yolov4/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/opencv_dnn_yolov4/src/GLViewer.cpp b/object detection/custom detector/cpp/opencv_dnn_yolov4/src/GLViewer.cpp new file mode 100644 index 00000000..51265df0 --- /dev/null +++ b/object detection/custom detector/cpp/opencv_dnn_yolov4/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/opencv_dnn_yolov4/src/main.cpp b/object detection/custom detector/cpp/opencv_dnn_yolov4/src/main.cpp new file mode 100644 index 00000000..1d4b0c13 --- /dev/null +++ b/object detection/custom detector/cpp/opencv_dnn_yolov4/src/main.cpp @@ -0,0 +1,210 @@ +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include + +#include "GLViewer.hpp" + +constexpr float CONFIDENCE_THRESHOLD = 0; +constexpr float NMS_THRESHOLD = 0.4; +constexpr int NUM_CLASSES = 80; +constexpr int INFERENCE_SIZE = 416; + +// colors for bounding boxes +const cv::Scalar colors[] = { + {0, 255, 255}, + {255, 255, 0}, + {0, 255, 0}, + {255, 0, 0} +}; +const auto NUM_COLORS = sizeof (colors) / sizeof (colors[0]); + +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::vector class_names; + { + std::ifstream class_file("coco.names.txt"); + if (!class_file) { + for (int i = 0; i < NUM_CLASSES; i++) + class_names.push_back(std::to_string(i)); + } else { + std::string line; + while (std::getline(class_file, line)) + class_names.push_back(line); + } + } + + /// 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.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 >= 2) { + std::string zed_opt = argv[1]; + 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; + // Let's define the model as custom box object to specify that the inference is done externally + 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); + sl::Mat left_sl, point_cloud; + sl::ObjectDetectionRuntimeParameters objectTracker_parameters_rt; + sl::Objects objects; + sl::Pose cam_w_pose; + cam_w_pose.pose_data.setIdentity(); + // --------- + + // Weight can be downloaded from https://github.com/AlexeyAB/darknet/releases/download/darknet_yolo_v3_optimal/yolov4.weights + auto net = cv::dnn::readNetFromDarknet("yolov4.cfg", "yolov4.weights"); + net.setPreferableBackend(cv::dnn::DNN_BACKEND_CUDA); + net.setPreferableTarget(cv::dnn::DNN_TARGET_CUDA); + // net.setPreferableBackend(cv::dnn::DNN_BACKEND_OPENCV); + // net.setPreferableTarget(cv::dnn::DNN_TARGET_CPU); + auto output_names = net.getUnconnectedOutLayersNames(); + + cv::Mat frame, blob; + std::vector detections; + 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, frame, cv::COLOR_BGRA2BGR); + + cv::dnn::blobFromImage(frame, blob, 0.00392, cv::Size(INFERENCE_SIZE, INFERENCE_SIZE), cv::Scalar(), true, false, CV_32F); + net.setInput(blob); + net.forward(detections, output_names); + + std::vector indices[NUM_CLASSES]; + std::vector boxes[NUM_CLASSES]; + std::vector scores[NUM_CLASSES]; + + for (auto& output : detections) { + const auto num_boxes = output.rows; + for (int i = 0; i < num_boxes; i++) { + auto x = output.at(i, 0) * frame.cols; + auto y = output.at(i, 1) * frame.rows; + auto width = output.at(i, 2) * frame.cols; + auto height = output.at(i, 3) * frame.rows; + cv::Rect rect(x - width / 2, y - height / 2, width, height); + + for (int c = 0; c < NUM_CLASSES; c++) { + auto confidence = *output.ptr(i, 5 + c); + if (confidence >= CONFIDENCE_THRESHOLD) { + boxes[c].push_back(rect); + scores[c].push_back(confidence); + } + } + } + } + + for (int c = 0; c < NUM_CLASSES; c++) + cv::dnn::NMSBoxes(boxes[c], scores[c], 0.0, NMS_THRESHOLD, indices[c]); + + std::vector objects_in; + for (int c = 0; c < NUM_CLASSES; c++) { + for (size_t i = 0; i < indices[c].size(); ++i) { + const auto color = colors[c % NUM_COLORS]; + + auto idx = indices[c][i]; + const auto& rect = boxes[c][idx]; + auto& rect_score = scores[c][idx]; + + // Fill the detections into the correct format + sl::CustomBoxObjectData tmp; + tmp.unique_object_id = sl::generate_unique_id(); + tmp.probability = rect_score; + tmp.label = c; + tmp.bounding_box_2d = cvt(rect); + tmp.is_grounded = (c == 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); + //-- + + cv::rectangle(frame, rect, color, 3); + + std::ostringstream label_ss; + label_ss << class_names[c] << ": " << std::fixed << std::setprecision(2) << scores[c][idx]; + auto label = label_ss.str(); + + int baseline; + auto label_bg_sz = cv::getTextSize(label.c_str(), cv::FONT_HERSHEY_COMPLEX_SMALL, 1, 1, &baseline); + cv::rectangle(frame, cv::Point(rect.x, rect.y - label_bg_sz.height - baseline - 10), cv::Point(rect.x + label_bg_sz.width, rect.y), color, cv::FILLED); + cv::putText(frame, label.c_str(), cv::Point(rect.x, rect.y - baseline - 5), cv::FONT_HERSHEY_COMPLEX_SMALL, 1, cv::Scalar(0, 0, 0)); + } + } + // Send the custom detected boxes to the ZED + zed.ingestCustomBoxObjects(objects_in); + + cv::imshow("Objects", frame); + 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); + } + } + return 0; +} \ No newline at end of file diff --git a/object detection/custom detector/cpp/opencv_dnn_yolov4/yolov4.cfg b/object detection/custom detector/cpp/opencv_dnn_yolov4/yolov4.cfg new file mode 100644 index 00000000..a7be12b3 --- /dev/null +++ b/object detection/custom detector/cpp/opencv_dnn_yolov4/yolov4.cfg @@ -0,0 +1,1158 @@ +[net] +batch=64 +subdivisions=8 +# Training +#width=512 +#height=512 +width=608 +height=608 +channels=3 +momentum=0.949 +decay=0.0005 +angle=0 +saturation = 1.5 +exposure = 1.5 +hue=.1 + +learning_rate=0.0013 +burn_in=1000 +max_batches = 500500 +policy=steps +steps=400000,450000 +scales=.1,.1 + +#cutmix=1 +mosaic=1 + +#:104x104 54:52x52 85:26x26 104:13x13 for 416 + +[convolutional] +batch_normalize=1 +filters=32 +size=3 +stride=1 +pad=1 +activation=mish + +# Downsample + +[convolutional] +batch_normalize=1 +filters=64 +size=3 +stride=2 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=64 +size=1 +stride=1 +pad=1 +activation=mish + +[route] +layers = -2 + +[convolutional] +batch_normalize=1 +filters=64 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=32 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=64 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=64 +size=1 +stride=1 +pad=1 +activation=mish + +[route] +layers = -1,-7 + +[convolutional] +batch_normalize=1 +filters=64 +size=1 +stride=1 +pad=1 +activation=mish + +# Downsample + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=2 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=64 +size=1 +stride=1 +pad=1 +activation=mish + +[route] +layers = -2 + +[convolutional] +batch_normalize=1 +filters=64 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=64 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=64 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=64 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=64 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=64 +size=1 +stride=1 +pad=1 +activation=mish + +[route] +layers = -1,-10 + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=mish + +# Downsample + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=2 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=mish + +[route] +layers = -2 + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=mish + +[route] +layers = -1,-28 + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=mish + +# Downsample + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=2 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=mish + +[route] +layers = -2 + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=mish + +[route] +layers = -1,-28 + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=mish + +# Downsample + +[convolutional] +batch_normalize=1 +filters=1024 +size=3 +stride=2 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=mish + +[route] +layers = -2 + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=mish + +[route] +layers = -1,-16 + +[convolutional] +batch_normalize=1 +filters=1024 +size=1 +stride=1 +pad=1 +activation=mish + +########################## + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=1024 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=leaky + +### SPP ### +[maxpool] +stride=1 +size=5 + +[route] +layers=-2 + +[maxpool] +stride=1 +size=9 + +[route] +layers=-4 + +[maxpool] +stride=1 +size=13 + +[route] +layers=-1,-3,-5,-6 +### End SPP ### + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=1024 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[upsample] +stride=2 + +[route] +layers = 85 + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[route] +layers = -1, -3 + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=512 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=512 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=leaky + +[upsample] +stride=2 + +[route] +layers = 54 + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=leaky + +[route] +layers = -1, -3 + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=256 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=256 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=leaky + +########################## + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=256 +activation=leaky + +[convolutional] +size=1 +stride=1 +pad=1 +filters=255 +activation=linear + + +[yolo] +mask = 0,1,2 +anchors = 12, 16, 19, 36, 40, 28, 36, 75, 76, 55, 72, 146, 142, 110, 192, 243, 459, 401 +classes=80 +num=9 +jitter=.3 +ignore_thresh = .7 +truth_thresh = 1 +scale_x_y = 1.2 +iou_thresh=0.213 +cls_normalizer=1.0 +iou_normalizer=0.07 +iou_loss=ciou +nms_kind=greedynms +beta_nms=0.6 +max_delta=5 + + +[route] +layers = -4 + +[convolutional] +batch_normalize=1 +size=3 +stride=2 +pad=1 +filters=256 +activation=leaky + +[route] +layers = -1, -16 + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=512 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=512 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=512 +activation=leaky + +[convolutional] +size=1 +stride=1 +pad=1 +filters=255 +activation=linear + + +[yolo] +mask = 3,4,5 +anchors = 12, 16, 19, 36, 40, 28, 36, 75, 76, 55, 72, 146, 142, 110, 192, 243, 459, 401 +classes=80 +num=9 +jitter=.3 +ignore_thresh = .7 +truth_thresh = 1 +scale_x_y = 1.1 +iou_thresh=0.213 +cls_normalizer=1.0 +iou_normalizer=0.07 +iou_loss=ciou +nms_kind=greedynms +beta_nms=0.6 +max_delta=5 + + +[route] +layers = -4 + +[convolutional] +batch_normalize=1 +size=3 +stride=2 +pad=1 +filters=512 +activation=leaky + +[route] +layers = -1, -37 + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=1024 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=1024 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=1024 +activation=leaky + +[convolutional] +size=1 +stride=1 +pad=1 +filters=255 +activation=linear + + +[yolo] +mask = 6,7,8 +anchors = 12, 16, 19, 36, 40, 28, 36, 75, 76, 55, 72, 146, 142, 110, 192, 243, 459, 401 +classes=80 +num=9 +jitter=.3 +ignore_thresh = .7 +truth_thresh = 1 +random=1 +scale_x_y = 1.05 +iou_thresh=0.213 +cls_normalizer=1.0 +iou_normalizer=0.07 +iou_loss=ciou +nms_kind=greedynms +beta_nms=0.6 +max_delta=5 + diff --git a/object detection/custom detector/cpp/tensorrt_yolov5/CMakeLists.txt b/object detection/custom detector/cpp/tensorrt_yolov5/CMakeLists.txt new file mode 100644 index 00000000..7c9668e9 --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5/CMakeLists.txt @@ -0,0 +1,83 @@ +cmake_minimum_required(VERSION 2.4) +PROJECT(yolov5_zed) + +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) + +if (NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "") +SET(CMAKE_BUILD_TYPE "RelWithDebInfo") +endif() + +SET(EXECUTABLE_OUTPUT_PATH ".") +SET(SPECIAL_OS_LIBS "") + +find_package(ZED 3 REQUIRED) +find_package(OpenCV REQUIRED) +find_package(GLUT REQUIRED) +find_package(GLEW REQUIRED) +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}) +include_directories(${GLEW_INCLUDE_DIRS}) +include_directories(${GLUT_INCLUDE_DIR}) +include_directories(${CUDA_INCLUDE_DIRS}) +include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) + +link_directories(${ZED_LIBRARY_DIR}) +link_directories(${GLEW_LIBRARY_DIRS}) +link_directories(${GLUT_LIBRARY_DIRS}) +link_directories(${OpenGL_LIBRARY_DIRS}) +link_directories(${CUDA_LIBRARY_DIRS}) +link_directories(${OpenCV_LIBRARY_DIRS}) +# cuda +include_directories(/usr/local/cuda/include) +link_directories(/usr/local/cuda/lib64) +# tensorrt +include_directories(/usr/include/x86_64-linux-gnu/) +link_directories(/usr/lib/x86_64-linux-gnu/) + +FILE(GLOB_RECURSE SRC_FILES src/*.c*) +FILE(GLOB_RECURSE HDR_FILES include/*.h*) + +set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} -std=c++11) +cuda_add_executable(${PROJECT_NAME} ${HDR_FILES} ${SRC_FILES}) +add_definitions(-std=c++14 -g -O3 -D_MWAITXINTRIN_H_INCLUDED -Wno-deprecated-declarations) + +if (LINK_SHARED_ZED) + SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY}) +else() + SET(ZED_LIBS ${ZED_STATIC_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_LIBRARY}) +endif() + +SET(TRT_LIBS nvinfer) + +target_link_libraries(${PROJECT_NAME} + ${TRT_LIBS} + ${SPECIAL_OS_LIBS} + ${ZED_LIBS} + ${OPENGL_LIBRARIES} + ${GLUT_LIBRARY} + ${GLEW_LIBRARIES} + ${OpenCV_LIBRARIES} + ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY}) + +if(INSTALL_SAMPLES) + LIST(APPEND SAMPLE_LIST ${PROJECT_NAME}) + SET(SAMPLE_LIST "${SAMPLE_LIST}" PARENT_SCOPE) +endif() diff --git a/object detection/custom detector/cpp/tensorrt_yolov5/LICENSE b/object detection/custom detector/cpp/tensorrt_yolov5/LICENSE new file mode 100644 index 00000000..e0718cfd --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2019-2020 Wang Xinyu + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/object detection/custom detector/cpp/tensorrt_yolov5/README.md b/object detection/custom detector/cpp/tensorrt_yolov5/README.md new file mode 100644 index 00000000..37801e52 --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5/README.md @@ -0,0 +1,78 @@ +# Yolov5 with ZED Custom Box input + +This sample is designed to run a state of the art object detection model using the highly optimized TensorRT framework. The image are taken from the ZED SDK, and the 2D box detections are then ingested into the ZED SDK to extract 3D informations (localization, 3D bounding boxes) and tracking. + +This sample is a fork from [wang-xinyu/tensorrtx](https://github.com/wang-xinyu/tensorrtx/tree/master/yolov5). The Pytorch implementation is [ultralytics/yolov5](https://github.com/ultralytics/yolov5). The default model was trained on COCO dataset (80 classes), a custom detector can be trained with the same architecture, it requires a configuration tweak (see below). This tutorial walk you through the workflow of training a custom detector https://github.com/ultralytics/yolov5/wiki/Train-Custom-Data. + + +## Getting Started + + - Get the latest [ZED SDK](https://www.stereolabs.com/developers/release/) + - Check the [Documentation](https://www.stereolabs.com/docs/) + - [TensorRT Documentation](https://docs.nvidia.com/deeplearning/tensorrt/developer-guide/index.html) + +## Different versions of yolov5 + +- For yolov5 v5.0, download .pt from [yolov5 release v5.0](https://github.com/ultralytics/yolov5/releases/tag/v5.0), `git clone -b v5.0 https://github.com/ultralytics/yolov5.git` + +## Config + +- Choose the model s/m/l/x/s6/m6/l6/x6 from command line arguments. +- Input shape defined in yololayer.h +- Number of classes defined in yololayer.h, **DO NOT FORGET TO ADAPT THIS, If using your own model** +- FP16/FP32 can be selected by the macro in yolov5.cpp, FP16 is faster if the GPU support it (all jetsons or GeForce RTX cards), +- GPU id can be selected by the macro in yolov5.cpp +- NMS thresh in yolov5.cpp +- BBox confidence thresh in yolov5.cpp + +## Using the sample + + +### 1. (Optional for first run) Generate .wts from pytorch with .pt + +**This file has already been generated and can be downloaded [here](https://download.stereolabs.com/sample_custom_objects/yolov5s.wts.zip)** (and needs to be unzipped) to run the sample. + +This procedure can be applied to other models (such as `l` or `m` variants) or custom dataset trained model. + +The goal is to export the PyTorch model `.pt` into a easily readable weight file `.wts`. + +```sh +git clone -b v5.0 https://github.com/ultralytics/yolov5.git +# Download the pretrained weight file +wget https://github.com/ultralytics/yolov5/releases/download/v5.0/yolov5s.pt +cp gen_wts.py {ultralytics}/yolov5 +cd {ultralytics}/yolov5 +python gen_wts.py yolov5s.pt +# a file 'yolov5s.wts' will be generated. +``` + + +### 2. Build the sample + +If a custom model is used, let's say trained on another dataset than COCO, with a different number of classes, `CLASS_NUM` in yololayer.h must be updated accordingly. + + - Build for [Windows](https://www.stereolabs.com/docs/app-development/cpp/windows/) + - Build for [Linux/Jetson](https://www.stereolabs.com/docs/app-development/cpp/linux/) + + +### 3. Generate the TensorRT engine + +TensorRT apply heavy optimisation by processing the network structure itself and benchmarking all the available implementation of each inference function to take the fastest. The result in the inference engine. This process can take a few minutes so we usually want to generate it the first time than saving it for later reload. This step should be done at each model or weight change, but only once. + +```sh +./yolov5 -s [.wts] [.engine] [s/m/l/x/s6/m6/l6/x6 or c/c6 gd gw] // serialize model to plan file +# For example yolov5s +./yolov5 -s yolov5s.wts yolov5s.engine s +# For example Custom model with depth_multiple=0.17, width_multiple=0.25 in yolov5.yaml +./yolov5 -s yolov5_custom.wts yolov5.engine c 0.17 0.25 +``` + +### 4. Running the sample with the engine generated + +```sh +./yolov5 -d [.engine] [optional svo filepath] // deserialize and run inference +# For example yolov5s +./yolov5 -d yolov5s.engine +# With an SVO file +./yolov5 -d yolov5.engine ./foo.svo +``` diff --git a/object detection/custom detector/cpp/tensorrt_yolov5/gen_wts.py b/object detection/custom detector/cpp/tensorrt_yolov5/gen_wts.py new file mode 100644 index 00000000..501fd638 --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5/gen_wts.py @@ -0,0 +1,21 @@ +import torch +import struct +import sys +from utils.torch_utils import select_device + +# Initialize +device = select_device('cpu') +pt_file = sys.argv[1] +# Load model +model = torch.load(pt_file, map_location=device)['model'].float() # load to FP32 +model.to(device).eval() + +with open(pt_file.split('.')[0] + '.wts', 'w') as f: + f.write('{}\n'.format(len(model.state_dict().keys()))) + for k, v in model.state_dict().items(): + vr = v.reshape(-1).cpu().numpy() + f.write('{} {} '.format(k, len(vr))) + for vv in vr: + f.write(' ') + f.write(struct.pack('>f',float(vv)).hex()) + f.write('\n') diff --git a/object detection/custom detector/cpp/tensorrt_yolov5/include/GLViewer.hpp b/object detection/custom detector/cpp/tensorrt_yolov5/include/GLViewer.hpp new file mode 100644 index 00000000..0a1394fc --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5/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/include/calibrator.h b/object detection/custom detector/cpp/tensorrt_yolov5/include/calibrator.h new file mode 100644 index 00000000..27f2d377 --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5/include/calibrator.h @@ -0,0 +1,45 @@ +#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 diff --git a/object detection/custom detector/cpp/tensorrt_yolov5/include/common.hpp b/object detection/custom detector/cpp/tensorrt_yolov5/include/common.hpp new file mode 100644 index 00000000..2e1a1e8b --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5/include/common.hpp @@ -0,0 +1,306 @@ +#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]) { + int 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(l, t, r - l, 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 / 2; + 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; +} + +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/include/cuda_utils.h b/object detection/custom detector/cpp/tensorrt_yolov5/include/cuda_utils.h new file mode 100644 index 00000000..8fbd3199 --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5/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/include/logging.h b/object detection/custom detector/cpp/tensorrt_yolov5/include/logging.h new file mode 100644 index 00000000..1339ee2f --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5/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/include/utils.h b/object detection/custom detector/cpp/tensorrt_yolov5/include/utils.h new file mode 100644 index 00000000..fe50d039 --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5/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/include/utils.hpp b/object detection/custom detector/cpp/tensorrt_yolov5/include/utils.hpp new file mode 100644 index 00000000..a96504f8 --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5/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/include/yololayer.h b/object detection/custom detector/cpp/tensorrt_yolov5/include/yololayer.h new file mode 100644 index 00000000..7224bde4 --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5/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/src/GLViewer.cpp b/object detection/custom detector/cpp/tensorrt_yolov5/src/GLViewer.cpp new file mode 100644 index 00000000..51265df0 --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5/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/src/calibrator.cpp b/object detection/custom detector/cpp/tensorrt_yolov5/src/calibrator.cpp new file mode 100644 index 00000000..2e2f5866 --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5/src/calibrator.cpp @@ -0,0 +1,80 @@ +#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); +} + diff --git a/object detection/custom detector/cpp/tensorrt_yolov5/src/yololayer.cu b/object detection/custom detector/cpp/tensorrt_yolov5/src/yololayer.cu new file mode 100644 index 00000000..4c042a66 --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5/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/src/yolov5.cpp b/object detection/custom detector/cpp/tensorrt_yolov5/src/yolov5.cpp new file mode 100644 index 00000000..3c43a067 --- /dev/null +++ b/object detection/custom detector/cpp/tensorrt_yolov5/src/yolov5.cpp @@ -0,0 +1,524 @@ +#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 focus0 = focus(network, weightMap, *data, 3, get_width(64, gw), 3, "model.0"); + auto conv1 = convBlock(network, weightMap, *focus0->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(9, 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 spp8 = SPP(network, weightMap, *conv7->getOutput(0), get_width(1024, gw), get_width(1024, gw), 5, 9, 13, "model.8"); + + /* ------ yolov5 head ------ */ + auto bottleneck_csp9 = C3(network, weightMap, *spp8->getOutput(0), get_width(1024, gw), get_width(1024, gw), get_depth(3, gd), false, 1, 0.5, "model.9"); + auto conv10 = convBlock(network, weightMap, *bottleneck_csp9->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; +} + +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 focus0 = focus(network, weightMap, *data, 3, get_width(64, gw), 3, "model.0"); + auto conv1 = convBlock(network, weightMap, *focus0->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(9, 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 spp10 = SPP(network, weightMap, *conv9->getOutput(0), get_width(1024, gw), get_width(1024, gw), 3, 5, 7, "model.10"); + auto c3_11 = C3(network, weightMap, *spp10->getOutput(0), get_width(1024, gw), get_width(1024, gw), get_depth(3, gd), false, 1, 0.5, "model.11"); + + /* ------ yolov5 head ------ */ + auto conv12 = convBlock(network, weightMap, *c3_11->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 (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] == '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] [s/m/l/x/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/README.md b/object detection/image viewer/README.md index 54e0d672..0d8961f3 100644 --- a/object detection/image viewer/README.md +++ b/object detection/image viewer/README.md @@ -10,4 +10,4 @@ ![](object_detection.jpg) ->*NOTE: A [ZED 2](https://store.stereolabs.com/products/zed-2) is required to use this module.* \ No newline at end of file +>*NOTE: The ZED v1 is not compatible with this module* diff --git a/object detection/image viewer/cpp/CMakeLists.txt b/object detection/image viewer/cpp/CMakeLists.txt index 9f23e241..262712c1 100644 --- a/object detection/image viewer/cpp/CMakeLists.txt +++ b/object detection/image viewer/cpp/CMakeLists.txt @@ -45,6 +45,23 @@ 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}) else() @@ -61,4 +78,4 @@ target_link_libraries(${PROJECT_NAME} if(INSTALL_SAMPLES) LIST(APPEND SAMPLE_LIST ${PROJECT_NAME}) SET(SAMPLE_LIST "${SAMPLE_LIST}" PARENT_SCOPE) -endif() \ No newline at end of file +endif() diff --git a/object detection/image viewer/cpp/README.md b/object detection/image viewer/cpp/README.md index eff65205..794ff4e7 100644 --- a/object detection/image viewer/cpp/README.md +++ b/object detection/image viewer/cpp/README.md @@ -11,7 +11,7 @@ This sample shows how to detect and track objects in space. - Build for [Linux/Jetson](https://www.stereolabs.com/docs/app-development/cpp/linux/) ## Run the program -*NOTE: A [ZED 2](https://store.stereolabs.com/products/zed-2) is required to use this module.* +*NOTE: The ZED v1 is not compatible with this module* - Navigate to the build directory and launch the executable - Or open a terminal in the build directory and run the sample : diff --git a/object detection/image viewer/csharp/CMakeLists.txt b/object detection/image viewer/csharp/CMakeLists.txt index 29a84306..3426a640 100644 --- a/object detection/image viewer/csharp/CMakeLists.txt +++ b/object detection/image viewer/csharp/CMakeLists.txt @@ -35,10 +35,12 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES "WindowsBase" ) +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.5.0" - "sl_zed_interface_3.5.0" + "Stereolabs.zed_3.6.0" ) \ 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 f7b9419e..e15b5496 100644 --- a/object detection/image viewer/csharp/MainWindow.cs +++ b/object detection/image viewer/csharp/MainWindow.cs @@ -65,16 +65,15 @@ public MainWindow(string[] args) if (err != ERROR_CODE.SUCCESS) Environment.Exit(-1); - if (!(zedCamera.CameraModel == sl.MODEL.ZED2 || zedCamera.CameraModel == sl.MODEL.ZED2i)) - { - Console.WriteLine(" ERROR : Use ZED2 Camera only"); - return; - } + if (zedCamera.CameraModel == sl.MODEL.ZED) + { + Console.WriteLine(" ERROR : not compatible camera model"); + return; + } // Enable tracking (mandatory for object detection) - Quaternion quat = Quaternion.Identity; - Vector3 vec = Vector3.Zero; - zedCamera.EnablePositionalTracking(ref quat, ref vec); + PositionalTrackingParameters trackingParams = new PositionalTrackingParameters(); + zedCamera.EnablePositionalTracking(ref trackingParams); runtimeParameters = new RuntimeParameters(); // Enable the Objects detection module @@ -154,7 +153,14 @@ public void CreateWindow() nativeWindow.Create((int)(zedCamera.ImageWidth * 0.05f), (int)(zedCamera.ImageHeight * 0.05f), (uint)width, (uint)height, NativeWindowStyle.Resizeable); nativeWindow.Show(); - nativeWindow.Run(); + try + { + nativeWindow.Run(); + } + catch (Exception e) + { + Console.WriteLine("Mouse wheel is broken in the current OPENGL .NET VERSION. Please do not use it."); + } } } diff --git a/object detection/image viewer/csharp/README.md b/object detection/image viewer/csharp/README.md index fc18b98a..e74847dc 100644 --- a/object detection/image viewer/csharp/README.md +++ b/object detection/image viewer/csharp/README.md @@ -10,7 +10,7 @@ This sample shows how to detect and track objects in space. - Build for [Windows](https://www.stereolabs.com/docs/app-development/cpp/windows/) ## Run the program -*NOTE: A [ZED 2](https://store.stereolabs.com/products/zed-2) is required to use this module.* +*NOTE: The ZED v1 is not compatible with this module* - Navigate to the build directory and launch the executable - Or open a terminal in the build directory and run the sample : diff --git a/object detection/image viewer/csharp/packages.config b/object detection/image viewer/csharp/packages.config index 0f7ea7dc..8038b422 100644 --- a/object detection/image viewer/csharp/packages.config +++ b/object detection/image viewer/csharp/packages.config @@ -1,8 +1,4 @@  - - - - \ No newline at end of file diff --git a/object detection/image viewer/python/README.md b/object detection/image viewer/python/README.md index 91a68425..a6f0fca0 100644 --- a/object detection/image viewer/python/README.md +++ b/object detection/image viewer/python/README.md @@ -7,7 +7,7 @@ This sample shows how to detect and track objects in space. - Check the [Documentation](https://www.stereolabs.com/docs/) ## Run the program -*NOTE: A [ZED 2](https://store.stereolabs.com/products/zed-2) is required to use this module.* +*NOTE: The ZED v1 is not compatible with this module* python "object_detection_image_viewer.py" diff --git a/other/multi camera/README.md b/other/multi camera/README.md index a145dadb..7d6c33f0 100644 --- a/other/multi camera/README.md +++ b/other/multi camera/README.md @@ -27,4 +27,4 @@ This sample shows how to use multiple ZED cameras in a single application. - 3 ZEDs in HD720 @ 15fps - 4 ZEDs in VGA @ 30fps - To use multiple ZED at full speed on a single computer, we recommend adding USB3.0 PCIe expansion cards. -- You can also use multiple GPUs to load-balance computations (use [`param.sdk_gpu_id`](https://www.stereolabs.com/docs/api/structsl_1_1InitParameters.html#ab85c9adecd2c9c29a1b255889be1eeb4) to select a GPU for a ZED) and improve performance. +- You can also use multiple GPUs to load-balance computations (use `param.device` to select a GPU for a ZED) and improve performance. diff --git a/plane detection/csharp/CMakeLists.txt b/plane detection/csharp/CMakeLists.txt index a2f7ecda..2c5b4eb1 100644 --- a/plane detection/csharp/CMakeLists.txt +++ b/plane detection/csharp/CMakeLists.txt @@ -35,9 +35,11 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES "WindowsBase" ) +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.5.0" - "sl_zed_interface_3.5.0" + "Stereolabs.zed_3.6.0" ) \ No newline at end of file diff --git a/plane detection/csharp/MainWindow.cs b/plane detection/csharp/MainWindow.cs index a3621898..9cbb8187 100644 --- a/plane detection/csharp/MainWindow.cs +++ b/plane detection/csharp/MainWindow.cs @@ -135,7 +135,14 @@ public void CreateWindow() nativeWindow.Create((int)(zedCamera.ImageWidth * 0.05f), (int)(zedCamera.ImageHeight * 0.05f), (uint)width, (uint)height, NativeWindowStyle.Resizeable); nativeWindow.Show(); - nativeWindow.Run(); + try + { + nativeWindow.Run(); + } + catch (Exception e) + { + Console.WriteLine("Mouse wheel is broken in the current OPENGL .NET VERSION. Please do not use it."); + } } } diff --git a/plane detection/csharp/packages.config b/plane detection/csharp/packages.config index 6579ad42..a9c5fe79 100644 --- a/plane detection/csharp/packages.config +++ b/plane detection/csharp/packages.config @@ -1,8 +1,4 @@  - - - - \ No newline at end of file diff --git a/positional tracking/csharp/CMakeLists.txt b/positional tracking/csharp/CMakeLists.txt index 70db7147..4dfca95a 100644 --- a/positional tracking/csharp/CMakeLists.txt +++ b/positional tracking/csharp/CMakeLists.txt @@ -36,9 +36,11 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES "WindowsBase" ) +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.5.0" - "sl_zed_interface_3.5.0" + "Stereolabs.zed_3.6.0" ) \ No newline at end of file diff --git a/positional tracking/csharp/MainWindow.cs b/positional tracking/csharp/MainWindow.cs index 3d9acac5..7d6cdf37 100644 --- a/positional tracking/csharp/MainWindow.cs +++ b/positional tracking/csharp/MainWindow.cs @@ -130,7 +130,14 @@ public void CreateWindow() nativeWindow.Create((int)(zedCamera.ImageWidth * 0.05f), (int)(zedCamera.ImageHeight * 0.05f), 1200, 700, NativeWindowStyle.Resizeable); nativeWindow.Show(); - nativeWindow.Run(); + try + { + nativeWindow.Run(); + } + catch (Exception e) + { + Console.WriteLine("Mouse wheel is broken in the current OPENGL .NET VERSION. Please do not use it."); + } } } diff --git a/positional tracking/csharp/packages.config b/positional tracking/csharp/packages.config index df3ea537..87967684 100644 --- a/positional tracking/csharp/packages.config +++ b/positional tracking/csharp/packages.config @@ -1,8 +1,4 @@  - - - - diff --git a/spatial mapping/basic/csharp/CMakeLists.txt b/spatial mapping/basic/csharp/CMakeLists.txt index 43d2636d..dd9f6c76 100644 --- a/spatial mapping/basic/csharp/CMakeLists.txt +++ b/spatial mapping/basic/csharp/CMakeLists.txt @@ -34,10 +34,11 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES "System.Drawing" "WindowsBase" ) +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.5.0" - "sl_zed_interface_3.5.0" + "Stereolabs.zed_3.6.0" ) \ No newline at end of file diff --git a/spatial mapping/basic/csharp/MainWindow.cs b/spatial mapping/basic/csharp/MainWindow.cs index 9be8d2d4..ada54987 100644 --- a/spatial mapping/basic/csharp/MainWindow.cs +++ b/spatial mapping/basic/csharp/MainWindow.cs @@ -95,7 +95,7 @@ public MainWindow(string[] args) spatialMappingParameters = new SpatialMappingParameters(); spatialMappingParameters.resolutionMeter = SpatialMappingParameters.get(MAPPING_RESOLUTION.MEDIUM); - spatialMappingParameters.saveTexture = false; + spatialMappingParameters.saveTexture = true; if (CREATE_MESH) spatialMappingParameters.map_type = SPATIAL_MAP_TYPE.MESH; else spatialMappingParameters.map_type = SPATIAL_MAP_TYPE.FUSED_POINT_CLOUD; @@ -162,7 +162,14 @@ public void CreateWindow() nativeWindow.Create((int)(zedCamera.ImageWidth * 0.05f), (int)(zedCamera.ImageHeight * 0.05f), (uint)width, (uint)height, NativeWindowStyle.Resizeable); nativeWindow.Show(); - nativeWindow.Run(); + try + { + nativeWindow.Run(); + } + catch (Exception e) + { + Console.WriteLine("Mouse wheel is broken in the current OPENGL .NET VERSION. Please do not use it."); + } } } diff --git a/spatial mapping/basic/csharp/packages.config b/spatial mapping/basic/csharp/packages.config index 6579ad42..a9c5fe79 100644 --- a/spatial mapping/basic/csharp/packages.config +++ b/spatial mapping/basic/csharp/packages.config @@ -1,8 +1,4 @@  - - - - \ No newline at end of file diff --git a/svo recording/export/csharp/CMakeLists.txt b/svo recording/export/csharp/CMakeLists.txt index 5a7042ae..8f06823d 100644 --- a/svo recording/export/csharp/CMakeLists.txt +++ b/svo recording/export/csharp/CMakeLists.txt @@ -27,8 +27,9 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES "WindowsBase" ) +set(CMAKE_SUPPRESS_REGENERATION true) + set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES - "Stereolabs.zed_3.5.0" - "sl_zed_interface_3.5.0" + "Stereolabs.zed_3.6.0" "OpenCvSharp4.Windows_4.5.0.20201013" ) \ No newline at end of file diff --git a/svo recording/export/csharp/packages.config b/svo recording/export/csharp/packages.config index 529deed2..8038b422 100644 --- a/svo recording/export/csharp/packages.config +++ b/svo recording/export/csharp/packages.config @@ -1,7 +1,4 @@  - - - \ No newline at end of file diff --git a/svo recording/export/python/svo_export.py b/svo recording/export/python/svo_export.py index 0bdb8bee..3a9d0417 100644 --- a/svo recording/export/python/svo_export.py +++ b/svo recording/export/python/svo_export.py @@ -135,8 +135,7 @@ def main(): nb_frames = zed.get_svo_number_of_frames() while True: - grab_result = zed.grab(rt_param) - if grab_result == sl.ERROR_CODE.SUCCESS: + if zed.grab(rt_param) == sl.ERROR_CODE.SUCCESS: svo_position = zed.get_svo_position() # Retrieve SVO images @@ -185,10 +184,6 @@ def main(): sys.stdout.write("\nSVO end has been reached. Exiting now.\n") break - else: - sys.stdout.write('\nUnexpected error: %s at frame %i / %i\n' % (grab_result, svo_position, nb_frames - 1)) - break - if output_as_video: # Close the video writer video_writer.release() diff --git a/svo recording/playback/csharp/CMakeLists.txt b/svo recording/playback/csharp/CMakeLists.txt index 269c2880..1354ac81 100644 --- a/svo recording/playback/csharp/CMakeLists.txt +++ b/svo recording/playback/csharp/CMakeLists.txt @@ -26,8 +26,9 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES "System.Numerics" ) +set(CMAKE_SUPPRESS_REGENERATION true) + set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES - "Stereolabs.zed_3.5.0" - "sl_zed_interface_3.5.0" + "Stereolabs.zed_3.6.0" "OpenCvSharp4.Windows_4.5.0.20201013" ) diff --git a/svo recording/playback/csharp/packages.config b/svo recording/playback/csharp/packages.config index 529deed2..8038b422 100644 --- a/svo recording/playback/csharp/packages.config +++ b/svo recording/playback/csharp/packages.config @@ -1,7 +1,4 @@  - - - \ No newline at end of file diff --git a/svo recording/recording/csharp/CMakeLists.txt b/svo recording/recording/csharp/CMakeLists.txt index b14554a7..771f1b34 100644 --- a/svo recording/recording/csharp/CMakeLists.txt +++ b/svo recording/recording/csharp/CMakeLists.txt @@ -27,7 +27,8 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES "WindowsBase" ) +set(CMAKE_SUPPRESS_REGENERATION true) + set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES - "Stereolabs.zed_3.5.0" - "sl_zed_interface_3.5.0" + "Stereolabs.zed_3.6.0" ) \ No newline at end of file diff --git a/svo recording/recording/csharp/packages.config b/svo recording/recording/csharp/packages.config index 8047d6ee..8038b422 100644 --- a/svo recording/recording/csharp/packages.config +++ b/svo recording/recording/csharp/packages.config @@ -1,6 +1,4 @@  - - \ No newline at end of file diff --git a/tutorials/tutorial 1 - hello ZED/c/CMakeLists.txt b/tutorials/tutorial 1 - hello ZED/c/CMakeLists.txt new file mode 100644 index 00000000..64f8aeb3 --- /dev/null +++ b/tutorials/tutorial 1 - hello ZED/c/CMakeLists.txt @@ -0,0 +1,22 @@ +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) +PROJECT(ZED_C_Tutorial_1) + +if(COMMAND cmake_policy) + cmake_policy(SET CMP0003 OLD) + cmake_policy(SET CMP0015 OLD) +endif(COMMAND cmake_policy) + +SET(EXECUTABLE_OUTPUT_PATH ".") + +if (WIN32) +include_directories($ENV{ZED_SDK_ROOT_DIR}/include/) +link_directories($ENV{ZED_SDK_ROOT_DIR}/lib/) +else (WIN32) +include_directories("/usr/local/zed/include/") +link_directories("/usr/local/zed/lib/") +endif(WIN32) + + +ADD_EXECUTABLE(${PROJECT_NAME} main.c) + +target_link_libraries(${PROJECT_NAME} sl_zed_c) diff --git a/tutorials/tutorial 1 - hello ZED/c/README.md b/tutorials/tutorial 1 - hello ZED/c/README.md new file mode 100644 index 00000000..1109f30c --- /dev/null +++ b/tutorials/tutorial 1 - hello ZED/c/README.md @@ -0,0 +1,95 @@ +# Tutorial 1: Hello ZED + +This tutorial simply shows how to configure and open the ZED, then print its serial number and then close the camera. This is the most basic step and a good start for using the ZED SDK. + +## Getting started + +- First, download the latest version of the ZED SDK on [stereolabs.com](https://www.stereolabs.com). +- For more information, read the ZED [API documentation](https://www.stereolabs.com/developers/documentation/API/). + +### Prerequisites + +- Windows 10, Ubuntu LTS, L4T +- [ZED SDK](https://www.stereolabs.com/developers/) and its dependencies ([CUDA](https://developer.nvidia.com/cuda-downloads)) +- ZED SDK C API + +# Code overview +The ZED API provides low-level access to camera control and configuration. To use the ZED in your application, you will need to create and open a Camera object. The API can be used with two different video inputs: the ZED live video (Live mode) or video files recorded in SVO format with the ZED API (Playback mode). + +## Camera Configuration +To configure the camera, create a Camera object and specify your `SL_InitParameters`. Initial parameters let you adjust camera resolution, FPS, depth sensing parameters and more. These parameters can only be set before opening the camera and cannot be changed while the camera is in use. + +```c +//Create a ZED camera object +int camera_id = 0; +sl_create_camera(camera_id); + +//Set configuration parameters +struct SL_InitParameters init_param; +init_param.camera_fps = 30; +init_param.resolution = SL_RESOLUTION_HD1080; +init_param.input_type = SL_INPUT_TYPE_USB; +init_param.camera_device_id = camera_id; +init_param.camera_image_flip = SL_FLIP_MODE_AUTO; +init_param.camera_disable_self_calib = false; +init_param.enable_image_enhancement = true; +init_param.svo_real_time_mode = true; +init_param.depth_mode = SL_DEPTH_MODE_PERFORMANCE; +init_param.depth_stabilization = true; +init_param.depth_maximum_distance = 40; +init_param.depth_minimum_distance = -1; +init_param.coordinate_unit = SL_UNIT_METER; +init_param.coordinate_system = SL_COORDINATE_SYSTEM_LEFT_HANDED_Y_UP; +init_param.sdk_gpu_id = -1; +init_param.sdk_verbose = false; +init_param.sensors_required = false; +init_param.enable_right_side_measure = false; +``` +Once initial configuration is done, open the camera. + +```c +# Open the camera +int state = sl_open_camera(camera_id, &init_param, "", "", "", "", "", ""); + + if (state != 0) { + printf("Error Open \n"); + return 1; + } +``` + +You can set the following initial parameters: +* Camera configuration parameters, using the `camera_*` entries (resolution, image flip...). +* SDK configuration parameters, using the `sdk_*` entries (verbosity, GPU device used...). +* Depth configuration parameters, using the `depth_*` entries (depth mode, minimum distance...). +* Coordinate frames configuration parameters, using the `coordinate_*` entries (coordinate system, coordinate units...). +* SVO parameters to use Stereolabs video files with the ZED SDK (filename, real-time mode...) + + +### Getting Camera Information +Camera parameters such as focal length, field of view or stereo calibration can be retrieved for each eye and resolution: + +- Focal length: fx, fy. +- Principal points: cx, cy. +- Lens distortion: k1, k2. +- Horizontal and vertical field of view. +- Stereo calibration: rotation and translation between left and right eye. + +In this tutorial, we simply retrieve the serial number of the camera: + +```c +// Get camera information (serial number) +int sn = sl_get_zed_serial(camera_id); +printf("Hello! This is my serial number: %d\n", sn); +``` + +In the console window, you should now see the serial number of the camera (also available on a sticker on the ZED USB cable). + + Note: `CameraInformation` also contains the firmware version of the ZED, as well as calibration parameters. + +To close the camera properly, use zed.close() and exit the program. + +```c +# Close the camera +sl_close_camera(camera_id); + return 0; +``` diff --git a/tutorials/tutorial 1 - hello ZED/c/main.c b/tutorials/tutorial 1 - hello ZED/c/main.c new file mode 100644 index 00000000..c9a22839 --- /dev/null +++ b/tutorials/tutorial 1 - hello ZED/c/main.c @@ -0,0 +1,69 @@ +/////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2021, STEREOLABS. +// +// All rights reserved. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +/////////////////////////////////////////////////////////////////////////// + + +#include +#include +#include + + +int main(int argc, char **argv) { + + // Create a ZED camera object + int camera_id = 0; + sl_create_camera(camera_id); + + struct SL_InitParameters init_param; + init_param.camera_fps = 30; + init_param.resolution = SL_RESOLUTION_HD1080; + init_param.input_type = SL_INPUT_TYPE_USB; + init_param.camera_device_id = camera_id; + init_param.camera_image_flip = SL_FLIP_MODE_AUTO; + init_param.camera_disable_self_calib = false; + init_param.enable_image_enhancement = true; + init_param.svo_real_time_mode = true; + init_param.depth_mode = SL_DEPTH_MODE_PERFORMANCE; + init_param.depth_stabilization = true; + init_param.depth_maximum_distance = 40; + init_param.depth_minimum_distance = -1; + init_param.coordinate_unit = SL_UNIT_METER; + init_param.coordinate_system = SL_COORDINATE_SYSTEM_LEFT_HANDED_Y_UP; + init_param.sdk_gpu_id = -1; + init_param.sdk_verbose = false; + init_param.sensors_required = false; + init_param.enable_right_side_measure = false; + + // Open the camera + int state = sl_open_camera(camera_id, &init_param, "", "", 0, "", "", ""); + + if (state != 0) { + printf("Error Open: %i \n", state); + return 1; + } + + // Get camera information (ZED serial number) + int sn = sl_get_zed_serial(camera_id); + printf("Hello! This is my serial number: %d\n", sn); + + + sl_close_camera(camera_id); + return 0; +} + diff --git a/tutorials/tutorial 1 - hello ZED/csharp/CMakeLists.txt b/tutorials/tutorial 1 - hello ZED/csharp/CMakeLists.txt index 5ebb47b2..cc730039 100644 --- a/tutorials/tutorial 1 - hello ZED/csharp/CMakeLists.txt +++ b/tutorials/tutorial 1 - hello ZED/csharp/CMakeLists.txt @@ -26,7 +26,11 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES "System.Numerics" ) -set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES - "Stereolabs.zed_3.5.0" - "sl_zed_interface_3.5.0" -) \ No newline at end of file +set(CMAKE_SUPPRESS_REGENERATION true) +set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES + "Stereolabs.zed_3.6.0" +<<<<<<< HEAD +) +======= +) +>>>>>>> release_3.6 diff --git a/tutorials/tutorial 1 - hello ZED/csharp/packages.config b/tutorials/tutorial 1 - hello ZED/csharp/packages.config index 8047d6ee..8038b422 100644 --- a/tutorials/tutorial 1 - hello ZED/csharp/packages.config +++ b/tutorials/tutorial 1 - hello ZED/csharp/packages.config @@ -1,6 +1,4 @@  - - \ No newline at end of file diff --git a/tutorials/tutorial 2 - image capture/c/CMakeLists.txt b/tutorials/tutorial 2 - image capture/c/CMakeLists.txt new file mode 100644 index 00000000..5e2ad195 --- /dev/null +++ b/tutorials/tutorial 2 - image capture/c/CMakeLists.txt @@ -0,0 +1,22 @@ +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) +PROJECT(ZED_C_Tutorial_2) + +if(COMMAND cmake_policy) + cmake_policy(SET CMP0003 OLD) + cmake_policy(SET CMP0015 OLD) +endif(COMMAND cmake_policy) + +SET(EXECUTABLE_OUTPUT_PATH ".") + +if (WIN32) +include_directories($ENV{ZED_SDK_ROOT_DIR}/include/) +link_directories($ENV{ZED_SDK_ROOT_DIR}/lib/) +else (WIN32) +include_directories("/usr/local/zed/include/") +link_directories("/usr/local/zed/lib/") +endif(WIN32) + + +ADD_EXECUTABLE(${PROJECT_NAME} main.c) + +target_link_libraries(${PROJECT_NAME} sl_zed_c) diff --git a/tutorials/tutorial 2 - image capture/c/main.c b/tutorials/tutorial 2 - image capture/c/main.c new file mode 100644 index 00000000..79e123fb --- /dev/null +++ b/tutorials/tutorial 2 - image capture/c/main.c @@ -0,0 +1,96 @@ +/////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2021, STEREOLABS. +// +// All rights reserved. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +/////////////////////////////////////////////////////////////////////////// + + +#include +#include +#include + + +int main(int argc, char **argv) { + + // Create a ZED camera object + int camera_id = 0; + sl_create_camera(camera_id); + + struct SL_InitParameters init_param; + init_param.camera_fps = 30; + init_param.resolution = SL_RESOLUTION_HD1080; + init_param.input_type = SL_INPUT_TYPE_USB; + init_param.camera_device_id = camera_id; + init_param.camera_image_flip = SL_FLIP_MODE_AUTO; + init_param.camera_disable_self_calib = false; + init_param.enable_image_enhancement = true; + init_param.svo_real_time_mode = true; + init_param.depth_mode = SL_DEPTH_MODE_PERFORMANCE; + init_param.depth_stabilization = true; + init_param.depth_maximum_distance = 40; + init_param.depth_minimum_distance = -1; + init_param.coordinate_unit = SL_UNIT_METER; + init_param.coordinate_system = SL_COORDINATE_SYSTEM_LEFT_HANDED_Y_UP; + init_param.sdk_gpu_id = -1; + init_param.sdk_verbose = false; + init_param.sensors_required = false; + init_param.enable_right_side_measure = false; + + // Open the camera + int state = sl_open_camera(camera_id, &init_param, "", "", 0, "", "", ""); + + if (state != 0) { + printf("Error Open \n"); + return 1; + } + + struct SL_RuntimeParameters rt_param; + rt_param.enable_depth = true; + rt_param.confidence_threshold = 100; + rt_param.reference_frame = SL_REFERENCE_FRAME_CAMERA; + rt_param.sensing_mode = SL_SENSING_MODE_STANDARD; + rt_param.texture_confidence_threshold = 100; + + int width = sl_get_width(camera_id); + int height = sl_get_height(camera_id); + + //Create image ptr. + int* image_ptr; + // Init pointer. + image_ptr = sl_mat_create_new(width, height, SL_MAT_TYPE_U8_C4, SL_MEM_CPU); + + // Capture 50 frames and stop + int i = 0; + while (i < 50) { + // Grab an image + state = sl_grab(camera_id, &rt_param); + // A new image is available if grab() returns ERROR_CODE::SUCCESS + if (state == 0) { + + // Get the left image + sl_retrieve_image(camera_id, image_ptr, SL_VIEW_LEFT, SL_MEM_CPU, width, height); + + // Display the image resolution and its acquisition timestamp + printf("Image resolution: %i x %i || %llu \n", width, height, sl_get_current_timestamp(camera_id)); + i++; + } + } + + sl_close_camera(camera_id); + return 0; +} + diff --git a/tutorials/tutorial 2 - image capture/csharp/CMakeLists.txt b/tutorials/tutorial 2 - image capture/csharp/CMakeLists.txt index 07bf87ff..a67cc934 100644 --- a/tutorials/tutorial 2 - image capture/csharp/CMakeLists.txt +++ b/tutorials/tutorial 2 - image capture/csharp/CMakeLists.txt @@ -26,7 +26,7 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES "System.Numerics" ) -set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES - "Stereolabs.zed_3.5.0" - "sl_zed_interface_3.5.0" +set(CMAKE_SUPPRESS_REGENERATION true) +set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES + "Stereolabs.zed_3.6.0" ) \ No newline at end of file diff --git a/tutorials/tutorial 2 - image capture/csharp/packages.config b/tutorials/tutorial 2 - image capture/csharp/packages.config index 8047d6ee..8038b422 100644 --- a/tutorials/tutorial 2 - image capture/csharp/packages.config +++ b/tutorials/tutorial 2 - image capture/csharp/packages.config @@ -1,6 +1,4 @@  - - \ No newline at end of file diff --git a/tutorials/tutorial 3 - depth sensing/c/CMakeLists.txt b/tutorials/tutorial 3 - depth sensing/c/CMakeLists.txt new file mode 100644 index 00000000..efe30c1d --- /dev/null +++ b/tutorials/tutorial 3 - depth sensing/c/CMakeLists.txt @@ -0,0 +1,22 @@ +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) +PROJECT(ZED_C_Tutorial_3) + +if(COMMAND cmake_policy) + cmake_policy(SET CMP0003 OLD) + cmake_policy(SET CMP0015 OLD) +endif(COMMAND cmake_policy) + +SET(EXECUTABLE_OUTPUT_PATH ".") + +if (WIN32) +include_directories($ENV{ZED_SDK_ROOT_DIR}/include/) +link_directories($ENV{ZED_SDK_ROOT_DIR}/lib/) +else (WIN32) +include_directories("/usr/local/zed/include/") +link_directories("/usr/local/zed/lib/") +endif(WIN32) + + +ADD_EXECUTABLE(${PROJECT_NAME} main.c) + +target_link_libraries(${PROJECT_NAME} sl_zed_c) diff --git a/tutorials/tutorial 3 - depth sensing/c/main.c b/tutorials/tutorial 3 - depth sensing/c/main.c new file mode 100644 index 00000000..8de8b0ca --- /dev/null +++ b/tutorials/tutorial 3 - depth sensing/c/main.c @@ -0,0 +1,122 @@ +/////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2021, STEREOLABS. +// +// All rights reserved. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +/////////////////////////////////////////////////////////////////////////// + +#include +#include +#include + +#include + +int main(int argc, char **argv) { + + // Create a ZED camera object + int camera_id = 0; + sl_create_camera(camera_id); + + struct SL_InitParameters init_param; + init_param.camera_fps = 30; + init_param.resolution = SL_RESOLUTION_HD1080; + init_param.input_type = SL_INPUT_TYPE_USB; + init_param.camera_device_id = camera_id; + init_param.camera_image_flip = SL_FLIP_MODE_AUTO; + init_param.camera_disable_self_calib = false; + init_param.enable_image_enhancement = true; + init_param.svo_real_time_mode = true; + init_param.depth_mode = SL_DEPTH_MODE_PERFORMANCE; + init_param.depth_stabilization = true; + init_param.depth_maximum_distance = 40; + init_param.depth_minimum_distance = -1; + init_param.coordinate_unit = SL_UNIT_METER; + init_param.coordinate_system = SL_COORDINATE_SYSTEM_LEFT_HANDED_Y_UP; + init_param.sdk_gpu_id = -1; + init_param.sdk_verbose = false; + init_param.sensors_required = false; + init_param.enable_right_side_measure = false; + + // Open the camera + int state = sl_open_camera(camera_id, &init_param, "", "", 0, "", "", ""); + + if (state != 0) { + printf("Error Open \n"); + return 1; + } + + struct SL_RuntimeParameters rt_param; + rt_param.enable_depth = true; + rt_param.confidence_threshold = 100; + rt_param.reference_frame = SL_REFERENCE_FRAME_CAMERA; + rt_param.sensing_mode = SL_SENSING_MODE_STANDARD; + rt_param.texture_confidence_threshold = 100; + + int width = sl_get_width(camera_id); + int height = sl_get_height(camera_id); + + //Create image ptr. + int* image_ptr; + // Init pointer. + image_ptr = sl_mat_create_new(width, height, SL_MAT_TYPE_U8_C4, SL_MEM_CPU); + //Create depth ptr. + int* depth_ptr; + // Init pointer. + depth_ptr = sl_mat_create_new(width, height, SL_MAT_TYPE_F32_C4, SL_MEM_CPU); + //Create point cloud ptr. + int* point_cloud_ptr; + // Init pointer. + point_cloud_ptr = sl_mat_create_new(width, height, SL_MAT_TYPE_F32_C4, SL_MEM_CPU); + + // Capture 50 frames and stop + int i = 0; + while (i < 50) { + // Grab an image + state = sl_grab(camera_id, &rt_param); + // A new image is available if grab() returns ERROR_CODE::SUCCESS + if (state == 0) { + + // Retrieve left image + sl_retrieve_image(camera_id, image_ptr, SL_VIEW_LEFT, SL_MEM_CPU, width, height); + // Retrieve depth map. Depth is aligned on the left image + sl_retrieve_measure(camera_id, depth_ptr, SL_MEASURE_DEPTH, SL_MEM_CPU, width, height); + // Retrieve colored point cloud. Point cloud is aligned on the left image. + sl_retrieve_measure(camera_id, point_cloud_ptr, SL_MEASURE_XYZRGBA, SL_MEM_CPU, width, height); + + // Get and print distance value in mm at the center of the image + // We measure the distance camera - object using Euclidean distance + int x = sl_mat_get_width(point_cloud_ptr) / 2; + int y = sl_mat_get_height(point_cloud_ptr) / 2; + + struct SL_Vector4 point_cloud_value; + sl_mat_get_value_float4(point_cloud_ptr, x, y, &point_cloud_value, SL_MEM_CPU); + + if (isfinite(point_cloud_value.z)) { + float distance = sqrt(point_cloud_value.x * point_cloud_value.x + point_cloud_value.y * point_cloud_value.y + point_cloud_value.z * point_cloud_value.z); + printf("Distance to the Camera at {%f; %f; %f}: %f mm \n", point_cloud_value.x, point_cloud_value.y, point_cloud_value.z, distance); + } + else { + printf("Distance can not be computed at {%i; %i} \n", x, y); + } + + i++; + } + } + + sl_close_camera(camera_id); + return 0; +} + diff --git a/tutorials/tutorial 3 - depth sensing/csharp/CMakeLists.txt b/tutorials/tutorial 3 - depth sensing/csharp/CMakeLists.txt index 5736da68..cae5249e 100644 --- a/tutorials/tutorial 3 - depth sensing/csharp/CMakeLists.txt +++ b/tutorials/tutorial 3 - depth sensing/csharp/CMakeLists.txt @@ -26,7 +26,7 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES "System.Numerics" ) -set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES - "Stereolabs.zed_3.5.0" - "sl_zed_interface_3.5.0" +set(CMAKE_SUPPRESS_REGENERATION true) +set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES + "Stereolabs.zed_3.6.0" ) \ No newline at end of file diff --git a/tutorials/tutorial 3 - depth sensing/csharp/packages.config b/tutorials/tutorial 3 - depth sensing/csharp/packages.config index 8047d6ee..8038b422 100644 --- a/tutorials/tutorial 3 - depth sensing/csharp/packages.config +++ b/tutorials/tutorial 3 - depth sensing/csharp/packages.config @@ -1,6 +1,4 @@  - - \ No newline at end of file diff --git a/tutorials/tutorial 4 - positional tracking/c/CMakeLists.txt b/tutorials/tutorial 4 - positional tracking/c/CMakeLists.txt new file mode 100644 index 00000000..e5151e5e --- /dev/null +++ b/tutorials/tutorial 4 - positional tracking/c/CMakeLists.txt @@ -0,0 +1,21 @@ +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) +PROJECT(ZED_C_Tutorial_4) + +if(COMMAND cmake_policy) + cmake_policy(SET CMP0003 OLD) + cmake_policy(SET CMP0015 OLD) +endif(COMMAND cmake_policy) + +SET(EXECUTABLE_OUTPUT_PATH ".") + +if (WIN32) +include_directories($ENV{ZED_SDK_ROOT_DIR}/include/) +link_directories($ENV{ZED_SDK_ROOT_DIR}/lib/) +else (WIN32) +include_directories("/usr/local/zed/include/") +link_directories("/usr/local/zed/lib/") +endif(WIN32) + +ADD_EXECUTABLE(${PROJECT_NAME} main.c) + +target_link_libraries(${PROJECT_NAME} sl_zed_c) \ No newline at end of file diff --git a/tutorials/tutorial 4 - positional tracking/c/main.c b/tutorials/tutorial 4 - positional tracking/c/main.c new file mode 100644 index 00000000..d0e11b75 --- /dev/null +++ b/tutorials/tutorial 4 - positional tracking/c/main.c @@ -0,0 +1,138 @@ +/////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2021, STEREOLABS. +// +// All rights reserved. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +/////////////////////////////////////////////////////////////////////////// + + +#include +#include +#include + + +int main(int argc, char **argv) { + + // Create a ZED camera object + int camera_id = 0; + sl_create_camera(camera_id); + + struct SL_InitParameters init_param; + init_param.camera_fps = 30; + init_param.resolution = SL_RESOLUTION_HD1080; + init_param.input_type = SL_INPUT_TYPE_USB; + init_param.camera_device_id = camera_id; + init_param.camera_image_flip = SL_FLIP_MODE_AUTO; + init_param.camera_disable_self_calib = false; + init_param.enable_image_enhancement = true; + init_param.svo_real_time_mode = true; + init_param.depth_mode = SL_DEPTH_MODE_PERFORMANCE; + init_param.depth_stabilization = true; + init_param.depth_maximum_distance = 40; + init_param.depth_minimum_distance = -1; + init_param.coordinate_unit = SL_UNIT_METER; + init_param.coordinate_system = SL_COORDINATE_SYSTEM_LEFT_HANDED_Y_UP; + init_param.sdk_gpu_id = -1; + init_param.sdk_verbose = false; + init_param.sensors_required = false; + init_param.enable_right_side_measure = false; + + // Open the camera + int state = sl_open_camera(camera_id, &init_param, "", "", 0, "", "", ""); + + if (state != 0) { + printf("Error Open \n"); + return 1; + } + + //Enable Positional tracking + struct SL_PositionalTrackingParameters tracking_param; + tracking_param.enable_area_memory = true; + tracking_param.enable_imu_fusion = true; + tracking_param.enable_pose_smothing = false; + + struct SL_Vector3 position; + position = (struct SL_Vector3) { .x = 0, .y = 0, .z = 0 }; + struct SL_Quaternion rotation; + rotation = (struct SL_Quaternion) { .x = 0, .y = 0, .z = 0, .w = 1 }; + + tracking_param.initial_world_position = position; + tracking_param.initial_world_rotation = rotation; + tracking_param.set_as_static = false; + tracking_param.set_floor_as_origin = false; + + state = sl_enable_positional_tracking(camera_id, &tracking_param, ""); + if (state != 0) { + printf("Error Enable Tracking %i , exit program.\n", state); + return 0; + } + + struct SL_RuntimeParameters rt_param; + rt_param.enable_depth = true; + rt_param.confidence_threshold = 100; + rt_param.reference_frame = SL_REFERENCE_FRAME_CAMERA; + rt_param.sensing_mode = SL_SENSING_MODE_STANDARD; + rt_param.texture_confidence_threshold = 100; + + int width = sl_get_width(camera_id); + int height = sl_get_height(camera_id); + + bool zed_has_imu = sl_get_sensors_configuration(camera_id)->gyroscope_parameters.is_available; + + //Create image ptr. + int* image_ptr; + // Init pointer. + image_ptr = sl_mat_create_new(width, height, SL_MAT_TYPE_U8_C4, SL_MEM_CPU); + + // Capture 1000 frames and stop + int i = 0; + while (i < 1000) { + // Grab an image + state = sl_grab(camera_id, &rt_param); + // A new image is available if grab() returns ERROR_CODE::SUCCESS + if (state == 0) { + + struct SL_PoseData pose; + sl_get_position_data(camera_id, &pose, SL_REFERENCE_FRAME_WORLD); + + struct SL_Vector3 zed_translation = pose.translation; + struct SL_Quaternion zed_orientation = pose.rotation; + + unsigned long long ts = pose.timestamp; + + printf("Camera Translation: {%f, %f, %f}, Orientation: {%f, %f, %f, %f}, timestamp: %lld \n", zed_translation.x, zed_translation.y, zed_translation.z, + zed_orientation.x, zed_orientation.y, zed_orientation.z, zed_orientation.w, ts); + + if (zed_has_imu) { + + struct SL_SensorData sensor_data; + sl_get_sensors_data(camera_id, &sensor_data, SL_TIME_REFERENCE_IMAGE); + + struct SL_Quaternion imu_orientation = sensor_data.imu.orientation; + struct SL_Vector3 acceleration = sensor_data.imu.linear_acceleration; + + printf("IMU Orientation: {%f, %f, %f, %f}, Acceleration: {%f, %f, %f}", imu_orientation.x, imu_orientation.y, imu_orientation.z, imu_orientation.w, + acceleration.x, acceleration.y, acceleration.z); + } + + i++; + } + } + + sl_disable_positional_tracking(camera_id, ""); + sl_close_camera(camera_id); + return 0; +} diff --git a/tutorials/tutorial 4 - positional tracking/csharp/CMakeLists.txt b/tutorials/tutorial 4 - positional tracking/csharp/CMakeLists.txt index 599b7148..af942a0a 100644 --- a/tutorials/tutorial 4 - positional tracking/csharp/CMakeLists.txt +++ b/tutorials/tutorial 4 - positional tracking/csharp/CMakeLists.txt @@ -26,7 +26,7 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES "System.Numerics" ) -set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES - "Stereolabs.zed_3.5.0" - "sl_zed_interface_3.5.0" +set(CMAKE_SUPPRESS_REGENERATION true) +set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES + "Stereolabs.zed_3.6.0" ) \ No newline at end of file diff --git a/tutorials/tutorial 4 - positional tracking/csharp/Program.cs b/tutorials/tutorial 4 - positional tracking/csharp/Program.cs index 092bbaa7..a522521e 100644 --- a/tutorials/tutorial 4 - positional tracking/csharp/Program.cs +++ b/tutorials/tutorial 4 - positional tracking/csharp/Program.cs @@ -40,9 +40,10 @@ static void Main(string[] args) zedCamera.GetPosition(ref pose,REFERENCE_FRAME.WORLD); // Display the translation and timestamp each 10 frames - if (i%10==0) - Console.WriteLine("Translation : " + pose.translation + ", Rotation : " + pose.rotation + ", Timestamp : " + pose.timestamp); - + if (i % 10 == 0) + { + Console.WriteLine("Translation : " + pose.translation + ",\n Rotation : " + pose.rotation + ",\n Timestamp : " + pose.timestamp); + } i++; } } diff --git a/tutorials/tutorial 4 - positional tracking/csharp/packages.config b/tutorials/tutorial 4 - positional tracking/csharp/packages.config index 8047d6ee..8038b422 100644 --- a/tutorials/tutorial 4 - positional tracking/csharp/packages.config +++ b/tutorials/tutorial 4 - positional tracking/csharp/packages.config @@ -1,6 +1,4 @@  - - \ No newline at end of file diff --git a/tutorials/tutorial 5 - spatial mapping/c/CMakeLists.txt b/tutorials/tutorial 5 - spatial mapping/c/CMakeLists.txt new file mode 100644 index 00000000..a7c56afb --- /dev/null +++ b/tutorials/tutorial 5 - spatial mapping/c/CMakeLists.txt @@ -0,0 +1,21 @@ +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) +PROJECT(ZED_C_Tutorial_5 C) + +if(COMMAND cmake_policy) + cmake_policy(SET CMP0003 OLD) + cmake_policy(SET CMP0015 OLD) +endif(COMMAND cmake_policy) + +SET(EXECUTABLE_OUTPUT_PATH ".") + +if (WIN32) +include_directories($ENV{ZED_SDK_ROOT_DIR}/include/) +link_directories($ENV{ZED_SDK_ROOT_DIR}/lib/) +else (WIN32) +include_directories("/usr/local/zed/include/") +link_directories("/usr/local/zed/lib/") +endif(WIN32) + +ADD_EXECUTABLE(${PROJECT_NAME} main.c) + +target_link_libraries(${PROJECT_NAME} sl_zed_c) diff --git a/tutorials/tutorial 5 - spatial mapping/c/main.c b/tutorials/tutorial 5 - spatial mapping/c/main.c new file mode 100644 index 00000000..c402b561 --- /dev/null +++ b/tutorials/tutorial 5 - spatial mapping/c/main.c @@ -0,0 +1,147 @@ +/////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2021, STEREOLABS. +// +// All rights reserved. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +/////////////////////////////////////////////////////////////////////////// + + +#include +#include +#include +#include + +int main() { + + // Create a ZED camera object + int camera_id = 0; + sl_create_camera(camera_id); + + struct SL_InitParameters init_param; + init_param.camera_fps = 30; + init_param.resolution = SL_RESOLUTION_HD1080; + init_param.input_type = SL_INPUT_TYPE_USB; + init_param.camera_device_id = camera_id; + init_param.camera_image_flip = SL_FLIP_MODE_AUTO; + init_param.camera_disable_self_calib = false; + init_param.enable_image_enhancement = true; + init_param.svo_real_time_mode = true; + init_param.depth_mode = SL_DEPTH_MODE_PERFORMANCE; + init_param.depth_stabilization = true; + init_param.depth_maximum_distance = 40; + init_param.depth_minimum_distance = -1; + init_param.coordinate_unit = SL_UNIT_METER; + init_param.coordinate_system = SL_COORDINATE_SYSTEM_LEFT_HANDED_Y_UP; + init_param.sdk_gpu_id = -1; + init_param.sdk_verbose = false; + init_param.sensors_required = false; + init_param.enable_right_side_measure = false; + + // Open the camera + int state = sl_open_camera(camera_id, &init_param, "", "", 0, "", "", ""); + + if (state != 0) { + printf("Error Open \n"); + return 1; + } + + //Enable Positional tracking + struct SL_PositionalTrackingParameters tracking_param; + tracking_param.enable_area_memory = true; + tracking_param.enable_imu_fusion = true; + tracking_param.enable_pose_smothing = false; + + struct SL_Vector3 position; + position = (struct SL_Vector3) { .x = 0, .y = 0, .z = 0 }; + struct SL_Quaternion rotation; + rotation = (struct SL_Quaternion) { .x = 0, .y = 0, .z = 0, .w = 1 }; + + tracking_param.initial_world_position = position; + tracking_param.initial_world_rotation = rotation; + tracking_param.set_as_static = false; + tracking_param.set_floor_as_origin = false; + + state = sl_enable_positional_tracking(camera_id, &tracking_param, ""); + if (state != 0) { + printf("Error Enable Tracking %i , exit program.\n", state); + return 0; + } + + struct SL_SpatialMappingParameters mapping_param; + mapping_param.map_type = SL_SPATIAL_MAP_TYPE_MESH; + mapping_param.max_memory_usage = 2048; + mapping_param.range_meter = 0; + mapping_param.resolution_meter = 0.05f; + mapping_param.save_texture = true; + mapping_param.use_chunk_only = true; + mapping_param.reverse_vertex_order = false; + + sl_enable_spatial_mapping(camera_id, &mapping_param); + + struct SL_RuntimeParameters rt_param; + rt_param.enable_depth = true; + rt_param.confidence_threshold = 100; + rt_param.reference_frame = SL_REFERENCE_FRAME_CAMERA; + rt_param.sensing_mode = SL_SENSING_MODE_STANDARD; + rt_param.texture_confidence_threshold = 100; + + int width = sl_get_width(camera_id); + int height = sl_get_height(camera_id); + + int i = 1; + // Grab data during 500 frames + while (i <= 500) { + // Grab an image + state = sl_grab(camera_id, &rt_param); + // A new image is available if grab() returns ERROR_CODE::SUCCESS + if (state == 0) { + + // In the background, spatial mapping will use newly retrieved images, depth and pose to update the mesh + enum SL_SPATIAL_MAPPING_STATE map_state = sl_get_spatial_mapping_state(camera_id); + + printf("\r Images captured: %i / 500 || Spatial mapping state : %i \t \n", i, map_state); + i++; + } + } + + printf("Extracting Mesh...\n"); + // Extract the whole mesh. + sl_extract_whole_spatial_map(camera_id); + // Filter the mesh + + int *nb_vertices_per_submesh = (int *)malloc(sizeof(int[MAX_SUBMESH])); + int *nb_triangles_per_submesh = (int *)malloc(sizeof(int[MAX_SUBMESH])); + int *updated_indices = (int *)malloc(sizeof(int[MAX_SUBMESH])); + int nb_updated_submeshes = 0; + int nb_vertices_tot = 0; + int nb_triangles_tot = 0; + + printf("Filtering Mesh...\n"); + sl_filter_mesh(camera_id, SL_MESH_FILTER_MEDIUM, nb_vertices_per_submesh, nb_triangles_per_submesh, &nb_updated_submeshes, updated_indices, &nb_vertices_tot, &nb_triangles_tot, MAX_SUBMESH); + + // Save the mesh + printf("Saving Mesh ... \n"); + sl_save_mesh(camera_id, "mesh.obj", SL_MESH_FILE_FORMAT_OBJ); + + free(nb_vertices_per_submesh); + free(nb_triangles_per_submesh); + free(updated_indices); + + sl_disable_spatial_mapping(camera_id); + sl_disable_positional_tracking(camera_id, ""); + sl_close_camera(camera_id); + return 0; +} diff --git a/tutorials/tutorial 5 - spatial mapping/csharp/CMakeLists.txt b/tutorials/tutorial 5 - spatial mapping/csharp/CMakeLists.txt index 6a8fbfbd..222c9e25 100644 --- a/tutorials/tutorial 5 - spatial mapping/csharp/CMakeLists.txt +++ b/tutorials/tutorial 5 - spatial mapping/csharp/CMakeLists.txt @@ -26,8 +26,8 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES "System.Numerics" ) -set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES - "Stereolabs.zed_3.5.0" - "sl_zed_interface_3.5.0" +set(CMAKE_SUPPRESS_REGENERATION true) +set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES + "Stereolabs.zed_3.6.0" ) \ No newline at end of file diff --git a/tutorials/tutorial 5 - spatial mapping/csharp/Program.cs b/tutorials/tutorial 5 - spatial mapping/csharp/Program.cs index f73244ac..352590ad 100644 --- a/tutorials/tutorial 5 - spatial mapping/csharp/Program.cs +++ b/tutorials/tutorial 5 - spatial mapping/csharp/Program.cs @@ -19,25 +19,41 @@ static void Main(string[] args) if (err != ERROR_CODE.SUCCESS) Environment.Exit(-1); - SensorsData sensors_data = new SensorsData(); - ulong last_imu_timestamp = 0; + // Configure spatial mapping parameters + SpatialMappingParameters mappingParams = new SpatialMappingParameters(); + mappingParams.resolutionMeter = SpatialMappingParameters.get(MAPPING_RESOLUTION.LOW); + mappingParams.rangeMeter = SpatialMappingParameters.get(MAPPING_RANGE.FAR); + mappingParams.saveTexture = false; + + //Enable tracking and mapping + PositionalTrackingParameters trackingParams = new PositionalTrackingParameters(); + zed.EnablePositionalTracking(ref trackingParams); + zed.EnableSpatialMapping(ref mappingParams); RuntimeParameters runtimeParameters = new RuntimeParameters(); - while (zed.Grab(ref runtimeParameters) == ERROR_CODE.SUCCESS) + + int i = 0; + Mesh mesh = new Mesh(); + + // Grab 500 frames and stop + while (i < 500) { - zed.GetSensorsData(ref sensors_data, TIME_REFERENCE.CURRENT); - if (sensors_data.imu.timestamp > last_imu_timestamp) + if (zed.Grab(ref runtimeParameters) == ERROR_CODE.SUCCESS) { - // Show Sensors Data - Console.WriteLine("IMU Orientation : " + sensors_data.imu.fusedOrientation); - Console.WriteLine("Angular Velocity : " + sensors_data.imu.angularVelocity); - Console.WriteLine("Magnetometer Magnetic field : " + sensors_data.magnetometer.magneticField); - Console.WriteLine("Barometer Atmospheric pressure : " + sensors_data.barometer.pressure); - last_imu_timestamp = sensors_data.imu.timestamp; - // Wait for the [ENTER] key to be pressed - Console.ReadLine(); + SPATIAL_MAPPING_STATE state = zed.GetSpatialMappingState(); + Console.WriteLine("Images captures: " + i + " /500 || Mapping state: " + state); + i++; } } + // Retrieve the spatial map + Console.WriteLine("Extracting mesh..."); + zed.ExtractWholeSpatialMap(); + // Filter the mesh + Console.WriteLine("Filtering mesh..."); + zed.FilterMesh(MESH_FILTER.LOW, ref mesh); // not available for fused point cloud + // Apply the texture + Console.WriteLine("Saving mesh..."); + zed.SaveMesh("mesh.obj", MESH_FILE_FORMAT.OBJ); zed.Close(); } } diff --git a/tutorials/tutorial 5 - spatial mapping/csharp/packages.config b/tutorials/tutorial 5 - spatial mapping/csharp/packages.config index 8047d6ee..8038b422 100644 --- a/tutorials/tutorial 5 - spatial mapping/csharp/packages.config +++ b/tutorials/tutorial 5 - spatial mapping/csharp/packages.config @@ -1,6 +1,4 @@  - - \ No newline at end of file diff --git a/tutorials/tutorial 5 - spatial mapping/python/README.md b/tutorials/tutorial 5 - spatial mapping/python/README.md index a243396f..73c5eb64 100644 --- a/tutorials/tutorial 5 - spatial mapping/python/README.md +++ b/tutorials/tutorial 5 - spatial mapping/python/README.md @@ -47,7 +47,7 @@ if (err != sl.ERROR_CODE.SUCCESS): Now that tracking is enabled, we need to enable the spatial mapping module. You will see that it is very close to the positional tracking: We create a spatial mapping parameters and call `enable_spatial_mapping()` function with this parameter. ```python -mapping_parameters = sl.SpatialMappingParameters() +mapping_parameters sl.SpatialMappingParameters() err = zed.enable_spatial_mapping(mapping_parameters) if (err != sl.ERROR_CODE.SUCCESS): exit(-1) @@ -84,7 +84,7 @@ We have now grabbed 500 frames and the mesh has been created in background. Now First, we need to create a mesh object to manipulate it: a `sl.Mesh`. Then launch the extraction with Camera.extract_whole_spatial_map(). This function will block until the mesh is available. ```python -zed.extract_whole_spatial_map(mesh) # Extract the whole mesh +zed.extrac_whole_spatial_map(mesh) # Extract the whole mesh ``` We have now a mesh. This mesh can be filtered (if needed) to remove duplicate vertices and unneeded faces. This will make the mesh lighter to manipulate.
diff --git a/tutorials/tutorial 6 - object detection/c/CMakeLists.txt b/tutorials/tutorial 6 - object detection/c/CMakeLists.txt new file mode 100644 index 00000000..799bc9a1 --- /dev/null +++ b/tutorials/tutorial 6 - object detection/c/CMakeLists.txt @@ -0,0 +1,21 @@ +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) +PROJECT(ZED_C_Tutorial_6) + +if(COMMAND cmake_policy) + cmake_policy(SET CMP0003 OLD) + cmake_policy(SET CMP0015 OLD) +endif(COMMAND cmake_policy) + +SET(EXECUTABLE_OUTPUT_PATH ".") + +if (WIN32) +include_directories($ENV{ZED_SDK_ROOT_DIR}/include/) +link_directories($ENV{ZED_SDK_ROOT_DIR}/lib/) +else (WIN32) +include_directories("/usr/local/zed/include/") +link_directories("/usr/local/zed/lib/") +endif(WIN32) + +ADD_EXECUTABLE(${PROJECT_NAME} main.c) + +target_link_libraries(${PROJECT_NAME} sl_zed_c) diff --git a/tutorials/tutorial 6 - object detection/c/main.c b/tutorials/tutorial 6 - object detection/c/main.c new file mode 100644 index 00000000..55c34503 --- /dev/null +++ b/tutorials/tutorial 6 - object detection/c/main.c @@ -0,0 +1,149 @@ +/////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2021, STEREOLABS. +// +// All rights reserved. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +/////////////////////////////////////////////////////////////////////////// + + +#include +#include +#include + + +int main(int argc, char **argv) { + + // Create a ZED camera object + int camera_id = 0; + sl_create_camera(camera_id); + + struct SL_InitParameters init_param; + init_param.camera_fps = 30; + init_param.resolution = SL_RESOLUTION_HD1080; + init_param.input_type = SL_INPUT_TYPE_USB; + init_param.camera_device_id = camera_id; + init_param.camera_image_flip = SL_FLIP_MODE_AUTO; + init_param.camera_disable_self_calib = false; + init_param.enable_image_enhancement = true; + init_param.svo_real_time_mode = true; + init_param.depth_mode = SL_DEPTH_MODE_PERFORMANCE; + init_param.depth_stabilization = true; + init_param.depth_maximum_distance = 40; + init_param.depth_minimum_distance = -1; + init_param.coordinate_unit = SL_UNIT_METER; + init_param.coordinate_system = SL_COORDINATE_SYSTEM_LEFT_HANDED_Y_UP; + init_param.sdk_gpu_id = -1; + init_param.sdk_verbose = false; + init_param.sensors_required = false; + init_param.enable_right_side_measure = false; + + // Open the camera + int state = sl_open_camera(camera_id, &init_param, "", "", 0, "", "", ""); + + if (state != 0) { + printf("Error Open \n"); + return 1; + } + + //Enable Positional tracking + struct SL_PositionalTrackingParameters tracking_param; + tracking_param.enable_area_memory = true; + tracking_param.enable_imu_fusion = true; + tracking_param.enable_pose_smothing = false; + + struct SL_Vector3 position; + position = (struct SL_Vector3) { .x = 0, .y = 0, .z = 0 }; + struct SL_Quaternion rotation; + rotation = (struct SL_Quaternion) { .x = 0, .y = 0, .z = 0, .w = 1 }; + + tracking_param.initial_world_position = position; + tracking_param.initial_world_rotation = rotation; + tracking_param.set_as_static = false; + tracking_param.set_floor_as_origin = false; + + state = sl_enable_positional_tracking(camera_id, &tracking_param, ""); + + struct SL_ObjectDetectionParameters objs_param; + objs_param.enable_body_fitting = false; + objs_param.enable_mask_output = false; + objs_param.enable_tracking = true; + objs_param.image_sync = true; + objs_param.max_range = 40; + objs_param.model = SL_DETECTION_MODEL_MULTI_CLASS_BOX_MEDIUM; + + sl_enable_objects_detection(camera_id, &objs_param); + + struct SL_ObjectDetectionRuntimeParameters objs_rt_param; + objs_rt_param.detection_confidence_threshold = 40; + + struct SL_RuntimeParameters rt_param; + rt_param.enable_depth = true; + rt_param.confidence_threshold = 100; + rt_param.reference_frame = SL_REFERENCE_FRAME_CAMERA; + rt_param.sensing_mode = SL_SENSING_MODE_STANDARD; + rt_param.texture_confidence_threshold = 100; + + struct SL_Objects objects; + + // Capture 50 frames and stop + int nb_detection = 0; + while (nb_detection < 100) { + // Grab an image + state = sl_grab(camera_id, &rt_param); + + // A new image is available if grab() returns ERROR_CODE::SUCCESS + if (state == 0) { + sl_retrieve_objects(camera_id, &objs_rt_param, &objects); + if (objects.is_new == 1) { + printf("%i Objects detected \n", objects.nb_object); + + if (objects.nb_object > 0) { + struct SL_ObjectData first_object = objects.object_list[0]; + + printf("First object attributes :\n"); + printf("Label '%i' (conf. %f / 100) \n", (int)first_object.label, first_object.confidence); + + if (objs_param.enable_tracking == true) { + printf(" Tracking ID: %i tracking state: %i / %i \n", (int)first_object.id, (int)first_object.tracking_state, (int)first_object.action_state); + } + + printf(" 3D Position: (%f, %f, %f) / Velocity: (%f, %f, %f) \n", first_object.position.x, first_object.position.y, first_object.position.z, + first_object.velocity.x, first_object.velocity.y, first_object.velocity.z); + + printf(" Bounding Box 2D \n"); + for (int i = 0; i < 4; i++) { + printf(" (%f,%f) \n", first_object.bounding_box_2d[i].x, first_object.bounding_box_2d[i].y); + } + + printf(" Bounding Box 3D \n"); + for (int i = 0; i < 4; i++) { + printf(" (%f,%f,%f) \n", first_object.bounding_box[i].x, first_object.bounding_box[i].y, first_object.bounding_box[i].z); + } + + printf("Press Enter to Continue"); + while (getchar() != '\n'); + } + nb_detection++; + } + } + } + + sl_disable_positional_tracking(camera_id, ""); + sl_disable_objects_detection(camera_id); + sl_close_camera(camera_id); + return 0; +} + diff --git a/tutorials/tutorial 6 - object detection/csharp/CMakeLists.txt b/tutorials/tutorial 6 - object detection/csharp/CMakeLists.txt index fe396197..2a215f86 100644 --- a/tutorials/tutorial 6 - object detection/csharp/CMakeLists.txt +++ b/tutorials/tutorial 6 - object detection/csharp/CMakeLists.txt @@ -26,7 +26,8 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES "System.Numerics" ) -set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES - "Stereolabs.zed_3.5.0" - "sl_zed_interface_3.5.0" +set(CMAKE_SUPPRESS_REGENERATION true) + +set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES + "Stereolabs.zed_3.6.0" ) \ 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 a56a33f2..f6289d5c 100644 --- a/tutorials/tutorial 6 - object detection/csharp/Program.cs +++ b/tutorials/tutorial 6 - object detection/csharp/Program.cs @@ -15,6 +15,7 @@ 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 @@ -26,43 +27,53 @@ static void Main(string[] args) PositionalTrackingParameters trackingParams = new PositionalTrackingParameters(); err = zedCamera.EnablePositionalTracking(ref trackingParams); if (err != ERROR_CODE.SUCCESS) + { + Console.WriteLine("ERROR in Enable Tracking. Exiting..."); Environment.Exit(-1); + } + // Enable Object Detection ObjectDetectionParameters object_detection_parameters = new ObjectDetectionParameters(); object_detection_parameters.detectionModel = sl.DETECTION_MODEL.MULTI_CLASS_BOX; object_detection_parameters.enableObjectTracking = true; + object_detection_parameters.maxRange = -1; + object_detection_parameters.batchParameters = new BatchParameters(); + object_detection_parameters.enable2DMask = false; + object_detection_parameters.enableObjectTracking = true; + err = zedCamera.EnableObjectDetection(ref object_detection_parameters); if (err != ERROR_CODE.SUCCESS) + { + Console.WriteLine("ERROR in Enable OD. Exiting..."); Environment.Exit(-1); - + } // Create Runtime parameters RuntimeParameters runtimeParameters = new RuntimeParameters(); // Create Object Detection frame handle (contains all the objects data) - sl.Objects object_frame = new sl.Objects(); + sl.Objects objects = new sl.Objects(); // Create object detection runtime parameters (confidence, ...) ObjectDetectionRuntimeParameters obj_runtime_parameters = new ObjectDetectionRuntimeParameters(); obj_runtime_parameters.detectionConfidenceThreshold = 50; - int i = 0; while (i < 1000) { if (zedCamera.Grab(ref runtimeParameters) == ERROR_CODE.SUCCESS) { - // Retrieve Objects from Object detection - err = zedCamera.RetrieveObjects(ref object_frame, ref obj_runtime_parameters); - - // Display the data each 10 frames - if (i % 10 == 0) - { - Console.WriteLine("Nb Objects Detection : " + object_frame.numObject); - for (int p = 0; p < object_frame.numObject; p++) - { - Console.WriteLine("Position of object " + p + " : " + object_frame.objectData[p].position + "Tracked? : " + object_frame.objectData[p].objectTrackingState); - } - } + //Retrieve Objects from Object detection + err = zedCamera.RetrieveObjects(ref objects, ref obj_runtime_parameters); + //Console.WriteLine("Retrieve objects state : " + (ERROR_CODE)err); + // Display the data each 10 frames + if (i % 10 == 0) + { + Console.WriteLine("Nb Objects Detection : " + objects.numObject); + for (int p = 0; p < objects.numObject; p++) + { + Console.WriteLine("Position of object " + p + " : " + objects.objectData[p].position + "Tracked? : " + objects.objectData[p].objectTrackingState); + } + } i++; } } diff --git a/tutorials/tutorial 6 - object detection/csharp/packages.config b/tutorials/tutorial 6 - object detection/csharp/packages.config index 8047d6ee..8038b422 100644 --- a/tutorials/tutorial 6 - object detection/csharp/packages.config +++ b/tutorials/tutorial 6 - object detection/csharp/packages.config @@ -1,6 +1,4 @@  - - \ No newline at end of file diff --git a/tutorials/tutorial 7 - sensor data/c/CMakeLists.txt b/tutorials/tutorial 7 - sensor data/c/CMakeLists.txt new file mode 100644 index 00000000..b8a0e5e4 --- /dev/null +++ b/tutorials/tutorial 7 - sensor data/c/CMakeLists.txt @@ -0,0 +1,21 @@ +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) +PROJECT(ZED_C_Tutorial_7) + +if(COMMAND cmake_policy) + cmake_policy(SET CMP0003 OLD) + cmake_policy(SET CMP0015 OLD) +endif(COMMAND cmake_policy) + +SET(EXECUTABLE_OUTPUT_PATH ".") + +if (WIN32) +include_directories($ENV{ZED_SDK_ROOT_DIR}/include/) +link_directories($ENV{ZED_SDK_ROOT_DIR}/lib/) +else (WIN32) +include_directories("/usr/local/zed/include/") +link_directories("/usr/local/zed/lib/") +endif(WIN32) + +ADD_EXECUTABLE(${PROJECT_NAME} main.c) + +target_link_libraries(${PROJECT_NAME} sl_zed_c) \ No newline at end of file diff --git a/tutorials/tutorial 7 - sensor data/c/main.c b/tutorials/tutorial 7 - sensor data/c/main.c new file mode 100644 index 00000000..f3c13c43 --- /dev/null +++ b/tutorials/tutorial 7 - sensor data/c/main.c @@ -0,0 +1,105 @@ +/////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2021, STEREOLABS. +// +// All rights reserved. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +/////////////////////////////////////////////////////////////////////////// + + +#include +#include +#include +#include + + +void printSensorConfiguration(struct SL_SensorParameters* sensor_parameters) { + if (sensor_parameters->is_available) { + printf("*****************************\n"); + printf("Sensor Type: %i \n", (int)sensor_parameters->type); + printf("Max Rate: %f Hz \n", sensor_parameters->sampling_rate); + printf("Range: [%f, %f] Sensor unit: %i \n", sensor_parameters->range.x, sensor_parameters->range.y, (int)sensor_parameters->sensor_unit); + printf("Resolution: %f " " Sensor unit: %i \n", sensor_parameters->resolution, sensor_parameters->sensor_unit); + if (isfinite(sensor_parameters->noise_density)) printf("Noise Density: %f Sensor unit: %i /sr(Hz) \n", sensor_parameters->noise_density, sensor_parameters->sensor_unit); + if (isfinite(sensor_parameters->random_walk)) printf("Random Walk: %f Sensor unit: %i /s/sr(Hz) \n", sensor_parameters->random_walk, sensor_parameters->sensor_unit); + } +} + +int main(int argc, char **argv) { + + // Create a ZED camera object + int camera_id = 0; + sl_create_camera(camera_id); + + struct SL_InitParameters init_param; + init_param.camera_fps = 30; + init_param.resolution = SL_RESOLUTION_HD1080; + init_param.input_type = SL_INPUT_TYPE_USB; + init_param.camera_device_id = camera_id; + init_param.camera_image_flip = SL_FLIP_MODE_AUTO; + init_param.camera_disable_self_calib = false; + init_param.enable_image_enhancement = true; + init_param.svo_real_time_mode = true; + init_param.depth_mode = SL_DEPTH_MODE_PERFORMANCE; + init_param.depth_stabilization = true; + init_param.depth_maximum_distance = 40; + init_param.depth_minimum_distance = -1; + init_param.coordinate_unit = SL_UNIT_METER; + init_param.coordinate_system = SL_COORDINATE_SYSTEM_LEFT_HANDED_Y_UP; + init_param.sdk_gpu_id = -1; + init_param.sdk_verbose = false; + init_param.sensors_required = false; + init_param.enable_right_side_measure = false; + + // Open the camera + int state = sl_open_camera(camera_id, &init_param, "", "", 0, "", "", ""); + + if (state != 0) { + printf("Error Open \n"); + return 1; + } + + enum SL_MODEL model = sl_get_camera_model(camera_id); + + if (model == SL_MODEL_ZED) { + printf("This tutorial does not work with ZED cameras which does not have additional sensors"); + return 0; + } + + struct SL_SensorData sensor_data; + + int n = 0; + + while (n < 500) { + + // Depending on your camera model, different sensors are available. + // NOTE: There is no need to acquire images with grab(). getSensorsData runs in a separate internal capture thread. + if (sl_get_sensors_data(camera_id, &sensor_data, SL_TIME_REFERENCE_CURRENT) == SL_ERROR_CODE_SUCCESS) { + + printf("Sample %i \n", n++); + printf(" - IMU:\n"); + printf(" \t Orientation: {%f,%f,%f,%f} \n", sensor_data.imu.orientation.x, sensor_data.imu.orientation.y, sensor_data.imu.orientation.z, sensor_data.imu.orientation.w); + printf(" \t Acceleration: {%f,%f,%f} [m/sec^2] \n", sensor_data.imu.linear_acceleration.x, sensor_data.imu.linear_acceleration.y, sensor_data.imu.linear_acceleration.z); + printf(" \t Angular Velocity: {%f,%f,%f} [deg/sec] \n", sensor_data.imu.angular_velocity.x, sensor_data.imu.angular_velocity.y, sensor_data.imu.angular_velocity.z); + + printf(" - Magnetometer \n \t Magnetic Field: {%f,%f,%f} [uT] \n", sensor_data.magnetometer.magnetic_field_c.x, sensor_data.magnetometer.magnetic_field_c.y, sensor_data.magnetometer.magnetic_field_c.z); + + printf(" - Barometer \n \t Atmospheric pressure: %f [hPa] \n", sensor_data.barometer.pressure); + } + } + + sl_close_camera(camera_id); + return 0; +} diff --git a/tutorials/tutorial 7 - sensor data/csharp/CMakeLists.txt b/tutorials/tutorial 7 - sensor data/csharp/CMakeLists.txt index 1123b30d..2af5a4ef 100644 --- a/tutorials/tutorial 7 - sensor data/csharp/CMakeLists.txt +++ b/tutorials/tutorial 7 - sensor data/csharp/CMakeLists.txt @@ -26,7 +26,7 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES "System.Numerics" ) -set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES - "Stereolabs.zed_3.5.0" - "sl_zed_interface_3.5.0" +set(CMAKE_SUPPRESS_REGENERATION true) +set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES + "Stereolabs.zed_3.6.0" ) \ No newline at end of file diff --git a/tutorials/tutorial 7 - sensor data/csharp/packages.config b/tutorials/tutorial 7 - sensor data/csharp/packages.config index 8047d6ee..8038b422 100644 --- a/tutorials/tutorial 7 - sensor data/csharp/packages.config +++ b/tutorials/tutorial 7 - sensor data/csharp/packages.config @@ -1,6 +1,4 @@  - - \ No newline at end of file diff --git a/tutorials/tutorial 8 - body tracking/c/CMakeLists.txt b/tutorials/tutorial 8 - body tracking/c/CMakeLists.txt new file mode 100644 index 00000000..84aeb249 --- /dev/null +++ b/tutorials/tutorial 8 - body tracking/c/CMakeLists.txt @@ -0,0 +1,21 @@ +CMAKE_MINIMUM_REQUIRED(VERSION 3.5) +PROJECT(ZED_C_Tutorial_8) + +if(COMMAND cmake_policy) + cmake_policy(SET CMP0003 OLD) + cmake_policy(SET CMP0015 OLD) +endif(COMMAND cmake_policy) + +SET(EXECUTABLE_OUTPUT_PATH ".") + +if (WIN32) +include_directories($ENV{ZED_SDK_ROOT_DIR}/include/) +link_directories($ENV{ZED_SDK_ROOT_DIR}/lib/) +else (WIN32) +include_directories("/usr/local/zed/include/") +link_directories("/usr/local/zed/lib/") +endif(WIN32) + +ADD_EXECUTABLE(${PROJECT_NAME} main.c) + +target_link_libraries(${PROJECT_NAME} sl_zed_c) diff --git a/tutorials/tutorial 8 - body tracking/c/main.c b/tutorials/tutorial 8 - body tracking/c/main.c new file mode 100644 index 00000000..e8c777a0 --- /dev/null +++ b/tutorials/tutorial 8 - body tracking/c/main.c @@ -0,0 +1,157 @@ +/////////////////////////////////////////////////////////////////////////// +// +// Copyright (c) 2021, STEREOLABS. +// +// All rights reserved. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// +/////////////////////////////////////////////////////////////////////////// + + +#include +#include +#include + + +int main(int argc, char **argv) { + + // Create a ZED camera object + int camera_id = 0; + sl_create_camera(camera_id); + + struct SL_InitParameters init_param; + init_param.camera_fps = 30; + init_param.resolution = SL_RESOLUTION_HD1080; + init_param.input_type = SL_INPUT_TYPE_USB; + init_param.camera_device_id = camera_id; + init_param.camera_image_flip = SL_FLIP_MODE_AUTO; + init_param.camera_disable_self_calib = false; + init_param.enable_image_enhancement = true; + init_param.svo_real_time_mode = true; + init_param.depth_mode = SL_DEPTH_MODE_PERFORMANCE; + init_param.depth_stabilization = true; + init_param.depth_maximum_distance = 40; + init_param.depth_minimum_distance = -1; + init_param.coordinate_unit = SL_UNIT_METER; + init_param.coordinate_system = SL_COORDINATE_SYSTEM_LEFT_HANDED_Y_UP; + init_param.sdk_gpu_id = -1; + init_param.sdk_verbose = false; + init_param.sensors_required = false; + init_param.enable_right_side_measure = false; + + // Open the camera + int state = sl_open_camera(camera_id, &init_param, "", "", 0, "", "", ""); + + if (state != 0) { + printf("Error Open \n"); + return 1; + } + + //Enable Positional tracking + struct SL_PositionalTrackingParameters tracking_param; + tracking_param.enable_area_memory = true; + tracking_param.enable_imu_fusion = true; + tracking_param.enable_pose_smothing = false; + + struct SL_Vector3 position; + position = (struct SL_Vector3) { .x = 0, .y = 0, .z = 0 }; + struct SL_Quaternion rotation; + rotation = (struct SL_Quaternion) { .x = 0, .y = 0, .z = 0, .w = 1 }; + + tracking_param.initial_world_position = position; + tracking_param.initial_world_rotation = rotation; + tracking_param.set_as_static = false; + tracking_param.set_floor_as_origin = false; + + state = sl_enable_positional_tracking(camera_id, &tracking_param, ""); + if (state != 0) { + printf("Error enable tracking \n"); + return 1; + } + + struct SL_ObjectDetectionParameters objs_param; + objs_param.enable_body_fitting = false; + objs_param.enable_mask_output = false; + objs_param.enable_tracking = true; + objs_param.image_sync = true; + objs_param.max_range = 40; + objs_param.model = SL_DETECTION_MODEL_HUMAN_BODY_ACCURATE; + + state = sl_enable_objects_detection(camera_id, &objs_param); + if (state != 0) { + printf("Error enable od \n"); + return 1; + } + + struct SL_ObjectDetectionRuntimeParameters objs_rt_param; + objs_rt_param.detection_confidence_threshold = 40; + + struct SL_RuntimeParameters rt_param; + rt_param.enable_depth = true; + rt_param.confidence_threshold = 100; + rt_param.reference_frame = SL_REFERENCE_FRAME_CAMERA; + rt_param.sensing_mode = SL_SENSING_MODE_STANDARD; + rt_param.texture_confidence_threshold = 100; + + struct SL_Objects objects; + + // Capture 50 frames and stop + int nb_detection = 0; + while (nb_detection < 100) { + // Grab an image + state = sl_grab(camera_id, &rt_param); + // A new image is available if grab() returns ERROR_CODE::SUCCESS + if (state == 0) { + sl_retrieve_objects(camera_id, &objs_rt_param, &objects); + + if (objects.is_new == 1) { + printf("%i Objects detected \n", objects.nb_object); + + if (objects.nb_object > 0) { + struct SL_ObjectData first_object = objects.object_list[0]; + + printf("First object attributes :\n"); + printf("Label '%i' (conf. %f / 100) \n", (int)first_object.label, first_object.confidence); + + if (objs_param.enable_tracking == true) { + printf(" Tracking ID: %i tracking state: %i / %i \n", (int)first_object.id, (int)first_object.tracking_state, (int)first_object.action_state); + } + + printf(" 3D Position: (%f, %f, %f) / Velocity: (%f, %f, %f) \n", first_object.position.x, first_object.position.y, first_object.position.z, + first_object.velocity.x, first_object.velocity.y, first_object.velocity.z); + + printf(" Bounding Box 2D \n"); + for (int i = 0; i < 4; i++) { + printf(" (%f,%f) \n", first_object.bounding_box_2d[i].x, first_object.bounding_box_2d[i].y); + } + + printf(" Bounding Box 3D \n"); + for (int i = 0; i < 4; i++) { + printf(" (%f,%f,%f) \n", first_object.bounding_box[i].x, first_object.bounding_box[i].y, first_object.bounding_box[i].z); + } + + printf("Press Enter to Continue"); + while (getchar() != '\n'); + } + nb_detection++; + } + } + } + + sl_disable_positional_tracking(camera_id, ""); + sl_disable_objects_detection(camera_id); + sl_close_camera(camera_id); + return 0; +} + diff --git a/tutorials/tutorial 8 - body tracking/csharp/CMakeLists.txt b/tutorials/tutorial 8 - body tracking/csharp/CMakeLists.txt index 9c82f445..32ff0841 100644 --- a/tutorials/tutorial 8 - body tracking/csharp/CMakeLists.txt +++ b/tutorials/tutorial 8 - body tracking/csharp/CMakeLists.txt @@ -2,6 +2,8 @@ cmake_minimum_required( VERSION 3.8.0 ) project(Body_tracking CSharp) +set(CMAKE_SUPPRESS_REGENERATION true) + add_executable(${PROJECT_NAME} Program.cs App.config @@ -26,7 +28,7 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY VS_DOTNET_REFERENCES "System.Numerics" ) + set_property(TARGET ${PROJECT_NAME} PROPERTY VS_PACKAGE_REFERENCES - "Stereolabs.zed_3.5.0" - "sl_zed_interface_3.5.0" + "Stereolabs.zed_3.6.0" ) \ 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 18754c8e..a830ef54 100644 --- a/tutorials/tutorial 8 - body tracking/csharp/Program.cs +++ b/tutorials/tutorial 8 - body tracking/csharp/Program.cs @@ -21,14 +21,22 @@ static void Main(string[] args) // Open the camera ERROR_CODE err = zedCamera.Open(ref init_params); if (err != ERROR_CODE.SUCCESS) + { + Console.WriteLine("ERROR in Open. Exiting..."); Environment.Exit(-1); + } + // Enable positional tracking PositionalTrackingParameters trackingParams = new PositionalTrackingParameters(); // If you want to have object tracking you need to enable positional tracking first err = zedCamera.EnablePositionalTracking(ref trackingParams); if (err != ERROR_CODE.SUCCESS) + { + Console.WriteLine("ERROR in Enable Tracking. Exiting..."); Environment.Exit(-1); + } + // Enable Object Detection object_detection_parameters = new ObjectDetectionParameters(); @@ -38,9 +46,14 @@ static void Main(string[] args) object_detection_parameters.enableObjectTracking = true; // run detection for every Camera grab object_detection_parameters.imageSync = true; + object_detection_parameters.bodyFormat = BODY_FORMAT.POSE_18; err = zedCamera.EnableObjectDetection(ref object_detection_parameters); if (err != ERROR_CODE.SUCCESS) + { + Console.WriteLine("ERROR in Enable OD. Exiting..."); Environment.Exit(-1); + } + // Create Runtime parameters RuntimeParameters runtimeParameters = new RuntimeParameters(); @@ -59,7 +72,7 @@ static void Main(string[] args) { // Retrieve Objects from Object detection zedCamera.RetrieveObjects(ref objects, ref obj_runtime_parameters); - + if (Convert.ToBoolean(objects.isNew)) { Console.WriteLine(objects.numObject + " Person(s) detected"); @@ -83,15 +96,15 @@ static void Main(string[] args) Console.WriteLine(" Keypoints 2D"); // The body part meaning can be obtained by casting the index into a BODY_PARTS // to get the BODY_PARTS index the getIdx function is available - for (int i = 0; i < firstObject.keypoints2D.Length; i++) + for (int i = 0; i < (int)sl.BODY_PARTS.LAST; i++) { var kp = firstObject.keypoints2D[i]; - Console.WriteLine(" " + (sl.BODY_PARTS)i + " " + kp.X + ", " + kp.Y); + Console.WriteLine(" " + (sl.BODY_PART)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 < firstObject.keypoints.Length; i++) + for (int i = 0; i < (int)sl.BODY_PARTS_POSE_18.LAST; i++) { var kp = firstObject.keypoints[i]; Console.WriteLine(" " + (sl.BODY_PARTS)i + " " + kp.X + ", " + kp.Y + ", " + kp.Z); @@ -101,7 +114,7 @@ static void Main(string[] args) Console.WriteLine("Press 'Enter' to continue..."); Console.ReadLine(); } - } + } } } @@ -111,4 +124,4 @@ static void Main(string[] args) zedCamera.Close(); } } -} \ No newline at end of file +} diff --git a/tutorials/tutorial 8 - body tracking/csharp/packages.config b/tutorials/tutorial 8 - body tracking/csharp/packages.config index 0adeb5af..fa2bea01 100644 --- a/tutorials/tutorial 8 - body tracking/csharp/packages.config +++ b/tutorials/tutorial 8 - body tracking/csharp/packages.config @@ -1,6 +1,4 @@  - - \ No newline at end of file