点准了
This commit is contained in:
parent
88c5e2b6fb
commit
21d105a0fc
@ -7,6 +7,7 @@ import sensor_msgs_py.point_cloud2 as pc2
|
||||
from visualization_msgs.msg import Marker
|
||||
import std_msgs.msg
|
||||
import geometry_msgs.msg
|
||||
import collections
|
||||
|
||||
class RectangleDetector(Node):
|
||||
def __init__(self):
|
||||
@ -20,11 +21,37 @@ class RectangleDetector(Node):
|
||||
self.marker_pub = self.create_publisher(Marker, '/rectangle_marker', 10)
|
||||
# 可选：发布原始点云
|
||||
self.cloud_pub = self.create_publisher(PointCloud2, '/rectangle_cloud', 10)
|
||||
self.history = collections.deque(maxlen=5)
|
||||
self.last_bbox = None
|
||||
self.last_time = None
|
||||
|
||||
# 目标矩形尺寸（单位：米）
|
||||
self.length = 1.8
|
||||
self.width = 1.05
|
||||
self.thickness = 0.04
|
||||
def filter_points(self, points):
|
||||
# 统计滤波：去除离群点
|
||||
pcd = o3d.geometry.PointCloud()
|
||||
pcd.points = o3d.utility.Vector3dVector(points)
|
||||
cl, ind = pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)
|
||||
filtered = np.asarray(cl.points)
|
||||
# 半径滤波
|
||||
cl2, ind2 = cl.remove_radius_outlier(nb_points=10, radius=0.05)
|
||||
return np.asarray(cl2.points)
|
||||
|
||||
def smooth_bbox(self, bbox):
|
||||
# 保存历史中心和尺寸
|
||||
center = bbox.center
|
||||
extent = bbox.extent
|
||||
self.history.append((center, extent))
|
||||
# 计算加权平均
|
||||
centers = np.array([h[0] for h in self.history])
|
||||
extents = np.array([h[1] for h in self.history])
|
||||
smooth_center = np.mean(centers, axis=0)
|
||||
smooth_extent = np.mean(extents, axis=0)
|
||||
bbox.center = smooth_center
|
||||
bbox.extent = smooth_extent
|
||||
return bbox
|
||||
|
||||
|
||||
def publish_marker(self, bbox):
|
||||
@ -33,17 +60,37 @@ class RectangleDetector(Node):
|
||||
marker.header.stamp = self.get_clock().now().to_msg()
|
||||
marker.ns = "rectangle_corners"
|
||||
marker.id = 0
|
||||
marker.type = Marker.SPHERE_LIST # 或 Marker.POINTS
|
||||
marker.type = Marker.SPHERE_LIST
|
||||
marker.action = Marker.ADD
|
||||
|
||||
# 获取8个旋转后的角点
|
||||
corners = np.asarray(bbox.get_box_points())
|
||||
center = bbox.center
|
||||
|
||||
# 展示角点
|
||||
for c in corners:
|
||||
pt = geometry_msgs.msg.Point()
|
||||
pt.x, pt.y, pt.z = c
|
||||
marker.points.append(pt)
|
||||
|
||||
marker.scale.x = 0.08 # 球半径或点大小
|
||||
# 展示中心点
|
||||
center_pt = geometry_msgs.msg.Point()
|
||||
center_pt.x, center_pt.y, center_pt.z = center
|
||||
marker.points.append(center_pt)
|
||||
|
||||
# 沿中心点到原点方向，向外移动10cm
|
||||
origin = np.array([0.0, 0.0, 0.0])
|
||||
direction = origin - center
|
||||
norm = np.linalg.norm(direction)
|
||||
if norm > 0:
|
||||
move_vec = direction / norm * 0.46
|
||||
moved_pt = center + move_vec
|
||||
else:
|
||||
moved_pt = center
|
||||
out_pt = geometry_msgs.msg.Point()
|
||||
out_pt.x, out_pt.y, out_pt.z = moved_pt
|
||||
marker.points.append(out_pt)
|
||||
|
||||
marker.scale.x = 0.08
|
||||
marker.scale.y = 0.08
|
||||
marker.scale.z = 0.08
|
||||
marker.color.r = 0.0
|
||||
@ -53,7 +100,6 @@ class RectangleDetector(Node):
|
||||
marker.lifetime.sec = 1
|
||||
marker.lifetime.nanosec = 0
|
||||
self.marker_pub.publish(marker)
|
||||
# ...existing code...
|
||||
# ...existing code...
|
||||
|
||||
def pointcloud_callback(self, msg):
|
||||
@ -68,39 +114,47 @@ class RectangleDetector(Node):
|
||||
return
|
||||
points = np.array(points_list)
|
||||
|
||||
# 发布原始点云到RViz
|
||||
# 点云预处理
|
||||
# points = self.filter_points(points)
|
||||
if len(points) == 0:
|
||||
self.get_logger().info('滤波后点云为空')
|
||||
return
|
||||
|
||||
self.cloud_pub.publish(msg)
|
||||
|
||||
# 转换为Open3D点云对象
|
||||
pcd = o3d.geometry.PointCloud()
|
||||
pcd.points = o3d.utility.Vector3dVector(points)
|
||||
|
||||
# 添加体素滤波，滤波分辨率可调
|
||||
voxel_size = 0.03 # 3cm分辨率
|
||||
voxel_size = 0.03
|
||||
pcd = pcd.voxel_down_sample(voxel_size=voxel_size)
|
||||
|
||||
# 使用RANSAC拟合矩形框（近似为平面+边界筛选）
|
||||
plane_model, inliers = pcd.segment_plane(
|
||||
distance_threshold=0.02,
|
||||
ransac_n=3,
|
||||
num_iterations=2000
|
||||
)
|
||||
inlier_cloud = pcd.select_by_index(inliers)
|
||||
# bbox = inlier_cloud.get_axis_aligned_bounding_box()
|
||||
bbox = inlier_cloud.get_oriented_bounding_box() # 修改为贴合实际方向
|
||||
bbox = inlier_cloud.get_oriented_bounding_box()
|
||||
extent = bbox.extent
|
||||
center = bbox.center
|
||||
|
||||
self.get_logger().info(f'滤波后bbox中心: {center}, 尺寸: {extent}')
|
||||
|
||||
dims = sorted(extent)
|
||||
if (abs(dims[0] - self.thickness) < 1.0 and
|
||||
abs(dims[1] - self.width) < 1.0 and
|
||||
abs(dims[2] - self.length) < 1.0):
|
||||
self.get_logger().info(f'检测到目标矩形框，位置: {center}, 尺寸: {extent}')
|
||||
# 检测阈值收紧
|
||||
if (abs(dims[0] - self.thickness) < 0.1 and
|
||||
abs(dims[1] - self.width) < 0.2 and
|
||||
abs(dims[2] - self.length) < 0.2):
|
||||
bbox = self.smooth_bbox(bbox)
|
||||
self.get_logger().info(f'检测到目标矩形框，位置: {bbox.center}, 尺寸: {bbox.extent}')
|
||||
self.publish_marker(bbox)
|
||||
self.last_bbox = bbox
|
||||
self.last_time = self.get_clock().now()
|
||||
else:
|
||||
self.get_logger().info('未检测到目标矩形框')
|
||||
# 若上一帧有结果且时间间隔<0.5s，则继续发布上一帧
|
||||
now = self.get_clock().now()
|
||||
if self.last_bbox and self.last_time and (now - self.last_time).nanoseconds < 5e8:
|
||||
self.get_logger().info('本帧未检测到，使用上一帧结果')
|
||||
self.publish_marker(self.last_bbox)
|
||||
else:
|
||||
self.get_logger().info('未检测到目标矩形框')
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user