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])); } }