/*!\rst Test that outerproduct is working with some small hand-checked cases. \return number of entries where the outerproduct is invalid \endrst*/ OL_WARN_UNUSED_RESULT int TestOuterProduct() noexcept { int total_errors = 0; const int size_m = 2; const int size_n = 3; const double vector_v[size_m] = {1.2, -2.1}; const double vector_u[size_n] = {-2.7, 0.0, 3.3}; double positive_scale = 1.0; double zero_scale = 0.0; const double outer_prod[size_m*size_n] = {0.0, 1.0, 2.0, 3.0, 4.0, 5.0}; double input[size_m*size_n] = {0.0}; const double result_positive_scale[size_m*size_n] = {0.0 + (-2.7*1.2), 1.0 + (-2.7*-2.1), 2.0, 3.0, 4.0 + (3.3*1.2), 5.0 + (3.3*-2.1)}; std::copy(outer_prod, outer_prod + size_m*size_n, input); OuterProduct(size_m, size_n, positive_scale, vector_v, vector_u, input); for (int i = 0; i < size_m*size_n; ++i) { if (!CheckDoubleWithinRelative(input[i], result_positive_scale[i], std::numeric_limits<double>::epsilon())) { ++total_errors; } } std::copy(outer_prod, outer_prod + size_m*size_n, input); OuterProduct(size_m, size_n, zero_scale, vector_v, vector_u, input); for (int i = 0; i < size_m*size_n; ++i) { if (!CheckDoubleWithinRelative(input[i], outer_prod[i], std::numeric_limits<double>::epsilon())) { ++total_errors; } } return total_errors; }
vnl_double_3x3 OuterProduct(const vgl_point_3d<double> &A, const vgl_point_3d<double> &B) { vnl_double_3 a = vgl_point_to_vnl_vector(A); vnl_double_3 b = vgl_point_to_vnl_vector(B); vnl_double_3x3 OP = OuterProduct(a, b); return OP; }
/******************************************************************* ** ポリゴンモデリング(法線も計算) ** 添字(0, 1, 2)の順にCW(時計回り)で入力 ** 各頂点の座標を入力 *******************************************************************/ void OpenGL::DrawPolygonSurface(double x0, double y0, double z0, double x1, double y1, double z1, double x2, double y2, double z2) { double norm; double x, y, z; double vx1,vy1,vz1,vx2,vy2,vz2; // ベクトルの差を計算 vx1 = x0 - x1; vy1 = y0 - y1; vz1 = z0 - z1; vx2 = x2 - x1; vy2 = y2 - y1; vz2 = z2 - z1; // 外積計算 OuterProduct(vx1, vy1, vz1, vx2, vy2, vz2, &x, &y, &z); norm = sqrt(x*x + y*y + z*z); if ( norm == 0) { fprintf(stderr, "OpenGL:: DrawPolygonSurface Error\n\n"); return; } x = x/norm; y = y/norm; z = z/norm; glBegin(GL_POLYGON); glNormal3f( (GLfloat)x, (GLfloat)y, (GLfloat)z ); glVertex3f( (GLfloat)x0, (GLfloat)y0, (GLfloat)z0 ); glVertex3f( (GLfloat)x1, (GLfloat)y1, (GLfloat)z1); glVertex3f( (GLfloat)x2, (GLfloat)y2, (GLfloat)z2); glEnd(); return; }
//-------------------------------------------------------------------------------- // SolveConstitutiveEquations // - Solves the constitutive equation given the applied strain rate; i.e., // find the stress state that is compatible with the strain rate defined. // // Applying Newton Raphson to solve the the stress state given the strain state of // the system. // //-------------------------------------------------------------------------------- EigenRep SolveConstitutiveEquations( const EigenRep & InitialStressState, // initial guess, either from Sach's or previous iteration const vector<EigenRep> & SchmidtTensors, const vector<Float> & CRSS, const vector<Float> & GammaDotBase, // reference shear rate const vector<int> & RateSensitivity, const EigenRep & StrainRate, // Current strain rate - constant from caller Float EpsilonConvergence, Float MaxResolvedStress, int MaxNumIterations ) { const Float Coeff = 0.2; // global fudge factor EigenRep CurrentStressState = InitialStressState; int RemainingIterations = MaxNumIterations; EigenRep NewStressState = InitialStressState; EigenRep SavedState(0, 0, 0, 0, 0); // used to return to old state while( RemainingIterations > 0 ) { bool AdmissibleStartPointFound = false; std::vector<Float> RSS( SchmidtTensors.size(), 0 ); // This is really RSS / tau do // refresh critical resolved shear stress. // check to see if it is outside of the yield // surface, and therefore inadmissible { AdmissibleStartPointFound = true; for( int i = 0; i < SchmidtTensors.size(); i ++ ) { RSS[i] = InnerProduct( SchmidtTensors[i], CurrentStressState) / CRSS[i]; if( std::fabs( RSS[i] ) < 1e-10 ) RSS[i] = 0; if( std::fabs( RSS[i] ) > MaxResolvedStress ) AdmissibleStartPointFound = false; } if( !AdmissibleStartPointFound ) CurrentStressState = SavedState + ( CurrentStressState - SavedState ) * Coeff; RemainingIterations --; if( RemainingIterations < 0 ) return NewStressState; } while ( !AdmissibleStartPointFound ); std::vector<Float> GammaDot( SchmidtTensors.size(), 0 ); // This is really RSS / tau for( int i = 0; i < SchmidtTensors.size(); i ++ ) { if( RateSensitivity[i] -1 > 0 ) GammaDot[i] = GammaDotBase[i] * std::pow( std::fabs( RSS[i] ), static_cast<int>( RateSensitivity[i] - 1 ) ); else GammaDot[i] = GammaDotBase[i]; } // Construct residual vector R(Sigma), where Sigma is stress. R(Sigma) is a 5d vector EigenRep Residual( 0, 0, 0, 0, 0 ); // current estimate SMatrix5x5 ResidualJacobian; ResidualJacobian.SetZero(); for( int i = 0; i < SchmidtTensors.size(); i ++ ) { Residual += SchmidtTensors[i] * GammaDot[i] * RSS[i]; // Residual = StrainRate - sum_{k} [ m^{k}_i * |r_ss/ crss|^n ] // Construct F', or the Jacobian ResidualJacobian -= OuterProduct( SchmidtTensors[i], SchmidtTensors[i] ) * RateSensitivity[i] * GammaDot[i] / CRSS[i]; } Residual = Residual - StrainRate ; // need the negative residual, instead of E - R SavedState = CurrentStressState; EigenRep Delta_Stress = LU_Solver( ResidualJacobian, Residual ); // <----------- Need to hangle error from this NewStressState = CurrentStressState + Delta_Stress; Float RelativeError = static_cast<Float>(2) * Delta_Stress.Norm() / ( NewStressState + CurrentStressState ).Norm(); CurrentStressState = NewStressState; if( RelativeError < EpsilonConvergence ) { break; } } // end while return NewStressState; }
//-------------------------------------------------------------------------------- // SolveConstrainedConstitutiveEquations // // -- This is *REALLY* close to the normal SolveConstitutiveEquations. One could // replace both of these functions with a function that takes a functor... but // may not do this till later. //-------------------------------------------------------------------------------- EigenRep SolveConstrainedConstitutiveEquations( const EigenRep & InitialStressState, const vector<EigenRep> & SchmidtTensors, const vector<Float> & CRSS, const vector<Float> & GammaDotBase, // reference shear rate const vector<int> & RateSensitivity, const EigenRep & MacroscopicStrainRate, const EigenRep & LagrangeMultiplier, // also known as - lagrange multiplier, or lambda(x) const EigenRep & LocalDisplacementVariation, const SMatrix5x5 & HomogeonousReference, Float EpsilonConvergence, Float MaxResolvedStress, int MaxNumIterations, Float *NR_Error_Out ) { const Float Coeff = 0.2; // global fudge factor EigenRep CurrentStressState = InitialStressState; int RemainingIterations = MaxNumIterations; EigenRep NewStressState( -1, -3, -7, -9, -11 ); Float RelativeError = 1e5; EigenRep SavedState = InitialStressState; //------------------- // DEBUG // std::cout << "MacroscopicStrainRate " << MacroscopicStrainRate << std::endl; // std::cout << "LagrangeMultiplier " << LagrangeMultiplier << std::endl; // std::cout << "Local d-dot " << LocalDisplacementVariation << std::endl; // std::cout << "Local L \n " << HomogeonousReference << std::endl; //------------------- while( RemainingIterations > 0 ) { EigenRep LoopStartStressState = CurrentStressState; bool AdmissibleStartPointFound = false; std::vector<Float> RSS( SchmidtTensors.size(), 0 ); // This is really RSS / tau do // refresh critical resolved shear stress. // check to see if it is outside of the yield // surface, and therefore inadmissible { AdmissibleStartPointFound = true; Float MaxCRSS = 0; for( int i = 0; i < SchmidtTensors.size(); i ++ ) { RSS[i] = InnerProduct( SchmidtTensors[i], CurrentStressState) / CRSS[i]; MaxCRSS = std::max( std::fabs( RSS[i] ), MaxCRSS ); if( std::fabs( RSS[i] ) > MaxResolvedStress ) AdmissibleStartPointFound = false; } if( !AdmissibleStartPointFound ) CurrentStressState = SavedState + ( CurrentStressState - SavedState ) * Coeff; RemainingIterations --; if( RemainingIterations < 0 ) { return CurrentStressState; // InitialStressState; // not failing gracefully at all } } while ( !AdmissibleStartPointFound ); std::vector<Float> GammaDot( SchmidtTensors.size(), 0 ); // This is really RSS / tau for( int i = 0; i < SchmidtTensors.size(); i ++ ) { if( RateSensitivity[i] != 1 ) GammaDot[i] = GammaDotBase[i] * std::pow( std::fabs( RSS[i] ), ( RateSensitivity[i] - 1 ) ); else GammaDot[i] = GammaDotBase[i]; } // Construct residual vector R(Sigma), where Sigma is stress. R(Sigma) is a 5d vector EigenRep ResolvedStressSum( 0, 0, 0, 0, 0 ); // current estimate SMatrix5x5 ResidualJacobian; ResidualJacobian.SetZero(); for( int i = 0; i < SchmidtTensors.size(); i ++ ) { ResolvedStressSum += SchmidtTensors[i] * GammaDot[i] * RSS[i]; // Residual = StrainRate - sum_{k} [ m^{k}_i * |r_ss/ crss|^n ] // Construct F', or the Jacobian ResidualJacobian += OuterProduct( SchmidtTensors[i], SchmidtTensors[i] ) * RateSensitivity[i] * GammaDot[i] / CRSS[i]; // may need a sign } // ------------------------------- // If we use Eigen for this section, the // expression templating will significantly improve // the efficiency of this section significantly via // explicit vectorization. // ------------------------------- SMatrix5x5 Identity5x5; Identity5x5.SetIdentity(); ResidualJacobian = -Identity5x5 - (HomogeonousReference * ResidualJacobian) ; // was I - EigenRep Residual = CurrentStressState - LagrangeMultiplier + HomogeonousReference * ( ResolvedStressSum - MacroscopicStrainRate - LocalDisplacementVariation ); // validated SavedState = CurrentStressState; EigenRep Delta_Stress = LU_Solver( ResidualJacobian, Residual ); // <----------- Need to hangle error from this NewStressState = CurrentStressState + Delta_Stress; // was - Delta RelativeError = (NewStressState - SavedState).Norm() /( ( NewStressState + SavedState) * static_cast<Float>(0.5)).Norm(); CurrentStressState = NewStressState; if( RelativeError < EpsilonConvergence ) break; } // end while *NR_Error_Out = RelativeError; return NewStressState; }
GaussMixture* GaussMixtureEstimator::Estimate(vector<VecD>& pts) const { assert(pts.size() >= ncomps); int npts = pts.size(); int dim = pts[0].size(); // Compute the number of free variables in the model and in the // data. If the former is less than the latter then the system is // underspecified and will lead to singularities in the likelihood // function. int model_params; if (spherical) { model_params = ncomps * (2 + dim); } else if (axis_aligned) { model_params = ncomps * (1 + 2*dim); } else { model_params = ncomps * (1 + dim + dim*(dim+1)/2); } int data_params = npts * dim; // number of free variables in the data assert(data_params >= model_params); // is the system under-specified? // Initialize the model using k-means clustering GaussMixture* model = new GaussMixture(ncomps, dim); MatD resps(npts, ncomps); vector<VecD> initmeans; KMeans::Estimate(pts, ncomps, initmeans, resps); for (int i = 0; i < ncomps; i++) { model->weights[i] = 1.0 / ncomps; model->comps[i]->mean = initmeans[i]; // set covariances to 1e-10 * Identity so that in the first // iteration each point is assigned entirely to the nearest // component model->comps[i]->cov.SetIdentity(1e-10); } // Begin iterating double loglik = 0.0, prev_loglik = 0.0; for (int i = 0; i < max_iters; i++) { GaussMixtureEvaluator eval(*model); // Check for small determinants (indicates poor support) for (int j = 0; j < model->ncomps; j++) { double logdetcov = eval.Component(j).GetLogDetCov(); if (logdetcov < -50*dim) { cerr << "Warning: log(det(covariance of component " << j << "))" << " is very small: " << logdetcov << endl; cerr << " its total support is " << resps.GetRow(j).Sum() << endl; cerr << " at iteration " << i << endl; } } // Compute responsibilities (E step) //DLOG << "E step\n"; prev_loglik = loglik; loglik = 0.0; for (int j = 0; j < npts; j++) { double logdenom = eval.EvaluateLog(pts[j]); loglik += logdenom; // Compute the responsibilities for (int k = 0; k < ncomps; k++) { const GaussianEvaluator& g = eval.Component(k); double logresp = eval.logweights[k] + g.EvaluateLog(pts[j]); resps[j][k] = exp(logresp - logdenom); } } // Test for convergence //DLOG << "After iteration " << i << " log likelihood = " << loglik << endl; double reldiff = fabs((prev_loglik - loglik) / prev_loglik); if (reldiff < exit_thresh) { cout << "EM converged after " << i << " iterations" << endl; return model; } prev_loglik = loglik; // Estimate new parameters (M step) //DLOG << "M step\n"; for (int k = 0; k < ncomps; k++) { Gaussian* comp = model->comps[k].get(); VecD col = resps.GetColumn(k); double colsum = resps.GetColumn(k).Sum(); // Estimate means comp->mean.Fill(0.0); for (int j = 0; j < npts; j++) { comp->mean += col[j] * pts[j]; } comp->mean /= colsum; // Estimate covariances // Initialize the forward diagonal to a small constant to // prevent singularities when components only have a small // number of points assigned to them. comp->cov.SetIdentity(1e-10); for (int j = 0; j < npts; j++) { if (spherical) { double d = col[j] * VectorSSD(pts[j], comp->mean) / dim; for (int k = 0; k < dim; k++) { comp->cov[k][k] += d; } } else if (axis_aligned) { VecD d = pts[j] - comp->mean; for (int k = 0; k < dim; k++) { comp->cov[k][k] += col[j] * d[k]*d[k]; } } else { VecD v = pts[j] - comp->mean; comp->cov += col[j] * OuterProduct(v, v); } } comp->cov /= colsum; if (spherical) { for (int k = 0; k < dim; k++) { comp->cov[k][k] = sqrt(comp->cov[k][k]); } } // Estimate weights model->weights[k] = colsum; } // Normalize mixing coefficients model->weights /= model->weights.Sum(); } DLOG << "EM failed to converge after " << max_iters << " iterations" << endl; return model; }