public void testCvSolve1() { Vector3D[] planeVecs = new Vector3D[3]; planeVecs[0] = new Vector3D(.0, .0, .0); planeVecs[1] = new Vector3D(.0, 1.0, .0); planeVecs[2] = new Vector3D(1.0, .0, .0); Matrix<float> A = new Matrix<float>(4, 3); A.SetZero(); Matrix<float> x = new Matrix<float>(3, 1); x.SetZero(); Matrix<float> b = new Matrix<float>(4, 1); //b.SetValue(1.0); b.SetZero(); for (int i = 0; i < planeVecs.Length; ++i) { Vector3D v = planeVecs[i]; A[0, 0] += (float)(v.X * v.X); A[0, 1] += (float)(v.X * v.Y); A[0, 2] += (float)(v.X * v.Z); A[1, 1] += (float)(v.Y * v.Y); A[1, 2] += (float)(v.Y * v.Z); A[2, 2] += (float)(v.Z * v.Z); b[0, 0] += (float)v.X; b[1, 0] += (float)v.Y; b[2, 0] += (float)v.Z; b[3, 0] += (float)1.0; } A[1, 0] = A[0, 1]; A[2, 0] = A[0, 2]; A[2, 1] = A[1, 2]; A[3, 0] = b[0, 0]; A[3, 1] = b[1, 0]; A[3, 2] = b[2, 0]; Emgu.CV.CvInvoke.cvSolve(A, b, x, Emgu.CV.CvEnum.SOLVE_METHOD.CV_SVD); Vector3D bestNormal = new Vector3D(); bestNormal.X = x[0, 0]; bestNormal.Y = x[1, 0]; bestNormal.Z = x[2, 0]; }
private bool isInitialized; // true if any data has been fed public KalmanFilter(int variables) { variablesCount = variables; int measurementVariables = variables; int dynamicVariables = variables * 2; float[] state = new float[dynamicVariables]; for (int i = 0; i < dynamicVariables; ++i) state[i] = 0.0f; Matrix<float> transitionMatrix = new Matrix<float>(dynamicVariables, dynamicVariables); transitionMatrix.SetZero(); for (int i = 0; i < dynamicVariables; ++i) { transitionMatrix[i, i] = 1.0f; if (i >= measurementVariables) transitionMatrix[i - measurementVariables, i] = 1; } Matrix<float> measurementMatrix = new Matrix<float>(measurementVariables, dynamicVariables); measurementMatrix.SetZero(); for (int i = 0; i < measurementVariables; ++i) measurementMatrix[i, i] = 1.0f; Matrix<float> processNoise = new Matrix<float>(dynamicVariables, dynamicVariables); processNoise.SetIdentity(new MCvScalar(1));//1.0e-4)); Matrix<float> measurementNoise = new Matrix<float>(measurementVariables, measurementVariables); measurementNoise.SetIdentity(new MCvScalar(4));//1.0e-1)); Matrix<float> errorCovariancePost = new Matrix<float>(dynamicVariables, dynamicVariables); errorCovariancePost.SetIdentity(); kalman = new Kalman(dynamicVariables, measurementVariables, 0); kalman.CorrectedState = new Matrix<float>(state); kalman.TransitionMatrix = transitionMatrix; kalman.MeasurementNoiseCovariance = measurementNoise; kalman.ProcessNoiseCovariance = processNoise; kalman.ErrorCovariancePost = errorCovariancePost; kalman.MeasurementMatrix = measurementMatrix; }
// this is from the blog entry public void Recompute() { if (source == null || dest == null || source.Count != dest.Count) throw new Exception("Input data null or not equal in length"); // compute covariance matrix Matrix<double> H = new Matrix<double>(2, 2); H.SetZero(); for (int i = 0; i < source.Count; i++) { var a = source[i].ToMatrix() - SourceCentroid; var b = dest[i].ToMatrix() - DestCentroid; H += a * b.Transpose(); } /* perform svd where A = U W VT * A IntPtr Source MxN matrix * W IntPtr Resulting singular value matrix (MxN or NxN) or vector (Nx1). * U IntPtr Optional left orthogonal matrix (MxM or MxN). If CV_SVD_U_T is specified, the number of rows and columns in the sentence above should be swapped * V IntPtr Optional right orthogonal matrix (NxN) */ Matrix<double> U = new Matrix<double>(2, 2); Matrix<double> W = new Matrix<double>(2, 2); Matrix<double> V = new Matrix<double>(2, 2); CvInvoke.cvSVD(H.Ptr, W.Ptr, U.Ptr, V.Ptr, SVD_TYPE.CV_SVD_DEFAULT); // compute rotational matrix R=V*UT Rotation = V * U.Transpose(); // find translation //Offset = DestCentroid - ( Rotation * SourceCentroid); Offset = DestCentroid - SourceCentroid; if (Angle > 5) { Global.Instance.mainForm.ShowSimpleMessageBox("Excessive Angle Detected - Problem detecting rotation\nOffset = " + new PartLocation(Offset-DestCentroid) + "\nAngle=" + Angle); } }
private void button7_Click(object sender, EventArgs e) { Matrix<double> K = new Matrix<double>(3, 3); K.SetZero(); Matrix<double> D = new Matrix<double>(8, 1); D.SetZero(); if (calibrator != null) { int i, j; for (i = 0; i < 3; i++) for (j = 0; j < 3; j++) K.Data[i, j] = calibrator.Intrinsics.IntrinsicMatrix.Data[i, j]; for (i = 0; i < 4; i++) D.Data[i, 0] = calibrator.Intrinsics.DistortionCoeffs.Data[i, 0]; calibrator.stopCalibration(); // saving calibration parameters StreamWriter sw = new StreamWriter("CalibParams.txt"); string intrinsicsStr = Convert.ToString(K.Data[0, 0]) + ","; intrinsicsStr += Convert.ToString(K.Data[0, 1]) + ","; intrinsicsStr += Convert.ToString(K.Data[0, 2]) + ","; intrinsicsStr += Convert.ToString(K.Data[1, 0]) + ","; intrinsicsStr += Convert.ToString(K.Data[1, 1]) + ","; intrinsicsStr += Convert.ToString(K.Data[1, 2]) + ","; intrinsicsStr += Convert.ToString(K.Data[2, 0]) + ","; intrinsicsStr += Convert.ToString(K.Data[2, 1]) + ","; intrinsicsStr += Convert.ToString(K.Data[2, 2]); sw.WriteLine(intrinsicsStr); string distcoeffsStr = Convert.ToString(D.Data[0, 0]) + "," + Convert.ToString(D.Data[1, 0]) + "," + Convert.ToString(D.Data[2, 0]) + "," + Convert.ToString(D.Data[3, 0]); sw.WriteLine(distcoeffsStr); sw.Close(); LatestCamIntrinsics.IntrinsicMatrix.Data[0, 0] = K.Data[0, 0]; LatestCamIntrinsics.IntrinsicMatrix.Data[0, 2] = K.Data[0, 2]; LatestCamIntrinsics.IntrinsicMatrix.Data[1, 1] = K.Data[1, 1]; LatestCamIntrinsics.IntrinsicMatrix.Data[1, 2] = K.Data[1, 2]; LatestCamIntrinsics.IntrinsicMatrix.Data[2, 2] = 1; LatestCamIntrinsics.DistortionCoeffs.Data[0, 0] = D.Data[0, 0]; LatestCamIntrinsics.DistortionCoeffs.Data[1, 0] = D.Data[1, 0]; LatestCamIntrinsics.DistortionCoeffs.Data[2, 0] = D.Data[2, 0]; LatestCamIntrinsics.DistortionCoeffs.Data[3, 0] = D.Data[3, 0]; } }
public void testSVD() { Vector3D[] planeVecs = new Vector3D[3]; planeVecs[0] = new Vector3D(.0, .0, 2.0); planeVecs[1] = new Vector3D(.0, 2.0, .0); planeVecs[2] = new Vector3D(2.0, .0, .0); Matrix<float> A = new Matrix<float>(4, 4); A.SetZero(); Matrix<float> x = new Matrix<float>(4, 1); x.SetZero(); Matrix<float> U = new Matrix<float>(4, 4); //b.SetValue(1.0); U.SetZero(); Matrix<float> V = new Matrix<float>(4, 4); //b.SetValue(1.0); U.SetZero(); for (int i = 0; i < planeVecs.Length; ++i) { Vector3D v = planeVecs[i]; A[0, 0] += (float)(v.X * v.X); A[0, 1] += (float)(v.X * v.Y); A[0, 2] += (float)(v.X * v.Z); A[1, 1] += (float)(v.Y * v.Y); A[1, 2] += (float)(v.Y * v.Z); A[2, 2] += (float)(v.Z * v.Z); A[0, 3] -= (float)(v.X); A[1, 3] -= (float)(v.Y); A[2, 3] -= (float)(v.Z); A[3, 3] -= (float)1.0; } A[1, 0] = A[0, 1]; A[2, 0] = A[0, 2]; A[2, 1] = A[1, 2]; A[3, 0] = -A[0, 3]; A[3, 1] = -A[1, 3]; A[3, 2] = -A[2, 3]; CvInvoke.cvSVD(A, x, U, V, Emgu.CV.CvEnum.SVD_TYPE.CV_SVD_DEFAULT ); }
public void testEigen() { Vector3D[] planeVecs = new Vector3D[3]; planeVecs[0] = new Vector3D(.0, .0, 1.0); planeVecs[1] = new Vector3D(.0, 1.0, .0); planeVecs[2] = new Vector3D(.0, .0, .0); Matrix<float> A = new Matrix<float>(4, 4); A.SetZero(); Matrix<float> x = new Matrix<float>(4, 1); x.SetZero(); Matrix<float> ev = new Matrix<float>(4, 4); //b.SetValue(1.0); ev.SetZero(); for (int i = 0; i < planeVecs.Length; ++i) { Vector3D v = planeVecs[i]; A[0, 0] += (float)(v.X * v.X); A[0, 1] += (float)(v.X * v.Y); A[0, 2] += (float)(v.X * v.Z); A[1, 1] += (float)(v.Y * v.Y); A[1, 2] += (float)(v.Y * v.Z); A[2, 2] += (float)(v.Z * v.Z); A[0, 3] -= (float)(v.X); A[1, 3] -= (float)(v.Y); A[2, 3] -= (float)(v.Z); A[3, 3] -= (float)1.0; } A[1, 0] = A[0, 1]; A[2, 0] = A[0, 2]; A[2, 1] = A[1, 2]; A[3, 0] = - A[0, 3]; A[3, 1] = - A[1, 3]; A[3, 2] = - A[2, 3]; CvInvoke.cvEigenVV(A, ev, x, 1e-4, -1, -1); }