Top K nearest points to the origin

本文介绍了一种使用最大堆实现寻找二维坐标系中距离原点最近的K个点的方法。通过定义比较函数并利用最大堆结构,可以高效地筛选出所需的点集。

There are n points in the xy-coordinate system. Find the top K nearest points to the origin.

{(1,2), (-1,2), (3.-3),...}

The idea for this problem is to use the max_heap(yes, the max heap, NOT the min heap) with size K, and define the compare function.

// top K nearest points to the origin
// a problem from mitbbs.com

struct Point {
    double x;
	double y;
	Point(double xx, double yy): x(xx), y(yy){};
};

double distance(const Point &p) {
    return p.x*p.x + p.y*p.y;
}

struct max_heap_compare {
	bool operator()(const Point &a, const Point &b) const {
		return distance(a) < distance(b);
	}
};

class Solution {
public:
	std::vector<Point> topKNearest(std::vector<Point> &pts, int k) {
	    std::vector<Point> res;
		if (pts.empty()) return res;
		std::priority_queue<Point, std::vector<Point>, max_heap_compare> max_heap;
		auto p = pts.begin();
		while(int(max_heap.size()) < k && p!= pts.end()) {
			Point a_pt(p->x, p->y);
			max_heap.push(a_pt);
			++p;
		}
		while(p!=pts.end()) {
			auto& top_pt = max_heap.top();
			if(distance(top_pt) > distance(*p))
			{
				Point ap(p->x, p->y);
				max_heap.pop();
				max_heap.push(ap);
			}
			++p;			
		}
		
		while(!max_heap.empty()) {
			Point p = max_heap.top();
			max_heap.pop();
			res.push_back(p);
		}
		return res;
	}
};


