双目相机标定

一、整体流程概述

1. 图像采集

   • 拍摄多角度、多距离的标定板图像（如棋盘格），确保覆盖整个视场范围。
   • 图像多样性是关键，避免因样本不足导致拟合误差。

2. 单目相机内参标定

   • 分别对两个摄像头单独进行内参标定。
   • 同时标定双目标定时难以保证标定板充满画面，因此先分别标定各自内参以提高精度。

3. 双目相机外参标定

   • 在固定内参的前提下，使用新拍摄的图像计算两个相机之间的旋转和平移矩阵（外参）。

4. 验证标定结果

   • 重投影误差：平均误差应小于一个像素，越小越好。
   • 外参合理性：平移向量模长应接近实际基线长度；若基线为水平方向，则图像对齐后左右图对应点应在同一水平线上（y坐标一致）或右图点与左图极线距离较小。

5. 工具对比：OpenCV vs MATLAB

   • 参考 知乎文章，MATLAB 在特征点匹配精度上通常优于 OpenCV。

6. 标定板选择建议

   • 棋盘格与圆形标定板各有优劣，可参考：棋盘格和圆形标定板对比分析。

二、详细操作步骤

1. 单目图像采集

• 原则：多角度、多距离拍摄，标定板尽量覆盖全画幅。

1. 如果不拍全画幅，主点估计容易出错
2. 如果缺少标定板在图像边缘的图像，则对畸变标定不友好，因为畸变越靠近边缘越大，越好标定

• 示例图像：
  

  

如上图所示，本次标定 X 轴范围为 -2000mm 至 1500mm，Y 轴为 -500mm 至 1500mm，Z 轴为 -5000mm 至 15000mm。
实际中建议按“三个距离 + 三个角度 + 两个高度”原则拍摄，具体范围根据研究深度确定（如 1000mm–5000mm），相机和标定板的位置尽量覆盖研究区域，以提升参数拟合精度。

• 效果验证：
  对应重投影误差为 0.12 pixel，表明标定精度较高。
  

2. 双目图像采集

• 注意事项：

  • 角度不足会导致外参平移量误差较大。
  • 建议手动测量基线长度辅助判断标定结果可靠性。

• 图像分布示意图：
  

  

• 重投影误差验证：
  

注意：重投影误差小并不代表标定结果一定可靠，仍需检查基线长度一致性以及图像对齐后特征点是否位于同一行（基线水平）或同一列（基线竖直）。

三、总结建议

• 标定图像需涵盖完整视场范围，避免过拟合；
• 内参与外参分开标定有助于提高稳定性；
• 多种验证方式结合（重投影误差 + 基线长度 + 极线对齐）更能判断标定质量；
• 工具选择上，MATLAB 精度更高，OpenCV 更易集成于工程系统。

四、代码

1.双目标定

下面给出matlab下的圆形标定代码：

clc;
clear;
close all;

% 参数设置
rows = 10; % 圆点行数 (例如 4 行)
cols = 19; % 圆点列数 (例如 11 列)
spacing = 100; % 圆点间距 (单位：毫米)

% 创建非对称圆网格的世界坐标
worldPoints = generateCircleGridPoints([rows, cols], spacing);

date = '20250619';


% 加载左右相机图像路径
leftImageDir = fullfile('/home/erge/work/Aceii/calibration/photos', date, 'leftSingleImage'); % 替换为左相机图像文件夹路径
rightImageDir = fullfile('/home/erge/work/Aceii/calibration/photos', date, '/rightSingleImage'); % 替换为右相机图像文件夹路径

left_stereo_path = fullfile('/home/erge/work/Aceii/calibration/photos', date,'/leftImage');
right_stereo_path = fullfile('/home/erge/work/Aceii/calibration/photos', date,'/rightImage');


%% 单目
leftImageFiles = dir(fullfile(leftImageDir, '*.png')); 
rightImageFiles = dir(fullfile(rightImageDir, '*.png'));

