/**
* Align p2 with a subsequence of p1
* Returns subsequence of p1 which is closest match
*/
// public List<mIndex> warpDTW(float[] p1, float[] p2)
public List<mIndex> warpDTW(mDistanceMeasurable[] p1, mDistanceMeasurable[] p2)
{
_init(p1, p2);
_createAccumCostMatrix(0,0);
_findWarpPath();
return _warpPath;
}
public float getDistance(mDistanceMeasurable d)
{
Float oth = (Float)d;
if (oth == null)
{
return(-1);
}
return(Mathf.Abs(oth.num - num));
}
// get distance between 2 poses
// 0 - 1 with 0 identical, 1 is not the same
public float getDistance(mDistanceMeasurable p)
{
Pose otherp = (Pose)p;
if (otherp == null)
{
return(-1);
}
float dist = 0;
for (int i = 0; i < joint_rotations.Length; i++)
{
// dist += Mathf.Abs(Quaternion.Angle(joint_rotations[i], otherp.joint_rotations[i]))*Mathf.Deg2Rad;
dist += Vector3.Distance(joint_locations[i], otherp.joint_locations[i]);
}
return(dist / (float)joint_rotations.Length);
}
/**
* Finds subsequence of seq closest to subseq which minimizes cost finction
*/
// public List<mIndex> warpSubsequenceDTW(float[] seq, float[] subseq)
public List<mIndex> warpSubsequenceDTW(mDistanceMeasurable[] seq, mDistanceMeasurable[] subseq)
{
_init(seq, subseq);
// _costMatrix = new float[][] {
// new float[] {1, 3, 5, 7, 9},
// new float[] {3, 2, 5, 6, 7},
// new float[] {2, 4, 3, 5, 7},
// new float[] {3, 4, 5, 4, 8},
// new float[] {2, 4, 6, 7, 5}
// };
// _accumCostMatrix = new float[][] {
// new float[] {float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity},
// new float[] {float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity},
// new float[] {float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity},
// new float[] {float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity},
// new float[] {float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity}
// };
for(int x = 1; x < _costMatrix[0].Length; x++)
{
_accumCostMatrix[0][x] = _costMatrix[0][x];
}
for(int y = 1; y < _costMatrix.Length; y++)
{
// float sum = 0;
// for(int x = 0; x < _costMatrix[y].Length; x++)
// {
// sum += _costMatrix[y][x];
// }
// _accumCostMatrix[y][0] = sum;
_accumCostMatrix[y][0] = _accumCostMatrix[y-1][0] + _costMatrix[y][0];
}
_createAccumCostMatrix(1,1);
_findSubsequenceWarpPath();
return _warpPath;
}
// get distance between 2 poses
// 0 - 1 with 0 identical, 1 is not the same
public float getDistance(mDistanceMeasurable p)
{
Pose otherp = (Pose)p;
if (otherp == null)
{
return -1;
}
float dist = 0;
for(int i = 0; i < joint_rotations.Length; i++)
{
// dist += Mathf.Abs(Quaternion.Angle(joint_rotations[i], otherp.joint_rotations[i]))*Mathf.Deg2Rad;
dist += Vector3.Distance(joint_locations[i], otherp.joint_locations[i]);
}
return (dist/(float)joint_rotations.Length);
}
public float getDistance(mDistanceMeasurable d)
{
Float oth = (Float)d;
if (oth == null)
{
return -1;
}
return Mathf.Abs(oth.num-num);
}
// private void _init(float[] xsig, float[] ysig)
private void _init(mDistanceMeasurable[] xsig, mDistanceMeasurable[] ysig)
{
// calculate cost matrix, calculate distance between each sample
// initialize accumulated cost matrix
_costMatrix = new float[ysig.Length][];
_accumCostMatrix = new float[ysig.Length][];
for(int y = 0; y < ysig.Length; y++)
{
_costMatrix[y] = new float[xsig.Length];
_accumCostMatrix[y] = new float[xsig.Length];
for(int x = 0; x < xsig.Length; x++)
{
// _costMatrix[y][x] = Math.Abs(xsig[x]-ysig[y]);
_costMatrix[y][x] = ysig[y].getDistance(xsig[x]);
_accumCostMatrix[y][x] = float.PositiveInfinity;
}
}
_accumCostMatrix[0][0] = _costMatrix[0][0];
// initialize optimal warp path
if (_warpPath != null)
{
_warpPath.Clear();
}
_warpPath = new List<mIndex>();
}