修改下面的代码,要求实现如下效果: 1.将矩形和矩形中点改成长方体和三维点 2.将split_by_angle的角度判断改为在x,y平面的角度判定,z不限 import math import numpy as np import matplotlib.pyplot as plt import matplotlib matplotlib.rc("font", family='YouYuan') EPS = 0.0001 MAX_ITER = 100 show_animation = False # ===== ICP 相关函数 ===== def icp_matching(previous_points, current_points): """ Iterative Closest Point matching """ H = None dError = np.inf preError = np.inf count = 0 while dError >= EPS: count += 1 indexes, error = nearest_neighbor_association(previous_points, current_points) Rt, Tt = svd_motion_estimation(previous_points[:, indexes], current_points) # 更新当前点 current_points = (Rt @ current_points) + Tt[:, np.newaxis] dError = preError - error if dError < 0: break preError = error H = update_homogeneous_matrix(H, Rt, Tt) if dError <= EPS or MAX_ITER <= count: break R = np.array(H[0:-1, 0:-1]) T = np.array(H[0:-1, -1]) return R, T def update_homogeneous_matrix(Hin, R, T): r_size = R.shape[0] H = np.zeros((r_size + 1, r_size + 1)) H[0:r_size, 0:r_size] = R H[0:r_size, r_size] = T H[r_size, r_size] = 1.0 if Hin is None: return H else: return Hin @ H def nearest_neighbor_association(previous_points, current_points): """ 找到 current_points 中每个点在 previous_points 中的最近邻索引。 """ n_prev = previous_points.shape[1] n_curr = current_points.shape[1] d = np.zeros((n_curr, n_prev)) for i in range(n_curr): diff = previous_points - current_points[:, i:i+1] d[i, :] = np.linalg.norm(diff, axis=0) indexes = np.argmin(d, axis=1) error = np.mean(np.min(d, axis=1)) return indexes, error def svd_motion_estimation(previous_points, current_points): pm = np.mean(previous_points, axis=1) cm = np.mean(current_points, axis=1) p_shift = previous_points - pm[:, np.newaxis] c_shift = current_points - cm[:, np.newaxis] W = c_shift @ p_shift.T u, s, vh = np.linalg.svd(W) R = (u @ vh).T t = pm - (R @ cm) return R, t def loop_closure(R_list, T_list, R_total, T_total, a, k, threshold=0.1): """ 回环估计:对于时刻 a 和时刻 a+k, 计算 ICP 变换,并更新回环变换。 """ if a + k >= len(R_list): # 如果 a+k 时刻不存在,跳过 return R_total, T_total A_prev = R_list[a] A_curr = R_list[a + k] # 使用 ICP 得到 A_a 和 A_(a+k) 之间的相对变换矩阵 R_a,a+k 和 T_a,a+k R_a_k, T_a_k = icp_matching(A_prev, A_curr) # 计算新的全局变换:R = R_a,a+k @ R_a 和 T = R_a,a+k @ T_a + T_a,a+k R_new = R_a_k @ R_total[a] T_new = R_a_k @ T_total[a] + T_a_k # 比较新旧变换,计算误差 error = np.linalg.norm(R_new - R_total[a]) + np.linalg.norm(T_new - T_total[a]) if error < threshold: # 如果误差足够小,执行调整,优化变换 R_total[a + k] = R_new T_total[a + k] = T_new print(f"回环检测成功,优化变换:R_new=\n{R_new}, T_new={T_new}") else: print(f"回环检测失败,误差过大:误差={error}") return R_total, T_total def interpolate(x_1, x_2, delta): """ 插值函数:对两个坐标点进行线性插值 """ return (1.0 - delta) * x_1 + delta * x_2 def generate_rectangle_points(length=20.0, width=10.0, x=0.0, y=0.0, theta=0,fen=0.005): """生成矩形边界点云并加噪声""" k=np.arctan2(width,length) t=np.sqrt((length/2)**2+(width/2)**2) top = np.array([x + t*np.cos(theta+k), y + t*np.sin(theta + k)]) bottom = np.array([x + t*np.cos(theta-k), y +t*np.sin(theta -k)]) left = np.array([x + t*np.cos(theta+k-np.pi), y +t*np.sin(theta+k-np.pi)]) right = np.array([x + t*np.cos(theta+np.pi-k), y +t*np.sin(theta +np.pi-k)]) points = [top, bottom, left, right] len_points = len(points) DELTA_LIST = np.arange(0.0, 1.0, fen) for i in range(len_points - 1): for delta in DELTA_LIST: points.append(interpolate(points[i], points[i+1], delta)) for delta in DELTA_LIST: points.append(interpolate(points[len_points-1], points[0], delta)) x_coords = [point[0] for point in points] y_coords = [point[1] for point in points] points= [x_coords, y_coords] print(points) return np.array(points) def split_by_angle(points, center=np.array([0, 0]), start=-30, step=8): """按角度扇区划分点云""" sectors = [] angles = np.degrees(np.arctan2(points[1, :] - center[1], points[0, :] - center[0])) angles = (angles + 360) % 360 t=int(360/step) for k in range(t): theta1 = (start + step*np.random.uniform(0,2)) % 360 theta2 = (theta1 + 60) % 360 # 每个扇区宽度为60° start=theta1 if theta1 < theta2: mask = (angles >= theta1) & (angles <= theta2) else: # 角度跨越360°边界 mask = (angles >= theta1) | (angles <= theta2) Ak = points[:, mask] sectors.append(Ak) return sectors def merge_point_clouds(A_list, R_list, T_list): """将所有点云A_k变换到A1坐标系并融合""" merged = [A_list[0]] R_total = [np.eye(2)] T_total = [np.zeros(2)] # 累积每个点云的全局变换 for k in range(1, len(A_list)): Rk, Tk = R_list[k - 1], T_list[k - 1] R_acc = R_total[-1] @ Rk T_acc = R_total[-1] @ T_list[k - 1] + T_total[-1] R_total.append(R_acc) T_total.append(T_acc) Ak_global = R_acc @ A_list[k] + T_acc[:, None] merged.append(Ak_global) merged_points = np.hstack(merged) merged_points = np.unique(np.round(merged_points, 2), axis=1) return merged_points, R_total, T_total # ===== 主逻辑 ===== def main(): print("开始运行改造后的 ICP 例子...") # Step 1: 生成矩形点云 rect_points = generate_rectangle_points() rect_points2 = generate_rectangle_points(length=5, width=4,) rect_points3 = generate_rectangle_points(length=4,width=3,x=7,y=3) rect_points= np.hstack((rect_points,rect_points2,rect_points3)) print(rect_points) steps=0.2 # Step 2: 按角度扇区切分 sectors = split_by_angle(rect_points,center=np.array([0, 0]), step=steps) mk=int(360/steps) t=720 R_list, T_list = [], [] used_sectors = [] # Step 3: 对相邻扇区进行 ICP merged = [sectors[0]] for k in range(1, len(sectors)): A_prev = sectors[k ] A_curr = sectors[k-1] if A_prev.shape[1] < 5 or A_curr.shape[1] < 5: continue R, T = icp_matching(A_prev, A_curr) R_list.append(R) T_list.append(T) used_sectors.append(A_curr) print(f"ICP {k}: R=\n{R}\nT={T}") # Step 4: 回环估计和全局优化 R_total = [np.eye(2)] T_total = [np.zeros(2)] # 累积每个点云的全局变换 for k in range(1, len(R_list)): Rk, Tk = R_list[k - 1], T_list[k - 1] R_acc = R_total[-1] @ Rk T_acc = R_total[-1] @ T_list[k - 1] + T_total[-1] R_total.append(R_acc) T_total.append(T_acc) for a in range(mk): # 假设有 10 个时刻 # 比如从 a+1 到 a+2 if a + t < len(R_list): R_total, T_total = loop_closure(R_list, T_list, R_total, T_total, a, t) for k in range(1, len(R_list)): Ak_global = R_total[k] @ sectors[k] + T_total[k][:, None] merged.append(Ak_global) # Step 5: 融合所有点云到A1坐标系 merged_points = np.hstack(merged) merged_points = np.unique(np.round(merged_points, 2), axis=1) # Step 6: 可视化结果 plt.figure() plt.scatter(rect_points[0, :], rect_points[1, :], s=3, label="原始矩形点云") plt.scatter(merged_points[0, :], merged_points[1, :], s=3, label="ICP融合点云") plt.legend() plt.axis("equal") plt.title("ICP融合结果对比") plt.show() if __name__ == "__main__": main()
最新发布
11-08
评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
红包 添加红包
表情包 插入表情
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值