标定demo

标定方法

大致概述：

1. 标定板在相机坐标系下的位姿（每张图一帧）

• 通过棋盘格角点 2D 像素坐标 + 棋盘格 3D 物理坐标
• 用 PnP（这里是 solvePnPRansac）解出 rvec, tvec
• 含义：把标定板坐标系的点变到相机坐标系：

$$
\mathbf{p}{cam} = \mathbf{R}{board\to cam}\mathbf{p}{board} + \mathbf{t}{board\to cam}
$$

1. 机器人坐标系与相机坐标系之间的外参（最终目标）

• 额外引入一个“物理约束”：机器人相对标定板的位置已知（dist/height + 板中心偏移 + 固定旋转）
• 于是你能得到 T_robot_board（机器人→标定板）
• 再结合 T_board_cam（标定板→相机）推导 T_robot_cam

最终你输出的 T_robot_cam 是一个 4×4 齐次矩阵，用于把相机坐标系下的点变到机器人坐标系：
$$
\mathbf{p}{robot} = \mathbf{T}{robot_cam},\mathbf{p}_{cam}
$$

---

3D->2D的流程：

先假设内参和标定板格子的宽度：

• $f_x=f_y=112.324$
• $c_x=115.284,;c_y=43.4792$

$$
K=
\begin{bmatrix}
112.324 & 0 & 115.284 \
0 & 112.324 & 43.4792 \
0 & 0 & 1
\end{bmatrix}
$$

宽度：s=3.5cm

假设

• 第 3 列第 2 行角点（从 0 开始计数，左上角第一个为0）

$$
P_{board}=(j\cdot s,; i\cdot s,;0)=(2\cdot0.035,;1\cdot0.035,;0)=(0.07,;0.035,;0)
$$

其中，我们假设求出来的R和t如下，真实值：

假设这帧 PnP 真正的位姿（你可以把它当作“真实答案”）：

• 旋转：绕 Y 轴 20°

$$
R_y(20^\circ)=
\begin{bmatrix}
\cos20^\circ & 0 & \sin20^\circ \
0 & 1 & 0 \
-\sin20^\circ & 0 & \cos20^\circ
\end{bmatrix}
$$

• 平移（单位 m）：

$$
t=(0.02,;-0.01,;0.6)
$$

Step 1：3D 点从棋盘坐标系变到相机坐标系
$$
P_{cam}=R,P_{board}+t
$$
先算 $R,P_{board}$。
$\cos20^\circ \approx 0.9396926,;\sin20^\circ\approx 0.3420201$
$$
\begin{aligned}
X_c &= \cos20^\circ\cdot 0.07 + \sin20^\circ\cdot 0 + 0.02
= 0.9396926\cdot0.07 + 0.02
= 0.0657785 + 0.02
= 0.0857785 \
Y_c &= 1\cdot0.035 + (-0.01) = 0.025 \
Z_c &= -\sin20^\circ\cdot0.07 + \cos20^\circ\cdot0 + 0.6
= -0.3420201\cdot0.07 + 0.6
= -0.0239414 + 0.6
= 0.5760586
\end{aligned}
$$
所以：
$$
P_{cam}\approx(0.0857785,;0.025,;0.5760586)
$$
Step 2：透视投影到归一化平面（除以 Z）
$$
x=\frac{X_c}{Z_c},\quad y=\frac{Y_c}{Z_c}
$$
Step 3：归一化坐标乘以内参得到像素坐标
$$
u=f_x x + c_x,\quad v=f_y y + c_y
$$
最终像素坐标：
$$
\boxed{(u,v)\approx(132.01,;48.35)}
$$
其中，反过来就是RANSAC + PnP（粗解阶段）+目标函数：重投影误差精细化

---

#include <opencv2/opencv.hpp>
#include <vector>
#include <iostream>
#include <fstream>
#include <string>
#include <sys/stat.h>
#include <algorithm> // 必须包含此头文件以使用 std::reverse

using namespace std;
using namespace cv;