leftImagePaths = {leftImageFiles.name};
rightImagePaths = {rightImageFiles.name};

% 初始化存储检测到的图像点
rightImagePoints = [];
leftImagePoints = [];

for i = 1:length(leftImagePaths)
    imgPath = fullfile(leftImageDir, leftImagePaths{i});
    I = imread(imgPath);
    
    % 检测圆网格
    detectedPoints = detectCircleGridPoints(I, [rows, cols]);
    
    if ~isempty(detectedPoints) % 检查是否成功检测到圆网格
        fprintf('成功检测到圆网格：%s\n', leftImagePaths{i});
         leftImagePoints= cat(3, leftImagePoints, detectedPoints); % 将检测到的点添加到 imagePoints
        
    else
        fprintf('未能检测到圆网格：%s\n', leftImagePaths{i});
    end
end


for i = 1:length(rightImagePaths)
    imgPath = fullfile(rightImageDir, rightImagePaths{i});
    I = imread(imgPath);
    
    % 检测圆网格
    detectedPoints = detectCircleGridPoints(I, [rows, cols]);
    
    if ~isempty(detectedPoints) % 检查是否成功检测到圆网格
        fprintf('成功检测到圆网格：%s\n', rightImagePaths{i});
         rightImagePoints= cat(3, rightImagePoints, detectedPoints); % 将检测到的点添加到 imagePoints
    else
        fprintf('未能检测到圆网格：%s\n', rightImagePaths{i});
    end
end


% 单目标定（分别标定左右相机）
[leftCamParams, leftReprojectionErrors] = estimateCameraParameters(leftImagePoints, ...
    worldPoints, ...
    'ImageSize', size(I,1:2));

[rightCamParams, rightReprojectionErrors] = estimateCameraParameters(rightImagePoints, ...
    worldPoints, ...
    'ImageSize', size(I,1:2));

% 可视化重投影误差
figure;
showReprojectionErrors(leftCamParams);
title('左目标定重投影误差');

% 可视化外参
figure;
showExtrinsics(leftCamParams);
title('左目标定外参可视化');

% 可视化重投影误差
figure;
showReprojectionErrors(rightCamParams);
title('右目标定重投影误差');

% 可视化外参
figure;
showExtrinsics(rightCamParams);
title('右目标定外参可视化');



%% 立体标定
left_stereo_file = dir(fullfile(left_stereo_path, '*.png')); 
right_stereo_file = dir(fullfile(right_stereo_path, '*.png'));




% 提取左文件名
file_names = {left_stereo_file.name};

% 提取文件名中的数字部分
numbers = zeros(length(file_names), 1);
for i = 1:length(file_names)
    % 假设文件名格式为 "prefix<number>.png"，提取数字部分
    [~, name_without_ext] = fileparts(file_names{i});
    numbers(i) = str2double(regexp(name_without_ext, '\d+', 'match', 'once'));
end

% 按数字排序
[~, sort_idx] = sort(numbers);

% 获取排序后的文件列表
left_stereo_file = left_stereo_file(sort_idx);


% 提取右文件名
file_names = {right_stereo_file.name};

% 提取文件名中的数字部分
numbers = zeros(length(file_names), 1);
for i = 1:length(file_names)
    % 假设文件名格式为 "prefix<number>.png"，提取数字部分
    [~, name_without_ext] = fileparts(file_names{i});
    numbers(i) = str2double(regexp(name_without_ext, '\d+', 'match', 'once'));
end

% 按数字排序
[~, sort_idx] = sort(numbers);

% 获取排序后的文件列表
right_stereo_file = right_stereo_file(sort_idx);

left_stereo_name = {left_stereo_file.name};
right_stereo_name = {right_stereo_file.name};




