public float solve(out Vec3 outx, float svd_tol,
int svd_sweeps, float pinv_tol)
{
if (this.data.numPoints == 0)
{
throw new UnityException("...");
}
this.massPoint.set(this.data.massPoint_x, this.data.massPoint_y, this.data.massPoint_z);
VecUtils.scale(out this.massPoint, 1.0f / this.data.numPoints, this.massPoint);
this.setAta();
this.setAtb();
Vec3 tmpv;
MatUtils.vmul_symmetric(out tmpv, this.ata, this.massPoint);
VecUtils.sub(out this.atb, this.atb, tmpv);
this.x.clear();
float result = Svd.solveSymmetric(this.ata, this.atb, out this.x,
svd_tol, svd_sweeps, pinv_tol);
VecUtils.addScaled(out this.x, 1.0f, this.massPoint);
this.setAtb();
outx = x;
this.hasSolution = true;
return(result);
}
internal static Plane FromPointsSVD(GeoPoint[] Points, out double MaxDistance, out bool isLinear)
{
isLinear = false;
MaxDistance = double.MaxValue;
GeoPoint centroid = new GeoPoint(Points);
Matrix m = DenseMatrix.OfArray(new double[Points.Length, 3]);
for (int i = 0; i < Points.Length; i++)
{
GeoVector v = Points[i] - centroid;
m[i, 0] = v.x;
m[i, 1] = v.y;
m[i, 2] = v.z;
}
try
{
Svd <double> svd = m.Svd();
GeoVector normal = new GeoVector(svd.U[0, 0], svd.U[0, 1], svd.U[0, 2]);
return(new Plane(centroid, normal));
}
catch
{
return(Plane.XYPlane);
}
}
private float[] MovePINV(float[] q_current, float[] error)
{
Matrix <float> jac = IIWAjacobian(q_current);
Vector <float> errorV = Vector <float> .Build.DenseOfArray(error);
Svd <float> svd = jac.Svd();
//float[] singularVal = new float[6];
Matrix <float> S = Matrix <float> .Build.DenseIdentity(7, 6);
for (int i = 0; i < 6; i++)
{
if (svd.S[i] != 0)
{
S[i, i] = svd.S[i] / (svd.S[i] * svd.S[i] + 1);
}
else
{
S[i, i] = 1000000000;
}
}
Matrix <float> jac3 = svd.VT.Transpose() * S * svd.U.Transpose();
Vector <float> qPINV = jac3 * errorV;
float[] qOut = new float[7];
for (int i = 0; i < 7; i++)
{
qOut[i] = qPINV[i];
}
return(qOut);
}
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());
}
private void polarize(Svd<double> DG_SVD, out Vector3d RotationAxis, out double RotationAngle)
{
DenseMatrix RotationalMatrix = (DenseMatrix)DG_SVD.U.Multiply(DG_SVD.VT);
Quaterniond RotationQ = OpenTK_To_MathNET.MatrixToQuaternion(RotationalMatrix);
RotationQ.Normalize();
RotationQ.ToAxisAngle(out RotationAxis, out RotationAngle);
}
public static async Task <ProcessingResult> Encrypt(string originalFilePath, string originalFileName, string originalKeyPath, int brightness, int contrast, int mode)
{
var originalContainerBitmap = new Bitmap(originalFilePath);
var originalKeyBitmap = new Bitmap(originalKeyPath);
var encryptionStopwatch = Stopwatch.StartNew();
switch (mode)
{
case 1:
originalContainerBitmap =
Helpers.SetContrast(Helpers.SetBrightness(originalContainerBitmap, brightness), contrast);
break;
case 2:
originalKeyBitmap =
Helpers.SetContrast(Helpers.SetBrightness(originalKeyBitmap, brightness), contrast);
break;
}
var result = await Svd.Encrypt(originalContainerBitmap, originalKeyBitmap, originalFileName);
encryptionStopwatch.Stop();
var encryptionPsnr = Helpers.CalculatePsnr(result.InputContainer, result.OutputContainer);
return(new ProcessingResult
{
Psnr = encryptionPsnr,
Time = encryptionStopwatch.Elapsed,
AverageBlueColor = result.AverageBlueColor,
AverageGreenColor = result.AverageGreenColor,
AverageRedColor = result.AverageRedColor
});
}
public void solve()
{
if (_weight > 0.0)
{
Matrix <double> covariance = (_moment - (_sum1 / _weight).ToColumnMatrix() * _sum2.ToRowMatrix()) / _weight;
Svd <double> svd = covariance.Svd();
Matrix <double> rotation = (svd.VT.Transpose()) * (svd.U.Transpose());
int sign = rotation.Determinant() > 0 ? 1 : -1;
Matrix <double> coordinate = DenseMatrix.Build.DenseIdentity(3, 3);
coordinate[2, 2] = sign;
rotation = (svd.VT.Transpose()) * coordinate * (svd.U.Transpose());
_resultRotation = QuaternionFromMatrix(rotation);
_startCentroid = numericToUnity(_sum1) / (float)_weight;
_endCentroid = numericToUnity(_sum2) / (float)_weight;
}
else
{
_resultRotation = Quaternion.identity;
_startCentroid = Vector3.zero;
_endCentroid = Vector3.zero;
}
}
public static async Task <ProcessingResult> HandleEncryption(Bitmap originalContainerBitmap, Bitmap originalKeyBitmap, string originalFileName)
{
var encryptionStopwatch = Stopwatch.StartNew();
var result = await Svd.Encrypt(originalContainerBitmap, originalKeyBitmap, originalFileName);
encryptionStopwatch.Stop();
var encryptionMse = Helpers.CalculateMse(result.InputContainer, result.OutputContainer);
var encryptionPsnr = Helpers.CalculatePsnr(encryptionMse);
return(new ProcessingResult
{
Psnr = encryptionPsnr,
Mse = encryptionMse,
Time = encryptionStopwatch.Elapsed,
AverageBlueColor = result.AverageBlueColor,
AverageGreenColor = result.AverageGreenColor,
AverageRedColor = result.AverageRedColor,
AverageBlueColorWatermark = result.AverageBlueColorWatermark,
AverageGreenColorWatermark = result.AverageGreenColorWatermark,
AverageRedColorWatermark = result.AverageRedColorWatermark,
ContainerHeight = result.ContainerHeight,
ContainerWidth = result.ContainerWidth,
WatermarkHeight = result.WatermarkHeight,
WatermarkWidth = result.WatermarkWidth,
ContainerWithWatermark = result.OutputContainer
});
}
static public Line GaussFitLine(PointSet PS)
{
Position pos_average = Position.Zero;
foreach (var p in PS)
{
pos_average += p.Pos;
}
pos_average /= PS.Count;
DenseMatrix jacobian = new DenseMatrix(PS.Count, 3);
foreach (var p in PS)
{
var gradient = (p.Pos - pos_average).Pos;
jacobian.SetRow(PS.IndexOf(p), gradient);
}
Svd <double> svd = jacobian.Svd(true);
// get matrix of left singular vectors with first n columns of U
Matrix <double> U1 = svd.U.SubMatrix(0, PS.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);
Line result = new Line(pos_average, new Vector(parameters[0], parameters[1], parameters[2]), 1);
return(result);
}
/// <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 svd_operation(Bitmap x, double[,] color)
{
row = x.Width >> 1;
col = x.Height >> 1;
src = new double[row, col];
src = init_src(color, row, col);
Input = DenseMatrix.OfArray(src);
svd = Input.Svd(true);
}
public void Test03()
{
Matrix m = new Matrix(
new RowVector(1, 2, 5),
new RowVector(2, 5, 7));
Svd svd = Func.Svd(m);
Assert.That(svd.U * svd.S * Matrix.T(svd.V), Is.EqualTo(m).Within(Ep));
}
public void Test01()
{
Matrix m = new Matrix(
new RowVector(0, 1, -1),
new RowVector(-1, 1, 0),
new RowVector(2, 1, 0));
Svd svd = Func.Svd(m);
Assert.That(svd.U * svd.S * Matrix.T(svd.V), Is.EqualTo(m).Within(Ep));
}
protected void Decompose(Transformation matrix)
{
Svd <Complex> svd = matrix.Matrix.Svd();
U = new Transformation(svd.U);
VT = new Transformation(svd.VT);
Sigma = new Transformation(CreateMatrix.DenseOfDiagonalVector(svd.S));
SingularValues = svd.S.ToArray();
SigmaRank = svd.Rank;
TraceNorm = svd.S.Sum().Real;
}
private static Matrix <Double> SVD(List <Double> values, int accuracy) //Singular Value Decomposition
{
Matrix <Double> X = BuildTrayectoryMatrix(values);
Matrix <Double> V = X.Transpose() * X;
Svd <Double> svd = V.Svd(true);
Matrix <Double> U = svd.VT;
Matrix <Double> rca = U * V.Inverse();
return(rca);
}
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 static float solveLeastSquares(Mat3 a, Vec3 b, out Vec3 x,
float svd_tol, int svd_sweeps, float pinv_tol)
{
Mat3 at;
SMat3 ata;
Vec3 atb;
MatUtils.transpose(out at, a);
MatUtils.mmul_ata(out ata, a);
MatUtils.vmul(out atb, at, b);
return(Svd.solveSymmetric(ata, atb, out x, svd_tol, svd_sweeps, pinv_tol));
}
public GPOut Predict(double TestPointX, Svd <double> svd1 = null, int results = 1)
{
//if(results< TrainingPointsX.Count)
// results = TrainingPointsX.Count;
double[] TestPointsX;
if (results > 1)
{
TestPointsX = MathNet.Numerics.Generate.LinearSpaced(results, TrainingPointsX.Min(), TestPointX);
}
else
{
TestPointsX = new double[] { TestPointX }
};
if (Kte == null || results != Kte.ColumnCount)
{
if (results == 1)
{
Kte = kernel.Main(Matrix <double> .Build.Dense(1, 1), length);
}
else
{
Kte = kernel.Main(DistanceMatrix.Instance.Matrix.SubMatrix(0, results, 0, results), length);
}
}
var gpOut = new GPOut();
Svd <double> svd;
if (TrainingPointsX.Count > 0 && svd1 != null /* && TestPointsX.Length>= TrainingPointsX.Count*/)
{
(gpOut.mu, svd) = MathHelper.Evaluate(TestPointsX, TrainingPointsY.ToArray(), TrainingPointsX.ToArray(), kernel, length, svd1, Kte);
}
else
{
gpOut.mu = Vector <double> .Build.DenseOfArray(Enumerable.Range(0, results).Select(_ => 0d).ToArray());
//gpOut.sd95 = 1.98 * (Kte.Diagonal()).PointwiseSqrt();
svd = Kte.Svd();
}
gpOut.proj = svd.U * Matrix <double> .Build.DenseOfDiagonalVector(svd.S.PointwiseSqrt());
gpOut.sd95 = 1.98 * (gpOut.proj * gpOut.proj.Transpose()).Diagonal().PointwiseSqrt();
gpOut.X = TestPointsX;
return(gpOut);
}
}
public void TestMakeLowRankMatrix()
{
Matrix <double> x = SampleGenerator.MakeLowRankMatrix(
numSamples: 50,
numFeatures: 25,
effectiveRank: 5,
tailStrength: 0.01,
randomState: new Random(0));
Assert.AreEqual(50, x.RowCount, "X shape mismatch");
Assert.AreEqual(25, x.ColumnCount, "X shape mismatch");
Svd svd = x.Svd(true);
double sum = svd.S().Sum();
Assert.IsTrue(Math.Abs(sum - 5) < 0.1, "X rank is not approximately 5");
}
public static async Task <ProcessingResult> Decrypt(string originalFileName, Bitmap originalKeyBitmap)
{
var decryptionFileName = $"{originalFileName}_Container";
var decryptionFilePath = Path.Combine(MainConstants.ContainersProcessedPath, $"{decryptionFileName}.bmp");
var decryptionStopwatch = Stopwatch.StartNew();
var decryptionResult = await Svd.Decrypt(new Bitmap(decryptionFilePath), decryptionFileName);
decryptionStopwatch.Stop();
var decryptionPsnr = Helpers.CalculatePsnr(originalKeyBitmap, decryptionResult);
return(new ProcessingResult
{
Psnr = decryptionPsnr,
Time = decryptionStopwatch.Elapsed
});
}
public void TestIdealMatrices()
{
List <Image <Arthmetic, double> > ptsReal = new List <Image <Arthmetic, double> >();
List <PointF> pts1 = new List <PointF>();
List <PointF> pts2 = new List <PointF>();
Random rand = new Random(1003);
for (int i = 0; i < 10; ++i)
{
var real = new Image <Arthmetic, double>(new double[, , ] {
{ { rand.Next(100, 200) } },
{ { rand.Next(100, 200) } },
{ { rand.Next(50, 100) } },
{ { 1 } },
});
ptsReal.Add(real);
var i1 = P1.Multiply(real).ToPointF();
pts1.Add(i1);
var i2 = P2.Multiply(real).ToPointF();
pts2.Add(i2);
}
MatchingResult match = new MatchingResult()
{
LeftPoints = new VectorOfPointF(pts1.ToArray()),
RightPoints = new VectorOfPointF(pts2.ToArray()),
};
var F = ComputeMatrix.F(match.LeftPoints, match.RightPoints);
var E = ComputeMatrix.E(F, K);
var svd = new Svd(E);
FindTransformation.DecomposeToRTAndTriangulate(pts1, pts2, K, E, out var RR, out var tt, out Image <Arthmetic, double> X);
var tt1 = T12.Mul(1 / T12.Norm);
var tt2 = tt.Mul(1 / tt.Norm);
var KK = EstimateCameraFromImagePair.K(F, 700, 400);
var EE = ComputeMatrix.E(F, KK);
var svd2 = new Svd(EE);
}
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 RandomVector(Vector <double> M, Matrix <double> Cov)
{
this.M = M;
Svd <double> Cov_svd = Cov.Svd();
Sigma = Cov_svd.U * Cov_svd.W.PointwiseSqrt() * Cov_svd.VT;
//if (Cov.FrobeniusNorm() == 0)
//{
// Sigma = Cov;
//}
//else
//{
// Sigma = Cov.Cholesky().Factor;
//}
distrs = new T[M.Count];
for (int i = 0; i < M.Count; i++)
{
distrs[i] = new T();
}
}
public static async Task <ProcessingResult> DecryptFromBitmap(Bitmap encrypptedContainer, string originalFileName, Bitmap originalKeyBitmap)
{
var decryptionFileName = $"{originalFileName}_Container";
var decryptionStopwatch = Stopwatch.StartNew();
var decryptionResult = await Svd.Decrypt(encrypptedContainer, decryptionFileName,
originalKeyBitmap.Width, originalKeyBitmap.Height);
decryptionStopwatch.Stop();
var decryptionMse = Helpers.CalculateMse(originalKeyBitmap, decryptionResult);
var decryptionPsnr = Helpers.CalculatePsnr(decryptionMse);
originalKeyBitmap.Dispose();
return(new ProcessingResult
{
Psnr = decryptionPsnr,
Mse = decryptionMse,
Time = decryptionStopwatch.Elapsed,
ExtractedWatermark = decryptionResult
});
}
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]));
}
/// <summary>
/// Calculates a rotation matrix. Points in given buffers have to be translated into origin of a coordinate system.
/// Algorithm can behave incorectly if they would not be translated.
/// </summary>
/// <param name="sourcePoints">Source points (points that are rotated).</param>
/// <param name="">Target points.</param>
/// <returns>The rotation matrix.</returns>
public Matrix <float> CalculateRotation(List <Vector <float> > sourcePoints, List <Vector <float> > targetPoints)
{
Log.Info("Calculating rotation matrix.");
// Creates matrices from points in origin.
Matrix <float> sourceMatrix = Matrix <float> .Build.DenseOfColumnVectors(sourcePoints.ToArray());
Matrix <float> targetMatrix = Matrix <float> .Build.DenseOfColumnVectors(targetPoints.ToArray());
// Mutiply target and source matrices and calculates singular value decomposition of the result.
Svd <float> singularDecomposition = (targetMatrix * sourceMatrix.Transpose()).Svd();
Matrix <float> rotationMatrix = singularDecomposition.U * singularDecomposition.VT;
rotationMatrix[3, 0] = 0;
rotationMatrix[3, 1] = 0;
rotationMatrix[3, 2] = 0;
rotationMatrix[3, 3] = 1;
Log.Debug("Found rotation matrix: " + rotationMatrix);
return(rotationMatrix);
}
public void UpdateCoefficients(double l2Penalty)
{
if (X_svd == null)
{
X_svd = X.Svd();
}
Matrix <double> V = X_svd.VT.Transpose();
double[] s = X_svd.W.ToRowWiseArray();
Matrix <double> U = X_svd.U;
for (int i = 0; i < s.Length; i++)
{
s[i] = s[i] / (s[i] * s[i] + l2Penalty);
}
Matrix <double> S = Matrix <double> .Build.DenseOfDiagonalArray(s);
Matrix <double> Z = V.Multiply(S).Multiply(U.Transpose());
_coefficients = Z.Multiply(Vector <double> .Build.DenseOfArray(Y)).ToArray();
_fitted = X.Multiply(Vector <double> .Build.DenseOfArray(_coefficients)).ToArray();
double errorVariance = 0;
for (int i = 0; i < _residuals.Length; i++)
{
_residuals[i] = Y[i] - _fitted[i];
errorVariance += _residuals[i] * _residuals[i];
}
errorVariance = errorVariance / (X.RowCount - X.ColumnCount);
Matrix <double> errorVarianceMatrix = Matrix <double> .Build.DenseIdentity(Y.Length).Multiply(errorVariance);
Matrix <double> coefficientsCovarianceMatrix = Z.Multiply(errorVarianceMatrix).Multiply(Z.Transpose());
_standarderrors = GetDiagonal(coefficientsCovarianceMatrix);
}
/// <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)
/// Edited by MH
/// </summary>
/// <param name="M">The matrix to pseudoinverse</param>
/// <returns>The pseudoinverse of this Matrix</returns>
public static Matrix <float> PseudoInverse(Matrix <float> M)
{
Svd <float> D = M.Svd(true);
var W = (Matrix <float>)D.W;
var s = (Vector <float>)D.S;
for (int i = 0; i < s.Count; i++)
{
if (s[i] < 0.1) // Tolerance suggested by dc paper (TODO: can s be negative?)
{
s[i] = 0;
}
else
{
s[i] = 1 / s[i];
}
}
W.SetDiagonal(s);
// (U * W * VT)T is equivalent with V * WT * UT
return((Matrix <float>)(D.U * W * D.VT).Transpose());
}
/// <summary>
/// <para>Generates sigma-points given the mean x, covarianve P and spread parameter lambda</para>
/// <para>- Xi_0 = x</para>
/// <para>- Xi_i = x + sqrt((L+lambda)P)_i, i = 1,...,L</para>
/// <para>- Xi_i = x - sqrt((L+lambda)P)_{i-L}, i = L+1,...,2L </para>
/// <para>L - dimention of x, sqrt(P) - Cholesky decomposition </para>
/// </summary>
/// <param name="x">Mean</param>
/// <param name="P">Covariance</param>
/// <param name="lambda">Spread parameter</param>
/// <returns>Matrix of sigma points</returns>
public static Matrix <double> Generate(Vector <double> x, Matrix <double> P, double lambda)
{
int L = x.Count;
//Matrix<double> Sqrt = (Math.Sqrt(lambda + L)) * P.Cholesky().Factor.Transpose();
Svd <double> Psvd = P.Svd();
Matrix <double> Sqrt = (Math.Sqrt(lambda + L)) * Psvd.U * Psvd.W.PointwiseSqrt() * Psvd.VT;
Matrix <double> Xi = x.ToColumnMatrix();
for (int i = 0; i < L; i++)
{
Xi = Xi.Append((x + Sqrt.Column(i)).ToColumnMatrix());
}
for (int i = 0; i < L; i++)
{
Xi = Xi.Append((x - Sqrt.Column(i)).ToColumnMatrix());
}
return(Xi);
}
public Matrix <double> SingulerValueDecomposition(Matrix <double> A, Matrix <double> b)
{
/*
* Svd<double> svd = A.Svd(true);
* double ConditionMumber = svd.ConditionNumber;
* Debug.WriteLine("Cond: {0}", svd.ConditionNumber);
*
* if((svd.ConditionNumber == double.PositiveInfinity) || (svd.ConditionNumber == double.NegativeInfinity)) {
* Debug.WriteLine("Condition number is infinity");
* return null;
* }
*
* // get matrix of left singular vectors with first n columns of U
* Matrix<double> U1 = svd.U.SubMatrix(0, A.RowCount, 0, A.ColumnCount);
*
* // get matrix of singular values
* Matrix<double> S = new DiagonalMatrix(A.ColumnCount, A.ColumnCount, svd.S.ToArray());
*
* // get matrix of right singular vectors
* Matrix<double> V = svd.VT.Transpose();
*
* return V.Multiply(S.Inverse()).Multiply(U1.Transpose().Multiply(b));
*/
Svd <double> svd = A.Svd(true);
double ConditionMumber = svd.ConditionNumber;
Debug.WriteLine("Rank, Det, Cond: {0},{1},{2}", svd.Rank, svd.Determinant, svd.ConditionNumber);
if ((svd.ConditionNumber == double.PositiveInfinity) || (svd.ConditionNumber == double.NegativeInfinity))
{
Debug.WriteLine("Condition number is infinity");
return(null);
}
return(svd.Solve(b));
}
public double MatchIteration(Matrix X, Matrix Y, Matrix V1, Matrix V2, Matrix t1, Matrix t2)
{
timer.Clear();
double timeused = 0;
timer.Restart();
var tk = t1.Clone(); // tk=t1;
int w = 4;
int ndum = (int)(nsamp * ndum_frac);
nsamp1 = nsamp2 = nsamp;
#region demo2迭代
Matrix Xk = X.Clone();
int N1 = V1.Rows, N2 = V2.Columns;
if (display_flag) {
Draw(X, Y, V1, V2, @"D:\Play Data\Iteration\原图.bmp", "原始图像和采样");
//DrawGradient(X, Y, t1, t2, N2, N1, @"D:\Play Data\Iteration\原图梯度.bmp", "原始图像切向量");
}
int k = 0;
var out_vec_1 = Utils.InitArray<bool>(nsamp1, false);//out_vec_1=zeros(1,nsamp1);
var out_vec_2 = Utils.InitArray<bool>(nsamp2, false);//out_vec_2=zeros(1,nsamp2);
double ori_weight = 0.1;
double tan_eps = 1.0;
bool affine_start_flag = true;
bool polarity_flag = true;
double matchcost = double.MaxValue;
double sc_cost = 0, aff_cost = 0, E = 0;
Matrix cx = null, cy = null; // cx, cy是插值线性方程组的解的两列
Matrix axt = null, wxt = null, ayt = null, wyt = null, d2 = null, U = null,
X2 = null, Y2 = null, X2b = null, X3b = null, Y3 = null;
double mean_dist_1 = 0, mean_dist_2 = 0;
var min1 = new[] { new { Val = 0.0, Idx = 0 } };
var min2 = min1;
#region 用于打网格的坐标
Matrix coordX = null, coordY = null;
int coordMargin = (int)(N1 * coordMarginRate);
MatrixUtils.CreateGrid(N1 + coordMargin * 2, N2 + coordMargin * 2, out coordX, out coordY);
coordX = coordX.Each(v => v - coordMargin * 2);
coordY = coordY.Each(v => v - coordMargin * 2);
//int MM = N1 * N2 / 25; // M=length(x);
#endregion
timeused += timer.StopAndSay("初始化");
while (k < n_iter) {
Debug("Iter={0}", k);
#region 计算两个形状上下文
timer.Restart();
// [BH1,mean_dist_1]=sc_compute(Xk',zeros(1,nsamp),mean_dist_global,nbins_theta,nbins_r,r_inner,r_outer,out_vec_1);
var BH1 = ComputeSC(Xk.Transpose(), Zeros(1, nsamp), mean_dist_global, out mean_dist_1, out_vec_1);
//var BH1 = ComputeSC(Xk.Transpose(), t1.Transpose(), mean_dist_global, out mean_dist_1, out_vec_1);
// [BH2,mean_dist_2]=sc_compute(Y',zeros(1,nsamp),mean_dist_global,nbins_theta,nbins_r,r_inner,r_outer,out_vec_2);
var BH2 = ComputeSC(Y.Transpose(), Zeros(1, nsamp), mean_dist_global, out mean_dist_2, out_vec_2);
//var BH2 = ComputeSC(Y.Transpose(), t2.Transpose(), mean_dist_global, out mean_dist_2, out_vec_2);
timeused += timer.StopAndSay("计算两个形状上下文");
Debug("Mean_dist_1:{0:F4}", mean_dist_1);
Debug("Mean_dist_2:{0:F4}", mean_dist_2);
#endregion
#region 计算lambda_o和beta_k
double lambda_o;
if (affine_start_flag) {
if (k == 0)
lambda_o = 1000;
else
lambda_o = beta_init * Math.Pow(r, k - 1); // lambda_o=beta_init*r^(k-2);
} else {
lambda_o = beta_init * Math.Pow(r, k); // lambda_o=beta_init*r^(k-1);
}
double beta_k = mean_dist_2 * mean_dist_2 * lambda_o;
#endregion
#region 计算代价矩阵
timer.Restart();
var costmat_shape = HistCost(BH1, BH2); // costmat_shape = hist_cost_2(BH1, BH2);
// theta_diff=repmat(tk,1,nsamp)-repmat(t2',nsamp,1);
var theta_diff = tk.RepMat(1, nsamp) - t2.Transpose().RepMat(nsamp, 1);
Matrix costmat_theta;
if (polarity_flag) {
// costmat_theta=0.5*(1-cos(theta_diff));
//costmat_theta = 0.5 * (Ones(costmat_shape.Rows, costmat_shape.Columns) - theta_diff.Each(v => Math.Cos(v)));
costmat_theta = theta_diff.Each(v => 0.5 * (1 - Math.Cos(v)));
} else {
// costmat_theta=0.5*(1-cos(2*theta_diff));
//costmat_theta = 0.5 * (Ones(costmat_shape.Rows, costmat_shape.Columns) - theta_diff.Each(v => Math.Cos(2 * v)));
costmat_theta = theta_diff.Each(v => 0.5 * (1 - Math.Cos(2 * v)));
}
// costmat=(1-ori_weight)*costmat_shape+ori_weight*costmat_theta;
var costmat = (1 - ori_weight) * costmat_shape + ori_weight * costmat_theta;
int nptsd = nsamp + ndum; // nptsd=nsamp+ndum;
var costmat2 = new DenseMatrix(nptsd, nptsd, eps_dum); // costmat2=eps_dum*ones(nptsd,nptsd);
costmat2.SetSubMatrix(0, nsamp, 0, nsamp, costmat); // costmat2(1:nsamp,1:nsamp)=costmat;
timeused += timer.StopAndSay("计算代价矩阵");
#endregion
#region 匈牙利算法
timer.Restart();
var costmat_int = new int[nptsd, nptsd];
for (int i = 0; i < nptsd; ++i) {
for (int j = 0; j < nptsd; ++j) {
costmat_int[i, j] = (int)(costmat2[i, j] * 10000);
}
}
var km = new KM(nptsd, costmat_int);
km.Match(false);
matchcost = km.MatchResult / 10000.0;
int[] cvec = km.MatchPair; // cvec=hungarian(costmat2);
timeused += timer.StopAndSay("匈牙利算法");
#endregion
#region 计算野点标记向量，重排匹配点
timer.Restart();
int[] cvec2 = cvec.Select((v, i) => new { Val = v, Idx = i })
.OrderBy(v => v.Val)
.Select(v => v.Idx)
.ToArray();// [a,cvec2]=sort(cvec);
out_vec_1 = cvec2.Take(nsamp1).Select(v => v > nsamp2).ToArray(); // out_vec_1=cvec2(1:nsamp1)>nsamp2;
out_vec_2 = cvec.Take(nsamp2).Select(v => v > nsamp1).ToArray(); // out_vec_2=cvec(1:nsamp2)>nsamp1;
//X2 = NaNs(nptsd, 2); // X2=NaN*ones(nptsd,2);
//X2.SetSubMatrix(0, nsamp1, 0, X2.Columns, Xk); // X2(1:nsamp1,:)=Xk;
//X2 = X2.SortRowsBy(cvec); // X2=X2(cvec,:);
X2b = NaNs(nptsd, 2); // X2b=NaN*ones(nptsd,2);
X2b.SetSubMatrix(0, nsamp1, 0, X2b.Columns, X); // X2b(1:nsamp1,:)=X;
X2b = X2b.SortRowsBy(cvec); // X2b=X2b(cvec,:);
Y2 = NaNs(nptsd, 2); // Y2=NaN*ones(nptsd,2);
Y2.SetSubMatrix(0, nsamp2, 0, Y2.Columns, Y); // Y2(1:nsamp2,:)=Y;
var ind_good = X2b.GetColumn(1).Take(nsamp).FindIdxBy(v => !double.IsNaN(v)); // ind_good=find(~isnan(X2b(1:nsamp,1)));
int n_good = ind_good.Length; // n_good=length(ind_good);
X3b = X2b.FilterRowsBy(ind_good);// X3b=X2b(ind_good,:);
Y3 = Y2.FilterRowsBy(ind_good); // Y3=Y2(ind_good,:);
timeused += timer.StopAndSay("计算野点标记向量，重排匹配点");
#endregion
#region figure 2
if (display_flag) {
//figure(2)
//plot(X2(:,1),X2(:,2),'b+',Y2(:,1),Y2(:,2),'ro')
//hold on
//h=plot([X2(:,1) Y2(:,1)]',[X2(:,2) Y2(:,2)]','k-');
//if display_flag
//% set(h,'linewidth',1)
//quiver(Xk(:,1),Xk(:,2),cos(tk),sin(tk),0.5,'b') // 画箭头
//quiver(Y(:,1),Y(:,2),cos(t2),sin(t2),0.5,'r')
DrawGradient(Xk, Y, tk, t2, N2, N1,
String.Format(@"D:\Play Data\Iteration\梯度\{0}.bmp", k),
String.Format("Iter={0}梯度方向\n匹配点数{1}", k, n_good));
//end
//hold off
//axis('ij')
//title([int2str(n_good) ' correspondences (warped X)'])
//axis([1 N2 1 N1])
//drawnow
}
#endregion
#region figure 3 显示未变形图的匹配关系
if (display_flag) {
//% show the correspondences between the untransformed images
//figure(3)
//plot(X(:,1),X(:,2),'b+',Y(:,1),Y(:,2),'ro')
//ind=cvec(ind_good);
//hold on
//plot([X2b(:,1) Y2(:,1)]',[X2b(:,2) Y2(:,2)]','k-')
//hold off
//axis('ij')
//title([int2str(n_good) ' correspondences (unwarped X)'])
//axis([1 N2 1 N1])
//drawnow
Draw(X, Y, cvec, null, N2, N1, String.Format(@"D:\Play Data\Iteration\匹配\{0}.bmp", k),
String.Format("Iter={0}\n匹配代价{1},匹配数{2}", k, matchcost, n_good));
}
#endregion
#region 求解变换矩阵
timer.Restart();
Bookstein(X3b, Y3, beta_k, ref cx, ref cy, ref E); // [cx,cy,E]=bookstein(X3b,Y3,beta_k);
timeused += timer.StopAndSay("求解变换矩阵");
#endregion
#region 通过解出来的变换对点和梯度进行变换
timer.Restart();
// % calculate affine cost
var A = MatrixUtils.RankHorizon(
cx.GetSubMatrix(n_good + 1, 2, 0, 1), cy.GetSubMatrix(n_good + 1, 2, 0, 1)
);//A=[cx(n_good+2:n_good+3,:) cy(n_good+2:n_good+3,:)];
var s = new Svd(A, true).S(); // s=svd(A);
aff_cost = Math.Log(s[0] / s[1]); // aff_cost=log(s(1)/s(2));
// % calculate shape context cost
min1 = costmat.GetColumns().Select(col => {
int minwhere = 0;
for (int i = 1; i < col.Count; ++i) {
if (col[i] < col[minwhere]) minwhere = i;
}
return new { Val = col[minwhere], Idx = minwhere };
}).ToArray();// [a1,b1]=min(costmat,[],1);
min2 = costmat.GetRows().Select(row => {
int minwhere = 0;
for (int i = 1; i < row.Count; ++i) {
if (row[i] < row[minwhere]) minwhere = i;
}
return new { Val = row[minwhere], Idx = minwhere };
}).ToArray(); // [a2,b2]=min(costmat,[],2);}
sc_cost = Math.Max(min1.Average(a => a.Val), min2.Average(a => a.Val)); // sc_cost=max(mean(a1),mean(a2));
// % warp each coordinate
axt = cx.GetSubMatrix(n_good, 3, 0, 1).Transpose(); // axt是cx中的最后三个，即a的x分量
wxt = cx.GetSubMatrix(0, n_good, 0, 1).Transpose(); // wxt是cs中的前n个，即w的x分量
ayt = cy.GetSubMatrix(n_good, 3, 0, 1).Transpose();
wyt = cy.GetSubMatrix(0, n_good, 0, 1).Transpose();
d2 = Dist2(X3b, X).Each(v => v > 0 ? v : 0); // d2=max(dist2(X3b,X),0);
U = d2.PointMultiply(d2.Each(v => Math.Log(v + Epsilon))); // U=d2.*log(d2+eps);
Debug("MatchCost:{0:F4}\taff_cost:{1:F4}\tsc_cost:{2:F4}\tE:{3:F8}", matchcost, aff_cost, sc_cost, E);
var Z = Transformation(X, U, axt, wxt, ayt, wyt);
//% apply the warp to the tangent vectors to get the new angles
var Xtan = X + tan_eps * MatrixUtils.RankHorizon(t1.Each(Math.Cos), t1.Each(Math.Sin)); // Xtan=X+tan_eps*[cos(t1) sin(t1)];
d2 = Dist2(X3b, Xtan).Each(v => v > 0 ? v : 0); // d2=max(dist2(X3b,Xtan),0);
U = d2.PointMultiply(d2.Each(v => Math.Log(v + Epsilon))); // U=d2.*log(d2+eps);
//Transformation(Xtan, U, axt, wxt, ayt, wyt, out fx, out fy);
//var Ztan = MatrixUtils.RankVertical(fx, fy).Transpose(); // Ztan=[fx; fy]';
var Ztan = Transformation(Xtan, U, axt, wxt, ayt, wyt);
for (int i = 0; i < nsamp; ++i) {
tk[i, 0] = Math.Atan2(Ztan[i, 1] - Z[i, 1], Ztan[i, 0] - Z[i, 0]);
}//tk=atan2(Ztan(:,2)-Z(:,2),Ztan(:,1)-Z(:,1));
timeused += timer.StopAndSay("通过解出来的变换对点和梯度进行变换");
#endregion
#region figure 4 显示变形后的点集
if (display_flag) {
//figure(4)
//plot(Z(:,1),Z(:,2),'b+',Y(:,1),Y(:,2),'ro');
//axis('ij')
//title(['k=' int2str(k) ', \lambda_o=' num2str(lambda_o) ', I_f=' num2str(E) ', aff.cost=' num2str(aff_cost) ', SC cost=' num2str(sc_cost)])
//axis([1 N2 1 N1])
//% show warped coordinate grid
//fx_aff=cx(n_good+1:n_good+3)'*[ones(1,M); x'; y'];
//d2=dist2(X3b,[x y]);
//fx_wrp=cx(1:n_good)'*(d2.*log(d2+eps));
//fx=fx_aff+fx_wrp;
//fy_aff=cy(n_good+1:n_good+3)'*[ones(1,M); x'; y'];
//fy_wrp=cy(1:n_good)'*(d2.*log(d2+eps));
//fy=fy_aff+fy_wrp;
//hold on
//plot(fx,fy,'k.','markersize',1)
//hold off
//drawnow
d2 = Dist2(X3b, MatrixUtils.RankHorizon(coordX, coordY));//d2=dist2(X3b,[x y]);
U = d2.PointMultiply(d2.Each(v => Math.Log(v + Epsilon)));
//Transformation(MatrixUtils.RankHorizon(coordX, coordY), U, axt, wxt, ayt, wyt, out fx, out fy);
//var coordsT = MatrixUtils.RankVertical(fx, fy).Transpose();
var coordsT = Transformation(MatrixUtils.RankHorizon(coordX, coordY), U, axt, wxt, ayt, wyt);
Draw(Z, Y, null, coordsT, N2, N1, String.Format(@"D:\Play Data\Iteration\变换\{0}.bmp", k),
String.Format("Iter={0}\nλo={1:F4},If={2:F4},aff_cost{3:F4},sc_cost{4:F4}", k, lambda_o, E, aff_cost, sc_cost));
}
#endregion
Xk = Z.Clone();
++k;
}
#endregion
#region 迭代完成后的代价计算
#region 我来尝试计算Dsc
// Xk是Q变换后的结果，而Y是模板图形P
/*
* Dsc = Avg_each_p_in_P(argmin(q in Q, C(p, T(q))) + Avg_each_q_in_Q(argmin(p in P, C(p, T(q)))
* */
double mean_dist_final_1;
var scQ = ComputeSC(Xk.Transpose(), Zeros(1, nsamp), mean_dist_global, out mean_dist_final_1, out_vec_1);
//var scQ = ComputeSC(Xk.Transpose(), Zeros(1, nsamp), mean_dist_1, out mean_dist_final_1, out_vec_1);
//var scQ = ComputeSC(Xk.Transpose(), t1.Transpose(), mean_dist_1, out mean_dist_final_1, out_vec_1);
double mean_dist_final_2;
var scP = ComputeSC(Y.Transpose(), Zeros(1, nsamp), mean_dist_global, out mean_dist_final_2, out_vec_2);
//var scP = ComputeSC(Y.Transpose(), Zeros(1, nsamp), mean_dist_2, out mean_dist_final_2, out_vec_2);
//var scP = ComputeSC(Y.Transpose(), t2.Transpose(), mean_dist_2, out mean_dist_final_2, out_vec_2);
var costmat_final = HistCost(scQ, scP);
double distance_sc = costmat_final.GetRows().Select(row => row.Min()).Average()
+ costmat_final.GetColumns().Select(col => col.Min()).Average();
Debug("distance_sc:{0}", distance_sc);
#endregion
#region 图像变换和插值
timer.Restart();
//[x,y]=meshgrid(1:N2,1:N1);
//x=x(:);y=y(:);
Matrix x = null, y = null;
MatrixUtils.CreateGrid(N1, N2, out x, out y);
//int M = N1 * N2; // M=length(x);
d2 = Dist2(X3b, MatrixUtils.RankHorizon(x, y));//d2=dist2(X3b,[x y]);
U = d2.PointMultiply(d2.Each(v => Math.Log(v + Epsilon)));
//Transformation(MatrixUtils.RankHorizon(x, y), U, axt, wxt, ayt, wyt, out fx, out fy);
var fxy = Transformation(MatrixUtils.RankHorizon(x, y), U, axt, wxt, ayt, wyt);
//disp('computing warped image...')
//V1w=griddata(reshape(fx,N1,N2),reshape(fy,N1,N2),V1,reshape(x,N1,N2),reshape(y,N1,N2));
Matrix V1w = Interpolation(
fxy.GetSubMatrix(0, fxy.Rows, 0, 1).Reshape(N1, N2),
fxy.GetSubMatrix(0, fxy.Rows, 1, 1).Reshape(N1, N2),
V1
);
#region 这个山寨插值方法会造成图像裂缝，用闭运算来尝试修补
Image<Gray, Byte> img = new Image<Gray, byte>(N2, N1);
for (int i = 0; i < N2; ++i) {
for (int j = 0; j < N1; ++j) {
img[i, j] = new Gray(V1w[i, j] * 255);
}
}
var see = new StructuringElementEx(new int[,] { { 1, 1, 1 }, { 1, 1, 1 }, { 1, 1, 1 } }, 1, 1);
//img = img.MorphologyEx(see, Emgu.CV.CvEnum.CV_MORPH_OP.CV_MOP_CLOSE, 1);
img = img.Dilate(1).Erode(1);
for (int i = 0; i < N2; ++i) {
for (int j = 0; j < N1; ++j) {
V1w[i, j] = img[i, j].Intensity / 255;
}
}
img.Dispose();
#endregion
timeused += timer.StopAndSay("图像变换和插值");
#endregion
//fz=find(isnan(V1w));
//V1w(fz)=0;
var ssd = (V2 - V1w).Each(v => v * v);//ssd=(V2-V1w).^2; %%%%%%SSD在这里
var ssd_global = ssd.SumAll();//ssd_global=sum(ssd(:));
Debug("ssd_global:{0}", ssd_global);
#region figure 5
if (display_flag) {
// figure(5)
// subplot(2,2,1)
// im(V1)
// subplot(2,2,2)
// im(V2)
// subplot(2,2,4)
// im(V1w)
// title('V1 after warping')
// subplot(2,2,3)
// im(V2-V1w)
// h=title(['SSD=' num2str(ssd_global)]);
// colormap(cmap)
}
#endregion
#region 窗口SSD比较
timer.Restart();
//%%%
//%%% windowed SSD comparison
//%%%
var wd = 2 * w + 1;//wd=2*w+1;
var win_fun = MatrixUtils.GaussianKernal(wd); //win_fun=gaussker(wd);
//% extract sets of blocks around each coordinate
//% first do 1st shape; need to use transformed coords.
var win_list_1 = Zeros(nsamp, wd * wd);//win_list_1=zeros(nsamp,wd^2);
for (int qq = 0; qq < nsamp; ++qq) {
int row_qq = (int)Xk[qq, 1],// row_qq=round(Xk(qq,2));
col_qq = (int)Xk[qq, 0];// col_qq=round(Xk(qq,1));
row_qq = Math.Max(w + 1, Math.Min(N1 - w - 1, row_qq));// row_qq=max(w+1,min(N1-w,row_qq));
col_qq = Math.Max(w + 1, Math.Min(N2 - w - 1, col_qq));// col_qq=max(w+1,min(N2-w,col_qq));
// tmp=V1w(row_qq-w:row_qq+w,col_qq-w:col_qq+w);
var tmp = V1w.GetSubMatrix(row_qq - w, w * 2 + 1, col_qq - w, w * 2 + 1);
tmp = win_fun.PointMultiply(tmp);// tmp=win_fun.*tmp;
// win_list_1(qq,:)=tmp(:)';
win_list_1.SetSubMatrix(qq, 1, 0, win_list_1.Columns, tmp.Reshape(1, tmp.Rows * tmp.Columns));
}
//% now do 2nd shape
var win_list_2 = Zeros(nsamp, wd * wd);//win_list_2=zeros(nsamp,wd^2);
for (int qq = 0; qq < nsamp; ++qq) {
int row_qq = (int)Y[qq, 1],// row_qq = round(Y(qq, 2));
col_qq = (int)Y[qq, 0];// col_qq = round(Y(qq, 1));
row_qq = Math.Max(w + 1, Math.Min(N1 - w - 1, row_qq));// row_qq=max(w+1,min(N1-w,row_qq));
col_qq = Math.Max(w + 1, Math.Min(N2 - w - 1, col_qq));// col_qq=max(w+1,min(N2-w,col_qq));
// tmp=V2(row_qq-w:row_qq+w,col_qq-w:col_qq+w)
var tmp = V2.GetSubMatrix(row_qq - w, w * 2 + 1, col_qq - w, w * 2 + 1);
tmp = win_fun.PointMultiply(tmp);// tmp=win_fun.*tmp;
win_list_2.SetSubMatrix(qq, 1, 0, win_list_2.Columns, tmp.Reshape(1, tmp.Rows * tmp.Columns));// win_list_2(qq,:)=tmp(:)';
}
var ssd_all = Dist2(win_list_1, win_list_2).Each(Math.Sqrt);//ssd_all=sqrt(dist2(win_list_1,win_list_2));
timeused += timer.StopAndSay("窗口SSD比较");
#endregion
#region 最后的KNN，不知道干嘛用
timer.Restart();
//%%%%%%% KNN在此
//% loop over nearest neighbors in both directions, project in
//% both directions, take maximum
double cost_1 = 0, cost_2 = 0;
//List<double> cost_1 = new List<double>(), cost_2 = new List<double>();
for (int qq = 0; qq < nsamp; ++qq) {
cost_1 += ssd_all[qq, min2[qq].Idx];// cost_1=cost_1+ssd_all(qq,b2(qq));
cost_2 += ssd_all[min1[qq].Idx, qq];// cost_2=cost_2+ssd_all(b1(qq),qq);
//cost_1.Add(ssd_all[qq, min2[qq].Idx]);
//cost_2.Add(ssd_all[min1[qq].Idx, qq]);
}
var ssd_local = (1.0 / nsamp) * Math.Max(cost_1, cost_2);
var ssd_local_avg = (1.0 / nsamp) * 0.5 * (cost_1 + cost_2);
//var ssd_local = (1.0 / nsamp) * Math.Max(cost_1.Average(), cost_2.Average());//ssd_local=(1/nsamp)*max(mean(cost_1),mean(cost_2));
//var ssd_local_avg = (1.0 / nsamp) * 0.5 * (cost_1.Average() + cost_2.Average());//ssd_local_avg=(1/nsamp)*0.5*(mean(cost_1)+mean(cost_2));
Debug("ssd_local:{0}", ssd_local);
Debug("ssd_local_avg:{0}", ssd_local_avg);
timeused += timer.StopAndSay("计算ssd_local");
#region 最后的组合图
//if display_flag
//% set(h,'string',['local SSD=' num2str(ssd_local) ', avg. local SSD=' num2str(ssd_local_avg)])
// set(h,'string',['local SSD=' num2str(ssd_local)])
//end
if (display_flag) {
Draw(V1, @"D:\Play Data\Iteration\结果\0-V1.bmp", "V1");
Draw(V2, @"D:\Play Data\Iteration\结果\1-V2.bmp", "V2");
Draw(V1w, @"D:\Play Data\Iteration\结果\2-V1w.bmp", "V1 after warping");
Draw(V2 - V1w, @"D:\Play Data\Iteration\结果\3-V2-V1w.bmp",
String.Format("local SSD={0:F8}", ssd_local_avg));
}
#endregion
#endregion
#endregion
var distance = 1.6 * ssd_local_avg + distance_sc + 0.3 * E; // 不知道最后的距离公式到底是什么
Debug("distance={0:F8}", distance);
return distance;
}
/// <summary>
///
/// </summary>
/// <param name="stiffnessMatrix"></param>
/// <param name="forceVector"></param>
/// <returns></returns>
protected override KeyedVector<NodalDegreeOfFreedom> Solve(StiffnessMatrix stiffnessMatrix, KeyedVector<NodalDegreeOfFreedom> forceVector)
{
Svd<NodalDegreeOfFreedom, NodalDegreeOfFreedom> svd = new Svd<NodalDegreeOfFreedom, NodalDegreeOfFreedom>(stiffnessMatrix, true);
return svd.Solve(forceVector);
}
