Yaw estim

src/prm_launch/launch/video2detector.py

@@ -7,15 +7,15 @@
 
 def generate_launch_description():
     webcam_publisher = get_package_share_path('webcam_publisher')
-    video_path = "/home/purduerm/Videos/moving_but_no_spinning.avi" # example, can change to your liking
+    video_path = "/home/user-accounts/public/spintop/moving_but_no_spinning.avi" # example, can change to your liking
     return LaunchDescription([
         Node(
             package='webcam_publisher',
             output='screen',
             emulate_tty=True,
             executable='VideoCaptureNode',
             parameters=[{'source': str(video_path),
-                         'fps': 4,
+                         'fps': 1,
                          'frame_id': 'video',
                          }]    
         ), 

src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp

@@ -40,19 +40,21 @@ std::vector<_Float32> OpenCVArmorDetector::search(cv::Mat &frame)
 
 // Display the cropped frame for debugging
 #ifdef DEBUG
-    cv::resize(croppedFrame, croppedFrame, cv::Size(WIDTH / 2, HEIGHT / 2));
+    if (_frame_count % 100 == 0) {
+        cv::resize(croppedFrame, croppedFrame, cv::Size(WIDTH / 2, HEIGHT / 2));
 
-    // Create a static window name
-    const std::string window_name = "Detection Results";
-    cv::imshow(window_name, croppedFrame);
+        // Create a static window name
+        const std::string window_name = "Detection Results";
+        cv::imshow(window_name, croppedFrame);
 
-    // Update the window title
-    cv::setWindowTitle(window_name,
-                       "detected: " + std::to_string(_detected_frame) + " / " +
-                           std::to_string(_frame_count) + " (" +
-                           std::to_string(_detected_frame * 100 / _frame_count) + "%) and missed: " + std::to_string(_missed_frames) + std::string(" frames"));
+        // Update the window title
+        cv::setWindowTitle(window_name,
+                        "detected: " + std::to_string(_detected_frame) + " / " +
+                            std::to_string(_frame_count) + " (" +
+                            std::to_string(_detected_frame * 100 / _frame_count) + "%) and missed: " + std::to_string(_missed_frames) + std::string(" frames"));
 
-    cv::waitKey(30);
+        cv::waitKey(1);
+    }
 #endif
 
     // If we didn't find an armor for a few frames (ROS2 param), reset the search area 

src/prm_vision/pose_estimator/include/PoseEstimator.h

@@ -35,6 +35,9 @@ class PoseEstimator
     // Setters
     void setNumFramesToFireAfter(int num_frames_to_fire_after) { _num_frames_to_fire_after = num_frames_to_fire_after; }
     void setAllowedMissedFramesBeforeNoFire(int allowed_missed_frames_before_no_fire) { _allowed_missed_frames_before_no_fire = allowed_missed_frames_before_no_fire; }
+    void setEpsilon(double epsilon) { _epsilon = epsilon; }
+    void setMaxIterations(int max_iterations) { _max_iterations = max_iterations; }
+    void setPitch(double pitch) { _pitch = pitch; }
 
 private:
     // Class methods
@@ -55,6 +58,11 @@ class PoseEstimator
     float _max_shift_distance = 150;
     int _prev_len = 5;
 
+    // Pose Estimator parameters (set dynamically)
+    double _epsilon = 0.4;
+    int _max_iterations = 50;
+    double _pitch = 15.0;
+
     /**
      * @brief Functor for the loss function and gradient computation.
      *

src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp

@@ -33,6 +33,7 @@ class PoseEstimatorNode : public rclcpp::Node
     // Class methods
     void publishZeroPredictedArmor(std_msgs::msg::Header header, std::string new_auto_aim_status);
     void drawTopDownViewGivenRotation(double yaw, double X, double Y, double Z);
+    void drawLineWithAngle(cv::Mat &image, cv::Point2f start_pt, double angle, double length, cv::Scalar color);
 
     // Callbacks and publishers/subscribers
     rclcpp::Subscription<vision_msgs::msg::KeyPoints>::SharedPtr key_points_subscriber;

src/prm_vision/pose_estimator/src/PoseEstimator.cpp

@@ -39,8 +39,8 @@ double PoseEstimator::estimateYaw(const double yaw_guess, const std::vector<cv::
 
     // L-BFGS-B parameters (experimentally tuned)
     LBFGSBParam<double> param;
-    param.epsilon = 0.4;
-    param.max_iterations = 50;
+    param.epsilon = _epsilon;
+    param.max_iterations = _max_iterations;
 
     LBFGSBSolver<double> solver(param);
     LossAndGradient loss(*this, image_points, tvec);
@@ -156,7 +156,7 @@ double PoseEstimator::gradientWrtYawFinitediff(double yaw, std::vector<cv::Point
 double PoseEstimator::computeLoss(double yaw_guess, std::vector<cv::Point2f> image_points, cv::Mat tvec)
 {
     // We assume -15 deg pitch and 0 roll
-    double pitch = -15 * (M_PI / 180);
+    double pitch = _pitch * CV_PI / 180;
 
     cv::Mat Ry = (cv::Mat_<double>(3, 3) << cos(yaw_guess), 0, sin(yaw_guess),
                   0, 1, 0,

src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp

@@ -11,6 +11,10 @@ PoseEstimatorNode::PoseEstimatorNode(const rclcpp::NodeOptions &options) : Node(
     // Dynamic parameters
     pose_estimator->setAllowedMissedFramesBeforeNoFire(this->declare_parameter("_allowed_missed_frames_before_no_fire", 150));
     pose_estimator->setNumFramesToFireAfter(this->declare_parameter("_num_frames_to_fire_after", 3));
+    pose_estimator->setEpsilon(this->declare_parameter("_epsilon", 0.4));
+    pose_estimator->setMaxIterations(this->declare_parameter("_max_iterations", 50));
+    pose_estimator->setPitch(this->declare_parameter("_pitch", 15.0));
+
     validity_filter_.setLockInAfter(this->declare_parameter("_lock_in_after", 3));
     validity_filter_.setMaxDistance(this->declare_parameter("_max_distance", 10000));
     validity_filter_.setMinDistance(this->declare_parameter("_min_distance", 10));
@@ -71,6 +75,21 @@ rcl_interfaces::msg::SetParametersResult PoseEstimatorNode::parameters_callback(
             pose_estimator->setAllowedMissedFramesBeforeNoFire(param.as_int());
             RCLCPP_INFO(this->get_logger(), "Parameter '_allowed_missed_frames_before_no_fire' updated to: %d", param.as_int());
         }
+        else if (param.get_name() == "_epsilon" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
+        {
+            pose_estimator->setEpsilon(param.as_double());
+            RCLCPP_INFO(this->get_logger(), "Parameter '_epsilon' updated to: %f", param.as_double());
+        }
+        else if (param.get_name() == "_max_iterations" && param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER)
+        {
+            pose_estimator->setMaxIterations(param.as_int());
+            RCLCPP_INFO(this->get_logger(), "Parameter '_max_iterations' updated to: %d", param.as_int());
+        }
+        else if (param.get_name() == "_pitch" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
+        {
+            pose_estimator->setPitch(param.as_double());
+            RCLCPP_INFO(this->get_logger(), "Parameter '_pitch' updated to: %f", param.as_double());
+        }
         else
         {
             result.successful = false;
@@ -91,6 +110,9 @@ void PoseEstimatorNode::keyPointsCallback(const vision_msgs::msg::KeyPoints::Sha
         return;
     }
 
+    static int ctr = 0;
+    ctr++;
+
     cv::Mat tvec, rvec;
     bool reset_kalman = false;
     std::string new_auto_aim_status;
@@ -132,25 +154,78 @@ void PoseEstimatorNode::keyPointsCallback(const vision_msgs::msg::KeyPoints::Sha
 
     // Draw top-down view
 #ifdef DEBUG
-    drawTopDownViewGivenRotation(_last_yaw_estimate, tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2));
+    if (ctr % 100 == 0) {
+        drawTopDownViewGivenRotation(_last_yaw_estimate, tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2));
+    }
 #endif
 }
 
 void PoseEstimatorNode::drawTopDownViewGivenRotation(double yaw, double X, double Y, double Z)
 {
-    // Draw a line rotated from the x-axis by yaw.
-    cv::Mat top_down_view = cv::Mat::zeros(1280, 720, CV_8UC3);
+    // Define image dimensions
+    const int imageWidth = 800;   // Width of the top-down view image
+    const int imageHeight = 1000; // Height of the top-down view image
+
+    // Create a blank image (white background)
+    cv::Mat topDownImage(imageHeight, imageWidth, CV_8UC3, cv::Scalar(255, 255, 255));
+
+    // Define scaling factor (pixels per millimeter)
+    // Since Z ranges from 500 mm to 4000 mm, we scale to fit within the image height
+    double scale = imageHeight / 4000.0; // Scale to fit 4000 mm (4 meters) in the image height
+
+    // Map real-world coordinates (X, Z) to image pixel coordinates
+    int centerX = static_cast<int>(imageWidth / 2 + X * scale); // Center X in image
+    int centerZ = static_cast<int>(imageHeight - Z * scale);    // Center Z in image (flip Y-axis for top-down view)
+
+    // Define line length (adjustable)
+    double lineLength = 20.0; // Length of the line in pixels
+    double robot_diameter = 150.0;
 
-    // Draw the target offset from the center of the image
-    int target_x = Z;
-    int target_y = X;
-    cv::circle(top_down_view, cv::Point(target_x, target_y), 5, cv::Scalar(0, 255, 0), -1);
+    // Calculate line endpoints based on yaw angle
+    double angleRad = -yaw; // Convert yaw to radians
 
-    cv::resize(top_down_view, top_down_view, cv::Size(100, 100));
-    cv::imshow("Top Down View", top_down_view);
+    // draw armor
+    drawLineWithAngle(topDownImage, cv::Point2f(centerX, centerZ), angleRad, lineLength, cv::Scalar(0, 0, 255));
+    drawLineWithAngle(topDownImage, cv::Point2f(centerX, centerZ), angleRad + CV_PI, lineLength, cv::Scalar(0, 0, 255));
+
+    // draw line of typical robot diameter
+    drawLineWithAngle(topDownImage, cv::Point2f(centerX, centerZ), angleRad - CV_PI / 2, robot_diameter, cv::Scalar(0, 255, 0));
+
+    // armor at other end of line
+    drawLineWithAngle(topDownImage, cv::Point2f(centerX + robot_diameter * cos(angleRad - CV_PI / 2), centerZ + robot_diameter * sin(angleRad - CV_PI / 2)), angleRad, lineLength, cv::Scalar(0, 0, 255));
+    drawLineWithAngle(topDownImage, cv::Point2f(centerX + robot_diameter * cos(angleRad - CV_PI / 2), centerZ + robot_diameter * sin(angleRad - CV_PI / 2)), angleRad + CV_PI, lineLength, cv::Scalar(0, 0, 255));
+
+    // more lines of typical robot diameter 90 degrees from the center line
+    cv::Point2f robot_center = cv::Point2f((centerX + robot_diameter / 2 * cos(angleRad - CV_PI / 2)), (centerZ + robot_diameter / 2 * sin(angleRad - CV_PI / 2)));
+    drawLineWithAngle(topDownImage, robot_center, angleRad - CV_PI, robot_diameter / 2, cv::Scalar(200, 255, 0));
+    drawLineWithAngle(topDownImage, robot_center, angleRad, robot_diameter / 2, cv::Scalar(200, 255, 0));
+
+    // armor at other end of these two lines, perpendicular to the center line
+    drawLineWithAngle(topDownImage, cv::Point2f(robot_center.x + robot_diameter / 2 * cos(angleRad), robot_center.y + robot_diameter / 2 * sin(angleRad)), angleRad - CV_PI / 2, lineLength, cv::Scalar(0, 0, 255));
+    drawLineWithAngle(topDownImage, cv::Point2f(robot_center.x + robot_diameter / 2 * cos(angleRad), robot_center.y + robot_diameter / 2 * sin(angleRad)), angleRad + CV_PI / 2, lineLength, cv::Scalar(0, 0, 255));
+
+    // same thing on the other side
+    drawLineWithAngle(topDownImage, cv::Point2f(robot_center.x - robot_diameter / 2 * cos(angleRad), robot_center.y - robot_diameter / 2 * sin(angleRad)), angleRad - CV_PI / 2, lineLength, cv::Scalar(0, 0, 255));
+    drawLineWithAngle(topDownImage, cv::Point2f(robot_center.x - robot_diameter / 2 * cos(angleRad), robot_center.y - robot_diameter / 2 * sin(angleRad)), angleRad + CV_PI / 2, lineLength, cv::Scalar(0, 0, 255));
+
+    RCLCPP_INFO(get_logger(), "Drawing top-down view with yaw = %.2f, X = %.2f, Y = %.2f, Z = %.2f", 180.0 * yaw / CV_PI, X, Y, Z);
+
+    // Display the image
+    cv::imshow("Top-Down View", topDownImage);
     cv::waitKey(1);
 }
 
+void PoseEstimatorNode::drawLineWithAngle(cv::Mat &image, cv::Point2f start_pt, double angle, double length, cv::Scalar color)
+{
+    // Calculate endpoint of the line
+    cv::Point2f end_pt;
+    end_pt.x = start_pt.x + length * cos(angle);
+    end_pt.y = start_pt.y + length * sin(angle);
+
+    // Draw the line
+    cv::line(image, start_pt, end_pt, color, 2);
+}
+
 void PoseEstimatorNode::publishZeroPredictedArmor(std_msgs::msg::Header header, std::string new_auto_aim_status)
 {
     vision_msgs::msg::PredictedArmor predicted_armor_msg;