当前位置:   article > 正文

NO.48------ C#实现卡尔曼滤波_c# 卡尔曼滤波

c# 卡尔曼滤波

通俗的讲,卡尔曼滤波就是综合上一步骤对当前步骤的预测值,结合当前步骤的测量值,来估计当前的状态。

       最近在做一个Unity项目时,用到了卡尔曼滤波消除抖动,主要是消除头部抖动,从Dlib实时获得68个特征值的points,由points.x points.y  points.z  获得头部的旋转四元数作为测量值,初始值可以任意定义,因为几次迭代后预测值就逼近真实值了。因为Unity用c#编写,c#本身不存在矩阵库,对矩阵运算比较吃力,需要自己导入矩阵库,方法很简单,首先定义矩阵库的命名空间,然后使用该命名空间即可。

矩阵库:

  1. using System;
  2. using System.Collections.Generic;
  3. using System.Linq;
  4. using System.Text;
  5. namespace Slash.Lastwork
  6. {
  7. public class Matrix
  8. {
  9. double[,] A;
  10. //m行n列
  11. int m, n;
  12. string name;
  13. public Matrix(int am, int an)
  14. {
  15. m = am;
  16. n = an;
  17. A = new double[m, n];
  18. name = "Result";
  19. }
  20. public Matrix(int am, int an, string aName)
  21. {
  22. m = am;
  23. n = an;
  24. A = new double[m, n];
  25. name = aName;
  26. }
  27. public int getM
  28. {
  29. get { return m; }
  30. }
  31. public int getN
  32. {
  33. get { return n; }
  34. }
  35. public double[,] Detail
  36. {
  37. get { return A; }
  38. set { A = value; }
  39. }
  40. public string Name
  41. {
  42. get { return name; }
  43. set { name = value; }
  44. }
  45. }
  46. public class MatrixOperator
  47. {
  48. MatrixOperator()
  49. { }
  50. /// <summary>
  51. /// 矩阵加法
  52. /// </summary>
  53. /// <param name="Ma"></param>
  54. /// <param name="Mb"></param>
  55. /// <returns></returns>
  56. public static Matrix MatrixAdd(Matrix Ma, Matrix Mb)
  57. {
  58. int m = Ma.getM;
  59. int n = Ma.getN;
  60. int m2 = Mb.getM;
  61. int n2 = Mb.getN;
  62. if ((m != m2) || (n != n2))
  63. {
  64. Exception myException = new Exception("数组维数不匹配");
  65. throw myException;
  66. }
  67. Matrix Mc = new Matrix(m, n);
  68. double[,] c = Mc.Detail;
  69. double[,] a = Ma.Detail;
  70. double[,] b = Mb.Detail;
  71. int i, j;
  72. for (i = 0; i < m; i++)
  73. for (j = 0; j < n; j++)
  74. c[i, j] = a[i, j] + b[i, j];
  75. return Mc;
  76. }
  77. /// <summary>
  78. /// 矩阵减法
  79. /// </summary>
  80. /// <param name="Ma"></param>
  81. /// <param name="Mb"></param>
  82. /// <returns></returns>
  83. public static Matrix MatrixSub(Matrix Ma, Matrix Mb)
  84. {
  85. int m = Ma.getM;
  86. int n = Ma.getN;
  87. int m2 = Mb.getM;
  88. int n2 = Mb.getN;
  89. if ((m != m2) || (n != n2))
  90. {
  91. Exception myException = new Exception("数组维数不匹配");
  92. throw myException;
  93. }
  94. Matrix Mc = new Matrix(m, n);
  95. double[,] c = Mc.Detail;
  96. double[,] a = Ma.Detail;
  97. double[,] b = Mb.Detail;
  98. int i, j;
  99. for (i = 0; i < m; i++)
  100. for (j = 0; j < n; j++)
  101. c[i, j] = a[i, j] - b[i, j];
  102. return Mc;
  103. }
  104. /// <summary>
  105. /// 矩阵打印
  106. /// </summary>
  107. /// <param name="Ma"></param>
  108. /// <returns></returns>
  109. public static string MatrixPrint(Matrix Ma)
  110. {
  111. string s;
  112. s = Ma.Name + ":\n";
  113. int m = Ma.getM;
  114. int n = Ma.getN;
  115. double[,] a = Ma.Detail;
  116. for (int i = 0; i < m; i++)
  117. {
  118. for (int j = 0; j < n; j++)
  119. {
  120. s += a[i, j].ToString("0.0000") + "\t";
  121. }
  122. s += "\n";
  123. }
  124. return s;
  125. }
  126. /// <summary>
  127. /// 矩阵乘法
  128. /// </summary>
  129. /// <param name="Ma"></param>
  130. /// <param name="Mb"></param>
  131. /// <returns></returns>
  132. public static Matrix MatrixMulti(Matrix Ma, Matrix Mb)
  133. {
  134. int m = Ma.getM;
  135. int n = Ma.getN;
  136. int m2 = Mb.getM;
  137. int n2 = Mb.getN;
  138. if (n != m2)
  139. {
  140. Exception myException = new Exception("数组维数不匹配");
  141. throw myException;
  142. }
  143. Matrix Mc = new Matrix(m, n2);
  144. double[,] c = Mc.Detail;
  145. double[,] a = Ma.Detail;
  146. double[,] b = Mb.Detail;
  147. int i, j, k;
  148. for (i = 0; i < m; i++)
  149. for (j = 0; j < n2; j++)
  150. {
  151. c[i, j] = 0;
  152. for (k = 0; k < n; k++)
  153. c[i, j] += a[i, k] * b[k, j];
  154. }
  155. return Mc;
  156. }
  157. /// <summary>
  158. /// 矩阵数乘
  159. /// </summary>
  160. /// <param name="k"></param>
  161. /// <param name="Ma"></param>
  162. /// <returns></returns>
  163. public static Matrix MatrixSimpleMulti(double k, Matrix Ma)
  164. {
  165. int n = Ma.getN;
  166. int m = Ma.getM;
  167. Matrix Mc = new Matrix(m, n);
  168. double[,] c = Mc.Detail;
  169. double[,] a = Ma.Detail;
  170. int i, j;
  171. for (i = 0; i < m; i++)
  172. for (j = 0; j < n; j++)
  173. c[i, j] = a[i, j] * k;
  174. return Mc;
  175. }
  176. /// <summary>
  177. /// 矩阵转置
  178. /// </summary>
  179. /// <param name="Ma"></param>
  180. /// <param name="Mb"></param>
  181. /// <returns></returns>
  182. public static Matrix MatrixTrans(Matrix Ma)
  183. {
  184. int m = Ma.getM;
  185. int n = Ma.getN;
  186. Matrix Mc = new Matrix(n, m);
  187. double[,] c = Mc.Detail;
  188. double[,] a = Ma.Detail;
  189. for (int i = 0; i < n; i++)
  190. for (int j = 0; j < m; j++)
  191. c[i, j] = a[j, i];
  192. return Mc;
  193. }
  194. /// <summary>
  195. /// 矩阵求逆(高斯法)
  196. /// </summary>
  197. /// <param name="Ma"></param>
  198. /// <returns></returns>
  199. public static Matrix MatrixInv(Matrix Ma)
  200. {
  201. int m = Ma.getM;
  202. int n = Ma.getN;
  203. if (m != n)
  204. {
  205. Exception myException = new Exception("数组维数不匹配");
  206. throw myException;
  207. }
  208. Matrix Mc = new Matrix(m, n);
  209. double[,] a0 = Ma.Detail;
  210. double[,] a = (double[,])a0.Clone();
  211. double[,] b = Mc.Detail;
  212. int i, j, row, k;
  213. double max, temp;
  214. //单位矩阵
  215. for (i = 0; i < n; i++)
  216. {
  217. b[i, i] = 1;
  218. }
  219. for (k = 0; k < n; k++)
  220. {
  221. max = 0; row = k;
  222. //找最大元,其所在行为row
  223. for (i = k; i < n; i++)
  224. {
  225. temp = Math.Abs(a[i, k]);
  226. if (max < temp)
  227. {
  228. max = temp;
  229. row = i;
  230. }
  231. }
  232. if (max == 0)
  233. {
  234. Exception myException = new Exception("没有逆矩阵");
  235. throw myException;
  236. }
  237. //交换k与row行
  238. if (row != k)
  239. {
  240. for (j = 0; j < n; j++)
  241. {
  242. temp = a[row, j];
  243. a[row, j] = a[k, j];
  244. a[k, j] = temp;
  245. temp = b[row, j];
  246. b[row, j] = b[k, j];
  247. b[k, j] = temp;
  248. }
  249. }
  250. //首元化为1
  251. for (j = k + 1; j < n; j++) a[k, j] /= a[k, k];
  252. for (j = 0; j < n; j++) b[k, j] /= a[k, k];
  253. a[k, k] = 1;
  254. //k列化为0
  255. //对a
  256. for (j = k + 1; j < n; j++)
  257. {
  258. for (i = 0; i < k; i++) a[i, j] -= a[i, k] * a[k, j];
  259. for (i = k + 1; i < n; i++) a[i, j] -= a[i, k] * a[k, j];
  260. }
  261. //对b
  262. for (j = 0; j < n; j++)
  263. {
  264. for (i = 0; i < k; i++) b[i, j] -= a[i, k] * b[k, j];
  265. for (i = k + 1; i < n; i++) b[i, j] -= a[i, k] * b[k, j];
  266. }
  267. for (i = 0; i < n; i++) a[i, k] = 0;
  268. a[k, k] = 1;
  269. }
  270. return Mc;
  271. }
  272. /// <summary>
  273. /// 矩阵求逆(伴随矩阵法)
  274. /// </summary>
  275. /// <param name="Ma"></param>
  276. /// <returns></returns>
  277. public static Matrix MatrixInvByCom(Matrix Ma)
  278. {
  279. double d = MatrixOperator.MatrixDet(Ma);
  280. if (d == 0)
  281. {
  282. Exception myException = new Exception("没有逆矩阵");
  283. throw myException;
  284. }
  285. Matrix Ax = MatrixOperator.MatrixCom(Ma);
  286. Matrix An = MatrixOperator.MatrixSimpleMulti((1.0 / d), Ax);
  287. return An;
  288. }
  289. /// <summary>
  290. /// 对应行列式的代数余子式矩阵
  291. /// </summary>
  292. /// <param name="Ma"></param>
  293. /// <returns></returns>
  294. public static Matrix MatrixSpa(Matrix Ma, int ai, int aj)
  295. {
  296. int m = Ma.getM;
  297. int n = Ma.getN;
  298. if (m != n)
  299. {
  300. Exception myException = new Exception("数组维数不匹配");
  301. throw myException;
  302. }
  303. int n2 = n - 1;
  304. Matrix Mc = new Matrix(n2, n2);
  305. double[,] a = Ma.Detail;
  306. double[,] b = Mc.Detail;
  307. //左上
  308. for (int i = 0; i < ai; i++)
  309. for (int j = 0; j < aj; j++)
  310. {
  311. b[i, j] = a[i, j];
  312. }
  313. //右下
  314. for (int i = ai; i < n2; i++)
  315. for (int j = aj; j < n2; j++)
  316. {
  317. b[i, j] = a[i + 1, j + 1];
  318. }
  319. //右上
  320. for (int i = 0; i < ai; i++)
  321. for (int j = aj; j < n2; j++)
  322. {
  323. b[i, j] = a[i, j + 1];
  324. }
  325. //左下
  326. for (int i = ai; i < n2; i++)
  327. for (int j = 0; j < aj; j++)
  328. {
  329. b[i, j] = a[i + 1, j];
  330. }
  331. //符号位
  332. if ((ai + aj) % 2 != 0)
  333. {
  334. for (int i = 0; i < n2; i++)
  335. b[i, 0] = -b[i, 0];
  336. }
  337. return Mc;
  338. }
  339. /// <summary>
  340. /// 矩阵的行列式
  341. /// </summary>
  342. /// <param name="Ma"></param>
  343. /// <returns></returns>
  344. public static double MatrixDet(Matrix Ma)
  345. {
  346. int m = Ma.getM;
  347. int n = Ma.getN;
  348. if (m != n)
  349. {
  350. Exception myException = new Exception("数组维数不匹配");
  351. throw myException;
  352. }
  353. double[,] a = Ma.Detail;
  354. if (n == 1) return a[0, 0];
  355. double D = 0;
  356. for (int i = 0; i < n; i++)
  357. {
  358. D += a[1, i] * MatrixDet(MatrixSpa(Ma, 1, i));
  359. }
  360. return D;
  361. }
  362. /// <summary>
  363. /// 矩阵的伴随矩阵
  364. /// </summary>
  365. /// <param name="Ma"></param>
  366. /// <returns></returns>
  367. public static Matrix MatrixCom(Matrix Ma)
  368. {
  369. int m = Ma.getM;
  370. int n = Ma.getN;
  371. Matrix Mc = new Matrix(m, n);
  372. double[,] c = Mc.Detail;
  373. double[,] a = Ma.Detail;
  374. for (int i = 0; i < m; i++)
  375. for (int j = 0; j < n; j++)
  376. c[i, j] = MatrixDet(MatrixSpa(Ma, j, i));
  377. return Mc;
  378. }
  379. }
  380. }