int main() {
    // ================= 1. 基础配置 =================
    cv::Size boardSize(7, 5);       
    float squareSize = 0.035f;      // 3.5cm
    string folderPath = "../images"; 

    // 【物理约束】
    double robot_to_board_dist = 0.600;  
    double robot_to_board_height = 0.105; 

    // 相机内参 (使用真实标定值)
    cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << 112.324, 0, 115.284, 0, 112.324, 43.4792, 0, 0, 1);
    cv::Mat distCoeffs = (cv::Mat_<double>(1, 5) << -0.0546285, 0.00154702, -0.000351417, 0.000541768, -0.00218002);

    // ================= 2. 初始化输出与滤波 =================
    string outDir = "./calib_check_images/";
    system(("mkdir -p " + outDir).c_str());
    
    ofstream outFile(outDir + "full_calibration_report.txt");
    auto log_both = [&](const string& msg) { cout << msg << endl; outFile << msg << endl; };

    cv::KalmanFilter KF(6, 6, 0, CV_64F);
    setIdentity(KF.transitionMatrix); 
    setIdentity(KF.measurementMatrix); 
    setIdentity(KF.processNoiseCov, Scalar::all(1e-5)); 
    setIdentity(KF.measurementNoiseCov, Scalar::all(1e-2)); 
    setIdentity(KF.errorCovPost, Scalar::all(1.0));

    bool is_initialized = false;
    cv::Mat current_state; 
    int success_count = 0;

    vector<String> fn;
    glob(folderPath + "/*.png", fn, false); 
    vector<String> fn_jpg;
    glob(folderPath + "/*.jpg", fn_jpg, false);
    fn.insert(fn.end(), fn_jpg.begin(), fn_jpg.end());

    if (fn.empty()) { cerr << "未找到图片！" << endl; return -1; }

    log_both("开始处理并标定 " + to_string(fn.size()) + " 张图片...");

    // ================= 3. 循环处理与实时标定 =================
    for (const auto& imgPath : fn) {
        cv::Mat img = cv::imread(imgPath);
        if (img.empty()) continue;

        cv::Mat gray;
        if (img.channels() == 3) cv::cvtColor(img, gray, COLOR_BGR2GRAY);
        else gray = img.clone();

        vector<cv::Point2f> imagePoints;
        bool found = cv::findChessboardCorners(img, boardSize, imagePoints,
                                               cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_NORMALIZE_IMAGE);

        if (found) {
            // A. 亚像素精修
            cv::cornerSubPix(gray, imagePoints, cv::Size(5, 5), cv::Size(-1, -1),
                             cv::TermCriteria(cv::TermCriteria::EPS | cv::TermCriteria::COUNT, 30, 0.01));

            // B. 顺序自动校正逻辑
            if (imagePoints.front().y > imagePoints.back().y) {
                std::reverse(imagePoints.begin(), imagePoints.end());
            }

            // C. 生成 3D 物理点
            vector<cv::Point3f> objectPoints;
            for (int i = 0; i < boardSize.height; ++i)
                for (int j = 0; j < boardSize.width; ++j)
                    objectPoints.push_back(cv::Point3f(j * squareSize, i * squareSize, 0.0f));

            // D. PnP 解算 (改用 RANSAC)
            cv::Mat rvec, tvec;
            vector<int> inliers;
            cv::solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, 
                               false, 100, 2.0f, 0.99, inliers, cv::SOLVEPNP_ITERATIVE);

            // 有效角点过少则舍弃该帧
            if (inliers.size() < 15) continue;

            // E. 卡尔曼滤波更新
            cv::Mat measurement(6, 1, CV_64F);
            for(int i=0; i<3; i++) { 
                measurement.at<double>(i) = tvec.at<double>(i); 
                measurement.at<double>(i+3) = rvec.at<double>(i); 
            }

            if (!is_initialized) { KF.statePost = measurement; current_state = measurement; is_initialized = true; } 
            else { KF.predict(); current_state = KF.correct(measurement); }
            success_count++;

            // F. 可视化保存 (完全恢复你原来的逻辑)
            string baseName = imgPath.substr(imgPath.find_last_of("/\\") + 1);
            double scale = 4.0;
            cv::Mat bigImg;
            cv::resize(img, bigImg, cv::Size(), scale, scale, INTER_CUBIC);
            if (bigImg.channels() == 1) cv::cvtColor(bigImg, bigImg, COLOR_GRAY2BGR);

            for (size_t i = 0; i < imagePoints.size(); i++) {
                cv::Point2f pt = imagePoints[i] * scale;
                cv::circle(bigImg, pt, 5, Scalar(0, 255, 0), -1);
                cv::putText(bigImg, to_string(i), pt + Point2f(8, 8), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255), 1);
            }
            cv::imwrite(outDir + "large_" + baseName, bigImg);
            cout << "已处理: " << baseName << " | 累计融合帧数: " << success_count << endl;
        }
    }

    if (!is_initialized) { log_both("失败：未识别到任何棋盘格！"); return -1; }

    // ================= 4. 物理闭环解算 =================
    cv::Mat rvec_filt = (cv::Mat_<double>(3, 1) << current_state.at<double>(3), current_state.at<double>(4), current_state.at<double>(5));
    cv::Mat tvec_filt = (cv::Mat_<double>(3, 1) << current_state.at<double>(0), current_state.at<double>(1), current_state.at<double>(2));
    cv::Mat R_board_cam; cv::Rodrigues(rvec_filt, R_board_cam);
    cv::Mat T_board_cam = cv::Mat::eye(4, 4, CV_64F);
    R_board_cam.copyTo(T_board_cam(cv::Rect(0, 0, 3, 3))); 
    tvec_filt.copyTo(T_board_cam(cv::Rect(3, 0, 1, 3)));

    cv::Mat R_robot_board = (cv::Mat_<double>(3, 3) << 0, 0, 1, -1, 0, 0, 0, -1, 0);
    double board_w = (boardSize.width - 1) * squareSize;
    double board_h = (boardSize.height - 1) * squareSize;
    cv::Mat t_robot_to_center = (cv::Mat_<double>(3, 1) << robot_to_board_dist, 0.0, robot_to_board_height);
    cv::Mat t_offset_in_board = (cv::Mat_<double>(3, 1) << board_w / 2.0, board_h / 2.0, 0.0);
    cv::Mat t_robot_to_origin = t_robot_to_center - R_robot_board * t_offset_in_board;

    cv::Mat T_robot_board = cv::Mat::eye(4, 4, CV_64F);
    R_robot_board.copyTo(T_robot_board(cv::Rect(0, 0, 3, 3))); 
    t_robot_to_origin.copyTo(T_robot_board(cv::Rect(3, 0, 1, 3)));

    cv::Mat T_robot_cam = T_robot_board * T_board_cam.inv();

    // ================= 5. 输出报告 =================
    log_both("\n================ 标定总结报告 ================");
    log_both("成功融合帧数: " + to_string(success_count));
    log_both("\n>>>>>> 1. 机器人相对于相机的 4x4 变换矩阵 (T_robot_cam) <<<<<<");
    for(int r=0; r<4; r++) {
        char buf[256];
        if(r<3) sprintf(buf, "  %.8f, %.8f, %.8f, %.8f,", T_robot_cam.at<double>(r,0), T_robot_cam.at<double>(r,1), T_robot_cam.at<double>(r,2), T_robot_cam.at<double>(r,3));
        else sprintf(buf, "  0.00000000, 0.00000000, 0.00000000, 1.00000000;");
        log_both(string(buf));
    }

    log_both("\n>>>>>> 2. 相机安装位置 (Robot Frame) <<<<<<");
    char pos_buf[256];
    sprintf(pos_buf, "  X: %.2f mm\n  Y: %.2f mm\n  Z: %.2f mm", 
            T_robot_cam.at<double>(0,3)*1000.0, T_robot_cam.at<double>(1,3)*1000.0, T_robot_cam.at<double>(2,3)*1000.0);
    log_both(string(pos_buf));

    // ================= 6. 验证新数据组 (Kalman) =================
    log_both("\n>>>>>> 4. 验证新位置数据组 (Kalman 平滑) <<<<<<");
    string testPath = "../new_images"; 
    vector<String> testFn;
    glob(testPath + "/*.png", testFn, false);

    double v_real_x = 0.585, v_real_z = 0.050; // 填入验证时的物理目标值

    KalmanFilter vKF(3, 3, 0);
    setIdentity(vKF.transitionMatrix);
    setIdentity(vKF.measurementMatrix);
    setIdentity(vKF.processNoiseCov, Scalar::all(1e-4));
    setIdentity(vKF.measurementNoiseCov, Scalar::all(1e-2));
    bool v_init = false;

