private static Matrix <double> PseudoInverse(Svd <double> svd)
{
Matrix <double> W = svd.W();
Vector <double> s = svd.S();
// The first element of W has the maximum value.
double tolerance = Precision.EpsilonOf(2) * Math.Max(svd.U().RowCount, svd.VT().ColumnCount) * W[0, 0];
for (int i = 0; i < s.Count; i++)
{
if (s[i] < tolerance)
{
s[i] = 0;
}
else
{
s[i] = 1 / s[i];
}
}
W.SetDiagonal(s);
// (U * W * VT)T is equivalent with V * WT * UT
return((svd.U() * W * svd.VT()).Transpose());
}
/// <summary>
/// Moore–Penrose pseudoinverse
/// If A = U • Σ • VT is the singular value decomposition of A, then A† = V • Σ† • UT.
/// For a diagonal matrix such as Σ, we get the pseudoinverse by taking the reciprocal of each non-zero element
/// on the diagonal, leaving the zeros in place, and transposing the resulting matrix.
/// In numerical computation, only elements larger than some small tolerance are taken to be nonzero,
/// and the others are replaced by zeros. For example, in the MATLAB or NumPy function pinv,
/// the tolerance is taken to be t = ε • max(m,n) • max(Σ), where ε is the machine epsilon. (Wikipedia)
/// </summary>
/// <param name="M">The matrix to pseudoinverse</param>
/// <returns>The pseudoinverse of this Matrix</returns>
public static Matrix PseudoInverse(this Matrix M)
{
Svd D = M.Svd(true);
Matrix W = (Matrix)D.W();
Vector s = (Vector)D.S();
// The first element of W has the maximum value.
double tolerance = Precision.EpsilonOf(2) * Math.Max(M.RowCount, M.ColumnCount) * W[0, 0];
for (int i = 0; i < s.Count; i++)
{
if (s[i] < tolerance)
{
s[i] = 0;
}
else
{
s[i] = 1 / s[i];
}
}
W.SetDiagonal(s);
// (U * W * VT)T is equivalent with V * WT * UT
return((Matrix)(D.U() * W * D.VT()).Transpose());
}
public static Transform Kabsch(List <Point3d> P, List <Point3d> Q)
{
Transform xf = Transform.Identity;
if (P.Count != Q.Count)
{
return(xf);
}
Point3d CenP = new Point3d(), CenQ = new Point3d();
for (int i = 0; i < P.Count; i++)
{
CenP += P[i];
CenQ += Q[i];
}
CenP /= P.Count; CenQ /= Q.Count;
DenseMatrix MX = new DenseMatrix(P.Count, 3);
DenseMatrix MY = new DenseMatrix(P.Count, 3);
for (int i = 0; i < P.Count; i++)
{
DenseVector v1 = new DenseVector(3);
DenseVector v2 = new DenseVector(3);
v1[0] = P[i].X - CenP.X; v2[0] = Q[i].X - CenQ.X;
v1[1] = P[i].Y - CenP.Y; v2[1] = Q[i].Y - CenQ.Y;
v1[2] = P[i].Z - CenP.Z; v2[2] = Q[i].Z - CenQ.Z;
MX.SetRow(i, v1); MY.SetRow(i, v2);
}
DenseMatrix H = DenseMatrix.OfMatrix(MX.TransposeThisAndMultiply(MY));
Svd svd = H.Svd(true);
Matrix <double> UT = svd.U().Transpose();
Matrix <double> V = svd.VT().Transpose();
Matrix <double> R = V.Multiply(UT);
double d = R.Determinant();
if (d > 0)
{
d = 1;
}
else
{
d = -1;
}
DenseMatrix I = new DenseMatrix(3, 3, new double[] { 1, 0, 0, 0, 1, 0, 0, 0, d });
R = V.Multiply(I).Multiply(UT);
xf.M00 = R[0, 0]; xf.M01 = R[0, 1]; xf.M02 = R[0, 2];
xf.M10 = R[1, 0]; xf.M11 = R[1, 1]; xf.M12 = R[1, 2];
xf.M20 = R[2, 0]; xf.M21 = R[2, 1]; xf.M22 = R[2, 2];
CenP.Transform(xf);
Transform tf = Transform.Translation(CenQ - CenP);
return(Transform.Multiply(tf, xf));
}
public override void Estimate(List <Point3D> datas)
{
double sum_x = 0;
double sum_y = 0;
double sum_z = 0;
foreach (Point3D temp in datas)
{
sum_x += temp.x;
sum_y += temp.y;
sum_z += temp.z;
}
sum_x /= datas.Count;
sum_y /= datas.Count;
sum_z /= datas.Count;
DenseMatrix jacobian = new DenseMatrix(datas.Count, 3);
foreach (Point3D temp in datas)
{
Vector <double> gradient = new DenseVector(3);
gradient[0] = temp.x - sum_x;
gradient[1] = temp.y - sum_y;
gradient[2] = temp.z - sum_z;
jacobian.SetRow(datas.IndexOf(temp), gradient);
}
Svd svd = jacobian.Svd(true);
// get matrix of left singular vectors with first n columns of U
Matrix <double> U1 = svd.U().SubMatrix(0, datas.Count, 0, 3);
// get matrix of singular values
Matrix <double> S = new DiagonalMatrix(3, 3, svd.S().ToArray());
// get matrix of right singular vectors
Matrix <double> V = svd.VT().Transpose();
Vector <double> parameters = new DenseVector(3);
parameters = V.Column(0);
x = sum_x;
y = sum_y;
z = sum_z;
i = parameters[0];
j = parameters[1];
k = parameters[2];
}
public static void LinePlaneEstimate(List <Point3d> datas, out Line line, out Plane plane)
{
Point3d cen = Point3d.GetCenter(datas);
DenseMatrix jacobian = new DenseMatrix(datas.Count, 3);
foreach (Point3d temp in datas)
{
Vector <double> gradient = new DenseVector(3);
gradient[0] = temp.X - cen.X;
gradient[1] = temp.Y - cen.Y;
gradient[2] = temp.Z - cen.Z;
jacobian.SetRow(datas.IndexOf(temp), gradient);
}
Svd svd = jacobian.Svd(true);
Matrix <double> V = svd.VT().Transpose();
Vector <double> parameters = V.Column(2);
Vector <double> parameters2 = V.Column(0);
plane = new Plane(cen, new Vector3d(parameters[0], parameters[1], parameters[2]));
line = new Line(cen, cen + new Vector3d(parameters2[0], parameters2[1], parameters2[2]));
}
public Transform KabschEstimate(List <Point3d> P, List <Point3d> Q)
{
/*
* 一组空间点匹配另一组空间点 kabsch算法
* 将两组点的中心算出来 ，然后平移中点到原点
* 协方差矩阵 H = X * Y.transpose();
* [U,S,VT]=SVD(H)
* I=(V*UT).Determinant判断方向
* 旋转R = V*I*UT;
* 平移T = -R * cenA + cenB;
* 新点坐标P=P*T*R
*/
Transform xf = Transform.Identity;
if (P.Count != Q.Count)
{
return(xf);
}
Point3d CenP = new Point3d(); Point3d CenQ = new Point3d();
for (int i = 0; i < P.Count; i++)
{
CenP += P[i];
CenQ += Q[i];
}
CenP /= P.Count; CenQ /= Q.Count;
DenseMatrix MX = new DenseMatrix(P.Count, 3);
DenseMatrix MY = new DenseMatrix(P.Count, 3);
for (int i = 0; i < P.Count; i++)
{
DenseVector v1 = new DenseVector(3);
DenseVector v2 = new DenseVector(3);
v1[0] = P[i].X - CenP.X; v2[0] = Q[i].X - CenQ.X;
v1[1] = P[i].Y - CenP.Y; v2[1] = Q[i].Y - CenQ.Y;
v1[2] = P[i].Z - CenP.Z; v2[2] = Q[i].Z - CenQ.Z;
MX.SetRow(i, v1); MY.SetRow(i, v2);
}
DenseMatrix H = DenseMatrix.OfMatrix(MX.TransposeThisAndMultiply(MY));
Svd svd = H.Svd(true);
Matrix <double> UT = svd.U().Transpose();
Matrix <double> V = svd.VT().Transpose();
Matrix <double> R = V.Multiply(UT);
double d = R.Determinant();
if (d > 0)
{
d = 1;
}
else
{
d = -1;
}
DenseMatrix I = new DenseMatrix(3, 3, new double[] { 1, 0, 0, 0, 1, 0, 0, 0, d });
R = V.Multiply(I).Multiply(UT);
xf.M00 = R[0, 0]; xf.M01 = R[0, 1]; xf.M02 = R[0, 2];
xf.M10 = R[1, 0]; xf.M11 = R[1, 1]; xf.M12 = R[1, 2];
xf.M20 = R[2, 0]; xf.M21 = R[2, 1]; xf.M22 = R[2, 2];
CenP.Transform(xf);
Transform tf = Transform.Translation(CenQ - CenP);
return(Transform.Multiply(tf, xf));
}
