寻找最近路径点

已于 2024-03-17 19:26:37 修改
阅读量566 收藏 9
点赞数 6
文章标签： 自动驾驶 数据结构
于 2024-03-03 17:46:27 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/beiyangxiaokai/article/details/136433611
版权

功能概述

给定当前位置position:[x,y],给定路径点序列path：[[x1,y1];[x2,y2];...;[xn,yn]],size=n*2，返回path中距离position最近的路径点索引nearest_index与最短距离nearest_dis。另外还有1个可选参数，index_range: [range_upper,range_lower],表示nearest_index所在的范围，其存在的意义有2个，1是可以减少搜索范围，从而减少计算量，2是为了应对交叉路径，防止最近点突变，具体情况如下图所示：

在这里插入图片描述

黑点为历史的位置轨迹，旁边的绿点为对应的最近路径点，蓝点为当前位置，大部分情况下（例如在自动驾驶轨迹跟踪任务中），我们希望找到的蓝点的最近点为旁边的绿点，但是如果仅用最近距离来判断则会返回红点，如果我们用index_range限制最近点的范围，则可以避免这种情况。如果不设置index_range,则默认返回最短距离的路径点。

项目github地址

完整工程见个人github仓库：

https://github.com/PeiyangKai/find_nearestPoint.git

后续持续更新，感兴趣的朋友帮忙点个star,跪谢！

具体实现

遍历暴力搜索

matlab
function [nearest_index, nearest_dis]= find_nearest_points_new(position,path,index_range)
% 输入参数
% position: [x,y]
% path: [x1,y1;x2,y2;...;xn,yn]
% index_range: [range_upper,range_lower] range_upper>1,range_lower<size(path,1)
% 输出参数
% nearest_index 最近点索引 nearest_dis 最近点距离
if nargin < 3 || isempty(index_range)
    index_range = [1,size(path,1)];  % 如果没有提供 y 参数或者 y 参数为空，则使用默认值 10
end
start_index=index_range(1);
end_index=index_range(2);
new_path=path(start_index:end_index,1:2);
distance=(new_path(:,1)-position(1)).^2+(new_path(:,2)-position(2)).^2;
[nearest_dis,relative_index]=min(abs(distance));
nearest_dis=sqrt(nearest_dis);
nearest_index=start_index+relative_index-1;
end
python
import numpy as np

def find_nearest_points_new(position, path, index_range=None):
    # 输入参数
    # position: [x,y] 类型 np.ndarray
    # path: [x1,y1;x2,y2;...;xn,yn] 类型 np.ndarray
    # index_range: [range_upper,range_lower] range_upper>1,range_lower<size(path,1) 类型 np.ndarray
    # 输出参数
    # nearest_index 最近点索引 nearest_dis 最近点距离
    if index_range is None:
        index_range = [0, path.shape[0]]
    start_index, end_index = index_range
    new_path = path[start_index:end_index, :2]
    distance = np.sum((new_path - position) ** 2, axis=1)
    nearest_index = start_index + np.argmin(distance)
    nearest_dis = np.sqrt(np.min(distance))
    return nearest_index, nearest_dis


# 测试程序

if __name__=='__main__':
    position=np.array([5.1,5])
    path=np.array([[i,i] for i in range(10)])
    nearest_index, nearest_dis=find_nearest_points_new(position,path)
    print(nearest_index, nearest_dis)
c++
template <typename T>
struct Point2D
{
	T x, y;
	int index;
	Point2D() : x(0), y(0), index(-1) {};
	Point2D(T x, T y) : x(x), y(y), index(-1) {};
	Point2D(T x, T y, int index) : x(x), y(y), index(index) {};
	inline T& operator[](int i) {
		return i == 0 ? x : y;
	}
};

std::pair<int, double> find_nearest_points_traverse(Point2D<double> position, std::vector<Point2D<double>>& path, std::pair<int, int> index_range = { 0, -1 }) {
    // 输入参数
    // position: [x,y]
    // path: [x1,y1;x2,y2;...;xn,yn]
    // index_range: [range_upper,range_lower] range_upper>1,range_lower<size(path,1)
    // 输出参数
    // nearest_index 最近点索引 nearest_dis 最近点距离
    if (index_range.second == -1) {
        index_range.second = path.size();
    }
    int start_index = index_range.first;
    int end_index = index_range.second;
    std::vector<Point2D<double>> new_path(path.begin() + start_index, path.begin() + end_index);
    double nearest_dis = std::numeric_limits<double>::max();
    int nearest_index = start_index;
    for (int i = 0; i < new_path.size(); i++) {
        double distance = pow(new_path[i].x - position.x, 2) + pow(new_path[i].y - position.y, 2);
        if (distance < nearest_dis) {
            nearest_dis = distance;
            nearest_index = start_index + i;
        }
    }
    nearest_dis = sqrt(nearest_dis);
    return std::make_pair(nearest_index, nearest_dis);
}

