//---------------------------------------------------------
CSG_Matrix calcRotnMatrix(CSG_Vector params)
{
CSG_Matrix rMat(3,3);
/*
omega = params[0];
kappa = params[1];
alpha = params[2];
*/
double sw = sin(params[0]);
double sk = sin(params[1]);
double sa = sin(params[2]);
double cw = cos(params[0]);
double ck = cos(params[1]);
double ca = cos(params[2]);
rMat[0][0] = ck * ca;
rMat[0][1] = ck * sa;
rMat[0][2] = -sk;
rMat[1][0] = sw * sk * ca - cw * sa;
rMat[1][1] = sw * sk * sa + cw * ca;
rMat[1][2] = sw * ck;
rMat[2][0] = cw * sk * ca + sw * sa;
rMat[2][1] = cw * sk * sa - sw * ca;
rMat[2][2] = cw * ck;
return rMat;
}
cv::Mat CutoutImage::scaleFCMI2InputColorImageSize( const cv::Mat srcMat )
{
cv::Mat aMat;
cv::Mat rMat = cv::Mat(inputColorImageSize, CV_8UC4, cv::Scalar(0,0,0,0));
if(srcMat.rows >= srcMat.cols) //最终图像是竖直的
{
int scaleToHeight = inputColorImageSize.height;
int scaleToWidth = (int)(((float)srcMat.cols/(float)srcMat.rows)*(float)inputColorImageSize.height);
if(scaleToWidth <= inputColorImageSize.width){
cv::resize(srcMat, aMat, cv::Size(scaleToWidth,scaleToHeight), 0, 0, CV_INTER_NN);
cv::Rect copyRect = cv::Rect( (inputColorImageSize.width - scaleToWidth)/2,0,scaleToWidth,scaleToHeight );
aMat.copyTo(rMat(copyRect));
}else{
scaleToWidth = inputColorImageSize.width;
scaleToHeight = (int)( ((float)srcMat.rows/(float)srcMat.cols)*(float)inputColorImageSize.width );
cv::resize(srcMat, aMat, cv::Size(scaleToWidth,scaleToHeight), 0, 0, CV_INTER_NN);
cv::Rect copyRect = cv::Rect( 0, (inputColorImageSize.height - scaleToHeight)/2, scaleToWidth, scaleToHeight );
aMat.copyTo(rMat(copyRect));
}
}
else //最终图像是横向的
{
int scaleToHeight = int(((float)srcMat.rows/(float)srcMat.cols)*(float)inputColorImageSize.width);
int scaleToWidth = inputColorImageSize.width;
cv::resize(srcMat, aMat, cv::Size(scaleToWidth,scaleToHeight), 0, 0, CV_INTER_NN);
cv::Rect copyRect = cv::Rect( (inputColorImageSize.width - scaleToWidth)/2,0,scaleToWidth,scaleToHeight );
aMat.copyTo(rMat(copyRect));
}
int rows = rMat.rows;
int cols = rMat.cols;
for(int y=0; y<rows; y++){
uchar *rowsData = rMat.ptr<uchar>(y);
for (int x = 0; x<cols*4; x = x+4) {
//rowsData[x] = 0;
rowsData[x + 3] = 0;
}
}
return rMat;
}
void NOX::Solver::AndersonAcceleration::qrAdd(NOX::Abstract::Vector& newCol)
{
// Increment storage depth and resize R factor
nStore++;
rMat.reshape(nStore,nStore);
// Update QR factors
// Orthogonalize against previous columns once
for (int ii = 0; ii<nStore-1; ii++) {
rMat(ii,nStore-1) = qMat[ii]->innerProduct(newCol);
newCol.update(-rMat(ii,nStore-1),*(qMat[ii]),1.0);
}
// Reorthogonalize
for (int ii = 0; ii<nStore-1; ii++) {
double alpha = qMat[ii]->innerProduct(newCol);
newCol.update(-alpha,*(qMat[ii]),1.0);
rMat(ii,nStore-1) += alpha;
}
rMat(nStore-1,nStore-1) = newCol.norm();
if (!disableChecksForUnitTesting)
TEUCHOS_TEST_FOR_EXCEPTION((rMat(nStore-1,nStore-1) < 1.0e-16),std::runtime_error,"Error - R factor is singular to machine precision!");
*(qMat[nStore-1]) = newCol.scale(1.0/rMat(nStore-1,nStore-1));
}
void NOX::Solver::AndersonAcceleration::qrDelete()
{
// Apply sequence of Givens rotations
for (int ii = 0; ii<nStore-1; ii++) {
double temp = sqrt(rMat(ii,ii+1)*rMat(ii,ii+1) + rMat(ii+1,ii+1)*rMat(ii+1,ii+1));
double c = rMat(ii,ii+1)/temp;
double s = rMat(ii+1,ii+1)/temp;
rMat(ii,ii+1) = temp;
rMat(ii+1,ii+1) = 0;
for (int jj = ii+2; jj<nStore; jj++) {
temp = c*rMat(ii,jj) + s*rMat(ii+1,jj);
rMat(ii+1,jj) = -s*rMat(ii,jj) + c*rMat(ii+1,jj);
rMat(ii,jj) = temp;
}
*workVec = *(qMat[ii]);
workVec->update(s, *(qMat[ii+1]), c);
qMat[ii+1]->update(-s, *(qMat[ii]), c);
*(qMat[ii]) = *workVec;
}
// Decrement storage depth and shrink R factor
nStore--;
for (int ii=0; ii<nStore; ii++) {
for (int jj = 0; jj<nStore; jj++)
rMat(ii,jj) = rMat(ii,jj+1);
}
rMat.reshape(nStore,nStore);
}
NOX::StatusTest::StatusType NOX::Solver::AndersonAcceleration::step()
{
prePostOperator.runPreIterate(*this);
Teuchos::ParameterList lsParams = paramsPtr->sublist("Direction").sublist("Newton").sublist("Linear Solver");
// On the first step, do some initializations
if (nIter == 0) {
// Compute F of initital guess
NOX::Abstract::Group::ReturnType rtype = solnPtr->computeF();
if (rtype != NOX::Abstract::Group::Ok) {
utilsPtr->out() << "NOX::Solver::AndersonAcceleration::init - "
<< "Unable to compute F" << std::endl;
throw "NOX Error";
}
// Test the initial guess
status = testPtr->checkStatus(*this, checkType);
if ((status == NOX::StatusTest::Converged) &&
(utilsPtr->isPrintType(NOX::Utils::Warning))) {
utilsPtr->out() << "Warning: NOX::Solver::AndersonAcceleration::init() - "
<< "The solution passed into the solver (either "
<< "through constructor or reset method) "
<< "is already converged! The solver wil not "
<< "attempt to solve this system since status is "
<< "flagged as converged." << std::endl;
}
printUpdate();
// First check status
if (status != NOX::StatusTest::Unconverged) {
prePostOperator.runPostIterate(*this);
printUpdate();
return status;
}
// Apply preconditioner if enabled
if (precond) {
if (recomputeJacobian)
solnPtr->computeJacobian();
solnPtr->applyRightPreconditioning(false, lsParams, solnPtr->getF(), *oldPrecF);
}
else
*oldPrecF = solnPtr->getF();
// Copy initial guess to old soln
*oldSolnPtr = *solnPtr;
// Compute step then first iterate with a line search.
workVec->update(mixParam,*oldPrecF);
bool ok = lineSearchPtr->compute(*solnPtr, stepSize, *workVec, *this);
if (!ok)
{
if (stepSize == 0.0)
{
utilsPtr->out() << "NOX::Solver::AndersonAcceleratino::iterate - line search failed" << std::endl;
status = NOX::StatusTest::Failed;
prePostOperator.runPostIterate(*this);
printUpdate();
return status;
}
else if (utilsPtr->isPrintType(NOX::Utils::Warning))
utilsPtr->out() << "NOX::Solver::AndersonAcceleration::iterate - using recovery step for line search" << std::endl;
}
// Compute F for the first iterate in case it isn't in the line search
rtype = solnPtr->computeF();
if (rtype != NOX::Abstract::Group::Ok)
{
utilsPtr->out() << "NOX::Solver::AndersonAcceleration::iterate - unable to compute F" << std::endl;
status = NOX::StatusTest::Failed;
prePostOperator.runPostIterate(*this);
printUpdate();
return status;
}
// Evaluate the current status.
status = testPtr->checkStatus(*this, checkType);
//Update iteration count
nIter++;
prePostOperator.runPostIterate(*this);
printUpdate();
return status;
}
// First check status
if (status != NOX::StatusTest::Unconverged) {
prePostOperator.runPostIterate(*this);
printUpdate();
return status;
}
// Apply preconditioner if enabled
if (precond) {
if (recomputeJacobian)
solnPtr->computeJacobian();
solnPtr->applyRightPreconditioning(false, lsParams, solnPtr->getF(), *precF);
}
else
*precF = solnPtr->getF();
// Manage the matrices of past iterates and QR factors
if (storeParam > 0) {
if (nIter == accelerationStartIteration) {
// Initialize
nStore = 0;
rMat.shape(0,0);
oldPrecF->update(1.0, *precF, -1.0);
qrAdd(*oldPrecF);
xMat[0]->update(1.0, solnPtr->getX(), -1.0, oldSolnPtr->getX(), 0.0);
}
else if (nIter > accelerationStartIteration) {
if (nStore == storeParam) {
Teuchos::RCP<NOX::Abstract::Vector> tempPtr = xMat[0];
for (int ii = 0; ii<nStore-1; ii++)
xMat[ii] = xMat[ii+1];
xMat[nStore-1] = tempPtr;
qrDelete();
}
oldPrecF->update(1.0, *precF, -1.0);
qrAdd(*oldPrecF);
xMat[nStore-1]->update(1.0, solnPtr->getX(), -1.0, oldSolnPtr->getX(), 0.0);
}
}
// Reorthogonalize
if ( (nStore > 1) && (orthoFrequency > 0) )
if (nIter % orthoFrequency == 0)
reorthogonalize();
// Copy current soln to the old soln.
*oldSolnPtr = *solnPtr;
*oldPrecF = *precF;
// Adjust for condition number
if (nStore > 0) {
Teuchos::LAPACK<int,double> lapack;
char normType = '1';
double invCondNum = 0.0;
int info = 0;
if ( WORK.size() < static_cast<std::size_t>(4*nStore) )
WORK.resize(4*nStore);
if (IWORK.size() < static_cast<std::size_t>(nStore))
IWORK.resize(nStore);
lapack.GECON(normType,nStore,rMat.values(),nStore,rMat.normOne(),&invCondNum,&WORK[0],&IWORK[0],&info);
if (utilsPtr->isPrintType(Utils::Details))
utilsPtr->out() << " R condition number estimate ("<< nStore << ") = " << 1.0/invCondNum << std::endl;
if (adjustForConditionNumber) {
while ( (1.0/invCondNum > dropTolerance) && (nStore > 1) ) {
Teuchos::RCP<NOX::Abstract::Vector> tempPtr = xMat[0];
for (int ii = 0; ii<nStore-1; ii++)
xMat[ii] = xMat[ii+1];
xMat[nStore-1] = tempPtr;
qrDelete();
lapack.GECON(normType,nStore,rMat.values(),nStore,rMat.normOne(),&invCondNum,&WORK[0],&IWORK[0],&info);
if (utilsPtr->isPrintType(Utils::Details))
utilsPtr->out() << " Adjusted R condition number estimate ("<< nStore << ") = " << 1.0/invCondNum << std::endl;
}
}
}
// Solve the least-squares problem.
Teuchos::SerialDenseMatrix<int,double> gamma(nStore,1), RHS(nStore,1), Rgamma(nStore,1);
for (int ii = 0; ii<nStore; ii++)
RHS(ii,0) = precF->innerProduct( *(qMat[ii]) );
//Back-solve for gamma
for (int ii = nStore-1; ii>=0; ii--) {
gamma(ii,0) = RHS(ii,0);
for (int jj = ii+1; jj<nStore; jj++) {
gamma(ii,0) -= rMat(ii,jj)*gamma(jj,0);
}
gamma(ii,0) /= rMat(ii,ii);
}
if (nStore > 0)
Rgamma.multiply(Teuchos::NO_TRANS,Teuchos::NO_TRANS,mixParam,rMat,gamma,0.0);
// Compute the step and new solution using the line search.
workVec->update(mixParam,*precF);
for (int ii=0; ii<nStore; ii++)
workVec->update(-gamma(ii,0), *(xMat[ii]), -Rgamma(ii,0),*(qMat[ii]),1.0);
bool ok = lineSearchPtr->compute(*solnPtr, stepSize, *workVec, *this);
if (!ok)
{
if (stepSize == 0.0)
{
utilsPtr->out() << "NOX::Solver::AndersonAcceleratino::iterate - line search failed" << std::endl;
status = NOX::StatusTest::Failed;
prePostOperator.runPostIterate(*this);
printUpdate();
return status;
}
else if (utilsPtr->isPrintType(NOX::Utils::Warning))
utilsPtr->out() << "NOX::Solver::AndersonAcceleration::iterate - using recovery step for line search" << std::endl;
}
// Compute F for new current solution in case the line search didn't .
NOX::Abstract::Group::ReturnType rtype = solnPtr->computeF();
if (rtype != NOX::Abstract::Group::Ok)
{
utilsPtr->out() << "NOX::Solver::AndersonAcceleration::iterate - unable to compute F" << std::endl;
status = NOX::StatusTest::Failed;
prePostOperator.runPostIterate(*this);
printUpdate();
return status;
}
// Update iteration count
nIter++;
// Evaluate the current status.
status = testPtr->checkStatus(*this, checkType);
prePostOperator.runPostIterate(*this);
printUpdate();
return status;
}
