常用算法-mark

本文介绍了几种常用的数据结构和算法,包括二分查找、最短路径算法(Floyd、Dijkstra 和 Bellman-Ford)、排序算法(冒泡、插入、选择、希尔、快速排序)以及最小生成树算法(Prim 和 Kruskal)。通过这些算法的学习,读者可以更好地理解和解决实际问题。
// 二分查找 key的位置
// arr是排好序的数组
template<class T>
int binary_search(T * arr, int len, T& key){
    int low = 0; hight = len - 1;
    while(low <= hight){
        int mid = (low + hight) >> 1;
        if(arr[mid] == key) return mid;
        else if(arr[mid] < key) low = mid + 1;
        else hight = mid - 1;
    }
    return -1;
}

// ------------------------------最短路径-------------------------------------------
// Folyd算法: 计算各个结点之间的最短路径
template<class T>
void Folyd(T** dist, int n){
	for(int k = 0; k < n; k++){
		for(int i = 0; i < n; i++){
			for(int j = 0; j < n; j++){
				if(dist[i][j] > dist[i][k] + dist[k][j]){
					dist[i][j] = dist[i][k] + dist[k][j];
				}
			}
		}
	}
}

// can work better with minheap, here demostrate the algorithm only.
// 计算单源最短路径
template<class T>
void dijkstra(T** graph, T * dist, int n, int s){
	bool * visited = new bool[n];
	memset(visited, 0, sizeof(bool) * n);
	for(int i = 0; i < n; i++){
		dist[i] = INF;
	}	
	dist[s] = 0;
	
	for(int i = 0; i < n; i ++){
		// find the vertex closest to s;
		T min = INF;
		int k = -1;
		for(int j = 0; j < n; j++){
			if(!visited[j] && dist[j] < min){
				k = j;
				min = dist[j];
			} 
		}
		
		// relax other vertex.
		for(int j = 0; j < n; j++){
			if(!visited[j] && dist[j] > dist[k] + graph[k][j]){
				dist[j] = dist[k] + graph[k][j];
			}
		}
		
		visted[k] = true;
	}
	
	delete [] visited;
}

// 
template<class T>
void bellman-fold(T** graph, T * dist, int n, int s){
	memset(dist, INF, n * sizeof(T));	
	dist[s] = 0;
	for(int k = 0; k < n; k++)
	for(int i = 0; i < n; i ++){
		for(int j = 0; j < n; j++){
			if(dist[j] > dist[i] + graph[i][j]){
				dist[j] = dist[i] + graph[i][j];
			}
		}
	}
}

// -------------------------------最小生成树-------------------------------------
// prime 算法, 每次取出最小的边,加入到生成树种
template<class T>
int prime(T** graph, int n, int start){
	bool * visited = new bool[n];
	memset(visited, 0, sizeof(bool) * n);
	T * dist = new T[n];
	for(int i = 0; i < n; i ++){
		dist[i] = graph[start][i];
	}
	visted[start] = true;
	int sum = 0;
	
	for(int i = 0; i < n; i ++){
		T min = INF;
		int k = -1;
		for(int j = 0; j < n; j++){
			if(!visited[j] && dist[j] < min){
				k = j;
				min = dist[j];
			}
		}
		visited[k] = true;
		sum += dist[j];
		for(int j = 0; j < n; j++){
			if(!visited[j] && dist[j] > graph[k][j]){
				dist[j] = graph[k][j];
			}
		}
		
	}
	
	delete [] visited;
	delete [] dist;
	
	return sum;
}

// -----kruskal----
// union set
class union_set{
public:
	union_set(int n){
		father = new int[n];
		rank = new int[n];
		size = n;
		make_set();
	}
	
	void make_set(){
		for(int i = 0; i < size; i ++){
			father[i] = i;
			rank[i] = 1;
		}
	}
	
	void find_father(int x){
		if(x != father[x]){
			father[x] = find_father(father[x]);
		}
		
		return father[x];
	}
	
	void union(int x, int y){
		int father_x = find_father(x);
		int father_y = find_father(y);
		if(father_x == father_y){
			return;
		}
		
		if(rank[father_x] < rank[father_y]){
			father[father_x] =  father_y;
		}else{
			if(rank[father_x] == rank[father_y]){
				rank[father_x] ++;
			}
			
			father[father_y] = father_x;
		}
	}
private:
	int size;
    int * father;
    int * rank;	
};

typedef struct{
 int start;
 int end;
 int val;
} Edge;

// 使用并查集计算最短生成路径。
// 把每个节点当作一颗树,把边上的两个节点所在的树逐渐合并,直到形成一颗完整的树
// Edge是按val大小排好序的数组。
int kruskal(Edge * edge, int n, int vertex_size){
	union_set uset(vertex_size); 
	int sum = 0;
	for(int i = 0; i< n; i++){
		if(uset.find_father(edge[i].start) != uset.find_father(edge[i].end)){
			uset.union(edge[i].start, edge[i].end);
			sum += edge[i].val;
		}
	}
	
	return sum;
}