for (const auto& path : testFn) {
        Mat testImg = imread(path);
        if (testImg.empty()) continue;

        Mat gray;
        if (testImg.channels() == 3) cvtColor(testImg, gray, COLOR_BGR2GRAY);
        else gray = testImg.clone();

        vector<Point2f> pts;
        // 1. 粗提取
        if (findChessboardCorners(testImg, boardSize, pts, CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE)) {
            
            // --- 关键：增加亚像素精修 ---
            cornerSubPix(gray, pts, Size(5, 5), Size(-1, -1), 
                         TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 30, 0.01));

            // --- 关键：增加顺序自动校正 (防止验证组图片拍反了) ---
            if (pts.front().y > pts.back().y) {
                std::reverse(pts.begin(), pts.end());
            }

            Mat r, t;
            vector<Point3f> obj;
            for(int i=0; i<boardSize.height; i++)
                for(int j=0; j<boardSize.width; j++)
                    obj.push_back(Point3f(j*squareSize, i*squareSize, 0));

            // 2. 使用 RANSAC 鲁棒解算
            solvePnPRansac(obj, pts, cameraMatrix, distCoeffs, r, t);
            
            // 3. 坐标转换逻辑
            Mat Rb; Rodrigues(r, Rb);
            // 这里使用了标定板中心的偏移
            Mat P_cam = Rb * (Mat_<double>(3,1) << (boardSize.width-1)*squareSize/2.0, (boardSize.height-1)*squareSize/2.0, 0.0) + t;
            Mat P_robot = T_robot_cam(Rect(0,0,3,3)) * P_cam + T_robot_cam(Rect(3,0,1,3));

            // 4. 卡尔曼滤波更新
            Mat meas = (Mat_<float>(3, 1) << (float)P_robot.at<double>(0), (float)P_robot.at<double>(1), (float)P_robot.at<double>(2));
            if(!v_init) { vKF.statePost = meas; v_init = true; }
            vKF.predict();
            Mat est = vKF.correct(meas);
            
            printf("验证帧: %s -> 平滑位置 X:%.2f Y:%.2f Z:%.2f cm\n", 
                   path.substr(path.find_last_of("/\\")+1).c_str(), 
                   est.at<float>(0)*100.0, est.at<float>(1)*100.0, est.at<float>(2)*100.0);
        }
    }

    outFile.close();
    return 0;
}

