自动驾驶感知算法工程师面试手撕代码题

 快排

#include <iostream>
#include <vector>
using namespace std;

int partition(vector<int>& arr, int left, int right){
	int pivot = arr[left];
	int j = left;
	for (int i = left + 1; i <= right; i++) { //保留第一个pivot位置不变，往后面遍历，遇到一个小于pivot的就交换
		if (arr[i] < pivot) {
			int temp = arr[i];
			arr[i] = arr[j + 1];
			arr[j + 1] = temp;
			j++;
		}
	}
	arr[left] = arr[j];
	arr[j] = pivot;
	return j;
}

void quicksort(vector<int>& arr, int left, int right) {
	if (left < right) { //这里是递归的终止条件。
		int mid = partition(arr, left, right);
		quicksort(arr, left, mid - 1);
		quicksort(arr, mid + 1, right);
	}
}

int main() {
	vector<int> arr = { 1, 2, 5, 7, 3, 5, 9 };
	quicksort(arr, 0, arr.size() - 1);
	for (int i = 0; i < arr.size(); i++) {
		cout << arr[i] << endl;
	}
	return 0;
}

topk 

#include <iostream>
#include <queue>
#include <unordered_map>
using namespace std;

//topk最大的k个用小顶堆，greater函数。或者自定义类 bool operator()(const........)
//topk最小的k个用大顶堆，默认即可。或者less，或者自定义
void topfrequent(vector<int>& arr, int k) {
	unordered_map<int, int> map;
	for (auto i : arr) {
		map[i]++;
	}
	priority_queue<pair<int, int>, vector<pair<int, int>>, greater<pair<int, int>>> heap;
	int num = 0;
	for (auto i : map) {
		heap.push({ i.second, i.first });
		num++;
		if (num > k) {
			heap.pop();
		}
	}
	for (int i = 0; i < k; i++) {
		cout << heap.top().second << endl;
		heap.pop();
	}
}

int main() {
	vector<int> arr = { 1,1,1,2,2,3 };
	topfrequent(arr, 2);
	return 0;
}

NMS

#include <iostream>
#include <vector>
#include <algorithm>
using namespace std;

struct BoundingBox {
	float x1, y1, x2, y2;
	float confidence;
};

//bool cmp(const BoundingBox& a1, const BoundingBox& a2) {
//	return a1.confidence > a2.confidence;
//}
class MyCompare {
public:
	bool operator() (const BoundingBox& a1, const BoundingBox& a2) {
		return a1.confidence > a2.confidence;
	}
};

float calculate_iou(const BoundingBox& box1, const BoundingBox& box2) {
	float heigh = max(0.f, min(box1.y2, box2.y2) - max(box1.y1, box2.y1));
	float width = max(0.f, min(box1.x2, box2.x2) - max(box1.x1, box2.x1));
	float iou = (heigh * width) / ((box1.x2 - box1.x1) * (box1.y2 - box1.y1) + (box2.x2 - box2.x1) * (box2.y2 - box2.y1) - heigh * width);
	return iou;
}

vector<BoundingBox> NMS(vector<BoundingBox>& boxes, float score_threshold) {
	sort(boxes.begin(), boxes.end(), MyCompare());
	vector<BoundingBox> result;
	for (auto& box : boxes) {
		bool keep = true;
		for (auto& keep_box : result) {
			float iou = calculate_iou(keep_box, box);
			if (iou > score_threshold) {
				keep = false;
				break;
			}
		}
		if (keep) {
			result.push_back(box);
		}
	}
	return result;
}


int main() {
	std::vector<BoundingBox> detections = {
	{10, 10, 50, 50, 0.9},   // 框1：左上角(10, 10)，右下角(50, 50)，置信度0.9
	{20, 20, 60, 60, 0.85}   // 框2：左上角(20, 20)，右下角(60, 60)，置信度0.85
	};

	float iouThreshold = 0.1;
	std::vector<BoundingBox> filteredBoxes = NMS(detections, iouThreshold);

	// 预期输出：只保留置信度更高的框1
	// 输出结果的顺序可能会有所不同
	for (const auto& box : filteredBoxes) {
		std::cout << "Left: " << box.x1 << ", Top: " << box.y1 << ", Right: " << box.x2 << ", Bottom: "
			<< box.y2 << ", Confidence: " << box.confidence << std::endl;
	}
}

Meanshift以及简化版

#include <iostream>
#include <vector>

using namespace std;
struct Point {
	float x;
	float y;
};

float calcaulate_dist(Point& p1, Point& p2) {
	return sqrt(pow((p1.x - p2.x), 2) + pow((p1.y - p2.y), 2));
}

