void matrixSquareRoot(Matrix* A, Matrix* S)
{
//Approach is to do an SVD of A, then set S to V * sqrt(Sigma) * Vtransposed
int i, j;
Matrix* U = allocateMatrix(A->rows, A->columns);
Matrix* Vtransposed = allocateMatrix(A->rows, A->columns);
Matrix* Sigma = allocateMatrix(A->rows, A->columns);
//First do an SVD
singularValueDecomposition(A, U, Vtransposed, Sigma);
//Now calculate sqrt(Sigma) * Vtransposed and stick it in U
for(i = 0; i < Sigma->rows; i++)
{
for(j = 0; j < Sigma->columns; j++)
{
U->pointer[i + j * U->rows] = Vtransposed->pointer[i + j * Vtransposed->rows] * sqrt(Sigma->pointer[i + i * Sigma->rows]);
}
}
multiplyMatrices(Vtransposed, U, S, 1, 0);
freeMatrix(U);
freeMatrix(Vtransposed);
freeMatrix(Sigma);
}
void PivotMDS::pivotMDSLayout(GraphAttributes& GA)
{
const Graph& G = GA.constGraph();
bool use3D = GA.has(GraphAttributes::threeD) && DIMENSION_COUNT > 2;
const int n = G.numberOfNodes();
// trivial cases
if (n == 0)
return;
if (n == 1) {
node v1 = G.firstNode();
GA.x(v1) = 0.0;
GA.y(v1) = 0.0;
if (use3D)
GA.z(v1) = 0.0;
return;
}
// check whether the graph is a path or not
const node head = getRootedPath(G);
if (head != nullptr) {
doPathLayout(GA, head);
}
else {
Array<Array<double> > pivDistMatrix;
// compute the pivot matrix
getPivotDistanceMatrix(GA, pivDistMatrix);
// center the pivot matrix
centerPivotmatrix(pivDistMatrix);
// init the coordinate matrix
Array<Array<double> > coord(DIMENSION_COUNT);
for (int i = 0; i < coord.size(); i++) {
coord[i].init(n);
}
// init the eigen values array
Array<double> eVals(DIMENSION_COUNT);
singularValueDecomposition(pivDistMatrix, coord, eVals);
// compute the correct aspect ratio
for (int i = 0; i < coord.size(); i++) {
eVals[i] = sqrt(eVals[i]);
for (int j = 0; j < n; j++) {
coord[i][j] *= eVals[i];
}
}
// set the new positions to the graph
int i = 0;
for (node v : G.nodes)
{
GA.x(v) = coord[0][i];
GA.y(v) = coord[1][i];
if (use3D){
GA.z(v) = coord[2][i];//cout << coord[2][i] << "\n";
}
++i;
}
}
}
void LeastSquares::compute_leastSquaresAproxForCurve(int n_)
{
data->lsq_points.clear();
n = n_;
if (n > m)
{
std::cout << "Grad größer als Punkteanzahl" << std::endl;
return;
}
TNT::Array2D<double> A(m, n+1);
for (int i = 0; i < m; i++)
{
for (int j = 0; j <= n; j++)
{
A[i][j] = std::pow(ti[i], j);
}
}
TNT::Array2D<double> P_plus = singularValueDecomposition(A, m, n+1);
c_x = TNT::Array2D<double>(m, 1);
c_y = TNT::Array2D<double>(m, 1);
c_z = TNT::Array2D<double>(m, 1);
for (int i = 0; i < m; i++)
{
c_x[i][0] = x_sorted[i];
c_y[i][0] = y_sorted[i];
c_z[i][0] = z_sorted[i];
}
c_x = matmult(P_plus, c_x);
c_y = matmult(P_plus, c_y);
c_z = matmult(P_plus, c_z);
result_x = matmult(A, c_x);
result_y = matmult(A, c_y);
result_z = matmult(A, c_z);
for (int i = 0; i < m; i++)
{
data->lsq_points.push_back(Vector3D(result_x[i][0], result_y[i][0], result_z[i][0]));
}
}
//----------------------------------------------------------------------
void calc_svd(){
//SVD(singular value decomposition)
int m = DIMENSION;
int n = DIMENSION;
A[0] = covariance_matrix[0][0]; A[1] = covariance_matrix[1][0];
A[2] = covariance_matrix[0][1]; A[3] = covariance_matrix[1][1];
singularValueDecomposition(&(A[0]), m, n, &(svd_value[0]), &(svd_vector[0]));
/*
for(int i=0; i<n; i++) {
printf("固有値 %d: %f\n", i, svd_value[i]);
}
for(int i=0; i<n; i++) {
printf("固有ベクトル: ");
for(int j=0; j<m; j++)
printf("%f ", svd_vector[m*i+j]);
printf("\n");
}
*/
}
void rectify::initializeProjectiveTransform()
{
Matrix3f matPCPT;
matPCPT.m[0][0] = (scrWidth -1)*(scrWidth -1); matPCPT.m[0][1] = (scrWidth -1)*(scrHeight -1); matPCPT.m[0][2] = (scrWidth -1)*2;
matPCPT.m[1][0] = (scrWidth -1)*(scrHeight -1);matPCPT.m[1][1] = (scrHeight -1)*(scrHeight -1); matPCPT.m[1][2] = (scrHeight -1)*2;
matPCPT.m[2][0] = (scrWidth -1)*2; matPCPT.m[2][1] = (scrHeight -1)*2; matPCPT.m[2][2] = 4;
matPCPT = matPCPT * 0.25;
Matrix3f matPPT;
matPPT.m[0][0] = (scrWidth * scrWidth -1); matPPT.m[0][1] = 0; matPPT.m[0][2] = 0;
matPPT.m[1][0] = 0; matPPT.m[1][1] = (scrHeight * scrHeight -1); matPPT.m[1][2] = 0;
matPPT.m[2][0] = 0; matPPT.m[2][1] = 0; matPPT.m[2][2] = 0;
float var = (scrWidth * scrHeight)/12.0;
matPPT = matPPT * var;
vector3d epipoleFirst = global::epipoleA;
Matrix3f epiASymmetric; //take global::epipoleA and create antiSymmetric Matrix: A = -transpose(A)
epiASymmetric.m[0][0] = 0; epiASymmetric.m[0][1] = epipoleFirst.z; epiASymmetric.m[0][2] = -epipoleFirst.y;
epiASymmetric.m[1][0] = -epipoleFirst.z; epiASymmetric.m[1][1] = 0; epiASymmetric.m[1][2] = epipoleFirst.x;
epiASymmetric.m[2][0] = epipoleFirst.y; epiASymmetric.m[2][1] = -epipoleFirst.x; epiASymmetric.m[2][2] = 0;
Matrix3f fundamentalMat = global::fundamentalMatrix;
//fundamentalMat.m[1][1] *= (-1);
Matrix3f rectMatA = epiASymmetric.ReturnTranspose() * matPPT * epiASymmetric;
Matrix3f rectMatB = epiASymmetric.ReturnTranspose() * matPCPT * epiASymmetric;
Matrix3f rectMatADash = fundamentalMat.ReturnTranspose() * matPPT * fundamentalMat;
Matrix3f rectMatBDash = fundamentalMat.ReturnTranspose() * matPCPT * fundamentalMat;
Matrix3f orthoMatD, orthoMatDInv, orthoMatD_Dash, orthoMatDInv_Dash;
singularValueDecomposition(rectMatA, orthoMatD, orthoMatDInv); //rectMatA = orthoMatD' * orthoMatD; ' = transpose
singularValueDecomposition(rectMatADash, orthoMatD_Dash, orthoMatDInv_Dash);
/*
std::cout<<std::endl<<" for rectMatA "<<std::endl;
rectMatA.Display("rectMatA");
orthoMatD.Display("matD");
(orthoMatD.ReturnTranspose() * orthoMatD).Display("MUL");
orthoMatDInv.Display("matDInv");
(orthoMatD * orthoMatDInv).Display("ID");
std::cout<<std::endl<<" for rectMatB "<<std::endl;
rectMatADash.Display("rectMatADash");
orthoMatD_Dash.Display("matDDash");
(orthoMatD_Dash.ReturnTranspose() * orthoMatD_Dash).Display("MUL");
orthoMatDInv_Dash.Display("matDDashInv");
(orthoMatD_Dash * orthoMatDInv_Dash).Display("IDDash");
*/
// to maximize weight matrix
Matrix3f weightMat, weightMatDash;
weightMat = orthoMatDInv.ReturnTranspose() * rectMatB * orthoMatDInv;
weightMatDash = orthoMatDInv_Dash.ReturnTranspose() * rectMatBDash * orthoMatDInv_Dash;
vector3d way, way_dash;
powerMaxEigenVec(weightMat, way);
powerMaxEigenVec(weightMatDash, way_dash);
vector3d zed, zed_dash;
zed = orthoMatDInv * way;
zed_dash = orthoMatDInv_Dash * way_dash;
zed.Normalize();
zed_dash.Normalize();
vector3d Zedd = zed * 0.5 + zed_dash * 0.5;
vector3d dublu, dubluDash;
dublu = epiASymmetric * Zedd;
dubluDash = fundamentalMat * Zedd;
dublu.change(dublu.x/dublu.z, dublu.y/dublu.z, 1.0);
dubluDash.change(dubluDash.x/dubluDash.z, dubluDash.y/dubluDash.z, 1.0);
projectiveTrans.m[0][0] = 1; projectiveTrans.m[0][1] = 0; projectiveTrans.m[0][2] = 0;
projectiveTrans.m[1][0] = 0; projectiveTrans.m[1][1] = 1; projectiveTrans.m[1][2] = 0;
projectiveTrans.m[2][0] = dublu.x; projectiveTrans.m[2][1] = dublu.y; projectiveTrans.m[2][2] = 1;
projectiveTransDash.m[0][0] = 1; projectiveTransDash.m[0][1] = 0; projectiveTransDash.m[0][2] = 0;
projectiveTransDash.m[1][0] = 0; projectiveTransDash.m[1][1] = 1; projectiveTransDash.m[1][2] = 0;
projectiveTransDash.m[2][0] = dubluDash.x; projectiveTransDash.m[2][1] = dubluDash.y; projectiveTransDash.m[2][2] = 1;
(projectiveTrans * global::epipoleA).Display("epiA");
(projectiveTransDash * global::epipoleB).Display("epiB");
projectiveTrans.Display("projT");
projectiveTransDash.Display("projTD");
}
void LeastSquares::smoothSurface(Data* data_, int n_, bool doNewton)
{
data = data_;
//clear alle verwendeten Vektoren
reset();
if (n_ < 3)
{
std::cout << "n muss >= 3 sein" << std::endl;
return;
}
//Bereite den Vektor vor, der die neu berechneten Punkte enthaelt
//Pro Punkte ein Vektor mit seinen zu mittelnden Punkten
smoothPoints = std::vector<std::vector<Vector3D>>(data->points.size());
//Pro Punkt, projeziere 20 nearest neighbors in Tangentialebene, benutze dann die x und z Werte als Parametrisierung um einen LeastSquares fit zu berechnen
//Sample dann mit der bereits erstellten Paramestrisierung die Punkte und speichere für jeden nearestNeighbors sein errechneten Punkt für Mitteln spaeter
//Wenn eingestellt, führe eine Newton-Iteration vor dem fit durch
for (unsigned int p = 0; p < data->points.size(); p++)
{
//Erst die Projektion in die Tangentialebene
//http://stackoverflow.com/questions/9605556/how-to-project-a-3d-point-to-a-3d-plane
projected_points.clear();
for (int i = 0; i < 20; i++)
{
Vector3D tmpVecP = Vector3D(data->k_nearest_neighbors[p][i].point.dx, data->k_nearest_neighbors[p][i].point.dy, data->k_nearest_neighbors[p][i].point.dz);
Vector3D tmpVec = tmpVecP - Vector3D(data->points[p].dx, data->points[p].dy, data->points[p].dz);
double dot = (tmpVec[0] * data->normals[p][0]) + (tmpVec[1] * data->normals[p][1]) + (tmpVec[2] * data->normals[p][2]);
tmpVec = tmpVecP - (data->normals[p] * dot);
projected_points.push_back(tmpVec);
//smoothPoints[data->k_nearest_neighbors[p][i].index].push_back(tmpVec);
}
ti.clear();
vi.clear();
//Erstelle Parametrisierung
for (unsigned int i = 0; i < projected_points.size(); i++)
{
ti.push_back(projected_points[i].dx);
vi.push_back(projected_points[i].dz);
}
m = projected_points.size();
n = (n_ + 1)*(n_ + 1);
int m_ = m;
//Fuehre LeastSquares fit ganz normal durch
TNT::Array2D<double> A(m, n);
for (int i = 0; i < m; i++)
{
for (int j = 0; j < n; j++)
{
int tExp = j % (n_ + 1);
int vExp = j / (n_ + 1);
A[i][j] = std::pow(ti[i], tExp) * std::pow(vi[i], vExp);
}
}
TNT::Array2D<double> P_plus = singularValueDecomposition(A, m_, n);
c_x = TNT::Array2D<double>(m_, 1);
c_y = TNT::Array2D<double>(m_, 1);
c_z = TNT::Array2D<double>(m_, 1);
for (int i = 0; i < m; i++)
{
c_x[i][0] = data->k_nearest_neighbors[p][i].point.dx;
c_y[i][0] = data->k_nearest_neighbors[p][i].point.dy;
c_z[i][0] = data->k_nearest_neighbors[p][i].point.dz;
}
c_x = matmult(P_plus, c_x);
c_y = matmult(P_plus, c_y);
c_z = matmult(P_plus, c_z);
//Berechne einen Schritt mit der Newton-Iteration wenn eingestellt
if (doNewton)
{
for (int h = 0; h < 1; h++)
{
//Fuer jeden nearest Neighbor
for (int i = 0; i < projected_points.size(); i++)
{
//Erstelle Jacobi-Matrix
TNT::Array2D<double> jacobi(2, 2);
jacobi[0][0] = 0.0;
jacobi[0][1] = 0.0;
jacobi[1][0] = 0.0;
jacobi[1][1] = 0.0;
//Partielle Ableitungen
for (int j = 0; j < n; j++)
{
int tExp = j % (n_ + 1);
int vExp = j / (n_ + 1);
//F1 nach x
if (!((tExp - 2) < 0))
jacobi[0][0] += tExp * (tExp - 1) * std::pow(ti[i], tExp - 2) * std::pow(vi[i], vExp);
//F1 nach y
if (!((vExp - 1) < 0 && (tExp - 1) < 0))
jacobi[0][1] += tExp * std::pow(ti[i], tExp - 1) * vExp * std::pow(vi[i], vExp - 1);
//F2 nach x
if (!((vExp - 1) < 0 && (tExp - 1) < 0))
jacobi[1][0] += tExp * std::pow(ti[i], tExp - 1) * vExp * std::pow(vi[i], vExp - 1);
//F2 nach y
if (!((vExp - 2) < 0))
jacobi[1][1] += std::pow(ti[i], tExp) * vExp * (vExp - 1)* std::pow(vi[i], vExp - 2);
}
//Berechne Determinante
double det = ((jacobi[0][0] * jacobi[1][1]) - (jacobi[0][1] * jacobi[1][0]));
//Nur wenn Determinante nicht 0, führe Newton-Iteration Schritt durch
if (det < -0.01 || det > 0.01)
{
//Inverse der Jacobi-Matrix berechnen
double one_det = 1.0f / det;
double a_ = jacobi[1][1] * one_det;
double b_ = -jacobi[0][1] * one_det;
double c_ = -jacobi[1][0] * one_det;
double d_ = jacobi[0][0] * one_det;
jacobi[0][0] = a_;
jacobi[0][1] = b_;
jacobi[1][0] = c_;
jacobi[1][1] = d_;
//Berechne noch fehlenden Vektor mit den ersten Ableitungen nach x und y
TNT::Array2D<double> tivi_delta(2, 1);
tivi_delta[0][0] = 0.0;
tivi_delta[1][0] = 0.0;
for (int j = 0; j < n; j++)
{
int tExp = j % (n_ + 1);
int vExp = j / (n_ + 1);
//Berechne F1, leite Polygon nach x ab
if (!((tExp - 1) < 0))
tivi_delta[0][0] += tExp * std::pow(ti[i], tExp - 1) * std::pow(vi[i], vExp);
//Berechne F2, leite Polygon nach y ab
if (!((vExp - 1) < 0))
tivi_delta[1][0] += std::pow(ti[i], tExp) * vExp * std::pow(vi[i], vExp - 1);
}
tivi_delta[0][0] *= -1.0;
tivi_delta[1][0] *= -1.0;
//Führe letzten Schritt, Multiplikation mit der Inversen der Jacobi-Matrix, durch
tivi_delta = matmult(jacobi, tivi_delta);
//Addire Delta t/v zu den ti/vi
ti[i] += tivi_delta[0][0];
vi[i] += tivi_delta[1][0];
}
}
}
}
for (int i = 0; i < m; i++)
{
for (int j = 0; j < n; j++)
{
int tExp = j % (n_ + 1);
int vExp = j / (n_ + 1);
A[i][j] = std::pow(ti[i], tExp) * std::pow(vi[i], vExp);
}
}
//Benutze Parametrisierung für den fit auch zum Berechnen der Punkte
result_x = matmult(A, c_x);
result_y = matmult(A, c_y);
result_z = matmult(A, c_z);
for (int l = 0; l < m_; l++)
{
smoothPoints[data->k_nearest_neighbors[p][l].index].push_back(Vector3D(result_x[l][0], result_y[l][0], result_z[l][0]));
}
//std::cout << p + 1 << " von " << data->points.size() << " Punkten geglaettet." << std::endl;
}
//Punkte müssen jetzt noch gemittelt werden
Vector3D tmpVec__;
for (unsigned int i = 0; i < smoothPoints.size(); i++)
{
tmpVec__ = Vector3D(0.0f, 0.0f, 0.0f);
for (unsigned int j = 0; j < smoothPoints[i].size(); j++)
{
tmpVec__ = Vector3D(tmpVec__.dx + smoothPoints[i][j].dx, tmpVec__.dy + smoothPoints[i][j].dy, tmpVec__.dz + smoothPoints[i][j].dz);
}
tmpVec__ = Vector3D(tmpVec__.dx / smoothPoints[i].size(), tmpVec__.dy / smoothPoints[i].size(), tmpVec__.dz / smoothPoints[i].size());
data->lsq_points.push_back(tmpVec__);
}
}
void LeastSquares::compute_leastSquaresAproxForPlane(int n_)
{
//clear den Vektor, der die Ergebnispunkte enthält
data->lsq_points.clear();
//Setzen von n (Anzahl der Komponenten pro Polygon) und m (Anzahl der Punkte)
n = (n_ + 1)*(n_ + 1);
int m_ = m;
if (n_ > m)
{
std::cout << "Grad größer als Punkteanzahl" << std::endl;
return;
}
//Erstellen von A, die dann bei der Singulärwertzerlegung benutzt wird
//Pro eingelesenem Punkt ein Eintrag in A
TNT::Array2D<double> A(m_, n);
for (int i = 0; i < m; i++)
{
for (int j = 0; j < n; j++)
{
int tExp = j % (n_ + 1);
int vExp = j / (n_ + 1);
//Ich benutze einfach die x und z Werte der Punkte (werden aus einer .off Datei gelesen) als Stützstellen
A[i][j] = std::pow(x_sorted[i], tExp) * std::pow(z_sorted[i], vExp);
}
}
//Singulärwertzerlegung nach Ingos Beispiel
TNT::Array2D<double> P_plus = singularValueDecomposition(A, m_, n);
c_x = TNT::Array2D<double>(m_, 1);
c_y = TNT::Array2D<double>(m_, 1);
c_z = TNT::Array2D<double>(m_, 1);
//Initialisiere c_* mit den eingelesenen Punkten
for (int i = 0; i < m; i++)
{
c_x[i][0] = x_sorted[i];
c_y[i][0] = y_sorted[i];
c_z[i][0] = z_sorted[i];
}
//Berechne Koeffizienten
c_x = matmult(P_plus, c_x);
c_y = matmult(P_plus, c_y);
c_z = matmult(P_plus, c_z);
//Erstelle zweite Matrix mit den Werten zum samplen
//Pro Punkt und x-Wert einmal alle z Werte durch um rechteckigen Samplebereich zu erreichen
/*
ti[0] vi[0] / ti[0] vi[1] / ti[0] vi[2] / ti[0] vi[3] ...
ti[1] vi[0] / ti[1] vi[1] / ti[1] vi[2] / ti[1] vi[3] ...
.
.
.
*/
//Die ti/vi Werte werden in der Funktion initPlane erstellt
TNT::Array2D<double> Atwo(size*size, n);
for (int k = 0; k < size; k++)
{
for (int i = 0; i < size; i++)
{
for (int j = 0; j < n; j++)
{
int tExp = j % (n_ + 1);
int vExp = j / (n_ + 1);
Atwo[(k * size) + i][j] = std::pow(ti[k], tExp) * std::pow(vi[i], vExp);
}
}
}
//Werte die Matrix Atwo mit den Samplestützstellen und die Koeffizienten aus
result_x = matmult(Atwo, c_x);
result_y = matmult(Atwo, c_y);
result_z = matmult(Atwo, c_z);
//Fülle Vektor mit Ergebnispunkten
for (int i = 0; i < size*size; i++)
{
data->lsq_points.push_back(Vector3D(result_x[i][0], result_y[i][0], result_z[i][0]));
}
}
