Ejemplo n.º 1
0
        /// <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);
        }