コード例 #1
0
        /// <summary>
        ///
        /// </summary>
        /// <param name="m1"></param>
        /// <param name="m2"></param>
        /// <param name="model"></param>
        /// <param name="maxIters"></param>
        /// <returns></returns>
        public override unsafe bool Refine(CvMat m1, CvMat m2, CvMat model, int maxIters)
        {
            CvLevMarq     solver    = new CvLevMarq(8, 0, new CvTermCriteria(maxIters, double.Epsilon));
            int           count     = m1.Rows * m1.Cols;
            CvPoint2D64f *M         = (CvPoint2D64f *)m1.DataByte;
            CvPoint2D64f *m         = (CvPoint2D64f *)m2.DataByte;
            CvMat         modelPart = new CvMat(solver.Param.Rows, solver.Param.Cols, model.ElemType, model.Data);

            Cv.Copy(modelPart, solver.Param);

            for (; ;)
            {
                CvMat  _param = null;
                CvMat  _JtJ = null, _JtErr = null;
                double _errNorm = 0;

                if (!solver.UpdateAlt(out _param, out _JtJ, out _JtErr, out _errNorm))
                {
                    break;
                }

                for (int i = 0; i < count; i++)
                {
                    double * h = _param.DataDouble;
                    double   Mx = M[i].X, My = M[i].Y;
                    double   ww  = 1.0 / (h[6] * Mx + h[7] * My + 1.0);
                    double   _xi = (h[0] * Mx + h[1] * My + h[2]) * ww;
                    double   _yi = (h[3] * Mx + h[4] * My + h[5]) * ww;
                    double[] err = { _xi - m[i].X, _yi - m[i].Y };
                    if (_JtJ != null || _JtErr != null)
                    {
                        double[,] J = new double[2, 8]
                        {
                            { Mx *ww, My *ww, ww, 0, 0, 0, -Mx *ww *_xi, -My *ww *_xi },
                            { 0, 0, 0, Mx *ww, My *ww, ww, -Mx * ww * _yi, -My * ww * _yi }
                        };

                        for (int j = 0; j < 8; j++)
                        {
                            for (int k = j; k < 8; k++)
                            {
                                _JtJ.DataDouble[j * 8 + k] += J[0, j] * J[0, k] + J[1, j] * J[1, k];
                            }
                            _JtErr.DataDouble[j] += J[0, j] * err[0] + J[1, j] * err[1];
                        }
                    }
                    if (_errNorm != 0)
                    {
                        solver.ErrNorm += err[0] * err[0] + err[1] * err[1];
                    }
                }
            }

            Cv.Copy(solver.Param, modelPart);
            return(true);
        }