KDtree高效搜索

KDtree搜索是一种高效的搜索方式，

使用说明

使用KDtree搜索之前需要先构建KDtree，相对于搜索最近点，构建KDtree的耗时更长。因此KDtree搜索并不太适合使用index_range参数，这是因为不同的index_range意味着不同的数据，需要构造不同的KDtree，这将大大增加耗时。

如果需要使用index_range参数，则更建议采用暴力遍历搜索，因为index_range已经限制了遍历搜索的范围，已经能够大量减少计算量，而KDtree则需要每次重新构建KDtree。

但为了使KDtree的搜索方式也可以适应index_range参数，采用了以下策略实现：
先搜索最近的10个点，检查是否存在点在index_range内，如果存在，则返回最近的点，如果不存在，则再次搜到最近的100个点，再次检查是否存在点在index_range内，不存在则搜索最近的1000个点，知道搜索到index_range内的最近点。（注意：该方法在某些情况很耗时，远超过遍历搜索）

c++

c++版本中，我们自己实现了KDTree的类，包括了KDTree的构建，以及K近邻搜索，其中K近邻搜索使用了BoundedPQueue。

  • BoundedPQueue.h（引用的作者Keith Schwarz的算法）
/**
 * Author: Keith Schwarz (htiek@cs.stanford.edu)
 *
 * An implementation of the bounded priority queue abstraction.
 * A bounded priority queue is in many ways like a regular priority
 * queue.  It stores a collection of elements tagged with a real-
 * valued priority, and allows for access to the element whose
 * priority is the smallest.  However, unlike a regular priority
 * queue, the number of elements in a bounded priority queue has
 * a hard limit that is specified in the constructor.  Whenever an
 * element is added to the bounded priority queue such that the
 * size exceeds the maximum, the element with the highest priority
 * value will be ejected from the bounded priority queue.  In this
 * sense, a bounded priority queue is like a high score table for
 * a video game that stores a fixed number of elements and deletes
 * the least-important entry whenever a new value is inserted.
 *
 * When creating a bounded priority queue, you must specify the
 * maximum number of elements to store in the queue as an argument
 * to the constructor.  For example:
 *
 * BoundedPQueue<int> bpq(15); // Holds up to fifteen values.
 *
 * The maximum size of the bounded priority queue can be obtained
 * using the maxSize() function, as in
 *
 * size_t k = bpq.maxSize();
 *
 * Beyond these restrictions, the bounded priority queue behaves
 * similarly to other containers.  You can query its size using
 * size() and check whether it is empty using empty().  You
 * can enqueue an element into the bounded priority queue by
 * writing
 *
 * bpq.enqueue(elem, priority);
 *
 * Note that after enqueuing the element, there is no guarantee
 * that the value will actually be in the queue.  If the queue
 * is full and the new element's priority exceeds the largest
 * priority in the container, it will not be added.
 *
 * You can dequeue elements from a bounded priority queue using
 * the dequeueMin() function, as in
 *
 * int val = bpq.dequeueMin();
 *
 * The bounded priority queue also allows you to query the min
 * and max priorities of the values in the queue.  These values
 * can be queried using the best() and worst() functions, which
 * return the smallest and largest priorities in the queue,
 * respectively.
 */

#ifndef BOUNDED_PQUEUE_INCLUDED
#define BOUNDED_PQUEUE_INCLUDED

#include <map>
#include <algorithm>
#include <limits>
#include <utility>

template <typename T>
class BoundedPQueue {
public:
    // Constructor: BoundedPQueue(size_t maxSize);
    // Usage: BoundedPQueue<int> bpq(15);
    // --------------------------------------------------
    // Constructs a new, empty BoundedPQueue with
    // maximum size equal to the constructor argument.
    ///
    explicit BoundedPQueue(std::size_t maxSize);