vector<Point> points = { {0.1, 0.2}, {5.4, 0.1}, {1.0, 3.2} };
vector<Point> meanshift(Point& centor, float R) {
	vector<Point> result;
	while (true) {
		Point new_centor = { 0.0, 0.0 };
		int n = 0;
		for (auto& p : points) {
			float dist = calcaulate_dist(p, centor);
			if (dist < R) {
				result.push_back(p);
				new_centor.x += p.x;
				new_centor.y += p.y;
				n++;
			}
		}
		new_centor.x /= n;
		new_centor.y /= n;
		if (calcaulate_dist(new_centor, centor) < 0.001) {
			break;
		}
	}
	return result;
}

int main() {
	vector<Point> keep_point;
	Point start = { 0.1, 0.2 };
	keep_point = meanshift(start, 1);
	cout << keep_point.size() << endl;
	return 0;
}

简化版本

#include <iostream>
#include <vector>

using namespace std;

struct Point {
	int x;
	int y;
};

struct embedding_dist {
	float x;
	float y;
};

float calculate_dist(embedding_dist& d1, embedding_dist& d2) {
	return sqrt(pow(d1.x - d2.x, 2) + pow(d1.y - d2.y, 2));
}

int main() {
	vector<pair<Point, embedding_dist>> curb_list;
	vector<pair<embedding_dist, int>> centor_mean_num; //里面保存的是中心点和属于这个中心点的个数
	vector<pair<Point, int>> centor_point_id; //里面保存的是每个像素点以及它的类别。
	float gap = 1;
	for (auto& curb : curb_list) {
		float min_gap = gap + 1;
		int min_id = -1;
		for (int i = 0; i < centor_mean_num.size(); i++) {
			float dist = calculate_dist(curb.second, centor_mean_num[i].first);
			if (dist < min_gap) {
				min_gap = dist;
				min_id = i;
			}
		}
		if (min_gap < gap) {
			centor_point_id.push_back({ curb.first, min_id });
			embedding_dist now_centor;
			int num;
			now_centor = centor_mean_num[min_id].first;
			num = centor_mean_num[min_id].second;
			embedding_dist new_centor;
			new_centor.x = (now_centor.x * num + curb.second.x) / (num + 1);
			new_centor.y = (now_centor.y * num + curb.second.y) / (num + 1);
			centor_mean_num[min_id] = { new_centor, num + 1 };
		}
		else {
			centor_mean_num.push_back({ curb.second, 1 });
			centor_point_id.push_back({ curb.first, centor_mean_num.size() - 1 });
		}
	}
	return 0;
}

有序点云后处理

#include <iostream>
#include <vector>
#include <algorithm>

using namespace std;

struct Point {
	float x;
	float y;
};

float calculate_dist(Point& p1, Point& p2) {
	return sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2));
}

int main() {
	vector<Point> points;
	sort(points.begin(), points.end(), [](const Point& p1, const Point& p2) {
		if (p1.x == p2.x) return abs(p1.y) > abs(p2.y);
		return p1.x > p2.x;
		});
	Point centor = points[0];
	float R = 1.0;
	vector<Point> result;
	result.push_back(centor);
	while (points.size()) {
		vector<Point> keep;
		Point new_centor = { 0.0, 0.0 };
		int n = 0;
		for (auto& p : points) {
			float dist = calculate_dist(p, centor);
			if (dist < R) {
				new_centor.x += p.x;
				new_centor.y += p.y;
			}
			else {
				keep.push_back(p);
			}
		}
		new_centor.x /= n;
		new_centor.y /= n;
		centor = new_centor;
		result.push_back(centor);
		points = keep;
	}
	//最终result就是一堆有序点。
	return 0;
}

坐标转换lidar2img

import numpy as np

def lidar_img(points, lidar2img, img): #lidar2ing是一个3*4的矩阵，内参3*4，外参矩阵4*4
    img_w = img.shape[0]
    img_h = img.shape[1]
    points = np.insert(points, 3, 1, axis = 1).T
    points = np.delete(points, np.where(points[0,:] < 0), axis = 1)
    img_points = np.dot(lidar2img, points)
    img_points = np.delete(img_points, np.where(img_points[2,:] < 0),axis = 1)
    img_points[:2] /= img_points[2,:]
    u,v,z = img_points[:3]
    u_out = np.logical_or(u<0, u>img_w)
    v_out = np.logical_or(v<0, v>img_h)
    outlier = np.logical_or(u_out, v_out)
    img_points = np.delete(img_points, np.where(outlier), axis = 1)