コード例 #2
0
        /// <summary>
        /// 
        /// </summary>
        /// <param name="m1"></param>
        /// <param name="m2"></param>
        /// <param name="model"></param>
        /// <param name="maxIters"></param>
        /// <returns></returns>
        public override unsafe bool Refine(CvMat m1, CvMat m2, CvMat model, int maxIters)
        {
            CvLevMarq solver = new CvLevMarq(8, 0, new CvTermCriteria(maxIters, double.Epsilon));
            int count = m1.Rows * m1.Cols;
            CvPoint2D64f* M = (CvPoint2D64f*)m1.DataByte;
            CvPoint2D64f* m = (CvPoint2D64f*)m2.DataByte;
            CvMat modelPart = new CvMat(solver.Param.Rows, solver.Param.Cols, model.ElemType, model.Data);
            Cv.Copy(modelPart, solver.Param);

            for (; ; )
            {
                CvMat _param = null;
                CvMat _JtJ = null, _JtErr = null;
                double _errNorm = 0;

                if (!solver.UpdateAlt(out _param, out _JtJ, out _JtErr, out _errNorm))
                    break;

                for (int i = 0; i < count; i++)
                {
                    double* h = _param.DataDouble;
                    double Mx = M[i].X, My = M[i].Y;
                    double ww = 1.0 / (h[6] * Mx + h[7] * My + 1.0);
                    double _xi = (h[0] * Mx + h[1] * My + h[2]) * ww;
                    double _yi = (h[3] * Mx + h[4] * My + h[5]) * ww;
                    double[] err = { _xi - m[i].X, _yi - m[i].Y };
                    if (_JtJ != null || _JtErr != null)
                    {
                        double[,] J = new double[2, 8]
                        {
                            { Mx*ww, My*ww, ww, 0, 0, 0, -Mx*ww*_xi, -My*ww*_xi },
                            { 0, 0, 0, Mx*ww, My*ww, ww, -Mx*ww*_yi, -My*ww*_yi }
                        };

                        for (int j = 0; j < 8; j++)
                        {
                            for (int k = j; k < 8; k++)
                                _JtJ.DataDouble[j * 8 + k] += J[0, j] * J[0, k] + J[1, j] * J[1, k];
                            _JtErr.DataDouble[j] += J[0, j] * err[0] + J[1, j] * err[1];
                        }
                    }
                    if (_errNorm != 0)
                        solver.ErrNorm += err[0] * err[0] + err[1] * err[1];
                }
            }

            Cv.Copy(solver.Param, modelPart);
            return true;
        }
コード例 #3
0
ファイル: Cv_F.cs プロジェクト: jorik041/opencvsharp
        /// <summary>
        /// 既知の内部パラメータを用いて,それぞれのビューにおける外部パラメータを推定する.
        /// 3次元のオブジェクトの点とそれに対応する2次元投影点が指定されなければならない.この関数も逆投影誤差の最小化を行う.
        /// </summary>
        /// <param name="objectPoints">オブジェクトの点の配列.3xNまたはNx3でNはビューにおける点の数.</param>
        /// <param name="imagePoints">対応する画像上の点の配列.2xNまたはNx2でNはビューにおける点の数.</param>
        /// <param name="cameraMatrix">カメラ内部行列 (A) [fx 0 cx; 0 fy cy; 0 0 1]. </param>
        /// <param name="distCoeffs">歪み係数のベクトル.4x1または1x4 [k1, k2, p1, p2].nullの場合,歪み係数はすべて0 であるとする.</param>
        /// <param name="rvec">出力される 3x1 の回転ベクトル</param>
        /// <param name="tvec">出力される 3x1 の並進ベクトル</param>
        /// <param name="useExtrinsicGuess"></param>
#else
        /// <summary>
        /// Finds extrinsic camera parameters for particular view
        /// </summary>
        /// <param name="objectPoints">The array of object points, 3xN or Nx3, where N is the number of points in the view. </param>
        /// <param name="imagePoints">The array of corresponding image points, 2xN or Nx2, where N is the number of points in the view. </param>
        /// <param name="cameraMatrix">The camera matrix (A) [fx 0 cx; 0 fy cy; 0 0 1]. </param>
        /// <param name="distCoeffs">The vector of distortion coefficients, 4x1 or 1x4 [k1, k2, p1, p2]. If it is NULL, all distortion coefficients are considered 0's. </param>
        /// <param name="rvec">The output 3x1 or 1x3 rotation vector (compact representation of a rotation matrix, see cvRodrigues2). </param>
        /// <param name="tvec">The output 3x1 or 1x3 translation vector. </param>
        /// <param name="useExtrinsicGuess"></param>
