public Vector3 update(Vector3 point) { generateSigmaPoints(); unscentedTransformation(syntheticData.transitionMatrix, sigmaPoints, L, syntheticData.processNoise); var x1 = trans_mean_mat; var x_capital_1 = trans_sigmaPoints; var P1 = trans_stateCovariance; var x_capital_2 = trans_deviation; unscentedTransformation(syntheticData.measurementMatrix, x_capital_1, m, syntheticData.measurementNoise); //updating trans_cross_covariance = x_capital_2 * covarianceWeightsDiagonal * trans_deviation.Transpose(); // inverse of P2 (trans_covariance) Emgu.CV.Matrix <double> inv_trans_covariance = new Matrix <double>(trans_stateCovariance.Rows, trans_stateCovariance.Cols); CvInvoke.cvInvert(trans_stateCovariance, inv_trans_covariance, SOLVE_METHOD.CV_SVD_SYM); KalmanGain = trans_cross_covariance * inv_trans_covariance; Emgu.CV.Matrix <double> thisMeasurement = new Matrix <double>(m, 1); thisMeasurement[0, 0] = point.x; thisMeasurement[1, 0] = point.y; thisMeasurement[2, 0] = point.z; //update state state = x1 + KalmanGain * (thisMeasurement - trans_mean_mat); //update covariance stateCovariance = P1 - KalmanGain * trans_cross_covariance.Transpose(); return(new Vector3((float)state[0, 0], (float)state[1, 0], (float)state[2, 0])); }
private void unscentedTransformation(Emgu.CV.Matrix <double> map, Emgu.CV.Matrix <double> points, int outputs, Emgu.CV.Matrix <double> additiveCovariance) { int sigma_point_number = points.Cols; // try points.cols better trans_mean_mat = new Matrix <double>(outputs, 1); trans_sigmaPoints = new Matrix <double>(outputs, sigma_point_number); for (int i = 0; i < sigma_point_number; i++) { Emgu.CV.Matrix <double> transformed_point = map * points.GetCol(i); trans_mean_mat += meansWeights[0, i] * transformed_point; // store transformed point for (int j = 0; j < outputs; j++) { trans_sigmaPoints[j, i] = transformed_point[j, 0]; } } Emgu.CV.Matrix <double> intermediate_matrix_1 = new Matrix <double>(trans_mean_mat.Rows, sigma_point_number); for (int i = 0; i < sigma_point_number; i++) { for (int j = 0; j < trans_mean_mat.Rows; j++) { intermediate_matrix_1[j, i] = trans_mean_mat[j, 0]; } } trans_deviation = trans_sigmaPoints - intermediate_matrix_1; // Y1=Y-y(:,ones(1,L)); trans_stateCovariance = trans_deviation * covarianceWeightsDiagonal * trans_deviation.Transpose() + additiveCovariance; }
private Emgu.CV.Matrix <double> chol(Emgu.CV.Matrix <double> input) { double[] source = new double[input.Rows * input.Cols]; int i = 0; for (int k = 0; k < input.Rows; k++) { for (int l = 0; l < input.Rows; l++) { source[i] = input[k, l]; i++; } } double[] destination = new double[input.Rows * input.Cols]; Cholesky(source, destination, input.Rows); Emgu.CV.Matrix <double> output = new Matrix <double>(input.Rows, input.Cols); i = 0; for (int k = 0; k < input.Rows; k++) { for (int l = 0; l < input.Rows; l++) { output[k, l] = (double)destination[i]; i++; } } return(output); }
// ukf trial from matlab and c++ examples // in progress public MyUnscentedKalman(int states, int measurements) { this.L = states; this.m = measurements; state = new Matrix <double>(this.L, 1); state[0, 0] = 1; state[1, 0] = 1; state[2, 0] = 1; state[3, 0] = 0.5; state[4, 0] = 0.5; state[5, 0] = 0.5; state[5, 0] = 0.1; sigmaPoints = new Matrix <double>(this.L, 2 * this.L + 1); stateCovariance = new Matrix <double>(L, L); stateCovariance.SetIdentity(new MCvScalar(1.0)); meansWeights = new Matrix <double>(1, 2 * this.L + 1); covarianceWeights = new Matrix <double>(1, 2 * this.L + 1); covarianceWeightsDiagonal = new Matrix <double>(2 * this.L + 1, 2 * this.L + 1); calculateVariables(); syntheticData = new VectorData(); }
public MCvPoint3D32f Kalman4Joints(float x, float y, float z, Kalman kalman, SyntheticData syntheticData) { //Kalman headPC1 Emgu.CV.Matrix <float> MatrixGet = new Emgu.CV.Matrix <float>(6, 1); MatrixGet[0, 0] = x; MatrixGet[1, 0] = y; MatrixGet[2, 0] = z; kalman = new Kalman(6, 3, 0); Emgu.CV.Matrix <float> state = MatrixGet; kalman.CorrectedState = state; kalman.TransitionMatrix = syntheticData.transitionMatrix; kalman.MeasurementNoiseCovariance = syntheticData.measurementNoise; kalman.ProcessNoiseCovariance = syntheticData.processNoise; kalman.ErrorCovariancePost = syntheticData.errorCovariancePost; kalman.MeasurementMatrix = syntheticData.measurementMatrix; Matrix <float> prediction = new Matrix <float>(3, 1); prediction = kalman.Predict(); MCvPoint3D32f predictPointheadPC1 = new MCvPoint3D32f(prediction[0, 0], prediction[1, 0], prediction[2, 0]); MCvPoint3D32f measurePointheadPC1 = new MCvPoint3D32f(syntheticData.GetMeasurement()[0, 0], syntheticData.GetMeasurement()[1, 0], syntheticData.GetMeasurement()[2, 0]); Matrix <float> estimated = kalman.Correct(syntheticData.GetMeasurement()); MCvPoint3D32f estimatedPoint = new MCvPoint3D32f(estimated[0, 0], estimated[1, 0], estimated[2, 0]); syntheticData.GoToNextState(); return(estimatedPoint); }
public static void Test() { //Matrix<float> md = new Matrix<float>(10, 3); //md[0,0] = 1; double[,] d = new double[10, 3] { { 1, 2, 3 }, { 3, 2, 1 }, { 4, 5, 6 }, { 2, 3, 4 }, { 4, 3, 2 }, { 6, 8, 9 }, { 2, 4, 6 }, { 1, 9, 5 }, { 1, 3, 9 }, { 9, 8, 7 } }; int[] idx = new int[3] { 1, 5, 9 }; double[,] arr_ys = new double[3, 2] { { 4, 9 }, { 1, 1 }, { 9, 6 } }; Emgu.CV.Matrix <double> X = new Emgu.CV.Matrix <double>(d); Emgu.CV.Matrix <double> Ys = new Emgu.CV.Matrix <double>(arr_ys); Console.WriteLine("TESTANDO"); Emgu.CV.Matrix <double> Y = AlgLAMP.LAMP(X, idx, Ys); }
private void calculateVariables() { lambda = Math.Pow(alpha, 2.0) * (L + ki) - L; c = L + lambda; // means weights meansWeights[0, 0] = (double)(lambda / c); for (int i = 1; i < 2 * L + 1; i++) { meansWeights[0, i] = (double)(0.5f / c); } // cov weights covarianceWeights = meansWeights.Clone(); covarianceWeights[0, 0] += (double)(1 - Math.Pow(alpha, 2.0) + beta); // diag of wc for (int i = 0; i < covarianceWeights.Cols; i++) { covarianceWeightsDiagonal[i, i] = covarianceWeights[0, i]; } c = Math.Sqrt(c); }
public static Emgu.CV.Matrix <int> HotMatrix(Emgu.CV.Matrix <int> y_label, int ClassCount) { var HotVector = new Emgu.CV.Matrix <int>(y_label.Rows, ClassCount); HotVector.SetZero(); for (int row = 0; row < y_label.Rows; row++) { HotVector[row, y_label[row, 0]] = 1; } return(HotVector); }
public static Emgu.CV.Matrix <int> Matrix2OneHotEncoding(Emgu.CV.Matrix <int> matrix, int ClassCount) { Emgu.CV.Matrix <int> HotVector = new Emgu.CV.Matrix <int>(matrix.Rows, ClassCount); HotVector.SetZero(); for (int row = 0; row < matrix.Height; row++) { var col = (int)(matrix[row, 0]); HotVector[row, col] = 1; } return(HotVector); }
public static int GetClassCount(Emgu.CV.Matrix <int> y_label) { try { var labels = new int[y_label.Rows]; y_label.Mat.CopyTo(labels); return(labels.Distinct().Count()); } catch (Exception ex) { throw new Exception(ex.Message); } }
public static Emgu.CV.Matrix <float> OneHotEncoding2Matrix(Emgu.CV.Matrix <float> matrix) { Emgu.CV.Matrix <float> DecodeMatrix = new Emgu.CV.Matrix <float>(matrix.Rows, 1); //DecodeMatrix.SetZero(); //var data = matrix.Data; //for (int i = 0; i < data.Rows; i++) //{ // var row = data.GetRow(i); // var maxIndex = row.IndexOf(row.Max()); // DecodeMatrix[i, 0] = maxIndex; //} return(DecodeMatrix); }
public VectorData() { // is linear and probably a non linear transition cannot be translated to a transition matrix transitionMatrix = new Emgu.CV.Matrix <double>(new double[, ] // n * n matrix A, that relates the state at k-1 step to state k step. In practice, it can change at each time step { // {1, 0, 0, 1f, 0, 0, 0.5f}, // x-pos, y-pos, z-pos, velocities acceleration expected combination // {0, 1, 0, 0, 1f, 0, 0.5f}, // {0, 0, 1, 0, 0, 1f, 0.5f}, // {0, 0, 0, 1, 0, 0, 1f}, // {0, 0, 0, 0, 1, 0, 1f}, // {0, 0, 0, 0, 0, 1, 1f}, // {0, 0, 0, 0, 0, 0, 1}, // {1, 0, 0, 1f, 0, 0, 0.5f}, // x-pos, y-pos, z-pos, velocities acceleration expected combination // {0, 1, 0, 0, 1f, 0, 0.5f}, // {0, 0, 1, 0, 0, 1f, 0.5f}, // {0, 0, 0, 1, 0, 0, 1}, // {0, 0, 0, 0, 1, 0, 1}, // {0, 0, 0, 0, 0, 1, 1}, // {0, 0, 0, 0, 0, 0, 1}, { 1, 0, 0, 1f, 0, 0 }, // x-pos, y-pos, z-pos, velocities (no accel) { 0, 1, 0, 0, 1f, 0 }, { 0, 0, 1, 0, 0, 1f }, { 0, 0, 0, 1, 0, 0 }, { 0, 0, 0, 0, 1, 0 }, { 0, 0, 0, 0, 0, 1 }, }); measurementMatrix = new Emgu.CV.Matrix <double>(new double[, ] // m * n matrix H. follows the same rules as transition matrix A { // { 1, 0, 0, 0, 0, 0, 0}, // { 0, 1, 0, 0, 0, 0, 0}, // { 0, 0, 1, 0, 0, 0, 0}, { 1, 0, 0, 0, 0, 0 }, { 0, 1, 0, 0, 0, 0 }, { 0, 0, 1, 0, 0, 0 }, }); measurementMatrix.SetIdentity(); processNoise = new Emgu.CV.Matrix <double>(6, 6); //Linked to the size of the transition matrix /* Q matrix */ processNoise.SetIdentity(new MCvScalar(1.0e-2)); //The smaller the value the more resistance to noise (default e-4) measurementNoise = new Emgu.CV.Matrix <double>(3, 3); //Fixed according to input data /* R matrix */ measurementNoise.SetIdentity(new MCvScalar(1.0e-2)); errorCovariancePost = new Emgu.CV.Matrix <double>(6, 6); //Linked to the size of the transition matrix errorCovariancePost.SetIdentity(); invMeasurementNoise = new Emgu.CV.Matrix <double>(3, 3); }
public static int[] Matrix2Array(Emgu.CV.Matrix <float> matrix) { int[] output = new int[matrix.Rows * matrix.Cols]; int index = 0; for (int row = 0; row < matrix.Rows; row++) { for (int col = 0; col < matrix.Cols; col++) { output[index++] = (int)matrix[row, col]; } } return(output); }
private void calibrateHomographyMaths(PointF[] points_kinect, int w, int h) { Debug.Log("Calbirating homography..."); var points_sorted = new PointF[4]; points_sorted[0] = get_closest(points_kinect, new PointF(0, 0)); points_sorted[1] = get_closest(points_kinect, new PointF(0, h)); points_sorted[2] = get_closest(points_kinect, new PointF(w, h)); points_sorted[3] = get_closest(points_kinect, new PointF(w, 0)); Debug.Log("WWWWW: " + w + " HHHHHH: " + h); var points_camera = new PointF[4]; points_camera[0] = get_camera_point(new Vector3(0.5f, 0, 0.5f) * calibration_artefact); points_camera[1] = get_camera_point(new Vector3(0.5f, 0, -0.5f) * calibration_artefact); points_camera[2] = get_camera_point(new Vector3(-0.5f, 0, -0.5f) * calibration_artefact); points_camera[3] = get_camera_point(new Vector3(-0.5f, 0, 0.5f) * calibration_artefact); for (int i = 0; i < 4; i++) { points_sorted[i] = new PointF(points_sorted[i].X / w, points_sorted[i].Y / h); } Debug.Log(points_sorted[0] + " " + points_camera[0]); Debug.Log(points_sorted[1] + " " + points_camera[1]); Debug.Log(points_sorted[2] + " " + points_camera[2]); Debug.Log(points_sorted[3] + " " + points_camera[3]); Emgu.CV.Matrix <double> homography = new Emgu.CV.Matrix <double>(3, 3); CvInvoke.FindHomography(points_camera, points_sorted, homography, Emgu.CV.CvEnum.HomographyMethod.LMEDS); Debug.Log(homography[0, 0] + " " + homography[0, 1] + " " + homography[0, 2]); Debug.Log(homography[1, 0] + " " + homography[1, 1] + " " + homography[1, 2]); Debug.Log(homography[2, 0] + " " + homography[2, 1] + " " + homography[2, 2]); UnityEngine.Matrix4x4 m = Matrix4x4.identity; m.m00 = (float)homography[0, 0]; m.m01 = (float)homography[0, 1]; m.m02 = (float)homography[0, 2]; m.m10 = (float)homography[1, 0]; m.m11 = (float)homography[1, 1]; m.m12 = (float)homography[1, 2]; m.m20 = (float)homography[2, 0]; m.m21 = (float)homography[2, 1]; m.m22 = (float)homography[2, 2]; this.GetComponent <MeshRenderer>().material.SetMatrix("_Homography", m); // this.GetComponent<MeshRenderer>().material.SetVector("_KinectDims", new Vector2(w, h)); }
public static Emgu.CV.Matrix <int> DecodeHotMatrix(Emgu.CV.Matrix <int> y_label) { var DecodedMatrix = new Emgu.CV.Matrix <int>(y_label.Rows, 1); DecodedMatrix.SetZero(); for (int row = 0; row < y_label.Rows; row++) { for (int col = 0; col < y_label.Cols; col++) { if (y_label[row, col] == 1) { DecodedMatrix[row, 0] = col; break; } } } return(DecodedMatrix); }
public override void Process() { var matrix = new Emgu.CV.Matrix <double>(2, 3); Matrix4x4 transform = new Matrix4x4(); lock (FTransformLock) { //copy the transform out for (int i = 0; i < 16; i++) { transform[i] = FTrasform[i]; } } if (UseCenter) { double halfWidth = FInput.ImageAttributes.Width / 2; double halfHeight = FInput.ImageAttributes.Height / 2; transform = VMath.Translate(-halfWidth, -halfHeight, 0) * transform * VMath.Translate(halfWidth, halfHeight, 0); } matrix[0, 0] = transform[0, 0]; matrix[1, 0] = transform[0, 1]; matrix[0, 1] = transform[1, 0]; matrix[1, 1] = transform[1, 1]; matrix[0, 2] = transform[3, 0]; matrix[1, 2] = transform[3, 1]; if (FInput.LockForReading()) { try { CvInvoke.cvWarpAffine(FInput.CvMat, FOutput.CvMat, matrix.Ptr, (int)Emgu.CV.CvEnum.WARP.CV_WARP_FILL_OUTLIERS, new MCvScalar(0, 0, 0)); } finally { FInput.ReleaseForReading(); } } FOutput.Send(); }
TestTrainSplit(Emgu.CV.Matrix <float> Data, Emgu.CV.Matrix <int> Labels, float split = 0.2f) { int N = (int)Math.Floor(Labels.Rows * split); int Min = 0; int Max = Labels.Rows - 1; var TestIndices = helperFunctions.RandomNumGenerator(Min, Max, N); Emgu.CV.Matrix <float> x_test = new Emgu.CV.Matrix <float>(N, Data.Cols); Emgu.CV.Matrix <int> y_test = new Emgu.CV.Matrix <int>(N, 1); Emgu.CV.Matrix <float> x_train = new Emgu.CV.Matrix <float>(Data.Rows - N, Data.Cols); Emgu.CV.Matrix <int> y_train = new Emgu.CV.Matrix <int>(Data.Rows - N, 1); int testIndex = 0; int trainIndex = 0; for (int row = 0; row < Data.Rows; row++) { if (TestIndices.Contains(row)) { for (int j = 0; j < Data.Cols; j++) { x_test[testIndex, j] = Data[row, j]; } y_test[testIndex, 0] = Labels[row, 0]; testIndex++; } else { for (int j = 0; j < Data.Cols; j++) { x_train[trainIndex, j] = Data[row, j]; } y_train[trainIndex, 0] = Labels[row, 0]; trainIndex++; } } return(x_train, y_train, x_test, y_test); }
public override void Process() { var matrix = new Emgu.CV.Matrix<double>(2, 3); Matrix4x4 transform = new Matrix4x4(); lock (FTransformLock) { //copy the transform out for (int i = 0; i < 16; i++) { transform[i] = FTrasform[i]; } } if (UseCenter) { double halfWidth = FInput.ImageAttributes.Width / 2; double halfHeight = FInput.ImageAttributes.Height / 2; transform = VMath.Translate(-halfWidth, -halfHeight, 0) * transform * VMath.Translate(halfWidth, halfHeight, 0); } matrix[0, 0] = transform[0, 0]; matrix[1, 0] = transform[0, 1]; matrix[0, 1] = transform[1, 0]; matrix[1, 1] = transform[1, 1]; matrix[0, 2] = transform[3, 0]; matrix[1, 2] = transform[3, 1]; if (FInput.LockForReading()) { try { CvInvoke.cvWarpAffine(FInput.CvMat, FOutput.CvMat, matrix.Ptr, (int)Emgu.CV.CvEnum.WARP.CV_WARP_FILL_OUTLIERS, new MCvScalar(0, 0, 0)); } finally { FInput.ReleaseForReading(); } } FOutput.Send(); }
//public static (Emgu.CV.Matrix<float>, Emgu.CV.Matrix<float>) ReadCSV(string path, bool FirstRowHeader = true, char sep=',', int LabelIndex=0) //{ // List<List<Single>> Data = new List<List<Single>>(); // List<float> Label = new List<float>(); // using (var reader = new StreamReader(path)) // { // if (FirstRowHeader) // { // reader.ReadLine(); // } // while (!reader.EndOfStream) // { // var line = reader.ReadLine().Split(',').Select(x => float.Parse(x, CultureInfo.InvariantCulture)).ToList(); // var label = int.Parse(line.ElementAt(LabelIndex).ToString()); // line.RemoveAt(LabelIndex); // Data.Add(line); // Label.Add(label); // } // var data = Data.Select(a => a.ToArray()).ToArray(); // var labels = Label.Select(a => a).ToArray(); // Emgu.CV.Matrix<float> x_data = new Emgu.CV.Matrix<float>(To2D<Single>(data)); // Emgu.CV.Matrix<float> y_labels = new Emgu.CV.Matrix<float>(labels); // return (x_data, y_labels); // } //} //public static (Emgu.CV.Matrix<float>, Emgu.CV.Matrix<float>) ReadCSV(string path, bool FirstRowHeader = true, char sep = ',', int LabelIndex = 0) //{ // Mat Data = new Mat(); // Mat Labels = new Mat(); // List<float> Label = new List<float>(); // using (var reader = new StreamReader(path)) // { // if (FirstRowHeader) // { // reader.ReadLine(); // } // while (!reader.EndOfStream) // { // var line = reader.ReadLine().Split(',').Select(x => float.Parse(x, CultureInfo.InvariantCulture)).ToList(); // var label = int.Parse(line.ElementAt(LabelIndex).ToString()); // line.RemoveAt(LabelIndex); // IntPtr DataPointer = GCHandle.Alloc(line.ToArray(), GCHandleType.Pinned) // .AddrOfPinnedObject(); // int[] sizes =new int[] { 1, line.Count}; // var row= new Mat(sizes, Emgu.CV.CvEnum.DepthType.Cv32F, DataPointer); // var mat = Mat.Zeros(1, 1, Emgu.CV.CvEnum.DepthType.Cv32F, 1); // mat.SetTo(new MCvScalar(label)); // Data.PushBack(row); // Labels.PushBack(mat); // } // Emgu.CV.Matrix<float> x_data = new Emgu.CV.Matrix<float>(Data.Rows,Data.Cols); // Emgu.CV.Matrix<float> y_labels = new Emgu.CV.Matrix<float>(Labels.Rows,Labels.Cols); // Data.CopyTo(x_data); // Labels.CopyTo(y_labels); // return (x_data, y_labels); // } //} public static (Emgu.CV.Matrix <float>, Emgu.CV.Matrix <int>) ReadCSV(string path, bool FirstRowHeader = true, char sep = ',', int LabelIndex = 0) { var list = File.ReadAllLines(path).ToList(); if (list != null) { if (FirstRowHeader) { list.RemoveAt(0); } int ROWS = list.Count; int COLS = list[0].Split(sep).Length - 1; Emgu.CV.Matrix <float> x_data = new Emgu.CV.Matrix <float>(ROWS, COLS); Emgu.CV.Matrix <int> y_labels = new Emgu.CV.Matrix <int>(ROWS, 1); for (int i = 0; i < list.Count; i++) { var line = list[i].Split(',').Select(x => float.Parse(x, CultureInfo.InvariantCulture)).ToList(); var label = int.Parse(line.ElementAt(LabelIndex).ToString()); line.RemoveAt(LabelIndex); var row = line.ToArray(); for (int j = 0; j < row.Length; j++) { x_data[i, j] = row[j]; } y_labels[i, 0] = label; } return(x_data, y_labels); } else { return(null, null); } }
private void generateSigmaPoints() { Emgu.CV.Matrix <double> A_mat = c * chol(stateCovariance).Transpose(); Emgu.CV.Matrix <double> Y_mat = new Matrix <double>(state.Rows, state.Rows); for (int i = 0; i < Y_mat.Cols; i++) { for (int j = 0; j < Y_mat.Rows; j++) { Y_mat[i, j] = state[j, 0]; // Y = x(:,ones(1,numel(x))); } } // 2 * numel(state) + 1, the reference point //first the reference point for (int i = 0; i < state.Rows; i++) { sigmaPoints[i, 0] = state[i, 0]; } for (int i = 0; i < state.Rows; i++) { for (int j = 0; j < state.Rows; j++) { sigmaPoints[i, j + 1] = Y_mat[j, i] + A_mat[j, i]; } } for (int i = 0; i < state.Rows; i++) { for (int j = 0; j < state.Rows; j++) { sigmaPoints[i, j + state.Rows + 1] = Y_mat[j, i] - A_mat[j, i]; } } }
public static ColorDst[] ConvertColors <ColorSrc, ColorDst>(ColorSrc[] colors) where ColorSrc : struct, Emgu.CV.IColor where ColorDst : struct, Emgu.CV.IColor { if (colors == null || colors.Length == 0) { throw new ArgumentNullException("colors", "The array is null or empty."); } int dimensionSrc = colors[0].Dimension; int dimensionDst = new ColorDst().Dimension; Emgu.CV.Matrix <byte> matSrc = new Emgu.CV.Matrix <byte>(1, colors.Length, dimensionSrc); Emgu.CV.Matrix <byte> matDst = new Emgu.CV.Matrix <byte>(1, colors.Length, dimensionDst); Emgu.CV.CvEnum.COLOR_CONVERSION code = Emgu.CV.Util.CvToolbox.GetColorCvtCode( typeof(ColorSrc), typeof(ColorDst) ); // Copy colors into matSrc for (int i = 0; i < colors.Length; i++) { Emgu.CV.Structure.MCvScalar colorComp = colors[i].MCvScalar; if (typeof(Rgba).IsAssignableFrom(typeof(ColorSrc))) { double swap = colorComp.v0; colorComp.v0 = colorComp.v2; colorComp.v2 = swap; } if (dimensionSrc > 0) { matSrc.Data[0, i *dimensionSrc + 0] = (byte)colorComp.v0; } if (dimensionSrc > 1) { matSrc.Data[0, i *dimensionSrc + 1] = (byte)colorComp.v1; } if (dimensionSrc > 2) { matSrc.Data[0, i *dimensionSrc + 2] = (byte)colorComp.v2; } if (dimensionSrc > 3) { matSrc.Data[0, i *dimensionSrc + 3] = (byte)colorComp.v3; } } // Convert colors Emgu.CV.CvInvoke.cvCvtColor(matSrc, matDst, code); // Copy matDst into new color array ColorDst[] newColors = new ColorDst[colors.Length]; for (int i = 0; i < colors.Length; i++) { newColors[i] = new ColorDst(); Emgu.CV.Structure.MCvScalar colorComp = new Emgu.CV.Structure.MCvScalar(); if (dimensionDst > 0) { colorComp.v0 = matDst.Data[0, i *dimensionDst + 0]; } if (dimensionDst > 1) { colorComp.v1 = matDst.Data[0, i *dimensionDst + 1]; } if (dimensionDst > 2) { colorComp.v2 = matDst.Data[0, i *dimensionDst + 2]; } if (dimensionDst > 3) { colorComp.v3 = matDst.Data[0, i *dimensionDst + 3]; } newColors[i].MCvScalar = colorComp; } return(newColors); }
public static void RecursiveGrowing(System.Drawing.Point seed, Image <Gray, Byte> _img, Image <Gray, Byte> _mask) { Emgu.CV.Matrix <byte> mat1 = new Emgu.CV.Matrix <Byte>(_img.Height, _img.Width); Emgu.CV.Matrix <Byte> mat2 = new Emgu.CV.Matrix <Byte>(_img.Height, _img.Width); mat1.SetZero(); mat2.SetZero(); //Emgu.CV.Structure.MCvConnectedComp conn = new Emgu.CV.Structure.MCvConnectedComp(); //CvInvoke.FloodFill(_img, mask, seed, new MCvScalar(128), out rect, new MCvScalar(1), new MCvScalar(1), Emgu.CV.CvEnum.Connectivity.EightConnected); //Console.WriteLine("Rectangle {0},{1}", rect.Width, rect.Height); //Emgu.CV.Structure.MCvSeq components = new Emgu.CV.Structure.MCvConnectedComp <MCvConnectedComp>(new MemStorage()); // Emgu.CV.CvInvoke.cvPyrSegmentation(image, result, storage, out components._ptr, 4, 255, 30); //CvInvoke.sh // Set the seed _mask.Data[seed.X, seed.Y, 0] = 1; // Set to 1 mat1[seed.X, seed.Y] = 1; mat2[seed.X, seed.Y] = 1; int iter = 0; while (iter < 1) { // Dilation _mask.Dilate(1); //System.Drawing.Point[] points = GetNeighbors(System.Drawing.Point loc, System.Drawing.Size size) iter++; } // recurse left //_row = _row - 1; //if ((_imData[_row, _col].Intensity != 0) && (_imMask[_row, _col].Intensity == 0)) //{ // RecursiveGrowing(_row, _col, _imData, _imMask); //} // recurse right //_row = _row - 1; //if ((_img[_row, _col].Intensity != 0) && (_imMask[_row, _col].Intensity == 0)) //{ // RecursiveGrowing(_row, _col, _img, _imMask); //} //// recurse up //_col = _col - 1; //_row = _row - 1; //if ((_imData[_row, _col].Intensity != 0) && (_imMask[_row, _col].Intensity == 0)) //{ // RecursiveGrowing(_row, _col, _imData, _imMask); //} // recurse down //_col = _col - 1; //if ((_img[_row, _col].Intensity != 0) && (_imMask[_row, _col].Intensity == 0)) //{ // RecursiveGrowing(_row, _col, _img, _imMask); //} }
public static Emgu.CV.Matrix <double> LAMP(Emgu.CV.Matrix <double> X, int[] idx, Emgu.CV.Matrix <double> Ys) { //if (!s_CheckInputErrorsLAMP(X, cp_index, Ys)) { // fprintf(stderr, "ERROR: lamp - Invalid input.\n"); // return Mat::zeros(1, 1, CV_8UC1); //} double tol = 1E-003; // Building an array with the indices of the points to be projected. //std::vector<int> proj_idx(X.rows); //for (int i = 0; i < X.rows; ++i) // proj_idx[i] = i; //for (int i = 0; i < cp_index.size(); ++i) // proj_idx[cp_index[i]] = -1; // // Building the control points and projected points matrices. Emgu.CV.Matrix <double> Xs = new Emgu.CV.Matrix <double>(idx.Length, X.Cols); Xs.SetZero(); Emgu.CV.Matrix <double> Y = new Emgu.CV.Matrix <double>(X.Rows, Ys.Cols); Y.SetZero(); //for (int i = 0; i < Xs.rows; ++i) { // X.row(cp_index[i]).copyTo(Xs.row(i)); // Ys.row(i).copyTo(Y.row(cp_index[i])); //} // //Mat alpha = Mat::zeros(1, cp_index.size(), CV_32FC1); //for (int i = 0; i < X.rows; ++i) { // if (proj_idx[i] == -1) // continue; // // // Building the weights of each control point over the current point. // for (int j = 0; j < cp_index.size(); ++j) // alpha.at<float>(0, j) = 1 / cv::max(cv::norm(Xs.row(j), X.row(proj_idx[i])), tol); // // float sum_alpha = cv::sum(alpha)[0]; // // Mat T = Mat::zeros(Xs.rows, Xs.cols, Xs.depth()); // for (int k = 0; k < Xs.cols; ++k) // T.col(k) = Xs.col(k).mul(alpha.t()); // // // Building the x-tilde and y-tilde variables (Eq. 3). // Mat Xtil; // cv::reduce(T, Xtil, 0, CV_REDUCE_SUM); // Xtil = Xtil * (1 / sum_alpha); // // T = Mat::zeros(Ys.rows, Ys.cols, Ys.depth()); // for (int k = 0; k < Ys.cols; ++k) // T.col(k) = Ys.col(k).mul(alpha.t()); // // Mat Ytil; // cv::reduce(T, Ytil, 0, CV_REDUCE_SUM); // Ytil = Ytil * (1 / sum_alpha); // // // Building the x-hat and y-hat variables (Eq. 4). // Mat Xhat = Mat::zeros(Xs.rows, Xs.cols, Xs.depth()); // for (int k = 0; k < Xs.rows; ++k) // Xhat.row(k) = Xs.row(k) - Xtil; // // Mat Yhat = Mat::zeros(Ys.rows, Ys.cols, Ys.depth()); // for (int k = 0; k < Ys.rows; ++k) // Yhat.row(k) = Ys.row(k) - Ytil; // // // Building the A and B matrices (Eq. 6) and calculating the SVD of t(A) * B. // Mat sqrt_alpha; // cv::sqrt(alpha, sqrt_alpha); // sqrt_alpha = sqrt_alpha.t(); // // Mat A; // Xhat.copyTo(A); // for (int k = 0; k < A.cols; ++k) // A.col(k) = A.col(k).mul(sqrt_alpha); // // Mat B; // Yhat.copyTo(B); // for (int k = 0; k < B.cols; k++) // B.col(k) = B.col(k).mul(sqrt_alpha); // // cv::SVD udv(A.t() * B); // // // Calculating the affine transform matrix (Eq. 7). // Mat M = udv.u * udv.vt; // // // Projecting X[i] using the matrix M (Eq 8) // Y.row(proj_idx[i]) = (X.row(proj_idx[i]) - Xtil) * M + Ytil; //} return(Y); }
public void DetectFaceRotationEmgu() { var imagePoints = new List <PointF>(); imagePoints.Add(new PointF(ProgramCore.MainForm.PhotoControl.Recognizer.RealPoints[66].X, ProgramCore.MainForm.PhotoControl.Recognizer.RealPoints[66].Y)); // уши imagePoints.Add(new PointF(ProgramCore.MainForm.PhotoControl.Recognizer.RealPoints[67].X, ProgramCore.MainForm.PhotoControl.Recognizer.RealPoints[67].Y)); imagePoints.Add(new PointF(ProgramCore.MainForm.PhotoControl.Recognizer.RealPoints[0].X, ProgramCore.MainForm.PhotoControl.Recognizer.RealPoints[0].Y)); // глаза центры imagePoints.Add(new PointF(ProgramCore.MainForm.PhotoControl.Recognizer.RealPoints[1].X, ProgramCore.MainForm.PhotoControl.Recognizer.RealPoints[1].Y)); imagePoints.Add(new PointF(ProgramCore.MainForm.PhotoControl.Recognizer.RealPoints[3].X, ProgramCore.MainForm.PhotoControl.Recognizer.RealPoints[3].Y)); // левый-правый угол рта imagePoints.Add(new PointF(ProgramCore.MainForm.PhotoControl.Recognizer.RealPoints[4].X, ProgramCore.MainForm.PhotoControl.Recognizer.RealPoints[4].Y)); imagePoints.Add(new PointF(ProgramCore.MainForm.PhotoControl.Recognizer.RealPoints[2].X, ProgramCore.MainForm.PhotoControl.Recognizer.RealPoints[2].Y)); // центр носа var modelPoints = new List <MCvPoint3D32f>(); modelPoints.Add(new MCvPoint3D32f(ProgramCore.MainForm.RenderControl.HeadPoints.Points[66].X, ProgramCore.MainForm.RenderControl.HeadPoints.Points[66].Y, ProgramCore.MainForm.RenderControl.HeadPoints.Points[66].Z)); modelPoints.Add(new MCvPoint3D32f(ProgramCore.MainForm.RenderControl.HeadPoints.Points[67].X, ProgramCore.MainForm.RenderControl.HeadPoints.Points[67].Y, ProgramCore.MainForm.RenderControl.HeadPoints.Points[67].Z)); modelPoints.Add(new MCvPoint3D32f(ProgramCore.MainForm.RenderControl.HeadPoints.Points[0].X, ProgramCore.MainForm.RenderControl.HeadPoints.Points[0].Y, ProgramCore.MainForm.RenderControl.HeadPoints.Points[0].Z)); modelPoints.Add(new MCvPoint3D32f(ProgramCore.MainForm.RenderControl.HeadPoints.Points[1].X, ProgramCore.MainForm.RenderControl.HeadPoints.Points[1].Y, ProgramCore.MainForm.RenderControl.HeadPoints.Points[1].Z)); modelPoints.Add(new MCvPoint3D32f(ProgramCore.MainForm.RenderControl.HeadPoints.Points[3].X, ProgramCore.MainForm.RenderControl.HeadPoints.Points[3].Y, ProgramCore.MainForm.RenderControl.HeadPoints.Points[3].Z)); modelPoints.Add(new MCvPoint3D32f(ProgramCore.MainForm.RenderControl.HeadPoints.Points[4].X, ProgramCore.MainForm.RenderControl.HeadPoints.Points[4].Y, ProgramCore.MainForm.RenderControl.HeadPoints.Points[4].Z)); modelPoints.Add(new MCvPoint3D32f(ProgramCore.MainForm.RenderControl.HeadPoints.Points[2].X, ProgramCore.MainForm.RenderControl.HeadPoints.Points[2].Y, ProgramCore.MainForm.RenderControl.HeadPoints.Points[2].Z)); #region CamMatrix var img = CvInvoke.Imread(ProgramCore.MainForm.PhotoControl.TemplateImage); //float imageWidth = img.Cols; // float imageHeight = img.Rows; float imageWidth = ProgramCore.MainForm.PhotoControl.Recognizer.ImageWidth; float imageHeight = ProgramCore.MainForm.PhotoControl.Recognizer.ImageHeight; var max_d = Math.Max(imageWidth, imageHeight); var camMatrix = new Emgu.CV.Matrix <double>(3, 3); camMatrix[0, 0] = max_d; camMatrix[0, 1] = 0; camMatrix[0, 2] = imageWidth / 2.0; camMatrix[1, 0] = 0; camMatrix[1, 1] = max_d; camMatrix[1, 2] = imageHeight / 2.0; camMatrix[2, 0] = 0; camMatrix[2, 1] = 0; camMatrix[2, 2] = 1.0; /* * float max_d = Mathf.Max (imageHeight, imageWidth); * camMatrix = new Mat (3, 3, CvType.CV_64FC1); * camMatrix.put (0, 0, max_d); * camMatrix.put (0, 1, 0); * camMatrix.put (0, 2, imageWidth / 2.0f); * camMatrix.put (1, 0, 0); * camMatrix.put (1, 1, max_d); * camMatrix.put (1, 2, imageHeight / 2.0f); * camMatrix.put (2, 0, 0); * camMatrix.put (2, 1, 0); * camMatrix.put (2, 2, 1.0f); */ #endregion var distArray = new double[] { 0, 0, 0, 0 }; var distMatrix = new Matrix <double>(distArray); // не используемый коэф. var rv = new double[] { 0, 0, 0 }; var rvec = new Matrix <double>(rv); var tv = new double[] { 0, 0, 1 }; var tvec = new Matrix <double>(tv); Emgu.CV.CvInvoke.SolvePnP(modelPoints.ToArray(), imagePoints.ToArray(), camMatrix, distMatrix, rvec, tvec, false, Emgu.CV.CvEnum.SolvePnpMethod.EPnP); // решаем проблему PNP double tvec_z = tvec[2, 0]; var rotM = new Matrix <double>(3, 3); if (!double.IsNaN(tvec_z)) { CvInvoke.Rodrigues(rvec, rotM); Matrix4 transformationM = new Matrix4(); transformationM.Row0 = new Vector4((float)rotM[0, 0], (float)rotM[0, 1], (float)rotM[0, 2], (float)tvec[0, 0]); transformationM.Row1 = new Vector4((float)rotM[1, 0], (float)rotM[1, 1], (float)rotM[1, 2], (float)tvec[1, 0]); transformationM.Row2 = new Vector4((float)rotM[2, 0], (float)rotM[2, 1], (float)rotM[2, 2], (float)tvec[2, 0]); transformationM.Row3 = new Vector4(0.0f, 0.0f, 0.0f, 1.0f); var quaternion = ExtractRotationFromMatrix(ref transformationM); //quaternion.Y = -quaternion.Y; // RotationMatrix = CreateRotationMatrix(quaternion); // RotationMatrix = Matrix4.CreateFromQuaternion(quaternion); // RotationMatrix = invertYM * RotationMatrix * invertZM; quaternion.X = -quaternion.X; quaternion.Y = -quaternion.Y; quaternion.Z = -quaternion.Z; MeshQuaternion = quaternion; var angles = ToEulerRad(MeshQuaternion); if (angles.X > -6f && angles.X < 3.0f) { angles.X = 0.0f; } HeadAngle = angles.Y; MeshQuaternion = quaternion = ToQ(angles); RotationMatrix = CreateRotationMatrix(quaternion); quaternion.Z = -MeshQuaternion.Z; ReverseRotationMatrix = CreateRotationMatrix(quaternion); } else { MeshQuaternion = Quaternion.Identity; HeadAngle = 0.0f; } }
public void GoToNextState() { Emgu.CV.Matrix <double> processNoise = new Emgu.CV.Matrix <double>(6, 1); processNoise.SetRandNormal(new MCvScalar(), new MCvScalar(processNoise[0, 0])); state = transitionMatrix * state + processNoise; }
public Emgu.CV.Matrix <double> GetMeasurement() { Emgu.CV.Matrix <double> measurementNoise = new Emgu.CV.Matrix <double>(3, 1); measurementNoise.SetRandNormal(new MCvScalar(), new MCvScalar(Math.Sqrt(measurementNoise[0, 0]))); return(measurementMatrix * state + measurementNoise); }
public void DetectFaceRotationEmgu(string filePath, Image templateImage, List <Vector2> imagePointsInVector, List <Vector3> realPoints) { var imagePoints = new List <PointF>(); imagePoints.Add(new PointF(imagePointsInVector[66].X, imagePointsInVector[66].Y)); // уши imagePoints.Add(new PointF(imagePointsInVector[67].X, imagePointsInVector[67].Y)); imagePoints.Add(new PointF(imagePointsInVector[0].X, imagePointsInVector[0].Y)); // глаза центры imagePoints.Add(new PointF(imagePointsInVector[1].X, imagePointsInVector[1].Y)); imagePoints.Add(new PointF(imagePointsInVector[3].X, imagePointsInVector[3].Y)); // левый-правый угол рта imagePoints.Add(new PointF(imagePointsInVector[4].X, imagePointsInVector[4].Y)); imagePoints.Add(new PointF(imagePointsInVector[2].X, imagePointsInVector[2].Y)); // центр носа var modelPoints = new List <MCvPoint3D32f>(); modelPoints.Add(new MCvPoint3D32f(realPoints[66].X, realPoints[66].Y, realPoints[66].Z)); modelPoints.Add(new MCvPoint3D32f(realPoints[67].X, realPoints[67].Y, realPoints[67].Z)); modelPoints.Add(new MCvPoint3D32f(realPoints[0].X, realPoints[0].Y, realPoints[0].Z)); modelPoints.Add(new MCvPoint3D32f(realPoints[1].X, realPoints[1].Y, realPoints[1].Z)); modelPoints.Add(new MCvPoint3D32f(realPoints[3].X, realPoints[3].Y, realPoints[3].Z)); modelPoints.Add(new MCvPoint3D32f(realPoints[4].X, realPoints[4].Y, realPoints[4].Z)); modelPoints.Add(new MCvPoint3D32f(realPoints[2].X, realPoints[2].Y, realPoints[2].Z)); #region CamMatrix var img = CvInvoke.Imread(filePath); float imageWidth = templateImage.Width; float imageHeight = templateImage.Height; var max_d = Math.Max(imageWidth, imageHeight); var camMatrix = new Emgu.CV.Matrix <double>(3, 3); camMatrix[0, 0] = max_d; camMatrix[0, 1] = 0; camMatrix[0, 2] = imageWidth / 2.0; camMatrix[1, 0] = 0; camMatrix[1, 1] = max_d; camMatrix[1, 2] = imageHeight / 2.0; camMatrix[2, 0] = 0; camMatrix[2, 1] = 0; camMatrix[2, 2] = 1.0; #endregion var distArray = new double[] { 0, 0, 0, 0 }; var distMatrix = new Matrix <double>(distArray); // не используемый коэф. var rv = new double[] { 0, 0, 0 }; var rvec = new Matrix <double>(rv); var tv = new double[] { 0, 0, 1 }; var tvec = new Matrix <double>(tv); Emgu.CV.CvInvoke.SolvePnP(modelPoints.ToArray(), imagePoints.ToArray(), camMatrix, distMatrix, rvec, tvec, false, Emgu.CV.CvEnum.SolvePnpMethod.EPnP); // решаем проблему PNP double tvec_z = tvec[2, 0]; var rotM = new Matrix <double>(3, 3); if (!double.IsNaN(tvec_z)) { CvInvoke.Rodrigues(rvec, rotM); Matrix4 transformationM = new Matrix4(); transformationM.Row0 = new Vector4((float)rotM[0, 0], (float)rotM[0, 1], (float)rotM[0, 2], (float)tvec[0, 0]); transformationM.Row1 = new Vector4((float)rotM[1, 0], (float)rotM[1, 1], (float)rotM[1, 2], (float)tvec[1, 0]); transformationM.Row2 = new Vector4((float)rotM[2, 0], (float)rotM[2, 1], (float)rotM[2, 2], (float)tvec[2, 0]); transformationM.Row3 = new Vector4(0.0f, 0.0f, 0.0f, 1.0f); var quaternion = ExtractRotationFromMatrix(ref transformationM); quaternion.X = -quaternion.X; quaternion.Y = -quaternion.Y; quaternion.Z = -quaternion.Z; MeshQuaternion = quaternion; var angles = ToEulerRad(MeshQuaternion); // if (angles.X > -6f && angles.X < 3.0f) angles.X = 0.0f; HeadAngle = angles.Y; MeshQuaternion = quaternion = ToQ(angles); RotationMatrix = CreateRotationMatrix(quaternion); //RotationMatrix = Matrix4.Identity; quaternion.Z = -MeshQuaternion.Z; ReverseRotationMatrix = CreateRotationMatrix(quaternion); } else { MeshQuaternion = Quaternion.Identity; HeadAngle = 0.0f; } }