OpenCvSharp中EstimatePoseSingleMarkers输出异常排查

基础检测实现 #

在测试图像上使用OpenCvSharp实现ArUco标记检测，参考官方ArucoSample编写检测逻辑：

核心检测代码如下：

Mat src = new Mat("./aruco_test_4.png", ImreadModes.Color);
Mat gray = src.CvtColor(ColorConversionCodes.BGR2GRAY);            
var detectorParameters = DetectorParameters.Create();
detectorParameters.CornerRefinementMethod = CornerRefineMethod.Subpix;
detectorParameters.CornerRefinementWinSize = 9;
var dictionary = CvAruco.GetPredefinedDictionary(PredefinedDictionaryName.Dict4X4_100);
CvAruco.DetectMarkers(gray, dictionary, out var corners, out var ids, detectorParameters, out var rejectedPoints);
using var detectedMarkers = src.Clone();
CvAruco.DrawDetectedMarkers(detectedMarkers, corners, ids, Scalar.Crimson);
detectedMarkers.SaveImage("./detectedMarkers.png");

检测返回的四个角点坐标准确，绘制的检测结果验证标记已被成功识别：

位姿估计异常复现 #

测试所用标记为直接生成的无畸变标准图，预期位姿估计结果和角点检测结果一致、稳定合理，但调用EstimatePoseSingleMarkers()计算位姿时，返回的rvecs、tvecs数值完全不稳定，异常随代码调整分阶段出现：

• 初始读取方式异常
  最初按float类型逐元素读取位姿返回值，代码如下：

  Mat rvecs = new Mat();
  Mat tvecs = new Mat();
  float markerLength = (float)0.1;
  cva.EstimatePoseSingleMarkers(corners, markerLength, cameraMatrix, distCoeffs, rvecs, tvecs);
  
  float rvecsVal1 = rvecs.At<float>(0, 0);
  float rvecsVal2 = rvecs.At<float>(0, 1);
  float rvecsVal3 = rvecs.At<float>(0, 2);
  
  float tvecsVal1 = tvecs.At<float>(0, 0);
  float tvecsVal2 = tvecs.At<float>(0, 1);
  float tvecsVal3 = tvecs.At<float>(0, 2);
  

  连续三次运行程序得到的结果差异极大，包含超大数值、0值、NaN等无效结果。基于该结果构建齐次变换矩阵处理点云时，200万点全部被变换为沿X轴的直线，结果完全错误，齐次矩阵构建代码如下：

  Mat homTransMat = Mat.Zeros(4, 4, MatType.CV_32FC1);
  // 拼接旋转矩阵
  for (int r = 0; r < rotatMat.Rows; r++)
      for (int c = 0; c < rotatMat.Cols; c++)
          homTransMat.At<float>(r, c) = tvecs.At<float>(r, c);
  // 拼接平移向量
  homTransMat.At<float>(0, 3) = tvecs.At<float>(0, 0);
  homTransMat.At<float>(1, 3) = tvecs.At<float>(0, 1);
  homTransMat.At<float>(2, 3) = tvecs.At<float>(0, 2);
  homTransMat.At<float>(3, 0) = homTransMat.At<float>(3, 1) = homTransMat.At<float>(3, 2) = 0;
  homTransMat.At<float>(3, 3) = 1;
  var homTransMatrix4x4Inverse = homTransMatrix4x4.Inverse();
  

• 调整读取方式后出现NaN
  经提示将数据读取方式改为Vec3d rvecsVal = rvecs.At<Vec3d>(0);后，数据一致性提升，但rvecs、tvecs全部为NaN。
• 调整矩阵类型后旋转矩阵异常
  将相机内参矩阵、畸变系数矩阵的类型从CV_32FC1改为CV_64FC1，修改代码如下：

  //static Mat cameraMatrix = new(3, 3, MatType.CV_32FC1);
  //static Mat distCoeffs = new(5, 1, MatType.CV_32FC1);
  // 修改为
  static Mat cameraMatrix = new(3, 3, MatType.CV_64FC1);
  static Mat distCoeffs = new(5, 1, MatType.CV_64FC1);
  

  修改后NaN消失，得到了稳定的rvecs、tvecs数值。随后调用Cv2.Rodrigues(rvecsVal, rotatMat, jacobian);生成旋转矩阵，但按float类型读取旋转矩阵元素时，得到的数值包含大量超大异常值，完全不符合标记无旋转时旋转矩阵应接近单位矩阵的预期。

待解决问题 #

• 上述一系列异常的根本错误原因是什么？
• 应当如何排查旋转矩阵的数值异常问题？

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