其中，我们是可以通过机械设备知道标定板中心和机器人底盘中心的大致距离，由于是机器人正对着标定板，所以就只有平移量，但是由于我们标定板的坐标系和机器人的坐标系是不同的，所以还是需要变化，以下是涉及到的坐标系：

1. 核心坐标系定义

程序涉及四个关键坐标系，它们之间的转换关系构成了整个数学模型：

• ${B}$ (Robot Base)：机器人基座坐标系（最终目标系）。
• ${C}$ (Camera)：相机光心坐标系。
• ${W}$ (World/Board)：标定板坐标系，原点位于棋盘格左上角 ID 0。
• ${P}$ (Pixel)：图像像素坐标系

---

1. 图像像素坐标系 ${P}$ (Pixel Coordinate System)

这是最基础的二维坐标系，用于描述点在屏幕/图像上的位置。

• 原点 ($O_p$)：位于图像的左上角。
• X轴 ($u$)：水平向右。
• Y轴 ($v$)：垂直向下。
• 单位：像素 (Pixel)。

注意：它通过相机内参矩阵 $K$ 与相机坐标系相联系。

2. 标定板坐标系 ${W}$ (World/Board Coordinate System)

这是一个用户自定义的右手三维坐标系，用于描述棋盘格上角点的物理位置。

• 原点 ($O_w$)：通常定义在棋盘格检测到的第一个角点（ID 0，左上角）。
• X轴 ($X_w$)：沿棋盘格横向排列的方向（通常是长边）。
• Y轴 ($Y_w$)：沿棋盘格纵向排列的方向（通常是短边）。
• Z轴 ($Z_w$)：垂直于标定板平面（根据右手定则确定方向，通常指向板子内部或外部）。
• 单位：米 (m) 或 毫米 (mm)。

3. 相机坐标系 ${C}$ (Camera Coordinate System)

这是以相机光心为原点的三维坐标系。

• 原点 ($O_c$)：位于相机透镜的光心。
• Z轴 ($Z_c$)：相机的光轴方向，即相机“看”出去的方向。
• X轴 ($X_c$)：平行于图像平面的水平轴（向右）。
• Y轴 ($Y_c$)：平行于图像平面的垂直轴（向下）。
• 特点：PnP 算法（如 solvePnPRansac）计算出的就是标定板系 ${W}$ 到这个系 ${C}$ 的转换。

4. 机器人基座坐标系 ${B}$ (Robot Base Coordinate System)

这是整个标定系统的最终目标参考系，通常是固定的。

• 原点 ($O_b$)：通常位于机器人底座的几何中心。
• Z轴 ($Z_b$)：垂直于地面向上。
• X轴 ($X_b$)：机器人正前方。
• Y轴 ($Y_b$)：根据右手定则确定的侧向方向。
• 意义：所有的视觉结果最终都要通过外参矩阵转换到这个坐标系下，机器人才能根据坐标进行抓取或导航。