    // void enqueue(const T& value, double priority);
    // Usage: bpq.enqueue("Hi!", 2.71828);
    // --------------------------------------------------
    // Enqueues a new element into the BoundedPQueue with
    // the specified priority. If this overflows the maximum
    // size of the queue, the element with the highest
    // priority will be deleted from the queue. Note that
    // this might be the element that was just added.
    void enqueue(const T& value, double priority);

    // T dequeueMin();
    // Usage: int val = bpq.dequeueMin();
    // --------------------------------------------------
    // Returns the element from the BoundedPQueue with the
    // smallest priority value, then removes that element
    // from the queue.
    std::pair<T, double> dequeueMin();

    // size_t size() const;
    // bool empty() const;
    // Usage: while (!bpq.empty()) { ... }
    // --------------------------------------------------
    // Returns the number of elements in the queue and whether
    // the queue is empty, respectively.
    std::size_t size() const;
    bool empty() const;

    // size_t maxSize() const;
    // Usage: size_t queueSize = bpq.maxSize();
    // --------------------------------------------------
    // Returns the maximum number of elements that can be
    // stored in the queue.
    std::size_t maxSize() const;

    // double best() const;
    // double worst() const;
    // Usage: double highestPriority = bpq.worst();
    // --------------------------------------------------
    // best() returns the smallest priority of an element
    // stored in the container (i.e. the priority of the
    // element that will be dequeued first using dequeueMin).
    // worst() returns the largest priority of an element
    // stored in the container.  If an element is enqueued
    // with a priority above this value, it will automatically
    // be deleted from the queue.  Both functions return
    // numeric_limits<double>::infinity() if the queue is
    // empty.
    double best()  const;
    double worst() const;

private:
    // This class is layered on top of a multimap mapping from priorities
    // to elements with those priorities.
    std::multimap<double, T> elems;
    std::size_t maximumSize;
};

/** BoundedPQueue class implementation details */

template <typename T>
BoundedPQueue<T>::BoundedPQueue(std::size_t maxSize) {
    maximumSize = maxSize;
}

// enqueue adds the element to the map, then deletes the last element of the
// map if there size exceeds the maximum size.
template <typename T>
void BoundedPQueue<T>::enqueue(const T& value, double priority) {
    // Add the element to the collection.
    elems.insert(std::make_pair(priority, value));

    // If there are too many elements in the queue, drop off the last one.
    if (size() > maxSize()) {
        typename std::multimap<double, T>::iterator last = elems.end();
        --last; // Now points to highest-priority element
        elems.erase(last);
    }
}

// dequeueMin copies the lowest element of the map (the one pointed at by
// begin()) and then removes it.
template <typename T>
std::pair<T, double> BoundedPQueue<T>::dequeueMin() {
    // Copy the best value.
    T value = elems.begin()->second;
    double priority= elems.begin()->first;
    // Remove it from the map.
    elems.erase(elems.begin());
    std::pair<T, double> result(value, priority);
    return result;
}

// size() and empty() call directly down to the underlying map.
template <typename T>
std::size_t BoundedPQueue<T>::size() const {
    return elems.size();
}

template <typename T>
bool BoundedPQueue<T>::empty() const {
    return elems.empty();
}

// maxSize just returns the appropriate data member.
template <typename T>
std::size_t BoundedPQueue<T>::maxSize() const {
    return maximumSize;
}

// The best() and worst() functions check if the queue is empty,
// and if so return infinity.
template <typename T>
double BoundedPQueue<T>::best() const {
    return empty() ? std::numeric_limits<double>::infinity() : elems.begin()->first;
}

template <typename T>
double BoundedPQueue<T>::worst() const {
    return empty() ? std::numeric_limits<double>::infinity() : elems.rbegin()->first;
}

#endif // BOUNDED_PQUEUE_INCLUDED
  • KDtree.h
#pragma once
#include <iostream>
#include <algorithm>
#include <vector>
#include <set>
#include <stack>
#include "BoundedPQueue.h"
// https://blog.csdn.net/weixin_42694889/article/details/124753575?ops_request_misc=&request_id=&biz_id=102&utm_term=KDtree%20c++&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduweb~default-3-124753575.142^v99^pc_search_result_base9&spm=1018.2226.3001.4187
template <typename T>
struct Point3D
{
	T x, y, z;
	int index;
	Point3D() : x(0), y(0), z(0), index(-1) {};
	Point3D(T x, T y, T z) : x(x), y(y), z(z), index(-1) {};
	Point3D(T x, T y, T z, int index) : x(x), y(y), z(z), index(index) {};
	inline T& operator[](int i) {
		return i == 0 ? x : i == 1 ? y : z;
	}
};

