求解T_SpSq=THS⁻¹·TPQ·THS时遇Eigen矩阵维度不匹配错误排查

核心原因 #

C2338是Eigen库的断言错误，直接指向运算中矩阵/向量的行列数不匹配，触发点在expMap(omega_perturbed) - R这一步，说明expMap的输出矩阵和R的尺寸不一致。

具体排查步骤 #

• 检查expMap输出与R的尺寸
  直接打印两者的行列数确认差异：

  std::cout << "expMap输出尺寸: " << expMap(omega_perturbed).rows() << "x" << expMap(omega_perturbed).cols() << std::endl;
  std::cout << "R的尺寸: " << R.rows() << "x" << R.cols() << std::endl;
  

  正常情况下，若R是3x3旋转矩阵，expMap输入3维角速度向量时也应输出3x3矩阵；如果expMap输出4x4变换矩阵，大概率是输入的omega_perturbed包含了平移分量（比如是6维位姿向量）。

• 验证omega_perturbed的维度
  确认omega_perturbed是3维向量（对应SO(3)旋转的扰动），而非6维位姿向量。检查定义：

  // 正确示例
  Eigen::Vector3d omega_perturbed = omega;
  omega_perturbed(j) += 1e-8;
  

  若误用Eigen::Vector6d或动态尺寸的VectorXd且未固定为3维，会导致expMap输出错误尺寸的矩阵。

• 检查dR_domega的预分配尺寸
  dR_domega是旋转矩阵对3维角速度的雅可比，尺寸必须是3x3。确认初始化代码：

  // 正确初始化
  Eigen::Matrix3d dR_domega = Eigen::Matrix3d::Zero();
  

  若预分配为3x6或其他错误尺寸，赋值列向量时会间接引发尺寸不匹配。

• 排查TPQ矩阵的尺寸合规性
  虽然错误触发点不在TPQ直接运算，但需确保TPQ尺寸符合方程要求：若THS是4x4变换矩阵，THS⁻¹和TPQ都必须是4x4，才能满足THS⁻¹·TPQ·THS的矩阵乘法规则。检查Calibration类中TPQ的定义是否为Eigen::Matrix4d。

快速验证方案 #

在错误行前添加断言，强制校验尺寸匹配：

Eigen::MatrixXd exp_result = expMap(omega_perturbed);
assert(exp_result.rows() == R.rows() && exp_result.cols() == R.cols() && "expMap输出与R尺寸不匹配");
dR_domega.col(j) = (exp_result - R) / 1e-8;

内容的提问来源于stack exchange，提问作者konjac s