% 初始化存储检测到的图像点
rightImagePoints = [];
leftImagePoints = [];
% image_path
% 假设 left_stereo_name 和 right_stereo_name 的长度相同
for i = 1:length(left_stereo_name)
    % 读取左图像并检测圆网格
    leftImgPath = fullfile(left_stereo_path, left_stereo_name{i});
    leftI = imread(leftImgPath);
    leftDetectedPoints = detectCircleGridPoints(leftI, [rows, cols]);
    
    % 读取右图像并检测圆网格
    rightImgPath = fullfile(right_stereo_path, right_stereo_name{i});
    rightI = imread(rightImgPath);
    rightDetectedPoints = detectCircleGridPoints(rightI, [rows, cols]);
    
    % 检查左右图像是否都成功检测到圆网格
    if ~isempty(leftDetectedPoints) && ~isempty(rightDetectedPoints)
        % 如果左右图像都成功检测到圆网格，则将点加入
        fprintf('成功检测到圆网格：左图像 %s 和 右图像 %s\n', left_stereo_name{i}, right_stereo_name{i});
        leftImagePoints = cat(3, leftImagePoints, leftDetectedPoints); % 将左图像检测到的点加入
        rightImagePoints = cat(3, rightImagePoints, rightDetectedPoints); % 将右图像检测到的点加入
    else
        % 如果任意一个检测失败，则跳过，不加入任何点
        fprintf('未能同时检测到圆网格：左图像 %s 或 右图像 %s\n', left_stereo_name{i}, right_stereo_name{i});
    end
end



imagePoints= cat(4, leftImagePoints, rightImagePoints);
% 
% [stereoParams, pairsUsed, estimationErrors] = estimateCameraParameters(imagePoints, ...
%     worldPoints, 'ImageSize', size(I,1:2));

% 左右相机的初始内参矩阵和畸变系数
% camExtrinsics = estimateExtrinsics(imagePoints,worldPoints,intrinsics);

% 标定双目外参
[stereoParams, pairsUsed, ReprojectionErrors] = estimateStereoBaseline(...
    imagePoints, worldPoints, ...
    leftCamParams, rightCamParams);  %并没有固定内参



% 显示标定结果
disp('左相机内参矩阵：');
disp(stereoParams.CameraParameters1.IntrinsicMatrix);
disp(leftCamParams.IntrinsicMatrix);

disp('右相机内参矩阵：');
disp(stereoParams.CameraParameters2.IntrinsicMatrix);
disp(rightCamParams.IntrinsicMatrix);


disp('径向畸变系数（左相机）：');
disp(stereoParams.CameraParameters1.RadialDistortion);
disp(leftCamParams.RadialDistortion);


disp('径向畸变系数（右相机）：');
disp(stereoParams.CameraParameters2.RadialDistortion);
disp(rightCamParams.RadialDistortion);

disp('旋转矩阵（从左相机到右相机）：');
disp(stereoParams.RotationOfCamera2);

disp('平移向量（从左相机到右相机）：');
disp(stereoParams.TranslationOfCamera2);
disp(norm(stereoParams.TranslationOfCamera2));

% 可视化重投影误差
figure;
showReprojectionErrors(stereoParams);
title('立体标定重投影误差');

% 可视化外参
figure;
showExtrinsics(stereoParams);
title('立体标定外参可视化');

% % 4. 剔除误差过大的帧，使用了RANSAC，不一定需要这一步
% errorThreshold = 0.3;
% left_error = stereoParams.CameraParameters1.ReprojectionErrors; 
% reight_error = stereoParams.CameraParameters2.ReprojectionErrors; 

% 获取实际用于标定的帧索引
usedIndices = find(pairsUsed);

% 提取左右相机重投影误差
errorsLeft = stereoParams.CameraParameters1.ReprojectionErrors;
errorsRight = stereoParams.CameraParameters2.ReprojectionErrors;
% 计算每帧的平均误差（左右相机平均）
meanErrors = zeros(numel(usedIndices), 1);
for i = 1:numel(usedIndices)
    idx = usedIndices(i);
    % 左相机误差
    errL = squeeze(errorsLeft(:, :,idx));
    rmseL = mean(sqrt(sum(errL.^2, 2)));
    % 右相机误差
    errR = squeeze(errorsRight( :, :,idx));
    rmseR = mean(sqrt(sum(errR.^2, 2)));
    meanErrors(i) = (rmseL + rmseR) / 2;