迭代过程:

  1. using UnityEngine;
  2. using System.Collections.Generic;
  3. using System;
  4. using System.IO;
  5. using Slash.Lastwork;
  6. #if UNITY_5_3 || UNITY_5_3_OR_NEWER
  7. using UnityEngine.SceneManagement;
  8. #endif
  9. using OpenCVForUnity;
  10. using DlibFaceLandmarkDetector;
  11. using DlibFaceLandmarkDetectorSample;
  12. using Moments;
  13. namespace Slash.Facemoji
  14. {
  15. /// <summary>
  16. /// FaceTracking, Use Dlib to detect face landmark and use Live2D model to track faces
  17. /// </summary>
  18. [AddComponentMenu("Facemoji/FaceTracking")]
  19. [RequireComponent (typeof(WebCamTextureToMatHelper))]
  20. public class FaceTracking : MonoBehaviour
  21. {
  22. ///矩阵的运算
  23. //F_K 状态转移矩阵
  24. Matrix stateConvert = new Matrix(6, 6, "stateConvert");
  25. //P_K 后验估计误差协方差矩阵,度量估计值的精确程度
  26. Matrix errorCovPost = new Matrix(6, 6, "errorCovPsot");
  27. //H_K 观测矩阵
  28. Matrix measureMatrix = new Matrix(6, 3, "measureMatrix");
  29. //Q_K 过程噪声
  30. Matrix processNoiseConvert = new Matrix(6, 6, "processNoiseConvert");
  31. //R_K 观测噪声
  32. Matrix measureNoiseConvert = new Matrix(3, 3, "measureNoiseConvert");
  33. //X_0 初始状态
  34. Matrix originState = new Matrix(3, 1, "originState");
  35. /// <summary>
  36. /// All Live2D Texture
  37. /// </summary>
  38. private enum WhichTexture
  39. {
  40. noir_santa = 1,
  41. uni = 2,
  42. };
  43. /// <summary>
  44. /// The selected Live2D Texture now.(Default shizuku)
  45. /// </summary>
  46. private WhichTexture selectedTexture = WhichTexture.uni;
  47. /// <summary>
  48. /// Start Button
  49. /// </summary>
  50. public static GameObject startBtn;
  51. /// <summary>
  52. /// Finish Button
  53. /// </summary>
  54. public static GameObject finishBtn;
  55. /// <summary>
  56. /// live2DModel.transform.localScale
  57. /// </summary>
  58. public float modelScale = 0.9f;
  59. /// <summary>
  60. /// The web cam texture to mat helper.
  61. /// </summary>
  62. WebCamTextureToMatHelper webCamTextureToMatHelper;
  63. /// <summary>
  64. /// The face landmark detector.
  65. /// </summary>
  66. FaceLandmarkDetector faceLandmarkDetector;
  67. /// <summary>
  68. /// The live2DModel.
  69. /// </summary>
  70. public Live2DModel live2DModel;
  71. /// <summary>
  72. /// The frontal face parameter.
  73. /// </summary>
  74. FrontalFaceParam frontalFaceParam;
  75. /// <summary>
  76. /// The shape_predictor_68_face_landmarks_dat_filepath.
  77. /// </summary>
  78. private string shape_predictor_68_face_landmarks_dat_filepath;
  79. /// <summary>
  80. /// Model file path.
  81. /// </summary>
  82. private string moc_filepath;
  83. private string physics_filepath;
  84. private string pose_filepath;
  85. private string[] texture_filepath = new string[6];
  86. /// <summary>
  87. /// 相关矩阵的计算.
  88. /// </summary>
  89. //F_K 状态转移矩阵
  90. Matrix F_K = new Matrix(6, 6, "F_K");
  91. //P_K 后验估计误差协方差矩阵,度量估计值的精确程度
  92. Matrix P_K = new Matrix(6, 6, "errorCovPsot");
  93. //P_k+1
  94. Matrix P_front = new Matrix(6, 6, "errorCovPsot");
  95. //H_K 观测矩阵
  96. Matrix H_K = new Matrix(3, 6, "H_K");
  97. //Q_K 过程噪声
  98. Matrix Q_K = new Matrix(6, 6, "Q_K");
  99. //R_K 观测噪声
  100. Matrix R_K = new Matrix(3, 3, "R_K");
  101. //Z_K 测量值
  102. Matrix Z_K = new Matrix(3, 1, "Z_K");
  103. //X_K
  104. Matrix X_K = new Matrix(6, 1, "state");
  105. //X_K+1
  106. Matrix X_front = new Matrix(6, 1, "state");
  107. //eye
  108. Matrix eye = new Matrix(6, 6, "state");
  109. // Use this for initialization
  110. void Start()
  111. {
  112. startBtn = GameObject.Find("StartButton");
  113. finishBtn = GameObject.Find("FinishButton");
  114. startBtn.SetActive(true);
  115. finishBtn.SetActive(false);
  116. webCamTextureToMatHelper = gameObject.GetComponent<WebCamTextureToMatHelper>();
  117. #if UNITY_WEBGL && !UNITY_EDITOR
  118. webCamTextureToMatHelper.flipHorizontal = true;
  119. StartCoroutine(getFilePathCoroutine());
  120. #else
  121. // FaceLandmark model filepath
  122. shape_predictor_68_face_landmarks_dat_filepath = DlibFaceLandmarkDetector.Utils.getFilePath("shape_predictor_68_face_landmarks.dat");
  123. // Load Texture filepath
  124. LoadTexture();
  125. Run();
  126. #endif
  127. }
  128. private void LoadTexture()
  129. {
  130. // Load model filepath
  131. switch (selectedTexture)
  132. {
  133. case WhichTexture.noir_santa:
  134. {
  135. moc_filepath = OpenCVForUnity.Utils.getFilePath("noir_santa/noir_santa.moc.bytes");
  136. physics_filepath = OpenCVForUnity.Utils.getFilePath("noir_santa/noir_santa.physics.json");
  137. pose_filepath = OpenCVForUnity.Utils.getFilePath("noir_santa/noir_santa.pose.json");
  138. for (int i = 0; i < texture_filepath.Length; i++)
  139. {
  140. texture_filepath[i] = OpenCVForUnity.Utils.getFilePath("noir_santa/noir_santa.1024/texture_0" + i + ".png");
  141. }
  142. break;
  143. }
  144. case WhichTexture.uni:
  145. {
  146. moc_filepath = OpenCVForUnity.Utils.getFilePath("uni/uni.moc.bytes");
  147. physics_filepath = OpenCVForUnity.Utils.getFilePath("uni/uni.physics.json");
  148. pose_filepath = OpenCVForUnity.Utils.getFilePath("uni/uni.pose.json");
  149. for (int i = 0; i < texture_filepath.Length; i++)
  150. {
  151. texture_filepath[i] = OpenCVForUnity.Utils.getFilePath("uni/uni.1024/texture_0" + i + ".png");
  152. }
  153. break;
  154. }
  155. default:
  156. {
  157. moc_filepath = OpenCVForUnity.Utils.getFilePath("uni/uni.moc.bytes");
  158. physics_filepath = OpenCVForUnity.Utils.getFilePath("uni/uni.physics.json");
  159. pose_filepath = OpenCVForUnity.Utils.getFilePath("uni/uni.pose.json");
  160. for (int i = 0; i < texture_filepath.Length; i++)
  161. {
  162. texture_filepath[i] = OpenCVForUnity.Utils.getFilePath("uni/uni.1024/texture_0" + i + ".png");
  163. }
  164. break;
  165. }
  166. }
  167. live2DModel.textureFiles = new Texture2D[texture_filepath.Length];
  168. for (int i = 0; i < texture_filepath.Length; i++)
  169. {
  170. if (string.IsNullOrEmpty(texture_filepath[i]))
  171. continue;
  172. Texture2D tex = new Texture2D(2, 2);
  173. tex.LoadImage(File.ReadAllBytes(texture_filepath[i]));
  174. live2DModel.textureFiles[i] = tex;
  175. }
  176. if (!string.IsNullOrEmpty(moc_filepath))
  177. live2DModel.setMocFileFromBytes(File.ReadAllBytes(moc_filepath));
  178. if (!string.IsNullOrEmpty(physics_filepath))
  179. live2DModel.setPhysicsFileFromBytes(File.ReadAllBytes(physics_filepath));
  180. if (!string.IsNullOrEmpty(pose_filepath))
  181. live2DModel.setPoseFileFromBytes(File.ReadAllBytes(pose_filepath));
  182. }
  183. private void Run ()
  184. {
  185. Debug.Log ("Run");
  186. faceLandmarkDetector = new FaceLandmarkDetector (shape_predictor_68_face_landmarks_dat_filepath);
  187. frontalFaceParam = new FrontalFaceParam ();
  188. // Use the front camera to Init
  189. webCamTextureToMatHelper.Init(null, webCamTextureToMatHelper.requestWidth, webCamTextureToMatHelper.requestHeight, !webCamTextureToMatHelper.requestIsFrontFacing);
  190. Default initialization
  191. //webCamTextureToMatHelper.Init();
  192. //webCamTextureToMatHelper.Init(null, 320, 240, false);
  193. }
  194. /// <summary>
  195. /// Raises the web cam texture to mat helper inited event.
  196. /// </summary>
  197. public void OnWebCamTextureToMatHelperInited ()
  198. {
  199. Debug.Log ("OnWebCamTextureToMatHelperInited");
  200. Mat webCamTextureMat = webCamTextureToMatHelper.GetMat ();
  201. gameObject.transform.localScale = new Vector3 (webCamTextureMat.cols (), webCamTextureMat.rows (), 1);
  202. Debug.Log ("Screen.width " + Screen.width + " Screen.height " + Screen.height + " Screen.orientation " + Screen.orientation);
  203. float width = gameObject.transform.localScale.x;
  204. float height = gameObject.transform.localScale.y;
  205. float widthScale = (float)Screen.width / width;
  206. float heightScale = (float)Screen.height / height;
  207. if (widthScale < heightScale) {
  208. Camera.main.orthographicSize = (width * (float)Screen.height / (float)Screen.width) / 2;
  209. } else {
  210. Camera.main.orthographicSize = height / 2;
  211. }
  212. if (live2DModel != null) {
  213. // Set live2DModel localScale
  214. live2DModel.transform.localScale = new Vector3 (Camera.main.orthographicSize * modelScale, Camera.main.orthographicSize * modelScale, 1);
  215. }
  216. }
  217. /// <summary>
  218. /// Raises the web cam texture to mat helper disposed event.
  219. /// </summary>
  220. public void OnWebCamTextureToMatHelperDisposed ()
  221. {
  222. Debug.Log ("OnWebCamTextureToMatHelperDisposed");
  223. }
  224. /// <summary>
  225. /// Raises the web cam texture to mat helper error occurred event.
  226. /// </summary>
  227. public void OnWebCamTextureToMatHelperErrorOccurred ()
  228. {
  229. Debug.Log("OnWebCamTextureToMatHelperErrorOccurred");
  230. }
  231. // Update is called once per frame
  232. void Update ()
  233. {
  234. if (Input.GetKeyDown(KeyCode.Escape))
  235. {
  236. #if UNITY_5_3 || UNITY_5_3_OR_NEWER
  237. SceneManager.LoadScene("FacemojiStart");
  238. #else
  239. Application.LoadLevel("FacemojiStart");
  240. #endif
  241. }
  242. if (webCamTextureToMatHelper.IsPlaying () && webCamTextureToMatHelper.DidUpdateThisFrame ()) {
  243. Mat rgbaMat = webCamTextureToMatHelper.GetMat();
  244. OpenCVForUnityUtils.SetImage(faceLandmarkDetector, rgbaMat);
  245. //获得特征匹配矩形
  246. List<UnityEngine.Rect> detectResult = faceLandmarkDetector.Detect ();
  247. foreach (var rect in detectResult) {
  248. //获得68个采样点
  249. List<Vector2> points = faceLandmarkDetector.DetectLandmark (rect);
  250. if (points.Count > 0) {
  251. // angle
  252. Vector3 angles = frontalFaceParam.getFrontalFaceAngle(points);
  253. //初始状态
  254. double[,] originState = X_K.Detail;
  255. originState[0, 0] = angles.x;
  256. originState[1, 0] = angles.y;
  257. originState[2, 0] = angles.z;
  258. originState[3, 0] = 0;
  259. originState[4, 0] = 0;
  260. originState[5, 0] = 0;
  261. //eye
  262. double[,] unit = eye.Detail;
  263. unit[0, 0] = 1;
  264. unit[0, 1] = 0;
  265. unit[0, 2] = 0;
  266. unit[0, 3] = 0;
  267. unit[0, 4] = 0;
  268. unit[0, 5] = 0;
  269. unit[1, 0] = 0;
  270. unit[1, 1] = 1;
  271. unit[1, 2] = 0;
  272. unit[1, 3] = 0;
  273. unit[1, 4] = 0;
  274. unit[1, 5] = 0;
  275. unit[2, 0] = 0;
  276. unit[2, 1] = 0;
  277. unit[2, 2] = 1;
  278. unit[2, 3] = 0;
  279. unit[2, 4] = 0;
  280. unit[2, 5] = 0;
  281. unit[3, 0] = 0;
  282. unit[3, 1] = 0;
  283. unit[3, 2] = 0;
  284. unit[3, 3] = 1;
  285. unit[3, 4] = 0;
  286. unit[3, 5] = 0;
  287. unit[4, 0] = 0;
  288. unit[4, 1] = 0;
  289. unit[4, 2] = 0;
  290. unit[4, 3] = 0;
  291. unit[4, 4] = 1;
  292. unit[4, 5] = 0;
  293. unit[5, 0] = 0;
  294. unit[5, 1] = 0;
  295. unit[5, 2] = 0;
  296. unit[5, 3] = 0;
  297. unit[5, 4] = 0;
  298. unit[5, 5] = 1;
  299. //F_K(F_k)
  300. double[,] stateC = F_K.Detail;
  301. stateC[0, 0] = 1;
  302. stateC[0, 1] = 0;
  303. stateC[0, 2] = 0;
  304. stateC[0, 3] = 1;
  305. stateC[0, 4] = 0;
  306. stateC[0, 5] = 0;
  307. stateC[1, 0] = 0;
  308. stateC[1, 1] = 1;
  309. stateC[1, 2] = 0;
  310. stateC[1, 3] = 0;
  311. stateC[1, 4] = 1;
  312. stateC[1, 5] = 0;
  313. stateC[2, 0] = 0;
  314. stateC[2, 1] = 0;
  315. stateC[2, 2] = 1;
  316. stateC[2, 3] = 0;
  317. stateC[2, 4] = 0;
  318. stateC[2, 5] = 1;
  319. stateC[3, 0] = 0;
  320. stateC[3, 1] = 0;
  321. stateC[3, 2] = 0;
  322. stateC[3, 3] = 1;
  323. stateC[3, 4] = 0;
  324. stateC[3, 5] = 0;
  325. stateC[4, 0] = 0;
  326. stateC[4, 1] = 0;
  327. stateC[4, 2] = 0;
  328. stateC[4, 3] = 0;
  329. stateC[4, 4] = 1;
  330. stateC[4, 5] = 0;
  331. stateC[5, 0] = 0;
  332. stateC[5, 1] = 0;
  333. stateC[5, 2] = 0;
  334. stateC[5, 3] = 0;
  335. stateC[5, 4] = 0;
  336. stateC[5, 5] = 1;
  337. //P_K(P_k)
  338. double[,] errCovPost = P_K.Detail;
  339. errCovPost[0, 0] = 0.1;
  340. errCovPost[0, 1] = 0;
  341. errCovPost[0, 2] = 0;
  342. errCovPost[0, 3] = 0;
  343. errCovPost[0, 4] = 0;
  344. errCovPost[0, 5] = 0;
  345. errCovPost[1, 0] = 0;
  346. errCovPost[1, 1] = 0.1;
  347. errCovPost[1, 2] = 0;
  348. errCovPost[1, 3] = 0;
  349. errCovPost[1, 4] = 0;
  350. errCovPost[1, 5] = 0;
  351. errCovPost[2, 0] = 0;
  352. errCovPost[2, 1] = 0;
  353. errCovPost[2, 2] = 0.1;
  354. errCovPost[2, 3] = 0;
  355. errCovPost[2, 4] = 0;
  356. errCovPost[2, 5] = 0;
  357. errCovPost[3, 0] = 0;
  358. errCovPost[3, 1] = 0;
  359. errCovPost[3, 2] = 0;
  360. errCovPost[3, 3] = 0.1;
  361. errCovPost[3, 4] = 0;
  362. errCovPost[3, 5] = 0;
  363. errCovPost[4, 0] = 0;
  364. errCovPost[4, 1] = 0;
  365. errCovPost[4, 2] = 0;
  366. errCovPost[4, 3] = 0;
  367. errCovPost[4, 4] = 0.1;
  368. errCovPost[4, 5] = 0;
  369. errCovPost[5, 0] = 0;
  370. errCovPost[5, 1] = 0;
  371. errCovPost[5, 2] = 0;
  372. errCovPost[5, 3] = 0;
  373. errCovPost[5, 4] = 0;
  374. errCovPost[5, 5] = 0.1;
  375. //Debug.Log(MatrixOperator.MatrixPrint(P_K));
  376. //H_K(H_K)
  377. double[,] measureMat = H_K.Detail;
  378. measureMat[0, 0] = 1;
  379. measureMat[0, 1] = 0;
  380. measureMat[0, 2] = 0;
  381. measureMat[0, 3] = 0;
  382. measureMat[0, 4] = 0;
  383. measureMat[0, 5] = 0;
  384. measureMat[1, 0] = 0;
  385. measureMat[1, 1] = 1;
  386. measureMat[1, 2] = 0;
  387. measureMat[1, 3] = 0;
  388. measureMat[1, 4] = 0;
  389. measureMat[1, 5] = 0;
  390. measureMat[2, 0] = 0;
  391. measureMat[2, 1] = 0;
  392. measureMat[2, 2] = 1;
  393. measureMat[2, 3] = 0;
  394. measureMat[2, 4] = 0;
  395. measureMat[2, 5] = 0;
  396. //Q_K(Q_K)
  397. double[,] processNoiseCov = Q_K.Detail;
  398. processNoiseCov[0, 0] = 1;
  399. processNoiseCov[0, 1] = 0;
  400. processNoiseCov[0, 2] = 0;
  401. processNoiseCov[0, 3] = 0;
  402. processNoiseCov[0, 4] = 0;
  403. processNoiseCov[0, 5] = 0;
  404. processNoiseCov[1, 0] = 0;
  405. processNoiseCov[1, 1] = 1;
  406. processNoiseCov[1, 2] = 0;
  407. processNoiseCov[1, 3] = 0;
  408. processNoiseCov[1, 4] = 0;
  409. processNoiseCov[1, 5] = 0;
  410. processNoiseCov[2, 0] = 0;
  411. processNoiseCov[2, 1] = 0;
  412. processNoiseCov[2, 2] = 1;
  413. processNoiseCov[2, 3] = 0;
  414. processNoiseCov[2, 4] = 0;
  415. processNoiseCov[2, 5] = 0;
  416. processNoiseCov[3, 0] = 0;
  417. processNoiseCov[3, 1] = 0;
  418. processNoiseCov[3, 2] = 0;
  419. processNoiseCov[3, 3] = 1;
  420. processNoiseCov[3, 4] = 0;
  421. processNoiseCov[3, 5] = 0;
  422. processNoiseCov[4, 0] = 0;
  423. processNoiseCov[4, 1] = 0;
  424. processNoiseCov[4, 2] = 0;
  425. processNoiseCov[4, 3] = 0;
  426. processNoiseCov[4, 4] = 1;
  427. processNoiseCov[4, 5] = 0;
  428. processNoiseCov[5, 0] = 0;
  429. processNoiseCov[5, 1] = 0;
  430. processNoiseCov[5, 2] = 0;
  431. processNoiseCov[5, 3] = 0;
  432. processNoiseCov[5, 4] = 0;
  433. processNoiseCov[5, 5] = 1;
  434. //R_K(R_K)
  435. double[,] measureNoiseCov = R_K.Detail;
  436. measureNoiseCov[0, 0] = 0.03;
  437. measureNoiseCov[0, 1] = 0;
  438. measureNoiseCov[0, 2] = 0;
  439. measureNoiseCov[1, 0] = 0;
  440. measureNoiseCov[1, 1] = 0.03;
  441. measureNoiseCov[1, 2] = 0;
  442. measureNoiseCov[2, 0] = 0;
  443. measureNoiseCov[2, 1] = 0;
  444. measureNoiseCov[2, 2] = 0.03;
  445. double[] Arr = new double[] { angles.x, angles.y, angles.z};
  446. //X_0 初始状态
  447. double[,] measure = Z_K.Detail;
  448. measure[0, 0] = Arr[0];
  449. measure[1, 0] = Arr[1];
  450. measure[2, 0] = Arr[2];
  451. //首先计算推测值
  452. Matrix X_front = MatrixOperator.MatrixMulti(F_K, X_K);
  453. //然后计算推测值与真实值之间的误差协方差矩阵
  454. Matrix P_front = MatrixOperator.MatrixMulti(MatrixOperator.MatrixMulti(MatrixOperator.MatrixMulti(F_K, P_K), MatrixOperator.MatrixTrans(F_K)), Q_K);
  455. //再计算卡尔曼增益
  456. 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)));
  457. //再得到估计值
  458. Matrix X_front_ = MatrixOperator.MatrixAdd(X_front, MatrixOperator.MatrixMulti(Kg, MatrixOperator.MatrixSub(Z_K, MatrixOperator.MatrixMulti(H_K, X_front))));
  459. //最后计算估计值与真实值之间的误差协方差矩阵,为下次递推做准备
  460. Matrix P_front_ = MatrixOperator.MatrixMulti(MatrixOperator.MatrixSub(eye, MatrixOperator.MatrixMulti(Kg, H_K)), P_front);
  461. X_K = X_front_;
  462. P_K = P_front_;
  463. double[,] c = X_K.Detail;
  464. double aa = c[0, 0];
  465. double bb = c[1,0];
  466. double cc = c[2, 0];
  467. angles.x = (int)aa;
  468. angles.y = (int)bb;
  469. angles.z = (int)cc;
  470. float rotateX = (angles.x > 180) ? angles.x - 360 : angles.x;
  471. float rotateY = (angles.y > 180) ? angles.y - 360 : angles.y;
  472. float rotateZ = (angles.z > 180) ? angles.z - 360 : angles.z;
  473. // Coordinate transformation
  474. //live2DModel.PARAM_ANGLE.Set(-rotateY, rotateX, -rotateZ);
  475. live2DModel.PARAM_ANGLE.Set(rotateX, rotateY, rotateZ);
  476. // Make sure your line of sight is always facing the camera
  477. // eye_ball_X
  478. live2DModel.PARAM_EYE_BALL_X = rotateX / 60f;
  479. // eye_ball_Y
  480. live2DModel.PARAM_EYE_BALL_Y = rotateY / 60f - 0.25f;
  481. live2DModelUpdate (points);
  482. break;
  483. }
  484. }
  485. }
  486. }
  487. private void live2DModelUpdate (List<Vector2> points)
  488. {
  489. if (live2DModel != null) {
  490. //live2DModel.PARAM_ANGLE.Set(-Roundf(rotateY, 0.5f), Roundf(rotateX, 0.5f), -Roundf(rotateZ, 0.5f));
  491. // brow_L_Y
  492. float brow_L_Y = getRaitoOfBROW_L_Y(points);
  493. // Keep three decimal places to reduce the vibration
  494. live2DModel.PARAM_BROW_L_Y = Roundf(brow_L_Y, 1000f);
  495. //live2DModel.PARAM_BROW_L_Y = (float)Math.Round(brow_L_Y, 2);
  496. // brow_R_Y
  497. float brow_R_Y = getRaitoOfBROW_R_Y(points);
  498. // Keep three decimal places to reduce the vibration
  499. live2DModel.PARAM_BROW_R_Y = Roundf(brow_R_Y, 1000f);
  500. //live2DModel.PARAM_BROW_R_Y = (float)Math.Round(brow_R_Y, 2);
  501. // eye_open_L
  502. float eyeOpen_L = getRaitoOfEyeOpen_L (points);
  503. if (eyeOpen_L > 0.6f && eyeOpen_L < 1.1f)
  504. eyeOpen_L = 1;
  505. else if (eyeOpen_L >= 1.1f && brow_L_Y >= 0 )
  506. eyeOpen_L = 2;
  507. else if (eyeOpen_L <= 0.6f)
  508. eyeOpen_L = 0;
  509. live2DModel.PARAM_EYE_L_OPEN = eyeOpen_L;
  510. // eye_open_R
  511. float eyeOpen_R = getRaitoOfEyeOpen_R (points);
  512. if (eyeOpen_R > 0.6f && eyeOpen_R < 1.1f)
  513. eyeOpen_R = 1;
  514. else if (eyeOpen_R >= 1.1f && brow_R_Y >= 0)
  515. eyeOpen_R = 2;
  516. else if (eyeOpen_R < 0.6f)
  517. eyeOpen_R = 0;
  518. live2DModel.PARAM_EYE_R_OPEN = eyeOpen_R;
  519. // mouth_open
  520. float mouthOpen = getRaitoOfMouthOpen_Y (points);
  521. if (mouthOpen < 0.6f)
  522. mouthOpen = 0;
  523. live2DModel.PARAM_MOUTH_OPEN_Y = mouthOpen;
  524. // mouth_size
  525. float mouthSize = getRaitoOfMouthSize (points);
  526. live2DModel.PARAM_MOUTH_SIZE = mouthSize;
  527. }
  528. }
  529. // Keep decimal places to reduce the vibration
  530. private float Roundf(float f, float multiple)
  531. {
  532. if (multiple == 0)
  533. return f;
  534. int i = (int)(f * multiple);
  535. return i / multiple;
  536. }
  537. // Calculate the degree of eye opening
  538. private float getRaitoOfEyeOpen_L (List<Vector2> points)
  539. {
  540. if (points.Count != 68)
  541. throw new ArgumentNullException ("Invalid landmark_points.");
  542. 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);
  543. }
  544. private float getRaitoOfEyeOpen_R (List<Vector2> points)
  545. {
  546. if (points.Count != 68)
  547. throw new ArgumentNullException ("Invalid landmark_points.");
  548. 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);
  549. }
  550. // Eyebrows move up and down
  551. private float getRaitoOfBROW_L_Y (List<Vector2> points)
  552. {
  553. if (points.Count != 68)
  554. throw new ArgumentNullException ("Invalid landmark_points.");
  555. //float y = Mathf.Ceil(Mathf.Abs(points[24].y - points[27].y)) / Mathf.Abs (points [27].y - points [29].y);
  556. float y = Mathf.Abs(points[24].y - points[27].y) / Mathf.Abs(points[27].y - points[29].y);
  557. y -= 1;
  558. y *= 4f;
  559. return Mathf.Clamp (y, -1.0f, 1.0f);
  560. }
  561. private float getRaitoOfBROW_R_Y (List<Vector2> points)
  562. {
  563. if (points.Count != 68)
  564. throw new ArgumentNullException ("Invalid landmark_points.");
  565. //float y = Mathf.Ceil(Mathf.Abs(points[19].y - points[27].y)) / Mathf.Abs(points[27].y - points[29].y);
  566. 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))) ;
  567. Debug.Log("y是:"+y);
  568. y -= 1;
  569. y *= 4f;
  570. return Mathf.Clamp (y, -1.0f, 1.0f);
  571. }
  572. // Calculate the degree of mouth opening
  573. private float getRaitoOfMouthOpen_Y (List<Vector2> points)
  574. {
  575. if (points.Count != 68)
  576. throw new ArgumentNullException ("Invalid landmark_points.");
  577. return (float)((points[57].y - points[51].y) / (points[64].x - points[60].x) - 0.2);
  578. //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)));
  579. }
  580. // Calculate the width of the mouth
  581. private float getRaitoOfMouthSize (List<Vector2> points)
  582. {
  583. if (points.Count != 68)
  584. throw new ArgumentNullException ("Invalid landmark_points.");
  585. float size = Mathf.Abs (points [48].x - points [54].x) / (Mathf.Abs (points [31].x - points [35].x) * 1.8f);
  586. size -= 1;
  587. size *= 4f;
  588. return Mathf.Clamp (size, -1.0f, 1.0f);
  589. }
  590. /// <summary>
  591. /// Raises the disable event.
  592. /// </summary>
  593. void OnDisable ()
  594. {
  595. if(webCamTextureToMatHelper != null) webCamTextureToMatHelper.Dispose ();
  596. if(faceLandmarkDetector != null) faceLandmarkDetector.Dispose ();
  597. if(frontalFaceParam != null) frontalFaceParam.Dispose ();
  598. }
  599. public void OnBackButton()
  600. {
  601. #if UNITY_5_3 || UNITY_5_3_OR_NEWER
  602. SceneManager.LoadScene("FacemojiStart");
  603. #else
  604. Application.LoadLevel("FacemojiStart");
  605. #endif
  606. }
  607. /// <summary>
  608. /// Raises the change camera button event.
  609. /// </summary>
  610. public void OnChangeCameraButton ()
  611. {
  612. webCamTextureToMatHelper.Init (null, webCamTextureToMatHelper.requestWidth, webCamTextureToMatHelper.requestHeight, !webCamTextureToMatHelper.requestIsFrontFacing);
  613. }
  614. public void OnStartButton()
  615. {
  616. startBtn.SetActive(false);
  617. if (!Record.isRecording)
  618. {
  619. //Start recording
  620. Record.isRecording = true;
  621. finishBtn.SetActive(true);
  622. }
  623. }
  624. public void OnFinishButton()
  625. {
  626. finishBtn.SetActive(false);
  627. if (Record.isRecording)
  628. {
  629. //Finish recording
  630. Record.isRecording = false;
  631. startBtn.SetActive(true);
  632. }
  633. }
  634. public void OnShizukuButton()
  635. {
  636. if(selectedTexture != WhichTexture.noir_santa)
  637. {
  638. selectedTexture = WhichTexture.noir_santa;
  639. LoadTexture();
  640. }
  641. }
  642. public void OnHaruButton()
  643. {
  644. if (selectedTexture != WhichTexture.uni)
  645. {
  646. selectedTexture = WhichTexture.uni;
  647. LoadTexture();
  648. }
  649. }
  650. }
  651. }

 

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/我家小花儿/article/detail/123020
推荐阅读
相关标签
  

闽ICP备14008679号