(1.5.2.10)寻找最近点对

本文介绍了几种寻找平面上最近点对的有效算法。首先探讨了暴力求解法,然后提出了一种减少比较次数的方法,并详细讲解了分治法的实现过程及代码示例。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

题目:平面中有若干个点,寻找距离最近的两个点。

分析:

方法1:两两点比较,寻找最近的两个点对,复杂度O(N^2),优点代码简单不容易出错

方法2:观察两两比较的方法,发现有很多无用的比较,对于每一个点只要计算到它最近的点的距离就可以了,枚举所有的点,最后得出距离最近的一对点,但对于一个给定的点,如何找到距离它最近的点呢?可以使用一些启发式规则,减少比较的次数,例如:对于(x,y)取x轴投影上最近的点p1(x1,y1)和取y轴上投影最近的点p2(x2,y2),以(x,y)为一个定点,取x1,x2中较大者x',y1,y2中较大者y',构成(x‘,y’)为矩形的另一个顶点,计算矩形内所有的点与(x,y)的距离,从而减少了比较的次数。再以(x,y)为中心,判断4个项限内的所有点,最后得出距离(x,y)最小的点。复杂度是O(N*K),K为要判断的点的平均个数


方法3:采用分治法,即从x轴将数据不断分成两部分,两部分最近的两点取其小,在进行两部分中间比较,通过两部分的最小距离,缩小了中间带状的点数量,详见编程之美的分析。

算法 

核心是分治算法

1. 分别根据点的 x,y 值进行排序

2. 在 x 轴上划一道垂线, 将点均分成两半

3. 假设最近点对都在左/右部分, 递归计算左/右半部分的最短距离并返回较小值 dis

4. 假设最近点对分别在左右两个部分, 横跨中心的竖线. 中心线为中心, 2*dis 为宽度画一个矩形, 横跨中心线的最近点对 candidate 都在这个矩形内. 将这些点按照 y 值的大小加入到数组中. 遍历数组中的点, 将该点与其后的 7 个点计算距离, 返回最小距离

5. 为什么仅和 7 个点作对比呢. 因为已经假设 dis 是左右不分最近点对的最小值, 这就说明在一个长(宽)为 dis 的正方形内, 至多有 4 个点. 长为 dis*2, 宽为 dis 的长方形至多 8 个.


按照方法3的程序如下:


