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 }