4. 损失函数设计

损失函数，说白了就是标定优化的“指挥棒”。你指挥棒指的方向不对，模型再怎么调也白搭。我见过不少团队，花了大把时间调标定参数，结果效果不好，回头一看——损失函数设计得太简单了。

手眼标定里，我们到底要优化什么？两个目标：一是让投影点尽量准，二是让机械臂末端位姿尽量对。单看一个，容易掉坑里。

4.1 为什么不能只用重投影误差？

重投影误差很直观——把3D点投影到图像上，算算和实际检测到的像素差多少。但只靠它，有个致命问题。

所以，必须把位姿误差也加进来。位姿误差直接约束机械臂末端在三维空间里的位置和姿态，和重投影误差互补。

4.2 联合损失函数的结构

我个人习惯把联合损失函数写成这样：

L_total = λ_reproj * L_reproj + λ_pose * L_pose

两个部分，两个权重。权重怎么定？看你的应用场景。

嗯，这里要注意：权重不是拍脑袋定的。我一般先用网格搜索跑一轮，看看不同权重组合下的验证集误差，再选最优。

4.3 重投影误差项 L_reproj 的细节

重投影误差的计算，核心就三步：

1. 投影：把标定板上的3D点，通过相机内参和外参，投影到图像平面。
2. 检测：从图像里检测出对应的2D点（比如角点）。
3. 求差：算投影点和检测点之间的欧氏距离。

公式长这样：

L_reproj = (1/N) * Σ || p_detected - p_projected ||²

其中 N 是所有点的总数。我建议用平方误差而不是值误差——它对大偏差惩罚更狠，能更快把离群点拉回来。

4.4 位姿误差项 L_pose 的设计

位姿误差稍微复杂点。它比较的是：通过标定参数算出来的机械臂末端位姿，和实际记录的位姿之间的差异。

我把它拆成位置误差和姿态误差两部分：

L_pose = α * L_position + β * L_orientation

位置误差直接用欧氏距离：

L_position = || t_estimated - t_measured ||²

姿态误差呢？我习惯用四元数角度差：

L_orientation = 2 * arccos(|q_estimated · q_measured|)

为什么用四元数？因为欧拉角有万向锁问题，旋转矩阵又太冗余。四元数插值和求差都稳定，我在项目里用了好几年，没出过问题。

α 和 β 的比值，我一般设 α : β = 1 : 100。为什么？因为位置单位是毫米，姿态单位是弧度，数值量级差很多。不缩放的话，姿态误差会被位置误差淹没。

4.5 联合损失函数的整体流程

下面这张图，是我自己画的一个流程框架，帮你理清联合损失函数在整个标定优化里的位置：

4.6 代码实现示例

下面是我在项目里用的一段核心代码，实现了联合损失函数的计算。你可以直接拿去改：

def compute_joint_loss(params, calib_data):
    """
    联合损失函数计算
    params: [fx, fy, cx, cy, k1, k2, tx, ty, tz, rx, ry, rz]
    calib_data: 包含3D点、2D点、机械臂位姿的字典
    """
    # 解包参数
    fx, fy, cx, cy, k1, k2 = params[:6]
    t = params[6:9]
    r = params[9:12]

    # 1. 重投影误差
    reproj_loss = 0.0
    for pts_3d, pts_2d in zip(calib_data['points_3d'], calib_data['points_2d']):
        pts_proj = project_points(pts_3d, fx, fy, cx, cy, k1, k2, t, r)
        reproj_loss += np.sum((pts_2d - pts_proj) ** 2)

    reproj_loss /= len(calib_data['points_3d'])

    # 2. 位姿误差
    pose_loss = 0.0
    for pose_est, pose_meas in zip(calib_data['poses_estimated'], calib_data['poses_measured']):
        # 位置误差
        pos_err = np.linalg.norm(pose_est[:3] - pose_meas[:3]) ** 2
        # 姿态误差（四元数角度差）
        q_est = pose_est[3:]
        q_meas = pose_meas[3:]
        dot = np.abs(np.dot(q_est, q_meas))
        angle_err = 2 * np.arccos(np.clip(dot, -1.0, 1.0))
        pose_loss += pos_err + 100.0 * angle_err

    pose_loss /= len(calib_data['poses_estimated'])

    # 3. 联合损失
    lambda_reproj = 0.5
    lambda_pose = 0.5
    total_loss = lambda_reproj * reproj_loss + lambda_pose * pose_loss

    return total_loss

4.7 避坑指南

最后，分享几个我亲身踩过的坑：

• 我曾经把重投影误差和位姿误差直接相加，没做量纲归一化。结果姿态误差的梯度太小，优化器几乎只优化位置，标定完姿态偏差巨大。后来加了缩放因子才解决。
• 我曾经在联合损失里用了L1范数（值误差），想着对离群点鲁棒。结果收敛速度慢得离谱，迭代了500轮还在震荡。换成L2范数后，50轮就稳了。
• 注意：如果标定板图像有遮挡或反光，检测到的2D点可能有离群值。我建议在计算重投影误差前，先做一步离群点剔除——比如用RANSAC或者简单的3σ准则。

嗯，损失函数设计这块，说白了就是平衡二字。重投影误差管“看得到”，位姿误差管“抓得准”，两者缺一不可。权重调好了，标定精度能上一个台阶。