public Aff3d GetPose(int i)
{
if (i >= 0 && i < poses_.Count)
{
return(poses_[i]);
}
else
{
return(Aff3d.Identity());
}
}
public override bool Match(Trajectory target, Trajectory source, out Aff3d T, out double score)
{
score = 0;
T = Aff3d.Identity();
//Debug.Log("matched");
List <int> ass_idx = new List <int>();
AssiciateByNearest(target, source, ref ass_idx);
Debug.Log("ASS:");
Debug.Log(ass_idx);
// FilterOutliers(ref ass_idx, 0.1f);
Matrix <double> tar = target.PositionMatrix(ass_idx, true);
Matrix <double> src = source.PositionMatrix(ass_idx, false);
Debug.Log("tar size: " + tar.ToString());
Debug.Log("src size: " + src.ToString());
SVDAlign(tar, src, ref T);
Debug.Log("MATCH OUTPUT: " + T.ToString());
//source.Transform(T);
return(true);
// determine correspondence by time
//use svd to minimize least squares
//calculate symmetric distance
//double score = Trajectory::Distance(target, src)
}
public virtual bool Match(Trajectory target, Trajectory source, out Aff3d T, out double score)
{
T = Aff3d.Identity();
score = 0;
return(true);
}