【C++】相机标定源码笔记- RGB 相机与 ToF 深度传感器校准类

类的设计目标是为了实现 RGB 相机与 ToF 深度传感器之间的高精度校准，从而使两种类型的数据能够在同一个坐标框架内被整合使用。这在很多场景下都是非常有用的，比如在3D重建、增强现实、机器人导航等应用中，能够提供更丰富的场景信息。

---------------变量-----------------

公有成员变量： 存储RGB和ToF图像的大小、存储物理世界中标定板的坐标点、分别存储在RGB和ToF图像中检测到的棋盘格角点、存储RGB和TOF角点的三维点集、存储RGB和ToF相机的内参矩阵、存储RGB和ToF相机鱼眼畸变系数、存储RGB和ToF相机一般畸变系数、RGB/ToF角点检测是否成功的bool向量。

私有变量： 是否为复杂RGB/ToF相机模型 ,RGB/ToF文件名向量,棋盘格角点的布局大小，角点之间的物理距离，是否使用圆点标定板进行角点检测，在RGB和ToF图像中用于进行角点检测的ROI(感兴趣区域)大小，  RGB/ToF是否采用鱼眼相机模型,RGB/ToF重投影误差阈值,实际计算出的RGB/ToF重投影误差,RGB/ToF点集,RGB和TOF之间的变换矩阵,平均误差距离.

---------------函数-----------------

默认构造函数：  初始化标志位为false表示不是复杂相机模型。初始化RGB、ToF最大重投影误差。 初始化RGB相机角点检测的ROI大小和TOF相机角点检测的ROI大小。

带文件名构造函数：调用加载参数方法加载参数。

析构函数：空。

设置棋盘格标定板参数： 设置角点布局大小，设置角点之间的物理距离，设置是否使用圆点标定板进行角点检测。

设置RGB相机的参数：清除标定点集（世界坐标点），清除RGB图像角点集，清除检测结果，清除点集，清除文件名列表。  设置RGB图像文件名列表，设置RGB图像大小，设置最大重投影误差，设置是否为鱼眼相机模型。设置是否为复杂相机模型。

设置ToF相机的参数：清除标定点集，清除TOF图像角点集，清除检测结果，清除点集，清除文件名列表。设置TOF图像文件名列表，设置TOF图像大小，设置最大重投影误差，设置是否为鱼眼相机模型，设置是否为复杂相机模型。

执行相机标定：清空之前所有的标定结果和临时变量，为新的标定过程做准备（清除标定板世界坐标点数据容器、清除TOF相机的图像角点数据，清除TOF相机的角点检测结果数据，清除TOF相机的点集数据，清除RGB相机的图像角点数据，清除RGB相机的角点检测结果数据，清除RGB相机的点集数据）。 根据RGB图像大小设置角点搜索区域大小，根据TOF图像大小设置角点搜索区域大小。检查RGB和TOF图像数量是否匹配。遍历每一对RGB和TOF图像进行标角点检测处理 {取出一对RGB\TOF图像并读取，定义RGB/TOF图像转换为灰度图的变量，初始化结果false，定义存放RGB/TOF图像检测到的角点。 如果是圆点标定板（检测RGB图像的圆点网格角点，设置检测结果为false并添加到RGB/TOF检测结果列表，打印RGB检测角点失败信息，继续下一对图像。否则检测成功，检查TOF图像，如果检测失败，将false结果添加到检测结果列表并打印tof检测失败信息，继续下一对图像，否则RGB和TOF图像都成功检测到角点，绘制RGB图像角点，标记检测成功并添加到结果列表。将RGB/TOF检测到的图像角点添加到 RGB/TOF校准图像角点集合，打印角点检测成功信息）。 如果是棋盘格角点，检测RGB图像的棋盘角点（如果失败将失败结果添加到结果列表打印失败信息，继续下一对图像），RGB检测成功则检测TOF图像（检测失败添加失败结果到结果列表并打印信息后继续下一对），RGB和TOF图像都成功检测到棋盘角点（ 转换RGB图像为灰度图，使用cornerSubPix算法对RGB图像中的角点进行精细化处理，提高角点定位的准确性，将精细化处理后的RGB图像角点添加到容器中，在灰度图像上绘制检测到的角点便于观察。转换TOF图像为灰度图，同样为焦点检测做准备，对TOF图像中的交点进行精细化处理，将精细化处理后的TOF图像角点添加到容器中，在TOF图像的灰度图上绘制检测到的角点。将当前图像对的检测结果标记为成功并记录。再次将RGB和TOF图像的角点数据添加到对应容器中，打印当前图像对成功完成标定的信息） ，如果角点检测成功并且设置为可视化（对灰度图像进行反色处理增强视觉效果，显示处理后的RGB灰度图像，对TOF图像进行同样的反色处理，显示处理后的TOF图像，短暂等待以便图像显示。）} 。  生成世界坐标系中的点[ 计算用于标定的图像数量，根据图像数量生成对应的世界坐标点]，根据角点数据计算RGB和TOF图像的各角点夹角， 定义RGB和TOF相机标定过程中计算得到的外参（旋转向量和平移向量），定义存储RGB和TOF相机去畸变后的映射矩阵。

