通俗的讲，卡尔曼滤波就是综合上一步骤对当前步骤的预测值，结合当前步骤的测量值，来估计当前的状态。
最近在做一个Unity项目时，用到了卡尔曼滤波消除抖动,主要是消除头部抖动，从Dlib实时获得68个特征值的points，由points.x points.y points.z 获得头部的旋转四元数作为测量值，初始值可以任意定义，因为几次迭代后预测值就逼近真实值了。因为Unity用c#编写，c#本身不存在矩阵库，对矩阵运算比较吃力，需要自己导入矩阵库，方法很简单，首先定义矩阵库的命名空间，然后使用该命名空间即可。
矩阵库：
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
namespace Slash.Lastwork
{
public class Matrix
{
double[,] A;
//m行n列
int m, n;
string name;
public Matrix(int am, int an)
{
m = am;
n = an;
A = new double[m, n];
name = "Result";
}
public Matrix(int am, int an, string aName)
{
m = am;
n = an;
A = new double[m, n];
name = aName;
}
public int getM
{
get { return m; }
}
public int getN
{
get { return n; }
}
public double[,] Detail
{
get { return A; }
set { A = value; }
}
public string Name
{
get { return name; }
set { name = value; }
}
}
public class MatrixOperator
{
MatrixOperator()
{ }
/// <summary>
/// 矩阵加法
/// </summary>
/// <param name="Ma"></param>
/// <param name="Mb"></param>
/// <returns></returns>
public static Matrix MatrixAdd(Matrix Ma, Matrix Mb)
{
int m = Ma.getM;
int n = Ma.getN;
int m2 = Mb.getM;
int n2 = Mb.getN;
if ((m != m2) || (n != n2))
{
Exception myException = new Exception("数组维数不匹配");
throw myException;
}
Matrix Mc = new Matrix(m, n);
double[,] c = Mc.Detail;
double[,] a = Ma.Detail;
double[,] b = Mb.Detail;
int i, j;
for (i = 0; i < m; i++)
for (j = 0; j < n; j++)
c[i, j] = a[i, j] + b[i, j];
return Mc;
}
/// <summary>
/// 矩阵减法
/// </summary>
/// <param name="Ma"></param>
/// <param name="Mb"></param>
/// <returns></returns>
public static Matrix MatrixSub(Matrix Ma, Matrix Mb)
{
int m = Ma.getM;
int n = Ma.getN;
int m2 = Mb.getM;
int n2 = Mb.getN;
if ((m != m2) || (n != n2))
{
Exception myException = new Exception("数组维数不匹配");
throw myException;
}
Matrix Mc = new Matrix(m, n);
double[,] c = Mc.Detail;
double[,] a = Ma.Detail;
double[,] b = Mb.Detail;
int i, j;
for (i = 0; i < m; i++)
for (j = 0; j < n; j++)
c[i, j] = a[i, j] - b[i, j];
return Mc;
}
/// <summary>
/// 矩阵打印
/// </summary>
/// <param name="Ma"></param>
/// <returns></returns>
public static string MatrixPrint(Matrix Ma)
{
string s;
s = Ma.Name + ":\n";
int m = Ma.getM;
int n = Ma.getN;
double[,] a = Ma.Detail;
for (int i = 0; i < m; i++)
{
for (int j = 0; j < n; j++)
{
s += a[i, j].ToString("0.0000") + "\t";
}
s += "\n";
}
return s;
}
/// <summary>
/// 矩阵乘法
/// </summary>
/// <param name="Ma"></param>
/// <param name="Mb"></param>
/// <returns></returns>
public static Matrix MatrixMulti(Matrix Ma, Matrix Mb)
{
int m = Ma.getM;
int n = Ma.getN;
int m2 = Mb.getM;
int n2 = Mb.getN;
if (n != m2)
{
Exception myException = new Exception("数组维数不匹配");
throw myException;
}
Matrix Mc = new Matrix(m, n2);
double[,] c = Mc.Detail;
double[,] a = Ma.Detail;
double[,] b = Mb.Detail;
int i, j, k;
for (i = 0; i < m; i++)
for (j = 0; j < n2; j++)
{
c[i, j] = 0;
for (k = 0; k < n; k++)
c[i, j] += a[i, k] * b[k, j];
}
return Mc;
}
/// <summary>
/// 矩阵数乘
/// </summary>
/// <param name="k"></param>
/// <param name="Ma"></param>
/// <returns></returns>
public static Matrix MatrixSimpleMulti(double k, Matrix Ma)
{
int n = Ma.getN;
int m = Ma.getM;
Matrix Mc = new Matrix(m, n);
double[,] c = Mc.Detail;
double[,] a = Ma.Detail;
int i, j;
for (i = 0; i < m; i++)
for (j = 0; j < n; j++)
c[i, j] = a[i, j] * k;
return Mc;
}
/// <summary>
/// 矩阵转置
/// </summary>
/// <param name="Ma"></param>
/// <param name="Mb"></param>
/// <returns></returns>
public static Matrix MatrixTrans(Matrix Ma)
{
int m = Ma.getM;
int n = Ma.getN;
Matrix Mc = new Matrix(n, m);
double[,] c = Mc.Detail;
double[,] a = Ma.Detail;
for (int i = 0; i < n; i++)
for (int j = 0; j < m; j++)
c[i, j] = a[j, i];
return Mc;
}
/// <summary>
/// 矩阵求逆(高斯法)
/// </summary>
/// <param name="Ma"></param>
/// <returns></returns>
public static Matrix MatrixInv(Matrix Ma)
{
int m = Ma.getM;
int n = Ma.getN;
if (m != n)
{
Exception myException = new Exception("数组维数不匹配");
throw myException;
}
Matrix Mc = new Matrix(m, n);
double[,] a0 = Ma.Detail;
double[,] a = (double[,])a0.Clone();
double[,] b = Mc.Detail;
int i, j, row, k;
double max, temp;
//单位矩阵
for (i = 0; i < n; i++)
{
b[i, i] = 1;
}
for (k = 0; k < n; k++)
{
max = 0; row = k;
//找最大元，其所在行为row
for (i = k; i < n; i++)
{
temp = Math.Abs(a[i, k]);
if (max < temp)
{
max = temp;
row = i;
}
}
if (max == 0)
{
Exception myException = new Exception("没有逆矩阵");
throw myException;
}
//交换k与row行
if (row != k)
{
for (j = 0; j < n; j++)
{
temp = a[row, j];
a[row, j] = a[k, j];
a[k, j] = temp;
temp = b[row, j];
b[row, j] = b[k, j];
b[k, j] = temp;
}
}
//首元化为1
for (j = k + 1; j < n; j++) a[k, j] /= a[k, k];
for (j = 0; j < n; j++) b[k, j] /= a[k, k];
a[k, k] = 1;
//k列化为0
//对a
for (j = k + 1; j < n; j++)
{
for (i = 0; i < k; i++) a[i, j] -= a[i, k] * a[k, j];
for (i = k + 1; i < n; i++) a[i, j] -= a[i, k] * a[k, j];
}
//对b
for (j = 0; j < n; j++)
{
for (i = 0; i < k; i++) b[i, j] -= a[i, k] * b[k, j];
for (i = k + 1; i < n; i++) b[i, j] -= a[i, k] * b[k, j];
}
for (i = 0; i < n; i++) a[i, k] = 0;
a[k, k] = 1;
}
return Mc;
}
/// <summary>
/// 矩阵求逆(伴随矩阵法)
/// </summary>
/// <param name="Ma"></param>
/// <returns></returns>
public static Matrix MatrixInvByCom(Matrix Ma)
{
double d = MatrixOperator.MatrixDet(Ma);
if (d == 0)
{
Exception myException = new Exception("没有逆矩阵");
throw myException;
}
Matrix Ax = MatrixOperator.MatrixCom(Ma);
Matrix An = MatrixOperator.MatrixSimpleMulti((1.0 / d), Ax);
return An;
}
/// <summary>
/// 对应行列式的代数余子式矩阵
/// </summary>
/// <param name="Ma"></param>
/// <returns></returns>
public static Matrix MatrixSpa(Matrix Ma, int ai, int aj)
{
int m = Ma.getM;
int n = Ma.getN;
if (m != n)
{
Exception myException = new Exception("数组维数不匹配");
throw myException;
}
int n2 = n - 1;
Matrix Mc = new Matrix(n2, n2);
double[,] a = Ma.Detail;
double[,] b = Mc.Detail;
//左上
for (int i = 0; i < ai; i++)
for (int j = 0; j < aj; j++)
{
b[i, j] = a[i, j];
}
//右下
for (int i = ai; i < n2; i++)
for (int j = aj; j < n2; j++)
{
b[i, j] = a[i + 1, j + 1];
}
//右上
for (int i = 0; i < ai; i++)
for (int j = aj; j < n2; j++)
{
b[i, j] = a[i, j + 1];
}
//左下
for (int i = ai; i < n2; i++)
for (int j = 0; j < aj; j++)
{
b[i, j] = a[i + 1, j];
}
//符号位
if ((ai + aj) % 2 != 0)
{
for (int i = 0; i < n2; i++)
b[i, 0] = -b[i, 0];
}
return Mc;
}
/// <summary>
/// 矩阵的行列式
/// </summary>
/// <param name="Ma"></param>
/// <returns></returns>
public static double MatrixDet(Matrix Ma)
{
int m = Ma.getM;
int n = Ma.getN;
if (m != n)
{
Exception myException = new Exception("数组维数不匹配");
throw myException;
}
double[,] a = Ma.Detail;
if (n == 1) return a[0, 0];
double D = 0;
for (int i = 0; i < n; i++)
{
D += a[1, i] * MatrixDet(MatrixSpa(Ma, 1, i));
}
return D;
}
/// <summary>
/// 矩阵的伴随矩阵
/// </summary>
/// <param name="Ma"></param>
/// <returns></returns>
public static Matrix MatrixCom(Matrix Ma)
{
int m = Ma.getM;
int n = Ma.getN;
Matrix Mc = new Matrix(m, n);
double[,] c = Mc.Detail;
double[,] a = Ma.Detail;
for (int i = 0; i < m; i++)
for (int j = 0; j < n; j++)
c[i, j] = MatrixDet(MatrixSpa(Ma, j, i));
return Mc;
}
}
}
迭代过程：
using UnityEngine;
using System.Collections.Generic;
using System;
using System.IO;
using Slash.Lastwork;
#if UNITY_5_3 || UNITY_5_3_OR_NEWER
using UnityEngine.SceneManagement;
#endif
using OpenCVForUnity;
using DlibFaceLandmarkDetector;
using DlibFaceLandmarkDetectorSample;
using Moments;
namespace Slash.Facemoji
{
/// <summary>
/// FaceTracking, Use Dlib to detect face landmark and use Live2D model to track faces
/// </summary>
[AddComponentMenu("Facemoji/FaceTracking")]
[RequireComponent (typeof(WebCamTextureToMatHelper))]
public class FaceTracking : MonoBehaviour
{
///矩阵的运算
//F_K 状态转移矩阵
Matrix stateConvert = new Matrix(6, 6, "stateConvert");
//P_K 后验估计误差协方差矩阵，度量估计值的精确程度
Matrix errorCovPost = new Matrix(6, 6, "errorCovPsot");
//H_K 观测矩阵
Matrix measureMatrix = new Matrix(6, 3, "measureMatrix");
//Q_K 过程噪声
Matrix processNoiseConvert = new Matrix(6, 6, "processNoiseConvert");
//R_K 观测噪声
Matrix measureNoiseConvert = new Matrix(3, 3, "measureNoiseConvert");
//X_0 初始状态
Matrix originState = new Matrix(3, 1, "originState");
/// <summary>
/// All Live2D Texture
/// </summary>
private enum WhichTexture
{
noir_santa = 1,
uni = 2,
};
/// <summary>
/// The selected Live2D Texture now.(Default shizuku)
/// </summary>
private WhichTexture selectedTexture = WhichTexture.uni;
/// <summary>
/// Start Button
/// </summary>
public static GameObject startBtn;
/// <summary>
/// Finish Button
/// </summary>
public static GameObject finishBtn;
/// <summary>
/// live2DModel.transform.localScale
/// </summary>
public float modelScale = 0.9f;
/// <summary>
/// The web cam texture to mat helper.
/// </summary>
WebCamTextureToMatHelper webCamTextureToMatHelper;
/// <summary>
/// The face landmark detector.
/// </summary>
FaceLandmarkDetector faceLandmarkDetector;
/// <summary>
/// The live2DModel.
/// </summary>
public Live2DModel live2DModel;
/// <summary>
/// The frontal face parameter.
/// </summary>
FrontalFaceParam frontalFaceParam;
/// <summary>
/// The shape_predictor_68_face_landmarks_dat_filepath.
/// </summary>
private string shape_predictor_68_face_landmarks_dat_filepath;
/// <summary>
/// Model file path.
/// </summary>
private string moc_filepath;
private string physics_filepath;
private string pose_filepath;
private string[] texture_filepath = new string[6];
/// <summary>
/// 相关矩阵的计算.
/// </summary>
//F_K 状态转移矩阵
Matrix F_K = new Matrix(6, 6, "F_K");
//P_K 后验估计误差协方差矩阵，度量估计值的精确程度
Matrix P_K = new Matrix(6, 6, "errorCovPsot");
//P_k+1
Matrix P_front = new Matrix(6, 6, "errorCovPsot");
//H_K 观测矩阵
Matrix H_K = new Matrix(3, 6, "H_K");
//Q_K 过程噪声
Matrix Q_K = new Matrix(6, 6, "Q_K");
//R_K 观测噪声
Matrix R_K = new Matrix(3, 3, "R_K");
//Z_K 测量值
Matrix Z_K = new Matrix(3, 1, "Z_K");
//X_K
Matrix X_K = new Matrix(6, 1, "state");
//X_K+1
Matrix X_front = new Matrix(6, 1, "state");
//eye
Matrix eye = new Matrix(6, 6, "state");
// Use this for initialization
void Start()
{
startBtn = GameObject.Find("StartButton");
finishBtn = GameObject.Find("FinishButton");
startBtn.SetActive(true);
finishBtn.SetActive(false);
webCamTextureToMatHelper = gameObject.GetComponent<WebCamTextureToMatHelper>();
#if UNITY_WEBGL && !UNITY_EDITOR
webCamTextureToMatHelper.flipHorizontal = true;
StartCoroutine(getFilePathCoroutine());
#else
// FaceLandmark model filepath
shape_predictor_68_face_landmarks_dat_filepath = DlibFaceLandmarkDetector.Utils.getFilePath("shape_predictor_68_face_landmarks.dat");
// Load Texture filepath
LoadTexture();
Run();
#endif
}
private void LoadTexture()
{
// Load model filepath
switch (selectedTexture)
{
case WhichTexture.noir_santa:
{
moc_filepath = OpenCVForUnity.Utils.getFilePath("noir_santa/noir_santa.moc.bytes");
physics_filepath = OpenCVForUnity.Utils.getFilePath("noir_santa/noir_santa.physics.json");
pose_filepath = OpenCVForUnity.Utils.getFilePath("noir_santa/noir_santa.pose.json");
for (int i = 0; i < texture_filepath.Length; i++)
{
texture_filepath[i] = OpenCVForUnity.Utils.getFilePath("noir_santa/noir_santa.1024/texture_0" + i + ".png");
}
break;
}
case WhichTexture.uni:
{
moc_filepath = OpenCVForUnity.Utils.getFilePath("uni/uni.moc.bytes");
physics_filepath = OpenCVForUnity.Utils.getFilePath("uni/uni.physics.json");
pose_filepath = OpenCVForUnity.Utils.getFilePath("uni/uni.pose.json");
for (int i = 0; i < texture_filepath.Length; i++)
{
texture_filepath[i] = OpenCVForUnity.Utils.getFilePath("uni/uni.1024/texture_0" + i + ".png");
}
break;
}
default:
{
moc_filepath = OpenCVForUnity.Utils.getFilePath("uni/uni.moc.bytes");
physics_filepath = OpenCVForUnity.Utils.getFilePath("uni/uni.physics.json");
pose_filepath = OpenCVForUnity.Utils.getFilePath("uni/uni.pose.json");
for (int i = 0; i < texture_filepath.Length; i++)
{
texture_filepath[i] = OpenCVForUnity.Utils.getFilePath("uni/uni.1024/texture_0" + i + ".png");
}
break;
}
}
live2DModel.textureFiles = new Texture2D[texture_filepath.Length];
for (int i = 0; i < texture_filepath.Length; i++)
{
if (string.IsNullOrEmpty(texture_filepath[i]))
continue;
Texture2D tex = new Texture2D(2, 2);
tex.LoadImage(File.ReadAllBytes(texture_filepath[i]));
live2DModel.textureFiles[i] = tex;
}
if (!string.IsNullOrEmpty(moc_filepath))
live2DModel.setMocFileFromBytes(File.ReadAllBytes(moc_filepath));
if (!string.IsNullOrEmpty(physics_filepath))
live2DModel.setPhysicsFileFromBytes(File.ReadAllBytes(physics_filepath));
if (!string.IsNullOrEmpty(pose_filepath))
live2DModel.setPoseFileFromBytes(File.ReadAllBytes(pose_filepath));
}
private void Run ()
{
Debug.Log ("Run");
faceLandmarkDetector = new FaceLandmarkDetector (shape_predictor_68_face_landmarks_dat_filepath);
frontalFaceParam = new FrontalFaceParam ();
// Use the front camera to Init
webCamTextureToMatHelper.Init(null, webCamTextureToMatHelper.requestWidth, webCamTextureToMatHelper.requestHeight, !webCamTextureToMatHelper.requestIsFrontFacing);
Default initialization
//webCamTextureToMatHelper.Init();
//webCamTextureToMatHelper.Init(null, 320, 240, false);
}
/// <summary>
/// Raises the web cam texture to mat helper inited event.
/// </summary>
public void OnWebCamTextureToMatHelperInited ()
{
Debug.Log ("OnWebCamTextureToMatHelperInited");
Mat webCamTextureMat = webCamTextureToMatHelper.GetMat ();
gameObject.transform.localScale = new Vector3 (webCamTextureMat.cols (), webCamTextureMat.rows (), 1);
Debug.Log ("Screen.width " + Screen.width + " Screen.height " + Screen.height + " Screen.orientation " + Screen.orientation);
float width = gameObject.transform.localScale.x;
float height = gameObject.transform.localScale.y;
float widthScale = (float)Screen.width / width;
float heightScale = (float)Screen.height / height;
if (widthScale < heightScale) {
Camera.main.orthographicSize = (width * (float)Screen.height / (float)Screen.width) / 2;
} else {
Camera.main.orthographicSize = height / 2;
}
if (live2DModel != null) {
// Set live2DModel localScale
live2DModel.transform.localScale = new Vector3 (Camera.main.orthographicSize * modelScale, Camera.main.orthographicSize * modelScale, 1);
}
}
/// <summary>
/// Raises the web cam texture to mat helper disposed event.
/// </summary>
public void OnWebCamTextureToMatHelperDisposed ()
{
Debug.Log ("OnWebCamTextureToMatHelperDisposed");
}
/// <summary>
/// Raises the web cam texture to mat helper error occurred event.
/// </summary>
public void OnWebCamTextureToMatHelperErrorOccurred ()
{
Debug.Log("OnWebCamTextureToMatHelperErrorOccurred");
}
// Update is called once per frame
void Update ()
{
if (Input.GetKeyDown(KeyCode.Escape))
{
#if UNITY_5_3 || UNITY_5_3_OR_NEWER
SceneManager.LoadScene("FacemojiStart");
#else
Application.LoadLevel("FacemojiStart");
#endif
}
if (webCamTextureToMatHelper.IsPlaying () && webCamTextureToMatHelper.DidUpdateThisFrame ()) {
Mat rgbaMat = webCamTextureToMatHelper.GetMat();
OpenCVForUnityUtils.SetImage(faceLandmarkDetector, rgbaMat);
//获得特征匹配矩形
List<UnityEngine.Rect> detectResult = faceLandmarkDetector.Detect ();
foreach (var rect in detectResult) {
//获得68个采样点
List<Vector2> points = faceLandmarkDetector.DetectLandmark (rect);
if (points.Count > 0) {
// angle
Vector3 angles = frontalFaceParam.getFrontalFaceAngle(points);
//初始状态
double[,] originState = X_K.Detail;
originState[0, 0] = angles.x;
originState[1, 0] = angles.y;
originState[2, 0] = angles.z;
originState[3, 0] = 0;
originState[4, 0] = 0;
originState[5, 0] = 0;
//eye
double[,] unit = eye.Detail;
unit[0, 0] = 1;
unit[0, 1] = 0;
unit[0, 2] = 0;
unit[0, 3] = 0;
unit[0, 4] = 0;
unit[0, 5] = 0;
unit[1, 0] = 0;
unit[1, 1] = 1;
unit[1, 2] = 0;
unit[1, 3] = 0;
unit[1, 4] = 0;
unit[1, 5] = 0;
unit[2, 0] = 0;
unit[2, 1] = 0;
unit[2, 2] = 1;
unit[2, 3] = 0;
unit[2, 4] = 0;
unit[2, 5] = 0;
unit[3, 0] = 0;
unit[3, 1] = 0;
unit[3, 2] = 0;
unit[3, 3] = 1;
unit[3, 4] = 0;
unit[3, 5] = 0;
unit[4, 0] = 0;
unit[4, 1] = 0;
unit[4, 2] = 0;
unit[4, 3] = 0;
unit[4, 4] = 1;
unit[4, 5] = 0;
unit[5, 0] = 0;
unit[5, 1] = 0;
unit[5, 2] = 0;
unit[5, 3] = 0;
unit[5, 4] = 0;
unit[5, 5] = 1;
//F_K（F_k）
double[,] stateC = F_K.Detail;
stateC[0, 0] = 1;
stateC[0, 1] = 0;
stateC[0, 2] = 0;
stateC[0, 3] = 1;
stateC[0, 4] = 0;
stateC[0, 5] = 0;
stateC[1, 0] = 0;
stateC[1, 1] = 1;
stateC[1, 2] = 0;
stateC[1, 3] = 0;
stateC[1, 4] = 1;
stateC[1, 5] = 0;
stateC[2, 0] = 0;
stateC[2, 1] = 0;
stateC[2, 2] = 1;
stateC[2, 3] = 0;
stateC[2, 4] = 0;
stateC[2, 5] = 1;
stateC[3, 0] = 0;
stateC[3, 1] = 0;
stateC[3, 2] = 0;
stateC[3, 3] = 1;
stateC[3, 4] = 0;
stateC[3, 5] = 0;
stateC[4, 0] = 0;
stateC[4, 1] = 0;
stateC[4, 2] = 0;
stateC[4, 3] = 0;
stateC[4, 4] = 1;
stateC[4, 5] = 0;
stateC[5, 0] = 0;
stateC[5, 1] = 0;
stateC[5, 2] = 0;
stateC[5, 3] = 0;
stateC[5, 4] = 0;
stateC[5, 5] = 1;
//P_K(P_k)
double[,] errCovPost = P_K.Detail;
errCovPost[0, 0] = 0.1;
errCovPost[0, 1] = 0;
errCovPost[0, 2] = 0;
errCovPost[0, 3] = 0;
errCovPost[0, 4] = 0;
errCovPost[0, 5] = 0;
errCovPost[1, 0] = 0;
errCovPost[1, 1] = 0.1;
errCovPost[1, 2] = 0;
errCovPost[1, 3] = 0;
errCovPost[1, 4] = 0;
errCovPost[1, 5] = 0;
errCovPost[2, 0] = 0;
errCovPost[2, 1] = 0;
errCovPost[2, 2] = 0.1;
errCovPost[2, 3] = 0;
errCovPost[2, 4] = 0;
errCovPost[2, 5] = 0;
errCovPost[3, 0] = 0;
errCovPost[3, 1] = 0;
errCovPost[3, 2] = 0;
errCovPost[3, 3] = 0.1;
errCovPost[3, 4] = 0;
errCovPost[3, 5] = 0;
errCovPost[4, 0] = 0;
errCovPost[4, 1] = 0;
errCovPost[4, 2] = 0;
errCovPost[4, 3] = 0;
errCovPost[4, 4] = 0.1;
errCovPost[4, 5] = 0;
errCovPost[5, 0] = 0;
errCovPost[5, 1] = 0;
errCovPost[5, 2] = 0;
errCovPost[5, 3] = 0;
errCovPost[5, 4] = 0;
errCovPost[5, 5] = 0.1;
//Debug.Log(MatrixOperator.MatrixPrint(P_K));
//H_K(H_K)
double[,] measureMat = H_K.Detail;
measureMat[0, 0] = 1;
measureMat[0, 1] = 0;
measureMat[0, 2] = 0;
measureMat[0, 3] = 0;
measureMat[0, 4] = 0;
measureMat[0, 5] = 0;
measureMat[1, 0] = 0;
measureMat[1, 1] = 1;
measureMat[1, 2] = 0;
measureMat[1, 3] = 0;
measureMat[1, 4] = 0;
measureMat[1, 5] = 0;
measureMat[2, 0] = 0;
measureMat[2, 1] = 0;
measureMat[2, 2] = 1;
measureMat[2, 3] = 0;
measureMat[2, 4] = 0;
measureMat[2, 5] = 0;
//Q_K(Q_K)
double[,] processNoiseCov = Q_K.Detail;
processNoiseCov[0, 0] = 1;
processNoiseCov[0, 1] = 0;
processNoiseCov[0, 2] = 0;
processNoiseCov[0, 3] = 0;
processNoiseCov[0, 4] = 0;
processNoiseCov[0, 5] = 0;
processNoiseCov[1, 0] = 0;
processNoiseCov[1, 1] = 1;
processNoiseCov[1, 2] = 0;
processNoiseCov[1, 3] = 0;
processNoiseCov[1, 4] = 0;
processNoiseCov[1, 5] = 0;
processNoiseCov[2, 0] = 0;
processNoiseCov[2, 1] = 0;
processNoiseCov[2, 2] = 1;
processNoiseCov[2, 3] = 0;
processNoiseCov[2, 4] = 0;
processNoiseCov[2, 5] = 0;
processNoiseCov[3, 0] = 0;
processNoiseCov[3, 1] = 0;
processNoiseCov[3, 2] = 0;
processNoiseCov[3, 3] = 1;
processNoiseCov[3, 4] = 0;
processNoiseCov[3, 5] = 0;
processNoiseCov[4, 0] = 0;
processNoiseCov[4, 1] = 0;
processNoiseCov[4, 2] = 0;
processNoiseCov[4, 3] = 0;
processNoiseCov[4, 4] = 1;
processNoiseCov[4, 5] = 0;
processNoiseCov[5, 0] = 0;
processNoiseCov[5, 1] = 0;
processNoiseCov[5, 2] = 0;
processNoiseCov[5, 3] = 0;
processNoiseCov[5, 4] = 0;
processNoiseCov[5, 5] = 1;
//R_K(R_K)
double[,] measureNoiseCov = R_K.Detail;
measureNoiseCov[0, 0] = 0.03;
measureNoiseCov[0, 1] = 0;
measureNoiseCov[0, 2] = 0;
measureNoiseCov[1, 0] = 0;
measureNoiseCov[1, 1] = 0.03;
measureNoiseCov[1, 2] = 0;
measureNoiseCov[2, 0] = 0;
measureNoiseCov[2, 1] = 0;
measureNoiseCov[2, 2] = 0.03;
double[] Arr = new double[] { angles.x, angles.y, angles.z};
//X_0 初始状态
double[,] measure = Z_K.Detail;
measure[0, 0] = Arr[0];
measure[1, 0] = Arr[1];
measure[2, 0] = Arr[2];
//首先计算推测值
Matrix X_front = MatrixOperator.MatrixMulti(F_K, X_K);
//然后计算推测值与真实值之间的误差协方差矩阵
Matrix P_front = MatrixOperator.MatrixMulti(MatrixOperator.MatrixMulti(MatrixOperator.MatrixMulti(F_K, P_K), MatrixOperator.MatrixTrans(F_K)), Q_K);
//再计算卡尔曼增益
Matrix Kg = MatrixOperator.MatrixMulti(MatrixOperator.MatrixMulti(P_front, MatrixOperator.MatrixTrans(H_K)), MatrixOperator.MatrixInv(MatrixOperator.MatrixAdd(MatrixOperator.MatrixMulti(MatrixOperator.MatrixMulti(H_K, P_front), MatrixOperator.MatrixTrans(H_K)), R_K)));
//再得到估计值
Matrix X_front_ = MatrixOperator.MatrixAdd(X_front, MatrixOperator.MatrixMulti(Kg, MatrixOperator.MatrixSub(Z_K, MatrixOperator.MatrixMulti(H_K, X_front))));
//最后计算估计值与真实值之间的误差协方差矩阵，为下次递推做准备
Matrix P_front_ = MatrixOperator.MatrixMulti(MatrixOperator.MatrixSub(eye, MatrixOperator.MatrixMulti(Kg, H_K)), P_front);
X_K = X_front_;
P_K = P_front_;
double[,] c = X_K.Detail;
double aa = c[0, 0];
double bb = c[1,0];
double cc = c[2, 0];
angles.x = (int)aa;
angles.y = (int)bb;
angles.z = (int)cc;
float rotateX = (angles.x > 180) ? angles.x - 360 : angles.x;
float rotateY = (angles.y > 180) ? angles.y - 360 : angles.y;
float rotateZ = (angles.z > 180) ? angles.z - 360 : angles.z;
// Coordinate transformation
//live2DModel.PARAM_ANGLE.Set(-rotateY, rotateX, -rotateZ);
live2DModel.PARAM_ANGLE.Set(rotateX, rotateY, rotateZ);
// Make sure your line of sight is always facing the camera
// eye_ball_X
live2DModel.PARAM_EYE_BALL_X = rotateX / 60f;
// eye_ball_Y
live2DModel.PARAM_EYE_BALL_Y = rotateY / 60f - 0.25f;
live2DModelUpdate (points);
break;
}
}
}
}
private void live2DModelUpdate (List<Vector2> points)
{
if (live2DModel != null) {
//live2DModel.PARAM_ANGLE.Set(-Roundf(rotateY, 0.5f), Roundf(rotateX, 0.5f), -Roundf(rotateZ, 0.5f));
// brow_L_Y
float brow_L_Y = getRaitoOfBROW_L_Y(points);
// Keep three decimal places to reduce the vibration
live2DModel.PARAM_BROW_L_Y = Roundf(brow_L_Y, 1000f);
//live2DModel.PARAM_BROW_L_Y = (float)Math.Round(brow_L_Y, 2);
// brow_R_Y
float brow_R_Y = getRaitoOfBROW_R_Y(points);
// Keep three decimal places to reduce the vibration
live2DModel.PARAM_BROW_R_Y = Roundf(brow_R_Y, 1000f);
//live2DModel.PARAM_BROW_R_Y = (float)Math.Round(brow_R_Y, 2);
// eye_open_L
float eyeOpen_L = getRaitoOfEyeOpen_L (points);
if (eyeOpen_L > 0.6f && eyeOpen_L < 1.1f)
eyeOpen_L = 1;
else if (eyeOpen_L >= 1.1f && brow_L_Y >= 0 )
eyeOpen_L = 2;
else if (eyeOpen_L <= 0.6f)
eyeOpen_L = 0;
live2DModel.PARAM_EYE_L_OPEN = eyeOpen_L;
// eye_open_R
float eyeOpen_R = getRaitoOfEyeOpen_R (points);
if (eyeOpen_R > 0.6f && eyeOpen_R < 1.1f)
eyeOpen_R = 1;
else if (eyeOpen_R >= 1.1f && brow_R_Y >= 0)
eyeOpen_R = 2;
else if (eyeOpen_R < 0.6f)
eyeOpen_R = 0;
live2DModel.PARAM_EYE_R_OPEN = eyeOpen_R;
// mouth_open
float mouthOpen = getRaitoOfMouthOpen_Y (points);
if (mouthOpen < 0.6f)
mouthOpen = 0;
live2DModel.PARAM_MOUTH_OPEN_Y = mouthOpen;
// mouth_size
float mouthSize = getRaitoOfMouthSize (points);
live2DModel.PARAM_MOUTH_SIZE = mouthSize;
}
}
// Keep decimal places to reduce the vibration
private float Roundf(float f, float multiple)
{
if (multiple == 0)
return f;
int i = (int)(f * multiple);
return i / multiple;
}
// Calculate the degree of eye opening
private float getRaitoOfEyeOpen_L (List<Vector2> points)
{
if (points.Count != 68)
throw new ArgumentNullException ("Invalid landmark_points.");
return Mathf.Clamp (Mathf.Abs (points [43].y - points [47].y) / (Mathf.Abs (points [43].x - points [44].x) * 0.75f), -0.1f, 2.0f);
}
private float getRaitoOfEyeOpen_R (List<Vector2> points)
{
if (points.Count != 68)
throw new ArgumentNullException ("Invalid landmark_points.");
return Mathf.Clamp (Mathf.Abs (points [38].y - points [40].y) / (Mathf.Abs (points [37].x - points [38].x) * 0.75f), -0.1f, 2.0f);
}
// Eyebrows move up and down
private float getRaitoOfBROW_L_Y (List<Vector2> points)
{
if (points.Count != 68)
throw new ArgumentNullException ("Invalid landmark_points.");
//float y = Mathf.Ceil(Mathf.Abs(points[24].y - points[27].y)) / Mathf.Abs (points [27].y - points [29].y);
float y = Mathf.Abs(points[24].y - points[27].y) / Mathf.Abs(points[27].y - points[29].y);
y -= 1;
y *= 4f;
return Mathf.Clamp (y, -1.0f, 1.0f);
}
private float getRaitoOfBROW_R_Y (List<Vector2> points)
{
if (points.Count != 68)
throw new ArgumentNullException ("Invalid landmark_points.");
//float y = Mathf.Ceil(Mathf.Abs(points[19].y - points[27].y)) / Mathf.Abs(points[27].y - points[29].y);
float y = Mathf.Abs(Mathf.Sqrt(Mathf.Pow(points[19].y - points[27].y,2)+Mathf.Pow(points[19].x - points[27].x,2))) / Mathf.Abs(Mathf.Sqrt(Mathf.Pow(points[27].y - points[29].y,2)+Mathf.Pow(points[27].x - points[29].x,2))) ;
Debug.Log("y是："+y);
y -= 1;
y *= 4f;
return Mathf.Clamp (y, -1.0f, 1.0f);
}
// Calculate the degree of mouth opening
private float getRaitoOfMouthOpen_Y (List<Vector2> points)
{
if (points.Count != 68)
throw new ArgumentNullException ("Invalid landmark_points.");
return (float)((points[57].y - points[51].y) / (points[64].x - points[60].x) - 0.2);
//return Mathf.Clamp01 (Mathf.Abs (points [62].y - points [66].y) / (Mathf.Abs (points [51].y - points [62].y) + Mathf.Abs (points [66].y - points [57].y)));
}
// Calculate the width of the mouth
private float getRaitoOfMouthSize (List<Vector2> points)
{
if (points.Count != 68)
throw new ArgumentNullException ("Invalid landmark_points.");
float size = Mathf.Abs (points [48].x - points [54].x) / (Mathf.Abs (points [31].x - points [35].x) * 1.8f);
size -= 1;
size *= 4f;
return Mathf.Clamp (size, -1.0f, 1.0f);
}
/// <summary>
/// Raises the disable event.
/// </summary>
void OnDisable ()
{
if(webCamTextureToMatHelper != null) webCamTextureToMatHelper.Dispose ();
if(faceLandmarkDetector != null) faceLandmarkDetector.Dispose ();
if(frontalFaceParam != null) frontalFaceParam.Dispose ();
}
public void OnBackButton()
{
#if UNITY_5_3 || UNITY_5_3_OR_NEWER
SceneManager.LoadScene("FacemojiStart");
#else
Application.LoadLevel("FacemojiStart");
#endif
}
/// <summary>
/// Raises the change camera button event.
/// </summary>
public void OnChangeCameraButton ()
{
webCamTextureToMatHelper.Init (null, webCamTextureToMatHelper.requestWidth, webCamTextureToMatHelper.requestHeight, !webCamTextureToMatHelper.requestIsFrontFacing);
}
public void OnStartButton()
{
startBtn.SetActive(false);
if (!Record.isRecording)
{
//Start recording
Record.isRecording = true;
finishBtn.SetActive(true);
}
}
public void OnFinishButton()
{
finishBtn.SetActive(false);
if (Record.isRecording)
{
//Finish recording
Record.isRecording = false;
startBtn.SetActive(true);
}
}
public void OnShizukuButton()
{
if(selectedTexture != WhichTexture.noir_santa)
{
selectedTexture = WhichTexture.noir_santa;
LoadTexture();
}
}
public void OnHaruButton()
{
if (selectedTexture != WhichTexture.uni)
{
selectedTexture = WhichTexture.uni;
LoadTexture();
}
}
}
}
143
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