template <typename T>
struct Point2D
{
	T x, y;
	int index;
	Point2D() : x(0), y(0), index(-1) {};
	Point2D(T x, T y) : x(x), y(y), index(-1) {};
	Point2D(T x, T y, int index) : x(x), y(y), index(index) {};
	inline T& operator[](int i) {
		return i == 0 ? x : y;
	}
};

struct KDNode 
{
	int index; //该节点存储数据点的索引
	int axis; //划分的维度
	KDNode* left;
	KDNode* right;
	KDNode(int index, int axis, KDNode* left = nullptr, KDNode* right = nullptr) {
		this->index = index;
		this->axis = axis;
		this->left = left;
		this->right = right;
	}
};

template <typename T>
class KDTree
{
private:
	int ndim; //维度
	KDNode* root;
	KDNode* build(std::vector<T>&); //构建KDTree
	std::set<int> visited; //记录回溯搜索时是否已访问
	std::stack<KDNode*> stackNode;
	std::vector<T> m_data;

	void release(KDNode*);
	void printNode(KDNode*);
	int chooseAxis(std::vector<T>&);//选择划分的轴
	void dfs(KDNode*, T);//深度优先搜索
	//计算点之间的距离
	inline double distanceT(KDNode*, T);
	inline double distanceT(int, T);
	//点于超平面的距离
	inline double distanceP(KDNode*, T);
	//判断父节点的超平面是否在超球体中
	inline bool checkParent(KDNode*, T, double);
public:
	int NodeNums;
	KDTree(std::vector<T>&,int dim);
	~KDTree();
	void Print();
	std::vector<std::pair<int, double>> findNearestPoint(T point, int k=1);
};

//构造函数
template <typename T> //T可以是Point3D，也可以是Point2D
KDTree<T>::KDTree(std::vector<T>& data, int dim) {
	this->ndim = dim;
	NodeNums = data.size();
	this->m_data = data;//拷贝原始数据
	root = build(data); 
}

//析构函数
template <typename T>
KDTree<T> ::~KDTree() {
	release(root);
}

template <typename T>
void KDTree<T>::Print() {
	printNode(root);
}

template <typename T>
void KDTree<T>:: release(KDNode* node) {
	if (node == nullptr) {
		return;
	}
	release(node->left);
	release(node->right);
	delete node;
	node = nullptr;
}

template <typename T>
KDNode*  KDTree<T>:: build(std::vector<T>& data) {
	if (data.empty()) return nullptr;
	std::vector<T> temp = data;
	int mid_index = static_cast<int>(data.size() / 2);//数据索引中间值
	int axis = data.size() > 1 ? chooseAxis(temp) : -1; // 根据每个维度的方差大小选择二分的维度,叶子结点无法二分，默认为-1
	std::sort(temp.begin(), temp.end(), [axis](T a, T b) {return a[axis] < b[axis]; }); //这里用了一个仿函数
	std::vector<T> leftData, rightData;
	leftData.assign(temp.begin(), temp.begin() + mid_index);
	rightData.assign(temp.begin() + mid_index + 1, temp.end());
	KDNode* leftNode = build(leftData);
	KDNode* rightNode = build(rightData);
	
	KDNode* root =new KDNode(temp[mid_index].index, axis, leftNode, rightNode);
	return root;

}


template <typename T>
void KDTree<T>::printNode(KDNode* node) {
	if (node) {
		std::cout << "Index: " << node->index << "\tAxis: " << node->axis << std::endl;
		printNode(node->left);
		printNode(node->right);
	}
}

template <typename T>
int KDTree<T>::chooseAxis(std::vector<T>& data) {
	int axis = -1;
	double max_var = -1;


	for (int i = 0; i < ndim; i++) {

		//计算均值
		double mean = 0;
		for (auto j : data) {
			mean += static_cast<double>(j[i]);
		}
		mean = mean / static_cast<double>(data.size());

		//计算方差
		double var = 0;
		for (auto j : data) {
			var += (static_cast<double>(j[i]) - mean) *(static_cast<double>(j[i]) - mean);
		}
		var = var / static_cast<double>(data.size());
		if (var > max_var) {
			max_var = var;
			axis = i;
		}
	}
	return axis;
}
template <typename T>
inline double KDTree<T>::distanceT(KDNode* node, T point)
{
	double dis = 0;
	for (int i = 0; i < ndim; i++) {
		dis += (m_data[node->index][i] - point[i]) * (m_data[node->index][i] - point[i]);
	}
	dis = sqrt(dis);
	return dis;
}