RGB相机标定{如果是鱼眼相机模型[ 定义标定选项是标志位，设置其为重新计算外参、检查条件数 、固定偏斜系数。 对RGB相机进行鱼眼模型标定，初始化去畸变映射矩阵。]，如果不是鱼眼相机模型[定义迭代终止条件，定义标志选项，（如果是复杂相机模型，设置标志启用k4 k5 k6三个畸变参数，启用倾斜模型，启用薄棱镜畸变模型）, 进行相机标定，计算相机内参和畸变系数，输出重投影误差]。}。计算角点重投影误差[ 初始化单词重投影误差为0，初始化平均重投影误差为0，初始化总的重投影误差为0.定义 用于存放重投影点的向量。遍历所有的世界坐标点集（获取当前索引下的世界坐标点集，通过得到的摄像机内外参数，对空间的三维点进行重投影计算，得到新的投影点（鱼眼相机模型和普通相机分别处理），计算新的投影点与旧的投影点之间的误差，获取当前索引下的图像坐标点集，创建一个用于存储旧的图像点的Mat对象1xN，创建一个用于存储新投影点的Mat对象1xN,将点集转换成Mat（1XN）格式以便计算误差。）使用欧氏距离计算两组点的误差，计算平均误差，将当前的平均误差累加到总误差中，打印重投影误差结果]。  

TOF相机标定{ 如果是鱼眼TOF相机模型[ 初始化标志位，设置重新计算外参、检查条件数、固定倾斜系数，标定鱼眼相机，初始化去畸变矫正映射矩阵 ]，如果不是鱼眼TOF相机模型[ 定义迭代终止条件，初始化标志位，如果是复杂相机模型（设置标志位启用k4 k5 k6三个畸变系数，设置倾斜模型，设置薄棱镜模型），进行相机标定计算内外参和畸变系数，输出总的重投影误差。] 。计算重投影误差[ 定义单词重投影误差、平均重投影误差、总重投影误差，遍历每组世界坐标点计算重投影误差（ 获取当前的世界坐标点，通过得到的摄像机内外参数对空间的三维点进行重投影计算，得到新的投影点（鱼眼相机和非鱼眼相机分别进行世界坐标点的重投影），计算新的投影点和旧的投影点之间的误差（将当前组的世界坐标点和图像坐标点 转换为1XN的Mat型，使用欧氏距离L2范数通过norm计算误差）计算所有角点的平均误差，累加角点平均误差）。 计算总的平均误差，输出校正后的总重投影误差]。  }

求解相机坐标系下所有帧的角点三维坐标集合：获取标定点集的数量，定义存储RGB相机的三维点集，存储TOF相机的三维点集，并使用reserve预先为以上两容器分配足够的内存，以容纳将要存储的三维点。遍历每一帧图像的标定点集[ 定义RGB相机的旋转矩阵和平移向量，初始化RGB图像世界坐标系角点到相机坐标系角点的旋转矩阵，将标定得到的旋转向量转换为旋转矩阵，将旋转矩阵Mat转换为Eigen]Eigen::Matrix3f ,   将 平移向量Mat转换为Eigen::Vector3f 。获取当前帧图像中的三维点数量，遍历角点（ 转换原始世界坐标系三维点为vector3f格式，根据旋转矩阵和平移向量计算 RGB/TOF相机坐标系下的三维点，存储到RGB/TOF点集中）可选的将点集保存到文件] 调整所有帧的RGB/TOF相机坐标系下的点集容器的存储空间至合适大小，将所有帧的RGB/TOF相机坐标系下的点集赋值到成员变量std::vector<Eigen::Vector3f>中。