[cpp]  view plain copy
  1. #include <stdio.h>  
  2. #include <algorithm>  
  3. #include <vector>  
  4. #include <math.h>  
  5. class Point {  
  6.  public:  
  7.   Point(int x, int y) : x_(x), y_(y) {}  
  8.   Point() : x_(0), y_(0) {}  
  9.   static bool OrderByX(const Point& left, const Point& right) {  
  10.     return left.x_ < right.x_;  
  11.   }  
  12.   static bool OrderByY(const Point& left, const Point& right) {  
  13.     return left.y_ < right.y_;  
  14.   }  
  15.   int x_;  
  16.   int y_;  
  17. };  
  18. float Distance(const Point& left, const Point& right) {  
  19.   return sqrt(pow(left.x_ - right.x_, 2) + pow(left.y_ - right.y_, 2));  
  20. }  
  21. int NearestPoints(const std::vector<Point>& points, int start, int end, Point* point1, Point* point2) {  
  22.   if (end > start) {  
  23.   int middle = (start + end) / 2;  
  24.   int left_min_distance = NearestPoints(points, start, middle, point1, point2);  
  25.   int right_min_distance = NearestPoints(points, middle + 1, end, point1, point2);  
  26.   int min_distance = left_min_distance > right_min_distance ? right_min_distance : left_min_distance;  
  27.   std::vector<Point> left_part_points;  
  28.   for (int i = start; i <= middle; ++i) {  
  29.     if (points[middle].x_ - points[i].x_ <= min_distance) {  
  30.       left_part_points.push_back(points[i]);  
  31.     }  
  32.   }  
  33.   sort(left_part_points.begin(), left_part_points.end(), Point::OrderByY);  
  34.   std::vector<Point> right_part_points;  
  35.   for (int i = middle + 1; i <= end; ++i) {  
  36.     if (points[i].x_ - points[middle].x_ <= min_distance) {  
  37.       right_part_points.push_back(points[i]);  
  38.     }  
  39.   }  
  40.   sort(right_part_points.begin(), right_part_points.end(), Point::OrderByY);  
  41.   int distance_y = 0;  
  42.   int point_distance = 0;  
  43.   for(int i = 0; i < left_part_points.size(); ++i) {  
  44.     for(int j = 0; j < right_part_points.size(); ++j) {  
  45.       distance_y = left_part_points[i].y_ > right_part_points[j].y_ ? left_part_points[i].y_ - right_part_points[j].y_ :  
  46.                    right_part_points[j].y_ - left_part_points[i].y_;  
  47.       if (distance_y <= min_distance) {  
  48.         point_distance = Distance(left_part_points[i], right_part_points[j]);  
  49.         if (point_distance < min_distance) {  
  50.           min_distance = point_distance;  
  51.           *point1 = left_part_points[i];  
  52.           *point2 = right_part_points[j];  
  53.         }  
  54.       }  
  55.     }  
  56.   }  
  57.   return min_distance;  
  58.   } else {  
  59.     return 0x7FFFFFFF;  
  60.   }  
  61. }  
  62.     
  63.       
  64. int main(int argc, char** argv) {  
  65.   std::vector<Point> points;  
  66.   points.push_back(Point(2,3));  
  67.   points.push_back(Point(1,4));  
  68.   points.push_back(Point(3,0));  
  69.   points.push_back(Point(5,0));  
  70.   points.push_back(Point(5,1));  
  71.   sort(points.begin(), points.end(), Point::OrderByX);  
  72.   Point point1;  
  73.   Point point2;  
  74.   NearestPoints(points, 0, points.size() - 1, &point1, &point2);  
  75.   printf("Point1: (%d, %d) <--> Point2: (%d, %d)\n", point1.x_, point1.y_, point2.x_, point2.y_);  
  76. }  


其中,寻找矩形带内的点可以通过二分搜索来寻找,为了简单,用了循环来寻找。


参考文献:

编程之美 2.11

