Abstract: 有个项目的车在前视标定F30,F120时，无法使用代码的ceres。可能是版本or其他原因。使用solvepnp的方式，F30的标定结果会有比较大的偏差，主要是位移。实现了不使用ceres，给定位移，标定优化旋转，达到了很高准确度。
算法与已知项
总体流程如下：
- 单独对F30, F120进行solvepnp求解参数
- 基于baseline和F120的标定结果，赋值F30的位移
- 基于李代数和高斯牛顿优化算法进行求导计算F30的旋转
- 获取结果与计算误差
已知项为：
- 两个相机的内参
- 识别出的Apriltag图像点与三维的Apriltag真值点坐标
- baseline的值
求解前的准备
主要是读取参数以及去畸变，还有baseline的修改。
cv::Mat m2s_t = cv::Mat::zeros(3, 1, CV_64FC1);
m2s_t.at<double>(0) = -fixedbaseline;
////////////////////////////////////////////////////////////////
vector<cv::Point2f> MasterUndistpoints, SlaveUndistpoints;
if (master->ins_type == 0) {
cv::undistortPoints(master->imgPoints, MasterUndistpoints, master->K, master->D, Mat(), master->K);
cv::undistortPoints(slave->imgPoints, SlaveUndistpoints, slave->K, slave->D, Mat(), slave->K);
} else if (master->ins_type == 1) {
cv::fisheye::undistortPoints(master->imgPoints, MasterUndistpoints, master->K, master->D, Mat(), master->K);
cv::fisheye::undistortPoints(slave->imgPoints, SlaveUndistpoints, slave->K, slave->D, Mat(), slave->K);
}
// 内参
double s_fx, s_fy, s_cx, s_cy;
s_fx = slave->K.at<double>(0, 0);
s_fy = slave->K.at<double>(1, 1);
s_cx = slave->K.at<double>(0, 2);
s_cy = slave->K.at<double>(1, 2);
solvepnp计算初始值
直接利用三维点3d和图片点2d进行求解。F120的外参就直接把这个作为解，F30是初值。
这是直接调用函数，没什么值得说的，贴一下代码，Master是F120，Slave是F30：
cv::Mat m_K = master->K;
cv::Mat m_distCoeffs = cv::Mat::zeros(4, 1, CV_64F);
cv::Mat m_rvec, m_tvec;
bool flag = cv::solvePnP(MasterobjectPoints, MasterimagePoints, m_K, m_distCoeffs, m_rvec, m_tvec, false, SOLVEPNP_ITERATIVE);
std::cout << "[Master] m_rvec: " << m_rvec.t() << ", m_tvec: " << m_tvec.t() << std::endl;
// 30pnp
std::vector<cv::Point2f> SlaveimagePoints;
std::vector<cv::Point3f> SlaveobjectPoints;
for (int i = 0; i < SlaveUndistpoints.size(); i++) {
SlaveimagePoints.emplace_back(SlaveUndistpoints[i].x, SlaveUndistpoints[i].y);
SlaveobjectPoints.emplace_back(slave->objPoints[i].x, slave->objPoints[i].y, slave->objPoints[i].z);
}
cv::Mat s_K = slave->K;
cv::Mat s_distCoeffs = cv::Mat::zeros(4, 1, CV_64F);
cv::Mat s_rvec, s_tvec;
flag = cv::solvePnP(SlaveobjectPoints, SlaveimagePoints, s_K, s_distCoeffs, s_rvec, s_tvec, false, SOLVEPNP_ITERATIVE);
std::cout << "[Slaveo] s_rvec: " << s_rvec.t() << ", s_tvec: " << s_tvec.t() << std::endl;
F30位移固定方式
统一一下符号，用小写t和r来表示向量，大写R表述矩阵。
m代表master主相机，也就是F120， s代表slave也就是F30
w代表world世界坐标系
主要是沿用了老代码的固定方式，我写个公式示意一下，首先定义master相机到slave的位移t： $$ t_{m->s} = \begin{bmatrix} -baseline \\ 0 \\ 0 \end{bmatrix} $$
solvepnp求解的结果分别是$r_{w->m}$与$r_{w->s}$，将这些向量转换成旋转矩阵cv::Rodrigues，然后做如下变换：
$$
R_{m->s} = R_{w->s} * R_{w->m}^{-1}
$$
这里取逆矩阵就是逆方向的旋转，左乘的旋转变换是连续的:
$$
R_{m->w} = R_{w->m}^{-1}
$$
最终的位移：
$$
t_{w->s} = R_{m->s} * t_{w->m} + t_{m->s}
$$
Matrix3d R0; // 初始R
Vector3d t; // 固定t
cv::Mat R_w2m, R_w2s;
cv::Rodrigues(m_rvec, R_w2m);
cv::Rodrigues(s_rvec, R_w2s);
cv::cv2eigen(R_w2s, R0); // R0初值确定
cv::Mat R_m2s = R_w2s * R_w2m.inv();
cv::Mat t_w2s;
t_w2s = R_m2s * m_tvec + m2s_t;
cv::cv2eigen(t_w2s, t); // t确定
优化部分
由成像原理可知，是可以通过求导的方式来获取每个待优化变量的导数值的。依着这个思路，可以想到用导数相关的优化算法。先推理吧，这是已经去了畸变的图像点，所以这里不考虑畸变的影响，成像原理可见[笔记]相机成像。
误差
定义最终的误差向量为: $$ e = \begin{bmatrix} u - u_{pred} \\ v - v_{pred} \\ 0 \end{bmatrix} $$
误差值为： $$ {||e||}^2 = (u - u_{pred})^2 + (v - v_{pred})^2 $$
成像与李代数
现在由三维点成像开始推理，顺带求导数，这是初始值计算出来的相机坐标系下的点$P_c0$： $$ P_c0 = R_0 * P_w + t $$
我做了如下假定，在t轻微移动的情况下，R应该也只需要轻微的旋转，量不大，就可以获得准确的值。所以定义一个新的旋转矩阵$\delta R$来表示这个微小的旋转，现在新的坐标如下： $$ P_c = \delta R R_0 * P_w + t $$
现在做：$\frac{\partial{P_c}}{\partial{\delta R}}$的求导。但是显然，$\delta R$是一个3x3的矩阵，不方便求导；从另一个角度讲，其实要求解的只有roll, pitch, yaw三个角度，也就是三个自由度，但是这个矩阵有9个自由度明显不合适： $$ \delta R = \begin{bmatrix} r_{1,1} & r_{1,2} &r_{1,3} \\ r_{2,1} & r_{2,2} &r_{2,3} \\ r_{3,1} & r_{3,2} &r_{3,3} \end{bmatrix} $$
由李代群的定义，可知旋转矩阵可做如下转换： $$ R = exp(\phi^{\wedge}) $$
$\phi$是一种李代数，他是定义在$\mathbb{R}^3$上的向量，每一个$\phi$都可以生成一个反对称矩阵： $$ \Phi = \phi^{\wedge} = \begin{bmatrix} 0 & -\phi_3 & \phi_2 \\ \phi_3 & 0 & -\phi_1 \\ -\phi_2 & \phi_1 & 0 \end{bmatrix} \in \mathbb{R}^{3x3} \tag{skew} $$
然后来看一下$exp$，直接写结论了： $$ exp(\phi^{\wedge}) = \sum_{n=0}^{\infty} \frac{1}{n!}(\phi^{\wedge})^n $$
设模长和方向分别为$a$和$\theta$。设a是一个长度为1的方向向量，即$||a||=1$，可有： $$ \begin{split} exp(\phi^{\wedge}) &= exp(\theta a^{\wedge}) \\ &= \sum_{n=0}^{\infty} \frac{1}{n!}(\theta a^{\wedge})^n \\ &= cos(\theta) I + (1-cos(\theta))a a^T + sin(\theta) a^{\wedge} \end{split} \tag{expSO3} $$
回到之前的成像公式得： $$ \begin{split} P_c &= \delta R R_0 * P_w + t \\ &= exp(\phi^{\wedge}) R_0 * P_w + t \end{split} $$
在$\phi =0$附近进行一阶得泰勒展开： $$ exp(\phi^{\wedge}) = I + \phi^{\wedge} + O(||\phi||^2) $$
可得： $$ \left. \frac{\partial P_c}{\partial \phi} \right\rvert_{\phi=0} = \left. \frac{\partial}{\partial \phi}[(I + \phi^{\wedge} + O(||\phi||^2))R_0 P_w] \right|_{{\phi=0}} $$
忽略二阶以上得高阶项： $$ 右式 = (I + \phi^{\wedge})R_0 P_w = R_0 P_w + \phi^{\wedge} (R_0 P_w) $$
此时，式子左边的$R_0 P_w$与求导无关，可以忽略，只看右侧。$\phi^{\wedge} (R_0 P_w)$可以看作是两个向量的叉积，因为$R_0 P_w$显然是一个向量。
补充一下向量叉积性质： $$ \begin{split} a \times b &= a^{\wedge} b \\ a \times b &= -b \times a \end{split} $$
将性质引用可到求导结果： $$ \left. \frac{\partial P_c}{\partial \phi} \right|_{\phi=0} = -(R_0 P_w)^{\wedge} $$
会用到的反对称矩阵和exp函数代码如下：
Eigen::Matrix3d Stereo_Calibrator::skewSymmetric(const Vector3d& v) {
Matrix3d S;
S << 0, -v.z(), v.y(), v.z(), 0, -v.x(), -v.y(), v.x(), 0;
return S;
}
Eigen::Matrix3d Stereo_Calibrator::expMap(const Vector3d& xi) {
double theta = xi.norm();
if (theta < 1e-6) {
return Matrix3d::Identity() + skewSymmetric(xi);
}
// Rodrigues 公式
Vector3d omega = xi / theta;
Matrix3d omega_hat = skewSymmetric(omega);
// return Matrix3d::Identity() + sin(theta) * omega_hat + (1 - cos(theta)) * omega_hat * omega_hat; 等价于下面
return cos(theta) * Matrix3d::Identity() + sin(theta) * omega_hat + (1 - cos(theta)) * omega * omega.transpose();
}
内参部分
现在有相机坐标系坐标$P_c$，要做归一化和内参投影。 $$ P_c = \begin{bmatrix} X_c \\ Y_c \\ Z_c \end{bmatrix} $$ 加了扰动后的图像坐标结果： $$ u_{pred} = f_x \frac{X_c}{Z_c} + c_x \\ v_{pred} = f_y \frac{Y_c}{Z_c} + c_y $$
带入误差公式可得： $$ e_u = u - u_{pred} = u - f_x \frac{X_c}{Z_c} + c_x \\ e_v = v - v_{pred} = v - f_y \frac{Y_c}{Z_c} + c_y $$
由求导雅可比可知： $$ \begin{aligned} \frac{\partial e}{\partial P_c} &= \begin{bmatrix} \frac{\partial e_u}{\partial X_c} & \frac{\partial e_u}{\partial Y_c} & \frac{\partial e_u}{\partial Z_c} \\ \frac{\partial e_v}{\partial X_c} & \frac{\partial e_v}{\partial Y_c} & \frac{\partial e_v}{\partial Z_c} \end{bmatrix} \\ &= \begin{bmatrix} \frac{f_x}{Z_c} & 0 & -\frac{f_x X_c}{Z_c^2} \\ 0 & \frac{f_y}{Z_c} & -\frac{f_y Y_c}{Z_c^2} \end{bmatrix} \end{aligned} $$
最终误差雅可比
链式求导 $$ \frac{\partial e}{\partial \phi} = \frac{\partial e}{\partial P_c} \frac{\partial P_c}{\partial \phi} = \begin{bmatrix} \frac{f_x}{Z_c} & 0 & -\frac{f_x X_c}{Z_c^2} \\ 0 & \frac{f_y}{Z_c} & -\frac{f_y Y_c}{Z_c^2} \end{bmatrix} * -(R_0 P_w)^{\wedge} $$
优化部分代码
用上高斯牛顿法进行优化，代码如下：
// 扰动李代数
Eigen::Vector3d xi = Eigen::Vector3d::Zero();
int max_inter = 10;
Matrix3d R; // 优化目标
for (int inter = 0; inter < max_inter; ++inter) {
Matrix3d dR = expMap(xi);
R = dR * R0;
// 构建线性系统
Eigen::Matrix3d H = Matrix3d::Zero();
Eigen::Vector3d g = Vector3d::Zero();
double total_error = 0;
int valid_points = 0;
for (size_t i = 0; i < SlaveobjectPoints.size(); ++i) {
Vector3d Pw(SlaveobjectPoints[i].x, SlaveobjectPoints[i].y, SlaveobjectPoints[i].z);
Vector2d Pc(SlaveimagePoints[i].x, SlaveimagePoints[i].y);
Eigen::Vector3d p_wc = R0 * Pw; // 注意：这里不加 t!
Eigen::Vector3d p_c = R * Pw + t; // 当前旋转 + 固定平移
double z = p_c.z();
if (z <= 1e-6) continue;
// 归一化坐标
double xn = p_c.x() / z;
double yn = p_c.y() / z;
// 像素坐标
double u_pred = s_fx * xn + s_cx;
double v_pred = s_fy * yn + s_cy;
// 误差
double eu = u_pred - Pc.x();
double ev = v_pred - Pc.y();
total_error += eu * eu + ev * ev;
valid_points++;
double inv_z = 1.0 / z;
double inv_z2 = inv_z * inv_z;
// ∂e/∂p_c
Eigen::Matrix<double, 2, 3> de_dpc;
de_dpc << s_fx * inv_z, 0, -s_fx * p_c.x() * inv_z2, 0, s_fy * inv_z, -s_fy * p_c.y() * inv_z2;
// ∂p_c/∂ξ = -[p_wc]×
// 这里用的是 p_wc = R0 * Pw，不是 p_c0 = R0 * Pw + t
Eigen::Matrix3d p_wc_hat = skewSymmetric(p_wc);
Eigen::Matrix3d dpc_dxi = -p_wc_hat;
// 完整的雅可比
Eigen::Matrix<double, 2, 3> Ji = de_dpc * dpc_dxi;
// 5. 添加到高斯牛顿系统
H += Ji.transpose() * Ji;
g += Ji.transpose() * Vector2d(eu, ev);
}
if (valid_points == 0) {
std::cout << "没有有效点！" << std::endl;
break;
}
// 6. 求解更新量
// (H + λI) Δξ = -g
double lambda = 1e-3;
Matrix3d H_lm = H + lambda * Matrix3d::Identity();
Vector3d delta_xi = -H_lm.ldlt().solve(g);
// 7. 更新扰动
xi += delta_xi;
std::cout << "iter " << inter << ", total_error=" << total_error << std::endl;
// 8. 检查收敛
if (delta_xi.norm() < 1e-6) break;
}
保存结果
Eigen::Matrix3d s_R = expMap(xi) * R0;
cv::Mat s_rvec_opt;
cv::Mat R_cv;
cv::eigen2cv(s_R, R_cv);
cv::Rodrigues(R_cv, s_rvec_opt);
// update wide's r t
master->world2camera_r = cv::Mat::zeros(3, 1, CV_64FC1);
master->world2camera_t = cv::Mat::zeros(3, 1, CV_64FC1);
slave->world2camera_r = cv::Mat::zeros(3, 1, CV_64FC1);
slave->world2camera_t = cv::Mat::zeros(3, 1, CV_64FC1);
master->world2camera_r.at<double>(0) = m_rvec.at<double>(0);
master->world2camera_r.at<double>(1) = m_rvec.at<double>(1);
master->world2camera_r.at<double>(2) = m_rvec.at<double>(2);
master->world2camera_t.at<double>(0) = m_tvec.at<double>(0);
master->world2camera_t.at<double>(1) = m_tvec.at<double>(1);
master->world2camera_t.at<double>(2) = m_tvec.at<double>(2);
slave->world2camera_r.at<double>(0) = s_rvec_opt.at<double>(0);
slave->world2camera_r.at<double>(1) = s_rvec_opt.at<double>(1);
slave->world2camera_r.at<double>(2) = s_rvec_opt.at<double>(2);
slave->world2camera_t.at<double>(0) = t_w2s.at<double>(0);
slave->world2camera_t.at<double>(1) = t_w2s.at<double>(1);
slave->world2camera_t.at<double>(2) = t_w2s.at<double>(2);
optimise2为本算法结果，ceres为原始结果，见图差异不大。绿色为图像识别角点，红色为3d点利用计算结果进行反投影。
Last modified on 2026-03-19