public static Trans3 GetTransformNoScale(Vector from1, Vector from2, Vector from3, Vector to1, Vector to2, Vector to3) { //HTLib.DoubleVector3 lfrom1 = new HTLib.DoubleVector3(from1); //HTLib.DoubleVector3 lfrom2 = new HTLib.DoubleVector3(from2); //HTLib.DoubleVector3 lfrom3 = new HTLib.DoubleVector3(from3); //HTLib.DoubleVector3 lto1 = new HTLib.DoubleVector3(to1); //HTLib.DoubleVector3 lto2 = new HTLib.DoubleVector3(to2); //HTLib.DoubleVector3 lto3 = new HTLib.DoubleVector3(to3); //HTLib.Trans3 ltrans = HTLib.Trans3.GetTransformNoScale(lto1, lto2, lto3, lfrom1, lfrom2, lfrom3); Trans3 ltrans; { Vector[] p = new Vector[] { to1, to2, to3 }; Vector[] x = new Vector[] { from1, from2, from3 }; Trans3 optTrans = ICP3.OptimalTransform(p, x); ltrans = optTrans; } if (HDebug.IsDebuggerAttached) { var nto1 = ltrans.DoTransform(from1); var err1 = (nto1 - to1).Dist; var nto2 = ltrans.DoTransform(from2); var err2 = (nto2 - to2).Dist; var nto3 = ltrans.DoTransform(from3); var err3 = (nto3 - to3).Dist; HDebug.AssertTolerance(0.00000001, err1, err2, err3); } return(ltrans); }
public static Trans3 GetTrans(IList <Vector> source , IList <Vector> target ) { Trans3 trans = ICP3.OptimalTransform(source, target); return(trans); }
public static Trans3 GetTrans(IList <Vector> source , IList <Vector> target , IList <double> weight ) { Trans3 trans = ICP3.OptimalTransformWeighted(source, target, weight); return(trans); }
public static Trans3 FromFile(string path) { //return new Trans3(HTLib.Trans3.FromFile(path)); { System.IO.StreamReader reader = new System.IO.StreamReader(path); string str = reader.ReadLine(); reader.Close(); reader.Dispose(); return(Trans3.Parse(str)); } }
public static Trans3 AppendTrans(Trans3 _trans, Trans3 dtrans) { //Trans3 newtrans = new Trans3(HTLib.Trans3.GetTransform(dtrans.trans, trans.trans)); Trans3 newtrans; { //public static Trans3 GetTransform(Trans3 trans2, Trans3 trans1) Trans3 trans2 = dtrans; Trans3 trans1 = _trans; // trans2 * trans2 // DoubleMatrix4 mat2 = trans2.TransformMatrix; // DoubleMatrix4 mat1 = trans1.TransformMatrix; // DoubleMatrix4 trans_mat = mat2*mat1; // DoubleVector3 dt = new DoubleVector3(trans_mat.m03, trans_mat.m13, trans_mat.m23); Vector dt = trans2.DoTransform(trans1.dt); double ds = trans2.ds * trans1.ds; Quaternion dr = trans2.dr * trans1.dr; //Quaternion.GetQuaternion(trans_mat, ds); Trans3 trans = new Trans3(dt, ds, dr); if (HDebug.IsDebuggerAttached) { Vector pt1, pt2, pt12; pt1 = new double[] { 10, 0, 0 }; pt2 = trans2.DoTransform(trans1.DoTransform(pt1)); pt12 = trans.DoTransform(pt1); HDebug.Assert((pt12 - pt2).Dist < 0.001); pt1 = new double[] { 0, 10, 0 }; pt2 = trans2.DoTransform(trans1.DoTransform(pt1)); pt12 = trans.DoTransform(pt1); HDebug.Assert((pt12 - pt2).Dist < 0.001); pt1 = new double[] { 0, 0, 10 }; pt2 = trans2.DoTransform(trans1.DoTransform(pt1)); pt12 = trans.DoTransform(pt1); HDebug.Assert((pt12 - pt2).Dist < 0.001); } //return trans; newtrans = trans; } if (HDebug.IsDebuggerAttached) { MatrixByArr mat = _trans.TransformMatrix; MatrixByArr dmat = dtrans.TransformMatrix; MatrixByArr newmat = newtrans.TransformMatrix; MatrixByArr diff = newmat - dmat * mat; HDebug.AssertTolerance(0.00001, diff); } return(newtrans); }
public static Trans3 GetTransformNoScale(Vector from1, Vector from2, Vector to1, Vector to2) { //HTLib.DoubleVector3 lfrom1 = new HTLib.DoubleVector3(from1); //HTLib.DoubleVector3 lfrom2 = new HTLib.DoubleVector3(from2); //HTLib.DoubleVector3 lto1 = new HTLib.DoubleVector3(to1 ); //HTLib.DoubleVector3 lto2 = new HTLib.DoubleVector3(to2 ); //HTLib.Trans3 ltrans = HTLib.Trans3.GetTransformNoScale(lfrom1, lfrom2, lto1, lto2); //return new Trans3(ltrans); { double ds = 1; //double dx = to2.x - from2.x; // dx + from2.x = to2.x //double dy = to2.y - from2.y; // dx + from2.y = to2.y //double dz = to2.z - from2.z; // dx + from2.z = to2.z Vector from = (from2 - from1).UnitVector(); Vector to = (to2 - to1).UnitVector(); Quaternion dr; double innerprod = LinAlg.DotProd(from, to); // InnerProduct(from,to); if (innerprod >= 1) { HDebug.Assert(innerprod == 1); dr = Quaternion.UnitRotation; } else { Vector axis = LinAlg.CrossProd(from, to); // CrossProduct(from, to); if (axis.Dist2 == 0) { // to avoid degenerate cases double mag = to.Dist / 100000000; axis = LinAlg.CrossProd(from, to + (new double[] { mag, mag * 2, mag * 3 })); //Vector.CrossProduct(from, to+new DoubleVector3(mag, mag*2, mag*3)); } double angle = Math.Acos(innerprod); dr = new Quaternion(axis, angle); HDebug.Assert(LinAlg.DotProd((dr.RotationMatrix * from), to) > 0.99999); } Trans3 trans = Trans3.GetTransform(-from1, 1, Quaternion.UnitRotation); trans = Trans3.AppendTrans(trans, Trans3.GetTransform(new double[3], ds, dr)); trans = Trans3.AppendTrans(trans, Trans3.GetTransform(to1, 1, Quaternion.UnitRotation)); //new Trans3(dx, dy, dz, ds, dr); if (HDebug.IsDebuggerAttached) { Vector fromto1 = trans.DoTransform(from1); HDebug.Assert((fromto1 - to1).Dist < 0.000001); Vector fromto2 = trans.DoTransform(from2); HDebug.Assert(((fromto2 - fromto1).UnitVector() - (to2 - to1).UnitVector()).Dist < 0.000001); } return(trans); } }
public static Tuple <double, Vector, bool> MaxCircleBetweenCircles(Vector pt1, double rad1, Vector pt2, double rad2, Vector pt3, double rad3) { /// b:pt3 /// / \ /// / \ /// / \ /// o:pt1 ------ a:pt2 /// /// o : (0 , 0), radii ro /// a : (pax, 0), radii ra /// b : (pbx, pby), radii rb Vector uvec12 = (pt2 - pt1).UnitVector(); Vector uvec23 = (pt3 - pt2).UnitVector(); Vector uvec31 = (pt1 - pt3).UnitVector(); double cos123 = LinAlg.VtV(-uvec12, uvec23); double cos231 = LinAlg.VtV(-uvec23, uvec31); double cos312 = LinAlg.VtV(-uvec31, uvec12); double cos_aob; double ro, ra, rb; Vector po, pa, pb; if (cos123 > 0) { pa = pt1; ra = rad1; po = pt2; ro = rad2; pb = pt3; rb = rad3; cos_aob = cos123; } else if (cos231 > 0) { pa = pt2; ra = rad2; po = pt3; ro = rad3; pb = pt1; rb = rad1; cos_aob = cos231; } else if (cos312 > 0) { pa = pt2; ra = rad2; po = pt1; ro = rad1; pb = pt3; rb = rad3; cos_aob = cos312; } else { throw new Exception("cos123>0 and cos231>0"); } Vector uvec_oa = (pa - po).UnitVector(); Vector uvec_ob = (pb - po).UnitVector(); Vector normal = LinAlg.CrossProd(uvec_oa, uvec_ob); double sin_aob = normal.Dist; double pax = (pa - po).Dist; double pbx = (pb - po).Dist * cos_aob; double pby = (pb - po).Dist * sin_aob; Trans3 trans; { Vector npo = new double[] { 0, 0, 0 }; Vector npa = new double[] { 0, pax, 0 }; Vector npb = new double[] { 0, pbx, pby }; trans = Trans3.GetTransformNoScale(npo, npa, npb, po, pa, pb); if (HDebug.IsDebuggerAttached) { Vector pox = trans.DoTransform(npo); HDebug.AssertTolerance(0.0000001, po - trans.DoTransform(npo)); HDebug.AssertTolerance(0.0000001, pa - trans.DoTransform(npa)); HDebug.AssertTolerance(0.0000001, pb - trans.DoTransform(npb)); } } Tuple <double, Vector, bool> rad_cent_in = MaxCircleBetweenCircles(ro, ra, rb, pax, pbx, pby); if (rad_cent_in == null) { return(null); } double radius = rad_cent_in.Item1; bool cenerInTriangle = rad_cent_in.Item3; Vector center; { HDebug.Assert(rad_cent_in.Item2.Size == 2); center = new double[] { 0, rad_cent_in.Item2[0], rad_cent_in.Item2[1] }; center = trans.DoTransform(center); } if (HDebug.IsDebuggerAttached) { double radx_1 = (center - pt1).Dist - rad1; double radx_2 = (center - pt2).Dist - rad2; double radx_3 = (center - pt3).Dist - rad3; HDebug.AssertTolerance(0.000001, radx_1 - radx_2, radx_2 - radx_3, radx_3 - radx_1); HDebug.AssertTolerance(0.00000001, LinAlg.VtV(normal, pt1 - pt2)); HDebug.AssertTolerance(0.00000001, LinAlg.VtV(normal, pt2 - pt3)); HDebug.AssertTolerance(0.00000001, LinAlg.VtV(normal, pt3 - pt1)); HDebug.AssertTolerance(0.00000001, LinAlg.VtV(normal, pt1 - center)); HDebug.AssertTolerance(0.00000001, LinAlg.VtV(normal, pt2 - center)); HDebug.AssertTolerance(0.00000001, LinAlg.VtV(normal, pt3 - center)); } return(new Tuple <double, Vector, bool>(radius, center, cenerInTriangle)); }
public static Trans3 OptimalTransformWeighted(IList <Vector> source, IList <Vector> target, IList <double> weight , double?rotateRatio = null ) { if (OptimalTransformWeighted_selftest) #region selftest { OptimalTransformWeighted_selftest = false; { Vector[] _source = new Vector[] { new double[] { 0, 0, 0 }, new double[] { 1, 0, 0 }, new double[] { 0, 1, 0 }, new double[] { 0, 0, 1 }, }; Vector[] _target = new Vector[] { new double[] { 0, 0, 0 }, new double[] { 0, 1, 0 }, new double[] { 0, 0, 1 }, new double[] { 1, 0, 0 }, }; double[] _weight = new double[] { 1, 1, 1, 1 }; Trans3 _trans = OptimalTransformWeighted(_source, _target, _weight, null); Vector[] _moved = _trans.GetTransformed(_source); double _err2 = 0; for (int i = 0; i < _source.Length; i++) { _err2 = (_target[i] - _moved[i]).Dist2; } HDebug.Assert(_err2 < 0.00000001); } { Vector[] _source = new Vector[] { new double[] { 1, 0, 0 }, new double[] { 0, 1, 0 }, new double[] { 0, 0, 1 }, }; Vector[] _target = new Vector[] { new double[] { 0, 1, 0 }, new double[] { 0, 0, 1 }, new double[] { 1, 0, 0 }, }; double[] _weight = new double[] { 1, 1, 1 }; Trans3 _trans = OptimalTransformWeighted(_source, _target, _weight, null); Vector[] _moved = _trans.GetTransformed(_source); double _err2 = 0; for (int i = 0; i < _source.Length; i++) { _err2 = (_target[i] - _moved[i]).Dist2; } HDebug.Assert(_err2 < 0.00000001); } } #endregion if (HDebug.IsDebuggerAttached) { foreach (double _w in weight) { HDebug.Assert(_w >= 0); } } Vector[] p = source.ToArray(); Vector[] x = target.ToArray(); double[] w = weight.ToArray(); HDebug.Assert(p.Length == x.Length); HDebug.Assert(w.Length == x.Length); Vector up = p.MeanWeighted(w); Vector ux = x.MeanWeighted(w); Quaternion quater = OptimalRotationWeighted(p, x, up, ux, w); if (rotateRatio != null) { double r = quater.RotationAngle; r = r % (2 * Math.PI); r = (r > Math.PI) ? (r - 2 * Math.PI) : r; r = r / 2; quater = new Quaternion(quater.RotationAxis, r); } MatrixByArr qR = quater.RotationMatrix; Vector qT = OptimalTransWeighted(up, ux, qR, w); return(new Trans3(qT, 1, quater)); }