/*!\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;
}
