SVD算法

奇异值分解的应用场景

奇异值分解在计算机视觉和机器人领域中主要用于解决点集配准（Point Set Registration）问题，具体包括：

核心应用

  • 位姿估计：通过两组对应的3D点集，计算它们之间的刚体变换关系
  • 相机标定：计算相机坐标系与世界坐标系之间的变换
  • 三维重建：从不同视角的点云数据恢复物体的三维结构
  • 机器人定位：计算机器人在环境中的位置和姿态

与2D平面仿射变换的对比

共同点

  • 都用于描述空间中点集之间的变换关系
  • 都可以将一个坐标系下的点映射到另一个坐标系
  • 都可以通过矩阵运算进行高效计算

主要区别

特性 3D刚体变换(SVD) 2D仿射变换
空间维度 三维空间 二维平面
变换类型 刚体变换（旋转+平移） 仿射变换（旋转+平移+缩放+剪切）
约束条件 保持距离和角度不变，det(R)=1 保持平行性，不保证距离和角度
自由度 6个（3个旋转+3个平移） 6个（2x3矩阵）
应用场景 3D点云配准、SLAM、相机标定 图像配准、2D图形变换、OCR纠正

数学表示对比

  • 3D刚体变换：\(Q = R \cdot P + T\)，其中\(R\)为3×3正交矩阵，\(T\)为3×1向量
  • 2D仿射变换：\(\begin{bmatrix} x' \\ y' \\ 1 \end{bmatrix} = \begin{bmatrix} a_{11} & a_{12} & t_x \\ a_{21} & a_{22} & t_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\ 1 \end{bmatrix}\)

为什么选择SVD

  • 数值稳定性：SVD算法在处理病态矩阵时更加稳定
  • 唯一解：对于给定点集，SVD能给出最优的刚体变换解
  • 全局最优：在最小二乘意义下得到最优的旋转和平移参数

思路

已知源坐标系下的3个点\(P_1,P_2,P_3\)（三维坐标，如\(Pi=(x_i,y_i,z_i)\)），以及目标坐标系下对应的3个点\(Q_1,Q_2,Q_3\)，两个坐标系的刚体变换可表示为 \(Q=R⋅P+T\)。其中：

  • R 是 3×3 的旋转矩阵（描述姿态变化）；
  • T 是 3×1 的平移向量（描述位置变化）；

计算核心是通过 中心化消除平移影响→ SVD求旋转矩阵 → 回代算平移矩阵 的三步逻辑。

具体计算步骤（4 步完成）

步骤 1：计算两点集的质心

通过计算源点集和目标点集的质心（均值点），消除平移对旋转计算的干扰。

  • 源点集 \(P= {P_1, P_2, P_3}\)（每个点为三维坐标 $P_i = (x_i, y_i, z_i)^T $），其质心为：

\[ \bar{P} = \frac{1}{3}(P_1 + P_2 + P_3) \]

  • 目标点集 $Q = {Q_1, Q_2, Q_3} $，其质心为：

\[ \bar{Q} = \frac{1}{3}(Q_1 + Q_2 + Q_3) \]

步骤 2：构造中心化点集与协方差矩阵

对每个点进行 “去质心” 处理，得到中心化点集，再构造描述两点集方向关联的协方差矩阵。

  1. 中心化点集计算：

  • 源点中心化：\(\tilde{P}_i = P_i - \bar{P}\)（共 3 个点，可表示为 3×3 矩阵 $ \tilde{P} $，每列对应一个中心化点）

  • 目标点中心化：$ \tilde{Q}_i = Q_i - \bar{Q} $（共 3 个点，可表示为 3×3 矩阵 $ \tilde{Q} $，每列对应一个中心化点）

  1. 协方差矩阵构造：

    协方差矩阵 $ H $ 关联源点集与目标点集的方向关系，公式为：

\[ H = \tilde{P} \cdot \tilde{Q}^T \]

（其中 $ \tilde{Q}^T $ 为 $ \tilde{Q} $ 的转置矩阵，$ H $ 为 3×3 矩阵）

步骤 3：通过 SVD 分解求旋转矩阵 $ R $

对协方差矩阵 $ H $ 进行奇异值分解（SVD），求解旋转矩阵，并修正可能的镜像变换。

  1. SVD 分解：

    对 $ H $ 做奇异值分解：$ H = U \cdot \Sigma \cdot V^T $，其中：

  • $U ， V $ 为 3×3 正交矩阵（列向量为单位正交向量）

  • $ \Sigma $ 为 3×3 对角矩阵（对角线元素为奇异值，非负）

  1. 旋转矩阵初步计算：

    初步旋转矩阵为：

