public static List <float> matchingAll(Trajectory traj1, Trajectory traj2)
{
List <float> result = new List <float>();
result.Add(0.0F);
TPose temp = new TPose(traj1.vec[0] - traj2.vec[0]);
float delta = (float)temp.norm();
float a = 0.0F, b = 0.0F;
int match_length = (int)(traj1.size() > traj2.size() ? traj1.size() : traj2.size());
for (int i = 1; i < match_length; ++i)
{
temp = new TPose(traj1.vec[i] - traj2.vec[i]);
b = (float)temp.norm() - delta;
result.Add(Math.Abs(a));
a = b;
}
return(result);
}
public static Dictionary <float, float> matching8Max(Trajectory traj1, Trajectory traj2)
{
Dictionary <float, float> temp_result = new Dictionary <float, float>();
TPose temp = new TPose(traj1.vec[0] - traj2.vec[0]);
float delta = (float)temp.norm();
float a = 0.0F, b = 0.0F;
int upORdown = 0;
int match_length = (int)(traj1.size() > traj2.size() ? traj1.size() : traj2.size());
for (int i = 1; i < match_length - 1; ++i)
{
temp = new TPose(traj1.vec[i] - traj2.vec[i]);
b = (float)temp.norm() - delta;
if (b > a && upORdown >= 0)
{
a = b;
upORdown = 1;
}
else if (b < a && upORdown <= 0)
{
a = b;
upORdown = -1;
}
else if (b > a && upORdown < 0)
{
temp_result.Add(((float)(i - 1) / match_length), Math.Abs(a));
a = b;
upORdown = 1;
}
else if (b < a && upORdown > 0)
{
temp_result.Add(((float)(i - 1) / match_length), Math.Abs(a));
a = b;
upORdown = -1;
}
}
temp_result = (from entry in temp_result
orderby entry.Value descending
select entry).ToDictionary(pair => pair.Key, pair => pair.Value);
Dictionary <float, float> result = new Dictionary <float, float>();
int num = 0;
foreach (KeyValuePair <float, float> dic in temp_result)
{
result.Add(dic.Key, dic.Value);
num++;
if (num >= 6)
{
break;
}
}
result.Add(0.0F, 0.0F);
temp = new TPose(traj1.vec[match_length - 1] - traj2.vec[match_length - 1]);
result.Add(1.0F, Math.Abs((float)(temp.norm() - delta)));
return(result);
}
public void push_back(Trajectory traj)
{
this.m_vecTrajs.Add(traj);
}
public void Add(Trajectory traj)
{
this.m_vecTrajs.Add(traj);
}
public Trajectory(Trajectory traj)
{
vec = traj.vec;
}