public static Trans3 GetTrans(IList <Vector> C1, IList <Anisou> anisou1, IList <Vector> C2, HPack <double> optEnergy = null)
{
int size = C1.Count;
HDebug.Assert(size == C1.Count, size == C2.Count);
Vector[] C1s = C1.ToArray().Clone(3);
Vector[] C2s = C2.ToArray().Clone(3);
double[] W1s = new double[size * 3];
double[] enrgs = new double[size * 3];
for (int i = 0; i < size; i++)
{
HDebug.Assert(anisou1[i].eigvals[0] >= 0); W1s[i * 3 + 0] = (anisou1[i].eigvals[0] <= 0) ? 0 : 1 / anisou1[i].eigvals[0];
HDebug.Assert(anisou1[i].eigvals[1] >= 0); W1s[i * 3 + 1] = (anisou1[i].eigvals[1] <= 0) ? 0 : 1 / anisou1[i].eigvals[1];
HDebug.Assert(anisou1[i].eigvals[2] >= 0); W1s[i * 3 + 2] = (anisou1[i].eigvals[2] <= 0) ? 0 : 1 / anisou1[i].eigvals[2];
enrgs[i * 3 + 0] = enrgs[i * 3 + 1] = enrgs[i * 3 + 2] = double.NaN;
}
//Trans3 trans = ICP3.OptimalTransform(C2, C1);
Trans3 trans = new Trans3(new double[] { 0, 0, 0 }, 1, Quaternion.UnitRotation);// = transICP3.OptimalTransformWeighted(C2, C1, W1s);
int iter = 0;
int iter_max = 1000;
//double enrg = double.NaN;
while (iter < iter_max)
{
iter++;
Vector[] C2sUpdated = trans.GetTransformed(C2s);
//for(int i=0; i<size; i++)
System.Threading.Tasks.Parallel.For(0, size, delegate(int i)
{
for (int j = 0; j < 3; j++)
{
Vector planeNormal = anisou1[i].axes[j];
Vector planeBase = C1[i];
Vector query = C2sUpdated[i * 3 + j];
Vector closest = query - LinAlg.DotProd(planeNormal, query - planeBase) * planeNormal;
HDebug.AssertTolerance(0.00001, LinAlg.DotProd(closest - planeBase, planeNormal));
C1s[i * 3 + j] = closest;
enrgs[i * 3 + j] = W1s[i * 3 + j] * (query - closest).Dist2;
}
});
Trans3 dtrans = ICP3.OptimalTransformWeighted(C2sUpdated, C1s, W1s);
trans = Trans3.AppendTrans(trans, dtrans);
double max_dtrans_matrix = (dtrans.TransformMatrix - LinAlg.Eye(4)).ToArray().HAbs().HMax();
if (max_dtrans_matrix < 0.0000001)
{
break;
}
}
if (optEnergy != null)
{
optEnergy.value = enrgs.Sum() / size;
}
return(trans);
}
public static Trans3 GetTrans(IList <Vector> C1
, IList <Vector> C2
, IList <double> weight
//, Pack<List<Vector>> C2new = null
)
{
if (HDebug.Selftest())
{
double[] tweight = new double[weight.Count];
for (int i = 0; i < tweight.Length; i++)
{
tweight[i] = 0.2;
}
Trans3 ttrans0 = GetTrans(C1, C2, tweight);
Trans3 ttrans1 = MinRMSD.GetTrans(C1, C2);
HDebug.AssertTolerance(0.0001, ttrans0.ds - ttrans1.ds);
HDebug.AssertTolerance(0.0001, ttrans0.dt - ttrans1.dt);
HDebug.AssertTolerance(0.0001, new Vector(ttrans0.dr.ToArray()) - ttrans1.dr.ToArray());
HDebug.AssertTolerance(0.0001, (ttrans0.TransformMatrix - ttrans1.TransformMatrix));
}
HDebug.Assert(C1.Count == C2.Count);
HDebug.Assert(C1.Count == weight.Count);
//Trans3 trans = ICP3.OptimalTransform(C2, C1);
Trans3 trans = ICP3.OptimalTransformWeighted(C2, C1, weight);
if (HDebug.IsDebuggerAttached)
{
Vector[] C2updated = trans.GetTransformed(C2).ToArray();
double RMSD0 = 0;
double RMSD1 = 0;
for (int i = 0; i < C1.Count; i++)
{
RMSD0 += (C1[i] - C2[i]).Dist2;
RMSD1 += (C1[i] - C2updated[i]).Dist2;
}
//Debug.AssertTolerance(0.00000001, Math.Abs(RMSD1 - RMSD0));
}
return(trans);
}
public static Trans3 GetTrans(IList <Vector> C1, IList <Anisou> anisou1, IList <Vector> C2, IList <Anisou> anisou2, HPack <double> optEnergy = null)
{
int size = C1.Count;
HDebug.Assert(size == C1.Count, size == C2.Count);
Vector[] srcs = new Vector[size * 3 * 2]; // sources
Vector[] tars = new Vector[size * 3 * 2]; // targets
double[] weis = new double[size * 3 * 2]; // weights
double[] engs = new double[size * 3 * 2]; // energies
//for(int i=0; i<size; i++)
//{
// Debug.Assert(anisou1[i].eigvals[0] >= 0); W1s[i*3+0] = (anisou1[i].eigvals[0] <= 0) ? 0 : 1 / anisou1[i].eigvals[0];
// Debug.Assert(anisou1[i].eigvals[1] >= 0); W1s[i*3+1] = (anisou1[i].eigvals[1] <= 0) ? 0 : 1 / anisou1[i].eigvals[1];
// Debug.Assert(anisou1[i].eigvals[2] >= 0); W1s[i*3+2] = (anisou1[i].eigvals[2] <= 0) ? 0 : 1 / anisou1[i].eigvals[2];
// enrgs[i*3+0] = enrgs[i*3+1] = enrgs[i*3+2] = double.NaN;
//}
//Trans3 trans = ICP3.OptimalTransform(C2, C1);
Trans3 trans = new Trans3(new double[] { 0, 0, 0 }, 1, Quaternion.UnitRotation);// = transICP3.OptimalTransformWeighted(C2, C1, W1s);
int iter = 0;
int iter_max = 1000;
//double enrg = double.NaN;
while (iter < iter_max)
{
iter++;
//for(int i=0; i<size; i++)
System.Threading.Tasks.Parallel.For(0, size, delegate(int i)
{
for (int j = 0; j < 3; j++)
{
Vector p1 = C1[i];
Vector n1 = anisou1[i].axes[j];
double w1 = anisou1[i].eigvals[j]; w1 = (w1 <= 0) ? 0 : 1 / w1;
Vector p2 = trans.DoTransform(C2[i]);
Vector n2 = (trans.DoTransform(C2[i] + anisou2[i].axes[j]) - p2).UnitVector();
double w2 = anisou2[i].eigvals[j]; w2 = (w2 <= 0) ? 0 : 1 / w2;
Vector clo12 = p2 - LinAlg.DotProd(n1, p2 - p1) * n1; // closest point from p1 to plane2
Vector clo21 = p1 - LinAlg.DotProd(n2, p1 - p2) * n2; // closest point from p1 to plane2
HDebug.AssertTolerance(0.00001, LinAlg.DotProd(clo12 - p1, n1));
HDebug.AssertTolerance(0.00001, LinAlg.DotProd(clo21 - p2, n2));
// p2 -> (pt closest to p2 on plane1 with w1)
srcs[(i * 3 + j) * 2 + 0] = p2;
tars[(i * 3 + j) * 2 + 0] = clo12;
weis[(i * 3 + j) * 2 + 0] = w1;
engs[(i * 3 + j) * 2 + 0] = w1 * (p2 - clo12).Dist2;
// inverse of {p1 -> (pt closest to p1 on plane2 with w2)}
// = (pt closest to p1 on plane2 with w2) -> p1
srcs[(i * 3 + j) * 2 + 1] = clo21;
tars[(i * 3 + j) * 2 + 1] = p1;
weis[(i * 3 + j) * 2 + 1] = w2;
engs[(i * 3 + j) * 2 + 1] = w2 * (clo21 - p1).Dist2;
}
});
Trans3 dtrans = ICP3.OptimalTransformWeighted(srcs, tars, weis);
trans = Trans3.AppendTrans(trans, dtrans);
double max_dtrans_matrix = (dtrans.TransformMatrix - LinAlg.Eye(4)).ToArray().HAbs().HMax();
if (max_dtrans_matrix < 0.0000001)
{
break;
}
}
if (optEnergy != null)
{
optEnergy.value = engs.Sum() / size;
}
return(trans);
}