static void phyanimal_fit_lambda(PhyAnimal model) {
nlopt_opt opt;
double lb, ub, lambda, loglh;
opt = nlopt_create(NLOPT_LN_NELDERMEAD, 1); /* one dimension */
lb = 0.0;
ub = 1.0;
nlopt_set_lower_bounds(opt,&lb);
nlopt_set_upper_bounds(opt,&ub);
/* No inequality constraints */
nlopt_set_max_objective(opt, loglh_worker, model);
nlopt_set_xtol_rel(opt, 1e-4);
lambda = 0.1;
if (nlopt_optimize(opt, &lambda, &loglh)<0) {
printf("NLOPT failed\n");
} else {
printf("Maximum lh %f at lambda %f\n",loglh,lambda);
printf("Sigma = %f\n",model->sigma);
printf("Sigma_b = %f\nSigma_e = %f\n",model->sigma*lambda,model->sigma*(1-lambda));
}
model->loglh = loglh;
model->lambda = lambda;
nlopt_destroy(opt);
return;
}
nlopt_opt make_opt(const mxArray *opts, unsigned n)
{
nlopt_opt opt = NULL, local_opt = NULL;
nlopt_algorithm algorithm;
double *tmp = NULL;
unsigned i;
algorithm = (nlopt_algorithm)
struct_val_default(opts, "algorithm", NLOPT_NUM_ALGORITHMS);
CHECK1(((int)algorithm) >= 0 && algorithm < NLOPT_NUM_ALGORITHMS,
"invalid opt.algorithm");
tmp = (double *) mxCalloc(n, sizeof(double));
opt = nlopt_create(algorithm, n);
CHECK1(opt, "nlopt: out of memory");
nlopt_set_lower_bounds(opt, struct_arrval(opts, "lower_bounds", n,
fill(tmp, n, -HUGE_VAL)));
nlopt_set_upper_bounds(opt, struct_arrval(opts, "upper_bounds", n,
fill(tmp, n, +HUGE_VAL)));
nlopt_set_stopval(opt, struct_val_default(opts, "stopval", -HUGE_VAL));
nlopt_set_ftol_rel(opt, struct_val_default(opts, "ftol_rel", 0.0));
nlopt_set_ftol_abs(opt, struct_val_default(opts, "ftol_abs", 0.0));
nlopt_set_xtol_rel(opt, struct_val_default(opts, "xtol_rel", 0.0));
nlopt_set_xtol_abs(opt, struct_arrval(opts, "xtol_abs", n,
fill(tmp, n, 0.0)));
nlopt_set_maxeval(opt, struct_val_default(opts, "maxeval", 0.0) < 0 ?
0 : struct_val_default(opts, "maxeval", 0.0));
nlopt_set_maxtime(opt, struct_val_default(opts, "maxtime", 0.0));
nlopt_set_population(opt, struct_val_default(opts, "population", 0));
nlopt_set_vector_storage(opt, struct_val_default(opts, "vector_storage", 0));
if (struct_arrval(opts, "initial_step", n, NULL))
nlopt_set_initial_step(opt,
struct_arrval(opts, "initial_step", n, NULL));
if (mxGetField(opts, 0, "local_optimizer")) {
const mxArray *local_opts = mxGetField(opts, 0, "local_optimizer");
CHECK1(mxIsStruct(local_opts),
"opt.local_optimizer must be a structure");
CHECK1(local_opt = make_opt(local_opts, n),
"error initializing local optimizer");
nlopt_set_local_optimizer(opt, local_opt);
nlopt_destroy(local_opt); local_opt = NULL;
}
mxFree(tmp);
return opt;
}
Eigen::VectorXd TargetTrackingController::getControl(const EKF *ekf, const MultiAgentMotionModel *motionModel, const std::vector<const SensorModel*> &sensorModel, double *f) const {
Evaluator evaluator(ekf, motionModel, sensorModel, params);
Eigen::VectorXd p = Eigen::VectorXd::Zero(motionModel->getControlDim());
Eigen::VectorXd lowerBound = Eigen::VectorXd::Constant(motionModel->getControlDim(), params("multi_rotor_control/controlMin").toDouble());
Eigen::VectorXd upperBound = Eigen::VectorXd::Constant(motionModel->getControlDim(), params("multi_rotor_control/controlMax").toDouble());
nlopt_opt opt = nlopt_create(NLOPT_LN_COBYLA, p.size());
// nlopt_opt opt = nlopt_create(NLOPT_LN_BOBYQA, p.size());
// nlopt_opt opt = nlopt_create(NLOPT_LN_NEWUOA_BOUND, p.size());
// nlopt_opt opt = nlopt_create(NLOPT_LN_PRAXIS, p.size());
// nlopt_opt opt = nlopt_create(NLOPT_LN_NELDERMEAD, p.size());
// nlopt_opt opt = nlopt_create(NLOPT_LN_SBPLX, p.size());
// nlopt_opt opt = nlopt_create(NLOPT_GN_ORIG_DIRECT, p.size()); // failed
// nlopt_opt opt = nlopt_create(NLOPT_GN_ORIG_DIRECT_L, p.size()); // very good: p 0.0118546 -6.27225e-05 6.27225e-05 -2.09075e-05 2.09075e-05 -8.51788e-06 -2.09075e-05 10
// nlopt_opt opt = nlopt_create(NLOPT_GN_ISRES, p.size()); // rather bad
// nlopt_opt opt = nlopt_create(NLOPT_GN_CRS2_LM, p.size());
// nlopt_opt opt = nlopt_create(NLOPT_LD_MMA, p.size());
// nlopt_opt opt = nlopt_create(NLOPT_LD_CCSAQ, p.size());
// nlopt_opt opt = nlopt_create(NLOPT_LD_SLSQP, p.size());
// nlopt_opt opt = nlopt_create(NLOPT_LD_LBFGS, p.size());
// nlopt_opt opt = nlopt_create(NLOPT_LD_TNEWTON_PRECOND, p.size()); // bad
// nlopt_opt opt = nlopt_create(NLOPT_LD_TNEWTON_PRECOND_RESTART, p.size()); // bad
// nlopt_opt opt = nlopt_create(NLOPT_LD_VAR2, p.size());
nlopt_set_min_objective(opt, f_evaluate, &evaluator);
nlopt_set_lower_bounds(opt, lowerBound.data());
nlopt_set_upper_bounds(opt, upperBound.data());
nlopt_set_ftol_abs(opt, 1E-6);
nlopt_set_xtol_rel(opt, 1E-3);
nlopt_set_maxeval(opt, 1E8);
nlopt_set_maxtime(opt, 7200);
double pa[p.size()];
memcpy(pa, p.data(), p.size()*sizeof(double));
double cost = 0;
// std::string tmp; std::cerr << "Press enter to start optimization\n"; std::getline(std::cin, tmp);
nlopt_result ret = nlopt_optimize(opt, pa, &cost);
Eigen::VectorXd p_res = Eigen::Map<Eigen::VectorXd>(pa, p.size());
if (f)
*f = cost;
std::cerr << "\nInitial guess:\n";
std::cerr << " p " << p.transpose() << "\n";
std::cerr << " value " << evaluator.evaluate(p) << "\n";
std::cerr << "Optimization result (return code " << ret << "):\n";
std::cerr << " p " << p_res.transpose() << "\n";
std::cerr << " value " << evaluator.evaluate(p_res) << "\n";
nlopt_destroy(opt);
return p_res;
}
double Optimize_Torsion_Parameters(int Idx_Soft_Tor)
{
double lb[N_TOR_PARA] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -100.0 }; /* lower bounds */
double ub[N_TOR_PARA] = { 12.0, 12.0, 12.0, 12.0, 12.0, 12.0, 12.0, 12.0, 12.0, 12.0, 100.0 }; /* lower bounds */
double Phase[32][5]={{0, 0, 0, 0, 0}, {0, 0, 0, 0, PI}, {0, 0, 0, PI, 0}, {0, 0, 0, PI, PI}, {0, 0, PI, 0, 0}, {0, 0, PI, 0, PI}, {0, 0, PI, PI, 0}, {0, 0, PI, PI, PI}, {0, PI, 0, 0, 0}, {0, PI, 0, 0, PI},
{0, PI, 0, PI, 0}, {0, PI, 0, PI, PI}, {0, PI, PI, 0, 0}, {0, PI, PI, 0, PI}, {0, PI, PI, PI, 0}, {0, PI, PI, PI, PI}, {PI, 0, 0, 0, 0}, {PI, 0, 0, 0, PI}, {PI, 0, 0, PI, 0}, {PI, 0, 0, PI, PI},
{PI, 0, PI, 0, 0}, {PI, 0, PI, 0, PI}, {PI, 0, PI, PI, 0}, {PI, 0, PI, PI, PI}, {PI, PI, 0, 0, 0}, {PI, PI, 0, 0, PI}, {PI, PI, 0, PI, 0}, {PI, PI, 0, PI, PI}, {PI, PI, PI, 0, 0}, {PI, PI, PI, 0, PI},
{PI, PI, PI, PI, 0}, {PI, PI, PI, PI, PI}};
nlopt_opt opt;
double Chi_SQ;
int i, j, Idx_Phi;
Chi_SQ_Min = 1.0E100;
opt = nlopt_create(NLOPT_LD_LBFGS, N_TOR_PARA);
nlopt_set_lower_bounds(opt, lb);
nlopt_set_upper_bounds(opt, ub);
nlopt_set_min_objective(opt, Callback_Eval_Gradient, NULL);
nlopt_set_xtol_rel(opt, 1e-8);
iseed = time(NULL);
ActiveRun = 0;
Chi_SQ_Min_Local[0] = 1.0E200;
FailCount = Iteration = 0;
Idx_Phi = IdxDihSelect[Idx_Soft_Tor];
Para_Tor[10] = 0.0;
for(i=0; i<N_TOR_PARA; i++) {
IsPara_Fixed[i] = 1;
}
IsPara_Fixed[N_TOR_PARA-1] = 0; // only optimize the last parameter
if (nlopt_optimize(opt, Para_Tor, &Chi_SQ) < 0) {
printf("nlopt failed!\n");
}
else {
printf("Chi_SQ_min = %lf Chi_SQ = %lf\n", Chi_SQ_Min, Chi_SQ);
}
memcpy(Para_Tor, Para_Best, sizeof(double)*N_TOR_PARA);
CalObjectiveFunction_Tor(Para_Tor);
nlopt_destroy(opt);
return Chi_SQ_Min;
}
int call_nlopt(nlopt_algorithm alg, double ftol, double *step_size, double *params, unsigned np, double (*func) (unsigned,const double*,double*,void*),double *lower, double *upper) {
nlopt_opt opt;
double minf; /* the minimum objective value, upon return */
int retval;
int i;
opt = nlopt_create(alg,np);
nlopt_set_lower_bounds(opt,lower);
nlopt_set_upper_bounds(opt,upper);
nlopt_set_min_objective(opt,func,NULL);
nlopt_set_ftol_abs(opt,ftol);
nlopt_set_initial_step(opt,step_size);
retval = nlopt_optimize(opt, params, &minf) ;
nlopt_destroy(opt);
return retval;
}
Shredder::Shredder(double lower_limit, double upper_limit, double tolerance)
: lower_limit(lower_limit)
, upper_limit(upper_limit)
, tolerance(tolerance)
, opt(NULL)
{
opt = nlopt_create(NLOPT_LD_SLSQP, 1);
nlopt_set_lower_bounds(opt, &lower_limit);
nlopt_set_upper_bounds(opt, &upper_limit);
nlopt_set_max_objective(opt, shredder_opt_objective,
reinterpret_cast<void*>(this));
nlopt_set_maxeval(opt, 20);
nlopt_set_ftol_abs(opt, 1e-7);
nlopt_set_xtol_abs(opt, &tolerance);
}
double Shredder::optimize(double x0)
{
// bounds may have changed
nlopt_set_lower_bounds(opt, &lower_limit);
nlopt_set_upper_bounds(opt, &upper_limit);
x0 = std::max<double>(std::min<double>(x0, upper_limit), lower_limit);
//x0 = (lower_limit + upper_limit) / 2;
double maxf;
nlopt_result result = nlopt_optimize(opt, &x0, &maxf);
if (result < 0 && result != NLOPT_ROUNDOFF_LIMITED) {
Logger::warn("Optimization failed with code %d", (int) result);
}
return std::max<double>(std::min<double>(x0, upper_limit), lower_limit);
}
/*
* NLOPT optimization routine for table tennis traj gen
*
*/
void nlopt_optim_poly_run(double *x, double *params) {
static double tol[EQ_CONSTR_DIM];
static double lb[OPTIM_DIM]; /* lower bounds */
static double ub[OPTIM_DIM]; /* upper bounds */
set_bounds(lb,ub);
const_vec(EQ_CONSTR_DIM,1e-2,tol); /* set tolerances equal to second argument */
nlopt_opt opt;
opt = nlopt_create(NLOPT_LN_COBYLA, OPTIM_DIM); /* LN = does not require gradients */
//nlopt_set_xtol_rel(opt, 1e-2);
//opt = nlopt_create(NLOPT_AUGLAG, OPTIM_DIM); /* algorithm and dimensionality */
//nlopt_set_local_optimizer(opt, opt);
nlopt_set_lower_bounds(opt, lb);
nlopt_set_upper_bounds(opt, ub);
nlopt_set_min_objective(opt, costfunc, params);
nlopt_add_inequality_mconstraint(opt, INEQ_CONSTR_DIM, joint_limits_ineq_constr, params, tol);
nlopt_add_equality_mconstraint(opt, EQ_CONSTR_DIM, kinematics_eq_constr, params, tol);
nlopt_set_xtol_rel(opt, 1e-2);
//int maxeval = 20000;
//nlopt_set_maxeval(opt, maxeval);
//double maxtime = 0.001;
//nlopt_set_maxtime(opt, maxtime);
//init_soln_to_rest_posture(x,params); //parameters are the initial joint positions q0
double initTime = get_time();
double minf; /* the minimum objective value, upon return */
if (nlopt_optimize(opt, x, &minf) < 0) {
printf("NLOPT failed!\n");
}
else {
//nlopt_example_run();
printf("NLOPT took %f ms\n", (get_time() - initTime)/1e3);
printf("Found minimum at f = %0.10g\n", minf);
test_optim(x,params);
}
nlopt_destroy(opt);
}
double NLfit::run_method(vector<realt>* best_a)
{
if (opt_ != NULL && na_ != (int) nlopt_get_dimension(opt_)) {
nlopt_destroy(opt_);
opt_ = NULL;
}
if (opt_ == NULL) {
opt_ = nlopt_create(algorithm_, na_);
nlopt_set_min_objective(opt_, calculate_for_nlopt, this);
}
// this is also handled in Fit::common_termination_criteria()
nlopt_set_maxtime(opt_, F_->get_settings()->max_fitting_time);
nlopt_set_maxeval(opt_, max_eval() - 1); // save 1 eval for final calc.
nlopt_set_ftol_rel(opt_, F_->get_settings()->ftol_rel);
nlopt_set_xtol_rel(opt_, F_->get_settings()->xtol_rel);
double *lb = new double[na_];
double *ub = new double[na_];
for (int i = 0; i < na_; ++i) {
const RealRange& d = F_->mgr.get_variable(i)->domain;
lb[i] = d.lo;
ub[i] = d.hi;
}
nlopt_set_lower_bounds(opt_, lb);
nlopt_set_upper_bounds(opt_, ub);
delete [] lb;
delete [] ub;
double opt_f;
double *a = new double[na_];
for (int i = 0; i < na_; ++i)
a[i] = a_orig_[i];
nlopt_result r = nlopt_optimize(opt_, a, &opt_f);
F_->msg("NLopt says: " + S(nlresult_to_string(r)));
best_a->assign(a, a+na_);
delete [] a;
return opt_f;
}
nlopt_result
NLOPT_STDCALL nlopt_set_local_optimizer(nlopt_opt opt,
const nlopt_opt local_opt)
{
if (opt) {
if (local_opt && local_opt->n != opt->n) return NLOPT_INVALID_ARGS;
nlopt_destroy(opt->local_opt);
opt->local_opt = nlopt_copy(local_opt);
if (local_opt) {
if (!opt->local_opt) return NLOPT_OUT_OF_MEMORY;
nlopt_set_lower_bounds(opt->local_opt, opt->lb);
nlopt_set_upper_bounds(opt->local_opt, opt->ub);
nlopt_remove_inequality_constraints(opt->local_opt);
nlopt_remove_equality_constraints(opt->local_opt);
nlopt_set_min_objective(opt->local_opt, NULL, NULL);
nlopt_set_munge(opt->local_opt, NULL, NULL);
opt->local_opt->force_stop = 0;
}
return NLOPT_SUCCESS;
}
return NLOPT_INVALID_ARGS;
}
void
FitOrientation(
const int NrOfFiles,
const int nLayers,
const double ExcludePoleAngle,
double Lsd[nLayers],
const long long int SizeObsSpots,
double TiltsOrig[3],
const double OmegaStart,
const double OmegaStep,
const double px,
double ybc[nLayers],
double zbc[nLayers],
const double gs,
double SpotsInfo[200][9],
int nSpots,
double OmegaRanges[MAX_N_OMEGA_RANGES][2],
const int NoOfOmegaRanges,
double BoxSizes[MAX_N_OMEGA_RANGES][4],
double P0[nLayers][3],
const int NrPixelsGrid,
int *ObsSpotsInfo,
double tol,
double hkls[5000][4],
double Thetas[5000],
int n_hkls,
double **SpotsOut,
double *LsdFit,
double *TiltsFit,
double **BCsFit,
double tolLsd,
double tolLsdRel,
double tolTilts,
double tolBCsa,
double tolBCsb)
{
unsigned n;
long int i,j;
n = 3+(nLayers*3)+(nSpots*3);
double x[n],xl[n],xu[n];
for (i=0;i<n;i++){
x[i] = 0;
xl[i] = 0;
xu[i] = 0;
}
int count = 0;
for (i=0;i<3;i++)
{
x[i] = TiltsOrig[count];
xl[i] = x[i] - tolTilts;
xu[i] = x[i] + tolTilts;
count++;
}
count = 0;
x[3] = Lsd[0];
xl[3] = x[3] - tolLsd;
xu[3] = x[3] + tolLsd;
count++;
for (i=4;i<nLayers+3;i++)
{
x[i] = Lsd[count] - Lsd[count-1];
xl[i] = x[i] - tolLsdRel;
xu[i] = x[i] + tolLsdRel;
count++;
}
count = 0;
for (i=3+nLayers;i<3+(nLayers*2);i++)
{
x[i] = ybc[count];
x[i+nLayers] = zbc[count];
xl[i] = x[i] - tolBCsa;
xl[i+nLayers] = x[i+nLayers] - tolBCsb;
xu[i] = x[i] + tolBCsa;
xu[i+nLayers] = x[i+nLayers] + tolBCsb;
count++;
}
count = 0;
int ps;
for( i=0; i<nSpots; i++)
{
for (j=0;j<3;j++){
ps = i*3+j+3+(nLayers*3);
x[ps] = SpotsInfo[i][6+j];
xl[ps] = x[ps] - (tol*M_PI/180);
xu[ps] = x[ps] + (tol*M_PI/180);
}
}
//for (i=0;i<n;i++) printf("%f %f %f\n",x[i],xl[i],xu[i]);
struct my_func_data f_data;
f_data.NrOfFiles = NrOfFiles;
f_data.nLayers = nLayers;
f_data.n_hkls = n_hkls;
for (i=0;i<5000;i++){
f_data.hkls[i][0] = hkls[i][0];
f_data.hkls[i][1] = hkls[i][1];
f_data.hkls[i][2] = hkls[i][2];
f_data.hkls[i][3] = hkls[i][3];
f_data.Thetas[i] = Thetas[i];
}
f_data.ExcludePoleAngle = ExcludePoleAngle;
f_data.SizeObsSpots = SizeObsSpots;
f_data.P0 = allocMatrixF(nLayers,3);
for (i=0;i<3;i++){
for (j=0;j<nSpots;j++){
f_data.XGrain[j][i] = SpotsInfo[j][2*i];
f_data.YGrain[j][i] = SpotsInfo[j][2*i+1];
}
for (j=0;j<nLayers;j++){
f_data.P0[j][i] = P0[j][i];
}
}
for (i=0;i<MAX_N_OMEGA_RANGES;i++){
for (j=0;j<2;j++){
f_data.OmegaRanges[i][j] = OmegaRanges[i][j];
}
for (j=0;j<4;j++){
f_data.BoxSizes[i][j] = BoxSizes[i][j];
}
}
f_data.ObsSpotsInfo = &ObsSpotsInfo[0];
f_data.OmegaStart = OmegaStart;
f_data.OmegaStep = OmegaStep;
f_data.px = px;
f_data.gs = gs;
f_data.nSpots = nSpots;
f_data.NoOfOmegaRanges = NoOfOmegaRanges;
f_data.NrPixelsGrid = NrPixelsGrid;
struct my_func_data *f_datat;
f_datat = &f_data;
void* trp = (struct my_func_data *) f_datat;
double tole = 1e-3;
nlopt_opt opt;
opt = nlopt_create(NLOPT_LN_NELDERMEAD, n);
nlopt_set_lower_bounds(opt, xl);
nlopt_set_upper_bounds(opt, xu);
nlopt_set_min_objective(opt, problem_function, trp);
double minf=1;
nlopt_optimize(opt, x, &minf);
nlopt_destroy(opt);
TiltsFit[0] = x[0];
TiltsFit[1] = x[1];
TiltsFit[2] = x[2];
LsdFit[0] = x[3];
for (i=1;i<nLayers;i++){
LsdFit[i] = LsdFit[i-1] + x[3+i];
}
for (i=0;i<nLayers;i++){
BCsFit[i][0] = x[3+nLayers+i];
BCsFit[i][1] = x[3+nLayers+nLayers+i];
}
for (i=0;i<nSpots;i++){
for (j=0;j<3;j++){
ps = i*3+j+3+(nLayers*3);
SpotsOut[i][j] = x[ps];
}
SpotsOut[i][3] = IndividualResults[i];
}
}
nlopt_result
NLOPT_STDCALL nlopt_minimize_econstrained(
nlopt_algorithm algorithm,
int n, nlopt_func_old f, void *f_data,
int m, nlopt_func_old fc, void *fc_data_, ptrdiff_t fc_datum_size,
int p, nlopt_func_old h, void *h_data_, ptrdiff_t h_datum_size,
const double *lb, const double *ub, /* bounds */
double *x, /* in: initial guess, out: minimizer */
double *minf, /* out: minimum */
double minf_max, double ftol_rel, double ftol_abs,
double xtol_rel, const double *xtol_abs,
double htol_rel, double htol_abs,
int maxeval, double maxtime)
{
char *fc_data = (char *) fc_data_;
char *h_data = (char *) h_data_;
nlopt_opt opt;
nlopt_result ret;
int i;
if (n < 0 || m < 0 || p < 0) return NLOPT_INVALID_ARGS;
opt = nlopt_create(algorithm, (unsigned) n);
if (!opt) return NLOPT_INVALID_ARGS;
ret = nlopt_set_min_objective(opt, (nlopt_func) f, f_data);
if (ret != NLOPT_SUCCESS) { nlopt_destroy(opt); return ret; }
for (i = 0; i < m; ++i) {
ret = nlopt_add_inequality_constraint(opt, (nlopt_func) fc,
fc_data + i*fc_datum_size,
0.0);
if (ret != NLOPT_SUCCESS) { nlopt_destroy(opt); return ret; }
}
(void) htol_rel; /* unused */
for (i = 0; i < p; ++i) {
ret = nlopt_add_equality_constraint(opt, (nlopt_func) h,
h_data + i*h_datum_size,
htol_abs);
if (ret != NLOPT_SUCCESS) { nlopt_destroy(opt); return ret; }
}
ret = nlopt_set_lower_bounds(opt, lb);
if (ret != NLOPT_SUCCESS) { nlopt_destroy(opt); return ret; }
ret = nlopt_set_upper_bounds(opt, ub);
if (ret != NLOPT_SUCCESS) { nlopt_destroy(opt); return ret; }
ret = nlopt_set_stopval(opt, minf_max);
if (ret != NLOPT_SUCCESS) { nlopt_destroy(opt); return ret; }
ret = nlopt_set_ftol_rel(opt, ftol_rel);
if (ret != NLOPT_SUCCESS) { nlopt_destroy(opt); return ret; }
ret = nlopt_set_ftol_abs(opt, ftol_abs);
if (ret != NLOPT_SUCCESS) { nlopt_destroy(opt); return ret; }
ret = nlopt_set_xtol_rel(opt, xtol_rel);
if (ret != NLOPT_SUCCESS) { nlopt_destroy(opt); return ret; }
ret = nlopt_set_xtol_abs(opt, xtol_abs);
if (ret != NLOPT_SUCCESS) { nlopt_destroy(opt); return ret; }
ret = nlopt_set_maxeval(opt, maxeval);
if (ret != NLOPT_SUCCESS) { nlopt_destroy(opt); return ret; }
ret = nlopt_set_maxtime(opt, maxtime);
if (ret != NLOPT_SUCCESS) { nlopt_destroy(opt); return ret; }
ret = nlopt_optimize(opt, x, minf);
nlopt_destroy(opt);
return ret;
}
double ColorLines::findMagnitude(const Mat *img, Mat *alpha_mat, double a[3])
{
int w=img->cols;
int h=img->rows;
int t=img->type();
int c;
if(img->type()==CV_8UC3){
c=3;
} else if(img->type()==CV_8UC1){
c=1;
}
uchar *p_data=img->data;
uchar *alpha_data=alpha_mat->data;
//cout << *alpha_mat << endl;
double img_div[h*w*c];
double *alpha=(double*)malloc(h*w*sizeof(double));
for(int i=0; i<h; i++){
for(int j=0; j<w; j++){
alpha[i*w+j]=(double)alpha_data[w*i+j]/255.0l;
for(int k=0; k<c; k++){
img_div[c*(w*i+j)+k]=(double(p_data[c*(w*i+j)+k])/255.0l)/a[k];
}
}
}
double *withoutA = (double*)malloc(h*w*c*sizeof(double));
double temp;
for(int i=0; i<h; i++){
for(int j=0; j<w; j++){
for(int k=0; k<c; k++){
temp=(double(p_data[c*(w*i+j)+k])/255.0l)-alpha[w*i+j]*a[k];
if(temp>0){
withoutA[c*(w*i+j)+k]=temp;
} else{
withoutA[c*(w*i+j)+k]=0;
}
}
}
}
double *gray=(double*)malloc(w*h*sizeof(double));
double initMag=0.5;
bool isNegative=true;
while(isNegative){
initMag+=0.1;
isNegative=false;
for(int i=0; i<h; i++){
for(int j=0; j<w; j++){
gray[w*i+j]=0;
for(int k=0; k<c; k++){
temp=withoutA[c*(w*i+j)+k]/(1-alpha[w*i+j]/initMag);
isNegative=temp<0;
gray[w*i+j]+=temp*temp;
}
gray[w*i+j]=sqrt(gray[w*i+j]);
}
}
}
double *transmission=(double*)malloc(w*h*sizeof(double));
for(int i=0; i<w*h; i++){
transmission[i]=1-alpha[i]/initMag;
}
double alpha_min=transmission[0];
double alpha_max=transmission[0];
for(int i=1; i<w*h; i++){
if(transmission[i]>alpha_max){
alpha_max=transmission[i];
} else if(transmission[i]<alpha_min){
alpha_min=transmission[i];
}
}
int numBins=50;
double binW=(alpha_max-alpha_min)/(double)numBins;
double mtrans[numBins+1];
double mshading[numBins+1];
mtrans[0]=alpha_min;
mshading[0]=0;
mtrans[numBins]=alpha_max;
for(int i=1; i<numBins; i++){
mtrans[i]=mtrans[i-1]+binW;
mshading[i]=0;
}
double val;
double *inBin=(double*)malloc(w*h*sizeof(double));
int ind;
for(int i=0; i<=numBins; i++){
ind=0;
val=mtrans[i]+binW/2.0;
for(int j=0; j<w*h; j++){
if(transmission[j]<val+binW/2.0 && transmission[j]>=val-binW/2.0){
inBin[ind]=gray[j];
ind++;
}
}
if(ind>100){
qsort(inBin, ind, sizeof(double), comp);
mshading[i]=inBin[(int)(0.995*ind+0.5)-1];
}
}
double ak[2];
ak[0]=1;
ak[1]=1;
struct_fitError entrada;
entrada.withoutA=withoutA;
entrada.alpha=alpha;
entrada.mshading=mshading;
entrada.h=w;
entrada.w=h;
entrada.c=c;
entrada.numBins=numBins;
entrada.initMag=initMag;
cout << " OPTIMIZATION " << endl;
nlopt_opt opt;
//algoritmos de minimização, nao sei qual e melhor..
opt = nlopt_create(NLOPT_GN_DIRECT_L, 2);
//opt = nlopt_create(NLOPT_GN_DIRECT, 2);
//opt = nlopt_create(NLOPT_GN_CRS2_LM, 2);
//opt = nlopt_create(NLOPT_GD_STOGO, 2);
//opt = nlopt_create( NLOPT_GD_STOGO_RAND, 2);
//opt = nlopt_create(NLOPT_GN_ISRES, 2);
//opt = nlopt_create(NLOPT_GN_ESCH, 2);
//valores maximos e minimos do espaço de busca, nao sei se 0 e 1 sao valores bons
double lb[2]={0,0};
double ub[2]={1,1};
nlopt_set_lower_bounds(opt, lb);
nlopt_set_upper_bounds(opt, ub);
nlopt_set_min_objective(opt, fitError, &entrada);
//acho que isso esta relacionado com a precisao, se aumentar o resultado fica ruim, se diminuir muito nao converge nunca
nlopt_set_xtol_rel(opt, 1e-1);
//numero maximo de iterações, pode ser que precise aumentar
nlopt_set_maxeval(opt, 500);
double minf;
if (nlopt_optimize(opt, ak, &minf) < 0) {
printf("nlopt failed!\n");
}
double mag=initMag/ak[0];
free(alpha);
free(gray);
free(inBin);
free(transmission);
//printf("resultado final: %f %f %f\n", ak[0], ak[1], mag);
return mag;
}
nlopt_result mlsl_minimize(int n, nlopt_func f, void *f_data,
const double *lb, const double *ub, /* bounds */
double *x, /* in: initial guess, out: minimizer */
double *minf,
nlopt_stopping *stop,
nlopt_opt local_opt,
int Nsamples, /* #samples/iteration (0=default) */
int lds) /* random or low-discrepancy seq. (lds) */
{
nlopt_result ret = NLOPT_SUCCESS;
mlsl_data d;
int i;
pt *p;
if (!Nsamples)
d.N = 4; /* FIXME: what is good number of samples per iteration? */
else
d.N = Nsamples;
if (d.N < 1) return NLOPT_INVALID_ARGS;
d.n = n;
d.lb = lb; d.ub = ub;
d.stop = stop;
d.f = f; d.f_data = f_data;
rb_tree_init(&d.pts, pt_compare);
rb_tree_init(&d.lms, lm_compare);
d.s = lds ? nlopt_sobol_create((unsigned) n) : NULL;
nlopt_set_min_objective(local_opt, fcount, &d);
nlopt_set_lower_bounds(local_opt, lb);
nlopt_set_upper_bounds(local_opt, ub);
nlopt_set_stopval(local_opt, stop->minf_max);
d.gamma = MLSL_GAMMA;
d.R_prefactor = sqrt(2./K2PI) * pow(gam(n) * MLSL_SIGMA, 1.0/n);
for (i = 0; i < n; ++i)
d.R_prefactor *= pow(ub[i] - lb[i], 1.0/n);
/* MLSL also suggests setting some minimum distance from points
to previous local minimiza and to the boundaries; I don't know
how to choose this as a fixed number, so I set it proportional
to R; see also the comments at top. dlm and dbound are the
proportionality constants. */
d.dlm = 1.0; /* min distance/R to local minima (FIXME: good value?) */
d.dbound = 1e-6; /* min distance/R to ub/lb boundaries (good value?) */
p = alloc_pt(n);
if (!p) { ret = NLOPT_OUT_OF_MEMORY; goto done; }
/* FIXME: how many sobol points to skip, if any? */
nlopt_sobol_skip(d.s, (unsigned) (10*n+d.N), p->x);
memcpy(p->x, x, n * sizeof(double));
p->f = f(n, x, NULL, f_data);
stop->nevals++;
if (!rb_tree_insert(&d.pts, (rb_key) p)) {
free(p); ret = NLOPT_OUT_OF_MEMORY;
}
if (nlopt_stop_forced(stop)) ret = NLOPT_FORCED_STOP;
else if (nlopt_stop_evals(stop)) ret = NLOPT_MAXEVAL_REACHED;
else if (nlopt_stop_time(stop)) ret = NLOPT_MAXTIME_REACHED;
else if (p->f < stop->minf_max) ret = NLOPT_MINF_MAX_REACHED;
while (ret == NLOPT_SUCCESS) {
rb_node *node;
double R;
get_minf(&d, minf, x);
/* sampling phase: add random/quasi-random points */
for (i = 0; i < d.N && ret == NLOPT_SUCCESS; ++i) {
p = alloc_pt(n);
if (!p) { ret = NLOPT_OUT_OF_MEMORY; goto done; }
if (d.s) nlopt_sobol_next(d.s, p->x, lb, ub);
else { /* use random points instead of LDS */
int j;
for (j = 0; j < n; ++j) p->x[j] = nlopt_urand(lb[j],ub[j]);
}
p->f = f(n, p->x, NULL, f_data);
stop->nevals++;
if (!rb_tree_insert(&d.pts, (rb_key) p)) {
free(p); ret = NLOPT_OUT_OF_MEMORY;
}
if (nlopt_stop_forced(stop)) ret = NLOPT_FORCED_STOP;
else if (nlopt_stop_evals(stop)) ret = NLOPT_MAXEVAL_REACHED;
else if (nlopt_stop_time(stop)) ret = NLOPT_MAXTIME_REACHED;
else if (p->f < stop->minf_max) ret = NLOPT_MINF_MAX_REACHED;
else {
find_closest_pt(n, &d.pts, p);
find_closest_lm(n, &d.lms, p);
pts_update_newpt(n, &d.pts, p);
}
}
/* distance threshhold parameter R in MLSL */
R = d.R_prefactor
* pow(log((double) d.pts.N) / d.pts.N, 1.0 / n);
/* local search phase: do local opt. for promising points */
node = rb_tree_min(&d.pts);
for (i = (int) (ceil(d.gamma * d.pts.N) + 0.5);
node && i > 0 && ret == NLOPT_SUCCESS; --i) {
p = (pt *) node->k;
if (is_potential_minimizer(&d, p,
R, d.dlm*R, d.dbound*R)) {
nlopt_result lret;
double *lm;
double t = nlopt_seconds();
if (nlopt_stop_forced(stop)) {
ret = NLOPT_FORCED_STOP; break;
}
if (nlopt_stop_evals(stop)) {
ret = NLOPT_MAXEVAL_REACHED; break;
}
if (stop->maxtime > 0 &&
t - stop->start >= stop->maxtime) {
ret = NLOPT_MAXTIME_REACHED; break;
}
lm = (double *) malloc(sizeof(double) * (n+1));
if (!lm) { ret = NLOPT_OUT_OF_MEMORY; goto done; }
memcpy(lm+1, p->x, sizeof(double) * n);
lret = nlopt_optimize_limited(local_opt, lm+1, lm,
stop->maxeval - stop->nevals,
stop->maxtime -
(t - stop->start));
p->minimized = 1;
if (lret < 0) { free(lm); ret = lret; goto done; }
if (!rb_tree_insert(&d.lms, lm)) {
free(lm); ret = NLOPT_OUT_OF_MEMORY;
}
else if (nlopt_stop_forced(stop)) ret = NLOPT_FORCED_STOP;
else if (*lm < stop->minf_max)
ret = NLOPT_MINF_MAX_REACHED;
else if (nlopt_stop_evals(stop))
ret = NLOPT_MAXEVAL_REACHED;
else if (nlopt_stop_time(stop))
ret = NLOPT_MAXTIME_REACHED;
else
pts_update_newlm(n, &d.pts, lm);
}
/* TODO: additional stopping criteria based
e.g. on improvement in function values, etc? */
node = rb_tree_succ(node);
}
}
get_minf(&d, minf, x);
done:
nlopt_sobol_destroy(d.s);
rb_tree_destroy_with_keys(&d.lms);
rb_tree_destroy_with_keys(&d.pts);
return ret;
}
static int test_function(int ifunc)
{
nlopt_opt opt;
testfunc func;
int i, iter;
double *x, minf, minf_max, f0, *xtabs, *lb, *ub;
nlopt_result ret;
double start = nlopt_seconds();
int total_count = 0, max_count = 0, min_count = 1 << 30;
double total_err = 0, max_err = 0;
bounds_wrap_data bw;
if (ifunc < 0 || ifunc >= NTESTFUNCS) {
fprintf(stderr, "testopt: invalid function %d\n", ifunc);
listfuncs(stderr);
return 0;
}
func = testfuncs[ifunc];
x = (double *) malloc(sizeof(double) * func.n * 5);
if (!x) {
fprintf(stderr, "testopt: Out of memory!\n");
return 0;
}
lb = x + func.n * 3;
ub = lb + func.n;
xtabs = x + func.n * 2;
bw.lb = lb;
bw.ub = ub;
bw.f = func.f;
bw.f_data = func.f_data;
for (i = 0; i < func.n; ++i)
xtabs[i] = xtol_abs;
minf_max = minf_max_delta > (-HUGE_VAL) ? minf_max_delta + func.minf : (-HUGE_VAL);
printf("-----------------------------------------------------------\n");
printf("Optimizing %s (%d dims) using %s algorithm\n", func.name, func.n, nlopt_algorithm_name(algorithm));
printf("lower bounds at lb = [");
for (i = 0; i < func.n; ++i)
printf(" %g", func.lb[i]);
printf("]\n");
printf("upper bounds at ub = [");
for (i = 0; i < func.n; ++i)
printf(" %g", func.ub[i]);
printf("]\n");
memcpy(lb, func.lb, func.n * sizeof(double));
memcpy(ub, func.ub, func.n * sizeof(double));
for (i = 0; i < func.n; ++i)
if (fix_bounds[i]) {
printf("fixing bounds for dim[%d] to xmin[%d]=%g\n", i, i, func.xmin[i]);
lb[i] = ub[i] = func.xmin[i];
}
if (force_constraints) {
for (i = 0; i < func.n; ++i) {
if (nlopt_iurand(2) == 0)
ub[i] = nlopt_urand(lb[i], func.xmin[i]);
else
lb[i] = nlopt_urand(func.xmin[i], ub[i]);
}
printf("adjusted lower bounds at lb = [");
for (i = 0; i < func.n; ++i)
printf(" %g", lb[i]);
printf("]\n");
printf("adjusted upper bounds at ub = [");
for (i = 0; i < func.n; ++i)
printf(" %g", ub[i]);
printf("]\n");
}
if (fabs(func.f(func.n, func.xmin, 0, func.f_data) - func.minf) > 1e-8) {
fprintf(stderr, "BUG: function does not achieve given lower bound!\n");
fprintf(stderr, "f(%g", func.xmin[0]);
for (i = 1; i < func.n; ++i)
fprintf(stderr, ", %g", func.xmin[i]);
fprintf(stderr, ") = %0.16g instead of %0.16g, |diff| = %g\n", func.f(func.n, func.xmin, 0, func.f_data), func.minf, fabs(func.f(func.n, func.xmin, 0, func.f_data) - func.minf));
free(x);
return 0;
}
for (iter = 0; iter < iterations; ++iter) {
double val;
testfuncs_counter = 0;
printf("Starting guess x = [");
for (i = 0; i < func.n; ++i) {
if (center_start)
x[i] = (ub[i] + lb[i]) * 0.5;
else if (xinit_tol < 0) { /* random starting point near center of box */
double dx = (ub[i] - lb[i]) * 0.25;
double xm = 0.5 * (ub[i] + lb[i]);
x[i] = nlopt_urand(xm - dx, xm + dx);
} else {
x[i] = nlopt_urand(-xinit_tol, xinit_tol)
+ (1 + nlopt_urand(-xinit_tol, xinit_tol)) * func.xmin[i];
if (x[i] > ub[i])
x[i] = ub[i];
else if (x[i] < lb[i])
x[i] = lb[i];
}
printf(" %g", x[i]);
}
printf("]\n");
f0 = func.f(func.n, x, x + func.n, func.f_data);
printf("Starting function value = %g\n", f0);
if (iter == 0 && testfuncs_verbose && func.has_gradient) {
printf("checking gradient:\n");
for (i = 0; i < func.n; ++i) {
double f;
x[i] *= 1 + 1e-6;
f = func.f(func.n, x, NULL, func.f_data);
x[i] /= 1 + 1e-6;
printf(" grad[%d] = %g vs. numerical derivative %g\n", i, x[i + func.n], (f - f0) / (x[i] * 1e-6));
}
}
testfuncs_counter = 0;
opt = nlopt_create(algorithm, func.n);
nlopt_set_min_objective(opt, bounds_wrap_func, &bw);
nlopt_set_lower_bounds(opt, lb);
nlopt_set_upper_bounds(opt, ub);
nlopt_set_stopval(opt, minf_max);
nlopt_set_ftol_rel(opt, ftol_rel);
nlopt_set_ftol_abs(opt, ftol_abs);
nlopt_set_xtol_rel(opt, xtol_rel);
nlopt_set_xtol_abs(opt, xtabs);
nlopt_set_maxeval(opt, maxeval);
nlopt_set_maxtime(opt, maxtime);
ret = nlopt_optimize(opt, x, &minf);
printf("finished after %g seconds.\n", nlopt_seconds() - start);
printf("return code %d from nlopt_minimize\n", ret);
if (ret < 0 && ret != NLOPT_ROUNDOFF_LIMITED && ret != NLOPT_FORCED_STOP) {
fprintf(stderr, "testopt: error in nlopt_minimize\n");
free(x);
return 0;
}
printf("Found minimum f = %g after %d evaluations (numevals = %d).\n", minf, testfuncs_counter, nlopt_get_numevals(opt));
nlopt_destroy(opt);
total_count += testfuncs_counter;
if (testfuncs_counter > max_count)
max_count = testfuncs_counter;
if (testfuncs_counter < min_count)
min_count = testfuncs_counter;
printf("Minimum at x = [");
for (i = 0; i < func.n; ++i)
printf(" %g", x[i]);
printf("]\n");
if (func.minf == 0)
printf("|f - minf| = %g\n", fabs(minf - func.minf));
else
printf("|f - minf| = %g, |f - minf| / |minf| = %e\n", fabs(minf - func.minf), fabs(minf - func.minf) / fabs(func.minf));
total_err += fabs(minf - func.minf);
if (fabs(minf - func.minf) > max_err)
max_err = fabs(minf - func.minf);
printf("vs. global minimum f = %g at x = [", func.minf);
for (i = 0; i < func.n; ++i)
printf(" %g", func.xmin[i]);
printf("]\n");
val = func.f(func.n, x, NULL, func.f_data);
if (fabs(val - minf) > 1e-12) {
fprintf(stderr, "Mismatch %g between returned minf=%g and f(x) = %g\n", minf - val, minf, val);
free(x);
return 0;
}
}
if (iterations > 1)
printf("average #evaluations = %g (%d-%d)\naverage |f-minf| = %g, max |f-minf| = %g\n", total_count * 1.0 / iterations, min_count, max_count, total_err / iterations, max_err);
free(x);
return 1;
}
nlopt_result mma_minimize(unsigned n, nlopt_func f, void *f_data,
unsigned m, nlopt_constraint *fc,
const double *lb, const double *ub, /* bounds */
double *x, /* in: initial guess, out: minimizer */
double *minf,
nlopt_stopping *stop,
nlopt_opt dual_opt)
{
nlopt_result ret = NLOPT_SUCCESS;
double *xcur, rho, *sigma, *dfdx, *dfdx_cur, *xprev, *xprevprev, fcur;
double *dfcdx, *dfcdx_cur;
double *fcval, *fcval_cur, *rhoc, *gcval, *y, *dual_lb, *dual_ub;
unsigned i, ifc, j, k = 0;
dual_data dd;
int feasible;
double infeasibility;
unsigned mfc;
m = nlopt_count_constraints(mfc = m, fc);
if (nlopt_get_dimension(dual_opt) != m) return NLOPT_INVALID_ARGS;
sigma = (double *) malloc(sizeof(double) * (6*n + 2*m*n + m*7));
if (!sigma) return NLOPT_OUT_OF_MEMORY;
dfdx = sigma + n;
dfdx_cur = dfdx + n;
xcur = dfdx_cur + n;
xprev = xcur + n;
xprevprev = xprev + n;
fcval = xprevprev + n;
fcval_cur = fcval + m;
rhoc = fcval_cur + m;
gcval = rhoc + m;
dual_lb = gcval + m;
dual_ub = dual_lb + m;
y = dual_ub + m;
dfcdx = y + m;
dfcdx_cur = dfcdx + m*n;
dd.n = n;
dd.x = x;
dd.lb = lb;
dd.ub = ub;
dd.sigma = sigma;
dd.dfdx = dfdx;
dd.dfcdx = dfcdx;
dd.fcval = fcval;
dd.rhoc = rhoc;
dd.xcur = xcur;
dd.gcval = gcval;
for (j = 0; j < n; ++j) {
if (nlopt_isinf(ub[j]) || nlopt_isinf(lb[j]))
sigma[j] = 1.0; /* arbitrary default */
else
sigma[j] = 0.5 * (ub[j] - lb[j]);
}
rho = 1.0;
for (i = 0; i < m; ++i) {
rhoc[i] = 1.0;
dual_lb[i] = y[i] = 0.0;
dual_ub[i] = HUGE_VAL;
}
dd.fval = fcur = *minf = f(n, x, dfdx, f_data);
stop->nevals++;
memcpy(xcur, x, sizeof(double) * n);
if (nlopt_stop_forced(stop)) { ret = NLOPT_FORCED_STOP; goto done; }
feasible = 1; infeasibility = 0;
for (i = ifc = 0; ifc < mfc; ++ifc) {
nlopt_eval_constraint(fcval + i, dfcdx + i*n,
fc + ifc, n, x);
i += fc[ifc].m;
if (nlopt_stop_forced(stop)) { ret = NLOPT_FORCED_STOP; goto done; }
}
for (i = 0; i < m; ++i) {
feasible = feasible && (fcval[i] <= 0 || isnan(fcval[i]));
if (fcval[i] > infeasibility) infeasibility = fcval[i];
}
/* For non-feasible initial points, set a finite (large)
upper-bound on the dual variables. What this means is that,
if no feasible solution is found from the dual problem, it
will minimize the dual objective with the unfeasible
constraint weighted by 1e40 -- basically, minimizing the
unfeasible constraint until it becomes feasible or until we at
least obtain a step towards a feasible point.
Svanberg suggested a different approach in his 1987 paper, basically
introducing additional penalty variables for unfeasible constraints,
but this is easier to implement and at least as efficient. */
if (!feasible)
for (i = 0; i < m; ++i) dual_ub[i] = 1e40;
nlopt_set_min_objective(dual_opt, dual_func, &dd);
nlopt_set_lower_bounds(dual_opt, dual_lb);
nlopt_set_upper_bounds(dual_opt, dual_ub);
nlopt_set_stopval(dual_opt, -HUGE_VAL);
nlopt_remove_inequality_constraints(dual_opt);
nlopt_remove_equality_constraints(dual_opt);
while (1) { /* outer iterations */
double fprev = fcur;
if (nlopt_stop_forced(stop)) ret = NLOPT_FORCED_STOP;
else if (nlopt_stop_evals(stop)) ret = NLOPT_MAXEVAL_REACHED;
else if (nlopt_stop_time(stop)) ret = NLOPT_MAXTIME_REACHED;
else if (feasible && *minf < stop->minf_max)
ret = NLOPT_MINF_MAX_REACHED;
if (ret != NLOPT_SUCCESS) goto done;
if (++k > 1) memcpy(xprevprev, xprev, sizeof(double) * n);
memcpy(xprev, xcur, sizeof(double) * n);
while (1) { /* inner iterations */
double min_dual, infeasibility_cur;
int feasible_cur, inner_done;
unsigned save_verbose;
int new_infeasible_constraint;
nlopt_result reti;
/* solve dual problem */
dd.rho = rho; dd.count = 0;
save_verbose = mma_verbose;
mma_verbose = 0; /* no recursive verbosity */
reti = nlopt_optimize_limited(dual_opt, y, &min_dual,
0,
stop->maxtime - (nlopt_seconds()
- stop->start));
mma_verbose = save_verbose;
if (reti < 0 || reti == NLOPT_MAXTIME_REACHED) {
ret = reti;
goto done;
}
dual_func(m, y, NULL, &dd); /* evaluate final xcur etc. */
if (mma_verbose) {
printf("MMA dual converged in %d iterations to g=%g:\n",
dd.count, dd.gval);
for (i = 0; i < MIN(mma_verbose, m); ++i)
printf(" MMA y[%d]=%g, gc[%d]=%g\n",
i, y[i], i, dd.gcval[i]);
}
fcur = f(n, xcur, dfdx_cur, f_data);
stop->nevals++;
if (nlopt_stop_forced(stop)) {
ret = NLOPT_FORCED_STOP; goto done; }
feasible_cur = 1; infeasibility_cur = 0;
new_infeasible_constraint = 0;
inner_done = dd.gval >= fcur;
for (i = ifc = 0; ifc < mfc; ++ifc) {
nlopt_eval_constraint(fcval_cur + i, dfcdx_cur + i*n,
fc + ifc, n, xcur);
i += fc[ifc].m;
if (nlopt_stop_forced(stop)) {
ret = NLOPT_FORCED_STOP; goto done; }
}
for (i = ifc = 0; ifc < mfc; ++ifc) {
unsigned i0 = i, inext = i + fc[ifc].m;
for (; i < inext; ++i)
if (!isnan(fcval_cur[i])) {
feasible_cur = feasible_cur
&& (fcval_cur[i] <= fc[ifc].tol[i-i0]);
if (!isnan(fcval[i]))
inner_done = inner_done &&
(dd.gcval[i] >= fcval_cur[i]);
else if (fcval_cur[i] > 0)
new_infeasible_constraint = 1;
if (fcval_cur[i] > infeasibility_cur)
infeasibility_cur = fcval_cur[i];
}
}
if ((fcur < *minf && (inner_done || feasible_cur || !feasible))
|| (!feasible && infeasibility_cur < infeasibility)) {
if (mma_verbose && !feasible_cur)
printf("MMA - using infeasible point?\n");
dd.fval = *minf = fcur;
infeasibility = infeasibility_cur;
memcpy(fcval, fcval_cur, sizeof(double)*m);
memcpy(x, xcur, sizeof(double)*n);
memcpy(dfdx, dfdx_cur, sizeof(double)*n);
memcpy(dfcdx, dfcdx_cur, sizeof(double)*n*m);
/* once we have reached a feasible solution, the
algorithm should never make the solution infeasible
again (if inner_done), although the constraints may
be violated slightly by rounding errors etc. so we
must be a little careful about checking feasibility */
if (infeasibility_cur == 0) {
if (!feasible) { /* reset upper bounds to infin. */
for (i = 0; i < m; ++i) dual_ub[i] = HUGE_VAL;
nlopt_set_upper_bounds(dual_opt, dual_ub);
}
feasible = 1;
}
else if (new_infeasible_constraint) feasible = 0;
}
if (nlopt_stop_forced(stop)) ret = NLOPT_FORCED_STOP;
else if (nlopt_stop_evals(stop)) ret = NLOPT_MAXEVAL_REACHED;
else if (nlopt_stop_time(stop)) ret = NLOPT_MAXTIME_REACHED;
else if (feasible && *minf < stop->minf_max)
ret = NLOPT_MINF_MAX_REACHED;
if (ret != NLOPT_SUCCESS) goto done;
if (inner_done) break;
if (fcur > dd.gval)
rho = MIN(10*rho, 1.1 * (rho + (fcur-dd.gval) / dd.wval));
for (i = 0; i < m; ++i)
if (!isnan(fcval_cur[i]) && fcval_cur[i] > dd.gcval[i])
rhoc[i] =
MIN(10*rhoc[i],
1.1 * (rhoc[i] + (fcval_cur[i]-dd.gcval[i])
/ dd.wval));
if (mma_verbose)
printf("MMA inner iteration: rho -> %g\n", rho);
for (i = 0; i < MIN(mma_verbose, m); ++i)
printf(" MMA rhoc[%d] -> %g\n", i,rhoc[i]);
}
if (nlopt_stop_ftol(stop, fcur, fprev))
ret = NLOPT_FTOL_REACHED;
if (nlopt_stop_x(stop, xcur, xprev))
ret = NLOPT_XTOL_REACHED;
if (ret != NLOPT_SUCCESS) goto done;
/* update rho and sigma for iteration k+1 */
rho = MAX(0.1 * rho, MMA_RHOMIN);
if (mma_verbose)
printf("MMA outer iteration: rho -> %g\n", rho);
for (i = 0; i < m; ++i)
rhoc[i] = MAX(0.1 * rhoc[i], MMA_RHOMIN);
for (i = 0; i < MIN(mma_verbose, m); ++i)
printf(" MMA rhoc[%d] -> %g\n", i, rhoc[i]);
if (k > 1) {
for (j = 0; j < n; ++j) {
double dx2 = (xcur[j]-xprev[j]) * (xprev[j]-xprevprev[j]);
double gam = dx2 < 0 ? 0.7 : (dx2 > 0 ? 1.2 : 1);
sigma[j] *= gam;
if (!nlopt_isinf(ub[j]) && !nlopt_isinf(lb[j])) {
sigma[j] = MIN(sigma[j], 10*(ub[j]-lb[j]));
sigma[j] = MAX(sigma[j], 0.01*(ub[j]-lb[j]));
}
}
for (j = 0; j < MIN(mma_verbose, n); ++j)
printf(" MMA sigma[%d] -> %g\n",
j, sigma[j]);
}
}
done:
free(sigma);
return ret;
}
void omxInvokeNLOPT(double *est, GradientOptimizerContext &goc)
{
goc.optName = "SLSQP";
goc.setupSimpleBounds();
goc.useGradient = true;
FitContext *fc = goc.fc;
int oldWanted = fc->wanted;
fc->wanted = 0;
omxState *globalState = fc->state;
nlopt_opt opt = nlopt_create(NLOPT_LD_SLSQP, fc->numParam);
goc.extraData = opt;
//local_opt = nlopt_create(NLOPT_LD_SLSQP, n); // Subsidiary algorithm
//nlopt_set_local_optimizer(opt, local_opt);
nlopt_set_lower_bounds(opt, goc.solLB.data());
nlopt_set_upper_bounds(opt, goc.solUB.data());
int eq, ieq;
globalState->countNonlinearConstraints(eq, ieq);
if (fc->CI) {
nlopt_set_xtol_rel(opt, 5e-3);
std::vector<double> tol(fc->numParam, std::numeric_limits<double>::epsilon());
nlopt_set_xtol_abs(opt, tol.data());
} else {
// The *2 is there to roughly equate accuracy with NPSOL.
nlopt_set_ftol_rel(opt, goc.ControlTolerance * 2);
nlopt_set_ftol_abs(opt, std::numeric_limits<double>::epsilon());
}
nlopt_set_min_objective(opt, SLSQP::nloptObjectiveFunction, &goc);
double feasibilityTolerance = Global->feasibilityTolerance;
SLSQP::context ctx(goc);
if (eq + ieq) {
ctx.origeq = eq;
if (ieq > 0){
goc.inequality.resize(ieq);
std::vector<double> tol(ieq, feasibilityTolerance);
nlopt_add_inequality_mconstraint(opt, ieq, SLSQP::nloptInequalityFunction, &goc, tol.data());
}
if (eq > 0){
goc.equality.resize(eq);
std::vector<double> tol(eq, feasibilityTolerance);
nlopt_add_equality_mconstraint(opt, eq, SLSQP::nloptEqualityFunction, &ctx, tol.data());
}
}
int priorIterations = fc->iterations;
int code = nlopt_optimize(opt, est, &fc->fit);
if (ctx.eqredundent) {
nlopt_remove_equality_constraints(opt);
eq -= ctx.eqredundent;
std::vector<double> tol(eq, feasibilityTolerance);
nlopt_add_equality_mconstraint(opt, eq, SLSQP::nloptEqualityFunction, &ctx, tol.data());
code = nlopt_optimize(opt, est, &fc->fit);
}
if (goc.verbose >= 2) mxLog("nlopt_optimize returned %d", code);
nlopt_destroy(opt);
fc->wanted = oldWanted;
if (code == NLOPT_INVALID_ARGS) {
Rf_error("NLOPT invoked with invalid arguments");
} else if (code == NLOPT_OUT_OF_MEMORY) {
Rf_error("NLOPT ran out of memory");
} else if (code == NLOPT_FORCED_STOP) {
if (fc->iterations - priorIterations <= 1) {
goc.informOut = INFORM_STARTING_VALUES_INFEASIBLE;
} else {
goc.informOut = INFORM_ITERATION_LIMIT;
}
} else if (code == NLOPT_ROUNDOFF_LIMITED) {
if (fc->iterations - priorIterations <= 2) {
Rf_error("%s: Failed due to singular matrix E or C in LSQ subproblem or "
"rank-deficient equality constraint subproblem or "
"positive directional derivative in line search", goc.optName);
} else {
goc.informOut = INFORM_NOT_AT_OPTIMUM; // is this correct? TODO
}
} else if (code < 0) {
Rf_error("NLOPT fatal error %d", code);
} else if (code == NLOPT_MAXEVAL_REACHED) {
goc.informOut = INFORM_ITERATION_LIMIT;
} else {
goc.informOut = INFORM_CONVERGED_OPTIMUM;
}
}
double Optimize_Torsion_Parameters(void)
{
double lb[N_TOR_PARA] = {
-20.0, -PI,
-20.0, -PI,
-20.0, -PI,
-20.0, -PI,
-20.0, -PI,
-100 };
double ub[N_TOR_PARA] = {
+20.0, +PI,
+20.0, +PI,
+20.0, +PI,
+20.0, +PI,
+20.0, +PI,
+100 };
// double ub[N_TOR_PARA] = { 8.0, 8.0, 8.0, 8.0, 8.0, 8.0, 8.0, 8.0, 8.0, 8.0, 100.0 }; /* lower bounds */
nlopt_opt opt;
double Chi_SQ;
int i, j;
Chi_SQ_Min = 1.0E100;
for(i=0; i<NRUN; i++) { // was 32
ActiveRun = i;
Chi_SQ_Min_Local[i] = 1.0E200;
FailCount = Iteration = 0;
// for(j=0; j<5; j++) {
// Para_Tor[2*j] = 0.1+1.0*rand(iseed);
// }
for( i=0; i<11; i++ ) {
Para_Tor[i] = 0.;
IsPara_Fixed[i] = 0.;
} // inital values
//opt = nlopt_create(NLOPT_LD_LBFGS, N_TOR_PARA);
printf("=== Fitting ===\n" );
opt = nlopt_create(NLOPT_LN_COBYLA, N_TOR_PARA);
nlopt_set_lower_bounds(opt, lb);
nlopt_set_upper_bounds(opt, ub);
nlopt_set_min_objective(opt, Callback_Eval_Gradient, NULL);
nlopt_set_xtol_rel(opt, 1e-8);
if (nlopt_optimize(opt, Para_Tor, &Chi_SQ) < 0) {
printf("nlopt didn't converge %lf %lf\n Please check carefully the rotamer results\n",Chi_SQ_Min, Chi_SQ);
}
else {
printf("Chi_SQ_min = %lf Chi_SQ = %lf\n", Chi_SQ_Min, Chi_SQ);
}
nlopt_destroy(opt);
}
memcpy(Para_Tor, Para_Best, sizeof(double)*N_TOR_PARA);
CalObjectiveFunction_Tor(Para_Tor);
return Chi_SQ_Min;
}
/********************************************************************
* nlopt main function
* *****************************************************************/
SEXP TKF92LikelihoodFunction3DMain_nlopt(SEXP seq1IntR, SEXP seq2IntR,
SEXP expectedLength, SEXP probMatR, SEXP eqFrequenciesR,
SEXP method){
int ncol, nrow;
ncol = INTEGER(GET_DIM(probMatR))[1];
nrow = INTEGER(GET_DIM(probMatR))[0];
int i, j;
// probMat
gsl_matrix *probMat = gsl_matrix_alloc(nrow, ncol);
for(i = 0; i < nrow; i++)
for(j = 0; j < ncol; j++)
gsl_matrix_set(probMat, i, j, REAL(probMatR)[i+j*ncol]);
// eqFrequenciesR
gsl_vector *eqFrequencies = gsl_vector_alloc(GET_LENGTH(eqFrequenciesR));
for(i = 0; i < GET_LENGTH(eqFrequenciesR); i++){
gsl_vector_set(eqFrequencies, i, REAL(eqFrequenciesR)[i]);
}
// seqInt preparation
int *seq1Int, *seq2Int;
seq1Int = (int *) R_alloc(GET_LENGTH(seq1IntR), sizeof(int));
seq2Int = (int *) R_alloc(GET_LENGTH(seq2IntR), sizeof(int));
for(i = 0; i < GET_LENGTH(seq1IntR); i++){
seq1Int[i] = INTEGER(seq1IntR)[i];
}
for(i = 0; i < GET_LENGTH(seq2IntR); i++){
seq2Int[i] = INTEGER(seq2IntR)[i];
}
// construct params
struct TKF92LikelihoodFunction3D_params params;
params.len = REAL(expectedLength)[0];
params.substModel = probMat;
params.eqFrequencies = eqFrequencies;
params.seq1Int = seq1Int;
params.seq2Int = seq2Int;
params.SA = GET_LENGTH(seq1IntR);
params.SB = GET_LENGTH(seq2IntR);
// nlopt main procedure
double lb[3] = {0.0494497, 1e-20, 1e-20}; // lower bounds
double ub[3] = {2000, 0.1, 1-1e-20}; // upper bounds
//double dx[3] = {20, 0.01, 0.1}; // The initial step size
nlopt_opt opt;
if(strcmp(CHAR(STRING_ELT(method, 0)), "NM") == 0){
opt = nlopt_create(NLOPT_LN_NELDERMEAD, 3); /* algorithm and dimensionality */
}else if(strcmp(CHAR(STRING_ELT(method, 0)), "Sbplx") == 0){
opt = nlopt_create(NLOPT_LN_SBPLX, 3);
}else if(strcmp(CHAR(STRING_ELT(method, 0)), "COBYLA") == 0){
opt = nlopt_create(NLOPT_LN_COBYLA, 3);
}else if(strcmp(CHAR(STRING_ELT(method, 0)), "BOBYQA") == 0){
opt = nlopt_create(NLOPT_LN_BOBYQA, 3);
}else if(strcmp(CHAR(STRING_ELT(method, 0)), "PRAXIS") == 0){
opt = nlopt_create(NLOPT_LN_PRAXIS, 3);
}else{
error("Wrong optimisation method!");
}
nlopt_set_lower_bounds(opt, lb);
nlopt_set_upper_bounds(opt, ub);
nlopt_set_min_objective(opt, TKF92LikelihoodFunction3D_nlopt, ¶ms);
nlopt_set_ftol_rel(opt, F_TOL); // stopping criteria
//nlopt_set_initial_step(opt, dx); // initial step size
nlopt_set_maxeval(opt, MAX_ITER);
double x[3] = {100, exp(-3), 0.5}; /* some initial guess */
double minf; /* the minimum objective value, upon return */
if (nlopt_optimize(opt, x, &minf) < 0) {
Rprintf("nlopt failed!\n");
}else{
Rprintf("found minimum at f(%g,%g,%g) = %0.10g using %s algorithm\n",
x[0], x[1], x[2], minf, CHAR(STRING_ELT(method, 0)));
}
SEXP ans, ansNames;
PROTECT(ans = NEW_NUMERIC(4)); // a vector of distance, mu, r and the negative log likelihood
PROTECT(ansNames = NEW_CHARACTER(4));
REAL(ans)[0] = x[0];
REAL(ans)[1] = x[1];
REAL(ans)[2] = x[2];
REAL(ans)[3] = minf;
SET_STRING_ELT(ansNames, 0, mkChar("PAM"));
SET_STRING_ELT(ansNames, 1, mkChar("Mu"));
SET_STRING_ELT(ansNames, 2, mkChar("r"));
SET_STRING_ELT(ansNames, 3, mkChar("negLogLikelihood"));
SET_NAMES(ans, ansNames);
// free everything
nlopt_destroy(opt);
gsl_vector_free(eqFrequencies);
gsl_matrix_free(probMat);
UNPROTECT(2);
return ans;
}
double Optimize_Torsion_Parameters(void)
{
double lb[N_TOR_PARA] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -100.0 }; /* lower bounds */
double ub[N_TOR_PARA] = { 12.0, 12.0, 12.0, 12.0, 12.0, 12.0, 12.0, 12.0, 12.0, 12.0, 100.0 }; /* lower bounds */
// double ub[N_TOR_PARA] = { 8.0, 8.0, 8.0, 8.0, 8.0, 8.0, 8.0, 8.0, 8.0, 8.0, 100.0 }; /* lower bounds */
double Phase[32][5]={{0, 0, 0, 0, 0}, {0, 0, 0, 0, PI}, {0, 0, 0, PI, 0}, {0, 0, 0, PI, PI}, {0, 0, PI, 0, 0}, {0, 0, PI, 0, PI}, {0, 0, PI, PI, 0}, {0, 0, PI, PI, PI}, {0, PI, 0, 0, 0}, {0, PI, 0, 0, PI},
{0, PI, 0, PI, 0}, {0, PI, 0, PI, PI}, {0, PI, PI, 0, 0}, {0, PI, PI, 0, PI}, {0, PI, PI, PI, 0}, {0, PI, PI, PI, PI}, {PI, 0, 0, 0, 0}, {PI, 0, 0, 0, PI}, {PI, 0, 0, PI, 0}, {PI, 0, 0, PI, PI},
{PI, 0, PI, 0, 0}, {PI, 0, PI, 0, PI}, {PI, 0, PI, PI, 0}, {PI, 0, PI, PI, PI}, {PI, PI, 0, 0, 0}, {PI, PI, 0, 0, PI}, {PI, PI, 0, PI, 0}, {PI, PI, 0, PI, PI}, {PI, PI, PI, 0, 0}, {PI, PI, PI, 0, PI},
{PI, PI, PI, PI, 0}, {PI, PI, PI, PI, PI}};
nlopt_opt opt;
double Chi_SQ;
int i, j;
Chi_SQ_Min = 1.0E100;
iseed = time(NULL);
for(i=0; i<32; i++) {
ActiveRun = i;
Chi_SQ_Min_Local[i] = 1.0E200;
FailCount = Iteration = 0;
// for(j=0; j<5; j++) {
// Para_Tor[2*j] = 0.1+1.0*rand(iseed);
// }
Para_Tor[0] = Para_Tor[2] = Para_Tor[4] = Para_Tor[6] = Para_Tor[8] = 1.0;
for(j=0; j<5; j++) {
Para_Tor[2*j+1] = Phase[i][j];
lb[2*j+1] = Para_Tor[2*j+1];
ub[2*j+1] = Para_Tor[2*j+1];
}
// Para_Tor[1] = Para_Tor[3] = Para_Tor[5] = Para_Tor[7] = Para_Tor[9] = 0.0;
Para_Tor[10] = 0.0;
IsPara_Fixed[1] = IsPara_Fixed[3] = IsPara_Fixed[5] = IsPara_Fixed[7] = IsPara_Fixed[9] = 1;
//for test
// IsPara_Fixed[6] = 1; Para_Tor[6] = 0.0;
opt = nlopt_create(NLOPT_LD_LBFGS, N_TOR_PARA);
nlopt_set_lower_bounds(opt, lb);
nlopt_set_upper_bounds(opt, ub);
nlopt_set_min_objective(opt, Callback_Eval_Gradient, NULL);
nlopt_set_xtol_rel(opt, 1e-8);
if (nlopt_optimize(opt, Para_Tor, &Chi_SQ) < 0) {
printf("nlopt didn't converge %lf %lf\n Please check carefully the rotamer results\n",Chi_SQ_Min, Chi_SQ);
}
else {
printf("Chi_SQ_min = %lf Chi_SQ = %lf\n", Chi_SQ_Min, Chi_SQ);
}
nlopt_destroy(opt);
}
memcpy(Para_Tor, Para_Best, sizeof(double)*N_TOR_PARA);
CalObjectiveFunction_Tor(Para_Tor);
return Chi_SQ_Min;
}
void
FitOrientation(
const int NrOfFiles,
const int nLayers,
const double ExcludePoleAngle,
double Lsd[nLayers],
const long long int SizeObsSpots,
const double XGrain[3],
const double YGrain[3],
double TiltsOrig[3],
const double OmegaStart,
const double OmegaStep,
const double px,
double ybc[nLayers],
double zbc[nLayers],
const double gs,
double OmegaRanges[MAX_N_OMEGA_RANGES][2],
const int NoOfOmegaRanges,
double BoxSizes[MAX_N_OMEGA_RANGES][4],
double P0[nLayers][3],
const int NrPixelsGrid,
int *ObsSpotsInfo,
double EulerIn[3],
double tol,
double *EulerOutA,
double *EulerOutB,
double *EulerOutC,
double *ResultFracOverlap,
double hkls[5000][4],
double Thetas[5000],
int n_hkls,
double *LsdFit,
double *TiltsFit,
double **BCsFit,
double tolLsd,
double tolLsdRel,
double tolTilts,
double tolBCsa,
double tolBCsb)
{
unsigned n;
long int i,j;
n = 6+(nLayers*3);
double x[n],xl[n],xu[n];
for (i=0;i<n;i++){
x[i] = 0;
xl[i] = 0;
xu[i] = 0;
}
for( i=0; i<3; i++)
{
x[i] = EulerIn[i];
xl[i] = x[i] - (tol*M_PI/180);
xu[i] = x[i] + (tol*M_PI/180);
}
int count = 0;
for (i=3;i<6;i++)
{
x[i] = TiltsOrig[count];
xl[i] = x[i] - tolTilts;
xu[i] = x[i] + tolTilts;
count++;
}
count = 0;
x[6] = Lsd[0];
xl[6] = x[6] - tolLsd;
xu[6] = x[6] + tolLsd;
count++;
for (i=7;i<nLayers+6;i++)
{
x[i] = Lsd[count] - Lsd[count-1];
xl[i] = x[i] - tolLsdRel;
xu[i] = x[i] + tolLsdRel;
count++;
}
count = 0;
for (i=6+nLayers;i<6+(nLayers*2);i++)
{
x[i] = ybc[count];
x[i+nLayers] = zbc[count];
xl[i] = x[i] - tolBCsa;
xl[i+nLayers] = x[i+nLayers] - tolBCsb;
xu[i] = x[i] + tolBCsa;
xu[i+nLayers] = x[i+nLayers] + tolBCsb;
count++;
}
struct my_func_data f_data;
f_data.NrOfFiles = NrOfFiles;
f_data.nLayers = nLayers;
f_data.n_hkls = n_hkls;
for (i=0;i<5000;i++){
f_data.hkls[i][0] = hkls[i][0];
f_data.hkls[i][1] = hkls[i][1];
f_data.hkls[i][2] = hkls[i][2];
f_data.hkls[i][3] = hkls[i][3];
f_data.Thetas[i] = Thetas[i];
}
f_data.ExcludePoleAngle = ExcludePoleAngle;
f_data.SizeObsSpots = SizeObsSpots;
f_data.P0 = allocMatrixF(nLayers,3);
for (i=0;i<3;i++){
f_data.XGrain[i] = XGrain[i];
f_data.YGrain[i] = YGrain[i];
for (j=0;j<nLayers;j++){
f_data.P0[j][i] = P0[j][i];
}
}
for (i=0;i<MAX_N_OMEGA_RANGES;i++){
for (j=0;j<2;j++){
f_data.OmegaRanges[i][j] = OmegaRanges[i][j];
}
for (j=0;j<4;j++){
f_data.BoxSizes[i][j] = BoxSizes[i][j];
}
}
f_data.ObsSpotsInfo = &ObsSpotsInfo[0];
f_data.OmegaStart = OmegaStart;
f_data.OmegaStep = OmegaStep;
f_data.px = px;
f_data.gs = gs;
f_data.NoOfOmegaRanges = NoOfOmegaRanges;
f_data.NrPixelsGrid = NrPixelsGrid;
struct my_func_data *f_datat;
f_datat = &f_data;
void* trp = (struct my_func_data *) f_datat;
double tole = 1e-3;
nlopt_opt opt;
opt = nlopt_create(NLOPT_LN_NELDERMEAD, n);
nlopt_set_lower_bounds(opt, xl);
nlopt_set_upper_bounds(opt, xu);
nlopt_set_min_objective(opt, problem_function, trp);
double minf=1;
nlopt_optimize(opt, x, &minf);
nlopt_destroy(opt);
*ResultFracOverlap = minf;
*EulerOutA = x[0];
*EulerOutB = x[1];
*EulerOutC = x[2];
TiltsFit[0] = x[3];
TiltsFit[1] = x[4];
TiltsFit[2] = x[5];
LsdFit[0] = x[6];
for (i=1;i<nLayers;i++){
LsdFit[i] = LsdFit[i-1] + x[6+i];
}
for (i=0;i<nLayers;i++){
BCsFit[i][0] = x[6+nLayers+i];
BCsFit[i][1] = x[6+nLayers+nLayers+i];
}
}
int main(int argc, char *argv[])
{
/* default values */
int NHITS = 10;
double XTOL = .1;
int c;
while ((c = getopt(argc, argv, "N:t:")) != -1)
switch (c) {
case 'N':
NHITS = atoi(optarg);
break;
case 't':
XTOL = atof(optarg);
break;
case '?':
if (optopt == 'N' || optopt == 't')
fprintf(stderr, "Option -%c requires argument.\n", optopt);
else
fprintf(stderr, "unknown option -%c\n", optopt);
return 1;
default:
;
}
/* initialize random number resources */
init_random();
/* generate event */
struct event e1;
make_event(&e1, NHITS);
sort_event(&e1);
nlopt_opt opt;
opt = nlopt_create(NLOPT_GN_ISRES, 4);
nlopt_result ret;
double lb[4] = {-6, -6, -6, -10*scint_time};
double ub[4] = {6, 6, 6, e1.hits[0].hit_time};
nlopt_set_lower_bounds(opt, lb);
nlopt_set_upper_bounds(opt, ub);
struct pos_data data;
data.p = NULL; /* no pmtmap required for timing fit */
data.e = &e1;
nlopt_set_min_objective(opt, mf_t, &data);
// nlopt_set_maxtime(opt, .5);
double tols[4] = {XTOL, XTOL, XTOL, XTOL/light_speed};
nlopt_set_xtol_abs(opt, tols);
ret = nlopt_add_inequality_constraint(opt, radius_check, &data, 1e-10);
ret = nlopt_add_inequality_constraint(opt, time_check, &data, 1e-15);
double x[4];
x[0] = x[1] = x[2] = x[3] = 0;
double fval = mf_t(4, x, NULL, &data);
ret = nlopt_optimize(opt, x, &fval);
printf("actual location: \n");
printf("(x0, y0, z0, t0) = (%g, %g, %g, %g)\n",
e1.spawn_pos[0], e1.spawn_pos[1],
e1.spawn_pos[2], e1.spawn_time);
printf("fitted to:\n");
printf("(x0, y0, z0, t0) = (%g, %g, %g, %g)\n",
x[0], x[1], x[2], x[3]);
printf("with log-likelihood %g\n", fval);
printf("and return value %d\n", ret);
printf("log-likelihood at actual value is %g\n",
mf_t(4, e1.spawn_pos, NULL, &data));
free_random();
return 0;
}
int main(int argc, char **argv)
{
/* -------Initialize and Get the parameters from command line ------*/
PetscInitialize(&argc, &argv, PETSC_NULL, PETSC_NULL);
PetscPrintf(PETSC_COMM_WORLD,"--------Initializing------ \n");
PetscErrorCode ierr;
PetscBool flg;
int myrank;
MPI_Comm_rank(MPI_COMM_WORLD,&myrank);
if(myrank==0)
mma_verbose=1;
/*-------------------------------------------------*/
int Mx,My,Mz,Mzslab, Npmlx,Npmly,Npmlz,DegFree, anisotropic;
PetscOptionsGetInt(PETSC_NULL,"-Nx",&Nx,&flg); MyCheckAndOutputInt(flg,Nx,"Nx","Nx");
PetscOptionsGetInt(PETSC_NULL,"-Ny",&Ny,&flg); MyCheckAndOutputInt(flg,Ny,"Ny","Nx");
PetscOptionsGetInt(PETSC_NULL,"-Nz",&Nz,&flg); MyCheckAndOutputInt(flg,Nz,"Nz","Nz");
PetscOptionsGetInt(PETSC_NULL,"-Mx",&Mx,&flg); MyCheckAndOutputInt(flg,Mx,"Mx","Mx");
PetscOptionsGetInt(PETSC_NULL,"-My",&My,&flg); MyCheckAndOutputInt(flg,My,"My","My");
PetscOptionsGetInt(PETSC_NULL,"-Mz",&Mz,&flg); MyCheckAndOutputInt(flg,Mz,"Mz","Mz");
PetscOptionsGetInt(PETSC_NULL,"-Mzslab",&Mzslab,&flg); MyCheckAndOutputInt(flg,Mzslab,"Mzslab","Mzslab");
PetscOptionsGetInt(PETSC_NULL,"-Npmlx",&Npmlx,&flg); MyCheckAndOutputInt(flg,Npmlx,"Npmlx","Npmlx");
PetscOptionsGetInt(PETSC_NULL,"-Npmly",&Npmly,&flg); MyCheckAndOutputInt(flg,Npmly,"Npmly","Npmly");
PetscOptionsGetInt(PETSC_NULL,"-Npmlz",&Npmlz,&flg); MyCheckAndOutputInt(flg,Npmlz,"Npmlz","Npmlz");
Nxyz = Nx*Ny*Nz;
// if anisotropic !=0, Degree of Freedom = 3*Mx*My*Mz; else DegFree = Mx*My*Mz;
PetscOptionsGetInt(PETSC_NULL,"-anisotropic",&anisotropic,&flg);
if(!flg) anisotropic = 0; // by default, it is isotropc.
DegFree = (anisotropic ? 3 : 1 )*Mx*My*((Mzslab==0)?Mz:1);
PetscPrintf(PETSC_COMM_WORLD," the Degree of Freedoms is %d \n ", DegFree);
int DegFreeAll=DegFree+1;
PetscPrintf(PETSC_COMM_WORLD," the Degree of Freedoms ALL is %d \n ", DegFreeAll);
int BCPeriod, Jdirection, Jdirectiontwo, LowerPML;
int bx[2], by[2], bz[2];
PetscOptionsGetInt(PETSC_NULL,"-BCPeriod",&BCPeriod,&flg); MyCheckAndOutputInt(flg,BCPeriod,"BCPeriod","BCPeriod given");
PetscOptionsGetInt(PETSC_NULL,"-Jdirection",&Jdirection,&flg); MyCheckAndOutputInt(flg,Jdirection,"Jdirection","Diapole current direction");
PetscOptionsGetInt(PETSC_NULL,"-Jdirectiontwo",&Jdirectiontwo,&flg); MyCheckAndOutputInt(flg,Jdirectiontwo,"Jdirectiontwo","Diapole current direction for source two");
PetscOptionsGetInt(PETSC_NULL,"-LowerPML",&LowerPML,&flg); MyCheckAndOutputInt(flg,LowerPML,"LowerPML","PML in the lower xyz boundary");
PetscOptionsGetInt(PETSC_NULL,"-bxl",bx,&flg); MyCheckAndOutputInt(flg,bx[0],"bxl","BC at x lower");
PetscOptionsGetInt(PETSC_NULL,"-bxu",bx+1,&flg); MyCheckAndOutputInt(flg,bx[1],"bxu","BC at x upper");
PetscOptionsGetInt(PETSC_NULL,"-byl",by,&flg); MyCheckAndOutputInt(flg,by[0],"byl","BC at y lower");
PetscOptionsGetInt(PETSC_NULL,"-byu",by+1,&flg); MyCheckAndOutputInt(flg,by[1],"byu","BC at y upper");
PetscOptionsGetInt(PETSC_NULL,"-bzl",bz,&flg); MyCheckAndOutputInt(flg,bz[0],"bzl","BC at z lower");
PetscOptionsGetInt(PETSC_NULL,"-bzu",bz+1,&flg); MyCheckAndOutputInt(flg,bz[1],"bzu","BC at z upper");
double epssub, RRT, sigmax, sigmay, sigmaz ;
PetscOptionsGetReal(PETSC_NULL,"-hx",&hx,&flg); MyCheckAndOutputDouble(flg,hx,"hx","hx");
hy = hx;
hz = hx;
hxyz = (Nz==1)*hx*hy + (Nz>1)*hx*hy*hz;
double omega, omegaone, omegatwo, wratio;
PetscOptionsGetReal(PETSC_NULL,"-omega",&omega,&flg); MyCheckAndOutputDouble(flg,omega,"omega","omega");
PetscOptionsGetReal(PETSC_NULL,"-wratio",&wratio,&flg); MyCheckAndOutputDouble(flg,wratio,"wratio","wratio");
omegaone=omega;
omegatwo=wratio*omega;
PetscPrintf(PETSC_COMM_WORLD,"---omegaone is %.16e and omegatwo is %.16e ---\n",omegaone, omegatwo);
PetscOptionsGetReal(PETSC_NULL,"-Qabs",&Qabs,&flg);
if (flg && Qabs>1e+15)
Qabs=1.0/0.0;
MyCheckAndOutputDouble(flg,Qabs,"Qabs","Qabs");
PetscOptionsGetReal(PETSC_NULL,"-epsair",&epsair,&flg); MyCheckAndOutputDouble(flg,epsair,"epsair","epsair");
PetscOptionsGetReal(PETSC_NULL,"-epssub",&epssub,&flg); MyCheckAndOutputDouble(flg,epssub,"epssub","epssub");
PetscOptionsGetReal(PETSC_NULL,"-RRT",&RRT,&flg); MyCheckAndOutputDouble(flg,RRT,"RRT","RRT given");
sigmax = pmlsigma(RRT,Npmlx*hx);
sigmay = pmlsigma(RRT,Npmly*hy);
sigmaz = pmlsigma(RRT,Npmlz*hz);
PetscPrintf(PETSC_COMM_WORLD,"----sigmax is %.12e \n",sigmax);
PetscPrintf(PETSC_COMM_WORLD,"----sigmay is %.12e \n",sigmay);
PetscPrintf(PETSC_COMM_WORLD,"----sigmaz is %.12e \n",sigmaz);
char initialdata[PETSC_MAX_PATH_LEN]; //filenameComm[PETSC_MAX_PATH_LEN];
PetscOptionsGetString(PETSC_NULL,"-initialdata",initialdata,PETSC_MAX_PATH_LEN,&flg); MyCheckAndOutputChar(flg,initialdata,"initialdata","Inputdata file");
PetscOptionsGetString(PETSC_NULL,"-filenameComm",filenameComm,PETSC_MAX_PATH_LEN,&flg); MyCheckAndOutputChar(flg,filenameComm,"filenameComm","Output filenameComm");
// add cx, cy, cz to indicate where the diapole current is;
int cx, cy, cz;
PetscOptionsGetInt(PETSC_NULL,"-cx",&cx,&flg);
if (!flg)
{cx=(LowerPML)*floor(Nx/2); PetscPrintf(PETSC_COMM_WORLD,"cx is %d by default \n",cx);}
else
{PetscPrintf(PETSC_COMM_WORLD,"the current poisiont cx is %d \n",cx);}
PetscOptionsGetInt(PETSC_NULL,"-cy",&cy,&flg);
if (!flg)
{cy=(LowerPML)*floor(Ny/2); PetscPrintf(PETSC_COMM_WORLD,"cy is %d by default \n",cy);}
else
{PetscPrintf(PETSC_COMM_WORLD,"the current poisiont cy is %d \n",cy);}
PetscOptionsGetInt(PETSC_NULL,"-cz",&cz,&flg);
if (!flg)
{cz=(LowerPML)*floor(Nz/2); PetscPrintf(PETSC_COMM_WORLD,"cz is %d by default \n",cz);}
else
{PetscPrintf(PETSC_COMM_WORLD,"the current poisiont cz is %d \n",cz);}
posj = (cx*Ny+ cy)*Nz + cz;
PetscPrintf(PETSC_COMM_WORLD,"the posj is %d \n. ", posj);
int fixpteps;
PetscOptionsGetInt(PETSC_NULL,"-fixpteps",&fixpteps,&flg); MyCheckAndOutputInt(flg,fixpteps,"fixpteps","fixpteps");
// Get minapproach;
PetscOptionsGetInt(PETSC_NULL,"-minapproach",&minapproach,&flg); MyCheckAndOutputInt(flg,minapproach,"minapproach","minapproach");
// Get withepsinldos;
PetscOptionsGetInt(PETSC_NULL,"-withepsinldos",&withepsinldos,&flg); MyCheckAndOutputInt(flg,withepsinldos,"withepsinldos","withepsinldos");
// Get outputbase;
PetscOptionsGetInt(PETSC_NULL,"-outputbase",&outputbase,&flg); MyCheckAndOutputInt(flg,outputbase,"outputbase","outputbase");
// Get cavityverbose;
PetscOptionsGetInt(PETSC_NULL,"-cavityverbose",&cavityverbose,&flg);
if(!flg) cavityverbose=0;
PetscPrintf(PETSC_COMM_WORLD,"the cavity verbose is set as %d \n", cavityverbose);
// Get refinedldos;
PetscOptionsGetInt(PETSC_NULL,"-refinedldos",&refinedldos,&flg);
if(!flg) refinedldos=0;
PetscPrintf(PETSC_COMM_WORLD,"the refinedldos is set as %d \n", refinedldos);
// Get cmpwrhs;
int cmpwrhs;
PetscOptionsGetInt(PETSC_NULL,"-cmpwrhs",&cmpwrhs,&flg);
if(!flg) cmpwrhs=0;
PetscPrintf(PETSC_COMM_WORLD,"the cmpwrhs is set as %d \n", cmpwrhs);
// Get lrzsqr;
PetscOptionsGetInt(PETSC_NULL,"-lrzsqr",&lrzsqr,&flg);
if(!flg) lrzsqr=0;
PetscPrintf(PETSC_COMM_WORLD,"the lrzsqr is set as %d \n", lrzsqr);
// Get newQdef;
PetscOptionsGetInt(PETSC_NULL,"-newQdef",&newQdef,&flg);
if(!flg) newQdef=0;
PetscPrintf(PETSC_COMM_WORLD,"the newQdef is set as %d \n", newQdef);
/*--------------------------------------------------------*/
/*--------------------------------------------------------*/
/*---------- Set the current source---------*/
//Mat D; //ImaginaryIMatrix;
ImagIMat(PETSC_COMM_WORLD, &D,6*Nxyz);
Vec J;
ierr = VecCreateMPI(PETSC_COMM_WORLD, PETSC_DECIDE, 6*Nxyz, &J);CHKERRQ(ierr);
ierr = PetscObjectSetName((PetscObject) J, "Source");CHKERRQ(ierr);
VecSet(J,0.0); //initialization;
if (Jdirection == 1)
SourceSingleSetX(PETSC_COMM_WORLD, J, Nx, Ny, Nz, cx, cy, cz,1.0/hxyz);
else if (Jdirection ==2)
SourceSingleSetY(PETSC_COMM_WORLD, J, Nx, Ny, Nz, cx, cy, cz,1.0/hxyz);
else if (Jdirection == 3)
SourceSingleSetZ(PETSC_COMM_WORLD, J, Nx, Ny, Nz, cx, cy, cz,1.0/hxyz);
else
PetscPrintf(PETSC_COMM_WORLD," Please specify correct direction of current: x (1) , y (2) or z (3)\n ");
Vec Jtwo;
ierr = VecDuplicate(J, &Jtwo);CHKERRQ(ierr);
ierr = PetscObjectSetName((PetscObject) Jtwo, "Sourcetwo");CHKERRQ(ierr);
VecSet(Jtwo,0.0); //initialization;
if (Jdirectiontwo == 1)
SourceSingleSetX(PETSC_COMM_WORLD, Jtwo, Nx, Ny, Nz, cx, cy, cz,1.0/hxyz);
else if (Jdirectiontwo ==2)
SourceSingleSetY(PETSC_COMM_WORLD, Jtwo, Nx, Ny, Nz, cx, cy, cz,1.0/hxyz);
else if (Jdirectiontwo == 3)
SourceSingleSetZ(PETSC_COMM_WORLD, Jtwo, Nx, Ny, Nz, cx, cy, cz,1.0/hxyz);
else
PetscPrintf(PETSC_COMM_WORLD," Please specify correct direction of current two: x (1) , y (2) or z (3)\n ");
//Vec b; // b= i*omega*J;
Vec bone, btwo;
ierr = VecDuplicate(J,&b);CHKERRQ(ierr);
ierr = PetscObjectSetName((PetscObject) b, "rhsone");CHKERRQ(ierr);
ierr = VecDuplicate(J,&bone);CHKERRQ(ierr);
ierr = PetscObjectSetName((PetscObject) bone, "rhsone");CHKERRQ(ierr);
ierr = VecDuplicate(Jtwo,&btwo);CHKERRQ(ierr);
ierr = PetscObjectSetName((PetscObject) btwo, "rhstwo");CHKERRQ(ierr);
if (cmpwrhs==0)
{
ierr = MatMult(D,J,b);CHKERRQ(ierr);
ierr = MatMult(D,Jtwo,btwo);CHKERRQ(ierr);
VecCopy(b,bone);
VecScale(bone,omegaone);
VecScale(btwo,omegatwo);
VecScale(b,omega);
}
else
{
double complex cmpiomega;
cmpiomega = cpow(1+I/Qabs,newQdef+1);
double sqrtiomegaR = -omega*cimag(csqrt(cmpiomega));
double sqrtiomegaI = omega*creal(csqrt(cmpiomega));
PetscPrintf(PETSC_COMM_WORLD,"the real part of sqrt cmpomega is %g and imag sqrt is % g ", sqrtiomegaR, sqrtiomegaI);
Vec tmpi;
ierr = VecDuplicate(J,&tmpi);
VecSet(b,0.0);
VecSet(tmpi,0.0);
CmpVecScale(J,b,sqrtiomegaR,sqrtiomegaI,D,tmpi);
VecDestroy(&tmpi);
}
/*-------Get the weight vector ------------------*/
//Vec weight;
ierr = VecDuplicate(J,&weight); CHKERRQ(ierr);
ierr = PetscObjectSetName((PetscObject) weight, "weight");CHKERRQ(ierr);
if(LowerPML==0)
GetWeightVec(weight, Nx, Ny,Nz); // new code handles both 3D and 2D;
else
VecSet(weight,1.0);
Vec weightedJ;
ierr = VecDuplicate(J,&weightedJ); CHKERRQ(ierr);
ierr = VecPointwiseMult(weightedJ,J,weight);
ierr = PetscObjectSetName((PetscObject) weightedJ, "weightedJ");CHKERRQ(ierr);
Vec weightedJtwo;
ierr = VecDuplicate(Jtwo,&weightedJtwo); CHKERRQ(ierr);
ierr = VecPointwiseMult(weightedJtwo,Jtwo,weight);
ierr = PetscObjectSetName((PetscObject) weightedJtwo, "weightedJtwo");CHKERRQ(ierr);
//Vec vR;
ierr = VecDuplicate(J,&vR); CHKERRQ(ierr);
GetRealPartVec(vR, 6*Nxyz);
// VecFReal;
if (lrzsqr)
{ ierr = VecDuplicate(J,&epsFReal); CHKERRQ(ierr);
ierr = PetscObjectSetName((PetscObject) epsFReal, "epsFReal");CHKERRQ(ierr);
if (newQdef==0)
{
sqrtomegaI = omega*cimag(csqrt(1+I/Qabs));
PetscPrintf(PETSC_COMM_WORLD,"the real part of sqrt cmpomega is %g and imag sqrt is % g ", omega*creal(csqrt(1+I/Qabs)), sqrtomegaI);
betar = 2*sqrtomegaI;
betai = betar/Qabs;
}
else
{
double gamma;
gamma = omega/Qabs;
betar = 2*gamma*(1-1.0/pow(Qabs,2));
betai = 2*gamma*(2.0/Qabs);
}
ierr = VecDuplicate(J,&nb); CHKERRQ(ierr);
ierr = PetscObjectSetName((PetscObject) nb, "nb"); CHKERRQ(ierr);
ierr = VecDuplicate(J,&y); CHKERRQ(ierr);
ierr = PetscObjectSetName((PetscObject) y, "y"); CHKERRQ(ierr);
ierr = VecDuplicate(J,&xsqr); CHKERRQ(ierr); // xsqr = x*x;
ierr = PetscObjectSetName((PetscObject) xsqr, "xsqr"); CHKERRQ(ierr);
CongMat(PETSC_COMM_WORLD, &C, 6*Nxyz);
}
/*----------- Define PML muinv vectors */
Vec muinvpml;
MuinvPMLFull(PETSC_COMM_SELF, &muinvpml,Nx,Ny,Nz,Npmlx,Npmly,Npmlz,sigmax,sigmay,sigmaz,omega, LowerPML);
//double *muinv;
muinv = (double *) malloc(sizeof(double)*6*Nxyz);
int add=0;
AddMuAbsorption(muinv,muinvpml,Qabs,add);
ierr = VecDestroy(&muinvpml); CHKERRQ(ierr);
/*---------- Define PML eps vectors: epspml---------- */
Vec epspml; //epspmlQ, epscoef;
ierr = VecDuplicate(J,&epspml);CHKERRQ(ierr);
ierr = PetscObjectSetName((PetscObject) epspml,"EpsPMLFull"); CHKERRQ(ierr);
EpsPMLFull(PETSC_COMM_WORLD, epspml,Nx,Ny,Nz,Npmlx,Npmly,Npmlz,sigmax,sigmay,sigmaz,omega, LowerPML);
ierr = VecDuplicate(J,&epspmlQ);CHKERRQ(ierr);
Vec epscoefone, epscoeftwo;
ierr = VecDuplicate(J,&epscoefone);CHKERRQ(ierr);
ierr = VecDuplicate(J,&epscoeftwo);CHKERRQ(ierr);
// compute epspmlQ,epscoef;
EpsCombine(D, weight, epspml, epspmlQ, epscoefone, Qabs, omegaone);
EpsCombine(D, weight, epspml, epspmlQ, epscoeftwo, Qabs, omegatwo);
/*--------- Setup the interp matrix ----------------------- */
/* for a samll eps block, interp it into yee-lattice. The interp matrix A and PML epspml only need to generated once;*/
//Mat A;
//new routine for myinterp;
myinterp(PETSC_COMM_WORLD, &A, Nx,Ny,Nz, LowerPML*floor((Nx-Mx)/2),LowerPML*floor((Ny-My)/2),LowerPML*floor((Nz-Mz)/2), Mx,My,Mz,Mzslab, anisotropic); // LoweerPML*Npmlx,..,.., specify where the interp starts;
//Vec epsSReal, epsgrad, vgrad; // create compatiable vectors with A.
ierr = MatGetVecs(A,&epsSReal, &epsgrad); CHKERRQ(ierr);
ierr = PetscObjectSetName((PetscObject) epsgrad, "epsgrad");CHKERRQ(ierr);
ierr = VecDuplicate(epsSReal, &vgrad); CHKERRQ(ierr);
ierr = PetscObjectSetName((PetscObject) epsSReal, "epsSReal");CHKERRQ(ierr);
ierr = PetscObjectSetName((PetscObject) vgrad, "vgrad");CHKERRQ(ierr);
/*---------Setup the epsmedium vector----------------*/
//Vec epsmedium;
ierr = VecDuplicate(J,&epsmedium); CHKERRQ(ierr);
GetMediumVec(epsmedium,Nz,Mz,epsair,epssub);
/*--------- Setup the finitie difference matrix-------------*/
//Mat M;
MoperatorGeneral(PETSC_COMM_WORLD, &M, Nx,Ny,Nz,hx,hy,hz, bx, by, bz,muinv,BCPeriod);
free(muinv);
/*--------Setup the KSP variables ---------------*/
KSP kspone;
PC pcone;
ierr = KSPCreate(PETSC_COMM_WORLD,&kspone);CHKERRQ(ierr);
//ierr = KSPSetType(ksp, KSPPREONLY);CHKERRQ(ierr);
ierr = KSPSetType(kspone, KSPGMRES);CHKERRQ(ierr);
ierr = KSPGetPC(kspone,&pcone);CHKERRQ(ierr);
ierr = PCSetType(pcone,PCLU);CHKERRQ(ierr);
ierr = PCFactorSetMatSolverPackage(pcone,MATSOLVERPASTIX);CHKERRQ(ierr);
ierr = PCSetFromOptions(pcone);
int maxkspit = 20;
ierr = KSPSetTolerances(kspone,1e-14,PETSC_DEFAULT,PETSC_DEFAULT,maxkspit);CHKERRQ(ierr);
ierr = KSPSetFromOptions(kspone);CHKERRQ(ierr);
KSP ksptwo;
PC pctwo;
ierr = KSPCreate(PETSC_COMM_WORLD,&ksptwo);CHKERRQ(ierr);
//ierr = KSPSetType(ksp, KSPPREONLY);CHKERRQ(ierr);
ierr = KSPSetType(ksptwo, KSPGMRES);CHKERRQ(ierr);
ierr = KSPGetPC(ksptwo,&pctwo);CHKERRQ(ierr);
ierr = PCSetType(pctwo,PCLU);CHKERRQ(ierr);
ierr = PCFactorSetMatSolverPackage(pctwo,MATSOLVERPASTIX);CHKERRQ(ierr);
ierr = PCSetFromOptions(pctwo);
ierr = KSPSetTolerances(ksptwo,1e-14,PETSC_DEFAULT,PETSC_DEFAULT,maxkspit);CHKERRQ(ierr);
ierr = KSPSetFromOptions(ksptwo);CHKERRQ(ierr);
/*--------- Create the space for solution vector -------------*/
//Vec x;
ierr = VecDuplicate(J,&x);CHKERRQ(ierr);
ierr = PetscObjectSetName((PetscObject) x, "Solution");CHKERRQ(ierr);
/*----------- Create the space for final eps -------------*/
//Vec epsC, epsCi, epsP;
ierr = VecDuplicate(J,&epsC);CHKERRQ(ierr);
ierr = PetscObjectSetName((PetscObject) epsC, "EpsC");CHKERRQ(ierr);
ierr = VecDuplicate(J,&epsCi);CHKERRQ(ierr);
ierr = VecDuplicate(J,&epsP);CHKERRQ(ierr);
ierr = VecSet(epsP,0.0); CHKERRQ(ierr);
ierr = VecAssemblyBegin(epsP); CHKERRQ(ierr);
ierr = VecAssemblyEnd(epsP); CHKERRQ(ierr);
/*------------ Create space used in the solver ------------*/
//Vec vgradlocal,tmp, tmpa,tmpb;
ierr = VecCreateSeq(PETSC_COMM_SELF, DegFree, &vgradlocal); CHKERRQ(ierr);
ierr = VecDuplicate(J,&tmp); CHKERRQ(ierr);
ierr = VecDuplicate(J,&tmpa); CHKERRQ(ierr);
ierr = VecDuplicate(J,&tmpb); CHKERRQ(ierr);
// Vec pickposvec; this vector is zero except that first entry is one;
if (withepsinldos)
{ ierr = VecDuplicate(J,&pickposvec); CHKERRQ(ierr);
ierr = VecSet(pickposvec,0.0); CHKERRQ(ierr);
ierr = VecSetValue(pickposvec,posj+Jdirection*Nxyz,1.0,INSERT_VALUES);
VecAssemblyBegin(pickposvec);
VecAssemblyEnd(pickposvec);
}
/*------------ Create scatter used in the solver -----------*/
//VecScatter scatter;
//IS from, to;
ierr =ISCreateStride(PETSC_COMM_SELF,DegFree,0,1,&from); CHKERRQ(ierr);
ierr =ISCreateStride(PETSC_COMM_SELF,DegFree,0,1,&to); CHKERRQ(ierr);
/*-------------Read the input file -------------------------*/
double *epsoptAll;
epsoptAll = (double *) malloc(DegFreeAll*sizeof(double));
FILE *ptf;
ptf = fopen(initialdata,"r");
PetscPrintf(PETSC_COMM_WORLD,"reading from input files \n");
int i;
// set the dielectric at the center is fixed, and alwyas high
//epsopt[0]=myub; is defined below near lb and ub;
for (i=0;i<DegFree;i++)
{ //PetscPrintf(PETSC_COMM_WORLD,"current eps reading is %lf \n",epsopt[i]);
fscanf(ptf,"%lf",&epsoptAll[i]);
}
epsoptAll[DegFreeAll-1]=0; //initialize auxiliary variable;
fclose(ptf);
/*----declare these data types, althought they may not be used for job 2 -----------------*/
double mylb,myub, *lb=NULL, *ub=NULL;
int maxeval, maxtime, mynloptalg;
double maxf;
nlopt_opt opt;
nlopt_result result;
/*--------------------------------------------------------------*/
/*----Now based on Command Line, Do the corresponding job----*/
/*----------------------------------------------------------------*/
//int Job; set Job to be gloabl variables;
PetscOptionsGetInt(PETSC_NULL,"-Job",&Job,&flg); MyCheckAndOutputInt(flg,Job,"Job","The Job indicator you set");
int numofvar=(Job==1)*DegFreeAll + (Job==3);
/*-------- convert the epsopt array to epsSReal (if job!=optmization) --------*/
if (Job==2 || Job ==3)
{
// copy epsilon from file to epsSReal; (different from FindOpt.c, because epsilon is not degree-of-freedoms in computeQ.
// i) create a array to read file (done above in epsopt); ii) convert the array to epsSReal;
int ns, ne;
ierr = VecGetOwnershipRange(epsSReal,&ns,&ne);
for(i=ns;i<ne;i++)
{ ierr=VecSetValue(epsSReal,i,epsoptAll[i],INSERT_VALUES);
CHKERRQ(ierr); }
if(withepsinldos)
{ epsatinterest = epsoptAll[cx*Ny*Nz + cy*Nz + cz] + epsair;
PetscPrintf(PETSC_COMM_WORLD, " the relative permitivity at the point of current is %.16e \n ",epsatinterest);}
ierr = VecAssemblyBegin(epsSReal); CHKERRQ(ierr);
ierr = VecAssemblyEnd(epsSReal); CHKERRQ(ierr);
}
if (Job==1 || Job==3) // optimization bounds setup;
{
PetscOptionsGetInt(PETSC_NULL,"-maxeval",&maxeval,&flg); MyCheckAndOutputInt(flg,maxeval,"maxeval","max number of evaluation");
PetscOptionsGetInt(PETSC_NULL,"-maxtime",&maxtime,&flg); MyCheckAndOutputInt(flg,maxtime,"maxtime","max time of evaluation");
PetscOptionsGetInt(PETSC_NULL,"-mynloptalg",&mynloptalg,&flg); MyCheckAndOutputInt(flg,mynloptalg,"mynloptalg","The algorithm used ");
PetscOptionsGetReal(PETSC_NULL,"-mylb",&mylb,&flg); MyCheckAndOutputDouble(flg,mylb,"mylb","optimization lb");
PetscOptionsGetReal(PETSC_NULL,"-myub",&myub,&flg); MyCheckAndOutputDouble(flg,myub,"myub","optimization ub");
lb = (double *) malloc(numofvar*sizeof(double));
ub = (double *) malloc(numofvar*sizeof(double));
// the dielectric constant at center is fixed!
for(i=0;i<numofvar;i++)
{
lb[i] = mylb;
ub[i] = myub;
} //initial guess, lower bounds, upper bounds;
// set lower and upper bounds for auxiliary variable;
lb[numofvar-1]=0;
ub[numofvar-1]=1.0/0.0;
//fix the dielectric at the center to be high for topology optimization;
if (Job==1 && fixpteps==1)
{
epsoptAll[0]=myub;
lb[0]=myub;
ub[0]=myub;
}
opt = nlopt_create(mynloptalg, numofvar);
myfundatatypeshg data[2] = {{omegaone, bone, weightedJ, epscoefone,kspone},{omegatwo, btwo, weightedJtwo, epscoeftwo,ksptwo}};
nlopt_add_inequality_constraint(opt,ldosconstraint, &data[0], 1e-8);
nlopt_add_inequality_constraint(opt,ldosconstraint, &data[1], 1e-8);
nlopt_set_lower_bounds(opt,lb);
nlopt_set_upper_bounds(opt,ub);
nlopt_set_maxeval(opt,maxeval);
nlopt_set_maxtime(opt,maxtime);
/*add functionality to choose local optimizer; */
int mynloptlocalalg;
nlopt_opt local_opt;
PetscOptionsGetInt(PETSC_NULL,"-mynloptlocalalg",&mynloptlocalalg,&flg); MyCheckAndOutputInt(flg,mynloptlocalalg,"mynloptlocalalg","The local optimization algorithm used ");
if (mynloptlocalalg)
{
local_opt=nlopt_create(mynloptlocalalg,numofvar);
nlopt_set_ftol_rel(local_opt, 1e-14);
nlopt_set_maxeval(local_opt,100000);
nlopt_set_local_optimizer(opt,local_opt);
}
}
switch (Job)
{
case 1:
{
if (minapproach)
nlopt_set_min_objective(opt,maxminobjfun,NULL);// NULL: no data to be passed because of global variables;
else
nlopt_set_max_objective(opt,maxminobjfun,NULL);
result = nlopt_optimize(opt,epsoptAll,&maxf);
}
break;
case 2 : //AnalyzeStructure
{
int Linear, Eig, maxeigit;
PetscOptionsGetInt(PETSC_NULL,"-Linear",&Linear,&flg); MyCheckAndOutputInt(flg,Linear,"Linear","Linear solver indicator");
PetscOptionsGetInt(PETSC_NULL,"-Eig",&Eig,&flg); MyCheckAndOutputInt(flg,Eig,"Eig","Eig solver indicator");
PetscOptionsGetInt(PETSC_NULL,"-maxeigit",&maxeigit,&flg); MyCheckAndOutputInt(flg,maxeigit,"maxeigit","maximum number of Eig solver iterations is");
/*----------------------------------*/
//EigenSolver(Linear, Eig, maxeigit);
/*----------------------------------*/
OutputVec(PETSC_COMM_WORLD, weight,filenameComm, "weight.m");
}
break;
default:
PetscPrintf(PETSC_COMM_WORLD,"--------Interesting! You're doing nothing!--------\n ");
}
if(Job==1 || Job==3)
{
/* print the optimization parameters */
#if 0
double xrel, frel, fabs;
// double *xabs;
frel=nlopt_get_ftol_rel(opt);
fabs=nlopt_get_ftol_abs(opt);
xrel=nlopt_get_xtol_rel(opt);
PetscPrintf(PETSC_COMM_WORLD,"nlopt frel is %g \n",frel);
PetscPrintf(PETSC_COMM_WORLD,"nlopt fabs is %g \n",fabs);
PetscPrintf(PETSC_COMM_WORLD,"nlopt xrel is %g \n",xrel);
//nlopt_result nlopt_get_xtol_abs(const nlopt_opt opt, double *tol);
#endif
/*--------------*/
if (result < 0) {
PetscPrintf(PETSC_COMM_WORLD,"nlopt failed! \n", result);
}
else {
PetscPrintf(PETSC_COMM_WORLD,"found extremum %0.16e\n", minapproach?1.0/maxf:maxf);
}
PetscPrintf(PETSC_COMM_WORLD,"nlopt returned value is %d \n", result);
if(Job==1)
{ //OutputVec(PETSC_COMM_WORLD, epsopt,filenameComm, "epsopt.m");
//OutputVec(PETSC_COMM_WORLD, epsgrad,filenameComm, "epsgrad.m");
//OutputVec(PETSC_COMM_WORLD, vgrad,filenameComm, "vgrad.m");
//OutputVec(PETSC_COMM_WORLD, x,filenameComm, "x.m");
int rankA;
MPI_Comm_rank(PETSC_COMM_WORLD, &rankA);
if(rankA==0)
{
ptf = fopen(strcat(filenameComm,"epsopt.txt"),"w");
for (i=0;i<DegFree;i++)
fprintf(ptf,"%0.16e \n",epsoptAll[i]);
fclose(ptf);
PetscPrintf(PETSC_COMM_WORLD,"the t parameter is %.8e \n",epsoptAll[DegFreeAll-1]);
}
}
nlopt_destroy(opt);
}
ierr = PetscPrintf(PETSC_COMM_WORLD,"--------Done!--------\n ");CHKERRQ(ierr);
/*------------------------------------*/
/* ----------------------Destroy Vecs and Mats----------------------------*/
free(epsoptAll);
free(lb);
free(ub);
ierr = VecDestroy(&J); CHKERRQ(ierr);
ierr = VecDestroy(&b); CHKERRQ(ierr);
ierr = VecDestroy(&weight); CHKERRQ(ierr);
ierr = VecDestroy(&weightedJ); CHKERRQ(ierr);
ierr = VecDestroy(&vR); CHKERRQ(ierr);
ierr = VecDestroy(&epspml); CHKERRQ(ierr);
ierr = VecDestroy(&epspmlQ); CHKERRQ(ierr);
ierr = VecDestroy(&epsSReal); CHKERRQ(ierr);
ierr = VecDestroy(&epsgrad); CHKERRQ(ierr);
ierr = VecDestroy(&vgrad); CHKERRQ(ierr);
ierr = VecDestroy(&epsmedium); CHKERRQ(ierr);
ierr = VecDestroy(&epsC); CHKERRQ(ierr);
ierr = VecDestroy(&epsCi); CHKERRQ(ierr);
ierr = VecDestroy(&epsP); CHKERRQ(ierr);
ierr = VecDestroy(&x); CHKERRQ(ierr);
ierr = VecDestroy(&vgradlocal);CHKERRQ(ierr);
ierr = VecDestroy(&tmp); CHKERRQ(ierr);
ierr = VecDestroy(&tmpa); CHKERRQ(ierr);
ierr = VecDestroy(&tmpb); CHKERRQ(ierr);
ierr = MatDestroy(&A); CHKERRQ(ierr);
ierr = MatDestroy(&D); CHKERRQ(ierr);
ierr = MatDestroy(&M); CHKERRQ(ierr);
ierr = VecDestroy(&epscoefone); CHKERRQ(ierr);
ierr = VecDestroy(&epscoeftwo); CHKERRQ(ierr);
ierr = KSPDestroy(&kspone);CHKERRQ(ierr);
ierr = KSPDestroy(&ksptwo);CHKERRQ(ierr);
ISDestroy(&from);
ISDestroy(&to);
if (withepsinldos)
{ierr=VecDestroy(&pickposvec); CHKERRQ(ierr);}
if (lrzsqr)
{
ierr=VecDestroy(&epsFReal); CHKERRQ(ierr);
ierr=VecDestroy(&xsqr); CHKERRQ(ierr);
ierr=VecDestroy(&y); CHKERRQ(ierr);
ierr=VecDestroy(&nb); CHKERRQ(ierr);
ierr=MatDestroy(&C); CHKERRQ(ierr);
}
ierr = VecDestroy(&bone); CHKERRQ(ierr);
ierr = VecDestroy(&btwo); CHKERRQ(ierr);
ierr = VecDestroy(&Jtwo); CHKERRQ(ierr);
/*------------ finalize the program -------------*/
{
int rank;
MPI_Comm_rank(PETSC_COMM_WORLD, &rank);
//if (rank == 0) fgetc(stdin);
MPI_Barrier(PETSC_COMM_WORLD);
}
ierr = PetscFinalize(); CHKERRQ(ierr);
return 0;
}
double Optimize_Torsion_Parameters(void)
{
double lb[N_PARA_MAX], ub[N_PARA_MAX];
double Chi_SQ;
int i, j, Pos;
n_Para = 10*n_Phi + 1;
memset(Is_Para_Fixed, 0, sizeof(int)*n_Para);
// memset(lb, 0, sizeof(double)*n_Para);
lb[n_Phi*10] = -HUGE_VAL;
for(i=0; i<n_Phi; i++) {
Pos = 10*i;
for(j=0; j<10; j+=2) { // ten parameters
lb[Pos+j] = -15.0;
ub[Pos+j] = 15.0;
}
for(j=1; j<10; j+=2) { // ten parameters
lb[Pos+j] = -M_PI;
ub[Pos+j] = +M_PI;
}
}
ub[n_Phi*10] = HUGE_VAL;
opt = nlopt_create(NLOPT_LN_COBYLA, n_Para);
nlopt_set_lower_bounds(opt, lb);
nlopt_set_upper_bounds(opt, ub);
nlopt_set_min_objective(opt, Callback_Eval_Gradient, NULL);
nlopt_set_xtol_rel(opt, 1e-6);
Collect_Torsion_Parameters();
Assign_Torsion_Parameters();
Cal_E_MM_Scaned();
Cal_E_MM_Rotamer();
Para_List[n_Phi*10] = E_Scan_QM_Mean-E_Scan_MM_Mean; // energy shift
E_Shift_0 = fabs(E_Scan_QM_Mean-E_Scan_MM_Mean);
for(i=0; i<n_Phi; i++) {
Pos = 10*i;
for(j=1; j<10; j+=2) { // the phase is fixed
Is_Para_Fixed[Pos+j] = 1;
}
}
Read_Parameters("saved-para.dat");
Chi_SQ_Min = 1.0E100;
memcpy(Para_Best, Para_List, sizeof(double)*n_Para);
Chi_SQ = CalObjectiveFunction(Para_List, 0);
if(ProgID == 0) {
fprintf(fFitting, "Iteration %4d Chi^2 = %.8lf Chi^2(1D) = %.8lf Chi^2(rotamer) = %.8lf\n",
0, Chi_SQ, Chi_SQ_1D, Chi_SQ_Rotamer);
fflush(fFitting);
}
FailCount = 0;
int ret= nlopt_optimize(opt, Para_List, &Chi_SQ);
if (ret < 0) {
printf("nlopt failed! :%d\n", ret);
}
else {
printf("Chi_SQ_min = %lf Chi_SQ = %lf\n", Chi_SQ_Min, Chi_SQ);
}
memcpy(Para_List, Para_Best, sizeof(double)*n_Para);
CalObjectiveFunction(Para_List, 1);
nlopt_destroy(opt);
return Chi_SQ_Min;
}
void
FitOrientation(
const int NrOfFiles,
const int nLayers,
const double ExcludePoleAngle,
double Lsd[nLayers],
const long long int SizeObsSpots,
const double XGrain[3],
const double YGrain[3],
double RotMatTilts[3][3],
const double OmegaStart,
const double OmegaStep,
const double px,
double ybc[nLayers],
double zbc[nLayers],
const double gs,
double OmegaRanges[MAX_N_OMEGA_RANGES][2],
const int NoOfOmegaRanges,
double BoxSizes[MAX_N_OMEGA_RANGES][4],
double P0[nLayers][3],
const int NrPixelsGrid,
int *ObsSpotsInfo,
double EulerIn[3],
double tol,
double *EulerOutA,
double *EulerOutB,
double *EulerOutC,
double *ResultFracOverlap,
double hkls[5000][4],
double Thetas[5000],
int n_hkls)
{
unsigned n;
long int i,j;
n = 3;
double x[n],xl[n],xu[n];
for( i=0; i<n; i++)
{
x[i] = EulerIn[i];
xl[i] = x[i] - (tol*M_PI/180);
xu[i] = x[i] + (tol*M_PI/180);
}
struct my_func_data f_data;
f_data.NrOfFiles = NrOfFiles;
f_data.nLayers = nLayers;
f_data.n_hkls = n_hkls;
for (i=0;i<5000;i++){
f_data.hkls[i][0] = hkls[i][0];
f_data.hkls[i][1] = hkls[i][1];
f_data.hkls[i][2] = hkls[i][2];
f_data.hkls[i][3] = hkls[i][3];
f_data.Thetas[i] = Thetas[i];
}
f_data.ExcludePoleAngle = ExcludePoleAngle;
f_data.SizeObsSpots = SizeObsSpots;
f_data.P0 = allocMatrixF(nLayers,3);
for (i=0;i<3;i++){
f_data.XGrain[i] = XGrain[i];
f_data.YGrain[i] = YGrain[i];
for (j=0;j<nLayers;j++){
f_data.P0[j][i] = P0[j][i];
}
for (j=0;j<3;j++){
f_data.RotMatTilts[i][j] = RotMatTilts[i][j];
}
}
for (i=0;i<MAX_N_OMEGA_RANGES;i++){
for (j=0;j<2;j++){
f_data.OmegaRanges[i][j] = OmegaRanges[i][j];
}
for (j=0;j<4;j++){
f_data.BoxSizes[i][j] = BoxSizes[i][j];
}
}
f_data.ObsSpotsInfo = &ObsSpotsInfo[0];
f_data.Lsd = &Lsd[0];
f_data.ybc = &ybc[0];
f_data.zbc = &zbc[0];
f_data.OmegaStart = OmegaStart;
f_data.OmegaStep = OmegaStep;
f_data.px = px;
f_data.gs = gs;
f_data.NoOfOmegaRanges = NoOfOmegaRanges;
f_data.NrPixelsGrid = NrPixelsGrid;
struct my_func_data *f_datat;
f_datat = &f_data;
void* trp = (struct my_func_data *) f_datat;
double tole = 1e-3;
nlopt_opt opt;
opt = nlopt_create(NLOPT_LN_NELDERMEAD, n);
nlopt_set_lower_bounds(opt, xl);
nlopt_set_upper_bounds(opt, xu);
nlopt_set_min_objective(opt, problem_function, trp);
double minf=1;
nlopt_optimize(opt, x, &minf);
nlopt_destroy(opt);
*ResultFracOverlap = minf;
*EulerOutA = x[0];
*EulerOutB = x[1];
*EulerOutC = x[2];
}
void NloptOptimizationSolver<T>::solve ()
{
START_LOG("solve()", "NloptOptimizationSolver");
this->init ();
unsigned int nlopt_size = this->system().solution->size();
// We have to have an objective function
libmesh_assert( this->objective_object );
// Set routine for objective and (optionally) gradient evaluation
{
nlopt_result ierr =
nlopt_set_min_objective(_opt,
__libmesh_nlopt_objective,
this);
if (ierr < 0)
libmesh_error_msg("NLopt failed to set min objective: " << ierr);
}
if (this->lower_and_upper_bounds_object)
{
// Need to actually compute the bounds vectors first
this->lower_and_upper_bounds_object->lower_and_upper_bounds(this->system());
std::vector<Real> nlopt_lb(nlopt_size);
std::vector<Real> nlopt_ub(nlopt_size);
for(unsigned int i=0; i<nlopt_size; i++)
{
nlopt_lb[i] = this->system().get_vector("lower_bounds")(i);
nlopt_ub[i] = this->system().get_vector("upper_bounds")(i);
}
nlopt_set_lower_bounds(_opt, &nlopt_lb[0]);
nlopt_set_upper_bounds(_opt, &nlopt_ub[0]);
}
// If we have an equality constraints object, tell NLopt about it.
if (this->equality_constraints_object)
{
// NLopt requires a vector to specify the tolerance for each constraint.
// NLopt makes a copy of this vector internally, so it's safe for us to
// let it go out of scope.
std::vector<double> equality_constraints_tolerances(this->system().C_eq->size(),
_constraints_tolerance);
// It would be nice to call the C interface directly, at least it should return an error
// code we could parse... unfortunately, there does not seem to be a way to extract
// the underlying nlopt_opt object from the nlopt::opt class!
nlopt_result ierr =
nlopt_add_equality_mconstraint(_opt,
equality_constraints_tolerances.size(),
__libmesh_nlopt_equality_constraints,
this,
&equality_constraints_tolerances[0]);
if (ierr < 0)
libmesh_error_msg("NLopt failed to add equality constraint: " << ierr);
}
// If we have an inequality constraints object, tell NLopt about it.
if (this->inequality_constraints_object)
{
// NLopt requires a vector to specify the tolerance for each constraint
std::vector<double> inequality_constraints_tolerances(this->system().C_ineq->size(),
_constraints_tolerance);
nlopt_add_inequality_mconstraint(_opt,
inequality_constraints_tolerances.size(),
__libmesh_nlopt_inequality_constraints,
this,
&inequality_constraints_tolerances[0]);
}
// Set a relative tolerance on the optimization parameters
nlopt_set_ftol_rel(_opt, this->objective_function_relative_tolerance);
// Set the maximum number of allowed objective function evaluations
nlopt_set_maxeval(_opt, this->max_objective_function_evaluations);
// Reset internal iteration counter
this->_iteration_count = 0;
// Perform the optimization
std::vector<Real> x(nlopt_size);
Real min_val = 0.;
_result = nlopt_optimize(_opt, &x[0], &min_val);
if (_result < 0)
libMesh::out << "NLopt failed!" << std::endl;
else
libMesh::out << "NLopt obtained optimal value: "
<< min_val
<< " in "
<< this->get_iteration_count()
<< " iterations."
<< std::endl;
STOP_LOG("solve()", "NloptOptimizationSolver");
}
int main(int argc, char *argv[])
{
double XTOL = -1; /* to be set by "user" */
int NHITS = -1;
int c;
while ((c = getopt(argc, argv, "N:t:")) != -1)
switch (c) {
case 'N':
NHITS = atoi(optarg);
break;
case 't':
XTOL = atof(optarg);
break;
case '?':
if (optopt == 'N' || optopt == 't')
fprintf(stderr, "Option -%c requires argument.\n", optopt);
else
fprintf(stderr, "unknown option -%c\n", optopt);
return 1;
default:
;
}
if (XTOL < 0 || NHITS < 0) {
fprintf(stderr,
"please specify NHITS and XTOL with -N and -t\n");
return 1;
}
/* geometry like SNO+'s (as close to 9,000 PMTS as possible) */
int NTHETA = 67;
int NPHI = 134;
struct pmtmap pmtmap;
pmtmap.N = NPHI*NTHETA;
pmtmap.nphi = NPHI;
pmtmap.ntheta = NTHETA;
struct event e1;
struct pos_data data;
data.p = &pmtmap;
data.e = &e1;
init_random();
init_pmtmap(&data);
make_event(&e1, NHITS);
fill_pmt_info(&data);
nlopt_opt opt;
opt = nlopt_create(NLOPT_GN_ISRES, 3);
double lb[3] = {-6, -6, -6};
double ub[3] = {6, 6, 6};
nlopt_set_lower_bounds(opt, lb);
nlopt_set_upper_bounds(opt, ub);
nlopt_set_min_objective(opt, mf_p, &data);
double tols[3] = {XTOL, XTOL, XTOL};
nlopt_set_xtol_abs(opt, tols);
nlopt_set_maxtime(opt, 10.0); /* unstick this */
nlopt_set_maxeval(opt, 4e5); /* if timing doesn't unstick it*/
nlopt_add_inequality_constraint(opt, radius_check, &data, 1e-10);
double x[3] = {0, 0, 0};
double fval = mf_p(3, x, NULL, &data);
nlopt_result ret;
ret = nlopt_optimize(opt, x, &fval);
if (ret > 0)
printf("%g %g %g\n", x[0] - e1.spawn_pos[0],
x[1] - e1.spawn_pos[1],
x[2] - e1.spawn_pos[2]);
else
fprintf(stderr, "optimizing failed with code %d\n", ret);
return 0;
}
nlopt_result auglag_minimize(int n, nlopt_func f, void *f_data,
int m, nlopt_constraint *fc,
int p, nlopt_constraint *h,
const double *lb, const double *ub, /* bounds */
double *x, /* in: initial guess, out: minimizer */
double *minf,
nlopt_stopping *stop,
nlopt_opt sub_opt, int sub_has_fc)
{
auglag_data d;
nlopt_result ret = NLOPT_SUCCESS;
double ICM = HUGE_VAL, minf_penalty = HUGE_VAL, penalty;
double *xcur = NULL, fcur;
int i, ii, feasible, minf_feasible = 0;
unsigned int k;
int auglag_iters = 0;
int max_constraint_dim;
/* magic parameters from Birgin & Martinez */
const double tau = 0.5, gam = 10;
const double lam_min = -1e20, lam_max = 1e20, mu_max = 1e20;
d.f = f; d.f_data = f_data;
d.m = m; d.fc = fc;
d.p = p; d.h = h;
d.stop = stop;
/* whether we handle inequality constraints via the augmented
Lagrangian penalty function, or directly in the sub-algorithm */
if (sub_has_fc)
d.m = 0;
else
m = 0;
max_constraint_dim = MAX(nlopt_max_constraint_dim(d.m, fc),
nlopt_max_constraint_dim(d.p, h));
d.mm = nlopt_count_constraints(d.m, fc);
d.pp = nlopt_count_constraints(d.p, h);
ret = nlopt_set_min_objective(sub_opt, auglag, &d); if (ret<0) return ret;
ret = nlopt_set_lower_bounds(sub_opt, lb); if (ret<0) return ret;
ret = nlopt_set_upper_bounds(sub_opt, ub); if (ret<0) return ret;
ret = nlopt_set_stopval(sub_opt,
d.m==0 && d.p==0 ? stop->minf_max : -HUGE_VAL);
if (ret<0) return ret;
ret = nlopt_remove_inequality_constraints(sub_opt); if (ret<0) return ret;
ret = nlopt_remove_equality_constraints(sub_opt); if (ret<0) return ret;
for (i = 0; i < m; ++i) {
if (fc[i].f)
ret = nlopt_add_inequality_constraint(sub_opt,
fc[i].f, fc[i].f_data,
fc[i].tol[0]);
else
ret = nlopt_add_inequality_mconstraint(sub_opt, fc[i].m,
fc[i].mf, fc[i].f_data,
fc[i].tol);
if (ret < 0) return ret;
}
xcur = (double *) malloc(sizeof(double) * (n
+ max_constraint_dim * (1 + n)
+ d.pp + d.mm));
if (!xcur) return NLOPT_OUT_OF_MEMORY;
memcpy(xcur, x, sizeof(double) * n);
d.restmp = xcur + n;
d.gradtmp = d.restmp + max_constraint_dim;
memset(d.gradtmp, 0, sizeof(double) * (n*max_constraint_dim + d.pp+d.mm));
d.lambda = d.gradtmp + n * max_constraint_dim;
d.mu = d.lambda + d.pp;
*minf = HUGE_VAL;
/* starting rho suggested by B & M */
if (d.p > 0 || d.m > 0) {
double con2 = 0;
++ *(d.stop->nevals_p);
fcur = f(n, xcur, NULL, f_data);
if (nlopt_stop_forced(stop)) {
ret = NLOPT_FORCED_STOP; goto done; }
penalty = 0;
feasible = 1;
for (i = 0; i < d.p; ++i) {
nlopt_eval_constraint(d.restmp, NULL, d.h + i, n, xcur);
if (nlopt_stop_forced(stop)) {
ret = NLOPT_FORCED_STOP; goto done; }
for (k = 0; k < d.h[i].m; ++k) {
double hi = d.restmp[k];
penalty += fabs(hi);
feasible = feasible && fabs(hi) <= h[i].tol[k];
con2 += hi * hi;
}
}
for (i = 0; i < d.m; ++i) {
nlopt_eval_constraint(d.restmp, NULL, d.fc + i, n, xcur);
if (nlopt_stop_forced(stop)) {
ret = NLOPT_FORCED_STOP; goto done; }
for (k = 0; k < d.fc[i].m; ++k) {
double fci = d.restmp[k];
penalty += fci > 0 ? fci : 0;
feasible = feasible && fci <= fc[i].tol[k];
if (fci > 0) con2 += fci * fci;
}
}
*minf = fcur;
minf_penalty = penalty;
minf_feasible = feasible;
d.rho = MAX(1e-6, MIN(10, 2 * fabs(*minf) / con2));
}
else
d.rho = 1; /* whatever, doesn't matter */
if (auglag_verbose) {
printf("auglag: initial rho=%g\nauglag initial lambda=", d.rho);
for (i = 0; i < d.pp; ++i) printf(" %g", d.lambda[i]);
printf("\nauglag initial mu = ");
for (i = 0; i < d.mm; ++i) printf(" %g", d.mu[i]);
printf("\n");
}
do {
double prev_ICM = ICM;
ret = nlopt_optimize_limited(sub_opt, xcur, &fcur,
stop->maxeval - *(stop->nevals_p),
stop->maxtime - (nlopt_seconds()
- stop->start));
if (auglag_verbose)
printf("auglag: subopt return code %d\n", ret);
if (ret < 0) break;
++ *(d.stop->nevals_p);
fcur = f(n, xcur, NULL, f_data);
if (nlopt_stop_forced(stop)) {
ret = NLOPT_FORCED_STOP; goto done; }
if (auglag_verbose)
printf("auglag: fcur = %g\n", fcur);
ICM = 0;
penalty = 0;
feasible = 1;
for (i = ii = 0; i < d.p; ++i) {
nlopt_eval_constraint(d.restmp, NULL, d.h + i, n, xcur);
if (nlopt_stop_forced(stop)) {
ret = NLOPT_FORCED_STOP; goto done; }
for (k = 0; k < d.h[i].m; ++k) {
double hi = d.restmp[k];
double newlam = d.lambda[ii] + d.rho * hi;
penalty += fabs(hi);
feasible = feasible && fabs(hi) <= h[i].tol[k];
ICM = MAX(ICM, fabs(hi));
d.lambda[ii++] = MIN(MAX(lam_min, newlam), lam_max);
}
}
for (i = ii = 0; i < d.m; ++i) {
nlopt_eval_constraint(d.restmp, NULL, d.fc + i, n, xcur);
if (nlopt_stop_forced(stop)) {
ret = NLOPT_FORCED_STOP; goto done; }
for (k = 0; k < d.fc[i].m; ++k) {
double fci = d.restmp[k];
double newmu = d.mu[ii] + d.rho * fci;
penalty += fci > 0 ? fci : 0;
feasible = feasible && fci <= fc[i].tol[k];
ICM = MAX(ICM, fabs(MAX(fci, -d.mu[ii] / d.rho)));
d.mu[ii++] = MIN(MAX(0.0, newmu), mu_max);
}
}
if (ICM > tau * prev_ICM) {
d.rho *= gam;
}
auglag_iters++;
if (auglag_verbose) {
printf("auglag %d: ICM=%g (%sfeasible), rho=%g\nauglag lambda=",
auglag_iters, ICM, feasible ? "" : "not ", d.rho);
for (i = 0; i < d.pp; ++i) printf(" %g", d.lambda[i]);
printf("\nauglag %d: mu = ", auglag_iters);
for (i = 0; i < d.mm; ++i) printf(" %g", d.mu[i]);
printf("\n");
}
if ((feasible && (!minf_feasible || penalty < minf_penalty
|| fcur < *minf)) ||
(!minf_feasible && penalty < minf_penalty)) {
ret = NLOPT_SUCCESS;
if (feasible) {
if (fcur < stop->minf_max)
ret = NLOPT_MINF_MAX_REACHED;
else if (nlopt_stop_ftol(stop, fcur, *minf))
ret = NLOPT_FTOL_REACHED;
else if (nlopt_stop_x(stop, xcur, x))
ret = NLOPT_XTOL_REACHED;
}
*minf = fcur;
minf_penalty = penalty;
minf_feasible = feasible;
memcpy(x, xcur, sizeof(double) * n);
if (ret != NLOPT_SUCCESS) break;
}
if (nlopt_stop_forced(stop)) {ret = NLOPT_FORCED_STOP; break;}
if (nlopt_stop_evals(stop)) {ret = NLOPT_MAXEVAL_REACHED; break;}
if (nlopt_stop_time(stop)) {ret = NLOPT_MAXTIME_REACHED; break;}
/* TODO: use some other stopping criterion on ICM? */
/* The paper uses ICM <= epsilon and DFM <= epsilon, where
DFM is a measure of the size of the Lagrangian gradient.
Besides the fact that these kinds of absolute tolerances
(non-scale-invariant) are unsatisfying and it is not
clear how the user should specify it, the ICM <= epsilon
condition seems not too different from requiring feasibility,
especially now that the user can provide constraint-specific
tolerances analogous to epsilon. */
if (ICM == 0) {ret = NLOPT_FTOL_REACHED; break;}
} while (1);
done:
free(xcur);
return ret;
}
int CNLopt::run(double (*error_func)(unsigned int nParams, const double* params, double* grad, void* misc))
{
// Create a member function pointer
unsigned int n_data = mWorkerThread->GetDataSize();
mOpt = nlopt_create(mAlgorithm, mNParams);
// xopt = new double [mNParams];
lb = new double [mNParams];
ub = new double [mNParams];
// Copy out the initial values for the parameters:
CModelListPtr model_list = mWorkerThread->GetModelList();
model_list->GetFreeParameters(mParams, mNParams, false);
vector<string> names = model_list->GetFreeParamNames();
vector< pair<double, double> > min_max = model_list->GetFreeParamMinMaxes();
// Init parameter values
for(int i = 0; i < mNParams; i++)
{
lb[i] = min_max[i].first;
ub[i] = min_max[i].second;
// xopt = mParams[i];
}
nlopt_set_lower_bounds(mOpt,lb);
nlopt_set_upper_bounds(mOpt,ub);
nlopt_set_ftol_rel(mOpt, 1e-4); // stopping criterion = when chi2 changes by less than 0.1%
nlopt_set_min_objective(mOpt, error_func, (void*)this);
int major=0, minor=0, bugfix=0;
nlopt_version(&major, &minor, &bugfix);
printf("Starting NLopt version %d.%d.%d\n", major, minor, bugfix);
printf("Algorithm = %s \n", nlopt_algorithm_name(mAlgorithm));
if(mAlgorithm == NLOPT_G_MLSL_LDS) // set local optimizer to be used with a global search
{
printf("Secondary Algorithm = %s \n", nlopt_algorithm_name(mAlgorithmSecondary));
mOptSecondary = nlopt_create(mAlgorithmSecondary, mNParams);
nlopt_set_lower_bounds(mOptSecondary,lb);
nlopt_set_upper_bounds(mOptSecondary,ub);
nlopt_set_ftol_rel(mOptSecondary, 1e-4); // stopping criterion = when chi2 changes by less than 0.1%
nlopt_set_min_objective(mOptSecondary, error_func, (void*)this);
nlopt_set_local_optimizer(mOpt, mOptSecondary);
}
mIsRunning = true;
mEvals = 0;
double minf;
// Call NLopt. Note, the results are saved in mParams upon completion.
nlopt_result result = nlopt_optimize(mOpt, mParams, &minf);
mIsRunning = false;
printresult(mParams, mNParams, n_data, names, minf, result);
delete(lb);
delete(ub);
nlopt_destroy(mOpt);
if(mAlgorithm == NLOPT_G_MLSL_LDS) // also destroy local optimizer
{
nlopt_destroy(mOptSecondary);
}
return mEvals;
}
void Fit2DPeaks(unsigned nPeaks, int NrPixelsThisRegion, double *z, int **UsefulPixels, double *MaximaValues,
int **MaximaPositions, double *IntegratedIntensity, double *IMAX, double *YCEN, double *ZCEN,
double *RCens, double *EtaCens,double Ycen, double Zcen, double Thresh, int *NrPx,double *OtherInfo)
{
unsigned n = 1 + (8*nPeaks);
double x[n],xl[n],xu[n];
x[0] = Thresh/2;
xl[0] = 0;
xu[0] = Thresh;
int i;
double *Rs, *Etas;
Rs = malloc(NrPixelsThisRegion*2*sizeof(*Rs));
Etas = malloc(NrPixelsThisRegion*2*sizeof(*Etas));
for (i=0;i<NrPixelsThisRegion;i++){
Rs[i] = CalcNorm2(UsefulPixels[i][0]-Ycen,UsefulPixels[i][1]-Zcen);
Etas[i] = CalcEtaAngle(UsefulPixels[i][0]-Ycen,UsefulPixels[i][1]-Zcen);
}
double Width = sqrt(NrPixelsThisRegion/nPeaks);
for (i=0;i<nPeaks;i++){
x[(8*i)+1] = MaximaValues[i]; // Imax
x[(8*i)+2] = CalcNorm2(MaximaPositions[i][0]-Ycen,MaximaPositions[i][1]-Zcen); //Radius
x[(8*i)+3] = CalcEtaAngle(MaximaPositions[i][0]-Ycen,MaximaPositions[i][1]-Zcen); // Eta
x[(8*i)+4] = 0.5; // Mu
x[(8*i)+5] = Width; //SigmaGR
x[(8*i)+6] = Width; //SigmaLR
x[(8*i)+7] = atand(Width/x[(8*i)+2]); //SigmaGEta //0.5;
x[(8*i)+8] = atand(Width/x[(8*i)+2]); //SigmaLEta //0.5;
double dEta = rad2deg*atan(1/x[(8*i)+2]);
xl[(8*i)+1] = MaximaValues[i]/2;
xl[(8*i)+2] = x[(8*i)+2] - 1;
xl[(8*i)+3] = x[(8*i)+3] - dEta;
xl[(8*i)+4] = 0;
xl[(8*i)+5] = 0.01;
xl[(8*i)+6] = 0.01;
xl[(8*i)+7] = 0.005;
xl[(8*i)+8] = 0.005;
xu[(8*i)+1] = MaximaValues[i]*2;
xu[(8*i)+2] = x[(8*i)+2] + 1;
xu[(8*i)+3] = x[(8*i)+3] + dEta;
xu[(8*i)+4] = 1;
xu[(8*i)+5] = 30;
xu[(8*i)+6] = 30;
xu[(8*i)+7] = 2;
xu[(8*i)+8] = 2;
}
struct func_data f_data;
f_data.NrPixels = NrPixelsThisRegion;
f_data.Rs = &Rs[0];
f_data.Etas = &Etas[0];
f_data.z = &z[0];
struct func_data *f_datat;
f_datat = &f_data;
void *trp = (struct func_data *) f_datat;
nlopt_opt opt;
opt = nlopt_create(NLOPT_LN_NELDERMEAD, n);
nlopt_set_lower_bounds(opt, xl);
nlopt_set_upper_bounds(opt, xu);
nlopt_set_maxtime(opt, 300);
nlopt_set_min_objective(opt, problem_function, trp);
double minf;
nlopt_optimize(opt, x, &minf);
nlopt_destroy(opt);
for (i=0;i<nPeaks;i++){
IMAX[i] = x[(8*i)+1];
RCens[i] = x[(8*i)+2];
EtaCens[i] = x[(8*i)+3];
if (x[(8*i)+5] > x[(8*i)+6]){
OtherInfo[2*i] = x[(8*i)+5];
}else{
OtherInfo[2*i] = x[(8*i)+6];
}
if (x[(8*i)+7] > x[(8*i)+8]){
OtherInfo[2*i+1] = x[(8*i)+7];
}else{
OtherInfo[2*i+1] = x[(8*i)+8];
}
}
YZ4mREta(nPeaks,RCens,EtaCens,YCEN,ZCEN);
CalcIntegratedIntensity(nPeaks,x,Rs,Etas,NrPixelsThisRegion,IntegratedIntensity,NrPx);
free(Rs);
free(Etas);
}
