From 6749a32ef583ff2cf4dea797856d44313430323f Mon Sep 17 00:00:00 2001 From: Chris Padwick Date: Sun, 27 Oct 2024 16:14:30 -0700 Subject: [PATCH 1/3] add a method to print settings --- src/ws_server.cu | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/src/ws_server.cu b/src/ws_server.cu index a40802a..fd924a7 100644 --- a/src/ws_server.cu +++ b/src/ws_server.cu @@ -189,6 +189,18 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { } } + void printCameraSettings(const cv::VideoCapture& cap) { + int frame_width = cap.get(cv::CAP_PROP_FRAME_WIDTH); + int frame_height = cap.get(cv::CAP_PROP_FRAME_HEIGHT); + int frame_rate = cap.get(cv::CAP_PROP_FPS); + + std::cout << " " << frame_width << "x" << frame_height << " @" + << frame_rate << "FPS" << std::endl; + + std::cout << " format is: " << cap.get(cv::CAP_PROP_FORMAT) << std::endl; + + } + void readAndSend(const int camera_idx, const std::string& cal_file) { std::cout << "Enabling video capture" << std::endl; bool camera_started = false; @@ -214,12 +226,8 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { cap.set(cv::CAP_PROP_FRAME_WIDTH, 1920); cap.set(cv::CAP_PROP_FRAME_HEIGHT, 1080); - int frame_width = cap.get(cv::CAP_PROP_FRAME_WIDTH); - int frame_height = cap.get(cv::CAP_PROP_FRAME_HEIGHT); - int frame_rate = cap.get(cv::CAP_PROP_FPS); - - std::cout << " " << frame_width << "x" << frame_height << " @" - << frame_rate << "FPS" << std::endl; + printCameraSettings(cap); + break; std::cout << "AUTO Exposure: " << cap.get(cv::CAP_PROP_AUTO_EXPOSURE) << std::endl; From fcace04999767a5a6527bcd8c5e415b20a1147a8 Mon Sep 17 00:00:00 2001 From: Chris Padwick Date: Mon, 28 Oct 2024 19:45:53 -0700 Subject: [PATCH 2/3] checkpoint a memory leak fix --- src/apriltag_gpu.cu | 12 ++++++++ src/apriltag_gpu.h | 2 ++ src/ws_server.cu | 71 ++++++++++++++++++++++++++++----------------- 3 files changed, 59 insertions(+), 26 deletions(-) diff --git a/src/apriltag_gpu.cu b/src/apriltag_gpu.cu index 5fd32f1..4020cce 100644 --- a/src/apriltag_gpu.cu +++ b/src/apriltag_gpu.cu @@ -206,6 +206,18 @@ GpuDetector::~GpuDetector() { zarray_destroy(poly0_); } +GpuDetector::ReinitializeDetections() { + //Convenience method to reinitialize the detections_ array + //so we don't have to call the destructor to free the memory. + for (int i = 0; i < zarray_size(detections_); ++i) { + apriltag_detection_t *det; + zarray_get(detections_, i, &det); + apriltag_detection_destroy(det); + } + zarray_clear(detections_); + zarray_ensure_capacity(detections_, kMaxBlobs); +} + namespace { // Computes a massive image of 4x QuadBoundaryPoint per pixel with a diff --git a/src/apriltag_gpu.h b/src/apriltag_gpu.h index cb40dbb..ac5a093 100644 --- a/src/apriltag_gpu.h +++ b/src/apriltag_gpu.h @@ -92,6 +92,8 @@ class GpuDetector { const zarray_t *Detections() const { return detections_; } + void ReinitializeDetections(); + // Debug methods to expose internal state for testing. void CopyGrayTo(uint8_t *output) const { gray_image_device_.MemcpyTo(output); diff --git a/src/ws_server.cu b/src/ws_server.cu index fd924a7..9a57c80 100644 --- a/src/ws_server.cu +++ b/src/ws_server.cu @@ -164,17 +164,13 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { return true; } - void rotateImage(cv::Mat* yuyvimg, const float angle) { - // Since the input image is yuyv it is convenient to convert - // to bgr, then rotate, then convert back. If we don't - // convert to bgr then the colors get swapped after rotation. - cv::Mat bgr; - cv::cvtColor(*yuyvimg, bgr, cv::COLOR_YUV2BGR_YUYV); - cv::Point2f center((bgr.cols - 1) / 2.0, (bgr.rows - 1) / 2.0); + // TODO: Implement a faster version of this method! + void rotateImage(cv::Mat* bgr_img, const float angle) { + cv::Point2f center((bgr_img->cols - 1) / 2.0, (bgr_img->rows - 1) / 2.0); cv::Mat matRotation = cv::getRotationMatrix2D(center, angle, 1.0); cv::Mat rotatedImage; - cv::warpAffine(bgr, rotatedImage, matRotation, bgr.size()); - cv::cvtColor(rotatedImage, *yuyvimg, cv::COLOR_BGR2YUV_YUYV); + cv::warpAffine(*bgr_img, rotatedImage, matRotation, bgr_img->size()); + *bgr_img = rotatedImage.clone(); } void startReadAndSendThread(const int camera_idx, @@ -199,6 +195,11 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { std::cout << " format is: " << cap.get(cv::CAP_PROP_FORMAT) << std::endl; + std::cout << "AUTO Exposure: " << cap.get(cv::CAP_PROP_AUTO_EXPOSURE) + << std::endl; + std::cout << "Brightness: " << cap.get(cv::CAP_PROP_BRIGHTNESS) + << std::endl; + std::cout << "Contrast: " << cap.get(cv::CAP_PROP_CONTRAST) << std::endl; } void readAndSend(const int camera_idx, const std::string& cal_file) { @@ -222,18 +223,18 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { std::this_thread::sleep_for(std::chrono::seconds(1)); } } - cap.set(cv::CAP_PROP_CONVERT_RGB, false); - cap.set(cv::CAP_PROP_FRAME_WIDTH, 1920); - cap.set(cv::CAP_PROP_FRAME_HEIGHT, 1080); - printCameraSettings(cap); - break; + // Set video mode, resolution and frame rate. + int fourcc = cv::VideoWriter::fourcc('M', 'J', 'P', 'G'); + cap.set(cv::CAP_PROP_FOURCC, fourcc); + cap.set(cv::CAP_PROP_FRAME_WIDTH, 1280); + cap.set(cv::CAP_PROP_FRAME_HEIGHT, 800); + cap.set(cv::CAP_PROP_FPS, 30); + cap.set(cv::CAP_PROP_CONVERT_RGB, true); - std::cout << "AUTO Exposure: " << cap.get(cv::CAP_PROP_AUTO_EXPOSURE) - << std::endl; - std::cout << "Brightness: " << cap.get(cv::CAP_PROP_BRIGHTNESS) - << std::endl; - std::cout << "Contrast: " << cap.get(cv::CAP_PROP_CONTRAST) << std::endl; + int frame_width = cap.get(cv::CAP_PROP_FRAME_WIDTH); + int frame_height = cap.get(cv::CAP_PROP_FRAME_HEIGHT); + printCameraSettings(cap); // Setup the apriltag detector. apriltag_family_t* tf = nullptr; @@ -259,6 +260,13 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { return; } + auto gpucreatestart = std::chrono::high_resolution_clock::now(); + frc971::apriltag::GpuDetector detector(frame_width, frame_height, td, cam, dist); + auto gpucreateend = std::chrono::high_resolution_clock::now(); + + auto gpucreateduration = std::chrono::duration_cast(gpucreateend - gpucreatestart); + std::cout << "GPU Detector Create Time: " << gpucreateduration.count() << " ms" << std::endl; + // Setup the detection info struct for use down below. apriltag_detection_info_t info; info.tagsize = @@ -287,21 +295,22 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { } try { - cap >> yuyv_img; + cap >> bgr_img; frame_counter++; + + auto overallstart = std::chrono::high_resolution_clock::now(); if (rotate_img_) { - rotateImage(&yuyv_img, 180.0); + rotateImage(&bgr_img, 180.0); } - cv::cvtColor(yuyv_img, bgr_img, cv::COLOR_YUV2BGR_YUYV); - - frc971::apriltag::GpuDetector detector(frame_width, frame_height, td, - cam, dist); + cv::cvtColor(bgr_img, yuyv_img, cv::COLOR_BGR2YUV_YUYV); + auto gpudetectstart = std::chrono::high_resolution_clock::now(); detector.Detect(yuyv_img.data); + auto gpudetectend = std::chrono::high_resolution_clock::now(); const zarray_t* detections = detector.Detections(); draw_detection_outlines(bgr_img, const_cast(detections)); // Broadcast the image to websocket clients. - if (frame_counter % 10 == 0) { + if (frame_counter % 50 == 0) { // Encode the image to JPEG std::vector buffer; cv::imencode(".jpg", bgr_img, buffer); @@ -357,6 +366,16 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { std::string pose_json = detections_record.dump(); broadcastPoseData(pose_json); tagSender_.sendValue(networktables_pose_data); + + auto overallend = std::chrono::high_resolution_clock::now(); + auto overallduration = std::chrono::duration_cast(overallend - overallstart); + auto gpudetectduration = std::chrono::duration_cast(gpudetectend - gpudetectstart); + std::cout << "Total Elapsed time: " << overallduration.count() << " ms" << std::endl; + //std::cout << "GPU Elapsed time: " << gpuoverallduration.count() << " ms" << std::endl; + //std::cout << "GPU Detector Create time: " << gpucreateduration.count() << " ms" << std::endl; + std::cout << "GPU Detect time: " << gpudetectduration.count() << " ms" << std::endl; + + detector.ReinitializeDetections(); } } catch (const std::exception& ex) { std::cout << "Encounted exception " << ex.what() << std::endl; From 6c16def2de28273dd08c0fe88d34d3056e794936 Mon Sep 17 00:00:00 2001 From: Chris Padwick Date: Tue, 29 Oct 2024 20:47:04 -0700 Subject: [PATCH 3/3] checkpoint changes --- src/apriltag_gpu.cu | 16 ++++++++++++---- src/ws_server.cu | 29 ++++++++++++++++++++--------- 2 files changed, 32 insertions(+), 13 deletions(-) diff --git a/src/apriltag_gpu.cu b/src/apriltag_gpu.cu index 4020cce..563f05c 100644 --- a/src/apriltag_gpu.cu +++ b/src/apriltag_gpu.cu @@ -206,15 +206,23 @@ GpuDetector::~GpuDetector() { zarray_destroy(poly0_); } -GpuDetector::ReinitializeDetections() { - //Convenience method to reinitialize the detections_ array - //so we don't have to call the destructor to free the memory. +void GpuDetector::ReinitializeDetections() { + // Convenience method to reinitialize the detections_ array + // so we don't have to call the destructor to free the memory. for (int i = 0; i < zarray_size(detections_); ++i) { apriltag_detection_t *det; zarray_get(detections_, i, &det); apriltag_detection_destroy(det); } - zarray_clear(detections_); + + zarray_destroy(detections_); + zarray_destroy(poly1_); + zarray_destroy(poly0_); + + poly0_ = g2d_polygon_create_zeros(4); + poly1_ = g2d_polygon_create_zeros(4); + + detections_ = zarray_create(sizeof(apriltag_detection_t *)); zarray_ensure_capacity(detections_, kMaxBlobs); } diff --git a/src/ws_server.cu b/src/ws_server.cu index 9a57c80..ea7123d 100644 --- a/src/ws_server.cu +++ b/src/ws_server.cu @@ -261,11 +261,15 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { } auto gpucreatestart = std::chrono::high_resolution_clock::now(); - frc971::apriltag::GpuDetector detector(frame_width, frame_height, td, cam, dist); + frc971::apriltag::GpuDetector detector(frame_width, frame_height, td, cam, + dist); auto gpucreateend = std::chrono::high_resolution_clock::now(); - auto gpucreateduration = std::chrono::duration_cast(gpucreateend - gpucreatestart); - std::cout << "GPU Detector Create Time: " << gpucreateduration.count() << " ms" << std::endl; + auto gpucreateduration = + std::chrono::duration_cast(gpucreateend - + gpucreatestart); + std::cout << "GPU Detector Create Time: " << gpucreateduration.count() + << " ms" << std::endl; // Setup the detection info struct for use down below. apriltag_detection_info_t info; @@ -368,12 +372,19 @@ class AprilTagHandler : public seasocks::WebSocket::Handler { tagSender_.sendValue(networktables_pose_data); auto overallend = std::chrono::high_resolution_clock::now(); - auto overallduration = std::chrono::duration_cast(overallend - overallstart); - auto gpudetectduration = std::chrono::duration_cast(gpudetectend - gpudetectstart); - std::cout << "Total Elapsed time: " << overallduration.count() << " ms" << std::endl; - //std::cout << "GPU Elapsed time: " << gpuoverallduration.count() << " ms" << std::endl; - //std::cout << "GPU Detector Create time: " << gpucreateduration.count() << " ms" << std::endl; - std::cout << "GPU Detect time: " << gpudetectduration.count() << " ms" << std::endl; + auto overallduration = + std::chrono::duration_cast( + overallend - overallstart); + auto gpudetectduration = + std::chrono::duration_cast( + gpudetectend - gpudetectstart); + std::cout << "Total Elapsed time: " << overallduration.count() + << " ms" << std::endl; + // std::cout << "GPU Elapsed time: " << gpuoverallduration.count() << + // " ms" << std::endl; std::cout << "GPU Detector Create time: " << + // gpucreateduration.count() << " ms" << std::endl; + std::cout << "GPU Detect time: " << gpudetectduration.count() << " ms" + << std::endl; detector.ReinitializeDetections(); }