if __name__ == '__main__': rospy.init_node('usb_only3', anonymous=True) rospy.Subscriber('/yolov5/object_detect', ObjectsInfo, BoundingBoxCallBack, queue_size=1) rospy.Subscriber('/usb_cam/image_raw', Image, cameraCallback, queue_size=1) rospy.Subscriber('/scan', LaserScan, laserCallback, queue_size=1) rospy.Subscriber('/qr/info', qrcode, qrCallback, queue_size=1) move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) move_base.wait_for_server(rospy.Duration(5.0)) rospy.logwarn("Connected to move base server") graspd = graspDemo() while inputvalue==0: inputvalue=input("输入任何非零数以启动:") if stage ==0: graspd.go_named('trans') rospy.sleep(0.5) client(0.87,0.39,0.0,0.1)#A> rotation(90,1)#A^ client(0.84,3.55,0.707,0.707)#B^ rotation(85,-1)#B> move2(2.0) client(5.18,3.59,0.0,0.1)#C> rotation(90,-1)#C! client(5.17,0.93,-0.707,0.707)#X! rotation(360,-1)#X< stage=stage+1 # modify a<=3 to a<=4 if stage ==1: while a<= 3: # rospy.loginfo(">>>>> >>>>> >>>>> %s %s",gobject[a],QR[a]) client(2.10,1.65,0.1,0.0)#D< las_switch = True findObject = False grasp_new() move2(-0.8) rotation(90,1) # client(3.60,0.54,-0.766, 0.643)#E!100 client(3.58,0.61,-0.737, 0.676)#E!95 # las_switch = True release() rospy.sleep(0.5) move2(-1.5) rospy.sleep(0.5) rotation(180,1) # rotation(90,1) findQR=False a = a+1 if a == 4: break stage = stage+1 if stage==2: graspd.go_named('trans') client(5.45,0.17,0.0,0.1)#F< add_thread3 = threading.Thread(target=thread_play(9)) add_thread3.start() add_thread1 = threading.Thread(target=thread_job1) add_thread1.start()解释这个main函数的作用和功能
最新发布
03-29
<think>好的,用户让我解释这个main函数的作用和功能。首先,我需要通读用户提供的代码,理解各个部分的功能。这段代码看起来是ROS(机器人操作系统)中的一个节,涉及到订阅多个话题、动作客户端、线程以及一些自定义函数。 首先,用户可能不太熟悉ROS的结构,所以需要先解释一下ROS的基本概念,比如节、话题、订阅者和发布者。然后,代码里用了rospy.init_node,这说明这是初始化一个ROS节,名字是'usb_only3',并且是匿名的。匿名节通常用于防止名称冲突,可能在调试或多次启动时使用。 接下来是四个Subscriber,分别订阅了不同的ROS话题。比如'/yolov5/object_detect'可能来自YOLOv5的目标检测结果,'/usb_cam/image_raw'是摄像头图像,'/scan'是激光雷达数据,'/qr/info'是二维码信息。每个订阅都关联了一个回调函数,比如BoundingBoxCallBack、cameraCallback等。用户可能想知道这些回调函数的作用,但代码中没有给出具体实现,可能需要解释这些回调函数可能处理的数据类型,比如ObjectsInfo、Image、LaserScan、qrcode等消息类型。 然后,创建了一个move_base的SimpleActionClient,连接到move_base动作服务器,用于导航。用户可能对动作客户端不太熟悉,需要说明动作在ROS中的用途,比如用于执行长时间运行的任务,如导航到目标,并提供反馈。 接下来是graspd = graspDemo(),这可能是一个自定义的类,用于执行抓取操作。用户可能不了解这个类的具体功能,但可以推测它封装了一些抓取相关的动作,比如移动到预设位置。 然后有一个循环等待输入,直到输入非零值才会继续。这可能是为了在启动前等待用户确认,确保一切就绪。用户可能好奇为什么需要这样的输入,可能用于调试或分步执行。 stage变量控制程序的不同阶段。stage 0中,调用了graspd.go_named('trans'),可能是指移动到某个预定义的位置,然后通过client函数发送导航目标,并进行旋转和移动。这里client函数可能是发送导航指令,参数可能包括坐标和方向。用户可能对client函数的参数具体含义不清楚,比如0.87,0.39,0.0,0.1可能代表x, y, z, w四元数,或者其它坐标形式。需要解释这些参数的可能意义,以及rotation函数的作用,比如旋转多少度,方向由参数决定。 在stage 1中,有一个循环直到a<=3,可能对应四个物体(因为a从0到3),每次循环执行一系列动作:移动到某个位置,开启激光,寻找物体,抓取,移动,旋转,释放物体,然后调整位置。这里可能涉及到抓取和放置物体的流程,用户可能想了解每个步骤的具体作用,比如las_switch控制激光,findObject标志物体是否找到,grasp_new()执行抓取动作,release()释放物体等。循环结束后stage增加到2,进入下一个阶段。 stage 2中,再次移动到某个位置,并启动线程thread_play(9),可能播放某种动作或音频。同时,启动另一个线程thread_job1,可能处理后台任务,比如持续监控或数据更新。 用户可能的问题包括:各个订阅的回调函数如何处理数据?move_base如何导航到指定?线程的作用是什么?stage的设计逻辑是怎样的?变量如a、las_switch、findObject如何影响程序流程? 需要注意的是,用户提供的代码中有一些注释,比如#A>、#B^等,可能是标记不同位置或动作,但具体含义需要结合项目上下文。此外,代码中的函数如client、rotation、move2、grasp_new、release、thread_play、thread_job1都是自定义的,没有给出实现,可能需要根据命名推测其功能。 用户可能希望了解整个main函数的工作流程,从初始化节到各个阶段的导航、抓取、放置,以及多线程的使用。同时,需要指出代码可能的用途,比如自动化抓取和搬运物体,结合视觉和激光雷达感知,通过ROS进行系统集成。 总结来说,这个main函数实现了一个多阶段的机器人控制流程,包括初始化、订阅传感器数据、导航到指定位置、抓取和放置物体,以及最后的清理或结束动作,使用多线程处理并发任务。用户可能需要进一步的细节解释各个部分如何协同工作,或者某些特定函数的作用。</think>这是一个基于ROS的机器人控制主程序,主要实现多传感器融合的物体抓取任务。以下分模块解析其功能: ### 一、ROS系统初始化 ```python rospy.init_node('usb_only3', anonymous=True) ``` 1. 创建名为`usb_only3`的ROS节,`anonymous=True`防止节重名冲突 2. 订阅关键传感器数据: - `/yolov5/object_detect`:YOLOv5目标检测结果(消息类型`ObjectsInfo`) - `/usb_cam/image_raw`:USB相机原始图像(`Image`消息) - `/scan`:激光雷达扫描数据(`LaserScan`消息) - `/qr/info`:二维码识别结果(`qrcode`消息) 3. 连接导航服务器: ```python move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) ``` 使用move_base包实现自主导航功能 ### 二、主控制流程 采用**分阶段状态机**设计(`stage`变量控制) #### 阶段0:导航初始化 ```python graspd.go_named('trans') # 移动到预设的过渡位置 client(0.87,0.39,0.0,0.1) # 发送导航目标(x,y,orientation) rotation(90,1) # 执行90度旋转,参数1表示方向 ``` 1. 通过`client()`发送坐标$(x,y)$和四元数$(qx,qy,qz,qw)$ 2. `rotation()`实现精确角度调整,参数2为旋转方向(1/-1) #### 阶段1:物体抓取循环 ```python while a <= 3: # 遍历4个物体(a从0开始) grasp_new() # 执行抓取动作 release() # 释放物体 rotation(180,1) # 调整朝向 ``` 1. `las_switch=True`激活激光测距 2. `findObject=False`重置检测状态 3. 配合`move2()`进行直线位移(参数为移动距离) #### 阶段2:任务收尾 ```python graspd.go_named('trans') # 返回过渡位置 client(5.45,0.17,0.0,0.1) # 最终导航 thread_play(9) # 启动编号9的演示动作 ``` ### 三、多线程控制 ```python threading.Thread(target=thread_job1) # 后台线程1 ``` 1. `thread_job1`可能用于: - 持续监控系统状态 - 处理传感器数据融合 - 异常状态检测 2. `thread_play(9)`单独线程执行展示动作 ### 四、关键技术特征 1. **分层状态机**:通过`stage`变量实现阶段切换 2. **位姿控制**:组合使用绝对坐标导航(`client()`)和相对运动(`rotation()`, `move2()`) 3. **多模态感知**:融合视觉(YOLOv5)、激光雷达(laser)、二维码(qrcode)数据 4. **动作同步**:`rospy.sleep()`确保动作序列的时序正确性 ### 五、典型工作流程 1. 等待用户输入启动(`inputvalue`检测) 2. 按预定路径移动到作业区域 3. 循环抓取4个目标物体 4. 返回起始并执行结束动作 5. 后台线程持续运行保障系统监控 该程序展现了一个典型的工业机器人抓取系统实现方案,适用于物流分拣、生产线装配等场景。
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值