简介：围绕点云着色与激光雷达—摄像头融合的 C++ 实践包，面向具备一定计算机视觉基础、希望用 PCL 与 OpenCV 完成多传感器数据关联的自动驾驶学习者。它针对点云缺少颜色、图像与三维点难以对应的问题，给出从 KITTI 数据读取、点云滤波预处理，到相机内参投影映射、外参变换与着色、融合思路验证的一套可编译示例，适合课程实验、算法复现和工程入门参考。压缩包共 26 个文件，约 16.2MB，包含 1 个 cpp 主程序、CMakeLists.txt，以及 6 个 pcd 点云、6 个 bin 原始数据、6 个 png 图像和 7 个 txt 标定/说明文件，覆盖数据与代码两部分，便于直接搭建运行环境。已有 4155 人学习下载，读者可据此理解二维像素与三维点的坐标变换关系，掌握点云着色流程，并以此扩展相机标定、噪声滤波和多传感器融合实验；目录同时保留主程序、构建脚本与样例数据，方便对照调试。
1. 点云着色：激光雷达与摄像头融合的第一步，为什么不是检测而是上色
点云着色这件事，第一次做的人都以为就是个矩阵乘法，真跑起来才发现问题全在标定和外参上。我手里这套 C++ 实现干的是最基础也最实用的一步：把激光雷达扫出来的三维点，按标定投影到摄像头图像上，用对应像素的 RGB 给点上色，输出一份能直接丢进 PCL 或 CloudCompare 里看的彩色点云。输入是 KITTI 格式的 .bin 点云、.png 图像和一份标定文本，依赖只有 PCL、OpenCV 和 Eigen，没有 ROS，也没有深度学习框架。它适合两类人：一类是要给 3D 检测做数据预处理的，彩色点云比纯几何点云好用得多，标注和看图都省事；另一类是刚接触传感器融合、想先把「外参到底怎么用」这件事弄明白的。整个工程不到一千行，但坐标链、遮挡、边界这些真问题一个都没绕开。
2. 技术栈与数据流：PCL、OpenCV、Eigen 在传感器融合里各管什么
2.1 选型：为什么是 C++ 加三个库，而不是全自己写
先说清楚为什么不用 Python。一帧 64 线激光雷达的点数在 10 万量级，投影加取色是逐点做的，Python 里哪怕用 numpy 向量化，中间还要处理边界判断和遮挡剔除，写起来别扭、调起来更别扭。C++ 里这个循环是纯算术，Release 下几十万点也就十几毫秒的事，后面想挂到实时管线里不用重写。
三个库的分工很明确：
- PCL管点云。读 .bin 其实用不上它，但
pcl::PointXYZRGB、pcl::io::savePCDFileBinary、pcl_viewer这一套省了大量格式化的工作。PCD 文件有 ASCII 和 binary 两种，binary 的体积大概是 ASCII 的三分之一，读写还快，量产数据我一律用 binary。 - OpenCV管图像。
cv::imread读进来是 BGR 顺序，这点后面会反复提到，因为 PCD 里存的是 RGB，搞反了整片点云就是蓝脸。 - Eigen管矩阵。标定矩阵是 3×3 和 3×4，投影时还要做齐次坐标变换，手写 float[12] 乘来乘去，调试时打印出来都对不上号，Eigen 的
Matrix<float,3,4>至少能让你在 gdb 里一眼看出哪个矩阵有问题。
选型上还有一个隐性理由：这三个库在大多数自动驾驶、机器人的工程环境里本来就装着，加这个模块不需要引入新的构建负担。你要是换成一个只有头文件的轻量库，反而要在 CI 里多配一套东西。
2.2 一条点从雷达到像素，中间要过四个坐标系
这一段是整个项目最核心的概念，投影对不上，九成是这里理解偏了。一个激光点要变成像素，得走完这条链：
| 步骤 | 所在坐标系 | 用的矩阵 | 矩阵含义 |
|---|---|---|---|
| 1 | 激光雷达系（x 前、y 左、z 上） | Tr_velo_to_cam | 雷达到相机的刚体外参，3×4 |
| 2 | 相机 0 系（x 右、y 下、z 前） | R0_rect | 立体校正矩阵，3×3 |
| 3 | 校正后相机系 | P0~P3 | 投影矩阵，3×4，含焦距、主点、基线 |
| 4 | 像素坐标 (u, v) | 除以 z 归一化 | 齐次坐标转欧氏 |
关键在第一步和第二步的顺序。Tr_velo_to_cam把点从雷达系搬到相机 0 系，此时坐标轴已经翻过来了，雷达的 x（向前）变成了相机的 z（向前），雷达的 z（向上）变成了相机的 -y（向下）。R0_rect是在相机系内部再做一次旋转，让左右相机的像平面共面、极线水平，这样P矩阵才能统一描述。先 Tr 后 R0，顺序反了结果会整体偏几米，而且偏得很像「外参不准」，特别容易误判。
P矩阵里其实已经包含了内参和基线。以常见的 KITTI 标定为例，一行 12 个数，前三个是 3×3 的内参块，第四个是平移项：
P2: 7.215377e+02 0.000000e+00 6.095593e+02 4.485728e+01 0.000000e+00 7.215377e+02 1.728540e+02 2.163791e-01 0.000000e+00 0.000000e+00 1.000000e+00 2.745884e-03这里fx = fy ≈ 721.5，主点在(609.6, 172.9)，第四列的4.4857e+01是fx × 基线的项。用人话说：你不需要自己再乘一遍内参，P已经替你做完了。新手最容易犯的错就是从别处抄了套cameraMatrix，又乘了一遍P，结果点云缩成一团。
2.3 目录结构：六个文件就能跑起来
工程刻意压得很扁，一个可执行文件，头文件和实现分开，数据单独放：
lidar_cam_colorize/ ├── CMakeLists.txt ├── include/ │ ├── calib.h # 标定结构体与解析接口 │ └── colorize.h # 投影上色的对外接口 ├── src/ │ ├── calib.cpp # calib.txt 解析 + 合成矩阵 │ ├── colorize.cpp # 逐点投影、z-buffer、取色 │ └── main.cpp # 命令行入口 ├── data/ │ ├── lidar/000000.bin │ ├── image_2/000000.png │ └── calib/000000.txt └── tools/run_batch.shinclude和src分离不是为了好看，是为了后面你要把它做成库塞进自己的管线时，别的模块只 include 头文件就行，不用把 .cpp 也拖过去。data目录保持和原始数据集一致的子目录名（lidar/image_2/calib），批量脚本里直接用文件名对齐，不用维护额外的映射表。
3. 核心模块拆解：从 .bin 点云到带 RGB 的 .pcd 全链路
3.1 读点云与读标定：两个解析器，各有各的小陷阱
激光雷达的 .bin 是裸的二进制流，没有文件头，格式极简：每个点四个 float32，依次是 x、y、z、intensity，一个点 16 字节，文件大小除以 16 就是点数。这种「无头格式」的好处是快，坏处是你没法从文件本身校验点数字段对不对，只能靠大小反推。
// src/colorize.cpp pcl::PointCloud<pcl::PointXYZI>::Ptr loadBin(const std::string& path) { std::ifstream ifs(path, std::ios::binary); if (!ifs) throw std::runtime_error("打不开点云文件: " + path); ifs.seekg(0, std::ios::end); const size_t bytes = static_cast<size_t>(ifs.tellg()); ifs.seekg(0, std::ios::beg); constexpr size_t kStride = 4 * sizeof(float); // x y z intensity if (bytes % kStride != 0) { throw std::runtime_error("文件大小不是 16 的整数倍，可能不是原始 bin"); } const size_t n = bytes / kStride; std::vector<float> buf(n * 4); ifs.read(reinterpret_cast<char*>(buf.data()), static_cast<std::streamsize>(bytes)); auto cloud = pcl::make_shared<pcl::PointCloud<pcl::PointXYZI>>(); cloud->resize(n); for (size_t i = 0; i < n; ++i) { auto& p = cloud->points[i]; p.x = buf[4 * i + 0]; p.y = buf[4 * i + 1]; p.z = buf[4 * i + 2]; p.intensity = buf[4 * i + 3]; } cloud->width = static_cast<uint32_t>(n); cloud->height = 1; cloud->is_dense = false; // 允许存在 NaN / Inf return cloud; }is_dense = false这一行不是可有可无的。真实雷达数据里偶尔会出现 NaN 或者 inf，标成 dense 之后 PCL 的一些滤波和 KD-Tree 构建会直接断言失败，这种崩溃在 release 里还不好复现，属于典型的「没写后悔药」。
标定文件是文本，形如键: 若干个数，R0_rect后面跟 9 个数，其余跟 12 个。解析本身不难，难在内存布局：
// src/calib.cpp bool loadCalib(const std::string& path, int cam_id, Calib& out) { std::ifstream ifs(path); if (!ifs) return false; std::string line; while (std::getline(ifs, line)) { const auto pos = line.find(':'); if (pos == std::string::npos) continue; const std::string key = line.substr(0, pos); std::istringstream iss(line.substr(pos + 1)); std::vector<float> v; float x; while (iss >> x) v.push_back(x); if (key == "R0_rect" && v.size() == 9) { // Eigen 默认列优先，文件是行优先存的，必须用 RowMajor 的 Map 接一下 out.R0_rect = Eigen::Map<Eigen::Matrix<float, 3, 3, Eigen::RowMajor>>(v.data()); } else if (key == "Tr_velo_to_cam" && v.size() == 12) { out.Tr = Eigen::Map<Eigen::Matrix<float, 3, 4, Eigen::RowMajor>>(v.data()); } else if (key == "P" + std::to_string(cam_id) && v.size() == 12) { out.P = Eigen::Map<Eigen::Matrix<float, 3, 4, Eigen::RowMajor>>(v.data()); } } // 三个矩阵缺一不可，缺了就在投影时炸，不如在这里提前返回 return out.P.cwiseAbs().sum() > 0.f && out.R0_rect.cwiseAbs().sum() > 0.f; }Eigen::RowMajor这段是绕不开的。Eigen 的Matrix3f默认列优先存储，你直接把按行读进来的 9 个数 map 上去，得到的就是转置矩阵，投影结果会莫名其妙地歪。这个坑非常隐蔽，因为转置后的矩阵看起来数值都「差不多对」，只有真正投影出来才发现对不上。
cam_id这个参数是给不同相机用的：彩色图image_2对应P2，灰度图image_0对应P0。硬编码成 P2 之后换数据集必崩，所以从一开始就做成参数。
3.2 投影矩阵拼接：三个矩阵预先合成一个 3×4
上一节拿到三个矩阵，如果老老实实逐点做三次矩阵乘法，一帧 12 万点就是 36 万次乘法，而且中间还要来回构造齐次向量。正确的做法是在循环外把它们合成一个 3×4：
$$ M = P_{3\times4} \cdot \begin{bmatrix} R0_{3\times3} \cdot Tr_{3\times4} \ 0\ 0\ 0\ 1 \end{bmatrix}_{4\times4} $$
// include/calib.h struct Calib { Eigen::Matrix3f R0_rect; Eigen::Matrix<float, 3, 4> Tr; Eigen::Matrix<float, 3, 4> P; // 预先合成 3x4：p_pixel = M * (x, y, z, 1) Eigen::Matrix<float, 3, 4> compose() const { Eigen::Matrix4f A = Eigen::Matrix4f::Identity(); A.block<3, 4>(0, 0) = R0_rect * Tr; // 先外参，再校正 return P * A; // 最后乘投影，顺序不能换 } };合成之后，每个点的投影就是一次3×4乘4×1，12 次乘加，编译器还能自动向量化。这一步带来的提速在小数据上看不出来，但如果你的管线后面要接 10Hz 的双雷达，差别就明显了。
compose()返回的M可以直接缓存成文件，下次启动读进来，省掉解析和乘法。我在批量处理几万帧的时候就是先把M序列化成一个 12 个 float 的小文件，主循环里只做内存读，整体耗时能压掉一截。
3.3 投影取色：边界裁剪、BGR 转 RGB、z-buffer 挡遮挡
这是整个项目最长的一段代码，也是出错最多的地方。要点有三个：点在相机后面要丢掉，落在图像外的要丢掉，被遮挡的点不能抢前景的颜色。
// src/colorize.cpp pcl::PointCloud<pcl::PointXYZRGB>::Ptr colorize( const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud, const cv::Mat& bgr, const Eigen::Matrix<float, 3, 4>& M) { const int W = bgr.cols, H = bgr.rows; auto out = pcl::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>(); out->reserve(cloud->size()); // 第一遍：给每个像素记录最近的点的深度，这就是最朴素的 z-buffer std::vector<float> zbuf(static_cast<size_t>(W) * H, std::numeric_limits<float>::max()); std::vector<Eigen::Vector3f> pix(cloud->size(), Eigen::Vector3f::Zero()); std::vector<char> valid(cloud->size(), 0); for (size_t i = 0; i < cloud->size(); ++i) { const auto& p = cloud->points[i]; Eigen::Vector4f v(p.x, p.y, p.z, 1.0f); const Eigen::Vector3f uv = M * v; if (uv.z() <= 1e-3f) continue; // 落在相机后方，直接丢 const float u = uv.x() / uv.z(); const float w = uv.y() / uv.z(); const int iu = static_cast<int>(std::lround(u)); const int iw = static_cast<int>(std::lround(w)); if (iu < 0 || iw < 0 || iu >= W || iw >= H) continue; // 视野外 pix[i] = Eigen::Vector3f(static_cast<float>(iu), static_cast<float>(iw), uv.z()); valid[i] = 1; float& d = zbuf[static_cast<size_t>(iw) * W + iu]; d = std::min(d, uv.z()); } // 第二遍：只有深度接近该像素最近深度的点才上色 constexpr float kOcclusionTol = 0.15f; // 米，按雷达距离分辨率调 for (size_t i = 0; i < cloud->size(); ++i) { if (!valid[i]) continue; const int iu = static_cast<int>(pix[i].x()); const int iw = static_cast<int>(pix[i].y()); const float z = pix[i].z(); if (z > zbuf[static_cast<size_t>(iw) * W + iu] + kOcclusionTol) continue; const cv::Vec3b c = bgr.at<cv::Vec3b>(iw, iu); pcl::PointXYZRGB q; q.x = cloud->points[i].x; q.y = cloud->points[i].y; q.z = cloud->points[i].z; q.r = c[2]; // OpenCV 是 BGR，PCD 要 RGB q.g = c[1]; q.b = c[0]; out->push_back(q); } out->width = static_cast<uint32_t>(out->size()); out->height = 1; out->is_dense = false; return out; }几处参数值得展开：
uv.z() <= 1e-3f而不是<= 0。因为浮点误差，恰好在像平面上的点除下来可能是 1e300，加上边界判断虽然也能挡住，但中间会先产生一次 inf，日志里很难看。给个 1 毫米的余量更稳。kOcclusionTol = 0.15f。太小了会把同一物体上前后差几厘米的点也剔掉，点云出现「麻点」；太大了遮挡就挡不住了，后面的树会透到前面的车上。0.1~0.3 之间按你的雷达测距精度试。- 用
std::lround而不是直接static_cast<int>。截断在负坐标上是向零取整，-0.6会变成0，边界判断就漏了。 out->push_back会触发重新分配，虽然前面reserve了，但 reserve 的是原始点数，实际留下的点更少，所以不会溢出。
**关于遮挡，还有一点要说清楚：**这套 z-buffer 是「按点」做的，不是「按面」。同一个像素上如果有一辆车和一个远处的墙，雷达点在车上只有一个（因为车是平面），在墙上可能是好几个。用最近深度做判断，车上的点保住，墙上的点被剔掉，视觉上是对的。但如果雷达线束较高，整车只被打到两三个点，这几个点就代表不了整辆车，穿过去的点依然会被误上色。想要更干净的结果，得上深度图渲染或者多帧累积，那就是另一个话题了。
3.4 写出 PCD 并做第一轮验证
保存这步一行就够，但有个细节要提醒：
pcl::io::savePCDFileBinary(out_path, *colored);binary PCD 会把PointXYZRGB的rgb字段按 float 打包写进去，读的时候 PCL 自己解包，所以你在文件里看到的是一个奇怪的浮点数，那不是 bug。反过来，如果你手工用 ASCII 写x y z r g b六个字段，PCL 能读，但pcl_viewer不会自动按 RGB 渲染，得手动指定颜色字段——这是新手常见的「我明明写了颜色为什么不显示」。
验证别急着开可视化，先看一个统计量：上色率。
const double ratio = static_cast<double>(colored->size()) / cloud->size(); std::cout << "着色点占比: " << ratio * 100.0 << "%" << std::endl;以 360° 激光雷达配前置相机为例，相机的水平视场角通常只有 80°~90°，所以能落到图像里的点天然就少，健康区间大概在 25%~40%。低于 15%，基本可以断定是标定读错了、矩阵顺序反了，或者图像尺寸和标定不匹配。这个数字我每次跑新数据第一件事就是看它，比直接看图快得多，而且能量化——换一套标定之后这个数掉一半，那就是外参有问题，不用怀疑到代码上。
再往后是看图：pcl_viewer out.pcd会自动按 RGB 上色，转动视角，看车、行人、路沿的颜色边界是不是和真实物体贴合。**贴合的意思是：车顶的点是车漆色，车身下沿的点是路面灰，中间没有一条斜着的错位带。**只要有系统性错位，就是矩阵的事，跟取色逻辑无关。
4. 编译与跑通：CMake 工程从零到出第一张彩色点云
4.1 CMakeLists 与依赖
PCL 的 CMake 脚本有点特殊，find_package之后要手动处理 include 目录和库列表：
cmake_minimum_required(VERSION 3.10) project(lidar_cam_colorize LANGUAGES CXX) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) if(NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) # 这种逐点循环 Debug 会慢十倍，默认给 Release endif() find_package(PCL REQUIRED COMPONENTS common io) find_package(OpenCV REQUIRED) find_package(Eigen3 REQUIRED) add_executable(colorize src/main.cpp src/calib.cpp src/colorize.cpp ) target_include_directories(colorize PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include ${PCL_INCLUDE_DIRS} ) target_link_libraries(colorize PRIVATE ${PCL_LIBRARIES} ${OpenCV_LIBS} Eigen3::Eigen )几个容易卡住的地方：
find_package(PCL REQUIRED COMPONENTS common io)里必须显式写出io，只写common的话savePCDFileBinary链接会报 undefined reference。- 如果你的环境里 PCL 是源码编译装在自定义前缀下，需要
set(PCL_DIR "/your/prefix/share/pcl-1.x")再 find，否则 CMake 找不到配置文件。 Eigen3::Eigen这个 imported target 要求 Eigen 版本别太老，老版本只有${EIGEN3_INCLUDE_DIR}，那就换成target_include_directories加变量。- 不高低整一个
-O3 -march=native也影响不大，但-DNDEBUG一定要有，Eigen 的断言在逐点循环里开销不小。
编译：
mkdir -p build && cd build cmake .. -DCMAKE_BUILD_TYPE=Release make -j"$(nproc)"第一次编译大概率会报找不到 Eigen 或者 PCL 版本冲突，这种属于环境问题，跟代码无关，把CMakeCache.txt删掉重来通常就好。
4.2 命令行入口与批量脚本
main.cpp保持极简，四个位置参数，不做复杂的 flag 解析：
// src/main.cpp int main(int argc, char** argv) { if (argc < 5) { std::cerr << "用法: colorize <cloud.bin> <image.png> <calib.txt> <out.pcd> [cam_id]\n" << " cam_id 默认 2，对应 image_2 / P2\n"; return 1; } const std::string bin_path = argv[1]; const std::string img_path = argv[2]; const std::string calib_path = argv[3]; const std::string out_path = argv[4]; const int cam_id = (argc > 5) ? std::atoi(argv[5]) : 2; cv::Mat img = cv::imread(img_path, cv::IMREAD_COLOR); if (img.empty()) { std::cerr << "读图失败: " << img_path << "\n"; return 2; } Calib calib; if (!loadCalib(calib_path, cam_id, calib)) { std::cerr << "标定解析失败，检查是否包含 P" << cam_id << " / R0_rect / Tr_velo_to_cam\n"; return 3; } auto cloud = loadBin(bin_path); auto colored = colorize(cloud, img, calib.compose()); pcl::io::savePCDFileBinary(out_path, *colored); std::cout << "输入点 " << cloud->size() << "，上色点 " << colored->size() << "，占比 " << 100.0 * colored->size() / cloud->size() << "%\n"; return 0; }注意这里calib.compose()是每次调用都重新算的。单帧没问题，批量跑的时候建议在外面算一次传进来，或者像前面说的缓存成文件。
跑一帧：
./colorize \ ../data/lidar/000000.bin \ ../data/image_2/000000.png \ ../data/calib/000000.txt \ ../data/out/000000.pcd批量就把这套逻辑塞进 shell，重点在set -euo pipefail，一帧失败整批停下，别让它默默跑完几万帧再告诉你全错了：
#!/usr/bin/env bash set -euo pipefail ROOT=${1:?用法: run_batch.sh <数据根目录>} BIN=${2:-./build/colorize} mkdir -p "$ROOT/out" for f in "$ROOT"/lidar/*.bin; do id=$(basename "$f" .bin) "$BIN" "$f" "$ROOT/image_2/$id.png" "$ROOT/calib/$id.txt" "$ROOT/out/$id.pcd" done如果标定是一整份文件对应全部帧（不少数据集就是这样），把$ROOT/calib/$id.txt换成固定路径即可，脚本不用改结构。
4.3 换成自己的数据，需要动哪几处
从公开数据集迁到自家设备，改动集中在四个点，都不在算法里：
- 点云解析。自家雷达如果是自定义二进制（比如带时间戳字段、或者每个点 20 字节），
loadBin里的kStride和字段偏移要改。带逐点时间戳的那种，顺手把时间戳也读出来，后面做运动补偿要用。 - 标定格式。如果你拿到的外参是「雷达到相机」的 4×4 齐次矩阵而不是 3×4，直接取前 3 行，别自己再拆旋转平移。如果是「相机到雷达」，记得求逆再填进
Tr。 R0_rect。单目相机没有立体校正，这个矩阵就是单位阵，但不能省，代码里仍要走一遍，否则分支逻辑会散。- 图像尺寸。代码里从
img.cols/rows取，没有硬编码，这点务必保持。我见过太多把 1242×375 写死的版本，换一台相机就整片空白。
改完之后先跑一帧，看上色率是不是落在合理区间，再看图确认边缘贴合。这两步过了，剩下的就是标定精度的活儿。
5. 踩坑与排查：投影对不上、颜色错位、点云空白的 5 个现场
5.1 现象：点云出来了，但整片是黑的
原因：PointXYZRGB的 r/g/b 赋值顺序搞反了，或者读进来的cv::Mat是灰度图被强行当三通道解。OpenCV 的imread默认 BGR，如果图本身是单通道还被IMREAD_COLOR转成三通道，取到的三个分量相等，那就是一片灰，不是黑，可以据此区分。
解决：先打印一个像素的三个通道值，确认不是全相等；再确认q.r = c[2]这个映射。另外pcl_viewer只有在点云带rgb字段时才自动上色，如果你用的是PointXYZI只往 intensity 里塞了颜色索引，那看到的就是单色图，这属于用错类型。
5.2 现象：颜色能上，但整体往一个方向偏了一截
原因：三个矩阵的乘法顺序。P * R0_rect * Tr * p和P * Tr * R0_rect * p在数学上都成立，但物理含义完全不同，后者是错的。还有一种可能是R0_rect用了Eigen::Map<Matrix3f>直接读，没加RowMajor，等于用了转置矩阵。
解决：拿一个已知的参照物验证，比如画面正中的一根杆子或者车牌。手动算一个点的投影，和代码输出对比：
// 临时调试：拿一个点的原始坐标，把中间结果全打出来 Eigen::Vector4f v(p.x, p.y, p.z, 1.0f); std::cout << "cam0: " << (calib.Tr * v).transpose() << "\n"; // 应该是 z 为正 std::cout << "rect: " << (calib.R0_rect * (calib.Tr * v).head<3>()).transpose() << "\n"; std::cout << "pix : " << (calib.P * Eigen::Vector4f( (calib.R0_rect * (calib.Tr * v).head<3>()).x(), (calib.R0_rect * (calib.Tr * v).head<3>()).y(), (calib.R0_rect * (calib.Tr * v).head<3>()).z(), 1.0f)).transpose() << "\n";第一步的输出如果 z 是负的，说明Tr的方向反了，你拿到的可能是相机到雷达的外参。
5.3 现象：上色率极低，点云只剩零零星星几百个点
原因：uv.z() <= 1e-3f这个判断把绝大多数点滤掉了。常见于雷达坐标系和相机坐标系搞混——很多国产雷达出厂是 x 向前、y 向右、z 向上，而标定文件给的外参是按 x 向前、y 向左、z 向上算的，y 轴反了之后整个点云镜像，投影过去全在图像外。
解决：临时把三个轴的统计打出来，min/max/mean各是多少，和标定文件里的量级对一下。再不行就把P矩阵单独拿出来，用它投影一个「相机正前方 10 米」的假点(0, 0, 10, 1)，看落点是不是在图像中心附近。这一步能把「矩阵问题」和「数据问题」彻底分开，我一般把它写成一个--selftest开关，省得每次重新手敲。
5.4 现象：用 image_2 的图，但颜色和点云差半个车身
原因：用错了相机编号。image_2必须配P2，image_3配P3，这两台相机的基线不一样，混用的话水平方向会有几十个像素的系统性偏移。更隐蔽的一种是标定文件里只有P0~P3而你要用的是校正前的原始图，那就得先做cv::undistort，否则畸变在图像边缘能差出十几个像素。
解决：把cam_id做成必填参数，跑之前打印一行使用 P2 + image_2。至于畸变，如果你的图像已经是*_rect版本就不用管，原始版本要先 undistort 再投影，顺序不能反。
5.5 现象：远处的树透过近处的车，被染成了车的颜色
原因：没做 z-buffer，每个点都直接按像素取色，被遮挡的点拿了前景的颜色。或者做了但阈值给太大，比如kOcclusionTol = 1.0f，一米之内全算同一层。
解决：把阈值降到 0.1~0.2 之间试，同时注意别降太狠——雷达点稀疏，同一个物体表面的相邻点深度差可能就有几厘米，阈值太小会把这些点也剔掉，点云上出现一条条黑缝。判断标准是：车的轮廓边缘干净，车身内部没有麻点。这两个条件同时满足的阈值就是对的。
6. 二次开发：把这套着色管线换成你自己的雷达和相机
代码跑通之后，真正有价值的改造方向有三个，按投入产出排：
第一，外参从「抄来的」变成「自己标出来的」。如果你的设备没有现成标定，标准做法是打一块棋盘格（或者带孔的靶标），同时用相机拍、用雷达扫，相机这边cv::calibrateCamera拿内参，雷达这边提取靶标平面，用平面约束求外参初值，最后用pcl::IterativeClosestPoint精修。精修之前一定要给个像样的初值，ICP 在这个问题上很容易陷到局部最优，这是血泪经验。
第二，加运动补偿。如果雷达是转的、车在动，一帧点云里第一个点和最后一个点相差 100ms，60km/h 下就是 1.6 米。这时候直接投影，边缘会有明显的「拖影」。做法是读逐点时间戳，用 IMU 或轮速积分出每个时刻的位姿，把点统一变换到帧首时刻的坐标系再投影。这一步加进去，代码量翻倍，但效果提升非常直观。
第三，实时化。把main.cpp里的读文件换成回调，compose()的结果缓存在成员变量里，只在外参变化时重算；zbuf和pix这两个 vector 提到类成员，循环里assign复用，别每帧重新分配几十万个 float。做完这三件事，单帧耗时会降一个量级。
验证方法我习惯用两层。第一层是数字：上色率、单帧耗时、着色点的深度分布直方图。第二层是重投影：把彩色点云按投影矩阵再投回图像，和原图做逐像素差分，边缘处差异应该集中在物体轮廓上，如果出现整体平移的亮带，就是外参又偏了。这个自检我写成了脚本，改完外参必跑一遍。
一个具体的小技巧：把你的融合矩阵M和图像尺寸一起序列化成一个小的二进制头文件，和点云放在同一目录。别人拿到数据不用问你要标定，程序读一下头文件就能投影。这个习惯是从一次数据交付的翻车事故之后养成的——那次标定文件漏发，对面拿着几万帧点云愣是没法上色，来回折腾了两天。从那以后我每次导出数据都强制走一遍「标定内嵌」的检查，宁可多几十字节，也不留这种坑。希望帮到你。
本文还有配套的精品资源，点击获取
