From 75ee2040222391caebccd48e60e305c52394c61f Mon Sep 17 00:00:00 2001 From: 2vyy <83983150+2vyy@users.noreply.github.com> Date: Mon, 17 Nov 2025 22:00:18 -0600 Subject: [PATCH] cv changes (greedy spiral search & crop to platform) --- combined/ball_cv.cpp | 149 ++++++++++++++++++++----------------- combined/ball_cv.hpp | 26 ++++++- combined/main.cpp | 173 ++++++++++++++++++++++++++----------------- 3 files changed, 207 insertions(+), 141 deletions(-) diff --git a/combined/ball_cv.cpp b/combined/ball_cv.cpp index d6cd61f..986279d 100644 --- a/combined/ball_cv.cpp +++ b/combined/ball_cv.cpp @@ -1,25 +1,25 @@ -#include "ball_cv.hpp" +#include #include #include - -const bool debug = true; -const int gridSize = 8; -const int pixelsPerPoint = 72; -const float real_diameter = 1.575; -const int width = 1024; -const int height = 576; -const int padding = 224; -const float fx = 534.15894866; -const float fy = 522.92638288; -const float cx = 340.66549491; -const float cy = 211.16012128; -const int directions[4][2] = { + +constexpr bool debug = false; +constexpr int gridSize = 8; +constexpr int pixelsPerPoint = 65; +constexpr float real_diameter = 1.575; +constexpr int width = 1024; +constexpr int height = 576; +constexpr int xPadding = 280; +constexpr int yPadding = 60; +constexpr float fx = 534.15894866; +constexpr float fy = 522.92638288; +constexpr float cx = 340.66549491; +constexpr float cy = 211.16012128; +constexpr int directions[4][2] = { {1, 0}, {0, 1}, {-1, 0}, {0, -1} }; - // Camera Matrix: // [[534.15894866 0. 340.66549491] // [ 0. 522.92638288 211.16012128] @@ -29,50 +29,88 @@ const int directions[4][2] = { // 2.56899703e+00]] int main() { - // precompute the points to check - std::vector gridPoints = generateSpiralGrid(gridSize, pixelsPerPoint); + cv::Point gridPoints[gridSize][gridSize]; + for (int y = 0; y < gridSize; ++y) { + for (int x = 0; x < gridSize; ++x) { + gridPoints[y][x] = cv::Point(pixelsPerPoint / 2 + x * pixelsPerPoint, + pixelsPerPoint / 2 + y * pixelsPerPoint); + } + } + cv::Point lastBallGridIdx(gridSize / 2, gridSize / 2); cv::VideoCapture cam(2, cv::CAP_V4L2); cam.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M','J','P','G')); cam.set(cv::CAP_PROP_FRAME_WIDTH, width); cam.set(cv::CAP_PROP_FRAME_HEIGHT, height); cam.set(cv::CAP_PROP_FPS, 60); - if (!cam.isOpened()) { std::cerr << "ERROR: Could not open camera" << std::endl; return 1; } + cv::Mat img; while(true) { std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now(); cam >> img; if (!img.empty()) { - img = img(cv::Rect(0 + padding, 0, width - padding * 2, height)); + img = img(cv::Rect(0 + xPadding, yPadding , width - xPadding * 2, height - yPadding * 2)); bool foundBall = false; - for (int i = 0; i < gridPoints.size(); i++) { - cv::Point& point = gridPoints[i]; - if (isBallColor(img, point.x, point.y) && !foundBall) { - cv::Point center = getCenter(img, point); - cv::rectangle(img, center, cv::Point(center.x + 5, center.y + 5), cv::Scalar(255, 0, 0), 2, cv::LINE_8); - foundBall = true; - if (!debug) { - break; - } - - } + int x = lastBallGridIdx.x; + int y = lastBallGridIdx.y; + cv::Point& startPoint = gridPoints[y][x]; + if (isBallColor(img, startPoint.x, startPoint.y)) { + cv::Point center = getCenter(img, startPoint); + cv::rectangle(img, center, cv::Point(center.x + 5, center.y + 5), + cv::Scalar(255, 0, 0), 2, cv::LINE_8); + foundBall = true; + lastBallGridIdx.x = x; + lastBallGridIdx.y = y; + } else { + int pointsChecked = 1; + int steps = 1; if (debug) { - cv::rectangle(img, point, cv::Point(point.x + 5, point.y + 5), cv::Scalar(0, 0, 0), 2, cv::LINE_8); + cv::rectangle(img, startPoint, cv::Point(startPoint.x + 5, startPoint.y + 5), + cv::Scalar(pointsChecked * 4, 0, 0), 2, cv::LINE_8); } + pointsChecked++; + while (!foundBall) { + for (int dir = 0; dir < 4; dir++) { + for (int i = 0; i < steps; i++) { + x += directions[dir][0]; + y += directions[dir][1]; + if (x >= 0 && x < gridSize && y >= 0 && y < gridSize) { + cv::Point& point = gridPoints[y][x]; + pointsChecked++; + if (isBallColor(img, point.x, point.y)) { + cv::Point center = getCenter(img, point); + if (debug) { + cv::rectangle(img, point, cv::Point(point.x + 5, point.y + 5), + cv::Scalar(pointsChecked * 4, 0, 0), 2, cv::LINE_8); + } + foundBall = true; + lastBallGridIdx.x = x; + lastBallGridIdx.y = y; + break; + } + if (debug) { + cv::rectangle(img, point, cv::Point(point.x + 5, point.y + 5), + cv::Scalar(0, 0, 0), 2, cv::LINE_8); + } + } + if (foundBall || pointsChecked == gridSize * gridSize) break; + } + if (foundBall || pointsChecked == gridSize * gridSize) break; + if (dir == 1 || dir == 3) steps++; + } + if (!debug && foundBall) break; + } } cv::imshow("camera", img); } - if (cv::waitKey(1) >= 0) { - break; - } + if (cv::waitKey(1) >= 0) break; std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now(); std::cout << "delta time: " << std::chrono::duration_cast(end - begin).count() << " microseconds" << std::endl; } - cam.release(); cv::destroyAllWindows(); return 0; @@ -82,10 +120,10 @@ cv::Point getCenter(cv::Mat& image, cv::Point mark) { // recall (0,0) is top left corner int top = mark.y, bottom = mark.y; while (top > 0 && isBallColor(image, mark.x, top - 1)) { - top-=2; + top--; } while (bottom < height - 1 && isBallColor(image, mark.x, bottom + 1)) { - bottom+=2; + bottom++; } if (debug) { cv::rectangle(image, cv::Point(mark.x, bottom), cv::Point(mark.x, top), cv::Scalar(255,255,255), 1, cv::LINE_8); @@ -94,10 +132,10 @@ cv::Point getCenter(cv::Mat& image, cv::Point mark) { int left = mark.x, right = mark.x; while (left > 0 && isBallColor(image, left - 1, mark.y)) { - left-=2; + left--; } while (right < width - 1 && isBallColor(image, right + 1, mark.y)) { - right+=2; + right++; } int centerX = (right + left) / 2; @@ -107,10 +145,10 @@ cv::Point getCenter(cv::Mat& image, cv::Point mark) { int leftCenterEdge = centerX, rightCenterEdge = centerX; while (leftCenterEdge > 0 && isBallColor(image, leftCenterEdge - 1, centerY)) { - leftCenterEdge -= 2; + leftCenterEdge--; } while (rightCenterEdge < width - 1 && isBallColor(image, rightCenterEdge + 1, centerY)) { - rightCenterEdge += 2; + rightCenterEdge++; } int diameter = rightCenterEdge - leftCenterEdge; float z = (fx * real_diameter) / diameter; @@ -123,34 +161,7 @@ cv::Point getCenter(cv::Mat& image, cv::Point mark) { bool isBallColor(cv::Mat& image, int x, int y) { cv::Vec3b color = image.at(y, x); // return 0.299 * color[2] + 0.587 * color[1] + 0.114 * color[0] < 120; - // return (color[0] + color[1] + color[2]) / 3 < 90; // faster but less accurate + // return (color[0] + color[1] + color[2]) / 3 < 90; int B = color[0], G = color[1], R = color[2]; return (R > 100 && R > G + 30 && R > B + 30); -} - -std::vector generateSpiralGrid(int gridSize, int pixelsPerPoint) { - std::vector gridPoints; - int center = gridSize / 2; - int x = center, y = center; - gridPoints.push_back(cv::Point(pixelsPerPoint / 2 + x * pixelsPerPoint, - pixelsPerPoint / 2 + y * pixelsPerPoint)); - int steps = 1; - while (gridPoints.size() < gridSize * gridSize) { - for (int dir = 0; dir < 4; dir++) { - for (int i = 0; i < steps; i++) { - x += directions[dir][0]; - y += directions[dir][1]; - if (x >= 0 && x < gridSize && y >= 0 && y < gridSize) { - gridPoints.push_back(cv::Point(pixelsPerPoint / 2 + x * pixelsPerPoint, - pixelsPerPoint / 2 + y * pixelsPerPoint)); - } - if (gridPoints.size() == gridSize * gridSize) - break; - } - if (dir == 1 || dir == 3) steps++; - if (gridPoints.size() == gridSize * gridSize) - break; - } - } - return gridPoints; } \ No newline at end of file diff --git a/combined/ball_cv.hpp b/combined/ball_cv.hpp index d629101..765f09e 100644 --- a/combined/ball_cv.hpp +++ b/combined/ball_cv.hpp @@ -1,6 +1,26 @@ #pragma once #include -cv::Point getCenter(cv::Mat& image, cv::Point mark); -bool isBallColor(cv::Mat& image, int x, int y); -std::vector generateSpiralGrid(int gridSize, int pixelsPerPoint); \ No newline at end of file +namespace ball_cv { + constexpr int grid_size = 8; + constexpr int pixels_per_point = 65; + constexpr float real_diameter = 1.575; // Ball diameter in inches (convert to meters if needed) + constexpr int width = 1024; + constexpr int height = 576; + constexpr int x_padding = 280; + constexpr int y_padding = 60; + constexpr float fx = 534.15894866; + constexpr float fy = 522.92638288; + constexpr float cx = 340.66549491; + constexpr float cy = 211.16012128; + constexpr int directions[4][2] = { + {1, 0}, + {0, 1}, + {-1, 0}, + {0, -1} + }; +} + + +void getCenter(cv::Mat& image, cv::Point mark, double& last_x, double& last_y); +bool isBallColor(cv::Mat& image, int x, int y); \ No newline at end of file diff --git a/combined/main.cpp b/combined/main.cpp index 208e409..25ba93d 100644 --- a/combined/main.cpp +++ b/combined/main.cpp @@ -7,7 +7,7 @@ #include "motor_driver.hpp" // Your servo driver #include "platform_controller.hpp" // Platform tilt helper #include "LQR_BezierController.hpp" // LQR controller -#include "ball_cv.h" // Your CV functions +#include "ball_cv.hpp" // Your CV functions using namespace std; using namespace std::chrono; @@ -38,30 +38,29 @@ time_t getFileModTime(const string& filepath) { */ class BallTracker { public: - BallTracker(int camera_id = 2, int width = 1024, int height = 576, int padding = 224) - : width_(width), height_(height), padding_(padding), - ball_found_(false), last_x_(0.0), last_y_(0.0) { - - // Camera calibration parameters - fx_ = 534.15894866f; - fy_ = 522.92638288f; - cx_ = 340.66549491f; - cy_ = 211.16012128f; - real_diameter_ = 1.575f; // Ball diameter in inches (convert to meters if needed) - + BallTracker(int camera_id = 2) : last_grid_index(ball_cv::grid_size / 2, ball_cv::grid_size / 2) { // Initialize camera cam_.open(camera_id, cv::CAP_V4L2); cam_.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M','J','P','G')); - cam_.set(cv::CAP_PROP_FRAME_WIDTH, width_); - cam_.set(cv::CAP_PROP_FRAME_HEIGHT, height_); + cam_.set(cv::CAP_PROP_FRAME_WIDTH, ball_cv::width); + cam_.set(cv::CAP_PROP_FRAME_HEIGHT, ball_cv::height); cam_.set(cv::CAP_PROP_FPS, 60); - + if (!cam_.isOpened()) { throw runtime_error("Could not open camera"); } - // Generate spiral grid for ball detection - grid_points_ = generateSpiralGrid(8, 72); + // Populate the 2D grid + for (int y = 0; y < ball_cv::grid_size; ++y) { + for (int x = 0; x < ball_cv::grid_size; ++x) { + gridPoints[y][x] = cv::Point(ball_cv::pixels_per_point / 2 + x * ball_cv::pixels_per_point, + ball_cv::pixels_per_point / 2 + y * ball_cv::pixels_per_point); + } + } + + last_x_ = 0.0; + last_y_ = 0.0; + ball_found_ = false; cout << "Camera initialized successfully" << endl; } @@ -77,50 +76,68 @@ class BallTracker { LQRBezierController::Measurement getBallPosition() { cv::Mat img; cam_ >> img; - if (img.empty()) { + ball_found_ = false; return {last_x_, last_y_}; } // Crop image to remove padding - img = img(cv::Rect(padding_, 0, width_ - padding_ * 2, height_)); - + img = img(cv::Rect( + 0 + ball_cv::x_padding, + ball_cv::y_padding, + ball_cv::width - ball_cv::x_padding * 2, + ball_cv::height - ball_cv::y_padding * 2 + )); ball_found_ = false; - - // Search for ball using spiral grid - for (const auto& point : grid_points_) { - if (isBallColor(img, point.x, point.y)) { - cv::Point center = getCenter(img, point); - - // Calculate 3D position - int leftEdge = center.x, rightEdge = center.x; - int centerY = center.y; - - // Find horizontal extent at center - while (leftEdge > 0 && isBallColor(img, leftEdge - 1, centerY)) { - leftEdge -= 2; - } - while (rightEdge < width_ - padding_ * 2 && isBallColor(img, rightEdge + 1, centerY)) { - rightEdge += 2; + int x = last_grid_index.x; // ARRAY INDEX COORDS + int y = last_grid_index.y; + + // Failsafe in case index is bad + if (x < 0 || x >= ball_cv::grid_size || y < 0 || y >= ball_cv::grid_size) { + x = ball_cv::grid_size / 2; + y = ball_cv::grid_size / 2; + } + + cv::Point& startPoint = gridPoints[y][x]; + + // Check starting point, then begin spiral navigation if not there + if (isBallColor(cropped_img, startPoint.x, startPoint.y)) { + // Pass last_x_ and last_y_ to be updated + getCenter(cropped_img, startPoint, last_x_, last_y_); + ball_found_ = true; + last_grid_index.x = x; + last_grid_index.y = y; + } else { + int pointsChecked = 1; + int steps = 1; + while (!ball_found_) { + for (int dir = 0; dir < 4; dir++) { + for (int i = 0; i < steps; i++) { + x += ball_cv::directions[dir][0]; + y += ball_cv::directions[dir][1]; + + if (x >= 0 && x < ball_cv::grid_size && y >= 0 && y < ball_cv::grid_size) { + cv::Point& point = gridPoints[y][x]; + pointsChecked++; + + if (isBallColor(cropped_img, point.x, point.y)) { + // Pass last_x_ and last_y_ to be updated + getCenter(cropped_img, point, last_x_, last_y_); + ball_found_ = true; + last_grid_index.x = x; + last_grid_index.y = y; + break; + } + } + // Stop if all points checked (should not happen) + if (pointsChecked >= ball_cv::grid_size * ball_cv::grid_size) break; + } + if (ball_found_ || pointsChecked >= ball_cv::grid_size * ball_cv::grid_size) break; // Break from 'dir' loop + if (dir == 1 || dir == 3) steps++; } - - int diameter_px = rightEdge - leftEdge; - float z = (fx_ * real_diameter_) / diameter_px; - float x = (center.x - cx_) * z / fx_; - float y = (center.y - cy_) * z / fy_; - - // Convert from inches to meters if needed - // x /= 39.3701; // Uncomment if real_diameter_ is in inches - // y /= 39.3701; - - last_x_ = x; - last_y_ = y; - ball_found_ = true; - - break; // Found ball, no need to continue + if (ball_found_ || pointsChecked >= ball_cv::grid_size * ball_cv::grid_size) break; } - } - + } return {last_x_, last_y_}; } @@ -130,12 +147,10 @@ class BallTracker { private: cv::VideoCapture cam_; - std::vector grid_points_; - int width_, height_, padding_; - float fx_, fy_, cx_, cy_; - float real_diameter_; - bool ball_found_; + cv::Point gridPoints[ball_cv::grid_size][ball_cv::grid_size]; + cv::Point2i last_grid_index; double last_x_, last_y_; + bool ball_found_; bool isBallColor(cv::Mat& image, int x, int y) { if (x < 0 || x >= image.cols || y < 0 || y >= image.rows) { @@ -146,26 +161,46 @@ class BallTracker { return (R > 100 && R > G + 30 && R > B + 30); } - cv::Point getCenter(cv::Mat& image, cv::Point mark) { + void getCenter(cv::Mat& image, cv::Point mark, double& last_x, double& last_y) { + // --- Find Y-Center and vertical bounds --- int top = mark.y, bottom = mark.y; while (top > 0 && isBallColor(image, mark.x, top - 1)) { - top -= 2; + top--; } while (bottom < image.rows - 1 && isBallColor(image, mark.x, bottom + 1)) { - bottom += 2; + bottom++; } - int centerY = (top + bottom) / 2; - + double centerY = (top + bottom) / 2; + + // --- Find X-Center and horizontal bounds --- int left = mark.x, right = mark.x; while (left > 0 && isBallColor(image, left - 1, mark.y)) { - left -= 2; + left--; } while (right < image.cols - 1 && isBallColor(image, right + 1, mark.y)) { - right += 2; + right++; + } + double centerX = (right + left) / 2; + + // --- Find diameter at the true center --- + double leftCenterEdge = centerX, rightCenterEdge = centerX; + while (leftCenterEdge > 0 && isBallColor(image, leftCenterEdge - 1, centerY)) { + leftCenterEdge--; + } + + while (rightCenterEdge < image.cols - 1 && isBallColor(image, rightCenterEdge + 1, centerY)) { + rightCenterEdge++; + } + int diameter = rightCenterEdge - leftCenterEdge; + + if (diameter > 0) { + double z = (ball_cv::fx * ball_cv::real_diameter) / diameter; + double x_real = (centerX - ball_cv::cx) * z / ball_cv::fx; + double y_real = (centerY - ball_cv::cy) * z / ball_cv::fy; + + last_x = x_real; + last_y = y_real; } - int centerX = (right + left) / 2; - - return cv::Point(centerX, centerY); } }; @@ -218,7 +253,7 @@ int main(int argc, char* argv[]) { // ==================== INITIALIZE CAMERA ==================== cout << "Initializing camera..." << endl; - BallTracker tracker(2, 1024, 576, 224); // camera_id=2, with your resolution + BallTracker tracker(2); // camera_id=2, with your resolution // ==================== INITIALIZE LQR CONTROLLER ====================