#endif
        public static void FindExtrinsicCameraParams2Cs(CvMat objectPoints, CvMat imagePoints, CvMat cameraMatrix, CvMat distCoeffs, CvMat rvec, CvMat tvec, bool useExtrinsicGuess)
        {

            if (objectPoints == null)
                throw new ArgumentNullException("objectPoints");
            if (imagePoints == null)
                throw new ArgumentNullException("imagePoints");
            if (cameraMatrix == null)
                throw new ArgumentNullException("cameraMatrix");
            if (rvec == null)
                throw new ArgumentNullException("rvec");
            if (tvec == null)
                throw new ArgumentNullException("tvec");
            //IntPtr distCoeffsPtr = ToPtr(distCoeffs);

            unsafe
            {
                const int maxIter = 20;

                double[] ar = new double[9] { 1, 0, 0, 0, 1, 0, 0, 0, 1 };

                double[] MM = new double[9],
                       U = new double[9],
                       V = new double[9],
                       W = new double[3];
                double* param = stackalloc double[6];

                CvMat matA = new CvMat(3, 3, MatrixType.F64C1);
                CvMat _Ar = new CvMat(3, 3, MatrixType.F64C1, ar);
                CvMat matR = new CvMat(3, 3, MatrixType.F64C1);
                CvMat _r = new CvMat(3, 1, MatrixType.F64C1, new IntPtr(param));
                CvMat _t = new CvMat(3, 1, MatrixType.F64C1, new IntPtr(param + 3));
                CvMat _Mc = new CvMat(1, 3, MatrixType.F64C1);
                CvMat _MM = new CvMat(3, 3, MatrixType.F64C1, MM);
                CvMat matU = new CvMat(3, 3, MatrixType.F64C1, U);
                CvMat matV = new CvMat(3, 3, MatrixType.F64C1, V);
                CvMat matW = new CvMat(3, 1, MatrixType.F64C1, W);
                CvMat _param = new CvMat(6, 1, MatrixType.F64C1, new IntPtr(param));

                CvMat _dpdr, _dpdt;


                if (!IS_MAT(objectPoints.CvPtr) ||
                    !IS_MAT(imagePoints.CvPtr) ||
                    !IS_MAT(cameraMatrix.CvPtr) ||
                    !IS_MAT(rvec.CvPtr) ||
                    !IS_MAT(tvec.CvPtr))
                {
                    throw new ArgumentException();
                }

                int count = Math.Max(objectPoints.Cols, objectPoints.Rows);
                CvMat matM = new CvMat(1, count, MatrixType.F64C3);
                CvMat _m = new CvMat(1, count, MatrixType.F64C2);

                ConvertPointsHomogeneous(objectPoints, matM);
                ConvertPointsHomogeneous(imagePoints, _m);
                Convert(cameraMatrix, matA);

                if (!((rvec.ElemType == MatrixType.F64C1 || rvec.ElemType == MatrixType.F32C1) &&
                    (rvec.Rows == 1 || rvec.Cols == 1) && rvec.Rows * rvec.Cols * rvec.ElemChannels == 3))
                {
                    throw new ArgumentException();
                }
                if (!((tvec.ElemType == MatrixType.F64C1 || tvec.ElemType == MatrixType.F32C1) &&
                    (tvec.Rows == 1 || tvec.Cols == 1) && tvec.Rows * tvec.Cols * tvec.ElemChannels == 3))
                {
                    throw new ArgumentException();
                }

                CvMat _mn = new CvMat(1, count, MatrixType.F64C2);
                CvMat _Mxy = new CvMat(1, count, MatrixType.F64C2);

                // normalize image points
                // (unapply the intrinsic matrix transformation and distortion)
                UndistortPoints_(_m, _mn, matA, distCoeffs, null, _Ar);

                if (useExtrinsicGuess)
                {
                    using (CvMat _r_temp = new CvMat(rvec.Rows, rvec.Cols, MatrixType.F64C1))
                    using (CvMat _t_temp = new CvMat(tvec.Rows, tvec.Cols, MatrixType.F64C1))
                    {
                        Convert(rvec, _r_temp);
                        Convert(tvec, _t_temp);
                        for (int i = 0; i < Math.Max(rvec.Rows, rvec.Cols); i++)
                        {
                            param[i] = _r_temp.GetReal1D(i);
                            param[i + 3] = _t_temp.GetReal1D(i);
                        }
                    }
                }
                else
                {
                    CvScalar Mc = Avg(matM);
                    _Mc[0] = Mc.Val0;
                    _Mc[1] = Mc.Val1;
                    _Mc[2] = Mc.Val2;
                    Reshape(matM, matM, 1, count);
                    MulTransposed(matM, _MM, true, _Mc);
                    SVD(_MM, matW, null, matV, SVDFlag.ModifyA | SVDFlag.V_T);

                    // initialize extrinsic parameters
                    if (W[2] / W[1] < 1e-3 || count < 4)
                    {
                        // a planar structure case (all M's lie in the same plane)
                        double[] h = new double[9];
                        CvMat R_transform = matV;
                        CvMat T_transform = new CvMat(3, 1, MatrixType.F64C1);
                        CvMat matH = new CvMat(3, 3, MatrixType.F64C1, h);
                        CvMat _h1, _h2, _h3;

                        if (V[2] * V[2] + V[5] * V[5] < 1e-10)
                            SetIdentity(R_transform);

                        if (Det(R_transform) < 0)
                            Scale(R_transform, R_transform, -1);

                        //GEMM(R_transform, _Mc, -1, null, 0, T_transform, GemmOperation.B_T);
                        for (int r = 0; r < 3; r++)
                        {                            
                            for (int c = 0; c < 1; c++)
                            {
                                double sum = 0;
                                for (int k = 0; k < 3; k++)
                                {
                                    sum += R_transform.GetReal2D(r, k) * _Mc.GetReal2D(c, k);
                                }
                                T_transform.SetReal2D(r, c, sum * -1);
                            }
                        }

                        for (int i = 0; i < count; i++)
                        {
                            double* Rp = R_transform.DataDouble;
                            double* Tp = T_transform.DataDouble;
                            double* src = matM.DataDouble + i * 3;
                            double* dst = _Mxy.DataDouble + i * 2;

                            dst[0] = Rp[0] * src[0] + Rp[1] * src[1] + Rp[2] * src[2] + Tp[0];
                            dst[1] = Rp[3] * src[0] + Rp[4] * src[1] + Rp[5] * src[2] + Tp[1];
                        }

                        FindHomography_(_Mxy, _mn, matH);

                        GetCol(matH, out _h1, 0);
                        GetCol(matH, out _h2, 0);
                        GetCol(matH, out _h3, 0);                        
                        _h2.DataDouble += 1;
                        _h3.DataDouble += 2;
                        double h1_norm = Math.Sqrt(h[0] * h[0] + h[3] * h[3] + h[6] * h[6]);
                        double h2_norm = Math.Sqrt(h[1] * h[1] + h[4] * h[4] + h[7] * h[7]);
                        Scale(_h1, _h1, 1.0 / h1_norm);
                        Scale(_h2, _h2, 1.0 / h2_norm);
                        Scale(_h3, _t, 2.0 / (h1_norm + h2_norm));                       
                        CrossProduct(_h1, _h2, _h3);

                        Rodrigues2_(matH, _r);
                        Rodrigues2_(_r, matH);
                        MatMulAdd(matH, T_transform, _t, _t);
                        MatMul(matH, R_transform, matR);
                        Rodrigues2_(matR, _r);
                    }
                    else
                    {
                        // non-planar structure. Use DLT method
                        double[] LL = new double[12 * 12],
                                 LW = new double[12],
                                 LV = new double[12 * 12];
                        CvMat _LL = new CvMat(12, 12, MatrixType.F64C1, LL);
                        CvMat _LW = new CvMat(12, 1, MatrixType.F64C1, LW);
                        CvMat _LV = new CvMat(12, 12, MatrixType.F64C1, LV);
                        CvMat _RR, _tt;
                        CvPoint3D64f* M = (CvPoint3D64f*)matM.DataDouble;
                        CvPoint2D64f* mn = (CvPoint2D64f*)_mn.DataDouble;

                        CvMat matL = new CvMat(2 * count, 12, MatrixType.F64C1);
                        double* L = matL.DataDouble;

                        for (int i = 0; i < count; i++, L += 24)
                        {
                            double x = -mn[i].X, y = -mn[i].Y;
                            L[0] = L[16] = M[i].X;
                            L[1] = L[17] = M[i].Y;
                            L[2] = L[18] = M[i].Z;
                            L[3] = L[19] = 1.0;
                            L[4] = L[5] = L[6] = L[7] = 0.0;
                            L[12] = L[13] = L[14] = L[15] = 0.0;
                            L[8] = x * M[i].X;
                            L[9] = x * M[i].Y;
                            L[10] = x * M[i].Z;
                            L[11] = x;
                            L[20] = y * M[i].X;
                            L[21] = y * M[i].Y;
                            L[22] = y * M[i].Z;
                            L[23] = y;
                        }

                        MulTransposed(matL, _LL, true);
                        SVD(_LL, _LW, null, _LV, SVDFlag.ModifyA | SVDFlag.V_T);
                        double[] LV12 = new double[12];
                        Array.Copy(LV, 11 * 12, LV12, 0, 12);
                        CvMat _RRt = new CvMat(3, 4, MatrixType.F64C1, LV12);
                        GetCols(_RRt, out _RR, 0, 3);
                        GetCol(_RRt, out _tt, 3);
                        if (Det(_RR) < 0)
                            Scale(_RRt, _RRt, -1);
                        double sc = Norm(_RR);
                        SVD(_RR, matW, matU, matV, SVDFlag.ModifyA | SVDFlag.U_T | SVDFlag.V_T);
                        GEMM(matU, matV, 1, null, 0, matR, GemmOperation.A_T);
                        Scale(_tt, _t, Norm(matR) / sc);
                        Rodrigues2_(matR, _r);
                    }
                }

                Cv.Reshape(matM, matM, 3, 1);
                Cv.Reshape(_mn, _mn, 2, 1);

                // refine extrinsic parameters using iterative algorithm
                CvLevMarq solver = new CvLevMarq(6, count * 2, new CvTermCriteria(maxIter, float.Epsilon), true);
                Copy(_param, solver.Param);

                /*
                Console.WriteLine("matM-----");
                for (int i = 0; i < matM.Rows * matM.Cols; i++)
                {
                    Console.WriteLine("{0}\t", matM[i].Val0);
                }
                Console.WriteLine("_mn-----");
                for (int i = 0; i < _mn.Rows * _mn.Cols; i++)
                {
                    Console.WriteLine(_mn[i].Val0);
                }
                Console.WriteLine("_param-----");
                for (int i = 0; i < _param.Rows * _param.Cols; i++)
                {
                    Console.WriteLine(_param[i].Val0);
                }*/

                for (; ; )
                {
                    CvMat matJ, _err, __param;
                    bool proceed = solver.Update(out __param, out matJ, out _err);
                    Copy(__param, _param);
                    if (!proceed || _err == null)
                        break;
                    Reshape(_err, _err, 2, 1);
                    if (matJ != null)
                    {
                        GetCols(matJ, out _dpdr, 0, 3);
                        GetCols(matJ, out _dpdt, 3, 6);
                        ProjectPoints2(matM, _r, _t, matA, distCoeffs,
                                          _err, _dpdr, _dpdt, null, null, null);
                    }
                    else
                    {
                        ProjectPoints2(matM, _r, _t, matA, distCoeffs, _err, null, null, null, null, null);
                    }
                    Sub(_err, _m, _err);
                    Reshape(_err, _err, 1, 2 * count);
                }
                Copy(solver.Param, _param);

                for (int i = 0; i < 3; i++)
                {
                    rvec.SetReal1D(i, param[i]);
                    tvec.SetReal1D(i, param[i + 3]);
                }
            }
            
        }