ICP_API float __stdcall ICP(Point3f *verts1, Point3f *verts2, int nVerts1, int nVerts2, float *R, float *t, int maxIter) { PointCloud cloud1; cloud1.pts = vector<Point3f>(verts1, verts1 + nVerts1); cv::Mat matR(3, 3, CV_32F, R); cv::Mat matT(1, 3, CV_32F, t); cv::Mat verts2Mat(nVerts2, 3, CV_32F, (float*)verts2); float error = 1; for (int iter = 0; iter < maxIter; iter++) { vector<Point3f> matched1, matched2; vector<float> distances(nVerts2); vector<size_t> indices(nVerts2); FindClosestPointForEach(cloud1, verts2Mat, distances, indices); vector<float> matchDistances; vector<int> matchIdxs(nVerts1, -1); for (int i = 0; i < nVerts2; i++) { int pos = matchIdxs[indices[i]]; if (pos != -1) { if (matchDistances[pos] < distances[i]) continue; } Point3f temp; temp.X = verts2Mat.at<float>(i, 0); temp.Y = verts2Mat.at<float>(i, 1); temp.Z = verts2Mat.at<float>(i, 2); if (pos == -1) { matched1.push_back(verts1[indices[i]]); matched2.push_back(temp); matchDistances.push_back(distances[i]); matchIdxs[indices[i]] = matched1.size() - 1; } else { matched2[pos] = temp; matchDistances[pos] = distances[i]; } } RejectOutlierMatches(matched1, matched2, matchDistances, 2.5); //error = 0; //for (int i = 0; i < matchDistances.size(); i++) //{ // error += sqrt(matchDistances[i]); //} //error /= matchDistances.size(); //cout << error << endl; cv::Mat matched1MatCv(matched1.size(), 3, CV_32F, matched1.data()); cv::Mat matched2MatCv(matched2.size(), 3, CV_32F, matched2.data()); cv::Mat tempT; cv::reduce(matched1MatCv - matched2MatCv, tempT, 0, CV_REDUCE_AVG); for (int i = 0; i < verts2Mat.rows; i++) { verts2Mat.row(i) += tempT; } for (int i = 0; i < matched2MatCv.rows; i++) { matched2MatCv.row(i) += tempT; } cv::Mat M = matched2MatCv.t() * matched1MatCv; cv::SVD svd; svd(M); cv::Mat tempR = svd.u * svd.vt; double det = cv::determinant(tempR); if (det < 0) { cv::Mat temp = cv::Mat::eye(3, 3, CV_32F); temp.at<float>(2, 2) = -1; tempR = svd.u * temp * svd.vt; } verts2Mat = verts2Mat * tempR; matT += tempT * matR.t(); matR = matR * tempR; } memcpy(verts2, verts2Mat.data, verts2Mat.rows * sizeof(float) * 3); memcpy(R, matR.data, 9 * sizeof(float)); memcpy(t, matT.data, 3 * sizeof(float)); return error; }
bool VersionManager::isResNeedUpdate() { return svd().ver.is_high_to(rvd().ver); }
void ForecastMachine::smap_prediction(const size_t start, const size_t end) { size_t curr_pred, effective_nn, E = data_vectors[0].size(); double avg_distance; vec weights; std::vector<size_t> nearest_neighbors; MatrixXd A, S_inv; VectorXd B, S, x; double max_s, pred; std::vector<size_t> temp_lib; for(size_t k = start; k < end; ++k) { curr_pred = which_pred[k]; // find nearest neighbors if(CROSS_VALIDATION) { temp_lib = which_lib; adjust_lib(curr_pred); nearest_neighbors = find_nearest_neighbors(distances.innerVector(curr_pred)); which_lib = temp_lib; } else { nearest_neighbors = find_nearest_neighbors(distances.innerVector(curr_pred)); } effective_nn = nearest_neighbors.size(); if(effective_nn == 0) { predicted[curr_pred] = qnan; LOG_WARNING("no nearest neighbors found; using NA for forecast"); continue; } weights.assign(effective_nn, 1.0); // default is for theta = 0 if(theta > 0.0) { // compute average distance avg_distance = 0; for(auto& neighbor: nearest_neighbors) { avg_distance += distances.coeff(curr_pred, neighbor); } avg_distance /= effective_nn; // compute weights for(size_t i = 0; i < effective_nn; ++i) weights[i] = exp(-theta * distances.coeff(curr_pred, nearest_neighbors[i]) / avg_distance); } // setup matrices for SVD A.resize(effective_nn, E+1); B.resize(effective_nn); for(size_t i = 0; i < effective_nn; ++i) { B(i) = weights[i] * targets[nearest_neighbors[i]]; for(size_t j = 0; j < E; ++j) A(i, j) = weights[i] * data_vectors[nearest_neighbors[i]][j]; A(i, E) = weights[i]; } // perform SVD JacobiSVD<MatrixXd> svd(A, ComputeThinU | ComputeThinV); // remove singular values close to 0 S = svd.singularValues(); S_inv = MatrixXd::Zero(E+1, E+1); max_s = S(0) * 1e-5; for(size_t j = 0; j <= E; ++j) { if(S(j) >= max_s) S_inv(j, j) = 1/S(j); } // perform back-substitution to solve x = svd.matrixV() * S_inv * svd.matrixU().transpose() * B; pred = 0; for(size_t j = 0; j < E; ++j) pred += x(j) * data_vectors[curr_pred][j]; pred += x(E); if(SAVE_SMAP_COEFFICIENTS) { for(size_t j = 0; j <= E; ++j) smap_coefficients[curr_pred][j] = x(j); } // save prediction predicted[curr_pred] = pred; } return; }
void Foam::FitData<FitDataType, ExtendedStencil, Polynomial>::calcFit ( scalarList& coeffsi, const List<point>& C, const scalar wLin, const label facei ) { vector idir(1,0,0); vector jdir(0,1,0); vector kdir(0,0,1); findFaceDirs(idir, jdir, kdir, facei); // Setup the point weights scalarList wts(C.size(), scalar(1)); wts[0] = centralWeight_; if (linearCorrection_) { wts[1] = centralWeight_; } // Reference point point p0 = this->mesh().faceCentres()[facei]; // Info << "Face " << facei << " at " << p0 << " stencil points at:\n" // << C - p0 << endl; // p0 -> p vector in the face-local coordinate system vector d; // Local coordinate scaling scalar scale = 1; // Matrix of the polynomial components scalarRectangularMatrix B(C.size(), minSize_, scalar(0)); for(label ip = 0; ip < C.size(); ip++) { const point& p = C[ip]; d.x() = (p - p0)&idir; d.y() = (p - p0)&jdir; # ifndef SPHERICAL_GEOMETRY d.z() = (p - p0)&kdir; # else d.z() = mag(p) - mag(p0); # endif if (ip == 0) { scale = cmptMax(cmptMag((d))); } // Scale the radius vector d /= scale; Polynomial::addCoeffs ( B[ip], d, wts[ip], dim_ ); } // Additional weighting for constant and linear terms for(label i = 0; i < B.n(); i++) { B[i][0] *= wts[0]; B[i][1] *= wts[0]; } // Set the fit label stencilSize = C.size(); coeffsi.setSize(stencilSize); bool goodFit = false; for(int iIt = 0; iIt < 8 && !goodFit; iIt++) { SVD svd(B, SMALL); scalar maxCoeff = 0; label maxCoeffi = 0; for(label i=0; i<stencilSize; i++) { coeffsi[i] = wts[0]*wts[i]*svd.VSinvUt()[0][i]; if (mag(coeffsi[i]) > maxCoeff) { maxCoeff = mag(coeffsi[i]); maxCoeffi = i; } } if (linearCorrection_) { goodFit = (mag(coeffsi[0] - wLin) < linearLimitFactor_*wLin) && (mag(coeffsi[1] - (1 - wLin)) < linearLimitFactor_*(1 - wLin)) && maxCoeffi <= 1; } else { // Upwind: weight on face is 1. goodFit = (mag(coeffsi[0] - 1.0) < linearLimitFactor_*1.0) && maxCoeffi <= 1; } // if (goodFit && iIt > 0) // { // Info << "FitData<Polynomial>::calcFit" // << "(const List<point>& C, const label facei" << nl // << "Can now fit face " << facei << " iteration " << iIt // << " with sum of weights " << sum(coeffsi) << nl // << " Weights " << coeffsi << nl // << " Linear weights " << wLin << " " << 1 - wLin << nl // << " sing vals " << svd.S() << endl; // } if (!goodFit) // (not good fit so increase weight in the centre and weight // for constant and linear terms) { // if (iIt == 7) // { // WarningIn // ( // "FitData<Polynomial>::calcFit" // "(const List<point>& C, const label facei" // ) << "Cannot fit face " << facei << " iteration " << iIt // << " with sum of weights " << sum(coeffsi) << nl // << " Weights " << coeffsi << nl // << " Linear weights " << wLin << " " << 1 - wLin << nl // << " sing vals " << svd.S() << endl; // } wts[0] *= 10; if (linearCorrection_) { wts[1] *= 10; } for(label j = 0; j < B.m(); j++) { B[0][j] *= 10; B[1][j] *= 10; } for(label i = 0; i < B.n(); i++) { B[i][0] *= 10; B[i][1] *= 10; } } } if (goodFit) { if (linearCorrection_) { // Remove the uncorrected linear coefficients coeffsi[0] -= wLin; coeffsi[1] -= 1 - wLin; } else { // Remove the uncorrected upwind coefficients coeffsi[0] -= 1.0; } } else { // if (debug) // { WarningIn ( "FitData<Polynomial>::calcFit(..)" ) << "Could not fit face " << facei << " Weights = " << coeffsi << ", reverting to linear." << nl << " Linear weights " << wLin << " " << 1 - wLin << endl; // } coeffsi = 0; } }
inline bool op_princomp::direct_princomp ( Mat< std::complex<typename T1::pod_type> >& coeff_out, Mat< std::complex<typename T1::pod_type> >& score_out, Col< typename T1::pod_type >& latent_out, const Base< std::complex<typename T1::pod_type>, T1 >& X, const typename arma_cx_only<typename T1::elem_type>::result* junk ) { arma_extra_debug_sigprint(); arma_ignore(junk); typedef typename T1::pod_type T; typedef std::complex<T> eT; const unwrap_check<T1> Y( X.get_ref(), score_out ); const Mat<eT>& in = Y.M; const uword n_rows = in.n_rows; const uword n_cols = in.n_cols; if(n_rows > 1) // more than one sample { // subtract the mean - use score_out as temporary matrix score_out = in; score_out.each_row() -= mean(in); // singular value decomposition Mat<eT> U; Col< T> s; const bool svd_ok = svd(U, s, coeff_out, score_out); if(svd_ok == false) { return false; } // normalize the eigenvalues s /= std::sqrt( double(n_rows - 1) ); // project the samples to the principals score_out *= coeff_out; if(n_rows <= n_cols) // number of samples is less than their dimensionality { score_out.cols(n_rows-1,n_cols-1).zeros(); Col<T> s_tmp = zeros< Col<T> >(n_cols); s_tmp.rows(0,n_rows-2) = s.rows(0,n_rows-2); s = s_tmp; } // compute the eigenvalues of the principal vectors latent_out = s%s; } else // 0 or 1 samples { coeff_out.eye(n_cols, n_cols); score_out.copy_size(in); score_out.zeros(); latent_out.set_size(n_cols); latent_out.zeros(); } return true; }
void HomographyModel::estimate_from_minimal_set(const vector< ScoredMatch> &data_points) { // given m points estimate the parameters vector // based on vxl rrel_homography2d_est :: fit_from_minimal_set if ( data_points.size() < get_num_points_to_estimate()) throw runtime_error("Not enough points to estimate the HomographyModel parameters"); vnl_matrix< double > A(9, 9, 0.0); for ( int i=0; i < get_num_points_to_estimate(); i+=1 ) { // for i = 0,1,2,3 vgl_homg_point_2d<double> from_point(data_points[i].feature_a->x, data_points[i].feature_a->y); vgl_homg_point_2d<double> to_point(data_points[i].feature_b->x, data_points[i].feature_b->y); if (false) { // just for debugging printf("from_points[%i] -> to_points[%i] ==", i,i); cout << from_point << " -> " << to_point << endl; } A( 2*i, 0 ) = A( 2*i+1, 3 ) = from_point.x() * to_point.w(); A( 2*i, 1 ) = A( 2*i+1, 4 ) = from_point.y() * to_point.w(); A( 2*i, 2 ) = A( 2*i+1, 5 ) = from_point.w() * to_point.w(); A( 2*i, 6 ) = -1 * from_point.x() * to_point.x(); A( 2*i, 7 ) = -1 * from_point.y() * to_point.x(); A( 2*i, 8 ) = -1 * from_point.w() * to_point.x(); A( 2*i+1, 6 ) = -1 * from_point.x() * to_point.y(); A( 2*i+1, 7 ) = -1 * from_point.y() * to_point.y(); A( 2*i+1, 8 ) = -1 * from_point.w() * to_point.y(); } vnl_svd<double> svd( A, 1.0e-8 ); if (false) { // just for debugging cout << "A == " << A << endl; cout << "svd(A) == " << svd << endl; } const unsigned int homog_dof_ = 8; if ( svd.rank() < homog_dof_ ) { // singular fit if (true) { // just for debugging cout << "svd.rank() == " << svd.rank() << endl; for ( unsigned int i=0; i<get_num_points_to_estimate(); ++i ) { vgl_homg_point_2d<double> from_point(data_points[i].feature_a->x, data_points[i].feature_a->y); vgl_homg_point_2d<double> to_point(data_points[i].feature_b->x, data_points[i].feature_b->y); cout << "from->to point[i]-> " << from_point << "->" << to_point << endl; } } throw runtime_error("HomographyModel::estimate_from_minimal_set failed"); } vnl_vector<double> params = svd.nullvector(); parameters.resize(get_num_parameters()); if (params.size() != parameters.size() ) { throw runtime_error("HomographyModel::estimate_from_minimal_set internal error"); } for ( unsigned int i=0; i<get_num_parameters(); i+=1 ) { parameters[i] = params[i]; // copy the result } if ( false ) { cout << "HomographyModel parameters: " << parameters << endl; } return; } // end of 'HomographyModel::estimate_from_minimal_set'
// Overloaded version: Return singular values only. //--------------------------------------------------------- DVec& svd(const DMat& mat) //--------------------------------------------------------- { DMat U, VT; return svd(mat, U, VT, 'N', 'N'); }
inline void r_and_t(MatrixXf &rot_cw, VectorXf &pos_cw,MatrixXf start_points, MatrixXf end_points, MatrixXf P1w,MatrixXf P2w,MatrixXf initRot_cw,VectorXf initPos_cw, int maxIterNum,float TerminateTh,int nargin) { if(nargin<6) { return; } if(nargin<8) { maxIterNum = 8; TerminateTh = 1e-5; } int n = start_points.cols(); if(n != end_points.cols() || n!= P1w.cols() || n!= P2w.cols()) { return; } if(n<4) { return; } //first compute the weight of each line and the normal of //the interpretation plane passing through to camera center and the line VectorXf w = VectorXf::Zero(n); MatrixXf nc = MatrixXf::Zero(3,n); for(int i = 0 ; i < n ; i++) { //the weight of a line is the inverse of its image length w[i] = 1/(start_points.col(i)-end_points.col(i)).norm(); vfloat3 v1 = start_points.col(i); vfloat3 v2 = end_points.col(i); vfloat3 temp = v1.cross(v2); nc.col(i) = temp/temp.norm(); } MatrixXf rot_wc = initPos_cw.transpose(); MatrixXf pos_wc = - initRot_cw.transpose() * initPos_cw; for(int iter = 1 ; iter < maxIterNum ; iter++) { //construct the equation (31) MatrixXf A = MatrixXf::Zero(6,7); MatrixXf C = MatrixXf::Zero(3,3); MatrixXf D = MatrixXf::Zero(3,3); MatrixXf F = MatrixXf::Zero(3,3); vfloat3 c_bar = vfloat3(0,0,0); vfloat3 d_bar = vfloat3(0,0,0); for(int i = 0 ; i < n ; i++) { //for first point on line vfloat3 Pi = rot_wc * P1w.col(i); vfloat3 Ni = nc.col(i); float wi = w[i]; vfloat3 bi = Pi.cross(Ni); C = C + wi*Ni*Ni.transpose(); D = D + wi*bi*bi.transpose(); F = F + wi*Ni*bi.transpose(); vfloat3 tempi = Pi + pos_wc; float scale = Ni.transpose() * tempi; scale *= wi; c_bar = c_bar + scale * Ni; d_bar = d_bar + scale*bi; //for second point on line Pi = rot_wc * P2w.col(i); Ni = nc.col(i); wi = w[i]; bi = Pi.cross(Ni); C = C + wi*Ni*Ni.transpose(); D = D + wi*bi*bi.transpose(); F = F + wi*Ni*bi.transpose(); scale = (Ni.transpose() * (Pi + pos_wc)); scale *= wi; c_bar = c_bar + scale * Ni; d_bar = d_bar + scale * bi; } A.block<3,3>(0,0) = C; A.block<3,3>(0,3) = F; (A.col(6)).segment(0,2) = c_bar; A.block<3,3>(3,0) = F.transpose(); A.block<3,3>(2,2) = D; (A.col(6)).segment(3,5) = d_bar; //sovle the system by using SVD; JacobiSVD<MatrixXf> svd(A, ComputeThinU | ComputeThinV); VectorXf vec(7); //the last column of Vmat; vec = (svd.matrixV()).col(6); //the condition that the last element of vec should be 1. vec = vec/vec[6]; //update the rotation and translation parameters; vfloat3 dT = vec.segment(0,2); vfloat3 dOmiga = vec.segment(3,5); MatrixXf rtemp(3,3); rtemp << 1, -dOmiga[2], dOmiga[1], dOmiga[2], 1, -dOmiga[1], -dOmiga[1], dOmiga[0], 1; rot_wc = rtemp * rot_wc; //newRot_wc = ( I + [dOmiga]x ) oldRot_wc //may be we can compute new R using rodrigues(r+dr) pos_wc = pos_wc + dT; if(dT.norm() < TerminateTh && dOmiga.norm() < 0.1*TerminateTh) { break; } } rot_cw = rot_wc.transpose(); pos_cw = -rot_cw * pos_wc; }
void pose_estimation_from_line_correspondence(Eigen::MatrixXf start_points, Eigen::MatrixXf end_points, Eigen::MatrixXf directions, Eigen::MatrixXf points, Eigen::MatrixXf &rot_cw, Eigen::VectorXf &pos_cw) { int n = start_points.cols(); if(n != directions.cols()) { return; } if(n<4) { return; } float condition_err_threshold = 1e-3; Eigen::VectorXf cosAngleThreshold(3); cosAngleThreshold << 1.1, 0.9659, 0.8660; Eigen::MatrixXf optimumrot_cw(3,3); Eigen::VectorXf optimumpos_cw(3); std::vector<float> lineLenVec(n,1); vfloat3 l1; vfloat3 l2; vfloat3 nc1; vfloat3 Vw1; vfloat3 Xm; vfloat3 Ym; vfloat3 Zm; Eigen::MatrixXf Rot(3,3); std::vector<vfloat3> nc_bar(n,vfloat3(0,0,0)); std::vector<vfloat3> Vw_bar(n,vfloat3(0,0,0)); std::vector<vfloat3> n_c(n,vfloat3(0,0,0)); Eigen::MatrixXf Rx(3,3); int line_id; for(int HowToChooseFixedTwoLines = 1 ; HowToChooseFixedTwoLines <=3 ; HowToChooseFixedTwoLines++) { if(HowToChooseFixedTwoLines==1) { #pragma omp parallel for for(int i = 0; i < n ; i++ ) { // to correct float lineLen = 10; lineLenVec[i] = lineLen; } std::vector<float>::iterator result; result = std::max_element(lineLenVec.begin(), lineLenVec.end()); line_id = std::distance(lineLenVec.begin(), result); vfloat3 temp; temp = start_points.col(0); start_points.col(0) = start_points.col(line_id); start_points.col(line_id) = temp; temp = end_points.col(0); end_points.col(0) = end_points.col(line_id); end_points.col(line_id) = temp; temp = directions.col(line_id); directions.col(0) = directions.col(line_id); directions.col(line_id) = temp; temp = points.col(0); points.col(0) = points.col(line_id); points.col(line_id) = temp; lineLenVec[line_id] = lineLenVec[1]; lineLenVec[1] = 0; l1 = start_points.col(0) - end_points.col(0); l1 = l1/l1.norm(); } for(int i = 1; i < n; i++) { std::vector<float>::iterator result; result = std::max_element(lineLenVec.begin(), lineLenVec.end()); line_id = std::distance(lineLenVec.begin(), result); l2 = start_points.col(line_id) - end_points.col(line_id); l2 = l2/l2.norm(); lineLenVec[line_id] = 0; MatrixXf cosAngle(3,3); cosAngle = (l1.transpose()*l2).cwiseAbs(); if(cosAngle.maxCoeff() < cosAngleThreshold[HowToChooseFixedTwoLines]) { break; } } vfloat3 temp; temp = start_points.col(1); start_points.col(1) = start_points.col(line_id); start_points.col(line_id) = temp; temp = end_points.col(1); end_points.col(1) = end_points.col(line_id); end_points.col(line_id) = temp; temp = directions.col(1); directions.col(1) = directions.col(line_id); directions.col(line_id) = temp; temp = points.col(1); points.col(1) = points.col(line_id); points.col(line_id) = temp; lineLenVec[line_id] = lineLenVec[1]; lineLenVec[1] = 0; // The rotation matrix R_wc is decomposed in way which is slightly different from the description in the paper, // but the framework is the same. // R_wc = (Rot') * R * Rot = (Rot') * (Ry(theta) * Rz(phi) * Rx(psi)) * Rot nc1 = x_cross(start_points.col(1),end_points.col(1)); nc1 = nc1/nc1.norm(); Vw1 = directions.col(1); Vw1 = Vw1/Vw1.norm(); //the X axis of Model frame Xm = x_cross(nc1,Vw1); Xm = Xm/Xm.norm(); //the Y axis of Model frame Ym = nc1; //the Z axis of Model frame Zm = x_cross(Xm,Zm); Zm = Zm/Zm.norm(); //Rot * [Xm, Ym, Zm] = I. Rot.col(0) = Xm; Rot.col(1) = Ym; Rot.col(2) = Zm; Rot = Rot.transpose(); //rotate all the vector by Rot. //nc_bar(:,i) = Rot * nc(:,i) //Vw_bar(:,i) = Rot * Vw(:,i) #pragma omp parallel for for(int i = 0 ; i < n ; i++) { vfloat3 nc = x_cross(start_points.col(1),end_points.col(1)); nc = nc/nc.norm(); n_c[i] = nc; nc_bar[i] = Rot * nc; Vw_bar[i] = Rot * directions.col(i); } //Determine the angle psi, it is the angle between z axis and Vw_bar(:,1). //The rotation matrix Rx(psi) rotates Vw_bar(:,1) to z axis float cospsi = (Vw_bar[1])[2];//the angle between z axis and Vw_bar(:,1); cospsi=[0,0,1] * Vw_bar(:,1);. float sinpsi= sqrt(1 - cospsi*cospsi); Rx.row(0) = vfloat3(1,0,0); Rx.row(1) = vfloat3(0,cospsi,-sinpsi); Rx.row(2) = vfloat3(0,sinpsi,cospsi); vfloat3 Zaxis = Rx * Vw_bar[1]; if(1-fabs(Zaxis[3]) > 1e-5) { Rx = Rx.transpose(); } //estimate the rotation angle phi by least square residual. //i.e the rotation matrix Rz(phi) vfloat3 Vm2 = Rx * Vw_bar[1]; float A2 = Vm2[0]; float B2 = Vm2[1]; float C2 = Vm2[2]; float x2 = (nc_bar[1])[0]; float y2 = (nc_bar[1])[1]; float z2 = (nc_bar[1])[2]; Eigen::PolynomialSolver<double, Eigen::Dynamic> solver; Eigen::VectorXf coeff(9); std::vector<float> coef(9,0); //coefficients of equation (7) Eigen::VectorXf polyDF = VectorXf::Zero(16); //%dF = ployDF(1) * t^15 + ployDF(2) * t^14 + ... + ployDF(15) * t + ployDF(16); //construct the polynomial F' #pragma omp parallel for for(int i = 3 ; i < n ; i++) { vfloat3 Vm3 = Rx*Vw_bar[i]; float A3 = Vm3[0]; float B3 = Vm3[1]; float C3 = Vm3[2]; float x3 = (nc_bar[i])[0]; float y3 = (nc_bar[i])[1]; float z3 = (nc_bar[i])[2]; float u11 = -z2*A2*y3*B3 + y2*B2*z3*A3; float u12 = -y2*A2*z3*B3 + z2*B2*y3*A3; float u13 = -y2*B2*z3*B3 + z2*B2*y3*B3 + y2*A2*z3*A3 - z2*A2*y3*A3; float u14 = -y2*B2*x3*C3 + x2*C2*y3*B3; float u15 = x2*C2*y3*A3 - y2*A2*x3*C3; float u21 = -x2*A2*y3*B3 + y2*B2*x3*A3; float u22 = -y2*A2*x3*B3 + x2*B2*y3*A3; float u23 = x2*B2*y3*B3 - y2*B2*x3*B3 - x2*A2*y3*A3 + y2*A2*x3*A3; float u24 = y2*B2*z3*C3 - z2*C2*y3*B3; float u25 = y2*A2*z3*C3 - z2*C2*y3*A3; float u31 = -x2*A2*z3*A3 + z2*A2*x3*A3; float u32 = -x2*B2*z3*B3 + z2*B2*x3*B3; float u33 = x2*A2*z3*B3 - z2*A2*x3*B3 + x2*B2*z3*A3 - z2*B2*x3*A3; float u34 = z2*A2*z3*C3 + x2*A2*x3*C3 - z2*C2*z3*A3 - x2*C2*x3*A3; float u35 = -z2*B2*z3*C3 - x2*B2*x3*C3 + z2*C2*z3*B3 + x2*C2*x3*B3; float u36 = -x2*C2*z3*C3 + z2*C2*x3*C3; float a4 = u11*u11 + u12*u12 - u13*u13 - 2*u11*u12 + u21*u21 + u22*u22 - u23*u23 -2*u21*u22 - u31*u31 - u32*u32 + u33*u33 + 2*u31*u32; float a3 =2*(u11*u14 - u13*u15 - u12*u14 + u21*u24 - u23*u25 - u22*u24 - u31*u34 + u33*u35 + u32*u34); float a2 =-2*u12*u12 + u13*u13 + u14*u14 - u15*u15 + 2*u11*u12 - 2*u22*u22 + u23*u23 + u24*u24 - u25*u25 +2*u21*u22+ 2*u32*u32 - u33*u33 - u34*u34 + u35*u35 -2*u31*u32- 2*u31*u36 + 2*u32*u36; float a1 =2*(u12*u14 + u13*u15 + u22*u24 + u23*u25 - u32*u34 - u33*u35 - u34*u36); float a0 = u12*u12 + u15*u15+ u22*u22 + u25*u25 - u32*u32 - u35*u35 - u36*u36 - 2*u32*u36; float b3 =2*(u11*u13 - u12*u13 + u21*u23 - u22*u23 - u31*u33 + u32*u33); float b2 =2*(u11*u15 - u12*u15 + u13*u14 + u21*u25 - u22*u25 + u23*u24 - u31*u35 + u32*u35 - u33*u34); float b1 =2*(u12*u13 + u14*u15 + u22*u23 + u24*u25 - u32*u33 - u34*u35 - u33*u36); float b0 =2*(u12*u15 + u22*u25 - u32*u35 - u35*u36); float d0 = a0*a0 - b0*b0; float d1 = 2*(a0*a1 - b0*b1); float d2 = a1*a1 + 2*a0*a2 + b0*b0 - b1*b1 - 2*b0*b2; float d3 = 2*(a0*a3 + a1*a2 + b0*b1 - b1*b2 - b0*b3); float d4 = a2*a2 + 2*a0*a4 + 2*a1*a3 + b1*b1 + 2*b0*b2 - b2*b2 - 2*b1*b3; float d5 = 2*(a1*a4 + a2*a3 + b1*b2 + b0*b3 - b2*b3); float d6 = a3*a3 + 2*a2*a4 + b2*b2 - b3*b3 + 2*b1*b3; float d7 = 2*(a3*a4 + b2*b3); float d8 = a4*a4 + b3*b3; std::vector<float> v { a4, a3, a2, a1, a0, b3, b2, b1, b0 }; Eigen::VectorXf vp; vp << a4, a3, a2, a1, a0, b3, b2, b1, b0 ; //coef = coef + v; coeff = coeff + vp; polyDF[0] = polyDF[0] + 8*d8*d8; polyDF[1] = polyDF[1] + 15* d7*d8; polyDF[2] = polyDF[2] + 14* d6*d8 + 7*d7*d7; polyDF[3] = polyDF[3] + 13*(d5*d8 + d6*d7); polyDF[4] = polyDF[4] + 12*(d4*d8 + d5*d7) + 6*d6*d6; polyDF[5] = polyDF[5] + 11*(d3*d8 + d4*d7 + d5*d6); polyDF[6] = polyDF[6] + 10*(d2*d8 + d3*d7 + d4*d6) + 5*d5*d5; polyDF[7] = polyDF[7] + 9 *(d1*d8 + d2*d7 + d3*d6 + d4*d5); polyDF[8] = polyDF[8] + 8 *(d1*d7 + d2*d6 + d3*d5) + 4*d4*d4 + 8*d0*d8; polyDF[9] = polyDF[9] + 7 *(d1*d6 + d2*d5 + d3*d4) + 7*d0*d7; polyDF[10] = polyDF[10] + 6 *(d1*d5 + d2*d4) + 3*d3*d3 + 6*d0*d6; polyDF[11] = polyDF[11] + 5 *(d1*d4 + d2*d3)+ 5*d0*d5; polyDF[12] = polyDF[12] + 4 * d1*d3 + 2*d2*d2 + 4*d0*d4; polyDF[13] = polyDF[13] + 3 * d1*d2 + 3*d0*d3; polyDF[14] = polyDF[14] + d1*d1 + 2*d0*d2; polyDF[15] = polyDF[15] + d0*d1; } Eigen::VectorXd coefficientPoly = VectorXd::Zero(16); for(int j =0; j < 16 ; j++) { coefficientPoly[j] = polyDF[15-j]; } //solve polyDF solver.compute(coefficientPoly); const Eigen::PolynomialSolver<double, Eigen::Dynamic>::RootsType & r = solver.roots(); Eigen::VectorXd rs(r.rows()); Eigen::VectorXd is(r.rows()); std::vector<float> min_roots; for(int j =0;j<r.rows();j++) { rs[j] = fabs(r[j].real()); is[j] = fabs(r[j].imag()); } float maxreal = rs.maxCoeff(); for(int j = 0 ; j < rs.rows() ; j++ ) { if(is[j]/maxreal <= 0.001) { min_roots.push_back(rs[j]); } } std::vector<float> temp_v(15); std::vector<float> poly(15); for(int j = 0 ; j < 15 ; j++) { temp_v[j] = j+1; } for(int j = 0 ; j < 15 ; j++) { poly[j] = polyDF[j]*temp_v[j]; } Eigen::Matrix<double,16,1> polynomial; Eigen::VectorXd evaluation(min_roots.size()); for( int j = 0; j < min_roots.size(); j++ ) { evaluation[j] = poly_eval( polynomial, min_roots[j] ); } std::vector<float> minRoots; for( int j = 0; j < min_roots.size(); j++ ) { if(!evaluation[j]<=0) { minRoots.push_back(min_roots[j]); } } if(minRoots.size()==0) { cout << "No solution" << endl; return; } int numOfRoots = minRoots.size(); //for each minimum, we try to find a solution of the camera pose, then //choose the one with the least reprojection residual as the optimum of the solution. float minimalReprojectionError = 100; // In general, there are two solutions which yields small re-projection error // or condition error:"n_c * R_wc * V_w=0". One of the solution transforms the // world scene behind the camera center, the other solution transforms the world // scene in front of camera center. While only the latter one is correct. // This can easily be checked by verifying their Z coordinates in the camera frame. // P_c(Z) must be larger than 0 if it's in front of the camera. for(int rootId = 0 ; rootId < numOfRoots ; rootId++) { float cosphi = minRoots[rootId]; float sign1 = sign_of_number(coeff[0] * pow(cosphi,4) + coeff[1] * pow(cosphi,3) + coeff[2] * pow(cosphi,2) + coeff[3] * cosphi + coeff[4]); float sign2 = sign_of_number(coeff[5] * pow(cosphi,3) + coeff[6] * pow(cosphi,2) + coeff[7] * cosphi + coeff[8]); float sinphi= -sign1*sign2*sqrt(abs(1-cosphi*cosphi)); Eigen::MatrixXf Rz(3,3); Rz.row(0) = vfloat3(cosphi,-sinphi,0); Rz.row(1) = vfloat3(sinphi,cosphi,0); Rz.row(2) = vfloat3(0,0,1); //now, according to Sec4.3, we estimate the rotation angle theta //and the translation vector at a time. Eigen::MatrixXf RzRxRot(3,3); RzRxRot = Rz*Rx*Rot; //According to the fact that n_i^C should be orthogonal to Pi^c and Vi^c, we //have: scalarproduct(Vi^c, ni^c) = 0 and scalarproduct(Pi^c, ni^c) = 0. //where Vi^c = Rwc * Vi^w, Pi^c = Rwc *(Pi^w - pos_cw) = Rwc * Pi^w - pos; //Using the above two constraints to construct linear equation system Mat about //[costheta, sintheta, tx, ty, tz, 1]. Eigen::MatrixXf Matrice(2*n-1,6); #pragma omp parallel for for(int i = 0 ; i < n ; i++) { float nxi = (nc_bar[i])[0]; float nyi = (nc_bar[i])[1]; float nzi = (nc_bar[i])[2]; Eigen::VectorXf Vm(3); Vm = RzRxRot * directions.col(i); float Vxi = Vm[0]; float Vyi = Vm[1]; float Vzi = Vm[2]; Eigen::VectorXf Pm(3); Pm = RzRxRot * points.col(i); float Pxi = Pm(1); float Pyi = Pm(2); float Pzi = Pm(3); //apply the constraint scalarproduct(Vi^c, ni^c) = 0 //if i=1, then scalarproduct(Vi^c, ni^c) always be 0 if(i>1) { Matrice(2*i-3, 0) = nxi * Vxi + nzi * Vzi; Matrice(2*i-3, 1) = nxi * Vzi - nzi * Vxi; Matrice(2*i-3, 5) = nyi * Vyi; } //apply the constraint scalarproduct(Pi^c, ni^c) = 0 Matrice(2*i-2, 0) = nxi * Pxi + nzi * Pzi; Matrice(2*i-2, 1) = nxi * Pzi - nzi * Pxi; Matrice(2*i-2, 2) = -nxi; Matrice(2*i-2, 3) = -nyi; Matrice(2*i-2, 4) = -nzi; Matrice(2*i-2, 5) = nyi * Pyi; } //solve the linear system Mat * [costheta, sintheta, tx, ty, tz, 1]' = 0 using SVD, JacobiSVD<MatrixXf> svd(Matrice, ComputeThinU | ComputeThinV); Eigen::MatrixXf VMat = svd.matrixV(); Eigen::VectorXf vec(2*n-1); //the last column of Vmat; vec = VMat.col(5); //the condition that the last element of vec should be 1. vec = vec/vec[5]; //the condition costheta^2+sintheta^2 = 1; float normalizeTheta = 1/sqrt(vec[0]*vec[1]+vec[1]*vec[1]); float costheta = vec[0]*normalizeTheta; float sintheta = vec[1]*normalizeTheta; Eigen::MatrixXf Ry(3,3); Ry << costheta, 0, sintheta , 0, 1, 0 , -sintheta, 0, costheta; //now, we get the rotation matrix rot_wc and translation pos_wc Eigen::MatrixXf rot_wc(3,3); rot_wc = (Rot.transpose()) * (Ry * Rz * Rx) * Rot; Eigen::VectorXf pos_wc(3); pos_wc = - Rot.transpose() * vec.segment(2,4); //now normalize the camera pose by 3D alignment. We first translate the points //on line in the world frame Pw to points in the camera frame Pc. Then we project //Pc onto the line interpretation plane as Pc_new. So we could call the point //alignment algorithm to normalize the camera by aligning Pc_new and Pw. //In order to improve the accuracy of the aligment step, we choose two points for each //lines. The first point is Pwi, the second point is the closest point on line i to camera center. //(Pw2i = Pwi - (Pwi'*Vwi)*Vwi.) Eigen::MatrixXf Pw2(3,n); Pw2.setZero(); Eigen::MatrixXf Pc_new(3,n); Pc_new.setZero(); Eigen::MatrixXf Pc2_new(3,n); Pc2_new.setZero(); for(int i = 0 ; i < n ; i++) { vfloat3 nci = n_c[i]; vfloat3 Pwi = points.col(i); vfloat3 Vwi = directions.col(i); //first point on line i vfloat3 Pci; Pci = rot_wc * Pwi + pos_wc; Pc_new.col(i) = Pci - (Pci.transpose() * nci) * nci; //second point is the closest point on line i to camera center. vfloat3 Pw2i; Pw2i = Pwi - (Pwi.transpose() * Vwi) * Vwi; Pw2.col(i) = Pw2i; vfloat3 Pc2i; Pc2i = rot_wc * Pw2i + pos_wc; Pc2_new.col(i) = Pc2i - ( Pc2i.transpose() * nci ) * nci; } MatrixXf XXc(Pc_new.rows(), Pc_new.cols()+Pc2_new.cols()); XXc << Pc_new, Pc2_new; MatrixXf XXw(points.rows(), points.cols()+Pw2.cols()); XXw << points, Pw2; int nm = points.cols()+Pw2.cols(); cal_campose(XXc,XXw,nm,rot_wc,pos_wc); pos_cw = -rot_wc.transpose() * pos_wc; //check the condition n_c^T * rot_wc * V_w = 0; float conditionErr = 0; for(int i =0 ; i < n ; i++) { float val = ( (n_c[i]).transpose() * rot_wc * directions.col(i) ); conditionErr = conditionErr + val*val; } if(conditionErr/n < condition_err_threshold || HowToChooseFixedTwoLines ==3) { //check whether the world scene is in front of the camera. int numLineInFrontofCamera = 0; if(HowToChooseFixedTwoLines<3) { for(int i = 0 ; i < n ; i++) { vfloat3 P_c = rot_wc * (points.col(i) - pos_cw); if(P_c[2]>0) { numLineInFrontofCamera++; } } } else { numLineInFrontofCamera = n; } if(numLineInFrontofCamera > 0.5*n) { //most of the lines are in front of camera, then check the reprojection error. int reprojectionError = 0; for(int i =0; i < n ; i++) { //line projection function vfloat3 nc = rot_wc * x_cross(points.col(i) - pos_cw , directions.col(i)); float h1 = nc.transpose() * start_points.col(i); float h2 = nc.transpose() * end_points.col(i); float lineLen = (start_points.col(i) - end_points.col(i)).norm()/3; reprojectionError += lineLen * (h1*h1 + h1*h2 + h2*h2) / (nc[0]*nc[0]+nc[1]*nc[1]); } if(reprojectionError < minimalReprojectionError) { optimumrot_cw = rot_wc.transpose(); optimumpos_cw = pos_cw; minimalReprojectionError = reprojectionError; } } } } if(optimumrot_cw.rows()>0) { break; } } pos_cw = optimumpos_cw; rot_cw = optimumrot_cw; }
void Mesher::check_feature() { auto contour = get_contour(); const auto normals = get_normals(contour); // Find the largest cone and the normals that enclose // the largest angle as n0, n1. float theta = 1; Vec3f n0, n1; for (auto ni : normals) { for (auto nj : normals) { float dot = ni.dot(nj); if (dot < theta) { theta = dot; n0 = ni; n1 = nj; } } } // If there isn't a feature in this fan, then return immediately. if (theta > 0.9) return; // Decide whether this is a corner or edge feature. const Vec3f nstar = n0.cross(n1); float phi = 0; for (auto n : normals) phi = fmax(phi, fabs(nstar.dot(n))); bool edge = phi < 0.7; // Find the center of the contour. Vec3f center(0, 0, 0); for (auto c : contour) center += c; center /= contour.size(); // Construct the matrices for use in our least-square fit. Eigen::MatrixX3d A(normals.size(), 3); { int i=0; for (auto n : normals) A.row(i++) << n.transpose(); } // When building the second matrix, shift position values to be centered // about the origin (because that's what the least-squares fit will // minimize). Eigen::VectorXd B(normals.size(), 1); { auto n = normals.begin(); auto c = contour.begin(); int i=0; while (n != normals.end()) B.row(i++) << (n++)->dot(*(c++) - center); } // Use singular value decomposition to solve the least-squares fit. Eigen::JacobiSVD<Eigen::MatrixX3d> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV); // Set the smallest singular value to zero to make fitting happier. if (edge) { auto singular = svd.singularValues(); svd.setThreshold(singular.minCoeff() / singular.maxCoeff() * 1.01); } // Solve for the new point's position. const Vec3f new_pt = svd.solve(B) + center; // Erase this triangle fan, as we'll be inserting a vertex in the center. triangles.erase(fan_start, voxel_start); // Construct a new triangle fan. contour.push_back(contour.front()); { auto p0 = contour.begin(); auto p1 = contour.begin(); p1++; while (p1 != contour.end()) push_swappable_triangle(Triangle(*(p0++), *(p1++), new_pt)); } }
inline void cal_campose(Eigen::MatrixXf XXc,Eigen::MatrixXf XXw, int n,Eigen::MatrixXf &R2,Eigen::VectorXf &t2) { //A Eigen::MatrixXf X = XXw; //B Eigen::MatrixXf Y = XXc; Eigen::MatrixXf eyen(n,n); eyen = Eigen::MatrixXf::Identity(n,n); Eigen::MatrixXf ones(n,n); ones.setOnes(); Eigen::MatrixXf K(n,n); K = eyen - ones/n; vfloat3 ux; for(int i =0; i < n; i++) { ux = ux + X.col(i); } ux = ux/n; vfloat3 uy; for(int i =0; i < n; i++) { uy = uy + Y.col(i); } uy = uy/n; Eigen::MatrixXf XK(3,n); XK = X*K; Eigen::MatrixXf XKarre(3,n); for(int i = 0 ; i < n ; i++) { XKarre(0,i) = XK(0,i)*XK(0,i); XKarre(1,i) = XK(1,i)*XK(1,i); XKarre(2,i) = XK(2,i)*XK(2,i); } Eigen::VectorXf sumXKarre(n); float sigmx2 = 0; for(int i = 0 ; i < n ; i++) { sumXKarre[i] = XKarre(0,i) + XKarre(1,i) + XKarre(2,i); sigmx2 += sumXKarre[i]; } sigmx2 /=n; Eigen::MatrixXf SXY(3,3); SXY = Y*K*(X.transpose())/n; JacobiSVD<MatrixXf> svd(SXY, ComputeThinU | ComputeThinV); Eigen::MatrixXf S(3,3); S = Eigen::MatrixXf::Identity(3,3); if(SXY.determinant() < 0) { S(3,3) = -1; } R2 = svd.matrixU() * S * (svd.matrixV()).transpose(); Eigen::MatrixXf D(3,3); D.setZero(); for(int i = 0 ; i < svd.singularValues().size() ; i++) { D(i,i) = (svd.singularValues())[i]; } float c2 = (D*S).trace()/sigmx2; t2 = uy - c2*R2*ux; vfloat3 Xx = R2.col(0); vfloat3 Yy = R2.col(1); vfloat3 Zz = R2.col(2); if((x_cross(Xx,Yy)-Zz).norm()>2e-2) { R2.col(2) = -Zz; } }
void solveLinear(Double_t eps = 1.e-12) { cout << "Perform the fit y = c0 + c1 * x in four different ways" << endl; const Int_t nrVar = 2; const Int_t nrPnts = 4; Double_t ax[] = {0.0,1.0,2.0,3.0}; Double_t ay[] = {1.4,1.5,3.7,4.1}; Double_t ae[] = {0.5,0.2,1.0,0.5}; // Make the vectors 'Use" the data : they are not copied, the vector data // pointer is just set appropriately TVectorD x; x.Use(nrPnts,ax); TVectorD y; y.Use(nrPnts,ay); TVectorD e; e.Use(nrPnts,ae); TMatrixD A(nrPnts,nrVar); TMatrixDColumn(A,0) = 1.0; TMatrixDColumn(A,1) = x; cout << " - 1. solve through Normal Equations" << endl; const TVectorD c_norm = NormalEqn(A,y,e); cout << " - 2. solve through SVD" << endl; // numerically preferred method // first bring the weights in place TMatrixD Aw = A; TVectorD yw = y; for (Int_t irow = 0; irow < A.GetNrows(); irow++) { TMatrixDRow(Aw,irow) *= 1/e(irow); yw(irow) /= e(irow); } TDecompSVD svd(Aw); Bool_t ok; const TVectorD c_svd = svd.Solve(yw,ok); cout << " - 3. solve with pseudo inverse" << endl; const TMatrixD pseudo1 = svd.Invert(); TVectorD c_pseudo1 = yw; c_pseudo1 *= pseudo1; cout << " - 4. solve with pseudo inverse, calculated brute force" << endl; TMatrixDSym AtA(TMatrixDSym::kAtA,Aw); const TMatrixD pseudo2 = AtA.Invert() * Aw.T(); TVectorD c_pseudo2 = yw; c_pseudo2 *= pseudo2; cout << " - 5. Minuit through TGraph" << endl; TGraphErrors *gr = new TGraphErrors(nrPnts,ax,ay,0,ae); TF1 *f1 = new TF1("f1","pol1",0,5); gr->Fit("f1","Q"); TVectorD c_graph(nrVar); c_graph(0) = f1->GetParameter(0); c_graph(1) = f1->GetParameter(1); // Check that all 4 answers are identical within a certain // tolerance . The 1e-12 is somewhat arbitrary . It turns out that // the TGraph fit is different by a few times 1e-13. Bool_t same = kTRUE; same &= VerifyVectorIdentity(c_norm,c_svd,0,eps); same &= VerifyVectorIdentity(c_norm,c_pseudo1,0,eps); same &= VerifyVectorIdentity(c_norm,c_pseudo2,0,eps); same &= VerifyVectorIdentity(c_norm,c_graph,0,eps); if (same) cout << " All solutions are the same within tolerance of " << eps << endl; else cout << " Some solutions differ more than the allowed tolerance of " << eps << endl; }
Alignment::Alignment(PharmacophoreMap& fMap) : _refMap(), _dbMap(), _refCenter(), _dbCenter(), _refRotMat(3,3,0.0), _dbRotMat(3,3,0.0), _dCdq(4,0.0), _d2Cdq2(4,4,0.0), _grad(4,0.0), _AkA(fMap.size()), _nbrPoints(0), _nbrExcl(0) { // compute centers of the two pharmacophore sets PharmacophoreMap::iterator mi; unsigned int nbrMatch(0); // compute the centroid of the pharmacophores double V1(0.0), v1(0.0), V2(0.0), v2(0.0); for (mi = fMap.begin(); mi != fMap.end(); ++mi) { if (mi->first->func == EXCL) { _nbrExcl++; continue; // do not use exclusion spheres when computing the center } _nbrPoints++; v1 = GCI * pow(PI/mi->first->alpha,1.5); V1 += v1; _refCenter.x += v1 * mi->first->point.x; _refCenter.y += v1 * mi->first->point.y; _refCenter.z += v1 * mi->first->point.z; v2 = GCI * pow(PI/mi->second->alpha,1.5); V2 += v2; _dbCenter.x += v2 * mi->second->point.x; _dbCenter.y += v2 * mi->second->point.y; _dbCenter.z += v2 * mi->second->point.z; ++nbrMatch; } _refCenter.x /= V1; _refCenter.y /= V1; _refCenter.z /= V1; _dbCenter.x /= V2; _dbCenter.y /= V2; _dbCenter.z /= V2; // translate the pharmacophores to the centers // and compute center of mass matrix SiMath::Matrix mass1(3,3,0.0); SiMath::Matrix mass2(3,3,0.0); for (mi = fMap.begin(); mi != fMap.end(); ++mi) { PharmacophorePoint p1, p2; p1.point.x = mi->first->point.x - _refCenter.x; p1.point.y = mi->first->point.y - _refCenter.y; p1.point.z = mi->first->point.z - _refCenter.z; p1.func = mi->first->func; p1.alpha = mi->first->alpha; p1.normal.x = mi->first->normal.x - mi->first->point.x; p1.normal.y = mi->first->normal.y - mi->first->point.y; p1.normal.z = mi->first->normal.z - mi->first->point.z; p2.point.x = mi->second->point.x - _dbCenter.x; p2.point.y = mi->second->point.y - _dbCenter.y; p2.point.z = mi->second->point.z - _dbCenter.z; p2.func = mi->second->func; p2.alpha = mi->second->alpha; p2.normal.x = mi->second->normal.x - mi->second->point.x; p2.normal.y = mi->second->normal.y - mi->second->point.y; p2.normal.z = mi->second->normal.z - mi->second->point.z; if (mi->first->func != EXCL) { v1 = GCI * pow(PI/mi->first->alpha,1.5); mass1[0][0] += v1 * p1.point.x * p1.point.x; mass1[0][1] += v1 * p1.point.x * p1.point.y; mass1[0][2] += v1 * p1.point.x * p1.point.z; mass1[1][1] += v1 * p1.point.y * p1.point.y; mass1[1][2] += v1 * p1.point.y * p1.point.z; mass1[2][2] += v1 * p1.point.z * p1.point.z; v2 = GCI * pow(PI/mi->second->alpha,1.5); mass2[0][0] += v2 * p2.point.x * p2.point.x; mass2[0][1] += v2 * p2.point.x * p2.point.y; mass2[0][2] += v2 * p2.point.x * p2.point.z; mass2[1][1] += v2 * p2.point.y * p2.point.y; mass2[1][2] += v2 * p2.point.y * p2.point.z; mass2[2][2] += v2 * p2.point.z * p2.point.z; } // add new points to local maps _refMap.push_back(p1); _dbMap.push_back(p2); } // use SVD to compute best rotations // set lower triangle mass1[1][0] = mass1[0][1]; mass1[2][0] = mass1[0][2]; mass1[2][1] = mass1[1][2]; // normalize mass matrix mass1 /= V1; // compute SVD of the mass matrix SiMath::SVD svd(mass1, true, true); _refRotMat = svd.getU(); // check if determinant is 1, otherwise it is a mirroring instead of rotation double det = _refRotMat[0][0]*_refRotMat[1][1]*_refRotMat[2][2] + _refRotMat[2][1]*_refRotMat[1][0]*_refRotMat[0][2] + _refRotMat[0][1]*_refRotMat[1][2]*_refRotMat[2][0] - _refRotMat[0][0]*_refRotMat[1][2]*_refRotMat[2][1] - _refRotMat[1][1]*_refRotMat[2][0]*_refRotMat[0][2] - _refRotMat[2][2]*_refRotMat[0][1]*_refRotMat[1][0]; // check if it is a rotation matrix and not a mirroring if (det < 0) { // switch sign of third column _refRotMat[0][2] = -_refRotMat[0][2]; _refRotMat[1][2] = -_refRotMat[1][2]; _refRotMat[2][2] = -_refRotMat[2][2]; } // set lower triangle mass2[1][0] = mass2[0][1]; mass2[2][0] = mass2[0][2]; mass2[2][1] = mass2[1][2]; // normalize mass matrix mass2 /= V2; // compute SVD of the mass matrix SiMath::SVD svd2(mass2, true, true); _dbRotMat = svd2.getU(); // check if determinant is 1, otherwise it is a mirroring instead of rotation det = _dbRotMat[0][0]*_dbRotMat[1][1]*_dbRotMat[2][2] + _dbRotMat[2][1]*_dbRotMat[1][0]*_dbRotMat[0][2] + _dbRotMat[0][1]*_dbRotMat[1][2]*_dbRotMat[2][0] - _dbRotMat[0][0]*_dbRotMat[1][2]*_dbRotMat[2][1] - _dbRotMat[1][1]*_dbRotMat[2][0]*_dbRotMat[0][2] - _dbRotMat[2][2]*_dbRotMat[0][1]*_dbRotMat[1][0]; // checif if it is a rotation matrix and not a mirroring if (det < 0) { // switch sign of third column _dbRotMat[0][2] = -_dbRotMat[0][2]; _dbRotMat[1][2] = -_dbRotMat[1][2]; _dbRotMat[2][2] = -_dbRotMat[2][2]; } // rotate points towards main axes for (unsigned int i(0); i < _refMap.size(); ++i) { // Rotate points double x = _refMap[i].point.x; double y = _refMap[i].point.y; double z = _refMap[i].point.z; _refMap[i].point.x = _refRotMat[0][0]*x + _refRotMat[1][0]*y + _refRotMat[2][0]*z; _refMap[i].point.y = _refRotMat[0][1]*x + _refRotMat[1][1]*y + _refRotMat[2][1]*z; _refMap[i].point.z = _refRotMat[0][2]*x + _refRotMat[1][2]*y + _refRotMat[2][2]*z; x = _dbMap[i].point.x; y = _dbMap[i].point.y; z = _dbMap[i].point.z; _dbMap[i].point.x = _dbRotMat[0][0]*x + _dbRotMat[1][0]*y + _dbRotMat[2][0]*z; _dbMap[i].point.y = _dbRotMat[0][1]*x + _dbRotMat[1][1]*y + _dbRotMat[2][1]*z; _dbMap[i].point.z = _dbRotMat[0][2]*x + _dbRotMat[1][2]*y + _dbRotMat[2][2]*z; double dx = _refMap[i].point.x - _dbMap[i].point.x; double dx2 = dx * dx; double dy = _refMap[i].point.y - _dbMap[i].point.y; double dy2 = dy * dy; double dz = _refMap[i].point.z - _dbMap[i].point.z; double dz2 = dz * dz; double sx = _refMap[i].point.x + _dbMap[i].point.x; double sx2 = sx * sx; double sy = _refMap[i].point.y + _dbMap[i].point.y; double sy2 = sy * sy; double sz = _refMap[i].point.z + _dbMap[i].point.z; double sz2 = sz * sz; _AkA[i] = new SiMath::Matrix(4,4,0.0); (*_AkA[i])[0][0] = dx2 + dy2 + dz2; (*_AkA[i])[0][1] = dy*sz - sy*dz; (*_AkA[i])[0][2] = sx*dz - dx*sz; (*_AkA[i])[0][3] = dx*sy - sx*dy; (*_AkA[i])[1][0] = (*_AkA[i])[0][1]; (*_AkA[i])[1][1] = dx2 + sy2 + sz2; (*_AkA[i])[1][2] = dx*dy - sx*sy; (*_AkA[i])[1][3] = dx*dz - sx*sz; (*_AkA[i])[2][0] = (*_AkA[i])[0][2]; (*_AkA[i])[2][1] = (*_AkA[i])[1][2]; (*_AkA[i])[2][2] = sx2 + dy2 + sz2; (*_AkA[i])[2][3] = dy*dz - sy*sz; (*_AkA[i])[3][0] = (*_AkA[i])[0][3]; (*_AkA[i])[3][1] = (*_AkA[i])[1][3]; (*_AkA[i])[3][2] = (*_AkA[i])[2][3]; (*_AkA[i])[3][3] = sx2 + sy2 + dz2; // Rotate normals x = _refMap[i].normal.x; y = _refMap[i].normal.y; z = _refMap[i].normal.z; _refMap[i].normal.x = _refRotMat[0][0]*x + _refRotMat[1][0]*y + _refRotMat[2][0]*z; _refMap[i].normal.y = _refRotMat[0][1]*x + _refRotMat[1][1]*y + _refRotMat[2][1]*z; _refMap[i].normal.z = _refRotMat[0][2]*x + _refRotMat[1][2]*y + _refRotMat[2][2]*z; x = _dbMap[i].normal.x; y = _dbMap[i].normal.y; z = _dbMap[i].normal.z; _dbMap[i].normal.x = _dbRotMat[0][0]*x + _dbRotMat[1][0]*y + _dbRotMat[2][0]*z; _dbMap[i].normal.y = _dbRotMat[0][1]*x + _dbRotMat[1][1]*y + _dbRotMat[2][1]*z; _dbMap[i].normal.z = _dbRotMat[0][2]*x + _dbRotMat[1][2]*y + _dbRotMat[2][2]*z; } return; }
int transform(const char * source, const char * destination, const char * output){ char src_pts_name[256]; char dest_pts_name[256]; char out_param_name[256]; int n=3; int m=0; int m2=0; int k,l; double **src_mat=NULL; double **dest_mat=NULL; double **dest_mat_T=NULL; double **src_mat_T=NULL; double **E_mat=NULL; double **C_mat=NULL; double **C_mat_interm=NULL; double **D_mat_interm=NULL; double **P_mat=NULL; double *D_vec=NULL; double *T_vec=NULL; double *one_vec=NULL; double **D_mat=NULL; double **Q_mat=NULL; double **P_mat_T=NULL; double **R_mat=NULL; double trace1=0.0; double trace2=0.0; double scal=0.0; double ppm=0.0; FILE *outfile; printf("\n*******************************\n"); printf( "* helmparms3d v%1.2f *\n",VERS); printf( "* (c) U. Niethammer 2011 *\n"); printf( "* http://helmparms3d.sf.net *\n"); printf( "*******************************\n"); memset(src_pts_name,0,sizeof(src_pts_name)); memset(dest_pts_name,0,sizeof(dest_pts_name)); memset(out_param_name,0,sizeof(out_param_name)); strcpy(src_pts_name, source); strcpy(dest_pts_name, destination); strcpy(out_param_name, output); m=get_m_size(src_pts_name); m2=get_m_size(dest_pts_name); if(m2!=m){ printf("Error, number of source and destination points is not equal!\n"); } else { src_mat=matrix(m,m, src_mat); dest_mat=matrix(m,m, dest_mat); read_points(src_pts_name, src_mat); read_points(dest_pts_name, dest_mat); D_vec=vector(n, D_vec); E_mat=matrix(m, m, E_mat); P_mat=matrix(m, m, P_mat); D_mat=matrix(m, m, D_mat); Q_mat=matrix(m, m, Q_mat); P_mat_T=matrix(m, m, P_mat_T); R_mat=matrix(m, m, R_mat); dest_mat_T=matrix(m, m, dest_mat_T); C_mat=matrix(m, m, C_mat); C_mat_interm=matrix(m, m, C_mat_interm); src_mat_T=matrix(m, m, src_mat_T); D_mat_interm=matrix(m, m, D_mat_interm); transpose_matrix(m, m, dest_mat, dest_mat_T); if(debug)printf("%s_T:\n",dest_pts_name); if(debug)plot_matrix(stdout, n, m, dest_mat_T); for(k=0;k<m;k++){ for(l=0;l<m;l++){ if(k!=l){ E_mat[k][l]=-1.0/(double)m; } else{ E_mat[k][l]=1.0-1.0/(double)m; } } } if(debug)printf("E:\n"); if(debug)plot_matrix(stdout, m, m, E_mat); if(debug)printf("dest_mat_T:\n"); if(debug)plot_matrix(stdout, n, m, dest_mat_T); matmult(dest_mat_T, m, m, E_mat, m, m, C_mat_interm, m, n); if(debug)printf("C_interm:\n"); if(debug)plot_matrix(stdout, n, m, C_mat_interm); matmult(C_mat_interm, n, m, src_mat, m, n, C_mat, n, n); if(debug)printf("C:\n"); if(debug)plot_matrix(stdout, n, n, C_mat); copy_matrix(n,n,C_mat,P_mat); if(debug)printf("P:\n"); if(debug)plot_matrix(stdout, n, n, P_mat); //Given matrix C[m][n], m>=n, using svd decomposition C = P D Q' to get P[m][n], diag D[n] and Q[n][n]. svd(n, n, C_mat, P_mat, D_vec, Q_mat); transpose_matrix(n, n, P_mat, P_mat_T); if(debug)printf("P\n"); if(debug)plot_matrix(stdout, n, n, P_mat); if(debug)printf("P_T\n"); if(debug)plot_matrix(stdout, n, n, P_mat_T); if(debug)printf("D_vec\n"); if(debug)plot_vector(stdout, n, D_vec); for(k=0;k<n;k++){ for(l=0;l<n;l++){ D_mat[k][l]=0.0; D_mat[l][l]=D_vec[l]; } } if(debug)printf("D\n"); if(debug)plot_matrix(stdout, n, n, D_mat); matmult(Q_mat, n, n, P_mat_T, n, n, R_mat, n, n); if(debug)printf("R_trans:\n"); if(debug)plot_matrix(stdout, n, n, R_mat); matmult(C_mat, m, n, R_mat, n, m, C_mat_interm, m, n); if(debug)printf("C_interm:\n"); if(debug)plot_matrix(stdout, n, n, C_mat_interm); trace1=trace(n,n,C_mat_interm); if(debug)printf("\ntra=%lf\n\n",trace1); transpose_matrix(m, m, src_mat, src_mat_T); if(debug)printf("%s_T:\n",src_pts_name); if(debug)plot_matrix(stdout, n, m, src_mat_T); init_matrix(m,m,C_mat); init_matrix(m,m,C_mat_interm); matmult(src_mat_T, m, m, E_mat, m, m, C_mat_interm, n, n); if(debug)printf("C_interm:\n"); if(debug)plot_matrix(stdout, n, m, C_mat_interm); matmult(C_mat_interm, n, m, src_mat, m, n, C_mat, n, n); if(debug)printf("C:\n"); if(debug)plot_matrix(stdout, n, n, C_mat); trace2=trace(n,n,C_mat); if(debug)printf("\ntra=%lf\n\n",trace2); scal=trace1/trace2; ppm=scal-1.0; if(debug)printf("\nscal = %10.10lf\nscal = %10.10lf ppm\n\n",scal, ppm); init_matrix(m,m,C_mat); init_matrix(m,m,C_mat_interm); matmult(src_mat, m, n, R_mat, n,m, D_mat_interm, m, n); if(debug)printf("C_mat_interm:\n"); if(debug)plot_matrix(stdout, m, n, D_mat_interm); scal_matrix(m, n, scal, D_mat_interm, C_mat_interm); if(debug)printf("C_mat_interm:\n"); if(debug)plot_matrix(stdout, m, n, C_mat_interm); subtract_matrix(m, n, dest_mat, C_mat_interm, D_mat_interm); if(debug)plot_matrix(stdout, m, n, D_mat_interm); scal_matrix(m, n, 1.0/m, D_mat_interm, C_mat_interm); if(debug)plot_matrix(stdout, m, n, C_mat_interm); init_matrix(m,m,src_mat_T); transpose_matrix(m, m, C_mat_interm, src_mat_T); if(debug)plot_matrix(stdout, n, m, src_mat_T); T_vec=vector(m, T_vec); one_vec=vector(m, one_vec); for(k=0;k<m;k++){ one_vec[k]=1.0; } matrix_multiply(n, m, src_mat_T, one_vec, T_vec); if(debug)printf("T:\n"); if(debug)plot_vector(stdout, 3, T_vec); outfile = fopen(out_param_name, "w"); if(outfile == NULL){ printf("Error writing %s\r\n",out_param_name); exit(-1); } init_matrix(m,m,src_mat_T); transpose_matrix(m, m, R_mat, src_mat_T); plot_matrix(outfile, n, n, src_mat_T); printf("R =\n");fflush(stdout); plot_matrix(stdout, n, n, src_mat_T); printf("\n");fflush(stdout); plot_vector(outfile, 3, T_vec); printf("T =\n");fflush(stdout); plot_vector(stdout, 3, T_vec); printf("\n");fflush(stdout); fprintf(outfile, "%10.10lf\n", scal); printf("s = %10.10lf (= %10.10lf ppm)\n\n",scal, ppm);fflush(stdout); fclose(outfile); freevector(D_vec); freevector(T_vec); freevector(one_vec); freematrix(m, src_mat); freematrix(m, dest_mat); freematrix(m, E_mat); freematrix(m, P_mat); freematrix(m, D_mat); freematrix(m, Q_mat); freematrix(m, P_mat_T); freematrix(m, R_mat); freematrix(m, dest_mat_T); freematrix(m, C_mat); freematrix(m, C_mat_interm); freematrix(m, src_mat_T); freematrix(m, D_mat_interm); printf("\n...done\n"); } }
void LocalLinearLeastSquaresExtrapolator::extrapolateElement( std::size_t const element_index, const unsigned num_components, ExtrapolatableElementCollection const& extrapolatables, const double t, GlobalVector const& current_solution, LocalToGlobalIndexMap const& dof_table, GlobalVector& counts) { auto const& integration_point_values = extrapolatables.getIntegrationPointValues( element_index, t, current_solution, dof_table, _integration_point_values_cache); auto const& N_0 = extrapolatables.getShapeMatrix(element_index, 0); auto const num_nodes = static_cast<unsigned>(N_0.cols()); auto const num_values = static_cast<unsigned>(integration_point_values.size()); if (num_values % num_components != 0) OGS_FATAL( "The number of computed integration point values is not divisable " "by the number of num_components. Maybe the computed property is " "not a %d-component vector for each integration point.", num_components); // number of integration points in the element const auto num_int_pts = num_values / num_components; if (num_int_pts < num_nodes) OGS_FATAL( "Least squares is not possible if there are more nodes than" "integration points."); auto const pair_it_inserted = _qr_decomposition_cache.emplace( std::make_pair(num_nodes, num_int_pts), CachedData{}); auto& cached_data = pair_it_inserted.first->second; if (pair_it_inserted.second) { DBUG("Computing new singular value decomposition"); // interpolation_matrix * nodal_values = integration_point_values // We are going to pseudo-invert this relation now using singular value // decomposition. auto& interpolation_matrix = cached_data.A; interpolation_matrix.resize(num_int_pts, num_nodes); interpolation_matrix.row(0) = N_0; for (unsigned int_pt = 1; int_pt < num_int_pts; ++int_pt) { auto const& shp_mat = extrapolatables.getShapeMatrix(element_index, int_pt); assert(shp_mat.cols() == num_nodes); // copy shape matrix to extrapolation matrix row-wise interpolation_matrix.row(int_pt) = shp_mat; } // JacobiSVD is extremely reliable, but fast only for small matrices. // But we usually have small matrices and we don't compute very often. // Cf. // http://eigen.tuxfamily.org/dox/group__TopicLinearAlgebraDecompositions.html // // Decomposes interpolation_matrix = U S V^T. Eigen::JacobiSVD<Eigen::MatrixXd> svd( interpolation_matrix, Eigen::ComputeThinU | Eigen::ComputeThinV); auto const& S = svd.singularValues(); auto const& U = svd.matrixU(); auto const& V = svd.matrixV(); // Compute and save the pseudo inverse V * S^{-1} * U^T. auto const rank = svd.rank(); assert(rank == num_nodes); // cf. http://eigen.tuxfamily.org/dox/JacobiSVD_8h_source.html cached_data.A_pinv.noalias() = V.leftCols(rank) * S.head(rank).asDiagonal().inverse() * U.leftCols(rank).transpose(); } else if (cached_data.A.row(0) != N_0) { OGS_FATAL("The cached and the passed shapematrices differ."); } auto const& global_indices = _dof_table_single_component(element_index, 0).rows; if (num_components == 1) { auto const integration_point_values_vec = MathLib::toVector(integration_point_values); // Apply the pre-computed pseudo-inverse. Eigen::VectorXd const nodal_values = cached_data.A_pinv * integration_point_values_vec; // TODO does that give rise to PETSc problems? E.g., writing to ghost // nodes? Furthermore: Is ghost nodes communication necessary for PETSc? _nodal_values->add(global_indices, nodal_values); counts.add(global_indices, std::vector<double>(global_indices.size(), 1.0)); } else { auto const integration_point_values_mat = MathLib::toMatrix( integration_point_values, num_components, num_int_pts); // Apply the pre-computed pseudo-inverse. Eigen::MatrixXd const nodal_values = cached_data.A_pinv * integration_point_values_mat.transpose(); std::vector<GlobalIndexType> indices; indices.reserve(num_components * global_indices.size()); // _nodal_values is ordered location-wise for (unsigned comp = 0; comp < num_components; ++comp) { for (auto i : global_indices) { indices.push_back(num_components * i + comp); } } // Nodal_values are passed as a raw pointer, because PETScVector and // EigenVector implementations differ slightly. _nodal_values->add(indices, nodal_values.data()); counts.add(indices, std::vector<double>(indices.size(), 1.0)); } }
int main() { //Specify data directory //const std::string dataDirectory = "/Users/ervislilaj/Desktop/data/SimudPuteh"; const std::string dataDirectory = "/Users/ervislilaj/Desktop/data/SmallCaveDownsampled"; //Calculate paths const std::string outputDirectory = dataDirectory + "/output"; const std::string offFile = dataDirectory + "/model.off"; const std::string skeletonFile = dataDirectory + "/model.skel"; const std::string segmentationFile = dataDirectory + "/segmentation.seg"; const std::string segmentationFile2 = dataDirectory + "/segmentation2.seg"; //Read files typedef std::vector<Triangle> TriangleList; std::vector<Eigen::Vector3f> vertices; std::vector<Eigen::Vector3f> newVertices /*(vertices.size()*2)*/; TriangleList triangles; std::vector<Eigen::Vector3f> border; std::vector<CurveSkeleton::Vertex> skeletonVertices; std::vector<Vertex> gang; std::vector<Eigen::Vector3f> borderVertices; std::vector<Eigen::Vector3f> intersectionVertexForTriangulation; std::vector<IndexedTriangle> triIndices; std::vector<IndexedTriangle> borderIndices; std::vector<IndexedTriangle> labelIndices; std::vector<IndexedTriangle> leerIndices; std::vector<Eigen::Vector3f> verwe; std::vector<int32_t> segmentation; std::vector<int32_t> segmentation2; std::vector<double> doubleSeg; std::vector<int> labels; //step & iteration parameter int iter = 490; double step = 0.001; std::cout << "Loading mesh file..." << std::endl; ReadOff(offFile, vertices, triangles, triIndices); std::cout << "Loading skeleton..." << std::endl; CurveSkeleton* skeleton = LoadCurveSkeleton(skeletonFile.c_str()); std::cout << "Loading segmentation..." << std::endl; ReadSegmentation(segmentationFile2, segmentation2, skeleton); //copy std::vector<Vertex> vec2; std::cout << "Calculating inverse correspondences..." << std::endl; //Calculate inverse correspondences std::vector<unsigned int> meshVertexCorrespondsTo(vertices.size()); int iVert = -1; for (auto& vert : skeleton->vertices) { ++iVert; for (auto c : vert.correspondingOriginalVertices) { meshVertexCorrespondsTo[c] = iVert; } } for (int i = 0; i < segmentation2.size(); ++i) { doubleSeg.push_back(segmentation2[i]); } for (int i = 0; i < vertices.size(); ++i) { Vertex* new_v = new Vertex(); new_v->p = vertices[i]; new_v->c = doubleSeg[meshVertexCorrespondsTo[i]] + 0.5; newVertices.push_back(vertices[i]); vec2.push_back(new_v[0]); } cgv::math::union_find ufTri((int)vertices.size()); borderIndices.clear(); labels.clear(); labelIndices.clear(); //set neighbors id for (int i = 0; i < triIndices.size(); ++i) { for (int j = 0; j < triIndices.size(); ++j) { if(i==j); else{ if (((triIndices[j].i[0] == triIndices[i].i[0]) && (triIndices[j].i[1] == triIndices[i].i[1])) || ((triIndices[j].i[1] == triIndices[i].i[0]) && (triIndices[j].i[0] == triIndices[i].i[1])) || ((triIndices[j].i[1] == triIndices[i].i[0]) && (triIndices[j].i[2] == triIndices[i].i[1])) || ((triIndices[j].i[2] == triIndices[i].i[0]) && (triIndices[j].i[1] == triIndices[i].i[1])) || ((triIndices[j].i[2] == triIndices[i].i[0]) && (triIndices[j].i[0] == triIndices[i].i[1])) || ((triIndices[j].i[0] == triIndices[i].i[0]) && (triIndices[j].i[2] == triIndices[i].i[1]))) { // neighbor 1 triIndices[i].n[0] = &triIndices[j]; } if (((triIndices[j].i[0] == triIndices[i].i[1]) && (triIndices[j].i[1] == triIndices[i].i[2])) || ((triIndices[j].i[1] == triIndices[i].i[1]) && (triIndices[j].i[0] == triIndices[i].i[2])) || ((triIndices[j].i[1] == triIndices[i].i[1]) && (triIndices[j].i[2] == triIndices[i].i[2])) || ((triIndices[j].i[2] == triIndices[i].i[1]) && (triIndices[j].i[1] == triIndices[i].i[2])) || ((triIndices[j].i[2] == triIndices[i].i[1]) && (triIndices[j].i[0] == triIndices[i].i[2])) || ((triIndices[j].i[0] == triIndices[i].i[1]) && (triIndices[j].i[2] == triIndices[i].i[2]))) { // neighbor 2 triIndices[i].n[1] = &triIndices[j]; } if (((triIndices[j].i[0] == triIndices[i].i[2]) && (triIndices[j].i[1] == triIndices[i].i[0])) || ((triIndices[j].i[1] == triIndices[i].i[2]) && (triIndices[j].i[0] == triIndices[i].i[0])) || ((triIndices[j].i[1] == triIndices[i].i[2]) && (triIndices[j].i[2] == triIndices[i].i[0])) || ((triIndices[j].i[2] == triIndices[i].i[2]) && (triIndices[j].i[1] == triIndices[i].i[0])) || ((triIndices[j].i[2] == triIndices[i].i[2]) && (triIndices[j].i[0] == triIndices[i].i[0])) || ((triIndices[j].i[0] == triIndices[i].i[2]) && (triIndices[j].i[2] == triIndices[i].i[0]))) { // neighbor 3 triIndices[i].n[2] = &triIndices[j]; } } } } /* std::vector<IndexedTriangle> toShow; for(int i = 0; i < triIndices.size(); i += 50) { toShow.push_back(triIndices[i]); toShow.push_back(*triIndices[i].n[0]); toShow.push_back(*triIndices[i].n[1]); toShow.push_back(*triIndices[i].n[2]); } */ borderIndices.clear(); labels.clear(); labelIndices.clear(); for (int i = 0; i < vec2.size(); ++i) //für jeden Vertex { for (int j = 0; j < triIndices.size(); ++j) //für jedes Dreieck { if ((i == triIndices[j].i[0] || i == triIndices[j].i[1] || i == triIndices[j].i[2])) { vec2[i].NeigborTri.push_back(j); } } } int nrOptimization = 0; double tmpLengthIter = 0; double lengthIter = 0; bool firstTimeIter = false; do { std::ofstream fout("iter/50iter" + std::to_string(nrOptimization) + ".xyz", std::ofstream::out); if(!firstTimeIter) tmpLengthIter = 99999999; tmpLengthIter = lengthIter; lengthIter = 0; // 1 richtung for (int i = 0; i < vec2.size(); ++i) //für jeden Vertex { double inzidenzEdgesLength = 0; double tmp = 0; int index = 0; do { Edge e; tmp = inzidenzEdgesLength; if (index == 0) tmp = 10000; inzidenzEdgesLength = 0; for (int j = 0; j < vec2[i].NeigborTri.size(); ++j) //für jedes Dreieck { triIndices[vec2[i].NeigborTri[j]].visible = true; if ((i == triIndices[vec2[i].NeigborTri[j]].i[0] || i == triIndices[vec2[i].NeigborTri[j]].i[1] || i == triIndices[vec2[i].NeigborTri[j]].i[2])) { //Kanten rechnen sowie deren länge if (ComputePointsBetweenEdges_v1(vec2, triIndices[vec2[i].NeigborTri[j]], e)) { e.calculateLength(); inzidenzEdgesLength += e.length; } } } index++; vec2[i].c += step; } while (tmp > inzidenzEdgesLength ); vec2[i].c -= step; vec2[i].edgeLength = inzidenzEdgesLength; } // 2 richtung for (int i = 0; i < vec2.size(); ++i) //für jeden Vertex { double inzidenzEdgesLength = 0; double tmp = 0; int index = 0; do { Edge e; tmp = inzidenzEdgesLength; if (index == 0) tmp = vec2[i].edgeLength; inzidenzEdgesLength = 0; for (int j = 0; j < vec2[i].NeigborTri.size(); ++j) //für jedes Dreieck { triIndices[vec2[i].NeigborTri[j]].visible = true; if ((i == triIndices[vec2[i].NeigborTri[j]].i[0] || i == triIndices[vec2[i].NeigborTri[j]].i[1] || i == triIndices[vec2[i].NeigborTri[j]].i[2])) { //Kanten rechnen sowie deren länge if (ComputePointsBetweenEdges_v1(vec2, triIndices[vec2[i].NeigborTri[j]], e)) { e.calculateLength(); inzidenzEdgesLength += e.length; } } } //edgeLengthBlock = inzidenzEdgesLength index++; vec2[i].c -= step; lengthIter += inzidenzEdgesLength; } while (tmp > inzidenzEdgesLength ); vec2[i].c += step; } for (int j = 0; j < triIndices.size(); ++j) //für jedes Dreieck { Edge e2; if (ComputePointsBetweenEdges_v1(vec2, triIndices[j], e2)) { fout << e2.a.x() << " " << e2.a.y() << " " << e2.a.z() << std::endl; fout << e2.b.x() << " " << e2.b.y() << " " << e2.b.z() << std::endl; } } nrOptimization++; fout.close(); std::cout << nrOptimization << std::endl; std::cout << lengthIter << std::endl; } while ( (nrOptimization < iter ) && ((tmpLengthIter >= lengthIter) || (nrOptimization < 450)) ); /*End Optimization*/ borderVertices.clear(); for (int j = 0; j < triIndices.size(); ++j) //für jedes Dreieck { triIndices[j].iD = j; Edge e2; if (ComputePointsBetweenEdges_v1(vec2, triIndices[j], e2)) { borderVertices.push_back(e2.a); borderVertices.push_back(e2.b); } } // übergang erkennen for (int i = 0; i < triIndices.size(); ++i) { auto v0 = vec2.begin() + (triIndices[i].i[0]); auto v1 = vec2.begin() + (triIndices[i].i[1]); auto v2 = vec2.begin() + (triIndices[i].i[2]); double first = v0[0].c; double second = v1[0].c; double third = v2[0].c; if ((first >= 0.f && second < 0.f) || (first < 0.f && second >= 0.f) || (third >= 0.f && second < 0.f) || (third < 0.f && second >= 0.f) || (third >= 0.f && first < 0.f) || (third < 0.f && first >= 0.f)) { IndexedTriangle t; t.i[0] = triIndices[i].i[0]; t.i[1] = triIndices[i].i[1]; t.i[2] = triIndices[i].i[2]; t.iD = triIndices[i].iD; t.n[0] = triIndices[i].n[0]; t.n[1] = triIndices[i].n[1]; t.n[2] = triIndices[i].n[2]; borderIndices.push_back(t); } } //schallen unite for (int i = 0; i < borderIndices.size(); ++i) for (int j = 0; j < borderIndices.size(); ++j) { if ((borderIndices[i].i[0] == borderIndices[j].i[0]) || (borderIndices[i].i[0] == borderIndices[j].i[1]) || (borderIndices[i].i[0] == borderIndices[j].i[2])) { ufTri.unite(i, j); } if ((borderIndices[i].i[1] == borderIndices[j].i[0]) || (borderIndices[i].i[1] == borderIndices[j].i[1]) || (borderIndices[i].i[1] == borderIndices[j].i[2])) { ufTri.unite(i, j); } if ((borderIndices[i].i[2] == borderIndices[j].i[0]) || (borderIndices[i].i[2] == borderIndices[j].i[1]) || (borderIndices[i].i[2] == borderIndices[j].i[2])) { ufTri.unite(i, j); } } std::map<int, std::vector<IndexedTriangle>> labelmain; // in LabelIndices 1 eiziger gang speichern for (int i = 0; i < borderIndices.size(); ++i) { if (ufTri.num_in_set(ufTri.find(i)) <= 1); else { if (std::find(labels.begin(), labels.end(), ufTri.find(i)) != labels.end()); else { labels.push_back(ufTri.find(i)); } } if (labels.size() != 0) { int key = ufTri.find(i); labelmain[key].push_back(borderIndices[i]); } } std::vector<IndexedTriangle> newTriangles; int offset = (int)vec2.size(); for (int label : labels) { labelIndices = labelmain[label]; std::vector<Eigen::Vector3f> labelVertices; std::vector<Eigen::Vector3f> halleVertices; labelVertices.clear(); Edge labelEdge; for (int i = 0; i < labelIndices.size(); ++i) { if (ComputePointsBetweenEdges_v1(vec2, labelIndices[i], labelEdge)) { labelVertices.push_back(labelEdge.a); labelVertices.push_back(labelEdge.b); } } /*Plane Fitting with SVD */ Eigen::Vector3f vor_c(0, 0, 0); Eigen::Vector3f c(0, 0, 0); Eigen::MatrixXf matA(3, labelVertices.size()); // calculate c for (int i = 0; i < labelVertices.size(); ++i) { vor_c += labelVertices[i]; } c = vor_c / labelVertices.size(); //fill the Matrix for (int i = 0; i < labelVertices.size(); i++) { float xB, yB, zB; xB = labelVertices[i].x() - c.x(); yB = labelVertices[i].y() - c.y(); zB = labelVertices[i].z() - c.z(); matA.col(i) << xB, yB, zB; } Eigen::JacobiSVD<Eigen::MatrixXf> svd(matA, Eigen::ComputeThinU | Eigen::ComputeThinV); Eigen::Vector3f planeNormal(0, 0, 0); planeNormal = svd.matrixU().col(2); float d = -(planeNormal.x()*c.x() + planeNormal.y()*c.y() + planeNormal.z()*c.z()); /*Begin Normal direction of the plane*/ std::vector<Eigen::Vector3f> halleVerticesSidePLane; std::vector<double> disthalleVerticesSidePLane; int indexHalleV; Eigen::Vector3f halle_cVector; //alle halle knoten hinzufügen for(int i = 0; i < labelIndices.size(); ++i) { if(vec2[labelIndices[i].i[0]].c >= 0) halleVerticesSidePLane.push_back(vec2[labelIndices[i].i[0]].p); if(vec2[labelIndices[i].i[1]].c >= 0) halleVerticesSidePLane.push_back(vec2[labelIndices[i].i[1]].p); if(vec2[labelIndices[i].i[2]].c >= 0) halleVerticesSidePLane.push_back(vec2[labelIndices[i].i[2]].p); } for(int i = 0; i < halleVerticesSidePLane.size(); ++i) { disthalleVerticesSidePLane.push_back( Dist2Plane(halleVerticesSidePLane[i], planeNormal, d)); } indexHalleV = *std::max_element(disthalleVerticesSidePLane.begin(), disthalleVerticesSidePLane.end()); halle_cVector = halleVerticesSidePLane[indexHalleV] - c; Eigen::Vector3f diffBetweenPlane_halle_positive = halle_cVector + planeNormal; Eigen::Vector3f diffBetweenPlane_halle_negative = halle_cVector - planeNormal; float lengthPositive =sqrt(diffBetweenPlane_halle_positive.x()* diffBetweenPlane_halle_positive.x() + diffBetweenPlane_halle_positive.y()* diffBetweenPlane_halle_positive.y() + diffBetweenPlane_halle_positive.z()* diffBetweenPlane_halle_positive.z() ); float lengthNegative =sqrt(diffBetweenPlane_halle_negative.x()* diffBetweenPlane_halle_negative.x() + diffBetweenPlane_halle_negative.y()* diffBetweenPlane_halle_negative.y() + diffBetweenPlane_halle_negative.z()* diffBetweenPlane_halle_negative.z() ); if (lengthPositive > lengthNegative) planeNormal = -planeNormal; else if(lengthPositive <= lengthNegative) planeNormal = planeNormal; // ax + bx + cx + d = 0 Plane equation d = -(planeNormal.x()*c.x() + planeNormal.y()*c.y() + planeNormal.z()*c.z()); /*Begin computing Intersection Points*/ //Point which has the largest distance to c std::vector<double> MaxDistanceToC; double radiusMax; for (int i = 0; i < labelVertices.size(); ++i) { MaxDistanceToC.push_back(Dist2Points(c, labelVertices[i])); } radiusMax = *std::max_element(MaxDistanceToC.begin(), MaxDistanceToC.end()); //Computing Intersection points int succesor = offset - 1; std::vector<Edge> intersectionEdges; intersectionEdges.clear(); //Label Nachbarn hinzufügen. 8schritte 3^12 // First Triangle Hinzufügen die geschnitten wird und teil von lableindices ist IndexedTriangle firstTri; int firstTriID = 0; bool firstTimeSearch = true; for (int i = 0; i < labelIndices.size(); ++i){ Eigen::Vector3f aP, bP, cP, intersection1, intersection2; int aI = 0, bI = 0, cI = 0; aI = labelIndices[i].i[0]; bI = labelIndices[i].i[1]; cI = labelIndices[i].i[2]; aP = vec2[aI].p; bP = vec2[bI].p; cP = vec2[cI].p; if((SegPlaneIntersection(aP, bP, intersection1, planeNormal, d) || SegPlaneIntersection(aP, cP, intersection2, planeNormal, d) || SegPlaneIntersection(bP, cP, intersection2, planeNormal, d)) && firstTimeSearch) { firstTri = triIndices[labelIndices[i].iD]; firstTimeSearch = false; firstTriID = labelIndices[i].iD; } } std::set<int> triIndicesHit ; int mico = 0; do{ for (int m = 0; m < 3 ; ++m) { Eigen::Vector3f aP, bP, cP, intersection1, intersection2; IndexedTriangle t1,t2; //indices int aI = firstTri.n[m]->i[0]; int bI = firstTri.n[m]->i[1]; int cI = firstTri.n[m]->i[2]; aP = vec2[aI].p; bP = vec2[bI].p; cP = vec2[cI].p; if((SegPlaneIntersection(aP, bP, intersection1, planeNormal, d) || SegPlaneIntersection(aP, cP, intersection2, planeNormal, d) || SegPlaneIntersection(bP, cP, intersection2, planeNormal, d)) && (triIndicesHit.find(firstTri.n[m]->iD) == triIndicesHit.end())){ triIndicesHit.insert(firstTri.n[m]->iD); if (SegPlaneIntersection(aP, bP, intersection1, planeNormal, d) && SegPlaneIntersection(aP, cP, intersection2, planeNormal, d) && (Dist2Plane(aP, planeNormal, d) < 0)) { newVertices.push_back(intersection1); newVertices.push_back(intersection2); t1.i[0] = aI; t1.i[2] = succesor + 2; t1.i[1] = succesor + 1; t1.visible = true; newTriangles.push_back(t1); firstTri = *firstTri.n[m]; succesor++; succesor++; } if (SegPlaneIntersection(bP, cP, intersection1, planeNormal, d) && SegPlaneIntersection(bP, aP, intersection2, planeNormal, d) && (Dist2Plane(bP, planeNormal, d) < 0)) { newVertices.push_back(intersection1); newVertices.push_back(intersection2); t1.i[0] = bI; t1.i[2] = succesor + 2; t1.i[1] = succesor + 1; t1.visible = true; newTriangles.push_back(t1); firstTri = *firstTri.n[m]; succesor++; succesor++; } if (SegPlaneIntersection(cP, aP, intersection1, planeNormal, d) && SegPlaneIntersection(cP, bP, intersection2, planeNormal, d) && (Dist2Plane(cP, planeNormal, d) < 0)) { newVertices.push_back(intersection1); newVertices.push_back(intersection2); t1.i[0] = cI; t1.i[2] = succesor + 2; t1.i[1] = succesor + 1; t1.visible = true; newTriangles.push_back(t1); firstTri = *firstTri.n[m]; succesor++; succesor++; } if (SegPlaneIntersection(aP, bP, intersection1, planeNormal, d) && SegPlaneIntersection(aP, cP, intersection2, planeNormal, d) && (Dist2Plane(bP, planeNormal, d) < 0) && (Dist2Plane(cP, planeNormal, d) < 0)) { newVertices.push_back(intersection1); newVertices.push_back(intersection2); t1.i[0] = bI; t1.i[2] = succesor + 1; t1.i[1] = succesor + 2; t2.i[0] = cI; t2.i[2] = bI; t2.i[1] = succesor + 2; t1.visible = true; t2.visible = true; newTriangles.push_back(t1); newTriangles.push_back(t2); firstTri = *firstTri.n[m]; succesor++; succesor++; } if (SegPlaneIntersection(bP, cP, intersection1, planeNormal, d) && SegPlaneIntersection(bP, aP, intersection2, planeNormal, d) && (Dist2Plane(cP, planeNormal, d) < 0) && (Dist2Plane(aP, planeNormal, d) < 0)) { newVertices.push_back(intersection1); newVertices.push_back(intersection2); t1.i[0] = cI; t1.i[2] = succesor + 1; t1.i[1] = succesor + 2; t2.i[0] = aI; t2.i[2] = cI; t2.i[1] = succesor + 2; t1.visible = true; t2.visible = true; newTriangles.push_back(t1); newTriangles.push_back(t2); firstTri = *firstTri.n[m]; succesor++; succesor++; } if (SegPlaneIntersection(cP, aP, intersection1, planeNormal, d) && SegPlaneIntersection(cP, bP, intersection2, planeNormal, d) && (Dist2Plane(aP, planeNormal, d) < 0) && (Dist2Plane(bP, planeNormal, d) < 0)) { newVertices.push_back(intersection1); newVertices.push_back(intersection2); t1.i[0] = aI; t1.i[2] = succesor + 1; t1.i[1] = succesor + 2; t2.i[0] = bI; t2.i[2] = aI; t2.i[1] = succesor + 2; t1.visible = true; t2.visible = true; newTriangles.push_back(t1); newTriangles.push_back(t2); firstTri = *firstTri.n[m]; succesor++; succesor++; } } } mico ++; }while (mico < 1000000); //nachbarn von Schnitt hinzufügen std::set<int> extendedLabelIndices ; for (int i = 0; i < triIndices.size(); ++i) { const bool is_in = triIndicesHit.find(triIndices[i].iD) != triIndicesHit.end(); if(is_in){ extendedLabelIndices.insert(triIndices[i].iD); for(int j = 0; j < 3; ++j){ extendedLabelIndices.insert(triIndices[i].n[j]->iD); for(int k = 0; k < 3; ++k){ extendedLabelIndices.insert(triIndices[i].n[j]->n[k]->iD); for(int l = 0; l < 3; ++l){ extendedLabelIndices.insert(triIndices[i].n[j]->n[k]->n[l]->iD); for(int f = 0; f < 3; ++f){ extendedLabelIndices.insert(triIndices[i].n[j]->n[k]->n[l]->n[f]->iD); for(int s = 0; s < 3; ++s){ extendedLabelIndices.insert(triIndices[i].n[j]->n[k]->n[l]->n[f]->n[s]->iD); for(int r = 0; r < 3; ++r){ extendedLabelIndices.insert(triIndices[i].n[j]->n[k]->n[l]->n[f]->n[s]->n[r]->iD); } } } } } } } } /* for (int i = 0; i < triIndices.size(); ++i) { int a = triIndices[i].i[0],b = triIndices[i].i[1] ,c = triIndices[i].i[2] ; if(( vec2[a].c >= 0 )&& ( vec2[b].c >= 0 ) && ( vec2[c].c >= 0 )) { triIndices[i].visible = true; } }*/ for (int i = 0; i < triIndices.size(); ++i) { //positions Eigen::Vector3f aP, bP, cP, intersection1, intersection2; //distance to middlepoint float dista, distb, distc; //indices int aI = 0, bI = 0, cI = 0; aI = triIndices[i].i[0]; bI = triIndices[i].i[1]; cI = triIndices[i].i[2]; aP = vec2[aI].p; bP = vec2[bI].p; cP = vec2[cI].p; dista = Dist2Points(c, aP); distb = Dist2Points(c, bP); distc = Dist2Points(c, cP); const bool is_in = extendedLabelIndices.find(triIndices[i].iD) != extendedLabelIndices.end(); if (is_in) { if ((Dist2Plane(aP, planeNormal, d) > 0) || (Dist2Plane(bP, planeNormal, d) > 0) || (Dist2Plane(cP, planeNormal, d) > 0)) { triIndices[i].visible = false; } if (((Dist2Plane(aP, planeNormal, d) <= 0) || (Dist2Plane(bP, planeNormal, d) <= 0) || (Dist2Plane(cP, planeNormal, d) <= 0)) && ((Dist2Plane(aP, planeNormal, d) > 10) || (Dist2Plane(bP, planeNormal, d) > 10) || (Dist2Plane(cP, planeNormal, d) > 10))) { triIndices[i].visible = true; } } } //umbrella Eigen::Vector3f vor_ca(0,0,0), ca(0, 0, 0); int anz = (int)newVertices.size() - (offset); for (int j = offset ; j < (int)newVertices.size() ; ++j) { vor_ca += newVertices[j]; } ca = vor_c / anz; newVertices.push_back(c); for (int j = offset; j < newVertices.size()-2; j+=2) { IndexedTriangle t; t.i[1] = j; t.i[2] = j + 1; t.i[0] = (int)newVertices.size() - 1; newTriangles.push_back(t); } offset = (int)newVertices.size(); /*End computing Intersection Points*/ } for(auto t : triIndices) if(t.visible && (vec2[t.i[0]].c >= 0 || vec2[t.i[1]].c >= 0 || vec2[t.i[2]].c >= 0)) newTriangles.push_back(t); WriteSegmentation(segmentationFile, segmentation2); ReadSegmentation(segmentationFile, segmentation, skeleton); //Test output std::cout << "Writing output file..." << std::endl; /*int colors[10][3] = { { 166, 206, 227 }, { 31, 120, 180 }, { 251, 154, 153 }, { 227, 26, 28 }, { 253, 191, 111 }, { 255, 127, 0 }, { 202, 178, 214 }, { 106, 61, 154 }, { 255, 255, 153 }, { 177, 89, 40 } };*/ auto colorFunc = [&](int i, int& r, int& g, int& b) { if (doubleSeg[i] == -1) { //color for passages r = 0; g = 175; b = 0; } else if (doubleSeg[i] >= 0) { r = 166; g = 175; b = 227; } else if (doubleSeg[i] == 0) { r = 0; g = 0; b = 175; } }; std::string segmentedMeshFile = outputDirectory + "/segmentedMesh.off"; WriteOff(segmentedMeshFile.c_str(), vertices, triIndices, [&](int i, int& r, int& g, int& b) {colorFunc(meshVertexCorrespondsTo[i], r, g, b); }); std::string segmentedMeshFile2 = outputDirectory + "/borderVertices.off"; WriteOff(segmentedMeshFile2.c_str(), vertices, borderIndices, [&](int i, int& r, int& g, int& b) {colorFunc(meshVertexCorrespondsTo[i], r, g, b); }); std::string segmentedMeshFile3 = outputDirectory + "/schalle.off"; WriteOff(segmentedMeshFile3.c_str(), vertices, borderIndices, [&](int i, int& r, int& g, int& b) {colorFunc(meshVertexCorrespondsTo[i], r, g, b); }); std::string segmentedMeshFile7 = outputDirectory + "/marchingBig.off"; WriteOff(segmentedMeshFile7.c_str(), verwe , leerIndices, [&](int i, int& r, int& g, int& b) { r = 175; g = 0; b = 0; }); std::string segmentedMeshFile8 = outputDirectory + "/new.off"; WriteOff(segmentedMeshFile8.c_str(), newVertices, newTriangles, [&](int i, int& r, int& g, int& b) { r = 200; g = 200; b = 0; }); //system("PAUSE"); }
Foam::label Foam::quadraticFitSnGradData::calcFit ( const List<point>& C, const label faci ) { vector idir(1,0,0); vector jdir(0,1,0); vector kdir(0,0,1); findFaceDirs(idir, jdir, kdir, mesh(), faci); scalarList wts(C.size(), scalar(1)); wts[0] = centralWeight_; wts[1] = centralWeight_; point p0 = mesh().faceCentres()[faci]; scalar scale = 0; // calculate the matrix of the polynomial components scalarRectangularMatrix B(C.size(), minSize_, scalar(0)); for(label ip = 0; ip < C.size(); ip++) { const point& p = C[ip]; scalar px = (p - p0)&idir; scalar py = (p - p0)&jdir; #ifdef SPHERICAL_GEOMETRY scalar pz = mag(p) - mag(p0); #else scalar pz = (p - p0)&kdir; #endif if (ip == 0) scale = max(max(mag(px), mag(py)), mag(pz)); px /= scale; py /= scale; pz /= scale; label is = 0; B[ip][is++] = wts[0]*wts[ip]; B[ip][is++] = wts[0]*wts[ip]*px; B[ip][is++] = wts[ip]*sqr(px); if (dim_ >= 2) { B[ip][is++] = wts[ip]*py; B[ip][is++] = wts[ip]*px*py; B[ip][is++] = wts[ip]*sqr(py); } if (dim_ == 3) { B[ip][is++] = wts[ip]*pz; B[ip][is++] = wts[ip]*px*pz; //B[ip][is++] = wts[ip]*py*pz; B[ip][is++] = wts[ip]*sqr(pz); } } // Set the fit label stencilSize = C.size(); fit_[faci].setSize(stencilSize); scalarList singVals(minSize_); label nSVDzeros = 0; const scalar& deltaCoeff = mesh().deltaCoeffs()[faci]; bool goodFit = false; for(int iIt = 0; iIt < 10 && !goodFit; iIt++) { SVD svd(B, SMALL); scalar fit0 = wts[0]*wts[0]*svd.VSinvUt()[1][0]/scale; scalar fit1 = wts[0]*wts[1]*svd.VSinvUt()[1][1]/scale; goodFit = fit0 < 0 && fit1 > 0 && mag(fit0 + deltaCoeff) < 0.5*deltaCoeff && mag(fit1 - deltaCoeff) < 0.5*deltaCoeff; if (goodFit) { fit_[faci][0] = fit0; fit_[faci][1] = fit1; for(label i = 2; i < stencilSize; i++) { fit_[faci][i] = wts[0]*wts[i]*svd.VSinvUt()[1][i]/scale; } singVals = svd.S(); nSVDzeros = svd.nZeros(); } else // (not good fit so increase weight in the centre and for linear) { wts[0] *= 10; wts[1] *= 10; for(label i = 0; i < B.n(); i++) { B[i][0] *= 10; B[i][1] *= 10; } for(label j = 0; j < B.m(); j++) { B[0][j] *= 10; B[1][j] *= 10; } } } if (goodFit) { // remove the uncorrected snGradScheme coefficients fit_[faci][0] += deltaCoeff; fit_[faci][1] -= deltaCoeff; } else { Pout<< "quadratifFitSnGradData could not fit face " << faci << " fit_[faci][0] = " << fit_[faci][0] << " fit_[faci][1] = " << fit_[faci][1] << " deltaCoeff = " << deltaCoeff << endl; fit_[faci] = 0; } return minSize_ - nSVDzeros; }
void computeConsistentRotations_Linf(double const sigma, int const nIterations, int const nViews, std::vector<Matrix3x3d> const& relativeRotations, std::vector<std::pair<int, int> > const& viewPairs, std::vector<Matrix3x3d>& rotations, std::vector<double>& zs) { double const gamma = 1.0; int const nRelPoses = relativeRotations.size(); rotations.resize(nViews); Matrix3x3d zero3x3d; makeZeroMatrix(zero3x3d); Matrix4x4d zeroQuat; makeZeroMatrix(zeroQuat); zeroQuat[0][0] = 1; double const denomT = 1.0 / (1.0 + nRelPoses); vector<double> denomQ(nViews, 1.0); // from the psd constraint for (int k = 0; k < nRelPoses; ++k) { int const i = viewPairs[k].first; int const j = viewPairs[k].second; denomQ[i] += 1; denomQ[j] += 1; } for (int i = 0; i < nViews; ++i) denomQ[i] = 1.0 / denomQ[i]; double T = 0.0; vector<double> T1(nRelPoses); vector<double> ZT1(nRelPoses, 0.0); double T2; double ZT2 = 0; vector<Matrix4x4d> Q(nViews, zeroQuat); vector<Matrix4x4d> Q1(nViews, zeroQuat); vector<Matrix4x4d> ZQ1(nViews, zeroQuat); vector<Matrix4x4d> Q2i(nRelPoses, zeroQuat); vector<Matrix4x4d> Q2j(nRelPoses, zeroQuat); vector<Matrix4x4d> ZQ2i(nRelPoses, zeroQuat); vector<Matrix4x4d> ZQ2j(nRelPoses, zeroQuat); vector<Matrix3x3d> A(nRelPoses, zero3x3d); vector<Matrix3x3d> A1(nRelPoses, zero3x3d); vector<Matrix3x3d> A2(nRelPoses, zero3x3d); vector<Matrix3x3d> ZA1(nRelPoses, zero3x3d); vector<Matrix3x3d> ZA2(nRelPoses, zero3x3d); for (int iter = 0; iter < nIterations; ++iter) { // Convex hull of rotation matrices for (int i = 0; i < nViews; ++i) { Matrix4x4d q = Q[i] + ZQ1[i]; if (i > 0) projectConvHull_SO3(q); else { makeZeroMatrix(q); q[0][0] = 1; } Q1[i] = q; addMatricesIP(Q[i] - q, ZQ1[i]); } // end for (i) // Shrinkage of T (we want to minimize T) T2 = std::max(0.0, T + ZT2 - gamma); ZT2 += T - T2; // Cone constraint for (int k = 0; k < nRelPoses; ++k) { double t = T + ZT1[k]; Matrix3x3d a = A[k] + ZA1[k]; proxDataResidual_Frobenius(sigma, t, a); T1[k] = t; ZT1[k] += T - t; A1[k] = a; addMatricesIP(A[k] - a, ZA1[k]); } // end for (k) // Enforce linear consistency for (int k = 0; k < nRelPoses; ++k) { int const i = viewPairs[k].first; int const j = viewPairs[k].second; Matrix4x4d qi = Q[i] + ZQ2i[k]; Matrix4x4d qj = Q[j] + ZQ2j[k]; Matrix3x3d a = A[k] + ZA2[k]; proxConsistency(relativeRotations[k], qi, qj, a); Q2i[k] = qi; Q2j[k] = qj; A2[k] = a; addMatricesIP(Q[i] - qi, ZQ2i[k]); addMatricesIP(Q[j] - qj, ZQ2j[k]); addMatricesIP(A[k] - a, ZA2[k]); } // end for (k) // Averaging of the solutions for (int i = 0; i < nViews; ++i) Q[i] = Q1[i] - ZQ1[i]; T = T2 - ZT2; for (int k = 0; k < nRelPoses; ++k) T += T1[k] - ZT1[k]; T *= denomT; T = std::max(0.0, T); for (int k = 0; k < nRelPoses; ++k) A[k] = A1[k] - ZA1[k]; for (int k = 0; k < nRelPoses; ++k) { int const i = viewPairs[k].first; int const j = viewPairs[k].second; addMatricesIP(Q2i[k] - ZQ2i[k], Q[i]); addMatricesIP(Q2j[k] - ZQ2j[k], Q[j]); addMatricesIP(A2[k] - ZA2[k], A[k]); } // end for (k) for (int i = 0; i < nViews; ++i) scaleMatrixIP(denomQ[i], Q[i]); for (int k = 0; k < nRelPoses; ++k) scaleMatrixIP(0.5, A[k]); if ((iter % 500) == 0) { cout << "iter: " << iter << " t = " << T << endl; cout << "T1 = "; displayVector(T1); cout << "ZT1 = "; displayVector(ZT1); cout << "T2 = " << T2 << " ZT2 = " << ZT2 << endl; Matrix<double> ZZ(4, 4); for (int i = 0; i < nViews; ++i) { copyMatrix(Q[i], ZZ); SVD<double> svd(ZZ); cout << "Q = "; displayMatrix(ZZ); cout << "SV = "; displayVector(svd.getSingularValues()); //Matrix3x3d R = getRotationFromQuat(Q[i]); //cout << "R = "; displayMatrix(R); } // end for (i) } } // end for (iter) rotations.resize(nViews); for (int i = 0; i < nViews; ++i) rotations[i] = getRotationFromQuat(Q[i]); zs = ZT1; } // end computeConsistentRotations_Linf()
// Norm: Return largest singular value for M-by-N matrix. //--------------------------------------------------------- double norm(const DMat& mat) //--------------------------------------------------------- { DVec s( svd(mat) ); return s(1); }
/** * @brief estimateHomography estimates an homography matrix H between image 1 to image 2 * @param points0 is an array of points computed from image 1. * @param points1 is an array of points computed from image 2. * @return It returns the homography matrix H. */ PIC_INLINE Eigen::Matrix3d estimateHomography(std::vector< Eigen::Vector2f > &points0, std::vector< Eigen::Vector2f > &points1) { Eigen::Matrix3d H; if((points0.size() != points1.size()) || (points0.size() < 4)) { H.setZero(); return H; } Eigen::Vector3f transform_0 = ComputeNormalizationTransform(points0); Eigen::Vector3f transform_1 = ComputeNormalizationTransform(points1); Eigen::Matrix3d mat_0 = getShiftScaleMatrix(transform_0); Eigen::Matrix3d mat_1 = getShiftScaleMatrix(transform_1); int n = int(points0.size()); Eigen::MatrixXd A(n * 2, 9); //set up the linear system for(int i = 0; i < n; i++) { //transform coordinates for increasing stability of the system Eigen::Vector2f p0 = points0[i]; Eigen::Vector2f p1 = points1[i]; p0[0] = (p0[0] - transform_0[0]) / transform_0[2]; p0[1] = (p0[1] - transform_0[1]) / transform_0[2]; p1[0] = (p1[0] - transform_1[0]) / transform_1[2]; p1[1] = (p1[1] - transform_1[1]) / transform_1[2]; int j = i * 2; A(j, 0) = 0.0; A(j, 1) = 0.0; A(j, 2) = 0.0; A(j, 3) = p0[0]; A(j, 4) = p0[1]; A(j, 5) = 1.0; A(j, 6) = -p1[1] * p0[0]; A(j, 7) = -p1[1] * p0[1]; A(j, 8) = -p1[1]; j++; A(j, 0) = p0[0]; A(j, 1) = p0[1]; A(j, 2) = 1.0; A(j, 3) = 0.0; A(j, 4) = 0.0; A(j, 5) = 0.0; A(j, 6) = -p1[0] * p0[0]; A(j, 7) = -p1[0] * p0[1]; A(j, 8) = -p1[0]; } //solve the linear system Eigen::JacobiSVD< Eigen::MatrixXd > svd(A, Eigen::ComputeFullV); Eigen::MatrixXd V = svd.matrixV(); n = int(V.cols()) - 1; //assign and transpose H(0, 0) = V(0, n); H(0, 1) = V(1, n); H(0, 2) = V(2, n); H(1, 0) = V(3, n); H(1, 1) = V(4, n); H(1, 2) = V(5, n); H(2, 0) = V(6, n); H(2, 1) = V(7, n); H(2, 2) = V(8, n); H = mat_1.inverse() * H * mat_0; return H / H(2, 2); }
bool Homography:: decompose() { decompositions.clear(); JacobiSVD<MatrixXd> svd(H_c2_from_c1, ComputeThinU | ComputeThinV); Vector3d singular_values = svd.singularValues(); double d1 = fabs(singular_values[0]); // The paper suggests the square of these (e.g. the evalues of AAT) double d2 = fabs(singular_values[1]); // should be used, but this is wrong. c.f. Faugeras' book. double d3 = fabs(singular_values[2]); Matrix3d U = svd.matrixU(); Matrix3d V = svd.matrixV(); // VT^T double s = U.determinant() * V.determinant(); double dPrime_PM = d2; int nCase; if(d1 != d2 && d2 != d3) nCase = 1; else if( d1 == d2 && d2 == d3) nCase = 3; else nCase = 2; if(nCase != 1) { printf("FATAL Homography Initialization: This motion case is not implemented or is degenerate. Try again. "); return false; } double x1_PM; double x2; double x3_PM; // All below deals with the case = 1 case. // Case 1 implies (d1 != d3) { // Eq. 12 x1_PM = sqrt((d1*d1 - d2*d2) / (d1*d1 - d3*d3)); x2 = 0; x3_PM = sqrt((d2*d2 - d3*d3) / (d1*d1 - d3*d3)); }; double e1[4] = {1.0,-1.0, 1.0,-1.0}; double e3[4] = {1.0, 1.0,-1.0,-1.0}; Vector3d np; HomographyDecomposition decomp; // Case 1, d' > 0: decomp.d = s * dPrime_PM; for(size_t signs=0; signs<4; signs++) { // Eq 13 decomp.R = Matrix3d::Identity(); double dSinTheta = (d1 - d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2; double dCosTheta = (d1 * x3_PM * x3_PM + d3 * x1_PM * x1_PM) / d2; decomp.R(0,0) = dCosTheta; decomp.R(0,2) = -dSinTheta; decomp.R(2,0) = dSinTheta; decomp.R(2,2) = dCosTheta; // Eq 14 decomp.t[0] = (d1 - d3) * x1_PM * e1[signs]; decomp.t[1] = 0.0; decomp.t[2] = (d1 - d3) * -x3_PM * e3[signs]; np[0] = x1_PM * e1[signs]; np[1] = x2; np[2] = x3_PM * e3[signs]; decomp.n = V * np; decompositions.push_back(decomp); } // Case 1, d' < 0: decomp.d = s * -dPrime_PM; for(size_t signs=0; signs<4; signs++) { // Eq 15 decomp.R = -1 * Matrix3d::Identity(); double dSinPhi = (d1 + d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2; double dCosPhi = (d3 * x1_PM * x1_PM - d1 * x3_PM * x3_PM) / d2; decomp.R(0,0) = dCosPhi; decomp.R(0,2) = dSinPhi; decomp.R(2,0) = dSinPhi; decomp.R(2,2) = -dCosPhi; // Eq 16 decomp.t[0] = (d1 + d3) * x1_PM * e1[signs]; decomp.t[1] = 0.0; decomp.t[2] = (d1 + d3) * x3_PM * e3[signs]; np[0] = x1_PM * e1[signs]; np[1] = x2; np[2] = x3_PM * e3[signs]; decomp.n = V * np; decompositions.push_back(decomp); } // Save rotation and translation of the decomposition for(unsigned int i=0; i<decompositions.size(); i++) { Matrix3d R = s * U * decompositions[i].R * V.transpose(); Vector3d t = U * decompositions[i].t; decompositions[i].T = Sophus::SE3(R, t); } return true; }
void Foam::CentredFitSnGradData<Polynomial>::calcFit ( scalarList& coeffsi, const List<point>& C, const scalar wLin, const scalar deltaCoeff, const label facei ) { vector idir(1,0,0); vector jdir(0,1,0); vector kdir(0,0,1); this->findFaceDirs(idir, jdir, kdir, facei); // Setup the point weights scalarList wts(C.size(), scalar(1)); wts[0] = this->centralWeight(); wts[1] = this->centralWeight(); // Reference point point p0 = this->mesh().faceCentres()[facei]; // p0 -> p vector in the face-local coordinate system vector d; // Local coordinate scaling scalar scale = 1; // Matrix of the polynomial components scalarRectangularMatrix B(C.size(), this->minSize(), scalar(0)); forAll(C, ip) { const point& p = C[ip]; const vector p0p = p - p0; d.x() = p0p & idir; d.y() = p0p & jdir; d.z() = p0p & kdir; if (ip == 0) { scale = cmptMax(cmptMag((d))); } // Scale the radius vector d /= scale; Polynomial::addCoeffs(B[ip], d, wts[ip], this->dim()); } // Additional weighting for constant and linear terms for (label i = 0; i < B.m(); i++) { B(i, 0) *= wts[0]; B(i, 1) *= wts[0]; } // Set the fit label stencilSize = C.size(); coeffsi.setSize(stencilSize); bool goodFit = false; for (int iIt = 0; iIt < 8 && !goodFit; iIt++) { SVD svd(B, small); scalarRectangularMatrix invB(svd.VSinvUt()); for (label i=0; i<stencilSize; i++) { coeffsi[i] = wts[1]*wts[i]*invB(1, i)/scale; } goodFit = ( mag(wts[0]*wts[0]*invB(0, 0) - wLin) < this->linearLimitFactor()*wLin) && (mag(wts[0]*wts[1]*invB(0, 1) - (1 - wLin) ) < this->linearLimitFactor()*(1 - wLin)) && coeffsi[0] < 0 && coeffsi[1] > 0 && mag(coeffsi[0] + deltaCoeff) < 0.5*deltaCoeff && mag(coeffsi[1] - deltaCoeff) < 0.5*deltaCoeff; if (!goodFit) { // (not good fit so increase weight in the centre and weight // for constant and linear terms) WarningInFunction << "Cannot fit face " << facei << " iteration " << iIt << " with sum of weights " << sum(coeffsi) << nl << " Weights " << coeffsi << nl << " Linear weights " << wLin << " " << 1 - wLin << nl << " deltaCoeff " << deltaCoeff << nl << " sing vals " << svd.S() << nl << "Components of goodFit:\n" << " wts[0]*wts[0]*invB(0, 0) = " << wts[0]*wts[0]*invB(0, 0) << nl << " wts[0]*wts[1]*invB(0, 1) = " << wts[0]*wts[1]*invB(0, 1) << " dim = " << this->dim() << endl; wts[0] *= 10; wts[1] *= 10; for (label j = 0; j < B.n(); j++) { B(0, j) *= 10; B(1, j) *= 10; } for (label i = 0; i < B.m(); i++) { B(i, 0) *= 10; B(i, 1) *= 10; } } } if (goodFit) { // Remove the uncorrected coefficients coeffsi[0] += deltaCoeff; coeffsi[1] -= deltaCoeff; } else { WarningInFunction << "Could not fit face " << facei << " Coefficients = " << coeffsi << ", reverting to uncorrected." << endl; coeffsi = 0; } }
int main(void){ // P2 unsigned int width(300), height(300); kn::ioEPS eps(width, height); //Draw axis eps.drawLine(0, height/2, width, height/2, 0.5, 0.5, 0.5, 1); eps.drawLine(width/2, 0, width/2, height, 0.5, 0.5, 0.5, 1); unsigned int nbPoints(100); #if 1 Eigen::MatrixXd pointsP2(nbPoints,3); srand(time(NULL)); for(unsigned int i=0; i < nbPoints; ++i){ double x = (double(width - 100)/(nbPoints-1.0) * i) - double(width)/2.0 + 50.0; double naturalOffset(10.0); double randomOffset = (rand()%50) - 25.0; pointsP2(i, 0) = x; pointsP2(i, 1) = 0.8*x + naturalOffset + randomOffset; pointsP2(i, 2) = 1.0; } //std::cout << " input data\n" << pointsP2 << std::endl << std::endl; Eigen::Matrix3d signP2 = Eigen::Matrix3d::Identity(); signP2(1, 1) = -1.0; //std::cout << " signP2\n" << signP2 << std::endl << std::endl; Eigen::MatrixXd tmp(pointsP2); tmp = pointsP2 * signP2; std::cout << " input data\n" << tmp << std::endl << std::endl; //SVD Eigen::JacobiSVD<Eigen::MatrixXd> svd(tmp, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::VectorXd h = svd.matrixV().rightCols(1); std::cout << " Solution\n" << h/h(1) << std::endl; double graduate(h(0)/h(1)); double offset = h(2)/h(1);// (h(2) / sqrt(h(0)*h(0) + h(1) * h(1))); //double offset((h(2)/h(1)) / sqrt(h(0)*h(0) + h(1) * h(1))); std::cout << "Pente: " << graduate << " Offset: " << offset << std::endl; #else Eigen::MatrixXd pointsP2(nbPoints,2); Eigen::VectorXd yP2(nbPoints); srand(time(NULL)); for(unsigned int i=0; i < nbPoints; ++i){ double x = (double(width - 100)/(nbPoints-1.0) * i) - double(width)/2.0 + 50.0; double naturalOffset(10.0); double randomOffset = (rand()%50) - 25.0; pointsP2(i, 0) = x; pointsP2(i, 1) = 1.0; yP2(i) = 0.8*x + naturalOffset + randomOffset; } std::cout << " input data\n" << pointsP2 << std::endl << std::endl; //SVD Eigen::JacobiSVD<Eigen::MatrixXd> svd(pointsP2, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::VectorXd h = svd.solve(yP2); std::cout << " Solution\n" << h << std::endl; double graduate(h(0)); double offset = h(1);// (h(2) / sqrt(h(0)*h(0) + h(1) * h(1))); //double offset((h(2)/h(1)) / sqrt(h(0)*h(0) + h(1) * h(1))); std::cout << "Pente: " << graduate << " Offset: " << offset << std::endl; #endif // affichage des points et droites ////////////////////////////////////////////////////////////////// double xTop = ( ( double(height) / 2.0f) - offset ) / graduate; double xBot = ( (-double(height) / 2.0f) - offset ) / graduate; double yLeft = graduate * (- double(width)/2.0f ) + offset; double yRight = graduate * ( double(width)/2.0f ) + offset; bool alreadyIntersect(false); double linePoint1x, linePoint1y; double linePoint2x, linePoint2y; if( (xTop >= (- double(width)/2.0f)) && (xTop <= double(width)/2.0f)){ std::cout << "Top" << std::endl; linePoint1x = xTop; linePoint1y = double(height)/2.0f; alreadyIntersect = true; } if( (xBot >= (- double(width)/2.0f)) && (xBot <= double(width)/2.0f) ){ std::cout << "Bot" << std::endl; if(alreadyIntersect){ linePoint2x = xBot; linePoint2y = -double(height)/2.0f; } else{ linePoint1x = xBot; linePoint1y = -double(height)/2.0f; } alreadyIntersect = true; } if( (yLeft > (- double(height)/2.0f)) && (yLeft < double(height)/2.0f) ){ std::cout << "Left" << std::endl; if(alreadyIntersect){ linePoint2x = -double(width)/2.0f; linePoint2y = yLeft; } else{ linePoint1x = -double(width)/2.0f; linePoint1y = yLeft; } alreadyIntersect = true; } if( (yRight > (- double(height)/2.0f)) && (yRight < double(height)/2.0f) ){ std::cout << "Right" << std::endl; if(alreadyIntersect){ linePoint2x = double(width)/2.0f; linePoint2y = yRight; } else{ linePoint1x = double(width)/2.0f; linePoint1y = yRight; } alreadyIntersect = true; } /* std::cout << linePoint1x << " " << linePoint1y << std::endl; std::cout << linePoint2x << " " << linePoint2y << std::endl; std::cout << linePoint1x + width / 2.0 << " " << height - (linePoint1y + height / 2.0) << std::endl; std::cout << linePoint2x + width / 2.0 << " " << height - (linePoint2y + height / 2.0) << std::endl; */ eps.drawLine(linePoint1x + width / 2.0, height - (linePoint1y + height / 2.0), linePoint2x + width / 2.0, height - (linePoint2y + height / 2.0), 0.0, 0.0, 1.0, 1); // draw points for( unsigned int i=0; i<pointsP2.rows(); ++i ){ eps.drawCircleFilled(width/2 + pointsP2(i, 0), height/2 - pointsP2(i, 1), 1, 1.0, 0.0, 0.0); //eps.drawCircleFilled(width/2 + pointsP2(i, 0), height/2 - yP2(i), 1, 1.0, 0.0, 0.0); } eps.saveEPS("out.eps"); return 0; }
slep_result_t malsar_low_rank( CDotFeatures* features, double* y, double rho, const slep_options& options) { int task; int n_feats = features->get_dim_feature_space(); SG_SDEBUG("n feats = %d\n", n_feats); int n_vecs = features->get_num_vectors(); SG_SDEBUG("n vecs = %d\n", n_vecs); int n_tasks = options.n_tasks; SG_SDEBUG("n tasks = %d\n", n_tasks); int iter = 0; // initialize weight vector and bias for each task MatrixXd Ws = MatrixXd::Zero(n_feats, n_tasks); VectorXd Cs = VectorXd::Zero(n_tasks); for (task=0; task<n_tasks; task++) { int n_pos = 0; int n_neg = 0; for (int i=options.ind[task]; i<options.ind[task+1]; i++) { if (y[i] > 0) n_pos++; else n_neg++; } Cs[task] = CMath::log(double(n_pos)/n_neg); } MatrixXd Wz=Ws, Wzp=Ws, Wz_old=Ws, delta_Wzp=Ws, gWs=Ws; VectorXd Cz=Cs, Czp=Cs, Cz_old=Cs, delta_Czp=Cs, gCs=Cs; double t=1, t_old=0; double gamma=1, gamma_inc=2; double obj=0.0, obj_old=0.0; internal::set_is_malloc_allowed(false); bool done = false; while (!done && iter <= options.max_iter) { double alpha = double(t_old - 1)/t; // compute search point Ws = (1+alpha)*Wz - alpha*Wz_old; Cs = (1+alpha)*Cz - alpha*Cz_old; // zero gradient gWs.setZero(); gCs.setZero(); // compute gradient and objective at search point double Fs = 0; for (task=0; task<n_tasks; task++) { for (int i=options.ind[task]; i<options.ind[task+1]; i++) { double aa = -y[i]*(features->dense_dot(i, Ws.col(task).data(), n_feats)+Cs[task]); double bb = CMath::max(aa,0.0); // avoid underflow when computing exponential loss Fs += (CMath::log(CMath::exp(-bb) + CMath::exp(aa-bb)) + bb)/n_vecs; double b = -y[i]*(1 - 1/(1+CMath::exp(aa)))/n_vecs; gCs[task] += b; features->add_to_dense_vec(b, i, gWs.col(task).data(), n_feats); } } gWs.noalias() += 2*rho*Ws; // add regularizer Fs += rho*Ws.squaredNorm(); double Fzp = 0.0; // line search, Armijo-Goldstein scheme while (true) { // compute trace projection of Ws - gWs/gamma with 2*rho/gamma internal::set_is_malloc_allowed(true); Wzp.setZero(); JacobiSVD<MatrixXd> svd(Ws - gWs/gamma,ComputeThinU | ComputeThinV); for (int i=0; i<svd.singularValues().size(); i++) { if (svd.singularValues()[i] > 2*rho/gamma) Wzp += svd.matrixU().col(i)* svd.singularValues()[i]* svd.matrixV().col(i).transpose(); } internal::set_is_malloc_allowed(false); // walk in direction of antigradient Czp = Cs - gCs/gamma; // compute objective at line search point Fzp = 0.0; for (task=0; task<n_tasks; task++) { for (int i=options.ind[task]; i<options.ind[task+1]; i++) { double aa = -y[i]*(features->dense_dot(i, Wzp.col(task).data(), n_feats)+Cs[task]); double bb = CMath::max(aa,0.0); Fzp += (CMath::log(CMath::exp(-bb) + CMath::exp(aa-bb)) + bb)/n_vecs; } } Fzp += rho*Wzp.squaredNorm(); // compute delta between line search point and search point delta_Wzp = Wzp - Ws; delta_Czp = Czp - Cs; // norms of delta double nrm_delta_Wzp = delta_Wzp.squaredNorm(); double nrm_delta_Czp = delta_Czp.squaredNorm(); double r_sum = (nrm_delta_Wzp + nrm_delta_Czp)/2; double Fzp_gamma = Fs + (delta_Wzp.transpose()*gWs).trace() + (delta_Czp.transpose()*gCs).trace() + (gamma/2)*nrm_delta_Wzp + (gamma/2)*nrm_delta_Czp; // break if delta is getting too small if (r_sum <= 1e-20) { done = true; break; } // break if objective at line searc point is smaller than Fzp_gamma if (Fzp <= Fzp_gamma) break; else gamma *= gamma_inc; } Wz_old = Wz; Cz_old = Cz; Wz = Wzp; Cz = Czp; // compute objective value obj_old = obj; obj = Fzp; internal::set_is_malloc_allowed(true); JacobiSVD<MatrixXd> svd(Wzp, EigenvaluesOnly); obj += rho*svd.singularValues().sum(); internal::set_is_malloc_allowed(false); // check if process should be terminated switch (options.termination) { case 0: if (iter>=2) { if ( CMath::abs(obj-obj_old) <= options.tolerance ) done = true; } break; case 1: if (iter>=2) { if ( CMath::abs(obj-obj_old) <= options.tolerance*CMath::abs(obj_old)) done = true; } break; case 2: if (CMath::abs(obj) <= options.tolerance) done = true; break; case 3: if (iter>=options.max_iter) done = true; break; } iter++; t_old = t; t = 0.5 * (1 + CMath::sqrt(1.0 + 4*t*t)); } SG_SDEBUG("%d iteration passed, objective = %f\n",iter,obj); SGMatrix<float64_t> tasks_w(n_feats, n_tasks); for (int i=0; i<n_feats; i++) { for (task=0; task<n_tasks; task++) tasks_w[i] = Wzp(i,task); } SGVector<float64_t> tasks_c(n_tasks); for (int i=0; i<n_tasks; i++) tasks_c[i] = Czp[i]; return slep_result_t(tasks_w, tasks_c); };
/* * Vertex positioning is based on [Kobbelt et al, 2001] */ float Octree::findVertex(Evaluator* e) { findIntersections(e); // Find the center of intersection positions glm::vec3 center = std::accumulate( intersections.begin(), intersections.end(), glm::vec3(), [](const glm::vec3& a, const Intersection& b) { return a + b.pos; }) / float(intersections.size()); /* The A matrix is of the form * [n1x, n1y, n1z] * [n2x, n2y, n2z] * [n3x, n3y, n3z] * ... * (with one row for each Hermite intersection) */ Eigen::MatrixX3f A(intersections.size(), 3); for (unsigned i=0; i < intersections.size(); ++i) { auto d = intersections[i].norm; A.row(i) << Eigen::Vector3f(d.x, d.y, d.z).transpose(); } /* The B matrix is of the form * [p1 . n1] * [p2 . n2] * [p3 . n3] * ... * (with one row for each Hermite intersection) * * Positions are pre-emtively shifted so that the center of the contoru * is at 0, 0, 0 (since the least-squares fix minimizes distance to the * origin); we'll unshift afterwards. */ Eigen::VectorXf B(intersections.size(), 1); for (unsigned i=0; i < intersections.size(); ++i) { B.row(i) << glm::dot(intersections[i].norm, intersections[i].pos - center); } // Use singular value decomposition to solve the least-squares fit. Eigen::JacobiSVD<Eigen::MatrixX3f> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV); // Truncate singular values below 0.1 auto singular = svd.singularValues(); svd.setThreshold(0.1 / singular.maxCoeff()); rank = svd.rank(); // Solve the equation and convert back to cell coordinates Eigen::Vector3f solution = svd.solve(B); vert = glm::vec3(solution.x(), solution.y(), solution.z()) + center; // Clamp vertex to be within the bounding box vert.x = std::min(X.upper(), std::max(vert.x, X.lower())); vert.y = std::min(Y.upper(), std::max(vert.y, Y.lower())); vert.z = std::min(Z.upper(), std::max(vert.z, Z.lower())); // Find and return QEF residual auto m = A * solution - B; return m.transpose() * m; }
at::Mat homographyToPose(at::real fx, at::real fy, at::real tagSize, const at::Mat& horig, bool openGLStyle) { // flip the homography along the Y axis to align the // conventional image coordinate system (y=0 at the top) with // the conventional camera coordinate system (y=0 at the // bottom). at::Mat h = horig; at::Mat F = at::Mat::eye(3,3); F(1,1) = F(2,2) = -1; h = F * horig; at::Mat M(4,4); M(0,0) = h(0,0) / fx; M(0,1) = h(0,1) / fx; M(0,3) = h(0,2) / fx; M(1,0) = h(1,0) / fy; M(1,1) = h(1,1) / fy; M(1,3) = h(1,2) / fy; M(2,0) = h(2,0); M(2,1) = h(2,1); M(2,3) = h(2,2); // Compute the scale. The columns of M should be made to be // unit vectors. This is over-determined, so we take the // geometric average. at::real scale0 = sqrt(sq(M(0,0)) + sq(M(1,0)) + sq(M(2,0))); at::real scale1 = sqrt(sq(M(0,1)) + sq(M(1,1)) + sq(M(2,1))); at::real scale = sqrt(scale0*scale1); M *= 1.0/scale; // recover sign of scale factor by noting that observations must // occur in front of the camera. if (M(2,3) > 0) { M *= -1; } // The bottom row should always be [0 0 0 1]. M(3,0) = 0; M(3,1) = 0; M(3,2) = 0; M(3,3) = 1; // recover third rotation vector by crossproduct of the other two // rotation vectors at::Vec3 a( M(0,0), M(1,0), M(2,0) ); at::Vec3 b( M(0,1), M(1,1), M(2,1) ); at::Vec3 ab = a.cross(b); M(0,2) = ab[0]; M(1,2) = ab[1]; M(2,2) = ab[2]; // pull out just the rotation component so we can normalize it. at::Mat R(3,3); for (int i=0; i<3; ++i) { for (int j=0; j<3; ++j) { R(i,j) = M(i,j); } } // polar decomposition, R = (UV')(VSV') cv::SVD svd(R); at::Mat MR = svd.u * svd.vt; if (!openGLStyle) { MR = F * MR; } for (int i=0; i<3; ++i) { for (int j=0; j<3; ++j) { M(i,j) = MR(i,j); } } // Scale the results based on the scale in the homography. The // homography assumes that tags span from -1 to +1, i.e., that // they are two units wide (and tall). for (int i = 0; i < 3; i++) { at::real scl = openGLStyle ? 1 : F(i,i); M(i,3) *= scl * tagSize / 2; } return M; }
// **************************************************************************** // This function finds a set of vectors (P) such that, for each vector p: // A.p = 0 // Note that A is a matrix of size rows x cols, P is a matrix of size cols x cols whose ROWS are the p vectors. // P must NOT point to any allocated memory prior to calling this function. // The rank deficit (nullity) is returned (0 if matrix is full rank, 1 if matrix has one singular value, etc). // tol defines how small a singular value must be in order to be considered a "zero". // **************************************************************************** long int find_null_space_vectors(double *A, long int rows, long int cols, double **P, double tolerance) { double *U, *D, *V, *R, max_val, min_val; long int i, rank_deficit, c, r; U=(double *)malloc(sizeof(double)*rows*cols); if (U==NULL) quit_error((char*)"Out of memory"); D=(double *)malloc(sizeof(double)*cols*cols); if (D==NULL) quit_error((char*)"Out of memory"); V=(double *)malloc(sizeof(double)*cols*cols); if (V==NULL) quit_error((char*)"Out of memory"); // Calculate the Singular Value Decomposition svd(A, rows, cols, U, D, V); // Find the maximum and minimum magnitude singular values max_val=0.0; min_val=DBL_MAX; for (i=0; i<cols; i++) { if (fabs(D[i*cols+i])<fabs(min_val)) min_val=fabs(D[i*cols+i]); if (fabs(D[i*cols+i])>fabs(max_val)) max_val=fabs(D[i*cols+i]); } // Count the rank deficit of the matrix, and extract // relevant rows of the transpose(V) matrix. rank_deficit=0; for (i=0; i<cols; i++) { if (fabs(D[i*cols+i])<tolerance) rank_deficit++; } // Transpose V matrix_transpose(V, cols, cols); // Generate a reduced version of transpose(V) R=(double *)malloc(sizeof(double)*cols*(rank_deficit)); if (R==NULL) quit_error((char*)"Out of memory"); r=0; for (i=0; i<cols; i++) { if (fabs(D[i*cols+i])<fabs(tolerance)) { for (c=0; c<cols; c++) { R[r*cols+c]=V[i*cols+c]; } r++; } } // Row reduce R matrix_row_reduce(R, rank_deficit, cols, tolerance); *P=R; free(V); free(D); free(U); return rank_deficit; }
void v4r::MultiPlaneSegmentation<PointT>::segment(bool force_unorganized) { models_.clear(); if(input_->isOrganized() && !force_unorganized) { pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>); if(!normals_set_) { pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne; ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX); ne.setMaxDepthChangeFactor (0.02f); ne.setNormalSmoothingSize (20.0f); ne.setBorderPolicy (pcl::IntegralImageNormalEstimation<PointT, pcl::Normal>::BORDER_POLICY_IGNORE); ne.setInputCloud (input_); ne.compute (*normal_cloud); } else { normal_cloud = normal_cloud_; } pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps; mps.setMinInliers (min_plane_inliers_); mps.setAngularThreshold (0.017453 * 2.f); // 2 degrees mps.setDistanceThreshold (0.01); // 1cm mps.setMaximumCurvature(0.002); mps.setInputNormals (normal_cloud); mps.setInputCloud (input_); std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions; std::vector<pcl::ModelCoefficients> model_coefficients; std::vector<pcl::PointIndices> inlier_indices; pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>); std::vector<pcl::PointIndices> label_indices; std::vector<pcl::PointIndices> boundary_indices; typename pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr ref_comp ( new pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label> ()); ref_comp->setDistanceThreshold (0.01f, false); ref_comp->setAngularThreshold (0.017453 * 2.f); mps.setRefinementComparator (ref_comp); mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices); //mps.segment (model_coefficients, inlier_indices); //std::cout << model_coefficients.size() << std::endl; if(merge_planes_) { //sort planes by size //check if the first plane can be merged against the others, if yes, define a new plane combining both and add it to the cue typedef boost::adjacency_matrix<boost::undirectedS, int> GraphPlane; GraphPlane mergeable_planes (model_coefficients.size ()); for(size_t i=0; i < model_coefficients.size(); i++) { Eigen::Vector3f plane_i = Eigen::Vector3f (model_coefficients[i].values[0], model_coefficients[i].values[1], model_coefficients[i].values[2]); plane_i.normalize(); for(size_t j=(i+1); j < model_coefficients.size(); j++) { Eigen::Vector3f plane_j = Eigen::Vector3f (model_coefficients[j].values[0], model_coefficients[j].values[1], model_coefficients[j].values[2]); plane_j.normalize(); //std::cout << "dot product:" << plane_i.dot(plane_j) << " diff:" << std::abs(model_coefficients[i].values[3] - model_coefficients[j].values[3]) << std::endl; if(plane_i.dot(plane_j) > 0.95) { if(std::abs(model_coefficients[i].values[3] - model_coefficients[j].values[3]) < 0.015) { boost::add_edge (static_cast<int>(i), static_cast<int>(j), mergeable_planes); } } } } boost::vector_property_map<int> components (boost::num_vertices (mergeable_planes)); int n_cc = static_cast<int> (boost::connected_components (mergeable_planes, &components[0])); std::vector<int> cc_sizes; std::vector< std::vector<int> > cc_to_model_coeff; cc_sizes.resize (n_cc, 0); cc_to_model_coeff.resize(n_cc); for (size_t i = 0; i < model_coefficients.size (); i++) { cc_sizes[components[i]]++; cc_to_model_coeff[components[i]].push_back(i); } std::vector<pcl::ModelCoefficients> new_model_coefficients; std::vector<pcl::PointIndices> new_inlier_indices; for(size_t i=0; i < cc_sizes.size(); i++) { if(cc_sizes[i] < 2) { new_model_coefficients.push_back(model_coefficients[cc_to_model_coeff[i][0]]); new_inlier_indices.push_back(inlier_indices[cc_to_model_coeff[i][0]]); continue; } //std::cout << "going to merge CC:" << cc_sizes[i] << std::endl; pcl::ModelCoefficients model_coeff; model_coeff.values.resize(4); for(size_t k=0; k < 4; k++) model_coeff.values[k] = 0.f; pcl::PointIndices merged_indices; for(size_t j=0; j < cc_to_model_coeff[i].size(); j++) { for(size_t k=0; k < 4; k++) model_coeff.values[k] += model_coefficients[cc_to_model_coeff[i][j]].values[k]; merged_indices.indices.insert(merged_indices.indices.end(), inlier_indices[cc_to_model_coeff[i][j]].indices.begin(), inlier_indices[cc_to_model_coeff[i][j]].indices.end()); } for(size_t k=0; k < 4; k++) model_coeff.values[k] /= static_cast<float>(cc_to_model_coeff[i].size()); new_model_coefficients.push_back(model_coeff); new_inlier_indices.push_back(merged_indices); } model_coefficients = new_model_coefficients; inlier_indices = new_inlier_indices; } for(size_t i=0; i < model_coefficients.size(); i++) { PlaneModel<PointT> pm; pm.coefficients_ = model_coefficients[i]; pm.cloud_ = input_; pm.inliers_ = inlier_indices[i]; //recompute coefficients based on distance to camera and normal? Eigen::Vector4f centroid; pcl::compute3DCentroid(*pm.cloud_, pm.inliers_, centroid); Eigen::Vector3f c(centroid[0],centroid[1],centroid[2]); Eigen::MatrixXf M_w(inlier_indices[i].indices.size(), 3); float sum_w = 0.f; for(size_t k=0; k < inlier_indices[i].indices.size(); k++) { const int &idx = inlier_indices[i].indices[k]; float d_c = (pm.cloud_->points[idx].getVector3fMap()).norm(); float w_k = std::max(1.f - std::abs(1.f - d_c), 0.f); //w_k = 1.f; M_w.row(k) = w_k * (pm.cloud_->points[idx].getVector3fMap() - c); sum_w += w_k; } Eigen::Matrix3f scatter; scatter.setZero (); scatter = M_w.transpose() * M_w; Eigen::JacobiSVD<Eigen::MatrixXf> svd(scatter, Eigen::ComputeFullV); //std::cout << svd.matrixV() << std::endl; Eigen::Vector3f n = svd.matrixV().col(2); //flip normal if required if(n.dot(c*-1) < 0) n = n * -1.f; float d = n.dot(c) * -1.f; //std::cout << "normal:" << n << std::endl; //std::cout << "d:" << d << std::endl; pm.coefficients_.values[0] = n[0]; pm.coefficients_.values[1] = n[1]; pm.coefficients_.values[2] = n[2]; pm.coefficients_.values[3] = d; pcl::PointIndices clean_inlier_indices; float dist_threshold_ = 0.01f; for(size_t k=0; k < inlier_indices[i].indices.size(); k++) { const int &idx = inlier_indices[i].indices[k]; Eigen::Vector3f p = pm.cloud_->points[idx].getVector3fMap(); float val = n.dot(p) + d; if(std::abs(val) <= dist_threshold_) clean_inlier_indices.indices.push_back( idx ); } pm.inliers_ = clean_inlier_indices; models_.push_back(pm); } } else { // Create the filtering object: downsample the dataset using a leaf size of 1cm pcl::VoxelGrid<PointT> vg; PointTCloudPtr cloud_filtered (new PointTCloud); vg.setInputCloud (input_); float leaf_size_ = 0.005f; float dist_threshold_ = 0.01f; vg.setLeafSize (leaf_size_, leaf_size_, leaf_size_); vg.filter (*cloud_filtered); std::vector<bool> pixel_has_not_been_labelled( cloud_filtered->points.size(), true ); // Create the segmentation object for the planar model and set all the parameters pcl::SACSegmentation<PointT> seg; pcl::ModelCoefficients coefficients; seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (1000); seg.setDistanceThreshold (dist_threshold_); PointTCloudPtr cloud_filtered_leftover (new PointTCloud (*cloud_filtered)); while (1) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered_leftover); pcl::PointIndices inliers_in_leftover; seg.segment (inliers_in_leftover, coefficients); std::cout << "inliers in left over: " << inliers_in_leftover.indices.size() << " of " << cloud_filtered_leftover->points.size() << std::endl; if ( (int)inliers_in_leftover.indices.size () < min_plane_inliers_) // Could not estimate a(nother) planar model big enough for the given cloud. break; // make indices correspond to complete downsampled cloud pcl::PointIndices indices_in_original_cloud; size_t current_index_in_leftover = 0; size_t px=0; indices_in_original_cloud.indices.resize(inliers_in_leftover.indices.size()); for(size_t inl_id=0; inl_id < inliers_in_leftover.indices.size(); inl_id++) { // assumes indices are sorted in ascending order bool found = false; do { if( pixel_has_not_been_labelled[px] ) { if( current_index_in_leftover == inliers_in_leftover.indices [inl_id] ) { indices_in_original_cloud.indices[ inl_id ] = px; found = true; } current_index_in_leftover++; } px++; } while( !found ); } for(size_t i=0; i<indices_in_original_cloud.indices.size(); i++) pixel_has_not_been_labelled[ indices_in_original_cloud.indices[i] ] = false; //save coefficients PlaneModel<PointT> pm; pm.coefficients_ = coefficients; pm.cloud_ = cloud_filtered; pm.inliers_ = indices_in_original_cloud; models_.push_back(pm); pcl::copyPointCloud(*cloud_filtered, pixel_has_not_been_labelled, *cloud_filtered_leftover); } std::cout << "Number of planes found:" << models_.size() << "organized:" << static_cast<int>(input_->isOrganized() && !force_unorganized) << std::endl; } }
void EM::mStep() { // Update means_k, covs_k and weights_k from probs_ik int dim = trainSamples.cols; // Update weights // not normalized first reduce(trainProbs, weights, 0, CV_REDUCE_SUM); // Update means means.create(nclusters, dim, CV_64FC1); means = Scalar(0); const double minPosWeight = trainSamples.rows * DBL_EPSILON; double minWeight = DBL_MAX; int minWeightClusterIndex = -1; for(int clusterIndex = 0; clusterIndex < nclusters; clusterIndex++) { if(weights.at<double>(clusterIndex) <= minPosWeight) continue; if(weights.at<double>(clusterIndex) < minWeight) { minWeight = weights.at<double>(clusterIndex); minWeightClusterIndex = clusterIndex; } Mat clusterMean = means.row(clusterIndex); for(int sampleIndex = 0; sampleIndex < trainSamples.rows; sampleIndex++) clusterMean += trainProbs.at<double>(sampleIndex, clusterIndex) * trainSamples.row(sampleIndex); clusterMean /= weights.at<double>(clusterIndex); } // Update covsEigenValues and invCovsEigenValues covs.resize(nclusters); covsEigenValues.resize(nclusters); if(covMatType == EM::COV_MAT_GENERIC) covsRotateMats.resize(nclusters); invCovsEigenValues.resize(nclusters); for(int clusterIndex = 0; clusterIndex < nclusters; clusterIndex++) { if(weights.at<double>(clusterIndex) <= minPosWeight) continue; if(covMatType != EM::COV_MAT_SPHERICAL) covsEigenValues[clusterIndex].create(1, dim, CV_64FC1); else covsEigenValues[clusterIndex].create(1, 1, CV_64FC1); if(covMatType == EM::COV_MAT_GENERIC) covs[clusterIndex].create(dim, dim, CV_64FC1); Mat clusterCov = covMatType != EM::COV_MAT_GENERIC ? covsEigenValues[clusterIndex] : covs[clusterIndex]; clusterCov = Scalar(0); Mat centeredSample; for(int sampleIndex = 0; sampleIndex < trainSamples.rows; sampleIndex++) { centeredSample = trainSamples.row(sampleIndex) - means.row(clusterIndex); if(covMatType == EM::COV_MAT_GENERIC) clusterCov += trainProbs.at<double>(sampleIndex, clusterIndex) * centeredSample.t() * centeredSample; else { double p = trainProbs.at<double>(sampleIndex, clusterIndex); for(int di = 0; di < dim; di++ ) { double val = centeredSample.at<double>(di); clusterCov.at<double>(covMatType != EM::COV_MAT_SPHERICAL ? di : 0) += p*val*val; } } } if(covMatType == EM::COV_MAT_SPHERICAL) clusterCov /= dim; clusterCov /= weights.at<double>(clusterIndex); // Update covsRotateMats for EM::COV_MAT_GENERIC only if(covMatType == EM::COV_MAT_GENERIC) { SVD svd(covs[clusterIndex], SVD::MODIFY_A + SVD::FULL_UV); covsEigenValues[clusterIndex] = svd.w; covsRotateMats[clusterIndex] = svd.u; } max(covsEigenValues[clusterIndex], minEigenValue, covsEigenValues[clusterIndex]); // update invCovsEigenValues invCovsEigenValues[clusterIndex] = 1./covsEigenValues[clusterIndex]; } for(int clusterIndex = 0; clusterIndex < nclusters; clusterIndex++) { if(weights.at<double>(clusterIndex) <= minPosWeight) { Mat clusterMean = means.row(clusterIndex); means.row(minWeightClusterIndex).copyTo(clusterMean); covs[minWeightClusterIndex].copyTo(covs[clusterIndex]); covsEigenValues[minWeightClusterIndex].copyTo(covsEigenValues[clusterIndex]); if(covMatType == EM::COV_MAT_GENERIC) covsRotateMats[minWeightClusterIndex].copyTo(covsRotateMats[clusterIndex]); invCovsEigenValues[minWeightClusterIndex].copyTo(invCovsEigenValues[clusterIndex]); } } // Normalize weights weights /= trainSamples.rows; }
bool VersionManager::isApkNeedUpdate() { return svd().ver.is_v2_high_to(lvd().ver); }