end
% 设定阈值（例如均值+2倍标准差）
meanError = mean(meanErrors);
stdError = std(meanErrors);
threshold = meanError + 2 * stdError;

% 找到高误差帧的索引
badInUsed = find(meanErrors > threshold);
badIndices = usedIndices(badInUsed);

% 创建新索引，排除高误差帧
newPairsUsed = pairsUsed;
newPairsUsed(badIndices) = false;

% 提取有效角点数据
imagePointsLeftGood = leftImagePoints( :, :,newPairsUsed);
imagePointsRightGood = rightImagePoints( :, :,newPairsUsed);

imagePoints= cat(4, imagePointsLeftGood, imagePointsRightGood);

[stereoParams, pairsUsed, estimationErrors] = estimateCameraParameters(imagePoints, ...
    worldPoints, 'ImageSize', size(I,1:2));

disp('平移向量（从左相机到右相机）：');
disp(stereoParams.TranslationOfCamera2);
disp(norm(stereoParams.TranslationOfCamera2));

% 可视化重投影误差
figure;
showReprojectionErrors(stereoParams);
title('立体标定重投影误差');

% 可视化外参
figure;
showExtrinsics(stereoParams);
title('立体标定外参可视化');


%% 转为opencv格式并保存
[intrinsicMatrix1,distortionCoefficients1,intrinsicMatrix2, ...
   distortionCoefficients2,rotationOfCamera2,translationOfCamera2] =... 
   stereoParametersToOpenCV(stereoParams);

save("result.mat","translationOfCamera2","rotationOfCamera2", ...
    "distortionCoefficients2","intrinsicMatrix2","distortionCoefficients1","intrinsicMatrix1");


calibData.camera_left.K = intrinsicMatrix1;
calibData.camera_left.distortion = distortionCoefficients1;

calibData.camera_right.K = intrinsicMatrix2;
calibData.camera_right.distortion = distortionCoefficients2;

calibData.R = rotationOfCamera2;
calibData.T = translationOfCamera2;

fid = fopen('stereo_calibration.yaml', 'w');
fprintf(fid, '%%YAML:1.0\n');

% 写入 camera_left
fprintf(fid, 'camera_left:\n');
fprintf(fid, '  K:\n');
fprintf(fid, '    - [%f, %f, %f]\n', calibData.camera_left.K(1, :));
fprintf(fid, '    - [%f, %f, %f]\n', calibData.camera_left.K(2, :));
fprintf(fid, '    - [%f, %f, %f]\n', calibData.camera_left.K(3, :));

fprintf(fid, '  distortion: [%f, %f, %f, %f, %f]\n', calibData.camera_left.distortion);

% 写入 camera_right
fprintf(fid, 'camera_right:\n');
fprintf(fid, '  K:\n');
fprintf(fid, '    - [%f, %f, %f]\n', calibData.camera_right.K(1, :));
fprintf(fid, '    - [%f, %f, %f]\n', calibData.camera_right.K(2, :));
fprintf(fid, '    - [%f, %f, %f]\n', calibData.camera_right.K(3, :));

fprintf(fid, '  distortion: [%f, %f, %f, %f, %f]\n', calibData.camera_right.distortion);

% 写入 R 和 T
fprintf(fid, 'R:\n');
fprintf(fid, '  - [%f, %f, %f]\n', calibData.R(1, :));
fprintf(fid, '  - [%f, %f, %f]\n', calibData.R(2, :));
fprintf(fid, '  - [%f, %f, %f]\n', calibData.R(3, :));