template <typename T>
inline double KDTree<T>::distanceT(int index, T point) {
	double dis = 0;
	for (int i = 0; i < ndim; i++) {
		dis += (m_data[index][i] - point[i]) * (m_data[index][i] - point[i]);
	}
	dis = sqrt(dis);
	return dis;
}

template <class T>
double KDTree<T>::distanceP(KDNode* node, T point) {
	int axis = node->axis;
	double dis = m_data[node->index][axis] - point[axis];
	return abs(dis);

}

template <class T>
bool KDTree<T>::checkParent(KDNode* node, T pt, double distT)
{
	double dis = distanceP(node, pt);
	return dis <= distT;
}

template <typename T>
std::vector<std::pair<int, double>> KDTree<T>::findNearestPoint(T point,int k) {
	visited.clear();
	while (!stackNode.empty()) 
	{
		stackNode.pop();
	}//清空stackNode
	BoundedPQueue<int> pQueue(k); // BPQ with maximum size k

	double min_dist = DBL_MAX;
	int resNodeIdx = -1;
	dfs(root, point);
	while (!stackNode.empty()) {
		KDNode* curNode = stackNode.top();
		stackNode.pop();
		double dist = distanceT(curNode, point);
		pQueue.enqueue(curNode->index, dist);

		if (!stackNode.empty()) {
			KDNode* parentNode = stackNode.top();
			int parentAxis = parentNode->axis;
			int parentIndex = parentNode->index;
			if (pQueue.size() < pQueue.maxSize() || checkParent(parentNode, point, pQueue.worst()))//如果最短半径超球体与分割超平面相交了
			{
				if (m_data[curNode->index][parentAxis] < m_data[parentIndex][parentAxis]) {
					dfs(parentNode->right, point);
				}
				else {
					dfs(parentNode->left, point);
				}

			}
		}
	}
	std::vector<std::pair<int,double>> NearestKPointIndex;
	while (!pQueue.empty()) {
		NearestKPointIndex.push_back(pQueue.dequeueMin());
	}
	return NearestKPointIndex;
}


template <class T>
void KDTree<T>::dfs(KDNode* node, T pt) 
{
	if (node) {
		if (visited.find(node->index) != visited.end())
			return;//访问过了
		stackNode.push(node);
		visited.insert(node->index);
		if (pt[node->axis] <= m_data[node->index][node->axis] && node->left)
			dfs(node->left,pt);
		else if(pt[node->axis] > m_data[node->index][node->axis] && node->right)
		{
			dfs(node->right,pt);
		}
		else if ((node->left == nullptr) ^ (node->right == nullptr))
		{
			dfs(node->left, pt);
			dfs(node->right, pt);
		}
	}
}
  • main.cpp
#include <iostream>
#include <vector>
#include <chrono>
#include <fstream>
#include <sstream>
#include "KDTree.h"
using namespace std;

void read_path(std::vector<Point2D<double>>& data, string fileName) {
    std::ifstream file(fileName);
    std::string line;
    Point2D<double> pt = {0,0,0};
    int id = 0;
    // 读取第一行（列名）但不处理
    std::getline(file, line);
    // 逐行读取文件
    while (std::getline(file, line)) {
        std::stringstream ss(line);
        std::string cell;
        std::vector<std::string> tokens;

        // 逐个读取逗号分隔的单元格
        while (std::getline(ss, cell, ',')) {
            tokens.push_back(cell);
        }

        // 检查是否有足够的数据（至少包含 x 和 y）
        if (tokens.size() >= 2) {
            // 将 x 和 y 值转换为浮点数
            pt.x = std::stod(tokens[0]);
            pt.y= std::stod(tokens[1]);
            pt.index = id++;
            data.push_back(pt);
        }
    }
}

std::pair<int, double> find_nearest_points_traverse(Point2D<double> position, std::vector<Point2D<double>>& path, std::pair<int, int> index_range = { 0, -1 }) {
    // 输入参数
    // position: [x,y]
    // path: [x1,y1;x2,y2;...;xn,yn]
    // index_range: [range_upper,range_lower] range_upper>1,range_lower<size(path,1)
    // 输出参数
    // nearest_index 最近点索引 nearest_dis 最近点距离
    if (index_range.second == -1) {
        index_range.second = path.size();
    }
    int start_index = index_range.first;
    int end_index = index_range.second;
    std::vector<Point2D<double>> new_path(path.begin() + start_index, path.begin() + end_index);
    double nearest_dis = std::numeric_limits<double>::max();
    int nearest_index = start_index;
    for (int i = 0; i < new_path.size(); i++) {
        double distance = pow(new_path[i].x - position.x, 2) + pow(new_path[i].y - position.y, 2);
        if (distance < nearest_dis) {
            nearest_dis = distance;
            nearest_index = start_index + i;
        }
    }
    nearest_dis = sqrt(nearest_dis);
    return std::make_pair(nearest_index, nearest_dis);
}