template<class T>
void bubble_sort(T * arr, int n){
	for(int i = 1; i < n; i++)
		for(int j = n - 1; j >= i; j--)
			if(arr[j] < arr[j - 1])
				swap(arr[j], arr[j - 1]);
}

template<class T>
void insert_sort(T * arr, int n){
	T t;
	int j;
	for(int i = 1; i < n; i++){
		t = arr[i];
		for(j = i; j > 0 && arr[j - 1] > arr[j]; j --){
			arr[j] = arr[j - 1];
		}
		arr[j] = t;
	}
}

template<class T>
void select_sort(T * arr, int n){
	int k;
	for(int i = 0; i < n; i ++){
		k = i;
		for(int j = i + 1; j < n; j++){
			if(arr[j] < arr[k]){
				k = j;
			}
		}
		swap(arr[i], arr[k]);
	}
}

template<class T>
void hill_sort(T * t, int n){
	for(int d = n/2; d >= 1; d /=2){
		for(int i = 0; i < d; i++){
			for(int j = n - 1; j >= i + d; j -= d){
				if ( arr[j] < arr[j-d]){
					swap(arr[j], arr[j-d]);
				}
			}
		}
	}
}

// 快排,比较短小精悍。
template<class T>
int partition(T * arr, int left, int right){
	int x = left - 1;
	for( int i = left; i < right; i++){
		if(arr[i] < arr[right]){
			x ++ ;
			swap(arr[x], arr[i]);
		}
	}
	x++;
	swap(arr[x], arr[right]);
	return x;
}

template<class T>
void quick_sort(T * arr, int left, int right){
	if(left >= right) return;
	int p = partition(arr, left, right);
	quick_sort(arr, left, p - 1);
	quick_sort(arr, p + 1, right);
}


在ROS2环境下使用C++实现充电桩V-Mark检测,通常涉及计算机视觉技术与ROS2节点间的通信机制。V-Mark是一种常见的视觉标记,用于定位或识别特定对象。实现该功能需要以下几个关键步骤: ### 图像采集与ROS2图像消息订阅 首先,需要通过摄像头采集图像数据。ROS2中常用`image_transport`包来订阅和发布图像消息。摄像头数据通常通过`sensor_msgs::msg::Image`类型进行传输,同时还需要`cv_bridge`将ROS图像消息转换为OpenCV支持的格式以便处理。 ```cpp #include <rclcpp/rclcpp.hpp> #include <image_transport/image_transport.hpp> #include <opencv2/opencv.hpp> #include <cv_bridge/cv_bridge.hpp> class VMarkDetector : public rclcpp::Node { public: VMarkDetector() : Node("vmark_detector") { subscription_ = image_transport::create_subscription( this, "camera/image_raw", [this](const sensor_msgs::msg::Image::ConstSharedPtr msg) { this->imageCallback(msg); }, image_transport::TransportHints::Constants::kRaw ); } private: void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& msg) { try { cv::Mat image = cv_bridge::toCvShare(msg, "bgr8")->image; // 图像处理逻辑 } catch (cv_bridge::Exception& e) { RCLCPP_ERROR(this->get_logger(), "CV-Bridge error: %s", e.what()); } } image_transport::SubscriptionPtr subscription_; }; ``` ### V-Mark检测算法实现 V-Mark检测可以使用OpenCV中的模板匹配、边缘检测或特征点检测等方法。以下是一个基于模板匹配的简单示例: ```cpp cv::Mat template_img = cv::imread("vmark_template.png", cv::IMREAD_COLOR); cv::Mat result; cv::matchTemplate(image, template_img, result, cv::TM_CCOEFF_NORMED); double threshold = 0.8; cv::Mat locations; cv::threshold(result, locations, threshold, 1., cv::THRESH_BINARY); // 提取匹配位置 std::vector<cv::Point> points; cv::findNonZero(locations, points); for (auto& pt : points) { cv::rectangle(image, pt, cv::Point(pt.x + template_img.cols, pt.y + template_img.rows), cv::Scalar(0, 255, 0), 2); } ``` ### ROS2图像发布与可视化 检测结果可以通过`image_transport`发布为新的图像话题,供其他节点使用或通过`rqt_image_view`等工具可视化。 ```cpp image_transport::Publisher publisher_; VMarkDetector() : Node("vmark_detector") { publisher_ = image_transport::create_publisher(this, "vmark/detection_result"); } void publishResult(const cv::Mat& result_image) { sensor_msgs::msg::Image::SharedPtr msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", result_image).toImageMsg(); publisher_.publish(msg); } ``` ### 节点集成与ROS2启动配置 最后,将上述逻辑集成到ROS2节点中,并通过`CMakeLists.txt`和`package.xml`配置编译依赖项。ROS2节点可通过`launch`文件启动[^1]。 ```xml <launch> <node name="vmark_detector" pkg="your_package_name" exec="vmark_detector_node" /> </launch> ``` ###
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值