#include <rclcpp/rclcpp.hpp>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include <opencv2/core.hpp>
#include <opencv2/calib3d.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
// 装甲板实际尺寸（单位：米）
const double ARMOR_WIDTH = 0.13;
const double ARMOR_HEIGHT = 0.055;
const double HALF_WIDTH = ARMOR_WIDTH / 2.0;
const double HALF_HEIGHT = ARMOR_HEIGHT / 2.0;
class ArmorDetector : public rclcpp::Node
{
public:
ArmorDetector() : Node("armor_detector")
{
// 订阅图像话题（需根据实际话题名修改）
image_sub_ = image_transport::create_subscription(
this, "/camera/image_raw",
std::bind(&ArmorDetector::imageCallback, this, std::placeholders::_1),
"raw", rmw_qos_profile_sensor_data);
// 发布处理后的图像
image_pub_ = image_transport::create_publisher(this, "armor_detection_result");
// TF广播器
tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
// 初始化相机内参（需根据实际相机标定修改）
cv::camera_matrix_ = (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.0 0.0 0.0 0.0 0.0);
// 定义装甲板3D角点（中心为原点）
object_points_ = {
{-HALF_WIDTH, -HALF_HEIGHT, 0.0}, // 左下
{HALF_WIDTH, -HALF_HEIGHT, 0.0}, // 右下
{HALF_WIDTH, HALF_HEIGHT, 0.0}, // 右上
{-HALF_WIDTH, HALF_HEIGHT, 0.0} // 左上
};
}
private:
void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& msg)
{
// 转换为OpenCV图像
void drawMyContours(std::string winName, cv::Mat &image, std::vector<std::vector<cv::Point>> contours);
cv::Point2d re_projection(
const Eigen::Vector3d& world_point,
const Eigen::Matrix3d& camera_matrix
);
cv::Point2d re_projection(
const Eigen::Vector3d& world_point,
const Eigen::Matrix3d& camera_matrix
);
cv::Mat image1 = cv_bridge::toCvShare(msg, "bgr8")->image;
cv::Mat binary,Gaussian,gray,kernal;
kernal = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
cv::cvtColor(image1,gray,COLOR_BGR2GRAY);
cv::threshold(gray,binary,120,255,0);
cv::imshow("original3", binary);
//
cv::dilate(binary,binary,kernal);
cv::erode(binary, binary, kernal);
cv::imshow("dilated", binary);
cv::waitKey(0);
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 = image1.clone();
cv::Mat result2 = image1.clone();
std::vector<RotatedRect> lightBarRects;
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(0);
std::vector<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++) {
RotatedRect bar1 = lightBarRects[i];
RotatedRect bar2 = lightBarRects[j];
// 简化角度计算：直接取旋转矩形的角度（OpenCV原生角度）
float angle1 = fabs(bar1.angle);
float angle2 = fabs(bar2.angle);
// 计算角度差（简化角度转换，直接用原生角度差）
double angleDiff = fabs(angle1 - angle2);
// 仅通过角度差判断是否匹配
if (angleDiff < maxAngleDiff) {
// 简化装甲板计算：直接用两灯条中心和尺寸构建
Point2f armorCenter = (bar1.center + bar2.center) / 2;
float distance = norm(bar1.center - bar2.center); // 两灯条间距
Size2f armorSize(distance * 1.1, (bar1.size.height + bar2.size.height) / 2 * 1.2);
float armorAngle = (bar1.angle + bar2.angle) / 2; // 平均角度
armorRects.push_back(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], Scalar(0, 0, 255), 2);
}
imshow("hihi",result2);
waitKey(0);
}
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::solvePnP(object_points_, image_points, camera_matrix_, dis,rvec, tvec, false, cv::SOLVEPNP_EPNP);
cv::Mat R_cv; // OpenCV格式的旋转矩阵
cv::Rodrigues(rVec, R_cv);
// 转换为Eigen矩阵
Eigen::Matrix3d R;
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
R(i, j) = R_cv.at<double>(i, j);
// 8. 将OpenCV的平移向量转换为Eigen向量
Eigen::Vector3d t;
t << tVec.at<double>(0), tVec.at<double>(1), tVec.at<double>(2);
// 9. 定义世界原点（装甲板中心）
Eigen::Vector3d yuandian(0, 0, 0); // 世界坐标系下的点
// 10. 坐标变换：世界坐标 → 相机坐标
Eigen::Vector3d cameraPoint = R * yuandian + t;
cout << "装甲板中心在相机坐标系下的坐标：" << endl;
cout << "X: " << cameraPoint(0) << " mm" << endl;
cout << "Y: " << cameraPoint(1) << " mm" << endl;
cout << "Z: " << cameraPoint(2) << " mm" << endl;
// 11. 计算相机坐标系原点到世界坐标系原点的距离
double distance = t.norm(); // 等价于 sqrt(tx*tx + ty*ty + tz*tz)
std::cout << "\n相机坐标系原点到世界坐标系原点的距离：" << std::endl;
std::cout << "distance = " << distance << " mm" << std::endl;
// 12. 使用re_projection函数进行重投影
Eigen::Matrix3d M;
for (int i = 0; i < 3; ++i)
for (int j = 0; j < 3; ++j)
M(i, j) = camera.at<double>(i, j);
cv::Point2d centerPixel = re_projection(cameraPoint, M);
std::cout << "\n重投影到图像上的像素坐标：" << std::endl;
std::cout << "u: " << centerPixel.x << " 像素" << std::endl;
std::cout << "v: " << centerPixel.y << " 像素" << std::endl;
// 13. 可视化结果
Mat result3 = image.clone();
circle(result3, centerPixel, 8, Scalar(0, 255, 255), -1); // 绘制黄色中心点
putText(result3, cv::format("(%.1f, %.1f)", centerPixel.x, centerPixel.y),
centerPixel + Point2d(10, -10),
FONT_HERSHEY_SIMPLEX, 0.7, Scalar(0, 255, 255), 2);
imshow("装甲板中心重投影", result3);
waitKey(0);
// 发布TF变换
publishTransform(rvec, tvec);
}
// 发布处理后的图像
auto result_msg = cv_bridge::CvImage(msg->header, "bgr8", image1).toImageMsg();
image_pub_.publish(result_msg);
} catch (const cv_bridge::Exception& e) {
RCLCPP_ERROR(this->get_logger(), "CV Bridge error: %s", e.what());
}
}
void publishTransform(const cv::Mat& rvec, const cv::Mat& tvec)
{
// 转换为tf2 Transform
tf2::Transform transform;
cv::Mat rotation_mat;
cv::Rodrigues(rvec, rotation_mat);
tf2::Matrix3x3 tf_rot(
rotation_mat.at<double>(0,0), rotation_mat.at<double>(0,1), rotation_mat.at<double>(0,2),
rotation_mat.at<double>(1,0), rotation_mat.at<double>(1,1), rotation_mat.at<double>(1,2),
rotation_mat.at<double>(2,0), rotation_mat.at<double>(2,1), rotation_mat.at<double>(2,2));
transform.setBasis(tf_rot);
transform.setOrigin(tf2::Vector3(
tvec.at<double>(0),
tvec.at<double>(1),
tvec.at<double>(2)));
// 创建TransformStamped消息
geometry_msgs::msg::TransformStamped tf_msg;
tf_msg.header.stamp = this->now();
tf_msg.header.frame_id = "camera_frame";
tf_msg.child_frame_id = "armor_center";
tf_msg.transform = tf2::toMsg(transform);
tf_broadcaster_->sendTransform(tf_msg);
}
};
int main(int argc,char** argv)
{
rclcpp::init(argc,argv);
auto node = std::make_shared<AromorDector>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
最新发布
本文展示了一种使用C++标准库中的std::max函数替代直接比较变量的方法。通过一个简单的例子，即比较两个整数i和j并取较大值，演示了如何利用模板函数std::max来实现这一功能。
扫一扫
554
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
