#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <image_transport/image_transport.h>
#include <vector>
#include <pcl/common/common.h>
#include <pcl/common/pca.h>
#include <algorithm>
#include <limits>
Eigen::Matrix4f estimatePoseMatrix(const std::vector<pcl::PointXYZ>& points) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
for (const auto& pt : points) {
cloud->points.push_back(pt);
}
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*cloud, centroid);
pcl::PCA<pcl::PointXYZ> pca;
pca.setInputCloud(cloud);
Eigen::Matrix3f eigenVectors = pca.getEigenVectors();
Eigen::Matrix4f poseMatrix = Eigen::Matrix4f::Identity();
poseMatrix.block<3, 3>(0, 0) = eigenVectors;
poseMatrix.block<3, 1>(0, 3) = centroid.head<3>();
return poseMatrix;
}
void imageCallback(const sensor_msgs::ImageConstPtr& color_msg,
const sensor_msgs::PointCloud2ConstPtr& cloud_msg,
const image_transport::Publisher& processed_image_pub) {
try {
cv_bridge::CvImagePtr cv_color = cv_bridge::toCvCopy(color_msg, sensor_msgs::image_encodings::BGR8);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*cloud_msg, *cloud);
cv::Mat gray_image, threshold_output;
cv::cvtColor(cv_color->image, gray_image, cv::COLOR_BGR2GRAY);
cv::threshold(gray_image, threshold_output, 200, 255, cv::THRESH_BINARY);
std::vector<std::vector<cv::Point>> contours;
cv::findContours(threshold_output, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
double maxArea = 0;
std::vector<cv::Point> largest_contour;
for (const auto& contour : contours) {
double area = cv::contourArea(contour);
if (area > maxArea) {
maxArea = area;
largest_contour = contour;
}
}
std::vector<pcl::PointXYZ> nearest_points;
if (!largest_contour.empty()) {
// 计算轮廓的中心
cv::drawContours(cv_color->image, std::vector<std::vector<cv::Point>>{largest_contour}, -1, cv::Scalar(0, 255, 0), 2);
// 计算轮廓点的3D坐标
cv::Moments m = cv::moments(largest_contour);
cv::Point2f center(m.m10 / m.m00, m.m01 / m.m00);
// 寻找离中心最近的十个点
std::vector<std::pair<float, pcl::PointXYZ>> distance_point_pairs;
for (const auto& point : cloud->points) {
float distance = std::sqrt(std::pow(point.x - center.x, 2) + std::pow(point.y - center.y, 2));
distance_point_pairs.push_back(std::make_pair(distance, point));
}
std::nth_element(distance_point_pairs.begin(), distance_point_pairs.begin() + 10, distance_point_pairs.end(),
[](const std::pair<float, pcl::PointXYZ>& a, const std::pair<float, pcl::PointXYZ>& b) {
return a.first < b.first;
});
for (int i = 0; i < 10 && i < distance_point_pairs.size(); ++i) {
nearest_points.push_back(distance_point_pairs[i].second);
}
// 使用这些点进行位姿估计
if (!nearest_points.empty()) {
Eigen::Matrix4f poseMatrix = estimatePoseMatrix(nearest_points);
std::cout << "Estimated Pose Matrix:\n" << poseMatrix << std::endl;
}
}
sensor_msgs::ImagePtr output_msg = cv_bridge::CvImage(color_msg->header, "bgr8", cv_color->image).toImageMsg();
processed_image_pub.publish(output_msg);
} catch (const cv_bridge::Exception& e) {
ROS_ERROR("cv_bridge exception: %s", e.what());
}
}
int main(int argc, char** argv) {
ros::init(argc, argv, "object_detection");
ros::NodeHandle nh;
image_transport::ImageTransport it(nh);
message_filters::Subscriber<sensor_msgs::Image> color_sub(nh, "/global_cam/rgb/rgb_raw", 1);
message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub(nh, "/global_cam/depth/berxel_cloudpoint", 1);
image_transport::Publisher processed_image_pub = it.advertise("/camera/processed_image", 1);
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::PointCloud2> SyncPolicy;
message_filters::Synchronizer<SyncPolicy> sync(SyncPolicy(10), color_sub, cloud_sub);
sync.registerCallback(boost::bind(&imageCallback, _1, _2, processed_image_pub));
ros::spin();
return 0;
}