bool make_statistic::run(viennamesh::algorithm_handle &)
{
info(1) << "make_statistic started:" << std::endl;
mesh_handle input_mesh = get_required_input<mesh_handle>("mesh");
/*original mesh for comparison statistics (hausdorff distance, curvature difference...)*/
mesh_handle original_mesh = get_input<mesh_handle>("original_mesh");
/*All provided cell quality metric types are implemented as header files, which are located in statistics/metrics.
* Note that most metrics are only implemented for triangles*/
data_handle<viennamesh_string> metric_type = get_required_input<viennamesh_string>("metric_type");
/*Decision threshold for triangle qualitity classification
* E.g., radius_ratio < 1.5
* Whether '<' or '>' is used for comparison is deduced from metric_ordering_tag*/
data_handle<viennagrid_numeric> good_element_threshold = get_input<viennagrid_numeric>("good_element_threshold");
/* comprehensive mesh quality metric is defined as
alpha * (1 - good_elements_counted/count) + beta * min_dist_rms + gamma * mean_curvature + delta * volume_deviation;
*/
data_handle<viennagrid_numeric> alpha = get_input<viennagrid_numeric>("alpha");
data_handle<viennagrid_numeric> beta = get_input<viennagrid_numeric>("beta");
data_handle<viennagrid_numeric> gamma = get_input<viennagrid_numeric>("gamma");
data_handle<viennagrid_numeric> delta = get_input<viennagrid_numeric>("delta");
#if HIST
data_handle<viennagrid_numeric> histogram_bins = get_input<viennagrid_numeric>("histogram_bin");
data_handle<viennagrid_numeric> histogram_min = get_input<viennagrid_numeric>("histogram_min");
data_handle<viennagrid_numeric> histogram_max = get_input<viennagrid_numeric>("histogram_max");
data_handle<int> histogram_bin_count = get_input<int>("histogram_bin_count");
#endif
typedef viennagrid::mesh MeshType;
typedef viennagrid::result_of::element<MeshType>::type ElementType; //=Triangle or Tetrahedron
typedef viennamesh::statistic<viennagrid_numeric> StatisticType;
StatisticType statistic;
#if HIST
typedef StatisticType::histogram_type HistogramType;
if (histogram_bins.valid())
{
std::vector<viennagrid_numeric> bins;
for (int i = 0; i != histogram_bins.size(); ++i)
bins.push_back( histogram_bins(i) );
statistic.set_histogram( StatisticType::histogram_type::make(bins.begin(), bins.end()) );
}
else if (histogram_min.valid() && histogram_max.valid() && histogram_bin_count.valid())
{
statistic.set_histogram( StatisticType::histogram_type::make_uniform(histogram_min(), histogram_max(), histogram_bin_count()) );
}
else
{
//If histograms are about to be added again, returing false here will prevent the basic statistics features from working!
error(1) << "No histogram configuration provided" << std::endl;
return false;
}
#endif
//Good element threshold input was set, call statistics with the appropriate metric
if(good_element_threshold.valid())
{
viennamesh::LoggingStack stack( std::string("High quality cell counter with metric type \"") + metric_type() + "\"" );
if (metric_type() == "aspect_ratio")
statistic.cell_quality_count<viennamesh::aspect_ratio_tag>( input_mesh(), viennamesh::aspect_ratio<ElementType>, good_element_threshold());
else if (metric_type() == "min_angle")
statistic.cell_quality_count<viennamesh::min_angle_tag>( input_mesh(), viennamesh::min_angle<ElementType>, good_element_threshold());
else if (metric_type() == "max_angle")
statistic.cell_quality_count<viennamesh::max_angle_tag>( input_mesh(), viennamesh::max_angle<ElementType>, good_element_threshold());
else if (metric_type() == "min_dihedral_angle")
statistic.cell_quality_count<viennamesh::min_dihedral_angle_tag>( input_mesh(), viennamesh::min_dihedral_angle<ElementType>, good_element_threshold());
else if (metric_type() == "radius_edge_ratio")
statistic.cell_quality_count<viennamesh::radius_edge_ratio_tag>( input_mesh(), viennamesh::radius_edge_ratio<ElementType>, good_element_threshold());
else if (metric_type() == "radius_ratio")
statistic.cell_quality_count<viennamesh::radius_ratio_tag>( input_mesh(), viennamesh::radius_ratio<ElementType>, good_element_threshold());
else if (metric_type() == "perimeter_inradius_ratio")
statistic.cell_quality_count<viennamesh::perimeter_inradius_ratio_tag>( input_mesh(), viennamesh::perimeter_inradius_ratio<ElementType>, good_element_threshold());
else if (metric_type() == "edge_ratio")
statistic.cell_quality_count<viennamesh::edge_ratio_tag>( input_mesh(), viennamesh::edge_ratio<ElementType>, good_element_threshold());
else if (metric_type() == "circum_perimeter_ratio")
statistic.cell_quality_count<viennamesh::circum_perimeter_ratio_tag>( input_mesh(), viennamesh::circum_perimeter_ratio<ElementType>, good_element_threshold());
else if (metric_type() == "stretch")
statistic.cell_quality_count<viennamesh::stretch_tag>( input_mesh(), viennamesh::stretch<ElementType>, good_element_threshold());
else if (metric_type() == "skewness")
statistic.cell_quality_count<viennamesh::skewness_tag>( input_mesh(), viennamesh::skewness<ElementType>, good_element_threshold());
else
{
error(1) << "Metric type \"" << metric_type() << "\" is not supported for cell quality classifaction" << std::endl;
return false;
}
}
else //no triangle classifiction takes place
{
viennamesh::LoggingStack stack( std::string("Cell statistics with metric type \"") + metric_type() + "\"" );
if (metric_type() == "aspect_ratio")
statistic.cell_stats( input_mesh(), viennamesh::aspect_ratio<ElementType> );
else if (metric_type() == "min_angle")
statistic.cell_stats( input_mesh(), viennamesh::min_angle<ElementType> );
else if (metric_type() == "max_angle")
statistic.cell_stats( input_mesh(), viennamesh::max_angle<ElementType> );
else if (metric_type() == "min_dihedral_angle")
statistic.cell_stats( input_mesh(), viennamesh::min_dihedral_angle<ElementType> );
else if (metric_type() == "radius_edge_ratio")
statistic.cell_stats( input_mesh(), viennamesh::radius_edge_ratio<ElementType> );
else if (metric_type() == "radius_ratio")
statistic.cell_stats( input_mesh(), viennamesh::radius_ratio<ElementType> );
else if (metric_type() == "perimeter_inradius_ratio")
statistic.cell_stats( input_mesh(), viennamesh::perimeter_inradius_ratio<ElementType> );
else if (metric_type() == "edge_ratio")
statistic.cell_stats( input_mesh(), viennamesh::edge_ratio<ElementType> );
else if (metric_type() == "circum_perimeter_ratio")
statistic.cell_stats( input_mesh(), viennamesh::circum_perimeter_ratio<ElementType> );
else if (metric_type() == "stretch")
statistic.cell_stats( input_mesh(), viennamesh::stretch<ElementType> );
else if (metric_type() == "skewness")
statistic.cell_stats( input_mesh(), viennamesh::skewness<ElementType> );
else
{
error(1) << "Metric type \"" << metric_type() << "\" is not supported" << std::endl;
return false;
}
}
if(original_mesh.valid())//a second mesh is set, calculate mesh comparison measures
{
viennamesh::LoggingStack stack( std::string("Calculation of Mesh Comparison Measures") );
statistic.mesh_comparison_quality(input_mesh(), original_mesh());
ConstTriangleRange tr_orig(original_mesh());
ConstTriangleRange tr(input_mesh());
StatisticType statistic_orig;
statistic_orig.cell_stats( original_mesh(), viennamesh::aspect_ratio<ElementType> );
//use median of orig mesh for input mesh triangle shape characterization
statistic.cell_quality_count<viennamesh::aspect_ratio_tag>( input_mesh(), viennamesh::aspect_ratio<ElementType>, statistic_orig.median());
if(alpha.valid() && beta.valid() && gamma.valid() && delta.valid() )
{
statistic.set_mesh_quality_weights(alpha(), beta(), gamma(), delta());
info(5) << "values for comprehensive mesh quality metric: alpha = " << alpha()
<< ", beta = " << beta() << ", gamma = " << gamma() << ", delta = "<< delta() << std::endl;
}
else
{
info(5) << "default values for comprehensive mesh quality metric used: alpha = 0.25, beta = 20, gamma = 1.0, delta = 1.3" << std::endl;
}
set_output("minimum_distance_rms", statistic.min_dist_rms());
set_output("mean_curvature_difference", statistic.mean_curvature());
set_output("area_deviation", statistic.volume_deviation());
set_output("triangle_shape", statistic.good_elements()/statistic.count());
set_output("mesh_quality_metric", statistic.mesh_quality_metric());
}
info(5) << statistic << "\n";
#if HIST
statistic.normalize();
std::vector<viennagrid_numeric> bins;
for (HistogramType::const_iterator bit = statistic.histogram().begin(); bit != statistic.histogram().end(); ++bit)
bins.push_back( (*bit).second );
bins.push_back( statistic.histogram().overflow_bin() );
data_handle<viennagrid_numeric> output_bins = make_data<viennagrid_numeric>();
output_bins.set( bins );
set_output( "bins", output_bins );
#endif
set_output( "min", statistic.min() );
set_output( "max", statistic.max() );
set_output( "mean", statistic.mean() );
set_output( "median", statistic.median());
return true;
}
bool svm_train(size_t l,
size_t n,
const float *y,
const FeatureNode **x,
double C,
double *w) {
size_t active_size = l;
double PGmax_old = kINF;
double PGmin_old = -kINF;
std::vector<double> QD(l);
std::vector<size_t> index(l);
std::vector<double> alpha(l);
std::fill(w, w + n, 0.0);
std::fill(alpha.begin(), alpha.end(), 0.0);
for (size_t i = 0; i < l; ++i) {
index[i] = i;
QD[i] = 0;
for (const FeatureNode *f = x[i]; f->index >= 0; ++f) {
QD[i] += (f->value * f->value);
}
}
static const size_t kMaxIteration = 2000;
for (size_t iter = 0; iter < kMaxIteration; ++iter) {
double PGmax_new = -kINF;
double PGmin_new = kINF;
std::random_shuffle(index.begin(), index.begin() + active_size);
for (size_t s = 0; s < active_size; ++s) {
const size_t i = index[s];
double G = 0;
for (const FeatureNode *f = x[i]; f->index >= 0; ++f) {
G += w[f->index] * f->value;
}
G = G * y[i] - 1;
double PG = 0.0;
if (alpha[i] == 0.0) {
if (G > PGmax_old) {
active_size--;
std::swap(index[s], index[active_size]);
s--;
continue;
} else if (G < 0.0) {
PG = G;
}
} else if (alpha[i] == C) {
if (G < PGmin_old) {
active_size--;
std::swap(index[s], index[active_size]);
s--;
continue;
} else if (G > 0.0) {
PG = G;
}
} else {
PG = G;
}
PGmax_new = std::max(PGmax_new, PG);
PGmin_new = std::min(PGmin_new, PG);
if (std::abs(PG) > 1.0e-12) {
const double alpha_old = alpha[i];
alpha[i] = std::min(std::max(alpha[i] - G/QD[i], 0.0), C);
const double d = (alpha[i] - alpha_old)* y[i];
for (const FeatureNode *f = x[i]; f->index >= 0; ++f) {
w[f->index] += d * f->value;
}
}
}
if (iter % 4 == 0) {
std::cout << "." << std::flush;
}
if ((PGmax_new - PGmin_new) <= kEPS) {
if (active_size == l) {
break;
} else {
active_size = l;
PGmax_old = kINF;
PGmin_old = -kINF;
continue;
}
}
PGmax_old = PGmax_new;
PGmin_old = PGmin_new;
if (PGmax_old <= 0) {
PGmax_old = kINF;
}
if (PGmin_old >= 0) {
PGmin_old = -kINF;
}
}
std::cout << std::endl;
return true;
}
void hmm::hmm_print_alignment(ofstream& of){
WordIndex i, j, l, m, jj;
SentPair sent;
double temp, temp_yita;
PositionIndex temp_index;
sHander.new_start();
while(sHander.getNextSentence(sent)){
vector<PositionIndex> al_ij;
al_ij.clear();
al_ij.push_back(0);
vector<WordIndex>& es = sent.esent;
vector<WordIndex>& fs = sent.fsent;
l = es.size() - 1;
m = fs.size() - 1;
double uniform = 1.0/(double)(l+1);
double temp_i, temp_max;
//learning alpha parameters
array2<double> alpha(m+1, l+1);
for(j=0;j <= l;j++)
alpha(1,j) = uniform * cal_ef[WordPairIds(es[j], fs[1])].prob;
for(i=1;i <= m-1;i++){
for(jj=0;jj <= l;jj++){
temp_max = alpha(i, 0) * p_jj_jl[jj*G2+0*G1+l];
for(j=1;j <= l;j++){
temp_i = alpha(i, j) * p_jj_jl[jj*G2+j*G1+l];
if(temp_i > temp_max)
temp_max = temp_i;
}
alpha(i+1, jj) = temp_max * cal_ef[WordPairIds(es[jj], fs[i+1])].prob;
}
}
//learning beta parameters
array2<double> beta(m+1, l+1);
for(j=0;j <= l;j++)
beta(m, j) = 1.0;
for(i=m;i >= 2;i--){
for(j=0;j <= l;j++){
temp_max = beta(i, 0) * cal_ef[WordPairIds(es[0], fs[i])].prob * p_jj_jl[0*G2+j*G1+l];
for(jj=1;jj <= l;jj++){
temp_i = beta(i, jj) * cal_ef[WordPairIds(es[jj], fs[i])].prob * p_jj_jl[jj*G2+j*G1+l];
if(temp_i > temp_max)
temp_max = temp_i;
}
beta(i-1, j) = temp_max;
}
}
//learning yita parameters
array2<double> yita(m+1, l+1);
for(i=1;i <= m;i++){
double sum_yita = 0.0;
for(j=0;j <= l;j++)
sum_yita += alpha(i, j) * beta(i, j);
for(j=0;j <= l;j++)
yita(i, j) = alpha(i, j) * beta(i, j) / sum_yita;
}
for(i=1;i <= m;i++){
temp = 0.0;
for(j=0;j <= l;j++){
temp_yita = yita(i, j);
if(temp_yita > temp){
temp = temp_yita;
temp_index = j;
}
}
al_ij.push_back(temp_index);
}
of<<"#sentence pair ("<<sent.sentenceNo<<") f sentence length "<<m<<", e sentence length "<<l<<"\n";
for(i=1;i <= m;i++)
of<<FList[fs[i]]<<" ";
of<<"\n";
for(j=0;j <= l;j++){
of<<EList[es[j]]<<" ({ ";
for(i=1;i <= m;i++){
if(al_ij[i] == j)
of<<i<<" ";
}
of<<"}) ";
}
of<<endl;
}
}
void turbulentTemperatureCoupledMixedSTFvPatchScalarField::updateCoeffs()
{
if (updated())
{
return;
}
// Get the coupling information from the directMappedPatchBase
const directMappedPatchBase& mpp =
refCast<const directMappedPatchBase>(patch().patch());
const polyMesh& nbrMesh = mpp.sampleMesh();
const label samplePatchI = mpp.samplePolyPatch().index();
const fvPatch& nbrPatch =
refCast<const fvMesh>(nbrMesh).boundary()[samplePatchI];
// Force recalculation of mapping and schedule
const mapDistribute& distMap = mpp.map();
scalarField Tc = patchInternalField();
scalarField& Tp = *this;
const turbulentTemperatureCoupledMixedSTFvPatchScalarField&
nbrField = refCast
<const turbulentTemperatureCoupledMixedSTFvPatchScalarField>
(
nbrPatch.lookupPatchField<volScalarField, scalar>(TnbrName_)
);
// Swap to obtain full local values of neighbour internal field
scalarField TcNbr = nbrField.patchInternalField();
mapDistribute::distribute
(
Pstream::defaultCommsType,
distMap.schedule(),
distMap.constructSize(),
distMap.subMap(), // what to send
distMap.constructMap(), // what to receive
TcNbr
);
// Swap to obtain full local values of neighbour K*delta
scalarField KDeltaNbr = nbrField.K()*nbrPatch.deltaCoeffs();
mapDistribute::distribute
(
Pstream::defaultCommsType,
distMap.schedule(),
distMap.constructSize(),
distMap.subMap(), // what to send
distMap.constructMap(), // what to receive
KDeltaNbr
);
scalarField KDelta = K()*patch().deltaCoeffs();
scalarField Qr(Tp.size(), 0.0);
if (QrName_ != "none")
{
Qr = patch().lookupPatchField<volScalarField, scalar>(QrName_);
}
scalarField QrNbr(Tp.size(), 0.0);
if (QrNbrName_ != "none")
{
QrNbr = nbrPatch.lookupPatchField<volScalarField, scalar>(QrNbrName_);
}
scalarField alpha(KDeltaNbr - (Qr + QrNbr)/Tp);
valueFraction() = alpha/(alpha + KDelta);
refValue() = (KDeltaNbr*TcNbr)/alpha;
mixedFvPatchScalarField::updateCoeffs();
}
int main()
{
MTtype=1;
for(int i=1; i<20; i++)
{
QR=1.999 + i*0.0005;
QP=1.999 + i*0.0005;
// QR=QP=2.0;
/*
Matrix col(3,1);
Matrix mat(3,3);
col.set_element(0,0,5.0);
col.set_element(1,0,8.0);
col.set_element(2,0,2.0);
mat.set_element(0,0,1.0);
mat.set_element(1,0,2.0);
mat.set_element(2,0,4.0);
mat.set_element(0,1,1.0);
mat.set_element(1,1,3.0);
mat.set_element(2,1,0.0);
mat.set_element(0,2,1.0);
mat.set_element(1,2,5.0);
mat.set_element(2,2,5.0);
col.print_matrix();
mat.print_matrix();
col=gauss_jordan(col,mat);
col.print_matrix();
*/
int N_mesh=20;
/*
//Test for iterative solution
Matrix id(3,3);
Matrix g(3,1);
Matrix f(3,1);
for(int i=0; i<3; i++)
{
for(int j=0; j<3; j++)
{
g.set_element(i,0,1.0);
if(i==j)
id.set_element(i,j,1.0);
else
id.set_element(i,j,0.0);
}
}
f=iterate(g,id,3);
f.print_matrix();
*/
Matrix x(N_mesh,1);
Matrix w(N_mesh,1);
//For N=10
if(N_mesh==10)
{
x.set_element(4,0,-0.1488743389816312);
x.set_element(3,0,-0.4333953941292472);
x.set_element(2,0,-0.6794095682990244);
x.set_element(1,0,-0.8650633666889845);
x.set_element(0,0,-0.9739065285171717);
x.set_element(5,0,0.1488743389816312);
x.set_element(6,0,0.4333953941292472);
x.set_element(7,0,0.6794095682990244);
x.set_element(8,0,0.8650633666889845);
x.set_element(9,0,0.9739065285171717);
// x.set_element(7,0,0.0);
// x.set_element(8,0,0.0);
// x.set_element(9,0,0.0);
w.set_element(4,0,0.2955242247147529);
w.set_element(3,0,0.2692667193099963);
w.set_element(2,0,0.2190863625159820);
w.set_element(1,0,0.1494513491505806);
w.set_element(0,0,0.0666713443086881);
w.set_element(5,0,0.2955242247147529);
w.set_element(6,0,0.2692667193099963);
w.set_element(7,0,0.2190863625159820);
w.set_element(8,0,0.1494513491505806);
w.set_element(9,0,0.0666713443086881);
// w.set_element(7,0,0.0);
// w.set_element(8,0,0.0);
// w.set_element(9,0,0.0);
}
else if(N_mesh==20)
{
x.set_element(0,0,-0.9931285991850949);
x.set_element(1,0,-0.9639719272779138);
x.set_element(2,0,-0.9122344282513259);
x.set_element(3,0,-0.8391169718222188);
x.set_element(4,0,-0.7463319064601508);
x.set_element(5,0,-0.63605368064601508);
x.set_element(6,0,-0.5108670019508271);
x.set_element(7,0,-0.3737060887154195);
x.set_element(8,0,-0.2277858511416451);
x.set_element(9,0,-0.0765265211334973);
x.set_element(10,0,0.0765265211334973);
x.set_element(11,0,0.2277858511416451);
x.set_element(12,0,0.3737060887154195);
x.set_element(13,0,0.5108670019508271);
x.set_element(14,0,0.63605368064601508);
x.set_element(15,0,0.7463319064601508);
x.set_element(16,0,0.8391169718222188);
x.set_element(17,0,0.9122344282513259);
x.set_element(18,0,0.9639719272779138);
x.set_element(19,0,0.9931285991850949);
w.set_element(0,0,0.0176140071391521);
w.set_element(1,0,0.0406014298003869);
w.set_element(2,0,0.0626720483341091);
w.set_element(3,0,0.0832767415767048);
w.set_element(4,0,0.1019301198172404);
w.set_element(5,0,0.1181945319615184);
w.set_element(6,0,0.1316886384491766);
w.set_element(7,0,0.1420961093183820);
w.set_element(8,0,0.1491729864726037);
w.set_element(9,0,0.1527533871307258);
w.set_element(19,0,0.0176140071391521);
w.set_element(18,0,0.0406014298003869);
w.set_element(17,0,0.0626720483341091);
w.set_element(16,0,0.0832767415767048);
w.set_element(15,0,0.1019301198172404);
w.set_element(14,0,0.1181945319615184);
w.set_element(13,0,0.1316886384491766);
w.set_element(12,0,0.1420961093183820);
w.set_element(11,0,0.1491729864726037);
w.set_element(10,0,0.1527533871307258);
}
// Matrix *px;
// Matrix *pw;
// px=&x;
// pw=&w;
// gauss_quad_mesh(px,pw,-1,1.0,N_mesh);
Matrix k(N_mesh, 1);
double ktemp=0.0;
for(int i=0; i<N_mesh; i++)
{
// ktemp=qp(x.get_element(i,0)+0.00001);
// k.set_element(i,0,ktemp);
k.set_element(i,0,i*0.5);
// std::cout << "Ki= " << k.get_element(i,0) << std::endl;
}
/*
std::cout << "V(10)=" << malftjon(10)/41.47 << std::endl;
std::cout << "V(10.0)=" << malftjon(10.0)/41.47 << std::endl;
std::cout << "V(0.2)=" << malftjon(0.2)/41.47 << std::endl;
std::cout << "Sin(10)=" << sin(10) << std::endl;
*/
/*
std::cout << "U(0,0)=" << U(x,w,0,0) << std::endl;
std::cout << "Uf(0,0)=" << Ufixed(x,w,0,0) << std::endl;
std::cout << "U(1,1)=" << U(x,w,1,1) << std::endl;
std::cout << "U(10,30)=" << U(x,w,10,30) << std::endl;
std::cout << "U(0.3,0.1)=" << U(x,w,0.3,0.1) << std::endl;
std::cout << "Uf(1,1)=" << Ufixed(x,w,1,1) << std::endl;
std::cout << "Uf(10,30)=" << Ufixed(x,w,10,30) << std::endl;
std::cout << "Uf(30,10)=" << Ufixed(x,w,30,10) << std::endl;
std::cout << "Uf(280,2)=" << Ufixed(x,w,280,2) << std::endl;
*/
/*
//Checking xi,weights, and q(xi)
x.print_matrix();
w.print_matrix();
for(int i=0; i<N_mesh; i++)
{
std::cout << "q(xi)=" << qp(x.get_element(i,0)) << std::endl;
std::cout << "wtilde(xi)=" << w.get_element(i,0)*qprime(x.get_element(i,0)) << std::endl;
}
*/
/*
//Testing gaussian integration of e^-x
std::function<double (double)> fint = exponential;
std::cout << "\n Int 0 to inf of e^-x = " << gauss_quad_int(fint,x,w);
*/
//Scattering Problem
/*
Matrix wmat(N_mesh, N_mesh);
Matrix *pwm;
pwm=&wmat;
wmatrix(pwm,x,w,k);
//Checking wmatrix stuff.
// std::cout << "Printing wmat\n";
// wmat.print_matrix();
// std::cout << "wmat element 10,12 = " << wmat.get_element(10,12);
// std::cout << "wmat element 5,12 = " << wmat.get_element(5,12);
Matrix wkk(N_mesh, N_mesh);
Matrix *pwkk;
pwkk=&wkk;
wmatrixkk(pwkk,x,w,k);
Matrix imf(N_mesh, 1);
Matrix *imagfk;
imagfk=&imf;
imagf(imagfk,wmat,x,k);
// imf.print_matrix();
Matrix ref(N_mesh, 1);
Matrix *realfk;
realfk=&ref;
realf(realfk,wmat,wmat,x,w,k);
// ref.print_matrix();
Matrix delta(N_mesh, 1);
Matrix *pdf;
pdf=δ
delk(pdf,imf,ref);
// delta.print_matrix();
// std::cout << "qp | a" << std::endl;
std::cout << QP << " " << alength(wmat,x,w) << std::endl;
// std::cout << U(x,w,3,16) << std::endl;
// std::cout << U(x,w,3,18) << std::endl;
// std::cout << U(x,w,3,50) << std::endl;
*/
// std::cout << "Bound State Problem: \n";
Matrix alpha(N_mesh,1);
for(int i=0; i<N_mesh; i++)
{
alpha.set_element(i,0,0.1 + i*0.01);
}
Matrix j(N_mesh,1);
j=jost(x,w,k,alpha);
// for(int i=0; i<N_mesh; i++)
// {
// std::cout << alpha.get_element(i,0) << " " << j.get_element(i,0) << std::endl;
// }
Matrix roots(40,1);
roots.set_element(i,0,rootoff(j,alpha));
std::cout << QP << " " << -roots.get_element(i,0)*roots.get_element(i,0)*41.47 << std::endl;
}
return 0;
}
List objectivex(const arma::mat& transition, const arma::cube& emission,
const arma::vec& init, const arma::ucube& obs, const arma::umat& ANZ,
const arma::ucube& BNZ, const arma::uvec& INZ, const arma::uvec& nSymbols,
const arma::mat& coef, const arma::mat& X, arma::uvec& numberOfStates,
unsigned int threads) {
unsigned int q = coef.n_rows;
arma::vec grad(
arma::accu(ANZ) + arma::accu(BNZ) + arma::accu(INZ) + (numberOfStates.n_elem- 1) * q,
arma::fill::zeros);
arma::mat weights = exp(X * coef).t();
if (!weights.is_finite()) {
grad.fill(-arma::datum::inf);
return List::create(Named("objective") = arma::datum::inf, Named("gradient") = wrap(grad));
}
weights.each_row() /= sum(weights, 0);
arma::mat initk(emission.n_rows, obs.n_slices);
for (unsigned int k = 0; k < obs.n_slices; k++) {
initk.col(k) = init % reparma(weights.col(k), numberOfStates);
}
arma::uvec cumsumstate = arma::cumsum(numberOfStates);
unsigned int error = 0;
double ll = 0;
#pragma omp parallel for if(obs.n_slices >= threads) schedule(static) reduction(+:ll) num_threads(threads) \
default(none) shared(q, grad, nSymbols, ANZ, BNZ, INZ, \
numberOfStates, cumsumstate, obs, init, initk, X, weights, transition, emission, error)
for (unsigned int k = 0; k < obs.n_slices; k++) {
if (error == 0) {
arma::mat alpha(emission.n_rows, obs.n_cols); //m,n
arma::vec scales(obs.n_cols); //n
arma::sp_mat sp_trans(transition);
uvForward(sp_trans.t(), emission, initk.col(k), obs.slice(k), alpha, scales);
arma::mat beta(emission.n_rows, obs.n_cols); //m,n
uvBackward(sp_trans, emission, obs.slice(k), beta, scales);
int countgrad = 0;
arma::vec grad_k(grad.n_elem, arma::fill::zeros);
// transitionMatrix
if (arma::accu(ANZ) > 0) {
for (unsigned int jj = 0; jj < numberOfStates.n_elem; jj++) {
arma::vec gradArow(numberOfStates(jj));
arma::mat gradA(numberOfStates(jj), numberOfStates(jj));
int ind_jj = cumsumstate(jj) - numberOfStates(jj);
for (unsigned int i = 0; i < numberOfStates(jj); i++) {
arma::uvec ind = arma::find(ANZ.row(ind_jj + i).subvec(ind_jj, cumsumstate(jj) - 1));
if (ind.n_elem > 0) {
gradArow.zeros();
gradA.eye();
gradA.each_row() -= transition.row(ind_jj + i).subvec(ind_jj, cumsumstate(jj) - 1);
gradA.each_col() %= transition.row(ind_jj + i).subvec(ind_jj, cumsumstate(jj) - 1).t();
for (unsigned int j = 0; j < numberOfStates(jj); j++) {
for (unsigned int t = 0; t < (obs.n_cols - 1); t++) {
double tmp = alpha(ind_jj + i, t);
for (unsigned int r = 0; r < obs.n_rows; r++) {
tmp *= emission(ind_jj + j, obs(r, t + 1, k), r);
}
gradArow(j) += tmp * beta(ind_jj + j, t + 1);
}
}
gradArow = gradA * gradArow;
grad_k.subvec(countgrad, countgrad + ind.n_elem - 1) = gradArow.rows(ind);
countgrad += ind.n_elem;
}
}
}
}
if (arma::accu(BNZ) > 0) {
// emissionMatrix
for (unsigned int r = 0; r < obs.n_rows; r++) {
arma::vec gradBrow(nSymbols(r));
arma::mat gradB(nSymbols(r), nSymbols(r));
for (unsigned int i = 0; i < emission.n_rows; i++) {
arma::uvec ind = arma::find(BNZ.slice(r).row(i));
if (ind.n_elem > 0) {
gradBrow.zeros();
gradB.eye();
gradB.each_row() -= emission.slice(r).row(i).subvec(0, nSymbols(r) - 1);
gradB.each_col() %= emission.slice(r).row(i).subvec(0, nSymbols(r) - 1).t();
for (unsigned int j = 0; j < nSymbols(r); j++) {
if (obs(r, 0, k) == j) {
double tmp = initk(i, k);
for (unsigned int r2 = 0; r2 < obs.n_rows; r2++) {
if (r2 != r) {
tmp *= emission(i, obs(r2, 0, k), r2);
}
}
gradBrow(j) += tmp * beta(i, 0);
}
for (unsigned int t = 0; t < (obs.n_cols - 1); t++) {
if (obs(r, t + 1, k) == j) {
double tmp = beta(i, t + 1);
for (unsigned int r2 = 0; r2 < obs.n_rows; r2++) {
if (r2 != r) {
tmp *= emission(i, obs(r2, t + 1, k), r2);
}
}
gradBrow(j) += arma::dot(alpha.col(t), transition.col(i)) * tmp;
}
}
}
gradBrow = gradB * gradBrow;
grad_k.subvec(countgrad, countgrad + ind.n_elem - 1) = gradBrow.rows(ind);
countgrad += ind.n_elem;
}
}
}
}
if (arma::accu(INZ) > 0) {
for (unsigned int i = 0; i < numberOfStates.n_elem; i++) {
int ind_i = cumsumstate(i) - numberOfStates(i);
arma::uvec ind = arma::find(
INZ.subvec(ind_i, cumsumstate(i) - 1));
if (ind.n_elem > 0) {
arma::vec gradIrow(numberOfStates(i), arma::fill::zeros);
for (unsigned int j = 0; j < numberOfStates(i); j++) {
double tmp = weights(i, k);
for (unsigned int r = 0; r < obs.n_rows; r++) {
tmp *= emission(ind_i + j, obs(r, 0, k), r);
}
gradIrow(j) += tmp * beta(ind_i + j, 0);
}
arma::mat gradI(numberOfStates(i), numberOfStates(i), arma::fill::zeros);
gradI.eye();
gradI.each_row() -= init.subvec(ind_i, cumsumstate(i) - 1).t();
gradI.each_col() %= init.subvec(ind_i, cumsumstate(i) - 1);
gradIrow = gradI * gradIrow;
grad_k.subvec(countgrad, countgrad + ind.n_elem - 1) = gradIrow.rows(ind);
countgrad += ind.n_elem;
}
}
}
for (unsigned int jj = 1; jj < numberOfStates.n_elem; jj++) {
unsigned int ind_jj = (cumsumstate(jj) - numberOfStates(jj));
for (unsigned int j = 0; j < emission.n_rows; j++) {
double tmp = 1.0;
for (unsigned int r = 0; r < obs.n_rows; r++) {
tmp *= emission(j, obs(r, 0, k), r);
}
if ((j >= ind_jj) & (j < cumsumstate(jj))) {
grad_k.subvec(countgrad + q * (jj - 1), countgrad + q * jj - 1) += tmp
* beta(j, 0) * initk(j, k) * X.row(k).t() * (1.0 - weights(jj, k));
} else {
grad_k.subvec(countgrad + q * (jj - 1), countgrad + q * jj - 1) -= tmp
* beta(j, 0) * initk(j, k) * X.row(k).t() * weights(jj, k);
}
}
}
if (!scales.is_finite() || !beta.is_finite()) {
#pragma omp atomic
error++;
} else {
ll -= arma::sum(log(scales));
#pragma omp critical
grad += grad_k;
}
}
}
if(error > 0){
ll = -arma::datum::inf;
grad.fill(-arma::datum::inf);
}
return List::create(Named("objective") = -ll, Named("gradient") = wrap(-grad));
}
LBR2ModelParameters getLBR2ModelParameters() {
LBR2ModelParameters p;
std::vector< LBR2Arm > & arm = p.arm;
TVector3 & g = p.g;
p.arm.resize(7);
TVectorX a(7);
a << 0.0, 0.25, 0.26, 0.0, 0.0000, 0.0, 0.0;
TVectorX d(7);
d << 0.213, 0.00, 0.00, 0.0, 0.3385, 0.0, 0.0785;
TVectorX alpha(7);
alpha << -M_PI / 2, M_PI / 2, -M_PI / 2, M_PI / 2, -M_PI / 2, M_PI / 2, 0.0;
TVectorX theta(7);
theta << 0.0, -M_PI / 2, 0.0, M_PI / 2, 0.0, 0.0, 0.0;
for (size_t i = 0; i < 7; ++i) {
p.arm[i].jointType = JointType::Revolute;
p.arm[i].a = a(i);
p.arm[i].d = d(i);
p.arm[i].alpha = alpha(i);
p.arm[i].theta = theta(i);
}
g = TVector3(0, 0, -9.81);
arm[0].axis = TVector3::UnitZ();
arm[0].l = TVector3(0.0, 0.0, 0.213);
arm[0].m = 1.7780;
arm[0].com = TVector3(0.0, 0.0, 0.0);
arm[0].I << 0.0, 0.0, 0.0,
0.0, 0.4996e-2, 0.0,
0.0, 0.0, 0.0;
//arm[0].I << 0.0, 0.0, 0.0,
// 0.0, 0.0, 0.0,
// 0.0, 0.0, 0.4996e-2;
arm[1].axis = TVector3::UnitZ();
arm[1].l = TVector3(0.0, -0.25, 0.0);
arm[1].m = 5.0249;
arm[1].com = TVector3(0.1042 - 0.25, 0.0008858, -0.0023359);
arm[1].I << 0.8870e-2, 0.0, 0.2528e-2,
0.0, 0.1234, 0.0,
0.2528e-2, 0.0, 0.1235;
//arm[1].I << 0.8870e-2, 0.2528e-2, 0.0,
// 0.2528e-2, 0.1235, 0.0,
// 0.0, 0.0, 0.1234;
arm[2].axis = TVector3::UnitZ();
arm[2].l = TVector3(0.26, 0.0, 0.0);
arm[2].m = 1.7877;
arm[2].com = TVector3(0.1111 - 0.26, -0.005409, -0.001836);
arm[2].I << 0.6763e-2, 0.0, 0.1746e-2,
0.0, 0.46153e-1, 0.0,
0.1746e-2, 0.0, 0.4613e-1;
//arm[2].I << 0.6763e-2, 0.1746e-2, 0.0,
// 0.1746e-2, 0.4613e-1, 0.0,
// 0.0, 0.0, 0.46153e-1;
arm[3].axis = TVector3::UnitZ();
arm[3].l = TVector3(0.0, 0.0, 0.0);
arm[3].m = 3.0685;
arm[3].com = TVector3(0.0017297, -0.002953, 0.01059);
arm[3].I << 0.2058e-1, 0.0, 0.0,
0.0, 0.2006e-1, 0.0,
0.0, 0.0, 0.4404e-2;
//arm[3].I << 0.2058e-1, 0.0, 0.0,
// 0.0, 0.4404e-2, 0.0,
// 0.0, 0.0, 0.2006e-1;
arm[4].axis = TVector3::UnitZ();
arm[4].l = TVector3(0.0, 0.0, 0.3385);
arm[4].m = 1.1915;
arm[4].com = TVector3(-0.00001299, -0.11986, 0.011362);
arm[4].I << 0.3077e-1, 0.0, 0.0,
0.0, 0.328e-2, 0.0,
0.0, 0.0, 0.2924e-1;
//arm[4].I << 0.3077e-1, 0.0, 0.0,
// 0.0, 0.2924e-1, 0.0,
// 0.0, 0.0, 0.328e-2;
arm[5].axis = TVector3::UnitZ();
arm[5].l = TVector3(0.0, 0.0, 0.0);
arm[5].m = 2.8785;
arm[5].com = TVector3(0.0018438, -0.00459189, 0.01130);
arm[5].I << 0.20423e-1, 0.0, 0.0,
0.0, 0.1997e-1, 0.0,
0.0, 0.0, 0.4233e-2;
//arm[5].I << 0.20423e-1, 0.0, 0.0,
// 0.0, 0.4233e-2, 0.0,
// 0.0, 0.0, 0.1997e-1;
arm[6].axis = TVector3::UnitZ();
arm[6].l = TVector3(0.0, 0.0, 0.0785);
arm[6].m = 0.5721;
arm[6].com = TVector3(-0.00002610, 0.0001501, 0.058534);
arm[6].I << 0.2657e-2, 0.0, 0.0,
0.0, 0.268e-2, 0.0,
0.0, 0.0, 0.8615e-3;
return p;
}
bool Encoder::learn(const char *templfile,
const char *trainfile,
const char *modelfile,
bool textmodelfile,
size_t maxitr,
size_t freq,
double eta,
double C,
unsigned short thread_num,
unsigned short shrinking_size,
int algorithm) {
std::cout << COPYRIGHT << std::endl;
CHECK_FALSE(eta > 0.0) << "eta must be > 0.0";
CHECK_FALSE(C >= 0.0) << "C must be >= 0.0";
CHECK_FALSE(shrinking_size >= 1) << "shrinking-size must be >= 1";
CHECK_FALSE(thread_num > 0) << "thread must be > 0";
#ifndef CRFPP_USE_THREAD
CHECK_FALSE(thread_num == 1)
<< "This architecture doesn't support multi-thrading";
#endif
if (algorithm == MIRA && thread_num > 1) {
std::cerr << "MIRA doesn't support multi-thrading. use thread_num=1"
<< std::endl;
}
EncoderFeatureIndex feature_index;
Allocator allocator(thread_num);
std::vector<TaggerImpl* > x;
x.reserve(max_line_nums); //adjust vector.capacity() to accelerate operation:push_back()
std::cout.setf(std::ios::fixed, std::ios::floatfield);
std::cout.precision(5);
#define WHAT_ERROR(msg) do { \
for (std::vector<TaggerImpl *>::iterator it = x.begin(); \
it != x.end(); ++it) \
delete *it; \
std::cerr << msg << std::endl; \
return false; } while (0)
CHECK_FALSE(feature_index.open(templfile, trainfile))
<< feature_index.what();
{
progress_timer pg;
std::ifstream ifs(WPATH(trainfile));
CHECK_FALSE(ifs) << "cannot open: " << trainfile;
std::cout << "reading training data: " << std::flush;
size_t line = 0;
while (ifs) {
TaggerImpl *_x = new TaggerImpl();
_x->open(&feature_index, &allocator);
if (!_x->read(&ifs) || !_x->shrink()) {
WHAT_ERROR(_x->what());
}
if (!_x->empty()) {
x.push_back(_x);
} else {
delete _x;
continue;
}
_x->set_thread_id(line % thread_num);
if (++line % 10000 == 0) {
std::cout << line << ".. " << std::endl << std::flush;
}
}
ifs.close();
std::cout << "\nDone!";
}
feature_index.shrink(freq, &allocator);
std::vector <double> alpha(feature_index.size()); // parameter
std::fill(alpha.begin(), alpha.end(), 0.0);
feature_index.set_alpha(&alpha[0]);
std::cout << "Number of sentences: " << x.size() << std::endl;
std::cout << "Number of features: " << feature_index.size() << std::endl;
std::cout << "Number of thread(s): " << thread_num << std::endl;
std::cout << "Freq: " << freq << std::endl;
std::cout << "eta: " << eta << std::endl;
std::cout << "C: " << C << std::endl;
std::cout << "shrinking size: " << shrinking_size
<< std::endl;
progress_timer pg;
switch (algorithm) {
case MIRA:
if (!runMIRA(x, &feature_index, &alpha[0],
maxitr, C, eta, shrinking_size, thread_num)) {
WHAT_ERROR("MIRA execute error");
}
break;
case CRF_L2:
if (!runCRF(x, &feature_index, &alpha[0],
maxitr, C, eta, shrinking_size, thread_num, false)) {
WHAT_ERROR("CRF_L2 execute error");
}
break;
case CRF_L1:
if (!runCRF(x, &feature_index, &alpha[0],
maxitr, C, eta, shrinking_size, thread_num, true)) {
WHAT_ERROR("CRF_L1 execute error");
}
break;
}
for (std::vector<TaggerImpl *>::iterator it = x.begin();
it != x.end(); ++it) {
delete *it;
}
if (!feature_index.save(modelfile, textmodelfile)) {
WHAT_ERROR(feature_index.what());
}
std::cout << "\nDone!";
return true;
}
bool valid(char c){ return alpha(c) || reserved(c); }
void MinresIter<ScalarType,MV,OP>::iterate()
{
//
// Allocate/initialize data structures
//
if (initialized_ == false) {
initialize();
}
Teuchos::BLAS<int,ScalarType> blas;
// Create convenience variables for zero and one.
const ScalarType one = SCT::one();
const MagnitudeType zero = SMT::zero();
// Allocate memory for scalars.
Teuchos::SerialDenseMatrix<int,ScalarType> alpha( 1, 1 );
Teuchos::SerialDenseMatrix<int,ScalarType> beta( beta1_ );
phibar_ = Teuchos::ScalarTraits<ScalarType>::magnitude( beta1_(0,0) );
ScalarType shift = zero; // TODO Allow for proper shift.
// Initialize a few variables.
ScalarType oldBeta = zero;
ScalarType epsln = zero;
ScalarType cs = -one;
ScalarType sn = zero;
ScalarType dbar = zero;
// Declare a few others that will be initialized in the loop.
ScalarType oldeps;
ScalarType delta;
ScalarType gbar;
ScalarType phi;
ScalarType gamma;
// Allocate workspace.
Teuchos::RCP<MV> V = MVT::Clone( *Y_, 1 );
Teuchos::RCP<MV> tmpY, tmpW; // Not allocated, just used to transfer ownership.
// Get the current solution vector.
Teuchos::RCP<MV> cur_soln_vec = lp_->getCurrLHSVec();
// Check that the current solution vector only has one column.
TEUCHOS_TEST_FOR_EXCEPTION( MVT::GetNumberVecs(*cur_soln_vec) != 1,
MinresIterateFailure,
"Belos::MinresIter::iterate(): current linear system has more than one vector!" );
////////////////////////////////////////////////////////////////
// Iterate until the status test tells us to stop.
//
while (stest_->checkStatus(this) != Passed) {
// Increment the iteration
iter_++;
// Normalize previous vector.
// v = y / beta(0,0);
MVT::MvAddMv (one / beta(0,0), *Y_, zero, *Y_, *V);
// Apply operator.
lp_->applyOp (*V, *Y_);
// Apply shift
if (shift != zero)
MVT::MvAddMv (one, *Y_, -shift, *V, *Y_);
if (iter_ > 1)
MVT::MvAddMv (one, *Y_, -beta(0,0)/oldBeta, *R1_, *Y_);
// alpha := dot(V, Y_)
MVT::MvTransMv (one, *V, *Y_, alpha);
// y := y - alpha/beta r2
MVT::MvAddMv (one, *Y_, -alpha(0,0)/beta(0,0), *R2_, *Y_);
// r1 = r2;
// r2 = y;
tmpY = R1_;
R1_ = R2_;
R2_ = Y_;
Y_ = tmpY;
// apply left preconditioner
if ( lp_->getLeftPrec() != Teuchos::null ) {
lp_->applyLeftPrec( *R2_, *Y_ );
} // else "y = r2"
else {
MVT::MvAddMv( one, *R2_, zero, *R2_, *Y_ );
}
// Get new beta.
oldBeta = beta(0,0);
MVT::MvTransMv( one, *R2_, *Y_, beta );
// Intercept beta <= 0.
//
// Note: we don't try to test for nonzero imaginary component of
// beta, because (a) it could be small and nonzero due to
// rounding error in computing the inner product, and (b) it's
// hard to tell how big "not small" should be, without computing
// some error bounds (for example, by modifying the linear
// algebra library to compute a posteriori rounding error bounds
// for the inner product, and then changing
// Belos::MultiVecTraits to make this information available).
TEUCHOS_TEST_FOR_EXCEPTION( SCT::real(beta(0,0)) <= zero,
MinresIterateFailure,
"Belos::MinresIter::iterate(): Encountered nonpositi"
"ve value " << beta(0,0) << " for r2^H*M*r2 at itera"
"tion " << iter_ << ": MINRES cannot continue." );
beta(0,0) = SCT::squareroot( beta(0,0) );
// Apply previous rotation Q_{k-1} to get
//
// [delta_k epsln_{k+1}] = [cs sn][dbar_k 0 ]
// [gbar_k dbar_{k+1} ] [-sn cs][alpha_k beta_{k+1}].
//
oldeps = epsln;
delta = cs*dbar + sn*alpha(0,0);
gbar = sn*dbar - cs*alpha(0,0);
epsln = sn*beta(0,0);
dbar = - cs*beta(0,0);
// Compute the next plane rotation Q_k.
this->symOrtho(gbar, beta(0,0), &cs, &sn, &gamma);
phi = cs * phibar_; // phi_k
phibar_ = Teuchos::ScalarTraits<ScalarType>::magnitude( sn * phibar_ ); // phibar_{k+1}
// w1 = w2;
// w2 = w;
MVT::MvAddMv( one, *W_, zero, *W_, *W1_ );
tmpW = W1_;
W1_ = W2_;
W2_ = W_;
W_ = tmpW;
// w = (v - oldeps*w1 - delta*w2) / gamma;
MVT::MvAddMv( one, *V, -oldeps, *W1_, *W_ );
MVT::MvAddMv( one, *W_, -delta, *W2_, *W_ );
MVT::MvScale( *W_, one / gamma );
// Update x:
// x = x + phi*w;
MVT::MvAddMv( one, *cur_soln_vec, phi, *W_, *cur_soln_vec );
lp_->updateSolution();
} // end while (sTest_->checkStatus(this) != Passed)
}
Int_t BToDstPi0Analysis::FitMES(){
//define cuts
Float_t mesmin=BCPDGMass-.009;
Float_t mesmax=BCPDGMass+.009;
if(!OpenReducedNtuple())return 0;
TH1F HmES("HmES","",40,5.20,5.30);
ReducedNtuple->Draw("bmes>>HmES");
TH1F HmESN("HmESN","",40,5.20,5.30);
ReducedNtuple->Draw("bmes>>HmESN","bcharge==-1");
TH1F HmESP("HmESP","",40,5.20,5.30);
ReducedNtuple->Draw("bmes>>HmESP","bcharge==1");
filename=_OutputDir+"/MES.ps";
Canvas.Print(filename+"[");
Canvas.Clear();
HmES.Draw("pe");
Canvas.Print(filename);
Canvas.Clear();
HmESN.Draw("pe");
Canvas.Print(filename);
Canvas.Clear();
HmESP.Draw("pe");
Canvas.Print(filename);
mass.setRange(HmES.GetXaxis()->GetXmin(),HmES.GetXaxis()->GetXmax());
mass.setBins(HmES.GetXaxis()->GetNbins());
RooRealVar bm0("bm0","bm0",5.29);
RooRealVar bc("bc","bc",-100,-10);
RooRealVar bp("bp","bp",.1,1);
RooArgusBG Bkg("Bkg","ArgusP",mass,bm0,bc,bp);
///-------------------------------Fit The RS
RooDataHist dataset("dataset","dataset",RooArgList(mass),&HmES,1);
RooDataHist datasetn("datasetn","datasetn",RooArgList(mass),&HmESN,1);
RooDataHist datasetp("datasetp","datasetp",RooArgList(mass),&HmESP,1);
//--------------------------------------------------------------------
//Fit RS data with Argus function
//--------------------------------------------------------------------
Bkg.fitTo(dataset,"m");
RooPlot*plot=mass.frame();
dataset.plotOn(plot);
Bkg.plotOn(plot);
Canvas.Clear();
plot->Draw();
Canvas.Print(filename);
delete plot;
//--------------------------------------------------------------------
//Fit RS data Bkg + CBShape
//--------------------------------------------------------------------
// RooHistPdf HBkg("HBkg","HBP",RooArgSet(mass),HB,0);
// RooRealVar Bsl0("Bsl0","Bsl0",HmES.GetXaxis()->GetXmin());
// RooRealVar Bsl("Bsl","Bsl",-100,100);
// RooGenericPdf BslP("BslP","BslP","1+Bsl*(mass-Bsl0)",RooArgList(Bsl0,Bsl,mass));
// RooProdPdf BkgProd("BkgProd","BkgProd",BslP,HBkg);
RooRealVar sm0("sm0","sm0",5.2794);
RooRealVar sigma("sigma","sigma",.003);
RooRealVar alpha("alpha","alpha",3.);
RooRealVar n("n","n",1,10);//1);
//RooCBShape CBP("CBP","CBP",mass,sm0,sigma,alpha,n);
RooGaussian CBP("CBP","CBP",mass,sm0,sigma);
RooRealVar SYield("SYield","SYield",.01,.9);
RooAddPdf FitPdf("FitPdf","FitPdf",RooArgList(CBP,Bkg),RooArgList(SYield));
mass.SetTitle("m_{ES}");
mass.setUnit("GeV^{2}/c^{4}");
///Fit The Total
FitPdf.fitTo(dataset,"m");
plot=mass.frame();
dataset.plotOn(plot,MarkerColor(0),LineColor(0));
FitPdf.plotOn(plot);
FitPdf.plotOn(plot,Components(Bkg),LineColor(kRed));
Canvas.Clear();
plot->Draw();
HmES.SetLineColor(1);HmES.SetStats(0);
HmES.Draw("same");
cutline.DrawLine(mesmin,0,mesmin,HmES.GetMaximum());
cutline.DrawLine(mesmax,0,mesmax,HmES.GetMaximum());
Canvas.Print(filename);
delete plot;
// //Calculate total signal yield
// Int_t Syieldtot=(int)(SYield.getVal()*HmES.Integral());
// Int_t Syieldtote=(int)(SYield.getError()*HmES.Integral());
// Int_t Byieldtot=(int)((1-SYield.getVal())*HmES.Integral());
// Int_t Byieldtote=(int)((SYield.getError())*HmES.Integral());
//Calculate signal yield in cut
mass.setRange("sigregion",mesmin,mesmax);
RooArgSet nset(mass);
RooAbsReal* sintegral=CBP.createIntegral(nset,nset,"sigregion");
Int_t syieldtot=(int)(HmES.Integral()*sintegral->getVal()*SYield.getVal());
RooAbsReal* bintegral=Bkg.createIntegral(nset,nset,"sigregion");
Int_t byieldtot=(int)(HmES.Integral()*bintegral->getVal()*(1-SYield.getVal()));
cout<<"Yield Results in sig region TotSig="<<syieldtot<<endl;
cout<<"Yield Results in sig region TotBkg="<<byieldtot<<endl;
/*
///Now fix the shape parameters
sm0.setConstant(1);
sigma.setConstant(1);
alpha.setConstant(1);
n.setConstant(1);
////Fit the B0
FitPdf.fitTo(datasetn,"m");
RooPlot*plotn=mass.frame();
datasetn.plotOn(plotn,MarkerColor(0),LineColor(0));
//FitPdf.plotOn(plotn,LineStyle(1),LineColor(2));
FitPdf.plotOn(plotn,Components(BkgProd),LineColor(kBlack));
//Calculate total signal yield
Int_t Syieldn=(int)(SYield.getVal()*HmESN.Integral());
Int_t Syieldne=(int)(SYield.getError()*HmESN.Integral());
Int_t Byieldn=(int)((1-SYield.getVal())*HmESN.Integral());
Int_t Byieldne=(int)((SYield.getError())*HmESN.Integral());
//Calculate signal yield in cut
Int_t syieldn=(int)(HmESN.Integral()*sintegral->getVal()*SYield.getVal());
Int_t byieldn=(int)(HmESN.Integral()*bintegral->getVal()*(1-SYield.getVal()));
////Fit the B0bar
FitPdf.fitTo(datasetp,"m");
RooPlot*plotp=mass.frame();
datasetp.plotOn(plotp,MarkerColor(0),LineColor(0));
//FitPdf.plotOn(plotp,LineStyle(1),LineColor(3));
FitPdf.plotOn(plotp,Components(BkgProd),LineColor(kBlack));
//Calculate total signal yield
//Calculate total signal yield
Int_t Syieldp=(int)(SYield.getVal()*HmESP.Integral());
Int_t Syieldpe=(int)(SYield.getError()*HmESP.Integral());
Int_t Byieldp=(int)((1-SYield.getVal())*HmESP.Integral());
Int_t Byieldpe=(int)((SYield.getError())*HmESP.Integral());
//Calculate signal yield in cut
Int_t syieldp=(int)(HmESP.Integral()*sintegral->getVal()*SYield.getVal());
Int_t byieldp=(int)(HmESP.Integral()*bintegral->getVal()*(1-SYield.getVal()));
Canvas.Clear();
plotn->Draw();
HmESN.SetLineColor(2);HmESN.SetStats(0);
HmESN.Draw("same");
plotp->Draw("same");
HmESP.SetLineColor(3);HmESP.SetStats(0);
HmESP.Draw("same");
Canvas.Print(filename);
delete plotn;
delete plotp;
Float_t diff=Syieldn-Syieldp;
Float_t diffe=sqrt((float)Syieldne*Syieldne+(float)Syieldpe*Syieldpe);
Float_t asym=diff/Syieldtot;
Float_t asyme=asym*sqrt(diffe*diffe/((float)diff*diff)+Syieldtote*Syieldtote/((float)Syieldtot*Syieldtot));
cout<<"Yield Results TotSig="<<Syieldtot<<"+-"<<Syieldtote
<<", SigN="<<Syieldn<<"+-"<<Syieldne
<<", SigP="<<Syieldp<<"+-"<<Syieldpe
<<" Asym=(SigN-SigP)/(SigN+SigP)="<<asym<<"+-"<<asyme
<<endl<<endl;
cout<<"Yield Results TotBkg="<<Byieldtot<<"+-"<<Byieldtote<<", BkgN="<<Byieldn<<"+-"<<Byieldne<<", BkgP="<<Byieldp<<"+-"<<Byieldpe<<endl<<endl;
cout<<"Yield Results in sig region TotSig="<<syieldtot<<", SigN="<<syieldn<<", SigP="<<syieldp<<endl;
cout<<"Yield Results in sig region TotBkg="<<byieldtot<<", BkgN="<<byieldn<<", BkgP="<<byieldp<<endl;
///---------------------------------------------------------------
////Plot Difference of the histograms:
TH1F*HDiff=(TH1F*)HmESN.Clone();
HDiff->SetName("HDiff");HDiff->SetTitle("Difference");
HDiff->Sumw2();
HDiff->Add(&HmESP,-1);
Canvas.Clear();
HDiff->SetStats(1);
HDiff->SetLineColor(1);
SetHistoXYLabels(HDiff,"m_{ES}","(GeV^{2}/c^{4})");
HDiff->Draw("pe");
Canvas.Update();
MoveStatsBox(HDiff,-1,1);
cutline.DrawLine(HmESN.GetXaxis()->GetXmin(),0,HmESN.GetXaxis()->GetXmax(),0);
Canvas.Update();
Canvas.Print(filename);
cout<<" Asym="<<(float)(HDiff->Integral())/HmES.Integral(28,36)<<endl;
delete HDiff;
*/
Canvas.Print(filename+"]");
return 1;
}
/* calculates DCT 8*8 but this time
using a the 3*3 values of the matrix as the main frequency
components, discards all others.
*/
void CKingimageView::OnAssignment310()
{
CKingimageDoc* pDoc = GetDocument();
int iBitPerPixel = pDoc->_bmp->bitsperpixel;
int iWidth = pDoc->_bmp->width;
int iHeight = pDoc->_bmp->height;
BYTE *pImg = pDoc->_bmp->point;
int red, blue, green, all;
if (iWidth%8 != 0) iWidth = iWidth -iWidth%8;
if (iHeight%8 != 0) iHeight = iHeight -iHeight%8;
if(iBitPerPixel == 8) ////Grey scale 8 bits image
{
for(int i=0; i<iHeight; i++)
for(int j=0; j<iWidth; j++) pImg[i*iWidth+j] = 255;
}
if(iBitPerPixel == 24) ////True color 24bits image
{
//reseting the dct array to 0
for (int u=0; u<8; ++u)
for (int v=0; v<8; ++v) dct[u][v] = 0;
for(int i=0; i<iHeight; i++)
for(int j=0; j<iWidth; j++)
{
blue = pImg[i*iWidth*3+j*3]; //B
green = pImg[i*iWidth*3+j*3+1]; //G
red = pImg[i*iWidth*3+j*3+2]; //R
all = red+green+blue;
//intensity array
if (all != 0) intense[i][j] = all/3;
}
for(int i=0; i<iHeight-7; i+=8)
for(int j=0; j<iWidth-7; j+=8)
{
for (int x=0; x<3; ++x)
for (int y=0; y<3; ++y)
{
dct[x][y] = 0;
for (int u=0; u<8; ++u)
for (int v=0; v<8; ++v)
{
dct[x][y] += intense[i+u][j+v]*cosine[x][u]*cosine[y][v];
}
dct[x][y] = alpha(x)*alpha(y)*dct[x][y];
}
for (int u=0; u<8; ++u)
for (int v=0; v<8; ++v)
{
for (int x=0; x<8; ++x)
for (int y=0; y<8; ++y)
{
intense[i+u][j+v] += alpha(x)*alpha(y)*dct[x][y]*cosine[x][u]*cosine[y][v];
}
if (intense[i+u][j+v] > 255) intense[i+u][j+v] = 255;
if (intense[i+u][j+v] < 0) intense[i+u][j+v]= 0;
}
for(int x=i; x<i+8; ++x)
for(int y=j; y<j+8; ++y)
{
pImg[x*iWidth*3+y*3] = intense[x][y]; //B
pImg[x*iWidth*3+y*3+1] = intense[x][y]; //G
pImg[x*iWidth*3+y*3+2] = intense[x][y]; //R
}
}
}
////redraw the screen
OnDraw(GetDC());
for(int i=0; i<iHeight; i++)
for(int j=0; j<iWidth; j++)
intense[i][j] = 0;
}
void filmPyrolysisRadiativeCoupledMixedFvPatchScalarField::updateCoeffs()
{
if (updated())
{
return;
}
// Get the coupling information from the mappedPatchBase
const mappedPatchBase& mpp =
refCast<const mappedPatchBase>(patch().patch());
const label patchI = patch().index();
const label nbrPatchI = mpp.samplePolyPatch().index();
const polyMesh& mesh = patch().boundaryMesh().mesh();
const polyMesh& nbrMesh = mpp.sampleMesh();
const fvPatch& nbrPatch =
refCast<const fvMesh>(nbrMesh).boundary()[nbrPatchI];
scalarField intFld(patchInternalField());
const filmPyrolysisRadiativeCoupledMixedFvPatchScalarField&
nbrField =
refCast
<
const filmPyrolysisRadiativeCoupledMixedFvPatchScalarField
>
(
nbrPatch.lookupPatchField<volScalarField, scalar>(TnbrName_)
);
// Swap to obtain full local values of neighbour internal field
scalarField nbrIntFld(nbrField.patchInternalField());
mpp.distribute(nbrIntFld);
scalarField& Tp = *this;
const scalarField K(this->kappa(*this));
const scalarField nbrK(nbrField.kappa(*this));
// Swap to obtain full local values of neighbour K*delta
scalarField KDeltaNbr(nbrK*nbrPatch.deltaCoeffs());
mpp.distribute(KDeltaNbr);
scalarField myKDelta(K*patch().deltaCoeffs());
scalarList Tfilm(patch().size(), 0.0);
scalarList htcwfilm(patch().size(), 0.0);
scalarList filmDelta(patch().size(), 0.0);
const pyrolysisModelType& pyrolysis = pyrModel();
const filmModelType& film = filmModel();
// Obtain Rad heat (Qr)
scalarField Qr(patch().size(), 0.0);
label coupledPatchI = -1;
if (pyrolysisRegionName_ == mesh.name())
{
coupledPatchI = patchI;
if (QrName_ != "none")
{
Qr = nbrPatch.lookupPatchField<volScalarField, scalar>(QrName_);
mpp.distribute(Qr);
}
}
else if (pyrolysis.primaryMesh().name() == mesh.name())
{
coupledPatchI = nbrPatch.index();
if (QrName_ != "none")
{
Qr = patch().lookupPatchField<volScalarField, scalar>(QrName_);
}
}
else
{
FatalErrorIn
(
"void filmPyrolysisRadiativeCoupledMixedFvPatchScalarField::"
"updateCoeffs()"
)
<< type() << " condition is intended to be applied to either the "
<< "primary or pyrolysis regions only"
<< exit(FatalError);
}
const label filmPatchI = pyrolysis.nbrCoupledPatchID(film, coupledPatchI);
const scalarField htcw(film.htcw().h()().boundaryField()[filmPatchI]);
// Obtain htcw
htcwfilm =
pyrolysis.mapRegionPatchField
(
film,
coupledPatchI,
filmPatchI,
htcw,
true
);
// Obtain Tfilm at the boundary through Ts.
// NOTE: Tf is not good as at the boundary it will retrieve Tp
Tfilm = film.Ts().boundaryField()[filmPatchI];
film.toPrimary(filmPatchI, Tfilm);
// Obtain delta
filmDelta =
pyrolysis.mapRegionPatchField<scalar>
(
film,
"deltaf",
coupledPatchI,
true
);
// Estimate wetness of the film (1: wet , 0: dry)
scalarField ratio
(
min
(
max
(
(filmDelta - filmDeltaDry_)/(filmDeltaWet_ - filmDeltaDry_),
scalar(0.0)
),
scalar(1.0)
)
);
scalarField qConv(ratio*htcwfilm*(Tfilm - Tp)*convectiveScaling_);
scalarField qRad((1.0 - ratio)*Qr);
scalarField alpha(KDeltaNbr - (qRad + qConv)/Tp);
valueFraction() = alpha/(alpha + (1.0 - ratio)*myKDelta);
refValue() = ratio*Tfilm + (1.0 - ratio)*(KDeltaNbr*nbrIntFld)/alpha;
mixedFvPatchScalarField::updateCoeffs();
if (debug)
{
scalar Qc = gSum(qConv*patch().magSf());
scalar Qr = gSum(qRad*patch().magSf());
scalar Qt = gSum((qConv + qRad)*patch().magSf());
Info<< mesh.name() << ':'
<< patch().name() << ':'
<< this->dimensionedInternalField().name() << " <- "
<< nbrMesh.name() << ':'
<< nbrPatch.name() << ':'
<< this->dimensionedInternalField().name() << " :" << nl
<< " convective heat[W] : " << Qc << nl
<< " radiative heat [W] : " << Qr << nl
<< " total heat [W] : " << Qt << nl
<< " wall temperature "
<< " min:" << gMin(*this)
<< " max:" << gMax(*this)
<< " avg:" << gAverage(*this)
<< endl;
}
}
void bridge_regression(double *betap,
double *up,
double *omegap,
double *shapep,
double *sig2p,
double *taup,
double *alphap,
const double *yp,
const double *Xp,
const double *sig2_shape,
const double *sig2_scale,
const double *nu_shape,
const double *nu_rate,
const double *alpha_a,
const double *alpha_b,
const double *true_sig2,
const double *true_tau,
const double *true_alpha,
const int *P,
const int *N,
const int *M,
const int *burn,
double *runtime,
const bool *ortho,
const int *betaburn,
const bool *use_hmc)
{
Matrix y(yp, *N, 1);
Matrix X(Xp, *N, *P);
Matrix beta(*P, 1, *M);
Matrix u (*P, 1, *M, 0.0);
Matrix omega(*P, 1, *M, 1.0);
Matrix shape(*P, 1, *M, 0.0);
Matrix sig2( 1, 1, *M);
Matrix tau ( 1, 1, *M);
Matrix alpha(1, 1, *M);
#ifdef USE_R
GetRNGstate();
#endif
if (!*ortho) {
*runtime = bridge_regression(beta, u, omega, shape, sig2, tau, alpha, y, X,
*sig2_shape, *sig2_scale,
*nu_shape, *nu_rate,
*alpha_a, *alpha_b,
*true_sig2, *true_tau, *true_alpha,
*burn, *betaburn, *use_hmc);
}
else {
*runtime = bridge_regression_ortho(beta, u, omega, shape, sig2, tau, alpha,
y, X,
*sig2_shape, *sig2_scale,
*nu_shape, *nu_rate,
*alpha_a, *alpha_b,
*true_sig2, *true_tau, *true_alpha,
*burn);
}
#ifdef USE_R
PutRNGstate();
#endif
MatrixFrame beta_mf (betap, *P, 1 , *M);
MatrixFrame u_mf (up , *P, 1, *M);
MatrixFrame omega_mf(omegap, *P, 1, *M);
MatrixFrame shape_mf(shapep, *P, 1, *M);
MatrixFrame sig2_mf (sig2p, 1 , 1 , *M);
MatrixFrame tau_mf (taup , 1 , 1 , *M);
MatrixFrame alpha_mf(alphap, 1, 1, *M);
beta_mf.copy(beta);
u_mf.copy(u);
omega_mf.copy(omega);
shape_mf.copy(shape);
sig2_mf.copy(sig2);
tau_mf.copy (tau );
alpha_mf.copy(alpha);
} // bridge_regression
static int runFusibile (int argc,
char **argv,
AlgorithmParameters &algParameters
)
{
InputFiles inputFiles;
string ext = ".png";
string results_folder = "results/";
const char* results_folder_opt = "-input_folder";
const char* p_input_folder_opt = "-p_folder";
const char* krt_file_opt = "-krt_file";
const char* images_input_folder_opt = "-images_folder";
const char* gt_opt = "-gt";
const char* gt_nocc_opt = "-gt_nocc";
const char* pmvs_folder_opt = "--pmvs_folder";
const char* remove_black_background_opt = "-remove_black_background";
//read in arguments
for ( int i = 1; i < argc; i++ )
{
if ( strcmp ( argv[i], results_folder_opt ) == 0 ){
results_folder = argv[++i];
cout << "input folder is " << results_folder << endl;
}else if ( strcmp ( argv[i], p_input_folder_opt ) == 0 ){
inputFiles.p_folder = argv[++i];
}
else if ( strcmp ( argv[i], krt_file_opt ) == 0 )
inputFiles.krt_file = argv[++i];
else if ( strcmp ( argv[i], images_input_folder_opt ) == 0 ){
inputFiles.images_folder = argv[++i];
}else if ( strcmp ( argv[i], gt_opt ) == 0 ){
inputFiles.gt_filename = argv[++i];
}else if ( strcmp ( argv[i], gt_nocc_opt ) == 0 ){
inputFiles.gt_nocc_filename = argv[++i];
}
else if ( strncmp ( argv[i], pmvs_folder_opt, strlen ( pmvs_folder_opt ) ) == 0 ) {
inputFiles.pmvs_folder = argv[++i];
}
else if ( strcmp ( argv[i], remove_black_background_opt ) == 0 )
algParameters.remove_black_background = true;
}
if (inputFiles.pmvs_folder.size()>0) {
inputFiles.images_folder = inputFiles.pmvs_folder + "/visualize/";
inputFiles.p_folder = inputFiles.pmvs_folder + "/txt/";
}
cout <<"image folder is " << inputFiles.images_folder << endl;
cout <<"p folder is " << inputFiles.images_folder << endl;
cout <<"pmvs folder is " << inputFiles.pmvs_folder << endl;
GTcheckParameters gtParameters;
gtParameters.dispTolGT = 0.1f;
gtParameters.dispTolGT2 = 0.02f;
gtParameters.divFactor = 1.0f;
// create folder to store result images
time_t timeObj;
time ( &timeObj );
tm *pTime = localtime ( &timeObj );
vector <Mat_<Vec3f> > view_vectors;
time(&timeObj);
pTime = localtime(&timeObj);
char output_folder[256];
sprintf(output_folder, "%s/consistencyCheck-%04d%02d%02d-%02d%02d%02d/",results_folder.c_str(), pTime->tm_year+1900, pTime->tm_mon+1,pTime->tm_mday,pTime->tm_hour, pTime->tm_min, pTime->tm_sec);
#if defined(_WIN32)
_mkdir(output_folder);
#else
mkdir(output_folder, 0777);
#endif
vector<string> subfolders;
get_subfolders(results_folder.c_str(), subfolders);
std::sort(subfolders.begin(), subfolders.end());
vector< Mat_<Vec3b> > warpedImages;
vector< Mat_<Vec3b> > warpedImages_inverse;
//vector< Mat_<float> > depthMaps;
vector< Mat_<float> > updateMaps;
vector< Mat_<Vec3f> > updateNormals;
vector< Mat_<float> > depthMapConsistent;
vector< Mat_<Vec3f> > normalsConsistent;
vector< Mat_<Vec3f> > groundTruthNormals;
vector< Mat_<uint8_t> > valid;
map< int,string> consideredIds;
for(size_t i=0;i<subfolders.size();i++) {
//make sure that it has the right format (DATE_TIME_INDEX)
size_t n = std::count(subfolders[i].begin(), subfolders[i].end(), '_');
if(n < 2)
continue;
if (subfolders[i][0] != '2')
continue;
//get index
//unsigned found = subfolders[i].find_last_of("_");
//find second index
unsigned posFirst = subfolders[i].find_first_of("_") +1;
unsigned found = subfolders[i].substr(posFirst).find_first_of("_") + posFirst +1;
string id_string = subfolders[i].substr(found);
//InputData dat;
//consideredIds.push_back(id_string);
consideredIds.insert(pair<int,string>(i,id_string));
//cout << "id_string is " << id_string << endl;
//cout << "i is " << i << endl;
//char outputPath[256];
//sprintf(outputPath, "%s.png", id_string);
if( access( (inputFiles.images_folder + id_string + ".png").c_str(), R_OK ) != -1 )
inputFiles.img_filenames.push_back((id_string + ".png"));
else if( access( (inputFiles.images_folder + id_string + ".jpg").c_str(), R_OK ) != -1 )
inputFiles.img_filenames.push_back((id_string + ".jpg"));
else if( access( (inputFiles.images_folder + id_string + ".ppm").c_str(), R_OK ) != -1 )
inputFiles.img_filenames.push_back((id_string + ".ppm"));
}
size_t numImages = inputFiles.img_filenames.size ();
cout << "numImages is " << numImages << endl;
cout << "img_filenames is " << inputFiles.img_filenames.size() << endl;
algParameters.num_img_processed = min ( ( int ) numImages, algParameters.num_img_processed );
vector<Mat_<Vec3b> > img_color; // imgLeft_color, imgRight_color;
vector<Mat_<uint8_t> > img_grayscale;
for ( size_t i = 0; i < numImages; i++ ) {
//printf ( "Opening image %ld: %s\n", i, ( inputFiles.images_folder + inputFiles.img_filenames[i] ).c_str () );
img_grayscale.push_back ( imread ( ( inputFiles.images_folder + inputFiles.img_filenames[i] ), IMREAD_GRAYSCALE ) );
if ( algParameters.color_processing ) {
img_color.push_back ( imread ( ( inputFiles.images_folder + inputFiles.img_filenames[i] ), IMREAD_COLOR ) );
}
if ( img_grayscale[i].rows == 0 ) {
printf ( "Image seems to be invalid\n" );
return -1;
}
}
size_t avail;
size_t total;
cudaMemGetInfo( &avail, &total );
size_t used = total - avail;
printf("Device memory used: %fMB\n", used/1000000.0f);
GlobalState *gs = new GlobalState;
gs->cameras = new CameraParameters_cu;
gs->pc = new PointCloud;
cudaMemGetInfo( &avail, &total );
used = total - avail;
printf("Device memory used: %fMB\n", used/1000000.0f);
uint32_t rows = img_grayscale[0].rows;
uint32_t cols = img_grayscale[0].cols;
CameraParameters camParams = getCameraParameters (*(gs->cameras),
inputFiles,algParameters.depthMin,
algParameters.depthMax,
algParameters.cam_scale,
false);
printf("Camera size is %lu\n", camParams.cameras.size());
for ( int i = 0; i < algParameters.num_img_processed; i++ ) {
algParameters.min_disparity = disparityDepthConversion ( camParams.f, camParams.cameras[i].baseline, camParams.cameras[i].depthMax );
algParameters.max_disparity = disparityDepthConversion ( camParams.f, camParams.cameras[i].baseline, camParams.cameras[i].depthMin );
}
selectViews ( camParams, cols, rows, false);
int numSelViews = camParams.viewSelectionSubset.size ();
cout << "Selected views: " << numSelViews << endl;
gs->cameras->viewSelectionSubsetNumber = numSelViews;
ofstream myfile;
for ( int i = 0; i < numSelViews; i++ ) {
cout << camParams.viewSelectionSubset[i] << ", ";
gs->cameras->viewSelectionSubset[i] = camParams.viewSelectionSubset[i];
}
cout << endl;
vector<InputData> inputData;
cout << "Reading normals and depth from disk" << endl;
cout << "Size consideredIds is " << consideredIds.size() << endl;
for (map<int,string>::iterator it=consideredIds.begin(); it!=consideredIds.end(); ++it){
//get corresponding camera
int i = it->first;
string id = it->second;//consideredIds[i];
//int id = atoi(id_string.c_str());
int camIdx = getCameraFromId(id,camParams.cameras);
//cout << "id is " << id << endl;
//cout << "camIdx is " << camIdx << endl;
if(camIdx < 0)// || camIdx == camParams.idRef)
continue;
InputData dat;
dat.id = id;
dat.camId = camIdx;
dat.cam = camParams.cameras[camIdx];
dat.path = results_folder + subfolders[i];
dat.inputImage = imread((inputFiles.images_folder + id + ext), IMREAD_COLOR);
//read normal
cout << "Reading normal " << i << endl;
readDmbNormal((dat.path + "/normals.dmb").c_str(),dat.normals);
//read depth
cout << "Reading disp " << i << endl;
readDmb((dat.path + "/disp.dmb").c_str(),dat.depthMap);
//inputData.push_back(move(dat));
inputData.push_back(dat);
}
// run gpu run
// Init parameters
gs->params = &algParameters;
// Init ImageInfo
//gs->iminfo.cols = img_grayscale[0].cols;
//gs->iminfo.rows = img_grayscale[0].rows;
gs->cameras->cols = img_grayscale[0].cols;
gs->cameras->rows = img_grayscale[0].rows;
gs->params->cols = img_grayscale[0].cols;
gs->params->rows = img_grayscale[0].rows;
gs->resize (img_grayscale.size());
gs->pc->resize (img_grayscale[0].rows * img_grayscale[0].cols);
PointCloudList pc_list;
pc_list.resize (img_grayscale[0].rows * img_grayscale[0].cols);
pc_list.size=0;
pc_list.rows = img_grayscale[0].rows;
pc_list.cols = img_grayscale[0].cols;
gs->pc->rows = img_grayscale[0].rows;
gs->pc->cols = img_grayscale[0].cols;
// Resize lines
for (size_t i = 0; i<img_grayscale.size(); i++)
{
gs->lines[i].resize(img_grayscale[0].rows * img_grayscale[0].cols);
gs->lines[i].n = img_grayscale[0].rows * img_grayscale[0].cols;
//gs->lines.s = img_grayscale[0].step[0];
gs->lines[i].s = img_grayscale[0].cols;
gs->lines[i].l = img_grayscale[0].cols;
}
vector<Mat > img_grayscale_float (img_grayscale.size());
vector<Mat > img_color_float (img_grayscale.size());
vector<Mat > img_color_float_alpha (img_grayscale.size());
vector<Mat > normals_and_depth (img_grayscale.size());
vector<Mat_<uint16_t> > img_grayscale_uint (img_grayscale.size());
for (size_t i = 0; i<img_grayscale.size(); i++)
{
//img_grayscale[i].convertTo(img_grayscale_float[i], CV_32FC1, 1.0/255.0); // or CV_32F works (too)
img_grayscale[i].convertTo(img_grayscale_float[i], CV_32FC1); // or CV_32F works (too)
img_grayscale[i].convertTo(img_grayscale_uint[i], CV_16UC1); // or CV_32F works (too)
if(algParameters.color_processing) {
vector<Mat_<float> > rgbChannels ( 3 );
img_color_float_alpha[i] = Mat::zeros ( img_grayscale[0].rows, img_grayscale[0].cols, CV_32FC4 );
img_color[i].convertTo (img_color_float[i], CV_32FC3); // or CV_32F works (too)
Mat alpha( img_grayscale[0].rows, img_grayscale[0].cols, CV_32FC1 );
split (img_color_float[i], rgbChannels);
rgbChannels.push_back( alpha);
merge (rgbChannels, img_color_float_alpha[i]);
}
/* Create vector of normals and disparities */
vector<Mat_<float> > normal ( 3 );
normals_and_depth[i] = Mat::zeros ( img_grayscale[0].rows, img_grayscale[0].cols, CV_32FC4 );
split (inputData[i].normals, normal);
normal.push_back( inputData[i].depthMap);
merge (normal, normals_and_depth[i]);
}
//int64_t t = getTickCount ();
// Copy images to texture memory
if (algParameters.saveTexture) {
if (algParameters.color_processing)
addImageToTextureFloatColor (img_color_float_alpha, gs->imgs);
else
addImageToTextureFloatGray (img_grayscale_float, gs->imgs);
}
addImageToTextureFloatColor (normals_and_depth, gs->normals_depths);
#define pow2(x) ((x)*(x))
#define get_pow2_norm(x,y) (pow2(x)+pow2(y))
runcuda(*gs, pc_list, numSelViews);
Mat_<Vec3f> norm0 = Mat::zeros ( img_grayscale[0].rows, img_grayscale[0].cols, CV_32FC3 );
Mat_<float> distImg;
char plyFile[256];
sprintf ( plyFile, "%s/final3d_model.ply", output_folder);
printf("Writing ply file %s\n", plyFile);
//storePlyFileAsciiPointCloud ( plyFile, pc_list, inputData[0].cam, distImg);
storePlyFileBinaryPointCloud ( plyFile, pc_list, distImg);
char xyzFile[256];
sprintf ( xyzFile, "%s/final3d_model.xyz", output_folder);
printf("Writing ply file %s\n", xyzFile);
//storeXYZPointCloud ( xyzFile, pc_list, inputData[0].cam, distImg);
return 0;
}
List objective(const arma::mat& transition, NumericVector emissionArray,
const arma::vec& init, IntegerVector obsArray, const arma::imat& ANZ,
IntegerVector emissNZ, const arma::ivec& INZ, const arma::ivec& nSymbols, int threads) {
IntegerVector eDims = emissionArray.attr("dim"); //m,p,r
IntegerVector oDims = obsArray.attr("dim"); //k,n,r
arma::cube emission(emissionArray.begin(), eDims[0], eDims[1], eDims[2], false, true);
arma::icube obs(obsArray.begin(), oDims[0], oDims[1], oDims[2], false, true);
arma::icube BNZ(emissNZ.begin(), emission.n_rows, emission.n_cols - 1, emission.n_slices, false, true);
arma::vec grad(arma::accu(ANZ) + arma::accu(BNZ) + arma::accu(INZ), arma::fill::zeros);
// arma::cube alpha(emission.n_rows, obs.n_cols, obs.n_slices); //m,n,k
// arma::cube beta(emission.n_rows, obs.n_cols, obs.n_slices); //m,n,k
// arma::mat scales(obs.n_cols, obs.n_slices); //m,n,k
//
// internalForward(transition, emission, init, obs, alpha, scales, threads);
// if (!scales.is_finite()) {
// grad.fill(-arma::math::inf());
// return List::create(Named("objective") = arma::math::inf(), Named("gradient") = wrap(grad));
// }
//
// internalBackward(transition, emission, obs, beta, scales, threads);
// if (!beta.is_finite()) {
// grad.fill(-arma::math::inf());
// return List::create(Named("objective") = arma::math::inf(), Named("gradient") = wrap(grad));
// }
//use this instead of local vectors with grad += grad_k;, uses more memory but gives bit-identical results
//arma::mat gradmat(arma::accu(ANZ) + arma::accu(BNZ) + arma::accu(INZ), obs.n_slices);
unsigned int error = 0;
double ll = 0;
#pragma omp parallel for if(obs.n_slices >= threads) schedule(static) reduction(+:ll) num_threads(threads) \
default(none) shared(grad, nSymbols, ANZ, BNZ, INZ, obs, init, transition, emission, error)
for (int k = 0; k < obs.n_slices; k++) {
if (error == 0) {
arma::mat alpha(emission.n_rows, obs.n_cols); //m,n
arma::vec scales(obs.n_cols); //n
arma::sp_mat sp_trans(transition);
uvForward(sp_trans.t(), emission, init, obs.slice(k), alpha, scales);
arma::mat beta(emission.n_rows, obs.n_cols); //m,n
uvBackward(sp_trans, emission, obs.slice(k), beta, scales);
int countgrad = 0;
arma::vec grad_k(grad.n_elem, arma::fill::zeros);
// transitionMatrix
arma::vec gradArow(emission.n_rows);
arma::mat gradA(emission.n_rows, emission.n_rows);
for (unsigned int i = 0; i < emission.n_rows; i++) {
arma::uvec ind = arma::find(ANZ.row(i));
if (ind.n_elem > 0) {
gradArow.zeros();
gradA.eye();
gradA.each_row() -= transition.row(i);
gradA.each_col() %= transition.row(i).t();
for (unsigned int t = 0; t < (obs.n_cols - 1); t++) {
for (unsigned int j = 0; j < emission.n_rows; j++) {
double tmp = 1.0;
for (unsigned int r = 0; r < obs.n_rows; r++) {
tmp *= emission(j, obs(r, t + 1, k), r);
}
gradArow(j) += alpha(i, t) * tmp * beta(j, t + 1) / scales(t + 1);
}
}
gradArow = gradA * gradArow;
grad_k.subvec(countgrad, countgrad + ind.n_elem - 1) = gradArow.rows(ind);
countgrad += ind.n_elem;
}
}
// emissionMatrix
for (unsigned int r = 0; r < obs.n_rows; r++) {
arma::vec gradBrow(nSymbols(r));
arma::mat gradB(nSymbols(r), nSymbols(r));
for (unsigned int i = 0; i < emission.n_rows; i++) {
arma::uvec ind = arma::find(BNZ.slice(r).row(i));
if (ind.n_elem > 0) {
gradBrow.zeros();
gradB.eye();
gradB.each_row() -= emission.slice(r).row(i).subvec(0, nSymbols(r) - 1);
gradB.each_col() %= emission.slice(r).row(i).subvec(0, nSymbols(r) - 1).t();
for (int j = 0; j < nSymbols(r); j++) {
if (obs(r, 0, k) == j) {
double tmp = 1.0;
for (unsigned int r2 = 0; r2 < obs.n_rows; r2++) {
if (r2 != r) {
tmp *= emission(i, obs(r2, 0, k), r2);
}
}
gradBrow(j) += init(i) * tmp * beta(i, 0) / scales(0);
}
for (unsigned int t = 0; t < (obs.n_cols - 1); t++) {
if (obs(r, t + 1, k) == j) {
double tmp = 1.0;
for (unsigned int r2 = 0; r2 < obs.n_rows; r2++) {
if (r2 != r) {
tmp *= emission(i, obs(r2, t + 1, k), r2);
}
}
gradBrow(j) += arma::dot(alpha.col(t), transition.col(i)) * tmp
* beta(i, t + 1) / scales(t + 1);
}
}
}
gradBrow = gradB * gradBrow;
grad_k.subvec(countgrad, countgrad + ind.n_elem - 1) = gradBrow.rows(ind);
countgrad += ind.n_elem;
}
}
}
// InitProbs
arma::uvec ind = arma::find(INZ);
if (ind.n_elem > 0) {
arma::vec gradIrow(emission.n_rows);
arma::mat gradI(emission.n_rows, emission.n_rows);
gradIrow.zeros();
gradI.zeros();
gradI.eye();
gradI.each_row() -= init.t();
gradI.each_col() %= init;
for (unsigned int j = 0; j < emission.n_rows; j++) {
double tmp = 1.0;
for (unsigned int r = 0; r < obs.n_rows; r++) {
tmp *= emission(j, obs(r, 0, k), r);
}
gradIrow(j) += tmp * beta(j, 0) / scales(0);
}
gradIrow = gradI * gradIrow;
grad_k.subvec(countgrad, countgrad + ind.n_elem - 1) = gradIrow.rows(ind);
countgrad += ind.n_elem;
}
if (!scales.is_finite() || !beta.is_finite()) {
#pragma omp atomic
error++;
} else {
ll += arma::sum(log(scales));
#pragma omp critical
grad += grad_k;
// gradmat.col(k) = grad_k;
}
// for (unsigned int ii = 0; ii < grad_k.n_elem; ii++) {
// #pragma omp atomic
// grad(ii) += grad_k(ii);
// }
}
}
if(error > 0){
ll = -arma::math::inf();
grad.fill(-arma::math::inf());
}
// } else {
// grad = sum(gradmat, 1);
// }
return List::create(Named("objective") = -ll, Named("gradient") = wrap(-grad));
}
void EM (char *filename, char *db_name, char *db_count_name, int SegLen) {
DBM *db_word_prob;
DBM *db_expected_count;
struct FileText *ft = load_File (filename);
struct PhraseTable *pt;
struct ForwardBackward *fb;
char buf[BLKSIZE];
char *sentence;
double expect_count;
double current_count, total_count = 0.0;
int n_rows = ft->n_rows;
int is_old;
// open word_prob and expected_count dbm.
// load corpus into memory
//
db_Open_ReadWrite (db_name, &db_word_prob);
db_Open_ReadWrite (db_count_name, &db_expected_count);
fb = malloc (sizeof(struct ForwardBackward));
// start to compute expected count
//
//
for (int sent=0; sent<n_rows; ++sent) {
sentence = ft->text[sent];
fprintf (stdout, "Sentence: %s\n", sentence);
fb->alpha = alpha (sentence, db_word_prob , SegLen); // get alpha
fb->beta = beta (sentence, db_word_prob , SegLen); // get beta
// phrase <=> word in this program.
// creat phrases uniqueliy
pt = creatPhrase (sentence);
// scan
for (int p = 0; p < pt->n_phrase; p++) {
if (pt->phrases[p].n_token <= SegLen && db_Is_Old_Record (db_word_prob, pt->phrases[p].content)) {
// return P type
current_count = getWordExpectCount (sentence, pt->phrases[p].content, db_word_prob, fb->alpha, fb->beta);
if (db_Is_Old_Record (db_expected_count, pt->phrases[p].content))
{
// return P type
expect_count = getWordProb (db_expected_count, pt->phrases[p].content);
expect_count += current_count;
// if expect_count => inf, given a small value
if ( 1.7e-307 > expect_count)
{
expect_count = 1.7e-307;
}
sprintf (buf, "%f", log(expect_count));
db_Update_String (db_expected_count, pt->phrases[p].content, buf, &is_old);
#ifdef DEBUG
fprintf (stdout, "Update: %f %s, %s\n", expect_count, buf, pt->phrases[p].content);
#endif
}
else
{
expect_count = current_count;
// if expect_count => inf, given a small value
if ( 1.7e-307 > expect_count)
{
expect_count = 1.7e-307;
}
sprintf (buf, "%f", log(expect_count));
db_Update_String (db_expected_count, pt->phrases[p].content, buf, &is_old);
#ifdef DEBUG
fprintf (stdout, "New: %f %s, %s\n", expect_count, buf, pt->phrases[p].content);
#endif
}
total_count += current_count;
}
}
}
db_Close (db_word_prob);
db_Close (db_expected_count);
}
vector<vector<double> > qFinderDMM::getHessian(){
try {
vector<double> alpha(numOTUs, 0.0000);
double alphaSum = 0.0000;
vector<double> pi = zMatrix[currentPartition];
vector<double> psi_ajk(numOTUs, 0.0000);
vector<double> psi_cjk(numOTUs, 0.0000);
vector<double> psi1_ajk(numOTUs, 0.0000);
vector<double> psi1_cjk(numOTUs, 0.0000);
for(int j=0;j<numOTUs;j++){
if (m->control_pressed) { break; }
alpha[j] = exp(lambdaMatrix[currentPartition][j]);
alphaSum += alpha[j];
for(int i=0;i<numSamples;i++){
double X = (double) countMatrix[i][j];
psi_ajk[j] += pi[i] * psi(alpha[j]);
psi1_ajk[j] += pi[i] * psi1(alpha[j]);
psi_cjk[j] += pi[i] * psi(alpha[j] + X);
psi1_cjk[j] += pi[i] * psi1(alpha[j] + X);
}
}
double psi_Ck = 0.0000;
double psi1_Ck = 0.0000;
double weight = 0.0000;
for(int i=0;i<numSamples;i++){
if (m->control_pressed) { break; }
weight += pi[i];
double sum = 0.0000;
for(int j=0;j<numOTUs;j++){ sum += alpha[j] + countMatrix[i][j]; }
psi_Ck += pi[i] * psi(sum);
psi1_Ck += pi[i] * psi1(sum);
}
double psi_Ak = weight * psi(alphaSum);
double psi1_Ak = weight * psi1(alphaSum);
vector<vector<double> > hessian(numOTUs);
for(int i=0;i<numOTUs;i++){ hessian[i].assign(numOTUs, 0.0000); }
for(int i=0;i<numOTUs;i++){
if (m->control_pressed) { break; }
double term1 = -alpha[i] * (- psi_ajk[i] + psi_Ak + psi_cjk[i] - psi_Ck);
double term2 = -alpha[i] * alpha[i] * (-psi1_ajk[i] + psi1_Ak + psi1_cjk[i] - psi1_Ck);
double term3 = 0.1 * alpha[i];
hessian[i][i] = term1 + term2 + term3;
for(int j=0;j<i;j++){
hessian[i][j] = - alpha[i] * alpha[j] * (psi1_Ak - psi1_Ck);
hessian[j][i] = hessian[i][j];
}
}
return hessian;
}
catch(exception& e){
m->errorOut(e, "qFinderDMM", "getHessian");
exit(1);
}
}
void ConvCode::caclAlpha()
{
int i,j;
double tmpd1,tmpd2;
int sl,sr;
int ub=0;
if (_dotailbitting)
{
ub = _k+MU;
}
else
{
ub = _k;
}
for (i = 1; i <= ub; i++)
{
for (j = 0; j < NSTATE; j++)
{
sr = j;
sl = _states[j]._forward[0];
#ifdef DO_QUANTIFICATION
tmpd1 = plus( alpha(i-1,sl) , _stateQuantizer.quantify(gama(i-1,sl,sr)) ) ;
#else
tmpd1 = plus( alpha(i-1,sl) , gama(i-1,sl,sr) ) ;
#endif
sl = _states[j]._forward[1];
#ifdef DO_QUANTIFICATION
tmpd2 = plus( alpha(i-1,sl) , _stateQuantizer.quantify(gama(i-1,sl,sr)) ) ;
#else
tmpd2 = plus( alpha(i-1,sl) , gama(i-1,sl,sr) ) ;
#endif
alpha(i,j) = maxAsterisk(tmpd1,tmpd2);
}
//normalize
double minVal = alpha(i,0);
for (j = 1; j < NSTATE; j++)
{
if (alpha(i,j) < minVal)
{
minVal = alpha(i,j);
}
}
if (minVal < -INF + EPSILON)
{
continue;
}
for (j = 0; j < NSTATE; j++)
{
alpha(i,j) -= minVal;
#ifdef DO_QUANTIFICATION
alpha(i,j) = _stateQuantizer.quantify(alpha(i,j));
#endif
}
}
}
void qFinderDMM::negativeLogDerivEvidenceLambdaPi(vector<double>& x, vector<double>& df){
try{
// cout << "\tstart negativeLogDerivEvidenceLambdaPi" << endl;
vector<double> storeVector(numSamples, 0.0000);
vector<double> derivative(numOTUs, 0.0000);
vector<double> alpha(numOTUs, 0.0000);
double store = 0.0000;
double nu = 0.1000;
double eta = 0.1000;
double weight = 0.0000;
for(int i=0;i<numSamples;i++){
weight += zMatrix[currentPartition][i];
}
for(int i=0;i<numOTUs;i++){
if (m->control_pressed) { return; }
// cout << "start i loop" << endl;
//
// cout << i << '\t' << alpha[i] << '\t' << x[i] << '\t' << exp(x[i]) << '\t' << store << endl;
alpha[i] = exp(x[i]);
store += alpha[i];
// cout << "before derivative" << endl;
derivative[i] = weight * psi(alpha[i]);
// cout << "after derivative" << endl;
// cout << i << '\t' << alpha[i] << '\t' << psi(alpha[i]) << '\t' << derivative[i] << endl;
for(int j=0;j<numSamples;j++){
double X = countMatrix[j][i];
double alphaX = X + alpha[i];
derivative[i] -= zMatrix[currentPartition][j] * psi(alphaX);
storeVector[j] += alphaX;
}
// cout << "end i loop" << endl;
}
double sumStore = 0.0000;
for(int i=0;i<numSamples;i++){
sumStore += zMatrix[currentPartition][i] * psi(storeVector[i]);
}
store = weight * psi(store);
df.resize(numOTUs, 0.0000);
for(int i=0;i<numOTUs;i++){
df[i] = alpha[i] * (nu + derivative[i] - store + sumStore) - eta;
// cout << i << '\t' << df[i] << endl;
}
// cout << df.size() << endl;
// cout << "\tend negativeLogDerivEvidenceLambdaPi" << endl;
}
catch(exception& e){
m->errorOut(e, "qFinderDMM", "negativeLogDerivEvidenceLambdaPi");
exit(1);
}
}
/* 'Data' Matrix of data containing observation data in rows, one
* row per observation and complying with this format:
* x y z P
* Where x,y,z are satellite coordinates in an ECEF system
* and P is pseudorange (corrected as much as possible,
* specially from satellite clock errors), all expresed
* in meters.
*
* 'X' Vector of position solution, in meters. There may be
* another solution, that may be accessed with vector
* "SecondSolution" if "ChooseOne" is set to "false".
*
* Return values:
* 0 Ok
* -1 Not enough good data
* -2 Singular problem
*/
int Bancroft::Compute( Matrix<double>& Data,
Vector<double>& X )
throw(Exception)
{
try
{
int N = Data.rows();
Matrix<double> B(0,4); // Working matrix
// Let's test the input data
if( testInput )
{
double satRadius = 0.0;
// Check each row of B Matrix
for( int i=0; i < N; i++ )
{
// If Data(i,3) -> Pseudorange is NOT between the allowed
// range, then drop line immediately
if( !( (Data(i,3) >= minPRange) && (Data(i,3) <= maxPRange) ) )
{
continue;
}
// Let's compute distance between Earth center and
// satellite position
satRadius = RSS(Data(i,0), Data(i,1) , Data(i,2));
// If satRadius is NOT between the allowed range, then drop
// line immediately
if( !( (satRadius >= minRadius) && (satRadius <= maxRadius) ) )
{
continue;
}
// If everything is ok so far, then extract the good
// data row and add it to working matrix
MatrixRowSlice<double> goodRow(Data,i);
B = B && goodRow;
}
// Let's redefine "N" and check if we have enough data rows
// left in a single step
if( (N = B.rows()) < 4 )
{
return -1; // We need at least 4 data rows
}
} // End of 'if( testInput )...'
else
{
// No input filtering. Working matrix (B) and
// input matrix (Data) are equal
B = Data;
}
Matrix<double> BT=transpose(B);
Matrix<double> BTBI(4,4), M(4,4,0.0);
Vector<double> aux(4), alpha(N), solution1(4), solution2(4);
// Temporary storage for BT*B. It will be inverted later
BTBI = BT * B;
// Let's try to invert BTB matrix
try
{
BTBI = inverseChol( BTBI );
}
catch(...)
{
return -2;
}
// Now, let's compute alpha vector
for( int i=0; i < N; i++ )
{
// First, fill auxiliar vector with corresponding satellite
// position and pseudorange
aux(0) = B(i,0);
aux(1) = B(i,1);
aux(2) = B(i,2);
aux(3) = B(i,3);
alpha(i) = 0.5 * Minkowski(aux, aux);
}
Vector<double> tau(N,1.0), BTBIBTtau(4), BTBIBTalpha(4);
BTBIBTtau = BTBI * BT * tau;
BTBIBTalpha = BTBI * BT * alpha;
// Now, let's find the coeficients of the second order-equation
double a(Minkowski(BTBIBTtau, BTBIBTtau));
double b(2.0 * (Minkowski(BTBIBTtau, BTBIBTalpha) - 1.0));
double c(Minkowski(BTBIBTalpha, BTBIBTalpha));
// Calculate discriminant and exit if negative
double discriminant = b*b - 4.0 * a * c;
if (discriminant < 0.0)
{
return -2;
}
// Find possible DELTA values
double DELTA1 = ( -b + SQRT(discriminant) ) / ( 2.0 * a );
double DELTA2 = ( -b - SQRT(discriminant) ) / ( 2.0 * a );
// We need to define M matrix
M(0,0) = 1.0;
M(1,1) = 1.0;
M(2,2) = 1.0;
M(3,3) = - 1.0;
// Find possible position solutions with their implicit radii
solution1 = M * BTBI * ( BT * DELTA1 * tau + BT * alpha );
double radius1(RSS(solution1(0), solution1(1), solution1(2)));
solution2 = M * BTBI * ( BT * DELTA2 * tau + BT * alpha );
double radius2(RSS(solution2(0), solution2(1), solution2(2)));
// Let's choose the right solution
if ( ChooseOne )
{
if ( ABS(CloseTo-radius1) < ABS(CloseTo-radius2) )
{
X = solution1;
}
else
{
X = solution2;
}
}
else
{
// Both solutions will be reported
X = solution1;
SecondSolution = solution2;
}
return 0;
} // end of first "try"
catch(Exception& e)
{
GPSTK_RETHROW(e);
}
} // end Bancroft::Compute()
bool StageRealizer::SaveNew(const TCHAR* dir, const TCHAR* basename0, Stage& stage, const MoveSequence& seq)
{
std::basic_string< TCHAR > basename;
std::basic_string< TCHAR > stagedir;
{
basename = basename0;
stagedir = GetDirPath(dir, basename0);
DWORD attr = GetFileAttributes(stagedir.c_str());
if (attr == -1) {
;
} else {
const TCHAR* pc;
int n = 0;
for (pc = basename0; *pc != _T('\0'); ++pc);
for (--pc; _T('0') <= *pc && *pc <= _T('9'); --pc) {
n *= 10;
n += (*pc - _T('0'));
}
++pc;
std::basic_string<TCHAR> alpha(basename0, pc-basename0);
for (++n; ; ++n) {
std::basic_ostringstream< TCHAR > o;
o << alpha << n;
basename = o.str();
stagedir = GetDirPath(dir, basename.c_str());
attr = GetFileAttributes(stagedir.c_str());
if (attr == -1) { break; }
}
}
if (attr != -1) {
return false;
}
}
if (! CreateDirectory(stagedir.c_str(), NULL)) {
return false;
}
const TCHAR *moviefile;
{
moviefile = stage.tc_moviepath.c_str();
for (const TCHAR* pc=moviefile; *pc != _T('\0'); ++pc) {
if (*pc == _T('\\') || *pc == _T('/')) { moviefile = pc+1; }
}
std::basic_ostringstream< TCHAR > o;
o << stagedir << _T("\\") << moviefile;
std::basic_string< TCHAR > moviepath1 = o.str();
if (! CopyFile(stage.tc_moviepath.c_str(), moviepath1.c_str(), true)) {
RemoveDirectory(stagedir.c_str());
return false;
}
}
stage.tc_moviepath = moviefile;
stage.tc_basename = basename;
{
char* tmp;
tmp = ::bootes::lib::util::TChar::T2C(stage.tc_moviepath.c_str());
if (tmp == NULL) { return false; }
stage.moviepath = tmp;
delete[] tmp;
tmp = ::bootes::lib::util::TChar::T2C(stage.tc_basename.c_str());
if (tmp == NULL) { return false; }
stage.basename = tmp;
delete[] tmp;
}
if (! Save(dir, basename.c_str(), stage, seq)) {
return false;
}
return true;
}
void LBFGSSolver::solve(const Function& function,
SolverResults* results) const
{
double global_start_time = wall_time();
// Dimension of problem.
size_t n = function.get_number_of_scalars();
if (n == 0) {
results->exit_condition = SolverResults::FUNCTION_TOLERANCE;
return;
}
// Current point, gradient and Hessian.
double fval = std::numeric_limits<double>::quiet_NaN();
double fprev = std::numeric_limits<double>::quiet_NaN();
double normg0 = std::numeric_limits<double>::quiet_NaN();
double normg = std::numeric_limits<double>::quiet_NaN();
double normdx = std::numeric_limits<double>::quiet_NaN();
Eigen::VectorXd x, g;
// Copy the user state to the current point.
function.copy_user_to_global(&x);
Eigen::VectorXd x2(n);
// L-BFGS history.
std::vector<Eigen::VectorXd> s_data(this->lbfgs_history_size),
y_data(this->lbfgs_history_size);
std::vector<Eigen::VectorXd*> s(this->lbfgs_history_size),
y(this->lbfgs_history_size);
for (int h = 0; h < this->lbfgs_history_size; ++h) {
s_data[h].resize(function.get_number_of_scalars());
s_data[h].setZero();
y_data[h].resize(function.get_number_of_scalars());
y_data[h].setZero();
s[h] = &s_data[h];
y[h] = &y_data[h];
}
Eigen::VectorXd rho(this->lbfgs_history_size);
rho.setZero();
Eigen::VectorXd alpha(this->lbfgs_history_size);
alpha.setZero();
Eigen::VectorXd q(n);
Eigen::VectorXd r(n);
// Needed from the previous iteration.
Eigen::VectorXd x_prev(n), s_tmp(n), y_tmp(n);
CheckExitConditionsCache exit_condition_cache;
//
// START MAIN ITERATION
//
results->startup_time += wall_time() - global_start_time;
results->exit_condition = SolverResults::INTERNAL_ERROR;
int iter = 0;
bool last_iteration_successful = true;
int number_of_line_search_failures = 0;
int number_of_restarts = 0;
while (true) {
//
// Evaluate function and derivatives.
//
double start_time = wall_time();
// y[0] should contain the difference between the gradient
// in this iteration and the gradient from the previous.
// Therefore, update y before and after evaluating the
// function.
if (iter > 0) {
y_tmp = -g;
}
fval = function.evaluate(x, &g);
normg = std::max(g.maxCoeff(), -g.minCoeff());
if (iter == 0) {
normg0 = normg;
}
results->function_evaluation_time += wall_time() - start_time;
//
// Update history
//
start_time = wall_time();
if (iter > 0 && last_iteration_successful) {
s_tmp = x - x_prev;
y_tmp += g;
double sTy = s_tmp.dot(y_tmp);
if (sTy > 1e-16) {
// Shift all pointers one step back, discarding the oldest one.
Eigen::VectorXd* sh = s[this->lbfgs_history_size - 1];
Eigen::VectorXd* yh = y[this->lbfgs_history_size - 1];
for (int h = this->lbfgs_history_size - 1; h >= 1; --h) {
s[h] = s[h - 1];
y[h] = y[h - 1];
rho[h] = rho[h - 1];
}
// Reuse the storage of the discarded data for the new data.
s[0] = sh;
y[0] = yh;
*y[0] = y_tmp;
*s[0] = s_tmp;
rho[0] = 1.0 / sTy;
}
}
results->lbfgs_update_time += wall_time() - start_time;
//
// Test stopping criteriea
//
start_time = wall_time();
if (iter > 1 && this->check_exit_conditions(fval, fprev, normg,
normg0, x.norm(), normdx,
last_iteration_successful,
&exit_condition_cache, results)) {
break;
}
if (iter >= this->maximum_iterations) {
results->exit_condition = SolverResults::NO_CONVERGENCE;
break;
}
if (this->callback_function) {
CallbackInformation information;
information.objective_value = fval;
information.x = &x;
information.g = &g;
if (!callback_function(information)) {
results->exit_condition = SolverResults::USER_ABORT;
break;
}
}
results->stopping_criteria_time += wall_time() - start_time;
//
// Compute search direction via L-BGFS two-loop recursion.
//
start_time = wall_time();
bool should_restart = false;
double H0 = 1.0;
if (iter > 0) {
// If the gradient is identical two iterations in a row,
// y will be the zero vector and H0 will be NaN. In this
// case the line search will fail and L-BFGS will be restarted
// with a steepest descent step.
H0 = s[0]->dot(*y[0]) / y[0]->dot(*y[0]);
// If isinf(H0) || isnan(H0)
if (H0 == std::numeric_limits<double>::infinity() ||
H0 == -std::numeric_limits<double>::infinity() ||
H0 != H0) {
should_restart = true;
}
}
q = -g;
for (int h = 0; h < this->lbfgs_history_size; ++h) {
alpha[h] = rho[h] * s[h]->dot(q);
q = q - alpha[h] * (*y[h]);
}
r = H0 * q;
for (int h = this->lbfgs_history_size - 1; h >= 0; --h) {
double beta = rho[h] * y[h]->dot(r);
r = r + (*s[h]) * (alpha[h] - beta);
}
// If the function improves very little, the approximated Hessian
// might be very bad. If this is the case, it is better to discard
// the history once in a while. This allows the solver to correctly
// solve some badly scaled problems.
double restart_test = std::fabs(fval - fprev) /
(std::fabs(fval) + std::fabs(fprev));
if (iter > 0 && iter % 100 == 0 && restart_test
< this->lbfgs_restart_tolerance) {
should_restart = true;
}
if (! last_iteration_successful) {
should_restart = true;
}
if (should_restart) {
if (this->log_function) {
char str[1024];
if (number_of_restarts <= 10) {
std::sprintf(str, "Restarting: fval = %.3e, deltaf = %.3e, max|g_i| = %.3e, test = %.3e",
fval, std::fabs(fval - fprev), normg, restart_test);
this->log_function(str);
}
if (number_of_restarts == 10) {
this->log_function("NOTE: No more restarts will be reported.");
}
number_of_restarts++;
}
r = -g;
for (int h = 0; h < this->lbfgs_history_size; ++h) {
(*s[h]).setZero();
(*y[h]).setZero();
}
rho.setZero();
alpha.setZero();
// H0 is not used, but its value will be printed.
H0 = std::numeric_limits<double>::quiet_NaN();
}
results->lbfgs_update_time += wall_time() - start_time;
//
// Perform line search.
//
start_time = wall_time();
double start_alpha = 1.0;
// In the first iteration, start with a much smaller step
// length. (heuristic used by e.g. minFunc)
if (iter == 0) {
double sumabsg = 0.0;
for (size_t i = 0; i < n; ++i) {
sumabsg += std::fabs(g[i]);
}
start_alpha = std::min(1.0, 1.0 / sumabsg);
}
double alpha_step = this->perform_linesearch(function, x, fval, g,
r, &x2, start_alpha);
if (alpha_step <= 0) {
if (this->log_function) {
this->log_function("Line search failed.");
char str[1024];
std::sprintf(str, "%4d %+.3e %9.3e %.3e %.3e %.3e %.3e",
iter, fval, std::fabs(fval - fprev), normg, alpha_step, H0, rho[0]);
this->log_function(str);
}
if (! last_iteration_successful || number_of_line_search_failures++ > 10) {
// This happens quite seldom. Every time it has happened, the function
// was actually converged to a solution.
results->exit_condition = SolverResults::GRADIENT_TOLERANCE;
break;
}
last_iteration_successful = false;
}
else {
// Record length of this step.
normdx = alpha_step * r.norm();
// Compute new point.
x_prev = x;
x = x + alpha_step * r;
last_iteration_successful = true;
}
results->backtracking_time += wall_time() - start_time;
//
// Log the results of this iteration.
//
start_time = wall_time();
int log_interval = 1;
if (iter > 30) {
log_interval = 10;
}
if (iter > 200) {
log_interval = 100;
}
if (iter > 2000) {
log_interval = 1000;
}
if (this->log_function && iter % log_interval == 0) {
if (iter == 0) {
this->log_function("Itr f deltaf max|g_i| alpha H0 rho");
}
this->log_function(
to_string(
std::setw(4), iter, " ",
std::setw(10), std::setprecision(3), std::scientific, std::showpos, fval, std::noshowpos, " ",
std::setw(9), std::setprecision(3), std::scientific, std::fabs(fval - fprev), " ",
std::setw(9), std::setprecision(3), std::setprecision(3), std::scientific, normg, " ",
std::setw(9), std::setprecision(3), std::scientific, alpha_step, " ",
std::setw(9), std::setprecision(3), std::scientific, H0, " ",
std::setw(9), std::setprecision(3), std::scientific, rho[0]
)
);
}
results->log_time += wall_time() - start_time;
fprev = fval;
iter++;
}
function.copy_global_to_user(x);
results->total_time += wall_time() - global_start_time;
if (this->log_function) {
char str[1024];
std::sprintf(str, " end %+.3e %.3e", fval, normg);
this->log_function(str);
}
}
int
FlatShadeTexture::draw(CVIEWptr& v)
{
if (_ctrl)
return _ctrl->draw(v);
_cb->alpha = alpha();
if (!_patch)
return 0;
// Don't draw w/ texture maps if there are no uv coords at all.
// but don't check every frame whether there are uv coords:
if (_check_uv_coords_stamp != _patch->stamp()) {
_has_uv_coords = _patch->cur_faces().any_satisfy(HasUVFaceFilter());
_check_uv_coords_stamp = _patch->stamp();
}
// Set gl state (lighting, shade model)
glPushAttrib(GL_ENABLE_BIT | GL_LIGHTING_BIT | GL_CURRENT_BIT);
glEnable(GL_LIGHTING); // GL_ENABLE_BIT
glShadeModel(GL_FLAT); // GL_LIGHTING_BIT
GL_COL(_patch->color(), alpha()); // GL_CURRENT_BIT
if (debug_uv()) {
//assign some texture coords if no tex_coord_gen is assigned
if (!_patch->tex_coord_gen()) {
_patch->set_tex_coord_gen(new GLSphirTexCoordGen);
}
// decide what image file to use to show the uv-coords:
str_ptr debug_uv_tex_name =
Config::get_var_str("DEBUG_UV_TEX_MAP", "checkerboard.png",true);
static str_ptr pre_path = Config::JOT_ROOT() + "nprdata/other_textures/";
str_ptr path = pre_path + debug_uv_tex_name;
// we only try a given pathname once. but if it fails
// and they reset DEBUG_UV_TEX_MAP while running the
// program we can pick up the change and try again.
if (path != _debug_tex_path) {
_debug_tex_path = path;
TEXTUREglptr tex = new TEXTUREgl(_debug_tex_path);
bool do_mipmap = Config::get_var_bool("DEBUG_TEX_USE_MIPMAP",false);
tex->set_mipmap(do_mipmap);
_debug_uv_tex = tex;
_debug_uv_in_dl = false;
if (debug) {
cerr << "Loading debug uv texture " << **path << " ..." << endl;
}
if (!_debug_uv_tex->load_texture()) {
cerr << "Can't load debug uv texture: "
<< **_debug_tex_path
<< endl
<< "Set environment variable DEBUG_UV_TEX_MAP "
<< "to an image file in "
<< **pre_path
<< " and try again."
<< endl;
_debug_uv_tex = 0;
}
}
// Apply the texture if any faces have uv coordinates:
if (_debug_uv_tex /*&& _has_uv_coords*/) //<- using auto UV
_debug_uv_tex->apply_texture(); // GL_ENABLE_BIT
} else {
// XXX - dumb hack
check_patch_texture_map();
// if (_has_uv_coords) <- using auto UV
_patch->apply_texture(); // GL_ENABLE_BIT
}
// Set material parameters for OGL:
GtexUtil::setup_material(_patch);
// Try for the display list if it's valid
if (!(_debug_uv == _debug_uv_in_dl && BasicTexture::draw(v))) {
// Try to generate a display list
int dl = _dl.get_dl(v, 1, _patch->stamp());
if (dl) {
glNewList(dl, GL_COMPILE);
_debug_uv_in_dl = _debug_uv;
}
// Set up face culling for closed surfaces
if (!set_face_culling())
glLightModeli(GL_LIGHT_MODEL_TWO_SIDE, GL_TRUE); // GL_LIGHTING_BIT
FlatShadeStripCB *flat_cb = dynamic_cast<FlatShadeStripCB*>(_cb);
if (flat_cb) {
// send texture coordinates?
if ((_debug_uv && _debug_uv_tex) ||
(_has_uv_coords && _patch->has_texture())) {
flat_cb->enable_texcoords();
} else {
flat_cb->disable_texcoords();
}
}
err_adv(debug && _debug_uv, " drawing uvs in flat shade");
// draw the triangle strips
_patch->draw_tri_strips(_cb);
// end the display list here
if (_dl.dl(v)) {
_dl.close_dl(v);
// the display list is built; now execute it
BasicTexture::draw(v);
}
}
// restore GL state
glPopAttrib();
// Have to move this outside the display list and gl push/pop attrib
// or else it's all:
/*
FlatShadeTexture::draw - End ***NOTE*** OpenGL Error: [500] 'Invalid Enumerator'
FlatShadeTexture::draw - End ***NOTE*** OpenGL Error: [500] 'Invalid Enumerator'
FlatShadeTexture::draw - End ***NOTE*** OpenGL Error: [500] 'Invalid Enumerator'
FlatShadeTexture::draw - End ***NOTE*** OpenGL Error: [500] 'Invalid Enumerator'
FlatShadeTexture::draw - End ***NOTE*** OpenGL Error: [500] 'Invalid Enumerator'
FlatShadeTexture::draw - End ***NOTE*** OpenGL Error: [500] 'Invalid Enumerator'
FlatShadeTexture::draw - End ***NOTE*** OpenGL Error: [500] 'Invalid Enumerator'
FlatShadeTexture::draw - End ***NOTE*** OpenGL Error: [500] 'Invalid Enumerator'
*/
if (_debug_uv) {
// Build the edge strip at the current subdivision level
Bedge_list edges = _patch->cur_edges();
edges.clear_flags();
// If secondary edges shouldn't be drawn, set their flags
// so they won't be drawn:
if (!BMESH::show_secondary_faces())
edges.secondary_edges().set_flags(1);
// Draw uv-discontinuity boundaries as yellow lines.
// Construct filter that accepts unreached uv-discontinuous
// edges of this patch:
UnreachedSimplexFilter unreached;
UVDiscontinuousEdgeFilter uvdisc;
PatchEdgeFilter mine(_patch->cur_patch());
EdgeStrip disc_edges(edges, unreached + uvdisc + mine);
if (!disc_edges.empty()) {
GtexUtil::draw_strip(disc_edges, 3, Color::yellow, 0.8);
}
}
GL_VIEW::print_gl_errors("FlatShadeTexture::draw - End");
return _patch->num_faces();
}
Color::operator rgb_color() const
{
return make_color(red(), green(), blue(), alpha());
}
// -(N-1)/2 <= n <= (N-1)/2
static REAL win(REAL n,int N)
{
return izero(alpha(aa)*sqrt(1-4*n*n/((N-1)*(N-1))))/iza;
}
int main( int argc, char* argv[] )
{
// Initialise window
pangolin::View& container = SetupPangoGLWithCuda(1024, 768);
size_t cu_mem_start, cu_mem_end, cu_mem_total;
cudaMemGetInfo( &cu_mem_start, &cu_mem_total );
glClearColor(1,1,1,0);
// Open video device
hal::Camera video = OpenRpgCamera(argc,argv);
// Capture first image
pb::ImageArray images;
// N cameras, each w*h in dimension, greyscale
const size_t N = video.NumChannels();
if( N != 2 ) {
std::cerr << "Two images are required to run this program!" << std::endl;
exit(1);
}
const size_t nw = video.Width();
const size_t nh = video.Height();
// Capture first image
video.Capture(images);
// Downsample this image to process less pixels
const int max_levels = 6;
const int level = roo::GetLevelFromMaxPixels( nw, nh, 640*480 );
// const int level = 4;
assert(level <= max_levels);
// Find centered image crop which aligns to 16 pixels at given level
const NppiRect roi = roo::GetCenteredAlignedRegion(nw,nh,16 << level,16 << level);
// Load Camera intrinsics from file
GetPot clArgs( argc, argv );
const std::string filename = clArgs.follow("","-cmod");
if( filename.empty() ) {
std::cerr << "Camera models file is required!" << std::endl;
exit(1);
}
const calibu::CameraRig rig = calibu::ReadXmlRig(filename);
if( rig.cameras.size() != 2 ) {
std::cerr << "Two camera models are required to run this program!" << std::endl;
exit(1);
}
Eigen::Matrix3f CamModel0 = rig.cameras[0].camera.K().cast<float>();
Eigen::Matrix3f CamModel1 = rig.cameras[1].camera.K().cast<float>();
roo::ImageIntrinsics camMod[] = {
{CamModel0(0,0),CamModel0(1,1),CamModel0(0,2),CamModel0(1,2)},
{CamModel1(0,0),CamModel1(1,1),CamModel1(0,2),CamModel1(1,2)}
};
for(int i=0; i<2; ++i ) {
// Adjust to match camera image dimensions
const double scale = nw / rig.cameras[i].camera.Width();
roo::ImageIntrinsics camModel = camMod[i].Scale( scale );
// Adjust to match cropped aligned image
camModel = camModel.CropToROI( roi );
camMod[i] = camModel;
}
const unsigned int w = roi.width;
const unsigned int h = roi.height;
const unsigned int lw = w >> level;
const unsigned int lh = h >> level;
const Eigen::Matrix3d& K0 = camMod[0].Matrix();
const Eigen::Matrix3d& Kl = camMod[0][level].Matrix();
std::cout << "K Matrix: " << std::endl << K0 << std::endl;
std::cout << "K Matrix - Level: " << std::endl << Kl << std::endl;
std::cout << "Video stream dimensions: " << nw << "x" << nh << std::endl;
std::cout << "Chosen Level: " << level << std::endl;
std::cout << "Processing dimensions: " << lw << "x" << lh << std::endl;
std::cout << "Offset: " << roi.x << "x" << roi.y << std::endl;
// print selected camera model
std::cout << "Camera Model used: " << std::endl << camMod[0][level].Matrix() << std::endl;
Eigen::Matrix3d RDFvision;RDFvision<< 1,0,0, 0,1,0, 0,0,1;
Eigen::Matrix3d RDFrobot; RDFrobot << 0,1,0, 0,0, 1, 1,0,0;
Eigen::Matrix4d T_vis_ro = Eigen::Matrix4d::Identity();
T_vis_ro.block<3,3>(0,0) = RDFvision.transpose() * RDFrobot;
Eigen::Matrix4d T_ro_vis = Eigen::Matrix4d::Identity();
T_ro_vis.block<3,3>(0,0) = RDFrobot.transpose() * RDFvision;
const Sophus::SE3d T_rl_orig = T_rlFromCamModelRDF(rig.cameras[0], rig.cameras[1], RDFvision);
// TODO(jmf): For now, assume cameras are rectified. Later allow unrectified cameras.
/*
double k1 = 0;
double k2 = 0;
if(cam[0].Type() == MVL_CAMERA_WARPED)
{
k1 = cam[0].GetModel()->warped.kappa1;
k2 = cam[0].GetModel()->warped.kappa2;
}
*/
const bool rectify = false;
if(!rectify) {
std::cout << "Using pre-rectified images" << std::endl;
}
// Check we received at least two images
if(images.Size() < 2) {
std::cerr << "Failed to capture first stereo pair from camera" << std::endl;
return -1;
}
// Define Camera Render Object (for view / scene browsing)
pangolin::OpenGlRenderState s_cam(
pangolin::ProjectionMatrixRDF_TopLeft(w,h,K0(0,0),K0(1,1),K0(0,2),K0(1,2),0.1,10000),
pangolin::IdentityMatrix(pangolin::GlModelViewStack)
);
pangolin::GlBufferCudaPtr vbo(pangolin::GlArrayBuffer, lw*lh,GL_FLOAT, 4, cudaGraphicsMapFlagsWriteDiscard, GL_STREAM_DRAW );
pangolin::GlBufferCudaPtr cbo(pangolin::GlArrayBuffer, lw*lh,GL_UNSIGNED_BYTE, 4, cudaGraphicsMapFlagsWriteDiscard, GL_STREAM_DRAW );
pangolin::GlBuffer ibo = pangolin::MakeTriangleStripIboForVbo(lw,lh);
// Allocate Camera Images on device for processing
roo::Image<unsigned char, roo::TargetHost, roo::DontManage> hCamImg[] = {{0,nw,nh},{0,nw,nh}};
roo::Image<float2, roo::TargetDevice, roo::Manage> dLookup[] = {{w,h},{w,h}};
roo::Image<unsigned char, roo::TargetDevice, roo::Manage> upload(w,h);
roo::Pyramid<unsigned char, max_levels, roo::TargetDevice, roo::Manage> img_pyr[] = {{w,h},{w,h}};
roo::Image<float, roo::TargetDevice, roo::Manage> img[] = {{lw,lh},{lw,lh}};
roo::Volume<float, roo::TargetDevice, roo::Manage> vol[] = {{lw,lh,MAXD},{lw,lh,MAXD}};
roo::Image<float, roo::TargetDevice, roo::Manage> disp[] = {{lw,lh},{lw,lh}};
roo::Image<float, roo::TargetDevice, roo::Manage> meanI(lw,lh);
roo::Image<float, roo::TargetDevice, roo::Manage> varI(lw,lh);
roo::Image<float, roo::TargetDevice, roo::Manage> temp[] = {{lw,lh},{lw,lh},{lw,lh},{lw,lh},{lw,lh}};
roo::Image<float,roo::TargetDevice, roo::Manage>& imgd = disp[0];
roo::Image<float,roo::TargetDevice, roo::Manage> depthmap(lw,lh);
roo::Image<float,roo::TargetDevice, roo::Manage> imga(lw,lh);
roo::Image<float2,roo::TargetDevice, roo::Manage> imgq(lw,lh);
roo::Image<float,roo::TargetDevice, roo::Manage> imgw(lw,lh);
roo::Image<float4, roo::TargetDevice, roo::Manage> d3d(lw,lh);
roo::Image<unsigned char, roo::TargetDevice,roo::Manage> Scratch(lw*sizeof(roo::LeastSquaresSystem<float,6>),lh);
typedef ulong4 census_t;
roo::Image<census_t, roo::TargetDevice, roo::Manage> census[] = {{lw,lh},{lw,lh}};
// Stereo transformation (post-rectification)
Sophus::SE3d T_rl = T_rl_orig;
const double baseline = T_rl.translation().norm();
std::cout << "Baseline: " << baseline << std::endl;
cudaMemGetInfo( &cu_mem_end, &cu_mem_total );
std::cout << "CuTotal: " << cu_mem_total/(1024*1024) << ", Available: " << cu_mem_end/(1024*1024) << ", Used: " << (cu_mem_start-cu_mem_end)/(1024*1024) << std::endl;
pangolin::Var<bool> step("ui.step", false, false);
pangolin::Var<bool> run("ui.run", false, true);
pangolin::Var<bool> lockToCam("ui.Lock to cam", false, true);
pangolin::Var<int> show_slice("ui.show slice",MAXD/2, 0, MAXD-1);
pangolin::Var<int> maxdisp("ui.maxdisp",MAXD, 0, MAXD);
pangolin::Var<bool> subpix("ui.subpix", true, true);
pangolin::Var<bool> use_census("ui.use census", true, true);
pangolin::Var<int> avg_rad("ui.avg_rad",0, 0, 100);
pangolin::Var<bool> do_dtam("ui.do dtam", false, true);
pangolin::Var<bool> dtam_reset("ui.reset", false, false);
pangolin::Var<float> g_alpha("ui.g alpha", 14, 0,4);
pangolin::Var<float> g_beta("ui.g beta", 2.5, 0,2);
pangolin::Var<float> theta("ui.theta", 100, 0,100);
pangolin::Var<float> lambda("ui.lambda", 20, 0,20);
pangolin::Var<float> sigma_q("ui.sigma q", 0.7, 0, 1);
pangolin::Var<float> sigma_d("ui.sigma d", 0.7, 0, 1);
pangolin::Var<float> huber_alpha("ui.huber alpha", 0.002, 0, 0.01);
pangolin::Var<float> beta("ui.beta", 0.00001, 0, 0.01);
pangolin::Var<float> alpha("ui.alpha", 0.9, 0,1);
pangolin::Var<float> r1("ui.r1", 100, 0,0.01);
pangolin::Var<float> r2("ui.r2", 100, 0,0.01);
pangolin::Var<bool> filter("ui.filter", false, true);
pangolin::Var<float> eps("ui.eps",0.01*0.01, 0, 0.01);
pangolin::Var<int> rad("ui.radius",9, 1, 20);
pangolin::Var<bool> leftrightcheck("ui.left-right check", false, true);
pangolin::Var<float> maxdispdiff("ui.maxdispdiff",1, 0, 5);
pangolin::Var<int> domedits("ui.median its",1, 1, 10);
pangolin::Var<bool> domed9x9("ui.median 9x9", false, true);
pangolin::Var<bool> domed7x7("ui.median 7x7", false, true);
pangolin::Var<bool> domed5x5("ui.median 5x5", false, true);
pangolin::Var<int> medi("ui.medi",12, 0, 24);
pangolin::Var<float> filtgradthresh("ui.filt grad thresh", 0, 0, 20);
pangolin::Var<bool> save_depthmaps("ui.save_depthmaps", false, true);
int jump_frames = 0;
pangolin::RegisterKeyPressCallback(' ', [&run](){run = !run;} );
pangolin::RegisterKeyPressCallback('l', [&lockToCam](){lockToCam = !lockToCam;} );
pangolin::RegisterKeyPressCallback(pangolin::PANGO_SPECIAL + GLUT_KEY_RIGHT, [&step](){step=true;} );
pangolin::RegisterKeyPressCallback(']', [&jump_frames](){jump_frames=100;} );
pangolin::RegisterKeyPressCallback('}', [&jump_frames](){jump_frames=1000;} );
pangolin::Handler2dImageSelect handler2d(lw,lh,level);
// ActivateDrawPyramid<unsigned char,max_levels> adleft(img_pyr[0],GL_LUMINANCE8, false, true);
// ActivateDrawPyramid<unsigned char,max_levels> adright(img_pyr[1],GL_LUMINANCE8, false, true);
pangolin::ActivateDrawImage<float> adleft(img[0],GL_LUMINANCE32F_ARB, false, true);
pangolin::ActivateDrawImage<float> adright(img[1],GL_LUMINANCE32F_ARB, false, true);
pangolin::ActivateDrawImage<float> adisp(disp[0],GL_LUMINANCE32F_ARB, false, true);
pangolin::ActivateDrawImage<float> adw(imgw,GL_LUMINANCE32F_ARB, false, true);
// ActivateDrawImage<float> adCrossSection(dCrossSection,GL_RGBA_FLOAT32_APPLE, false, true);
pangolin::ActivateDrawImage<float> adVol(vol[0].ImageXY(show_slice),GL_LUMINANCE32F_ARB, false, true);
SceneGraph::GLSceneGraph graph;
SceneGraph::GLVbo glvbo(&vbo,&ibo,&cbo);
graph.AddChild(&glvbo);
SetupContainer(container, 6, (float)w/h);
container[0].SetDrawFunction(boost::ref(adleft)).SetHandler(&handler2d);
container[1].SetDrawFunction(boost::ref(adright)).SetHandler(&handler2d);
container[2].SetDrawFunction(boost::ref(adisp)).SetHandler(&handler2d);
container[3].SetDrawFunction(boost::ref(adVol)).SetHandler(&handler2d);
container[4].SetDrawFunction(SceneGraph::ActivateDrawFunctor(graph, s_cam))
.SetHandler( new pangolin::Handler3D(s_cam, pangolin::AxisNone) );
container[5].SetDrawFunction(boost::ref(adw)).SetHandler(&handler2d);
for(unsigned long frame=0; !pangolin::ShouldQuit();)
{
bool go = frame==0 || jump_frames > 0 || run || Pushed(step);
for(; jump_frames > 0; jump_frames--) {
video.Capture(images);
}
if(go) {
if(frame>0) {
if( video.Capture(images) == false) {
exit(1);
}
}
frame++;
/////////////////////////////////////////////////////////////
// Upload images to device (Warp / Decimate if necessery)
for(int i=0; i<2; ++i ) {
hCamImg[i].ptr = images[i].data();
if(rectify) {
upload.CopyFrom(hCamImg[i].SubImage(roi));
Warp(img_pyr[i][0], upload, dLookup[i]);
}else{
img_pyr[i][0].CopyFrom(hCamImg[i].SubImage(roi));
}
roo::BoxReduce<unsigned char, max_levels, unsigned int>(img_pyr[i]);
}
}
go |= avg_rad.GuiChanged() | use_census.GuiChanged();
if( go ) {
for(int i=0; i<2; ++i ) {
roo::ElementwiseScaleBias<float,unsigned char,float>(img[i], img_pyr[i][level],1.0f/255.0f);
if(avg_rad > 0 ) {
roo::BoxFilter<float,float,float>(temp[0],img[i],Scratch,avg_rad);
roo::ElementwiseAdd<float,float,float,float>(img[i], img[i], temp[0], 1, -1, 0.5);
}
if(use_census) {
Census(census[i], img[i]);
}
}
}
if( go | g_alpha.GuiChanged() || g_beta.GuiChanged() ) {
ExponentialEdgeWeight(imgw, img[0], g_alpha, g_beta);
}
go |= filter.GuiChanged() | leftrightcheck.GuiChanged() | rad.GuiChanged() | eps.GuiChanged() | alpha.GuiChanged() | r1.GuiChanged() | r2.GuiChanged();
if(go) {
if(use_census) {
roo::CensusStereoVolume<float, census_t>(vol[0], census[0], census[1], maxdisp, -1);
if(leftrightcheck) roo::CensusStereoVolume<float, census_t>(vol[1], census[1], census[0], maxdisp, +1);
}else{
CostVolumeFromStereoTruncatedAbsAndGrad(vol[0], img[0], img[1], -1, alpha, r1, r2);
if(leftrightcheck) CostVolumeFromStereoTruncatedAbsAndGrad(vol[1], img[1], img[0], +1, alpha, r1, r2);
}
if(filter) {
// Filter Cost volume
for(int v=0; v<(leftrightcheck?2:1); ++v)
{
roo::Image<float, roo::TargetDevice, roo::Manage>& I = img[v];
roo::ComputeMeanVarience<float,float,float>(varI, temp[0], meanI, I, Scratch, rad);
for(int d=0; d<maxdisp; ++d)
{
roo::Image<float> P = vol[v].ImageXY(d);
roo::ComputeCovariance(temp[0],temp[2],temp[1],P,meanI,I,Scratch,rad);
GuidedFilter(P,temp[0],varI,temp[1],meanI,I,Scratch,temp[2],temp[3],temp[4],rad,eps);
}
}
}
}
static int n = 0;
// static float theta = 0;
// go |= Pushed(dtam_reset);
// if(go )
if(Pushed(dtam_reset))
{
n = 0;
theta.Reset();
// Initialise primal and auxillary variables
CostVolMinimumSubpix(imgd,vol[0], maxdisp,-1);
imga.CopyFrom(imgd);
// Initialise dual variable
imgq.Memset(0);
}
const double min_theta = 1E-0;
if(do_dtam && theta > min_theta)
{
for(int i=0; i<5; ++i ) {
// Auxillary exhaustive search
CostVolMinimumSquarePenaltySubpix(imga, vol[0], imgd, maxdisp, -1, lambda, (theta) );
// Dual Ascent
roo::WeightedHuberGradU_DualAscentP(imgq, imgd, imgw, sigma_q, huber_alpha);
// Primal Descent
roo::WeightedL2_u_minus_g_PrimalDescent(imgd, imgq, imga, imgw, sigma_d, 1.0f / (theta) );
theta= theta * (1-beta*n);
++n;
}
if( theta <= min_theta && save_depthmaps ) {
cv::Mat dmap = cv::Mat( lh, lw, CV_32FC1 );
// convert disparity to depth
roo::Disp2Depth(imgd, depthmap, Kl(0,0), baseline );
depthmap.MemcpyToHost( dmap.data );
// save depth image
char Index[10];
sprintf( Index, "%05d", frame );
std::string DepthPrefix = "SDepth-";
std::string DepthFile;
DepthFile = DepthPrefix + Index + ".pdm";
std::cout << "Depth File: " << DepthFile << std::endl;
std::ofstream pDFile( DepthFile.c_str(), std::ios::out | std::ios::binary );
pDFile << "P7" << std::endl;
pDFile << dmap.cols << " " << dmap.rows << std::endl;
unsigned int Size = dmap.elemSize1() * dmap.rows * dmap.cols;
pDFile << 4294967295 << std::endl;
pDFile.write( (const char*)dmap.data, Size );
pDFile.close();
// save grey image
std::string GreyPrefix = "Left-";
std::string GreyFile;
GreyFile = GreyPrefix + Index + ".pgm";
std::cout << "Grey File: " << GreyFile << std::endl;
cv::Mat gimg = cv::Mat( lh, lw, CV_8UC1 );
img_pyr[0][level].MemcpyToHost( gimg.data );
cv::imwrite( GreyFile, gimg );
// reset
step = true;
dtam_reset = true;
}
}
go |= pangolin::GuiVarHasChanged();
// if(go) {
// if(subpix) {
// CostVolMinimumSubpix(disp[0],vol[0], maxdisp,-1);
// if(leftrightcheck) CostVolMinimumSubpix(disp[1],vol[1], maxdisp,+1);
// }else{
// CostVolMinimum<float,float>(disp[0],vol[0], maxdisp);
// if(leftrightcheck) CostVolMinimum<float,float>(disp[1],vol[1], maxdisp);
// }
// }
if(go) {
for(int di=0; di<(leftrightcheck?2:1); ++di) {
for(int i=0; i < domedits; ++i ) {
if(domed9x9) MedianFilterRejectNegative9x9(disp[di],disp[di], medi);
if(domed7x7) MedianFilterRejectNegative7x7(disp[di],disp[di], medi);
if(domed5x5) MedianFilterRejectNegative5x5(disp[di],disp[di], medi);
}
}
if(leftrightcheck ) {
LeftRightCheck(disp[1], disp[0], +1, maxdispdiff);
LeftRightCheck(disp[0], disp[1], -1, maxdispdiff);
}
if(filtgradthresh > 0) {
FilterDispGrad(disp[0], disp[0], filtgradthresh);
}
}
// if(go)
{
// Generate point cloud from disparity image
DisparityImageToVbo(d3d, disp[0], baseline, Kl(0,0), Kl(1,1), Kl(0,2), Kl(1,2) );
// if(container[3].IsShown())
{
// Copy point cloud into VBO
{
pangolin::CudaScopedMappedPtr var(vbo);
roo::Image<float4> dVbo((float4*)*var,lw,lh);
dVbo.CopyFrom(d3d);
}
// Generate CBO
{
pangolin::CudaScopedMappedPtr var(cbo);
roo::Image<uchar4> dCbo((uchar4*)*var,lw,lh);
roo::ConvertImage<uchar4,unsigned char>(dCbo, img_pyr[0][level]);
}
}
// Update texture views
adisp.SetImageScale(1.0f/maxdisp);
// adleft.SetLevel(show_level);
// adright.SetLevel(show_level);
adVol.SetImage(vol[0].ImageXY(show_slice));
}
/////////////////////////////////////////////////////////////
// Draw
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glColor3f(1,1,1);
pangolin::FinishFrame();
}
}
//---------------------------------------------------------------------
void CqImagersource::Initialise( const CqRegion& DRegion, IqChannelBuffer* buffer )
{
AQSIS_TIME_SCOPE(Imager_shading);
// We use one less than the bucket width and height here, since these
// resolutions really represent one less than the number of shaded points
// in each direction. (Usually they describe the number of micropolygons
// on a grid which is one less than the number of shaded vertices. This
// concept has no real analogue in context of an imager shader.)
TqInt uGridRes = DRegion.width()-1;
TqInt vGridRes = DRegion.height()-1;
TqInt x = DRegion.xMin();
TqInt y = DRegion.yMin();
m_uYOrigin = static_cast<TqInt>( y );
m_uXOrigin = static_cast<TqInt>( x );
m_uGridRes = uGridRes;
m_vGridRes = vGridRes;
TqInt mode = QGetRenderContext() ->poptCurrent()->GetIntegerOption( "System", "DisplayMode" ) [ 0 ];
TqFloat components;
TqInt j, i;
TqFloat shuttertime = QGetRenderContext() ->poptCurrent()->GetFloatOption( "System", "Shutter" ) [ 0 ];
components = mode & DMode_RGB ? 3 : 0;
components += mode & DMode_A ? 1 : 0;
components = mode & DMode_Z ? 1 : components;
TqInt Uses = ( 1 << EnvVars_P ) | ( 1 << EnvVars_Ci ) | ( 1 << EnvVars_Oi | ( 1 << EnvVars_ncomps ) | ( 1 << EnvVars_time ) | ( 1 << EnvVars_alpha ) | ( 1 << EnvVars_s ) | ( 1 << EnvVars_t ) );
m_pShaderExecEnv->Initialise( uGridRes, vGridRes, uGridRes * vGridRes, (uGridRes+1)*(vGridRes+1), true, IqAttributesPtr(), IqTransformPtr(), m_pShader.get(), Uses );
// Initialise the geometric parameters in the shader exec env.
TqInt numShadingPoints = (uGridRes+1) * (vGridRes+1);
P() ->Initialise( numShadingPoints );
Ci() ->Initialise( numShadingPoints );
Oi() ->Initialise( numShadingPoints );
alpha() ->Initialise( numShadingPoints );
s() ->Initialise( numShadingPoints );
t() ->Initialise( numShadingPoints );
//TODO dtime is not initialised yet
//dtime().Initialise(uGridRes, vGridRes, i);
ncomps() ->SetFloat( components );
time() ->SetFloat( shuttertime );
m_pShader->Initialise( uGridRes, vGridRes, (uGridRes+1)*(vGridRes+1), m_pShaderExecEnv.get() );
TqUint CiIndex = buffer->getChannelIndex("Ci");
TqUint OiIndex = buffer->getChannelIndex("Oi");
TqUint coverageIndex = buffer->getChannelIndex("coverage");
for ( j = 0; j < vGridRes+1; j++ )
{
for ( i = 0; i < uGridRes+1; i++ )
{
TqInt off = j * ( uGridRes + 1 ) + i;
P() ->SetPoint( CqVector3D( x + i, y + j, 0.0 ), off );
Ci() ->SetColor( CqColor((*buffer)(i, j, CiIndex)[0], (*buffer)(i, j, CiIndex)[1], (*buffer)(i, j, CiIndex)[2]), off );
CqColor opa((*buffer)(i, j, OiIndex)[0], (*buffer)(i, j, OiIndex)[1], (*buffer)(i, j, OiIndex)[2]);
Oi() ->SetColor( opa, off );
TqFloat avopa = ( opa.r() + opa.g() + opa.b() ) /3.0f;
alpha() ->SetFloat( (*buffer)(i, j, coverageIndex)[0] * avopa, off );
s() ->SetFloat( x + i + 0.5, off );
t() ->SetFloat( y + j + 0.5, off );
}
}
// Execute the Shader VM
if ( m_pShader )
{
m_pShader->Evaluate( m_pShaderExecEnv.get() );
alpha() ->SetFloat( 1.0f ); /* by default 3delight/bmrt set it to 1.0 */
}
}
void hmm::hmm_algo(int noIterations){
WordIndex i, j, jj, l, m;
SentPair sent;
time_t a_time1, a_time2, b_time1, b_time2, y_time1, y_time2, k_time1, k_time2, d_time1, d_time2, e_time1, e_time2;
double a_time, b_time, y_time, k_time, d_time, e_time;
for(int it=1;it <= noIterations; it++){
cout<<"hidden markov iteration ("<<it<<")"<<endl;
cout<<"---------------------------------------"<<endl;
//Initialization
cout<<"...........Initializing..........."<<endl;
sHander.new_start();
while(sHander.getNextSentence(sent)){
vector<WordIndex>& es = sent.esent;
vector<WordIndex>& fs = sent.fsent;
l = es.size() - 1;
m = fs.size() - 1;
for(j=0;j <= l;j++){
count_e[es[j]] = 0;
count_jl[j*G1+l] = 0;
for(i=1;i <= m;i++)
cal_ef[WordPairIds(es[j], fs[i])].count = 0;
for(jj=0;jj <= l;jj++)
count_jj_jl[jj*G2+j*G1+l] = 0;
}
}
a_time = 0;b_time = 0;y_time = 0;k_time = 0;d_time = 0;e_time = 0;
//backward-forward learning
cout<<"...........backward-forward learning..........."<<endl;
sHander.new_start();
while(sHander.getNextSentence(sent)){
vector<WordIndex>& es = sent.esent;
vector<WordIndex>& fs = sent.fsent;
l = es.size() - 1;
m = fs.size() - 1;
double uniform = 1.0/(double)(l+1);
double temp_i, temp_max;
time(&a_time1);
//learning alpha parameters
array2<double> alpha(m+1, l+1);
for(j=0;j <= l;j++)
alpha(1,j) = uniform * cal_ef[WordPairIds(es[j], fs[1])].prob;
for(i=1;i <= m-1;i++){
for(jj=0;jj <= l;jj++){
temp_max = alpha(i, 0) * p_jj_jl[jj*G2+0*G1+l];
for(j=1;j <= l;j++){
temp_i = alpha(i, j) * p_jj_jl[jj*G2+j*G1+l];
if(temp_i > temp_max)
temp_max = temp_i;
}
alpha(i+1, jj) = temp_max * cal_ef[WordPairIds(es[jj], fs[i+1])].prob;
}
}
time(&a_time2);
a_time += difftime(a_time2, a_time1);
time(&b_time1);
//learning beta parameters
array2<double> beta(m+1, l+1);
for(j=0;j <= l;j++)
beta(m, j) = 1.0;
for(i=m;i >= 2;i--){
for(j=0;j <= l;j++){
temp_max = beta(i, 0) * cal_ef[WordPairIds(es[0], fs[i])].prob * p_jj_jl[0*G2+j*G1+l];
for(jj=1;jj <= l;jj++){
temp_i = beta(i, jj) * cal_ef[WordPairIds(es[jj], fs[i])].prob * p_jj_jl[jj*G2+j*G1+l];
if(temp_i > temp_max)
temp_max = temp_i;
}
beta(i-1, j) = temp_max;
}
}
time(&b_time2);
b_time += difftime(b_time2, b_time1);
time(&y_time1);
//learning yita parameters
array2<double> yita(m+1, l+1);
for(i=1;i <= m;i++){
double sum_yita = 0.0;
for(j=0;j <= l;j++)
sum_yita += alpha(i, j) * beta(i, j);
for(j=0;j <= l;j++)
yita(i, j) = alpha(i, j) * beta(i, j) / sum_yita;
}
time(&y_time2);
y_time += difftime(y_time2, y_time1);
time(&k_time1);
//learning kesi parameters
map<WordIndex, double> kesi;
for(i=1;i <= m-1;i++){
double sum_kesi = 0.0;
for(j=0;j <= l;j++)
for(jj=0;jj <= l;jj++)
sum_kesi += alpha(i, j) * p_jj_jl[jj*G2+j*G1+l] * cal_ef[WordPairIds(es[jj], fs[i+1])].prob * beta(i+1, jj);
for(j=0;j <= l;j++)
for(jj=0;jj <= l;jj++)
kesi[i*G2+j*G1+jj] = alpha(i, j) * p_jj_jl[jj*G2+j*G1+l] * cal_ef[WordPairIds(es[jj], fs[i+1])].prob * beta(i+1, jj) / sum_kesi;
}
time(&k_time2);
k_time += difftime(k_time2, k_time1);
time(&d_time1);
//calculate d_jj_jl, d_ij;
array2<double> d_jj_jl(l+1, l+1);
for(j=0;j <= l;j++){
for(jj=0;jj <= l;jj++){
double sum_nume = 0, sum_deno = 0;
for(i=1;i <= m-1;i++){
sum_nume += kesi[i*G2+j*G1+jj];
sum_deno += yita(i, j);
}
d_jj_jl(jj, j) = sum_nume / sum_deno;
}
}
array2<double> d_ij(m+1, l+1, 0.0);
for(j=0;j <= l;j++){
double sum_deno = 0;
for(i=1;i <= m;i++)
sum_deno += yita(i, j);
for(i=1;i <= m;i++)
d_ij(i, j) = yita(i, j) / sum_deno;
}
time(&d_time2);
d_time += difftime(d_time2, d_time1);
time(&e_time1);
//em algorithm calculation count
for(j=0;j <= l;j++){
for(jj=0;jj <= l;jj++){
count_jj_jl[jj*G2+j*G1+l] += d_jj_jl(jj, j);
count_jl[j*G1+l] += d_jj_jl(jj, j);
}
}
for(i=1;i <= m;i++){
for(j=0;j <= l;j++){
cal_ef[WordPairIds(es[j], fs[i])].count += d_ij(i, j);
count_e[es[j]] += d_ij(i, j);
}
}
time(&e_time2);
e_time += difftime(e_time2, e_time1);
}//end of backward-forward learning
printf("alpha time:%.4fs beta time:%.4fs yita time:%.4fs kesi time:%.4fs d time:%.4fs em time:%.4fs", a_time, b_time, y_time, k_time, d_time, e_time);
//calculate new probability cal_ef and p_jj_jl
cout<<"...........calculate new probability..........."<<endl;
sHander.new_start();
while(sHander.getNextSentence(sent)){
vector<WordIndex>& es = sent.esent;
vector<WordIndex>& fs = sent.fsent;
l = es.size() - 1;
m = fs.size() - 1;
for(j=0;j <= l;j++){
for(i=1;i <= m;i++)
cal_ef[WordPairIds(es[j], fs[i])].prob = cal_ef[WordPairIds(es[j], fs[i])].count / count_e[es[j]];
for(jj=0;jj <= l;jj++)
p_jj_jl[jj*G2+j*G1+l] = count_jj_jl[jj*G2+j*G1+l] / count_jl[j*G1+l];
}
}//end of update probability cal_ed and p_jj_jl
}//end of Iterations
}//end of hmm_algo
void run( Vector<Real> &s, Real &snorm, Real &del, int &iflag, int &iter, const Vector<Real> &x,
const Vector<Real> &grad, const Real &gnorm, ProjectedObjective<Real> &pObj ) {
Real tol = std::sqrt(ROL_EPSILON<Real>()), zero(0), one(1), two(2), half(0.5);
const Real gtol = std::min(tol1_,tol2_*gnorm);
// Gradient Vector
g_->set(grad);
Real normg = gnorm;
if ( pObj.isConActivated() ) {
primalVector_->set(grad.dual());
pObj.pruneActive(*primalVector_,grad.dual(),x);
g_->set(primalVector_->dual());
normg = g_->norm();
}
// Old and New Step Vectors
s.zero(); s_->zero();
snorm = zero;
Real snorm2(0), s1norm2(0);
// Preconditioned Gradient Vector
//pObj.precond(*v,*g,x,tol);
pObj.reducedPrecond(*v_,*g_,x,grad.dual(),x,tol);
// Basis Vector
p_->set(*v_);
p_->scale(-one);
Real pnorm2 = v_->dot(g_->dual());
iter = 0; iflag = 0;
Real kappa(0), beta(0), sigma(0), alpha(0), tmp(0), sMp(0);
Real gv = v_->dot(g_->dual());
pRed_ = zero;
for (iter = 0; iter < maxit_; iter++) {
//pObj.hessVec(*Hp,*p,x,tol);
pObj.reducedHessVec(*Hp_,*p_,x,grad.dual(),x,tol);
kappa = p_->dot(Hp_->dual());
if (kappa <= zero) {
sigma = (-sMp+sqrt(sMp*sMp+pnorm2*(del*del-snorm2)))/pnorm2;
s.axpy(sigma,*p_);
iflag = 2;
break;
}
alpha = gv/kappa;
s_->set(s);
s_->axpy(alpha,*p_);
s1norm2 = snorm2 + two*alpha*sMp + alpha*alpha*pnorm2;
if (s1norm2 >= del*del) {
sigma = (-sMp+sqrt(sMp*sMp+pnorm2*(del*del-snorm2)))/pnorm2;
s.axpy(sigma,*p_);
iflag = 3;
break;
}
pRed_ += half*alpha*gv;
s.set(*s_);
snorm2 = s1norm2;
g_->axpy(alpha,*Hp_);
normg = g_->norm();
if (normg < gtol) {
break;
}
//pObj.precond(*v,*g,x,tol);
pObj.reducedPrecond(*v_,*g_,x,grad.dual(),x,tol);
tmp = gv;
gv = v_->dot(g_->dual());
beta = gv/tmp;
p_->scale(beta);
p_->axpy(-one,*v_);
sMp = beta*(sMp+alpha*pnorm2);
pnorm2 = gv + beta*beta*pnorm2;
}
if (iflag > 0) {
pRed_ += sigma*(gv-half*sigma*kappa);
}
if (iter == maxit_) {
iflag = 1;
}
if (iflag != 1) {
iter++;
}
snorm = s.norm();
TrustRegion<Real>::setPredictedReduction(pRed_);
}
