相机位姿比较
单个相机旋转矩阵计算运动范围是否太大
double normofTransform(cv::Mat rvec, cv::Mat tvec)
{
return fabs(min(cv::norm(rvec), 2*M_PI-cv::norm(rvec)))+fabs(cv::norm(tvec));
}
计算旋转向量和平移向量的范数的函数：
其中，旋转向量用rvec表示，平移向量用tvec表示。
具体的，用cv::norm函数来计算rvec和tvec的范数，而fabs函数则用于取范数的绝对值。
在计算rvec的范数时，用到了min函数来比较rvec的范数和\(2 \pi\)减去rvec的范数，然后返回二者中的最小值。
这是因为旋转向量在数学上可以表示为一个角度和一个方向，而由于旋转向量的方向无法确定，因此只有在角度小于\(\pi\)时才是唯一的，而当角度大于\(\pi\)时，可以通过旋转向量反向来得到相同的旋转效果。因此，这里需要将大于\(\pi\)的角度对应的旋转向量转换为小于\(\pi\)的角度对应的旋转向量。
最后，将rvec的范数和tvec的范数的绝对值相加，得到最终结果。
\[\left \| \begin{bmatrix}
R & t\\
0 & 1
\end{bmatrix} \right \| = \sqrt{
\left \|
\begin{bmatrix}
R & t \\
0 & 1
\end{bmatrix}
\begin{bmatrix}
R^T & -R^Tt\\
0 & 1
\end{bmatrix}
\right \|
}
\]
相机位姿的范数可以用于评估相机的位姿精度，通常用于相机姿态估计，相机定位等计算机视觉任务中。
在实际应用中，相机位姿的精度越高，通常对应着更高的任务准确度和鲁棒性，因此，通过计算相机位姿的范数可以对相机姿态估计算法的优劣进行评估和比较。
对两个位姿进行比较
- 计算两个位姿之间的欧式距离：可以将两个位姿分别转换未欧拉角或四元数表示，然后计算它们之间的欧式距离。欧式距离越小，表示两个位姿越接近。
- 计算两个位姿之间的角度差：可以将两个位姿分别转换为欧拉角或四元数表示，然后计算它们之间的角度差。角度差越小，表示两个位姿越接近。
- 计算两个位姿之间的相对误差：可以将两个位姿分别转换为变换矩阵表示，然后计算它们之间的相对误差。相对误差越小，表示两个位姿越接近。
- 计算两个位姿之间的欧式距离公式：
\[d = \sqrt{(x_1 - x_2)^2 + (y_1 - y_2)^2 + (z_1 - z_2)^2 }
\]
其中，\((x_1,y_1,z_1)和(x_2,y_2,z_2)\)分别表示两个位姿的欧拉角或四元数表示。
C++实现，点击查看代码
double poseDistance(const cv::Mat& pose1, const cv::Mat& pose2)
{
cv::Mat rvec1, tvec1, rvec2, tvec2;
cv::Mat R1, R2;
cv::Rodrigues(pose1(cv::Range(0, 3), cv::Range(0, 3)), R1);
cv::Rodrigues(pose2(cv::Range(0, 3), cv::Range(0, 3)), R2);
cv::Rodrigues(R1.t() * R2, rvec1);
cv::Rodrigues(pose1(cv::Range(0, 3), cv::Range(3, 4)) - R1.t() * R2 * pose2(cv::Range(0, 3), cv::Range(3, 4)), tvec1);
return cv::norm(tvec1);
}
- 计算两个位姿之间的角度差公式：
\[\bigtriangleup \theta = cos^{-1}(q_1^{-1}q_2)
\]
其中，\(q_1\)和\(q_2\)分别表示两个位姿的四元数表示。
C++实现，点击查看代码
double poseAngleDiff(const cv::Mat& pose1, const cv::Mat& pose2)
{
cv::Mat q1, q2;
cv::Rodrigues(pose1(cv::Range(0, 3), cv::Range(0, 3)), q1);
cv::Rodrigues(pose2(cv::Range(0, 3), cv::Range(0, 3)), q2);
double dot_prod = q1.dot(q2);
double norm_prod = cv::norm(q1) * cv::norm(q2);
return acos(dot_prod / norm_prod);
}
- 计算两个位姿之间的相对误差公式：
\[\epsilon = \left \|
\begin{bmatrix}
R_1 & t_1\\
0 & 1
\end{bmatrix}^{-1}
\begin{bmatrix}
R_2 & t_2 \\
0 & 1
\end{bmatrix}
-
\begin{bmatrix}
I & 0 \\
0 & 1
\end{bmatrix}
\right \|
\]
其中，\(R_1\)和\(t_1\)分别表示第一个位姿的旋转矩阵和平移向量，
\(R_2\)和\(t_2\)分别表示第一个位姿的旋转矩阵和平移向量。
C++实现，点击查看代码
double poseRelativeError(const cv::Mat& pose1, const cv::Mat& pose2)
{
cv::Mat T1 = cv::Mat::eye(4, 4, CV_64F);
cv::Mat T2 = cv::Mat::eye(4, 4, CV_64F);
pose1.copyTo(T1(cv::Range(0, 3), cv::Range(0, 4)));
pose2.copyTo(T2(cv::Range(0, 3), cv::Range(0, 4)));
cv::Mat T12 = T1.inv() * T2;
double error = cv::norm(T12 - cv::Mat::eye(4, 4, CV_64F));
return error;
}