---

理论公式推导

我们要让点在机器人坐标系下和相机坐标系下实现转换。其闭环转换关系如下：

$$P_{robot} = T_{robot}^{board} \cdot P_{board}$$

同时，由相机观测可知：

$$P_{cam} = T_{cam}^{board} \cdot P_{board} \implies P_{board} = (T_{cam}^{board})^{-1} \cdot P_{cam}$$

将 $P_{board}$ 代入第一个方程，得到：

$$P_{robot} = (T_{robot}^{board} \cdot (T_{cam}^{board})^{-1}) \cdot P_{cam}$$

由此得出最终的目标外参矩阵：

$$T_{robot}^{cam} = T_{robot}^{board} \cdot (T_{cam}^{board})^{-1}$$

---

由于标定板位置固定(通过机械设备计算出来的)，程序通过手动输入的物理约束构建 $T_{robot}^{board}$。

• 旋转矩阵 $R_{robot}^{board}$：代码中定义为 0, 0, 1; -1, 0, 0; 0, -1, 0。

  这代表了机器人轴向与标定板轴向的映射关系（例如机器人 X 轴对应标定板 Z 轴）。

• 平移补偿：

  $$t_{robot}^{origin} = t_{robot}^{center} - R_{robot}^{board} \cdot t_{offset}$$

  因为物理测量通常是测到标定板中心，但 PnP 算的是左上角 ID 0。需要减去一半的板宽和板高偏移量

---

简单的demo做完了，后续需要重构以下函数：

1. 空间坐标转换与数学库重构

现状：使用 cv::Mat 处理 4x4 矩阵，频繁进行旋转向量（rvec）与矩阵的转换。

重构方向：全面转向 Eigen。

• 重写逻辑：
  • 使用 Eigen::Matrix4d 作为位姿载体。
  • 使用 Eigen::Quaterniond（四元数）表示旋转。四元数在 Ceres 优化中没有欧拉角的“万向锁”问题，且计算开销小。
• 关键函数：T_robot_cam 的逆变换、标定板到相机的相对位姿链。

2. 亚像素精修 (Sub-pixel Corner Refinement)

现状：直接调用 cv::cornerSubPix。

重构方向：基于梯度正交性原理手写迭代解算器。

• 重写逻辑：
  • 计算梯度：使用 Sobel 算子获取邻域内每个像素的 $G_x, G_y$。
  • 构建方程：根据公式 $\nabla I(p_i)^T \cdot (q - p_i) = 0$，对邻域内所有点构建最小二乘方程 $Ax = b$。
  • 迭代求解：使用 Eigen 的 ldlt() 求解 $q$ 的位置。
• 面试价值：手写此函数能证明你理解角点提取的物理本质，而不是只会调包。

3. 位姿估计：RANSAC + P3P (初值估计)

现状：调用 cv::solvePnPRansac。

重构方向：手动实现 RANSAC 循环。

• 重写逻辑：
  • 随机采样：每次随机选 4 个特征点。
  • P3P 求解：手写或引用 P3P 算法（解四次方程）得到 $R, t$ 候选解。
  • 内点校验：计算所有点的重投影误差，统计 Inliers。
• 目标：为下一步的 Ceres 非线性优化提供高质量的初始值。

4. 非线性优化 (后端优化层)

现状：OpenCV 内部集成的 LM 算法，黑盒运行。

重构方向：使用 Ceres Solver 实现 BA (Bundle Adjustment)。

• 重写逻辑：
  • 定义残差块 (Residual Block)：计算 $e = p_{pixel} - K(RP_w + t)$。
  • Jacobian 计算：Ceres 会处理自动求导（Auto-diff），计算误差对位姿 $R, t$ 的偏导数。
  • 鲁棒核函数：引入 Huber Loss 进一步抑制外点干扰，这比 OpenCV 的 RANSAC 阈值更柔性。
• 关键点：这是 VIO 的核心，能够实现“滑动窗口优化”的基础。

5. 状态估计：从 KF 到预积分/图优化

现状：基于单位阵预测的单点卡尔曼滤波。

重构方向：基于 Eigen 的协方差传递与融合。

• 重写逻辑：
  • 预测模型：引入运动模型（如果后续加 IMU，则涉及预积分）。
  • 状态向量：扩展为 15 维以上（位姿、速度、零偏等）。
  • 协方差更新：手动实现 $P = FPF^T + Q$，这需要用 Eigen 处理高维矩阵。