SVD求解左右相机外参(TOF相机坐标系的三维点到RGB相机坐标系的变换矩阵)： 定义对应点集的对象，定义变换矩阵，定义平均点距离，定义均方根误差（RSME），定义内点数量，获取对应点集（所有帧的RGB相机坐标系下的点集）的数量，调整对应点集的大小以容纳所有的对应点，为所有对应点分配索引（初始化对应点集），使用SVD求解两个相机的外参（旋转和平移矩阵），输出变换矩阵（将TOF坐标系下的三维点变换到RGB坐标系下的变换矩阵）。  计算在工作空间范围内的误差：定义平均距离误差为0，遍历每个点（计算当前TOF点经过变换矩阵后的RGB坐标系下的坐标，累积变换后的点与RGB坐标系下三维点的欧氏距离L2范数），计算平均误差距离，输出平均误差距离。保存平均误差距离和TOF相机到RGB相机的变换矩阵。 返回true.

使用RGB和深度数据创建带颜色的点云（输入RGB Mat，深度图mat，输出点云，保存文件名）：定义一个用于存储点云的向量，用reserve为该向量预先分配内存以优化性能，定义用于存储变换矩阵的变量，定义深度图的比例尺因子，获取深度相机的内参（fx,fy,cx,cy），【遍历深度图的每一行，遍历深度图的每一列，获取深度图中第v行第u列的深度值，如果深度值为0则跳过此点，定义一个三维点并计算其TOF坐标系下的空间坐标，将计算得到的三维点加入点云向量】。shrink_to_fit调整点云向量的大小以适应实际数据。获取变换矩阵的逆矩阵，对TOF坐标系下三维点云进行坐标变换变换到RGB坐标系下。定义存储rgb物理点的向量，并设置大小为点云尺寸，将变换后的三维点复制到rgb物理点向量中。定义用于存储rgb投影点的向量，定义RGB相机的外参R T为0向量，将RGB三维点投影到图像坐标系下。清空并调整输出点云大小为三维点数。获取rgb图像的列和行数。遍历输出点云，将 RGB 图像的颜色信息分配给点云中的每个点{ 获取rgb投影点i的坐标x,y. 如果重投影点在图像外，则使用黑色（设置输出点云i的r g b为0，坐标即RGB三维点坐标）。如果rgb重投影点在图像内，则使用图像中的颜色。 }，最后PCDWriter保存结果点云。

保存标定参数到文件：检查文件名是否是.yml后缀。创建文件写入对象cv::FileStorage，cv::eigen2cv将变换矩阵转换为Mat格式，保存变换矩阵和平均距离误差。  保存RGB相机内参，如果是鱼眼RGB相机保存鱼眼参数模型bool标志和鱼眼RBG相机畸变系数，否则保存普通相机畸变系数。  保存TOF相机内参，如果是鱼眼TOF相机，保存鱼眼模型bool标志和鱼眼TOF相机畸变系数，否则保存普通相机畸变系数。释放文件写入对象，返回true.

从文件加载标定参数：检查是否是.yml文件，打开文件，定义变换矩阵Mat，读取变换矩阵Mat后转换为Eigen::Matrix4f格式,赋值给私有变量。 读取RGB相机内参、是否鱼眼RGB相机模型bool标志，如果是鱼眼RGB相机读取鱼眼RGB相机畸变系数，否则读取普通RGB相机畸变系数。  读取TOF相机内参，读取是否鱼眼TOF相机模型bool标志，如果是鱼眼TOF相机则读取鱼眼TOF相机畸变系数，否则读取普通相机畸变系数。释放文件对象，返回true。

https://blog.csdn.net/T_T_T_T_/article/details/125439151

https://blog.csdn.net/oakchina/article/details/130703831