#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "cv_bridge/cv_bridge.h"
#include <opencv2/opencv.hpp>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <stdexcept> // 关键头文件
class ImageSubscriber : public rclcpp::Node {
public:
ImageSubscriber() : Node("image_subscriber") {
// 使用lambda避免绑定错误
subscription_ = create_subscription<sensor_msgs::msg::Image>(
"/detector/initial_img", 10,
[this](sensor_msgs::msg::Image::ConstSharedPtr msg) {
this->image_callback(msg);
});
}
private:
void image_callback(sensor_msgs::msg::Image::ConstSharedPtr msg) {
try {
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
cv::Mat image=cv_ptr->image;
cv::imshow("Preview", image);
cv::waitKey(1);
std::vector<cv::Mat> channels;
split(image,channels);
cv::Mat R = channels[2];
cv::Mat G = channels[1];
cv::Mat B = channels[0];
cv::Mat diffRG,diffRB;
cv::subtract(R,G,diffRG);
cv::subtract(R,B,diffRB);
cv::Mat orangeMask;
cv::addWeighted(diffRG,0.5,diffRB,0.5,0,orangeMask);
cv::imshow("orangeMask", orangeMask);
cv::Mat binary,Gaussian,gray,kernal;
kernal = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
cv::cvtColor(orangeMask,gray,cv::COLOR_BGR2GRAY);
cv::threshold(gray,binary,120,255,0);
cv::waitKey(1);
cv::dilate(binary,binary,kernal);
cv::erode(binary, binary, kernal);
cv::imshow("dilated", binary);
cv::waitKey(1);
std::vector<std::vector<cv::Point>> contours; // 存储所有轮廓（二维点集）
std::vector<cv::Vec4i> hierarchy;
cv::findContours(binary,contours,cv::RETR_EXTERNAL,cv::CHAIN_APPROX_SIMPLE);
cv::Mat result = image.clone();
cv::Mat result2 = image.clone();
std::vector<cv::RotatedRect> lightBarRects;
for (int i = 0; i < contours.size(); i++) {
double area = contourArea(contours[i
if (area < 5 || contours[i].size() <= 1)
continue;
// 用椭圆拟合区域得到外接矩形（特殊的处理方式：因为灯条是椭圆型的，所以用椭圆去拟合轮廓，再直接获取旋转外接矩形即可）
cv::RotatedRect Light_Rec = fitEllipse(contours[i]);
// 长宽比和轮廓面积比限制（由于要考虑灯条的远近都被识别到，所以只需要看比例即可）
if (Light_Rec.size.width / Light_Rec.size.height > 4)
continue;
cv::lightInfos.push_back(cv::LightDescriptor(Light_Rec));
}
//二重循环多条件匹配灯条
for (size_t i = 0; i < lightInfos.size(); i++) {
for (size_t j = i + 1; (j < lightInfos.size()); j++) {
LightDescriptor& leftLight = lightInfos[i];
LightDescriptor& rightLight = lightInfos[j];
float angleGap_ = abs(leftLight.angle - rightLight.angle);
//由于灯条长度会因为远近而受到影响，所以按照比值去匹配灯条
float LenGap_ratio = abs(leftLight.length - rightLight.length) / max(leftLight.length, rightLight.length);
float dis = pow(pow((leftLight.center.x - rightLight.center.x), 2) + pow((leftLight.center.y - rightLight.center.y), 2), 0.5);
//均长
float meanLen = (leftLight.length + rightLight.length) / 2;
float lengap_ratio = abs(leftLight.length - rightLight.length) / meanLen;
float yGap = abs(leftLight.center.y - rightLight.center.y);
float yGap_ratio = yGap / meanLen;
float xGap = abs(leftLight.center.x - rightLight.center.x);
float xGap_ratio = xGap / meanLen;
float ratio = dis / meanLen;
//匹配不通过的条件
if (angleGap_ > 15 ||
LenGap_ratio > 1.0 ||
lengap_ratio > 0.8 ||
yGap_ratio > 1.5 ||
xGap_ratio > 2.2 ||
xGap_ratio < 0.8 ||
ratio > 3 ||
ratio < 0.8) {
continue;
}
//绘制矩形
cv::Point center =cv::Point((leftLight.center.x + rightLight.center.x) / 2, (leftLight.center.y + rightLight.center.y) / 2);
cv::RotatedRect rect = cv::RotatedRect(center, cv::Size(dis, meanLen), (leftLight.angle + rightLight.angle) / 2);
cv::Point2f vertices[4];
rect.points(vertices);
for (int i = 0; i < 4; i++) {
line(result, vertices[i], vertices[(i + 1) % 4], cv::Scalar(0, 0, 255), 2.2);
}
}
}
for (size_t i = 0; i < contours.size(); i++) {
// 计算最小外接矩形
cv::RotatedRect minRect = cv::minAreaRect(contours[i]);
lightBarRects.push_back(minRect);
// 获取矩形的四个顶点
cv::Point2f vertices[4];
minRect.points(vertices);
// 绘制最小外接矩形
for (int j = 0; j < 4; j++) {
cv::line(result, vertices[j], vertices[(j+1)%4], cv::Scalar(0, 255, 0), 2);
}
}
cv::imshow("Armor Detection", result);
cv::waitKey(1);
std::vector<cv::RotatedRect> armorRects;
const double maxAngleDiff = 15.0;
cv::Point2f vertices2[4];
for (size_t i = 0; i < lightBarRects.size(); i++) {
for (size_t j = i + 1; j < lightBarRects.size(); j++) {
cv::RotatedRect bar1 = lightBarRects[i];
cv::RotatedRect bar2 = lightBarRects[j];
// 简化角度计算：直接取旋转矩形的角度（OpenCV原生角度）
float angle1 = fabs(bar1.angle);
float angle2 = fabs(bar2.angle);
// 计算角度差（简化角度转换，直接用原生角度差）
double angleDiff = fabs(angle1 - angle2);
// 仅通过角度差判断是否匹配
if (angleDiff < maxAngleDiff) {
// 简化装甲板计算：直接用两灯条中心和尺寸构建
cv::Point2f armorCenter = (bar1.center + bar2.center) / 2;
float distance = norm(bar1.center - bar2.center); // 两灯条间距
cv::Size2f armorSize(distance * 1.1, (bar1.size.height + bar2.size.height) / 2 * 1.2);
float armorAngle = (bar1.angle + bar2.angle) / 2; // 平均角度
armorRects.push_back(cv::RotatedRect(armorCenter, armorSize, armorAngle));
}
}
}
for (auto& armor : armorRects) {
armor.points(vertices2);
for (int j = 0; j < 4; j++) {
cv::line(result2, vertices2[j], vertices2[(j+1)%4], cv::Scalar(0, 0, 255), 2);
}
imshow("hihi",result2);
cv::waitKey(1);
}
std::vector<cv::Point2f> vertices2_vec(vertices2, vertices2 + 4);
std::vector<cv::Point3f>obj=std::vector<cv::Point3f>{
{-0.027555,0.675,0},
{-0.027555,-0.675,0},
{0.027555,-0.675,0},
{0.027555,0.675,0}
};
cv::Mat rVec=cv::Mat::zeros(3,1,CV_64FC1);
cv::Mat tVec=cv::Mat::zeros(3,1,CV_64FC1);
cv::Mat camera=(cv::Mat_<double>(3,3)<<1749.23969217601,0,711.302879207889,0,1748.77539275011,562.465887239595,0,0,1);
cv::Mat dis=(cv::Mat_<double>(1,5)<<0.0846,-0.5788,0,0,0);
cv::solvePnP(obj,vertices2_vec,camera,dis,rVec,tVec,false,cv::SOLVEPNP_EPNP);
}
catch (const cv_bridge::Exception& e) {
RCLCPP_ERROR(get_logger(), "cv_bridge异常: %s", e.what());
throw std::runtime_error("CV错误: " + std::string(e.what()));
}
}
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_;
};
int main(int argc, char* argv[]) {
rclcpp::init(argc, argv);
auto node = std::make_shared<ImageSubscriber>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
修改下
最新发布
本文详细介绍了Linux系统中LED的控制方法，包括LED类的基本概念、LED触发器的工作原理及如何利用内核提供的API实现LED的亮度调节和闪烁等功能。
扫一扫
663
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