\[ R_{init} = V \cdot U^T \]

  1. 修正镜像变换：

  • 刚体旋转矩阵需满足行列式为 1（$ \det(R) = 1 $）。

  • 若 $ \det(R_{init}) = -1 $（表示镜像变换，非刚体旋转），则：

  • 取 $ V $ 的最后一列乘以 -1，得到修正后的 $ V' $

  • 重新计算旋转矩阵：$R = V' \cdot U^T $，确保 $ \det(R) = 1 $

步骤 4：计算平移向量 T

  • 根据质心的变换关系，反推平移向量。平移向量 $ T $ 满足 目标质心 = 旋转后的源质心 + 平移，整理得：

\[ T = \bar{Q} - R \cdot \bar{P} \]

最终变换关系

  • 通过上述步骤得到旋转矩阵 $ R $ 和平移向量 $ T $ 后，两个坐标系的变换关系为：

\[ Q = R \cdot P + T \]

（其中 \(P\) 为源坐标系下的点，$ Q $ 为对应目标坐标系下的点）

编程

	// 输入有效性检查
	if (m_objp.size() < 3 || points3d.size() != m_objp.size()) {
		return false;  // 至少需要3对点才能确定刚体变换，且数量必须一致
	}

	// ==================== 第一步：计算两组点的质心 ====================
	// 质心是点集的几何中心，用于后续的去中心化操作
	cv::Point3f centroid_obj(0, 0, 0);  // 目标坐标系质心
	cv::Point3f centroid_cam(0, 0, 0);  // 源坐标系质心

	// 累加所有点的坐标
	for (size_t i = 0; i < m_objp.size(); ++i) {
		centroid_obj += m_objp[i];        // 标定板坐标系点累加
		cv::Point3f point3d_mm = points3d[i];  // 3D相机坐标系点
		centroid_cam += point3d_mm;       // 相机坐标系点累加
	}

	// 计算平均值，得到质心坐标
	centroid_obj /= (float)m_objp.size();  // 标定板质心
	centroid_cam /= (float)m_objp.size();  // 相机质心

	// ==================== 第二步：去中心化（去除平移影响，只保留旋转关系） ====================
	// 去中心化是为了消除平移分量的影响，使得后续计算只关注旋转关系
	std::vector<cv::Point3f> obj_centered, cam_centered;
	for (size_t i = 0; i < m_objp.size(); ++i) {
		obj_centered.push_back(m_objp[i] - centroid_obj);  // 标定板点去中心化
		cv::Point3f point3d_mm = points3d[i];             
		cam_centered.push_back(point3d_mm - centroid_cam); // 相机点去中心化
	}

	// ==================== 第三步：构造H矩阵（协方差矩阵） ====================
	// H矩阵描述了两个去中心化点集之间的协方差关系
	// H = Σ(Ai * Bi^T)，其中Ai是标定板点，Bi是相机点
	cv::Mat H = cv::Mat::zeros(3, 3, CV_32F);
	for (size_t i = 0; i < m_objp.size(); ++i) {
		// 外积累加：每个点的外积贡献给协方差矩阵
		H += cv::Mat(obj_centered[i]) * cv::Mat(cam_centered[i]).t();
	}

	// ==================== 第四步：SVD分解求旋转矩阵 ====================
	// 对协方差矩阵H进行奇异值分解：H = U * S * V^T
	// 最优旋转矩阵：R = V * U^T
	cv::Mat U, S, Vt;
	cv::SVD::compute(H, S, U, Vt);  // 执行SVD分解
	cv::Mat R_temp = Vt.t() * U.t(); // 计算旋转矩阵 

	// ==================== 第五步：确保旋转矩阵行列式为1（避免镜像变换） ====================
	// 有效的旋转矩阵行列式应该为+1，如果为-1则存在镜像变换
	if (cv::determinant(R_temp) < 0) {
		// 修正镜像问题：翻转V的最后一行并重新计算
		Vt.row(2) *= -1;
		R_temp = Vt.t() * U.t();
	}

	// ==================== 第六步：计算平移向量 ====================
	// 根据质心关系计算平移向量
	// 公式：标定板坐标 = R * 相机坐标 + t
	// 推导：t = 公式：标定板坐标 - R * 相机坐标
	cv::Mat rvec, tvec;
	rvec = R_temp;  // 旋转矩阵：相机→标定板变换
	tvec = cv::Mat(centroid_obj) - rvec * cv::Mat(centroid_cam);  // 平移向量计算

参考链接：

https://blog.csdn.net/qq_44144025/article/details/121465695

posted @ 2025-10-23 21:09  一楼二栋  阅读(12)  评论(0)    收藏  举报