std::pair<int, double> find_nearest_points_KDTree(KDTree<Point2D<double>>& kdt,Point2D<double> position, std::pair<int, int> index_range = { 0, -1 })
{
    // 输入参数
    // position: [x,y]
    // path: [x1,y1;x2,y2;...;xn,yn]
    // index_range: [range_upper,range_lower] range_upper>1,range_lower<size(path,1)
    // 输出参数
    // nearest_index 最近点索引 nearest_dis 最近点距离
    if (index_range.second == -1) {
        index_range.second = kdt.NodeNums;
    }
    int searchKPoint = 10;
    bool searchFlag = true;
    while (searchFlag) {
        if (searchKPoint > kdt.NodeNums) {
            searchKPoint = kdt.NodeNums;
            searchFlag = false;
        }
        auto nearestKPoint = kdt.findNearestPoint(position, searchKPoint);
        for (auto pt : nearestKPoint) {
            if (pt.first >= index_range.first && pt.first < index_range.second) {
                return pt;
            }
        }
        
        searchKPoint = searchKPoint * 10;

    }
}
 

int main() {

    //读取数据
    std::vector<Point2D<double>> path;
    read_path(path, "E:\\Desktop\\blog\\road.csv");

    //搜索目标
    Point2D<double> position = { -550.9,37.5 };
    std::pair<int, int> index_range = { 0, 6500 };
    std::pair<int, double> nearest_point;
    //暴力遍历搜索
    auto t1 = std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch())
        .count();
    for (int i = 0; i < 1000; i++) {
        nearest_point = find_nearest_points_traverse(position, path, index_range);
    }
    auto t2 = std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch())
        .count();
    cout << "最近点索引： " << nearest_point.first << " 最近点距离： " << nearest_point.second << endl;
    cout << "遍历搜索耗时： " << t2 - t1 << " ms" << endl;

    //KD树搜索
    auto t3 = std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch())
        .count();
	int dim = 2;
    KDTree<Point2D<double>> kdt(path, dim);
    auto t4 = std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch())
        .count();
    cout << "构建KD树耗时： " << t4 - t3 << " ms" << endl;




    auto t5 = std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch())
        .count();
    vector<pair<int, double>> nearestKPoint;
    for (int i = 0; i < 1000; i++) {
        nearest_point = find_nearest_points_KDTree(kdt,position, index_range);
    }
    auto t6 = std::chrono::duration_cast<std::chrono::milliseconds>(
        std::chrono::system_clock::now().time_since_epoch())
        .count();
    cout << "最近点索引： " << nearest_point.first << " 最近点距离： " << nearest_point.second << endl;
    cout << "KD树搜索耗时： " << t6 - t5 << " ms" << endl;



	return 0;
}

在main.cpp中，对暴力遍历搜索和KDtree搜索算法的耗时进行了对比。

  • 无index_range参数：KD树搜索快于遍历搜索
    在这里插入图片描述

  • 有index_range参数：遍历搜索快于KD树搜索
    在这里插入图片描述

python

python版本我们采用scipy的库来导入kdtree模块。

import pandas as pd
import numpy as np
from scipy.spatial import KDTree
import time
# KDTree搜索
def find_nearest_points_KDTree(kdtree,point,index_range=None):
    if index_range is None:
        nearest_dist, nearest_idx = kdtree.query(point)
        return nearest_dist, nearest_idx
    else:
        searchFlag = True;
        k_neighbors =10
        while(searchFlag):
            if(k_neighbors > len(kdtree.data)):
                k_neighbors = len(kdtree.data)
                searchFlag=False
            nearest_dist, nearest_idx = kdtree.query(point,k_neighbors)
            for i in range(len(nearest_idx)):
                if nearest_idx[i]>=index_range[0] and nearest_idx[i]<index_range[1]:
                    return nearest_dist[i], nearest_idx[i]
            k_neighbors = k_neighbors*10
            
