/// <summary>
/// 立体校正评价函数
/// </summary>
private void Stereo_Calib_Quanlity_Check()
{
int board_n = patternSize.Width * patternSize.Height;
float a = 0, b = 0, c = 0;
float x = 0, y = 0;
double err = 0;
double allerr = 0;
VectorOfPoint3D32F leftLines = new VectorOfPoint3D32F();
VectorOfPoint3D32F rightLines = new VectorOfPoint3D32F();
//存储校正映射后点集
VectorOfVectorOfPointF leftpoints_rectify = new VectorOfVectorOfPointF();
VectorOfVectorOfPointF rightpoints_rectify = new VectorOfVectorOfPointF();
VectorOfPointF tempPoints = new VectorOfPointF();
VectorOfPointF rimgPoints_temp = new VectorOfPointF();
VectorOfPointF limgPoints_temp = new VectorOfPointF();
for (int i = 0; i < left_corners_set.Size; i++)
{
//稀疏点校正，参考Learning OpenCV3
CvInvoke.UndistortPoints(left_corners_set[i], tempPoints, leftCamMatrix, leftDistCoeffs, null, leftCamMatrix);
leftpoints_rectify.Push(tempPoints);
}
for (int i = 0; i < right_corners_set.Size; i++)
{
//稀疏点校正，参考Learning OpenCV3
CvInvoke.UndistortPoints(right_corners_set[i], tempPoints, rightCamMatrix, rightDistCoeffs, null, rightCamMatrix);
rightpoints_rectify.Push(tempPoints);
}
//计算左相机图像点与极线的距离
for (int i = 0; i < left_corners_set.Size; i++)
{
rimgPoints_temp = rightpoints_rectify[i];
limgPoints_temp = leftpoints_rectify[i];
CvInvoke.ComputeCorrespondEpilines(rimgPoints_temp, 2, F, leftLines); //计算极线
for (int j = 0; j < board_n; j++)
{
//读取存储的数据
a = leftLines[j].X;
b = leftLines[j].Y;
c = leftLines[j].Z;
x = limgPoints_temp[j].X;
y = limgPoints_temp[j].Y;
//计算每个点到直线的距离 (点到直线距离公式)
err = Math.Abs(a * x + b * y + c) / Math.Sqrt(a * a + b * b);
allerr += err;
}
}
//计算右相机图像点与极线的距离
for (int i = 0; i < right_corners_set.Size; i++)
{
rimgPoints_temp = rightpoints_rectify[i];
limgPoints_temp = leftpoints_rectify[i];
CvInvoke.ComputeCorrespondEpilines(limgPoints_temp, 1, F, rightLines); //计算极线
for (int j = 0; j < board_n; j++)
{
//读取存储的数据
a = rightLines[j].X;
b = rightLines[j].Y;
c = rightLines[j].Z;
x = rimgPoints_temp[j].X;
y = rimgPoints_temp[j].Y;
//计算每个点到直线的距离 (点到直线距离公式)
err = Math.Abs(a * x + b * y + c) / Math.Sqrt(a * a + b * b);
allerr += err;
}
}
dist_err = allerr / (2 * (left_corners_set.Size + right_corners_set.Size) * board_n);
}