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;
}
}
