2. 相机模型与图像处理：针孔相机模型、畸变校正、图像滤波与特征提取、OpenCV基础操作
好，咱们进入第二章。这一章，说白了就是视觉SLAM的“眼睛”怎么工作的。你想想看，机器人要感知世界，总得有个摄像头吧？但摄像头拍出来的图像，和我们人眼看到的，其实差得挺远。这里面涉及两个核心问题：相机是怎么成像的，以及怎么从图像里提取有用的信息。
我个人习惯，讲这部分时一定先讲模型，再讲处理。因为只有理解了成像的物理过程，你才知道后面为什么要做畸变校正，为什么要做滤波。嗯，咱们开始。
2.1 针孔相机模型：SLAM的几何基础
针孔相机模型，是SLAM里最基础的几何模型。它把三维世界里的一个点，映射到二维图像平面上。说白了，就是一个小孔成像的原理。
我记得刚入行时，总觉得这个模型太简单，不就是个相似三角形吗？但后来在项目里吃过亏——如果你不理解这个模型，后面做三角化、做PnP，全都会乱套。
模型的核心公式是这样的：
// 世界坐标系下的点 P = [X, Y, Z]^T
// 经过相机内参 K 和 外参 [R|t] 投影到像素坐标 [u, v]^T
// 齐次坐标形式：
// s * [u, v, 1]^T = K * [R|t] * [X, Y, Z, 1]^T
// 其中内参矩阵 K：
// K = | fx 0 cx |
// | 0 fy cy |
// | 0 0 1 |
这里 fx, fy 是焦距（以像素为单位），cx, cy 是光心偏移。这些参数，我们通常通过标定得到。
2.2 畸变校正：别让镜头骗了你
现实中的镜头，不是完美的针孔。它会有畸变。最常见的两种：径向畸变和切向畸变。
- 径向畸变：图像边缘的直线会变弯。桶形畸变（广角镜头常见）和枕形畸变（长焦镜头常见）。
- 切向畸变：镜头和成像平面不平行导致的，像个梯形。
畸变校正的数学表达，其实就是用几个多项式系数去“拉直”图像。OpenCV里直接提供了函数：
// 假设你已经有了畸变系数 distCoeffs = [k1, k2, p1, p2, k3]
// 以及相机内参 cameraMatrix
cv::Mat map1, map2;
cv::initUndistortRectifyMap(cameraMatrix, distCoeffs, cv::Mat(),
cameraMatrix, imageSize, CV_32FC1, map1, map2);
// 然后用 remap 一次性校正整张图
cv::remap(distorted_img, undistorted_img, map1, map2, cv::INTER_LINEAR);
undistort()，结果发现CPU占用率飙升到90%。后来改用 initUndistortRectifyMap 预计算映射表，再用 remap，性能提升了近10倍。记住：映射表只需计算一次。
2.3 图像滤波：去噪与增强
图像处理里，滤波是基本功。SLAM里常用的滤波，主要有两个目的：去噪和增强特征。
我常用的几种滤波：
| 滤波类型 | 作用 | OpenCV函数 | 适用场景 |
|---|---|---|---|
| 高斯滤波 | 平滑去噪，保留边缘 | GaussianBlur() |
特征点提取前的预处理 |
| 中值滤波 | 去除椒盐噪声 | medianBlur() |
传感器噪声较大的场景 |
| 双边滤波 | 保边去噪 | bilateralFilter() |
需要保留纹理细节时 |
| 拉普拉斯滤波 | 边缘增强 | Laplacian() |
特征检测前的锐化 |
你想想看，如果图像全是噪点，你提取出来的特征点位置会来回跳，SLAM系统根本没法稳定。所以，滤波这一步，看似简单，但参数调不好，后面全白费。
2.4 特征提取：SLAM的“路标”
特征提取，是视觉SLAM里最核心的一步。我们得从图像里找到那些“与众不同”的点——角点、斑点、边缘。这些点，就是SLAM里的“路标”。
常用的特征点有：
- ORB：速度快，适合实时SLAM。我自己的项目里90%都用它。
- SIFT：尺度不变性最好，但计算量太大，不适合嵌入式。
- SURF：SIFT的加速版，但专利问题，商用要小心。
- FAST：只检测角点，不计算描述子，常用于光流法。
ORB特征提取的OpenCV代码示例：
cv::Ptr<cv::ORB> orb = cv::ORB::create(1000); // 最多提取1000个特征点
std::vector<cv::KeyPoint> keypoints;
cv::Mat descriptors;
orb->detectAndCompute(image, cv::Mat(), keypoints, descriptors);
// 绘制特征点
cv::Mat img_with_keypoints;
cv::drawKeypoints(image, keypoints, img_with_keypoints, cv::Scalar::all(-1));
2.5 OpenCV基础操作：你的瑞士军刀
OpenCV是SLAM开发者的必备工具。我建议你把它当成“瑞士军刀”——不用每个功能都精通，但常用的几个操作要烂熟于心。
这里列几个我每天都会用到的操作：
// 1. 读取和显示图像
cv::Mat img = cv::imread("image.jpg");
cv::imshow("Window", img);
cv::waitKey(0);
// 2. 颜色空间转换
cv::Mat gray;
cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
// 3. 图像缩放
cv::Mat resized;
cv::resize(img, resized, cv::Size(640, 480));
// 4. ROI提取（感兴趣区域）
cv::Rect roi(100, 100, 200, 200);
cv::Mat img_roi = img(roi);
// 5. 图像保存
cv::imwrite("output.jpg", img);
这些操作，看起来简单，但组合起来能解决90%的图像预处理问题。我个人习惯，把常用的图像处理流程封装成一个类，比如 ImagePreprocessor，里面包含灰度化、缩放、滤波、直方图均衡化等步骤。这样在SLAM系统里调用起来特别清爽。
cv::imshow 显示，没问题；但如果你用其他库（比如Matplotlib）显示，记得先转换：cv::cvtColor(img, img, cv::COLOR_BGR2RGB)。
知识体系总览
为了让你更直观地理解本章的知识结构，我画了一张图：
这张图把本章的核心脉络串起来了。从相机模型出发，到畸变校正，再到图像滤波和特征提取，最后用OpenCV落地实现。每一步都环环相扣。