public void Transform(Aff3d Toffset)
{
for (int i = 0; i < poses_.Count; i++)
{
poses_[i] = Toffset * poses_[i];
}
}
private void ReadPosesFromFile(string path)
{
try
{
using (StreamReader sr = new StreamReader(path))
{
string line;
//Debug.Log("starting while loop");
while ((line = sr.ReadLine()) != null)
{
string[] values = line.Split(' ');
float x = float.Parse(values[0], CultureInfo.InvariantCulture);
float y = float.Parse(values[1], CultureInfo.InvariantCulture);
float z = float.Parse(values[2], CultureInfo.InvariantCulture);
float ex = float.Parse(values[3], CultureInfo.InvariantCulture);
float ey = float.Parse(values[4], CultureInfo.InvariantCulture);
float ez = float.Parse(values[5], CultureInfo.InvariantCulture);
Aff3d aff3d = new Aff3d(x, y, z, ex, ey, ez);
poses_.Add(aff3d);
}
}
}
catch (Exception e)
{
Debug.Log("Could not read file: " + path + ". Error: " + e);
}
}
public static Aff3d operator *(Aff3d T1, Aff3d T2)
{
Aff3d T3 = new Aff3d();
T3.Rotation = T1.Rotation * T2.Rotation;
T3.Translation = T1.Translation + T1.Rotation * T2.Translation;
return(T3);
}
public Aff3d Inverse()
{
Aff3d inv = new Aff3d(this);
//negate translation and use the quaternion inverse operation to invert rotation
trans = -trans;
rot = Quaternion.Inverse(rot);
return(inv);
}
public Trajectory GetTransformed(Aff3d Toffset)
{
Trajectory transformed = new Trajectory();
foreach (Aff3d p in poses_)
{
transformed.push_back(Toffset * p);
}
return(transformed);
}
public Aff3d GetPose(int i)
{
if (i >= 0 && i < poses_.Count)
{
return(poses_[i]);
}
else
{
return(Aff3d.Identity());
}
}
void Start()
{
Trajectory src = new Trajectory("data/wand_SmallHeart.txt");
Trajectory target = new Trajectory("data/wand_SmallHeart.txt"); //= new Trajectory("data/wand_LargeHeart.txt");
Trajectory registered = new Trajectory("data/wand_LargeHeart.txt");
//src.Normalize();
//registered.Center();
Aff3d T = new Aff3d(0.0f, 0.0f, 0.0f, 0.4f, 0.4f, 0.4f);
registered.Transform(T);
//target.RescaleByDimension();
//src.Center();
//src.RescaleByDimension();
//registered.Normalize();
//registered.Transform(Toffset);
Aff3d vis_offset = new Aff3d(1.0f, 2.5f, -1.5f, 0, 0, 0);
TrajectoryMatcher tmatch = new ICPMatcher();
Aff3d Talign = new Aff3d();
double score = 0;
//src.Transform(Aff3d.Identity());
//Plot(target, src, src, vis_offset);
/*for (int i = 0; i < 1; i++)
* {
* if (tmatch.Match(target, registered, out Talign, out score))
* {
* registered.Transform(Talign);
*
* }
* }*/
target.Rescale();
registered.Rescale();
//Plot(target, src, registered, vis_offset);
for (int i = 0; i < 10; i++)
{
if (tmatch.Match(target, registered, out Talign, out score))
{
registered.Transform(Talign);
}
}
Plot(target, src, registered, vis_offset);
}
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)
}
// Start is called before the first frame update
void Plot(Trajectory target, Trajectory src, Trajectory registered, Aff3d offset)
{
List <Aff3d> posesA_ = target.GetTrajectory();
foreach (Aff3d pose in posesA_)
{
GameObject gameObject = Instantiate(spellParticlePlot);
gameObject.GetComponent <Renderer>().material.color = Color.red;
gameObject.transform.position = new Vector3(pose.Translation.x, pose.Translation.y, pose.Translation.z) + offset.Translation;
Vector3 rotation = pose.Rotation.eulerAngles;
gameObject.transform.rotation = Quaternion.Euler(rotation);
}
/* List<Aff3d> posesB_ = src.GetTrajectory();
*
* foreach (Aff3d pose in posesB_)
* {
* GameObject gameObject = Instantiate(spellParticlePlot);
* gameObject.GetComponent<Renderer>().material.color = Color.blue;
* gameObject.transform.position = new Vector3(pose.Translation.x, pose.Translation.y, pose.Translation.z) + offset.Translation;
* Vector3 rotation = pose.Rotation.eulerAngles;
* gameObject.transform.rotation = Quaternion.Euler(rotation);
* }*/
List <Aff3d> posesC_ = registered.GetTrajectory();
foreach (Aff3d pose in posesC_)
{
GameObject gameObject = Instantiate(spellParticlePlot);
gameObject.GetComponent <Renderer>().material.color = Color.green;
gameObject.transform.position = new Vector3(pose.Translation.x, pose.Translation.y, pose.Translation.z) + offset.Translation;
Vector3 rotation = pose.Rotation.eulerAngles;
gameObject.transform.rotation = Quaternion.Euler(rotation);
}
}
public void SVDAlign(Matrix <double> target, Matrix <double> source, ref Aff3d T) //target
{
if (target.RowCount != source.RowCount || source.RowCount == 0)
{
//ERROR
Debug.LogError("Rows in matrices not consistent inside alignment");
return;
}
Vector <double> c0 = Vector <double> .Build.Dense(3);
Vector <double> c1 = Vector <double> .Build.Dense(3);
for (int i = 0; i < target.RowCount; i++) //calculate centroid
{
c0 += target.Row(i);
c1 += source.Row(i);
}
c0 *= (1.0 / ((double)target.RowCount)); //normalize
c1 *= (1.0 / ((double)target.RowCount));
Matrix <double> tar = target;
Matrix <double> src = source;
for (int i = 0; i < tar.RowCount; i++)
{
tar.SetRow(i, tar.Row(i) - c0);
src.SetRow(i, src.Row(i) - c1);
}
//Debug.Log("NO CENTROID " + tar.ToString());
Matrix <double> H = tar.Transpose() * src;
var solved = H.Svd(true);
Matrix <double> V = solved.VT.Transpose();
Matrix <double> Vt = solved.VT;
Matrix <double> U = solved.U;
//Matrix<double> det_mat = Matrix<double>.Build.DenseIdentity(3,3);
//det_mat[2, 2] = (Vt.Transpose() * U.Transpose()).Determinant();
Matrix <double> R = Vt.Transpose() * U.Transpose();
double det = R.Determinant();
if (det < 0.0)
{
Vt.SetRow(2, -Vt.Row(2));
//det_mat[2, 2] = (Vt.Transpose() * U.Transpose()).Determinant();
R = Vt.Transpose() * U.Transpose();
//Debug.Log("Incorrect");
}
// Debug.Log("correct");
Vector <double> tr = -(c1 - R * c0);
//Debug.Log("offset T: " + tr);
//Debug.Log("offset R: " + R.ToString());
Quaternion q = Aff3d.RotToQuaternion(R.Inverse());
q.Normalize();
T.Rotation = q;
//Debug.Log("EULER R: " + q.eulerAngles.ToString());
T.Translation = new Vector3((float)tr[0], (float)tr[1], (float)tr[2]);
}
public virtual bool Match(Trajectory target, Trajectory source, out Aff3d T, out double score)
{
T = Aff3d.Identity();
score = 0;
return(true);
}
public static Aff3d Identity()
{ // return an aff3d with zeroes trans and rot, aka Identity trans and rot
Aff3d ident = new Aff3d(0, 0, 0, 0, 0, 0);
return(ident);
}
public Aff3d(Aff3d aff3d)
{
this.trans = aff3d.Translation;
this.rot = aff3d.Rotation;
}
public void push_back(Aff3d T)
{
//Debug.Log("adding : " + T.ToString());
poses_.Add(T);
//add T to poses
}
