第1章：工欲善其事——OpenCV与Eigen实战

各位同学，欢迎来到《从理论到量产：视觉SLAM全流程实战》的第一章。

说实话，每次带新人入门SLAM，我最怕听到的一句话就是：「代码跑不通」。不是算法太难，而是环境没配好，库不会用。你想想看，一个矩阵乘法写错了，整个优化就崩了；一张图像没读对，特征点全偏了。这些基础工具，才是SLAM的「地基」。

这一章，我们就来夯实这个地基。我会带你亲手过一遍OpenCV、Eigen、Sophus和可视化工具。这些都是我每天吃饭的家伙，也是你后续所有章节的「左膀右臂」。

1.1 OpenCV：图像读取与处理

OpenCV，说白了就是计算机视觉界的「瑞士军刀」。在SLAM里，它负责两件事：读图、算图。

1.1.1 图像读取

我个人习惯用 cv::imread() 来读图。注意第二个参数，它决定了图像的色彩模式。

#include <opencv2/opencv.hpp>
#include <iostream>

int main() {
    // 读取彩色图像
    cv::Mat color_img = cv::imread("scene.jpg", cv::IMREAD_COLOR);
    // 读取灰度图像
    cv::Mat gray_img = cv::imread("scene.jpg", cv::IMREAD_GRAYSCALE);

    if (color_img.empty()) {
        std::cerr << "图像读取失败！" << std::endl;
        return -1;
    }

    std::cout << "图像尺寸: " << color_img.cols << " x " << color_img.rows << std::endl;
    std::cout << "通道数: " << color_img.channels() << std::endl;

    // 显示图像
    cv::imshow("彩色图像", color_img);
    cv::imshow("灰度图像", gray_img);
    cv::waitKey(0);

    return 0;
}

⚠️ 避坑指南

我曾经在量产项目里踩过一个坑：用 cv::IMREAD_UNCHANGED 读了一张16位深度的PNG图，结果后续所有算法都报错。因为OpenCV默认的很多函数只支持8位或32位浮点图。建议你读图后，用 img.type() 检查一下数据类型。

1.1.2 图像预处理

SLAM前端最常用的预处理，就是转灰度、高斯模糊和直方图均衡化。我一般这么写：

// 高斯模糊，去除噪声
cv::Mat blurred;
cv::GaussianBlur(gray_img, blurred, cv::Size(5, 5), 1.5);

// 直方图均衡化，增强对比度
cv::Mat equalized;
cv::equalizeHist(blurred, equalized);

// 边缘检测（Canny）
cv::Mat edges;
cv::Canny(equalized, edges, 50, 150);

cv::imshow("预处理结果", edges);
cv::waitKey(0);

嗯，这里要注意：高斯核的大小必须是奇数。我见过有人写成 Size(4,4)，然后程序直接崩了。OpenCV不会给你任何提示，就是默默地抛异常。

1.2 Eigen：矩阵运算库

Eigen 是C++里的矩阵运算王者。SLAM里的位姿变换、优化求解，全得靠它。说白了，它就是SLAM的「数学引擎」。

1.2.1 基本矩阵操作

#include <Eigen/Core>
#include <iostream>

int main() {
    // 定义一个3x3的浮点矩阵
    Eigen::Matrix3f mat;
    mat << 1, 2, 3,
            4, 5, 6,
            7, 8, 9;

    // 定义一个3维向量
    Eigen::Vector3f vec(1, 0, -1);

    // 矩阵乘法
    Eigen::Vector3f result = mat * vec;

    std::cout << "矩阵:\n" << mat << std::endl;
    std::cout << "向量:\n" << vec << std::endl;
    std::cout << "结果:\n" << result << std::endl;

    // 转置、逆、行列式
    std::cout << "转置:\n" << mat.transpose() << std::endl;
    std::cout << "行列式: " << mat.determinant() << std::endl;

    return 0;
}

💡 个人经验

我在项目中遇到过一个问题：用 Eigen::MatrixXd（动态大小）做大量矩阵运算，性能比 Eigen::Matrix4d（固定大小）慢了将近3倍。因为固定大小矩阵在编译期就分配了栈内存，而动态矩阵需要堆分配。所以，能固定就固定，别偷懒。

1.2.2 几何变换

SLAM里最常用的，就是旋转矩阵、旋转向量和四元数。Eigen 提供了现成的模块：

#include <Eigen/Geometry>

// 定义旋转向量：绕Z轴旋转45度
Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d::UnitZ());

// 旋转向量转旋转矩阵
Eigen::Matrix3d rotation_matrix = rotation_vector.toRotationMatrix();

// 旋转矩阵转四元数
Eigen::Quaterniond quat(rotation_matrix);

std::cout << "旋转矩阵:\n" << rotation_matrix << std::endl;
std::cout << "四元数: " << quat.coeffs().transpose() << std::endl;

// 用四元数旋转一个点
Eigen::Vector3d point(1, 0, 0);
Eigen::Vector3d rotated_point = quat * point;
std::cout << "旋转后的点: " << rotated_point.transpose() << std::endl;

1.3 Sophus：李代数库

Sophus 是 Eigen 的「好兄弟」，专门处理李群和李代数。在SLAM后端优化里，你几乎离不开它。为什么？因为旋转矩阵有约束（正交且行列式为1），直接优化很麻烦。李代数把约束变成了无约束优化，简单多了。