def find_nearest_points_traverse(path,position, index_range=None):
    # 输入参数
    # position: [x,y] 类型 np.ndarray
    # path: [x1,y1;x2,y2;...;xn,yn] 类型 np.ndarray
    # index_range: [range_upper,range_lower] range_upper>1,range_lower<size(path,1) 类型 np.ndarray
    # 输出参数
    # nearest_index 最近点索引 nearest_dis 最近点距离
    if index_range is None:
        index_range = [0, path.shape[0]]
    start_index, end_index = index_range
    new_path = path[start_index:end_index, :2]
    distance = np.sum((new_path - position) ** 2, axis=1)
    nearest_index = start_index + np.argmin(distance)
    nearest_dis = np.sqrt(np.min(distance))
    return nearest_dis, nearest_index             

            
if __name__ == '__main__':
    # 读取数据
    df=pd.read_csv('road.csv',header=0)
    path=df[['x','y']].values
    # 构建KDtree
    t1=time.time()
    kdtree = KDTree(path)
    t2=time.time()
    print("kdtree构建耗时:",1000*(t2-t1),"ms")
    # 查询点
    point = [-550.9,37.5]
    t3=time.time()
    for i in range(1000):
        nearest_dist, nearest_idx = find_nearest_points_KDTree(kdtree,point,index_range=[0,6500])
        # nearest_dist, nearest_idx = find_nearest_points_KDTree(kdtree,point)
    t4=time.time()
    print("kdtree搜索耗时:",1000*(t4-t3),"ms")

    print("Nearest point index:", nearest_idx, "Distance:", nearest_dist)

    t5=time.time()
    for i in range(1000):
        nearest_dist, nearest_idx = find_nearest_points_traverse(path,point,index_range=[0,6500])

        # nearest_dist, nearest_idx = find_nearest_points_traverse(path,point)
    t6=time.time()
    print("遍历搜索耗时:",1000*(t6-t5),"ms")
    
    print("Nearest point index:", nearest_idx, "Distance:", nearest_dist)

在main函数中，对暴力遍历搜索和KDtree搜索算法的耗时进行了对比。

  • 无index_range参数：KD树搜索快于遍历搜索
    在这里插入图片描述
  • 有index_range参数：遍历搜索快于KD树搜索在这里插入图片描述