fprintf(fid, 'T: [%f, %f, %f]\n', calibData.T(:)');

fclose(fid);



% 创建 Excel 文件
filename = [date,'.xlsx'];

% 写入每个参数到单独的工作表



writematrix(intrinsicMatrix1, filename, 'Sheet', 'IntrinsicMatrix1');
writematrix(distortionCoefficients1, filename, 'Sheet', 'DistortionCoefficients1');
writematrix(intrinsicMatrix2, filename, 'Sheet', 'IntrinsicMatrix2');
writematrix(distortionCoefficients2, filename, 'Sheet', 'DistortionCoefficients2');
writematrix(rotationOfCamera2, filename, 'Sheet', 'RotationOfCamera2');
writematrix(translationOfCamera2, filename, 'Sheet', 'TranslationOfCamera2');

disp(['数据已成功保存到 ', filename]);

2.鱼眼标定：

参考文章：https://blog.csdn.net/ouyangandy/article/details/107088939

圆形标定板的鱼眼标定代码，最后输出的是Scaramuzza模型：

clc;
clear;
close all;

% 参数设置
rows = 7; % 圆点行数 (例如 4 行)
cols = 7; % 圆点列数 (例如 7 列)
spacing = 40; % 圆点间距 (单位：毫米)

% 创建对称圆网格的世界坐标
worldPoints = generateCircleGridPoints([rows, cols], spacing,PatternType="symmetric");


imageDir = fullfile('/home/erge/python/Aceii/calibration/photos/single_fisheye/20250515');

%% 单目
imageFiles = dir(fullfile(imageDir, '*.jpg')); 
imagePaths = {imageFiles.name};

calibrationImages = imageDatastore(imageDir);
imageFileNames = calibrationImages.Files;

% 初始化存储检测到的图像点
imagePoints = [];

for i = 1:length(imagePaths)
    imgPath = fullfile(imageDir, imagePaths{i});
    I = imread(imgPath);
    
    % 检测圆网格
    detectedPoints = detectCircleGridPoints(I, [rows, cols],PatternType="symmetric");
    
    if ~isempty(detectedPoints) && size(detectedPoints,1)==rows*cols% 检查是否成功检测到圆网格
        fprintf('成功检测到圆网格：%s\n', imagePaths{i});
        imagePoints= cat(3, imagePoints, detectedPoints); % 将检测到的点添加到 imagePoints
        figure(1);
        imshow(I); hold on;
        plot(detectedPoints(:,1), detectedPoints(:,2), 'y+');
        hold off;
        title(imagePaths{i})
        pause();
        close;
        
    else
        fprintf('未能检测到圆网格：%s\n', imagePaths{i});
    end
end


% 单目标定
[camParams, reprojectionErrors] = estimateFisheyeParameters(imagePoints, ...
    worldPoints, ...
     [size(I,1) size(I,2)]);% 可视化重投影误差
figure;
showReprojectionErrors(camParams);
title('标定重投影误差');

% 可视化外参
figure;
showExtrinsics(camParams);
title('标定外参可视化');


J1 = undistortFisheyeImage(I,camParams.Intrinsics,'OutputView','full');
figure
imshowpair(I,J1,'montage')
title('Original Image (left) vs. Corrected Image (right)')

J2 = undistortFisheyeImage(I,camParams.Intrinsics,'OutputView','same', 'ScaleFactor', 0.5);
figure
imshow(J2)
title('Output View with low Scale Factor')

points = detectCheckerboardPoints(I);
[undistortedPoints,intrinsics1] = undistortFisheyePoints(points,camParams.Intrinsics);
[J, intrinsics2] = undistortFisheyeImage(I,camParams.Intrinsics,'OutputView','full'); %根据虚拟相机可得到归一化坐标



%% 转为opencv格式并保存
MappingCoefficients = camParams.Intrinsics.MappingCoefficients;
StretchMatrix = camParams.Intrinsics.StretchMatrix  ;
DistortionCenter = camParams.Intrinsics.DistortionCenter;
ImageSize = camParams.Intrinsics.ImageSize;
save("resultSingleFishEye.mat","MappingCoefficients","StretchMatrix", ...
     "DistortionCenter","ImageSize");

matlab使用的是Scaramuzza模型，标定后对图像的矫正代码如下：

// cam_model_general.h
#pragma once

#ifndef CAM_MODEL_GENERAL_H
#define CAM_MODEL_GENERAL_H

#include <opencv2/opencv.hpp>

// horner scheme for evaluating polynomials at a value x
template <typename T>
T horner(T *coeffs, int s, T x)
{
    T res = 0.0;
    for (int i = s - 1; i >= 0; i--)
        res = res * x + coeffs[i];

    return res;
}

// template class implementation of the general atan model
template <typename T>
class cCamModelGeneral
{
public:
    // construtors
    cCamModelGeneral() : c(T(1)),
                         d(T(0)),
                         e(T(0)),
                         u0(T(0)),
                         v0(T(0)),
                         p((cv::Mat_<T>(1, 1) << T(1))),
                         invP((cv::Mat_<T>(1, 1) << T(1))),
                         p_deg(1),
                         invP_deg(1),
                         Iwidth(T(0)), Iheight(T(0))
    {
    }

    cCamModelGeneral(cv::Vec<T, 5> cdeu0v0,
                     cv::Mat_<T> p_,
                     cv::Mat_<T> invP_) : c(cdeu0v0[0]),
                                          d(cdeu0v0[1]),
                                          e(cdeu0v0[2]),
                                          u0(cdeu0v0[3]),
                                          v0(cdeu0v0[4]),
                                          p(p_),
                                          invP(invP_)
    {
        // initialize degree of polynomials
        p_deg = (p_.rows > 1) ? p_.rows : p_.cols;
        invP_deg = (p_.rows > 1) ? invP_.rows :  invP_.cols;

        cde1 = (cv::Mat_<T>(2, 2) << c, d, e, T(1));
    }

    cCamModelGeneral(cv::Vec<T, 5> cdeu0v0,
                     cv::Mat_<T> p_,
                     cv::Mat_<T> invP_,
                     int Iw_, int Ih_) : c(cdeu0v0[0]),
                                         d(cdeu0v0[1]),
                                         e(cdeu0v0[2]),
                                         u0(cdeu0v0[3]),
                                         v0(cdeu0v0[4]),
                                         p(p_),
                                         invP(invP_),
                                         Iwidth(Iw_),
                                         Iheight(Ih_)
    {
        // initialize degree of polynomials
        p_deg = (p_.rows > 1) ? p_.rows :  p_.cols;
        invP_deg = (p_.rows > 1) ?  invP_.rows :  invP_.cols;

        cde1 = (cv::Mat_<T>(2, 2) << c, d, e, T(1));
    }

    ~cCamModelGeneral() {}

    // template <typename T>
    inline void
    WorldToImg(const T &x, const T &y, const T &z, // 3D scene point
               T &u, T &v)                         // 2D image point
    {
        T norm = sqrt(x * x + y * y);
        if (norm == T(0))
            norm = 1e-14;

        T theta = atan(-z / norm);
        T rho = horner<T>((T *)invP.data, invP_deg, theta);

        T uu = x / norm * rho;
        T vv = y / norm * rho;

        u = uu * c + vv * d + u0;
        v = uu * e + vv + v0;
    }

    // template <typename T>
    inline void
    WorldToImg(const cv::Point3_<T> &X, // 3D scene point
               cv::Point_<T> &m)        // 2D image point
    {
        T norm = sqrt(X.x * X.x + X.y * X.y);

        if (norm == T(0))
            norm = 1e-14;

        T theta = atan(-X.z / norm);

        T rho = horner<T>((T *)invP.data, invP_deg, theta);

        T uu = X.x / norm * rho;
        T vv = X.y / norm * rho;

        m.x = uu * c + vv * d + u0;
        m.y = uu * e + vv + v0;
    }

    // fastest by about factor 2
    // template <typename T>
    inline void
    WorldToImg(const cv::Vec<T, 3> &X, // 3D scene point
               cv::Vec<T, 2> &m)       // 2D image point
    {

        double norm = cv::sqrt(X(0) * X(0) + X(1) * X(1));

        if (norm == 0.0)
            norm = 1e-14;

        double theta = atan(-X(2) / norm);

        double rho = horner<T>((T *)invP.data, invP_deg, theta);

        double uu = X(0) / norm * rho;
        double vv = X(1) / norm * rho;

        m(0) = uu * c + vv * d + u0;
        m(1) = uu * e + vv + v0;
    }

    // template <typename T>
    inline void
    ImgToWorld(T &x, T &y, T &z,       // 3D scene point
               const T &u, const T &v) // 2D image point
    {
        T invAff = c - d * e;
        T u_t = u - u0;
        T v_t = v - v0;
        // inverse affine matrix image to sensor plane conversion
        x = (1 * u_t - d * v_t) / invAff;
        y = (-e * u_t + c * v_t) / invAff;
        T X2 = x * x;
        T Y2 = y * y;
        z = -horner<T>((T *)p.data, p_deg, sqrt(X2 + Y2));

        // normalize vectors spherically
        T norm = sqrt(X2 + Y2 + z * z);
        x /= norm;
        y /= norm;
        z /= norm;
    }

    // template <typename T>
    inline void
    ImgToWorld(cv::Point3_<T> &X,      // 3D scene point
               const cv::Point_<T> &m) // 2D image point
    {
        T invAff = c - d * e;
        T u_t = m.x - u0;
        T v_t = m.y - v0;
        // inverse affine matrix image to sensor plane conversion
        X.x = (1 * u_t - d * v_t) / invAff;
        X.y = (-e * u_t + c * v_t) / invAff;
        T X2 = X.x * X.x;
        T Y2 = X.y * X.y;
        X.z = -horner<T>((T *)p.data, p_deg, sqrt(X2 + Y2));

        // normalize vectors spherically
        T norm = sqrt(X2 + Y2 + X.z * X.z);
        X.x /= norm;
        X.y /= norm;
        X.z /= norm;
    }

    // template <typename T>
    inline void ImgToWorld(cv::Vec<T, 3> &X,       // 3D scene point
                           const cv::Vec<T, 2> &m) // 2D image point
    {
        T invAff = c - d * e;
        T u_t = m(0) - u0;
        T v_t = m(1) - v0;
        // inverse affine matrix image to sensor plane conversion
        X(0) = (1 * u_t - d * v_t) / invAff;
        X(1) = (-e * u_t + c * v_t) / invAff;
        T X2 = X(0) * X(0);
        T Y2 = X(1) * X(1); //径向距离
        X(2) = -horner<T>((T *)p.data, p_deg, sqrt(X2 + Y2)); //获取f(r)

        // normalize vectors spherically
        T norm = sqrt(X2 + Y2 + X(2) * X(2));
        X(0) /= norm;
        X(1) /= norm;
        X(2) /= norm;
    }

    // get functions
    T Get_c() { return c; }
    T Get_d() { return d; }
    T Get_e() { return e; }

    T Get_u0() { return u0; }
    T Get_v0() { return v0; }

    int GetInvDeg() { return invP_deg; }
    int GetPolDeg() { return p_deg; }

    cv::Mat_<T> Get_invP() { return invP; }
    cv::Mat_<T> Get_P() { return p; }

    T GetWidth() { return Iwidth; }
    T GetHeight() { return Iheight; }

    cv::Mat GetMirrorMask(int pyrL) { return mirrorMasks[pyrL]; }
    void SetMirrorMasks(std::vector<cv::Mat> mirrorMasks_) { mirrorMasks = mirrorMasks_; }

    bool isPointInMirrorMask(const T &u, const T &v, int pyr)
    {
        int ur = cvRound(u);
        int vr = cvRound(v);
        // check image bounds
        if (ur >= mirrorMasks[pyr].cols || ur <= 0 ||
            vr >= mirrorMasks[pyr].rows || vr <= 0)
            return false;
        // check mirror
        if (mirrorMasks[pyr].at<uchar>(vr, ur) > 0)
            return true;
        else
            return false;
    }

private:
    // affin parameters
    T c;
    T d;
    T e;
    cv::Mat_<T> cde1;
    // principal point
    T u0;
    T v0;
    // polynomial
    cv::Mat_<T> p;
    int p_deg;
    // inverse polynomial
    cv::Mat_<T> invP;
    int invP_deg;
    // image width and height
    int Iwidth;
    int Iheight;
    // mirror mask on pyramid levels
    std::vector<cv::Mat> mirrorMasks;
};

#endif

// cam_model_general.cpp
#include <iostream>
#include <chrono>

#include "cam_model_general.h"

using namespace std;
using namespace cv;

double time2double(std::chrono::steady_clock::time_point start,
	std::chrono::steady_clock::time_point end)
{
	return static_cast<double>(
		std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count() * (double)1e-9);
}

double dRand(double fMin, double fMax)
{
	return fMin + (double)rand() / RAND_MAX * (fMax - fMin);
}

int main()
{
	// number of iterations for speed test
	long iterations = 1e8;

	// take some real world cam model
	// this is the camera model of data set Fisheye1_
	// ATTENTION!! I also switched the principal point coordinates
	Vec<double, 5> interior_orientation(0.998883018922937, -0.0115128845387445,	//cdeu0v0 均是标定得到
		0.0107836324042904, 544.763473297893, 378.781825009886);
	Mat_<double> p = (Mat_<double>(5, 1) << -338.405137634369,  // poly系数，标定得到
		0.0,
		0.00120189826837736,
		-1.27438189154991e-06,
		2.85466623521256e-09);
	// attention: this is the reverse order of findinvpoly 
	// as matlab evaluates the polynomials differently
	Mat_<double> pInv = (Mat_<double>(11, 1) << 510.979186217526, // invPoly系数，标定得到
		291.393724562448,
		-13.8758863124724,
		42.4238251854176,
		23.054291112414,
		-7.18539785128328,
		14.1452111052043,
		18.5034196957122,
		-2.39675686593404,
		-7.18896323060144,
		-1.85081569557094);

	// here comes the camera model
	cCamModelGeneral<double> camModel(interior_orientation, p, pInv);

	// test the correctness of the implementation, at least internally
	double x0 = dRand(0, 5);//不应该是随机数产生的3D点么？为什么每次运行都一样呢？
	double y0 = dRand(0, 5);
	double z0 = dRand(0, 5);

	Vec3d vec3d(x0, y0, z0);
	Vec3d vec3d_normalized = (1 / norm(vec3d)) * vec3d;
	Vec2d projection;
	Vec3d unprojected;
	camModel.WorldToImg(vec3d, projection);//3D点投影到2D点
	cout << "projected point: " << projection << endl;

	camModel.ImgToWorld(unprojected, projection);//把2D点投影到3D点，但这个3D点是归一化的三维点
	cout << "unprojected:" << unprojected << endl;
	cerr << "difference after unproject: " << norm(vec3d_normalized - unprojected) << endl;

	std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
	// timings
	for (int i = 0; i < iterations; ++i)
	{
		camModel.WorldToImg(vec3d, projection);
		camModel.ImgToWorld(unprojected, projection);
	}
	std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();
	cout << "total time for " << iterations << " iterations of world2cam and cam2world: " << time2double(begin, end) << " s" << endl;
	cout << "time for one iteration: " << time2double(begin, end) / iterations * 1e9 << " nano seconds" << endl;
	return 0;
}