1.3.1 SO(3) 与 SE(3)

#include <sophus/so3.hpp>
#include <sophus/se3.hpp>

// 创建一个SO(3)对象（从旋转矩阵）
Eigen::Matrix3d R = Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ()).toRotationMatrix();
Sophus::SO3d SO3_R(R);

// 从四元数创建
Eigen::Quaterniond q(R);
Sophus::SO3d SO3_q(q);

// 对数映射：SO(3) -> so(3)
Eigen::Vector3d so3 = SO3_R.log();
std::cout <> SO(3)
Sophus::SO3d SO3_updated = Sophus::SO3d::exp(so3 + Eigen::Vector3d(0.1, 0, 0));

// SE(3) 示例
Eigen::Vector3d t(1, 0, 0);
Sophus::SE3d SE3_Rt(R, t);  // 从旋转矩阵和平移向量
Sophus::SE3d SE3_qt(q, t);  // 从四元数和平移向量

🔑 核心理解

李代数 so(3) 是一个三维向量，对应旋转向量。李群 SO(3) 是旋转矩阵。它们之间通过指数映射和对数映射互相转换。说白了，李代数就是旋转矩阵的「导数形式」，方便我们做优化。

1.4 可视化工具：Pangolin 与 Matplotlib

做SLAM，看不见结果等于白做。我习惯用 Pangolin 做3D实时显示，用 Matplotlib 做离线数据分析。

1.4.1 Pangolin：3D可视化

#include <pangolin/pangolin.h>

int main() {
    // 创建窗口
    pangolin::CreateWindowAndBind("SLAM可视化", 1024, 768);
    glEnable(GL_DEPTH_TEST);

    // 定义相机视角
    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024, 768, 420, 420, 512, 384, 0.1, 1000),
        pangolin::ModelViewLookAt(0, -5, 3, 0, 0, 0, pangolin::AxisY)
    );

    // 创建交互视图
    pangolin::Handler3D handler(s_cam);
    pangolin::View& d_cam = pangolin::CreateDisplay()
        .SetBounds(0.0, 1.0, 0.0, 1.0)
        .SetHandler(&handler);

    while (!pangolin::ShouldQuit()) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);

        // 绘制坐标系
        pangolin::glDrawAxis(3);

        // 绘制一个点
        glPointSize(5);
        glBegin(GL_POINTS);
        glColor3f(1.0, 0.0, 0.0);
        glVertex3f(1, 1, 1);
        glEnd();

        pangolin::FinishFrame();
    }

    return 0;
}

1.4.2 Matplotlib（Python端）

有时候C++跑完数据，我喜欢用Python的Matplotlib画图分析。比如画轨迹误差：

import matplotlib.pyplot as plt
import numpy as np

# 假设这是从C++导出的轨迹数据
estimated = np.loadtxt("estimated_trajectory.txt")
ground_truth = np.loadtxt("ground_truth.txt")

plt.figure(figsize=(10, 8))
plt.plot(estimated[:, 0], estimated[:, 1], 'r-', label='估计轨迹', linewidth=2)
plt.plot(ground_truth[:, 0], ground_truth[:, 1], 'b--', label='真实轨迹', linewidth=2)
plt.xlabel('X (m)')
plt.ylabel('Y (m)')
plt.title('轨迹对比')
plt.legend()
plt.grid(True)
plt.axis('equal')
plt.show()

💡 我的工作流

我一般这样搭配：C++里用Pangolin做实时3D显示，同时把关键数据（位姿、特征点）保存到文本文件。跑完后，用Python脚本加载数据，用Matplotlib画详细的误差分析图。这样既保证了实时性，又方便做论文级的图表。

知识体系总览

下面这张图，概括了本章的核心内容。你可以把它当作一张「工具地图」：

视觉SLAM工具链总览 OpenCV 图像读取与处理 Eigen 矩阵运算与几何变换 Sophus 李群与李代数 可视化工具 Pangolin / Matplotlib SLAM 系统核心 前端视觉里程计 → 后端优化 → 建图 每个工具负责一个环节，组合起来就是完整的SLAM流水线

这张图很直观：OpenCV负责「看」图像，Eigen负责「算」矩阵，Sophus负责「优化」位姿，可视化工具负责「展示」结果。四者缺一不可。

📌 本章小结

  • OpenCV：图像读取用 cv::imread()，预处理用高斯模糊和Canny边缘检测。注意数据类型和通道数。
  • Eigen：固定大小矩阵性能更优。几何变换模块 Eigen/Geometry 提供了旋转矩阵、四元数、旋转向量的互转。
  • Sophus：李群 SO(3) 和 SE(3) 是对应李代数的指数映射结果。优化时用李代数，更新后用指数映射回到李群。
  • 可视化：Pangolin 适合实时3D显示，Matplotlib 适合离线数据分析。两者配合使用效果最佳。

好了，这一章的内容就到这里。这些工具你用得越熟，后面学SLAM算法就越轻松。别急，慢慢来，把每个例子都亲手敲一遍。代码这东西，看十遍不如写一遍。

公众号:蓝海资料掘金营，微信deep3321

← 上一章 📖 返回目录 下一章 →