北洋树人
关注
  • 6
    点赞
  • 踩
  • 9
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
查找道路上距离指定点最近的点 / 求某一点到路网上的最近点坐标/ shapely nearest point
weixin_42351518的博客
08-28 1162
个人代码写的极烂，仅供参考，并非常欢迎大家给出修改意见~ 更欢迎大家给出更简单的方法~ 目标：给定某一点A的坐标，求其到给定路网上的最近点的坐标 主要思路：读取路网shp文件，先根据给定点的坐标求处其一定范围（如50米）的buffer区域（可以缩小查找范围），求出在该缓冲区内的道路，再找出指定点到这些道路中最近的点是哪一个。 主要用到的库： shapely：文档——https://shapely.readthedocs.io/en/stable/index.html geopandas：文档——https:
寻找最近点对
ZCC的专栏
12-13 1055
问题:给定平面上N个点的坐标，找出距离最近的两个点。 解法一.蛮力法(两两之间的差值都求出来)时间复杂度O(N*N) 解法二:一维情况下排序求解时间复杂度O(N*logN) 解法三:分治思想，用于一般的情况    思想如下:(1)根据水平方向的坐标把平面上的N个点分成两个部分Left和Right                    (2)由Min(lfet,ri
python list中找一个最近的
最新发布
weixin_42064338的博客
06-30 23
Python相关视频讲解：python的or运算赋值用法用python编程Excel有没有用处？011_编程到底好玩在哪？查看python文件_输出py文件_cat_运行python文件_shel如何在Python List中找到最近的值 作为一名经验丰富的开发者，我将教你如何在Python List中找到最近的值。...
寻找最近点
xiaoma_bk的博客
11-07 204
寻找最近点1. FLANN 1. FLANN FLANN库全称是Fast Library for Approximate Nearest Neighbors，它是目前最完整的（近似）最近邻开源库。不但实现了一系列查找算法，还包含了一种自动选取最快算法的机制。 用户手册下载 ...
VTK笔记——寻找最近点(vtkCellLocator)
Andy的专栏
04-22 3507
当你看到找最近点时，可能会立刻想到在一堆点中，通过距离找出最近的点，没错，这是一种寻找最近点的需求。今天我们要说的是另一种寻找最近点的需求，就是在Mesh上找最近点，这个点不一定是Mesh的顶点。
C语言寻找无向图两点间的最短路径
08-26
"C语言寻找无向图两点间的最短路径" 本资源主要介绍了使用C语言寻找无向图两点间的最短路径，通过邻接表实现无向图，并使用广度优先遍历找到两点之间的最短路径。下面是相关的知识点： 1. 无向图：无向图是一种图...
取两点的最近路径.zip易语言项目例子源码下载
03-22
本项目“取两点的最近路径”是基于易语言编写的，主要目的是解决在二维平面上寻找两个点之间最短路径的问题。这个项目对于学习易语言的开发者，尤其是学生和小团队来说，具有很好的学习和参考价值。 首先，我们需要...
取两点的最近路径.rar
04-06
在计算机科学和算法设计中，"取两点的最近路径"是一个经典的几何问题，通常与图论、数据结构和最优化算法相关。这个问题涉及到在给定的几何空间或网络中找到两个点之间的最短路径。在实际应用中，这可能包括在地图上...
快速寻找无向图中两点间的所有路径
05-23
函数功能：找到图中两个节点之间的所有路径 参数说明：1、Matrix 初始矩阵，将路径矩阵的形式存储，本程序对应的是一个无向图。 2、headNode 初始节点 3、endNode 结束节点 主要的思想 利用深度优先遍历的...
编程之美之寻找最近点对
luckyu1的博客
04-23 353
分治的思想 首先，按照一个x进行分块，尽量保证左右差不多相等 然后求出最小，划分，在2m * m的块中至多有八个，直接解决
分治寻找最近点对
weixin_30462049的博客
10-26 114
***改天增加下算法总结 #include <iostream> #include <cstdio> #include <cmath> #include <algorithm> #include <vector> #define n 100000 using namespace std; struc...
找到距离最近的点，性能最好的方法
hhh314159的博客
06-14 1803
要找到距离最近的点并且性能最好，一种常用的方法是使用空间数据结构来加速搜索过程。这些空间数据结构可以根据实际需求选择和实现。构建数据结构的时间复杂度可能较高，但在之后的最近邻搜索过程中可以快速找到最近的点，从而提高性能。请注意，对于非常大的数据集，您可能还需要考虑使用更高级的数据结构或算法来进一步优化搜索效率。另外，如果点的数量不是非常大，你也可以使用简单的遍历方法，在遍历过程中计算并更新最近的点。这种方法的时间复杂度为O(n)，其中n是点的数量。虽然这种方法的性能较差，但对于小规模的数据集可能是足够的。
python寻找距离最近的点_Python最短距离点
weixin_39938935的博客
12-08 3911
Write a function called dist that takes in two points (so two lists of two elements each), and computes the distance between them. Make sure this works on the example below before proceeding to the ne...
Python_寻找并绘制距离某一点最近的k个点
yangxin30520的博客
10-19 3372
def Find_Recent(x, y, x_, y_, k): """ 找到点集中距离目标点(x, y)最近的k个点 :param x: 点集的x坐标 :param y: 点集的y坐标 :param x_: 目标点的x坐标 :param y_: 目标点的y坐标 :param k: 距离目标点最近的k个值 :return: k个与目标点最近的点的集合 """ list_stack_temp = [] # 建立一个空的栈 .
python 计算并查找两个空间元素最近距离点
cuisidong1997的博客
12-05 2073
result2 = list(nearest_points(a[1], b[1])[1].coords)[0] 生成的点result2对应的为b[1] 上距离 a[1] 最近的点 result2 = list(nearest_points(a[1], b[1])[0].coords)[0] 生成的点result2对应的为a[1] 上距离 b[1] 最近的点
python:实现找到最近的点对之间的距离算法(附完整源码)
希望我的博客，能帮上你解决学习中工作中所遇到的问题
07-21 1421
python:实现找到最近的点对之间的距离算法(附完整源码)
寻找crontab路径
09-15
在大多数 Linux 系统上，`crontab` 的默认路径通常是 `/usr/bin/crontab`。你可以使用以下命令来查找 `crontab` 的实际路径： ``` which crontab ``` 或者，你也可以使用下面的命令来定位 `crontab` 的路径： ``` whereis crontab ``` 这将返回 `crontab` 可执行文件的路径。请注意，具体路径可能因操作系统和配置而有所不同。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值