bool HeightmapSampling::generateTemplateOnHull(GraspTemplate& templt,
const Vector3d& ref, double z_angle)
{
/* find nearest point */
double min_dist = numeric_limits<double>::max();
if (search_points_->size() == 0 || normals_.size() == 0)
return false;
PointXYZ closest = *search_points_->begin();
Normal min_n = *normals_.begin();
PointCloud<Normal>::const_iterator n_it = normals_.begin();
for (PointCloud<PointXYZ>::const_iterator p_it = search_points_->begin();
n_it != normals_.end() && p_it != search_points_->end(); n_it++, p_it++)
{
Vector3d diff = ref - Vector3d(p_it->x, p_it->y, p_it->z);
double dist = diff.norm();
if (dist < min_dist)
{
min_dist = dist;
closest = *p_it;
min_n = *n_it;
}
}
Vector3d center(closest.x, closest.y, closest.z);
Quaterniond orientation;
computeTempltOrientation(min_n, 0, orientation);
return generateTemplate(templt, center, orientation);
}
bool HeightmapSampling::generateTemplateOnHull(GraspTemplate& templt, const HsIterator& it)
{
const PointXYZ& p = search_points_->points[it.index_];
Vector3d center(p.x, p.y, p.z);
Quaterniond orientation;
computeTempltOrientation(normals_.points[it.index_], it.current_angle_, orientation);
return generateTemplate(templt, center, orientation);
}
void SelfLocator::resampling()
{
// swap sample arrays
Sample* oldSet = samples->swap();
const int numberOfSamples(samples->size());
const float weightingsSum(totalWeighting);
const float resamplingPercentage(sampleTemplateGenerator.templatesAvailable() ? max(0.0f, 1.0f - fastWeighting / slowWeighting) : 0.0f);
const float numberOfResampledSamples = parameter->disableSensorResetting ? numberOfSamples : numberOfSamples * (1.0f - resamplingPercentage);
const float threshold = parameter->resamplingThreshold * weightingsSum / numberOfSamples;
const float weightingBetweenTwoDrawnSamples((weightingsSum + threshold * numberOfSamples) / numberOfResampledSamples);
float nextPos(randomFloat() * weightingBetweenTwoDrawnSamples);
float currentSum(0);
// resample:
int j(0);
for(int i = 0; i < numberOfSamples; ++i)
{
currentSum += oldSet[i].weighting + threshold;
while(currentSum > nextPos && j < numberOfSamples)
{
samples->at(j++) = oldSet[i];
nextPos += weightingBetweenTwoDrawnSamples;
}
}
// fill up remaining samples with new poses:
if(sampleTemplateGenerator.templatesAvailable())
for(; j < numberOfSamples; ++j)
generateTemplate(samples->at(j));
else if(j) // in rare cases, a sample is missing, so add one (or more...)
for(; j < numberOfSamples; ++j)
samples->at(j) = samples->at(rand() % j);
else // resampling was not possible (for unknown reasons), so create a new sample set (fail safe)
#ifdef NDEBUG
{
for(int i = 0; i < numberOfSamples; ++i)
{
Sample& sample = samples->at(i);
Pose2D pose(bb->theFieldDimensions.randomPoseOnField());
sample.translation = Vector2<int>(int(pose.translation.x), int(pose.translation.y));
sample.rotation = Vector2<int>(int(cos(pose.rotation) * 1024), int(sin(pose.rotation) * 1024));
}
poseCalculator->init();
}
#else
ASSERT(false);
#endif
}
static int SendTemplate(MHD_Connection* connection, bool redirect = false,
const char* redirectUrl = nullptr) {
SkString debuggerTemplate = generateTemplate(SkString(FLAGS_source[0]));
MHD_Response* response = MHD_create_response_from_buffer(
debuggerTemplate.size(),
(void*) const_cast<char*>(debuggerTemplate.c_str()),
MHD_RESPMEM_MUST_COPY);
MHD_add_response_header (response, "Access-Control-Allow-Origin", "*");
int status = MHD_HTTP_OK;
if (redirect) {
MHD_add_response_header (response, "Location", redirectUrl);
status = MHD_HTTP_SEE_OTHER;
}
int ret = MHD_queue_response(connection, status, response);
MHD_destroy_response(response);
return ret;
}
void SelfLocator::update(PotentialRobotPose& robotPose)
{
//MODIFY("module:SelfLocator:parameter", *parameter);
// recreate sampleset if number of samples was changed
ASSERT(samples);
if(parameter->numberOfSamples != samples->size())
init();
// Maybe pose has already been computed by call to other update function (for clusters)
if(lastPoseComputationTimeStamp == bb->theFrameInfo.time)
{
robotPose = (PotentialRobotPose&)lastComputedPose;
return;
}
// Normal computation:
preExecution(robotPose);
bool templatesOnly(false);
bool odometryOnly(false);
//DEBUG_RESPONSE("module:SelfLocator:templates only", templatesOnly = true;);
//DEBUG_RESPONSE("module:SelfLocator:odometry only", odometryOnly = true;);
if(templatesOnly) //debug
{
if(sampleTemplateGenerator.templatesAvailable())
for(int i = 0; i < this->samples->size(); ++i)
generateTemplate(samples->at(i));
poseCalculator->calcPose(robotPose);
}
else if(odometryOnly) //debug
{
robotPose += bb->theOdometryData - lastOdometry;
lastOdometry = bb->theOdometryData;
}
else //normal case
{
motionUpdate(updatedBySensors);
updatedBySensors = applySensorModels();
if(updatedBySensors)
{
adaptWeightings();
resampling();
}
poseCalculator->calcPose(robotPose);
}
robotPose.deviation = 100000.f;
lastComputedPose = robotPose;
lastPoseComputationTimeStamp = bb->theFrameInfo.time;
// drawings
/*DECLARE_DEBUG_DRAWING("module:SelfLocator:poseCalculator", "drawingOnField"); // Draws the internal representations for pose computation
COMPLEX_DRAWING("module:SelfLocator:poseCalculator", poseCalculator->draw());
DECLARE_DEBUG_DRAWING("module:SelfLocator:perceptValidityChecker", "drawingOnField"); // Draws the internal representations for percept validity checking
COMPLEX_DRAWING("module:SelfLocator:perceptValidityChecker", perceptValidityChecker->draw());
DECLARE_DEBUG_DRAWING("module:SelfLocator:samples", "drawingOnField"); // Draws the internal representations of the goal locator
COMPLEX_DRAWING("module:SelfLocator:samples", drawSamples());
DECLARE_DEBUG_DRAWING("module:SelfLocator:templates", "drawingOnField"); // Draws all available templates
COMPLEX_DRAWING("module:SelfLocator:templates", sampleTemplateGenerator.draw(););
DECLARE_DEBUG_DRAWING("module:SelfLocator:selectedPoints", "drawingOnField");*/
//fieldModel.draw();
}
void buildModel(int sel_i, TFile *fin, TDirectory *fout){
// Define outputs
RooWorkspace *wout = new RooWorkspace();
wout->SetName(Form("normalization_cat%d",sel_i));
fout->cd();
// Input to this is TTrees
// Setup the "x" variable and weights
RooRealVar mvamet("metRaw","metRaw",200,1200);
//RooRealVar mvamet("jet1mprune","jet1mprune",0,200);
//RooRealVar mvamet("jet1tau2o1","jet1tau2o1",0,1);
wout->import(mvamet);
// TH1F Base Style
std::string lName = "basehist";
//const int numberofBins = 18;
//double myBins[numberofBins+1] = {200,210,220,230,240,250,260,270,280,290,300,310,320,330,350,380,430,500,1200};
//TH1F *lMet = new TH1F(lName.c_str(),lName.c_str(),numberofBins,myBins);
TH1F *lMet = new TH1F(lName.c_str(),lName.c_str(),20,200,1200);
//TH1F *lMet = new TH1F(lName.c_str(),lName.c_str(),20,0,1);
// Make Datasets
makeAndImportDataSets(fin,wout,mvamet);
// ==========================================================================================================
const int nProcs = 27;
std::string procnames[nProcs];
procnames[0] = "DY";
procnames[1] = "RDY";
procnames[2] = "W";
procnames[3] = "WHT";
procnames[4] = "TT";
procnames[5] = "T";
procnames[6] = "ZZ";
procnames[7] = "WW";
procnames[8] = "WZ";
procnames[9] = "WH0";
procnames[10] = "ZH0";
procnames[11] = "GGH0";
procnames[12] = "VBFH0";
//procnames[12] = "DY_control_bkg_mc";
procnames[13] = "Zvv_control_mc";
procnames[14] = "T_control_bkg_mc";
procnames[15] = "TT_control_bkg_mc";
procnames[16] = "WW_control_bkg_mc";
procnames[17] = "WZ_control_bkg_mc";
procnames[18] = "ZZ_control_bkg_mc";
procnames[19] = "Wlv_control_mc_1";
procnames[20] = "Wlv_control_mc_2";
procnames[21] = "T_sl_control_bkg_mc";
procnames[22] = "TT_sl_control_bkg_mc";
procnames[23] = "WW_sl_control_bkg_mc";
procnames[24] = "WZ_sl_control_bkg_mc";
procnames[25] = "ZZ_sl_control_bkg_mc";
procnames[26] = "DY_sl_control_bkg_mc";
// Fill TF1s which do not need corrections
for (int p0=0;p0<nProcs;p0++){
std::cout << "Filling hist for " << procnames[p0] << std::endl;;
TH1F *hist_ = (TH1F*)generateTemplate(lMet, (TTree*)fin->Get(procnames[p0].c_str()), mvamet.GetName(), "weight",cutstring); // standard processes are TTrees
hist_->Write();
}
std::cout << "Filling hist for " << "data_obs" << std::endl;;
TH1F *hist_ = (TH1F*)generateTemplate(lMet, (TTree*)fin->Get("data_obs"), mvamet.GetName(), "",cutstring); // standard processes are TTrees
hist_->Write();
std::cout << "Filling hist for " << "Zvv_control" << std::endl;;
hist_ = (TH1F*)generateTemplate(lMet, (TTree*)fin->Get("Zvv_control"), mvamet.GetName(), "",cutstring); // standard processes are TTrees
hist_->Write();
std::cout << "Filling hist for " << "Wlv_control" << std::endl;;
hist_ = (TH1F*)generateTemplate(lMet, (TTree*)fin->Get("Wlv_control"), mvamet.GetName(), "",cutstring); // standard processes are TTrees
hist_->Write();
// ==========================================================================================================
// Fit backgrounds to produce fit model
#ifdef RUN_CORRECTION
buildAndFitModels(fout,wout,mvamet,"Zvv");
buildAndFitModels(fout,wout,mvamet,"Wlv");
double mcyield = wout->data("DY")->sumEntries();
double datayield = wout->var("num_Zvv")->getVal(); // post fit number of data Z->mumu in control
std::cout << "sfactor" << brscaleFactorZvv*datayield/mcyield << std::endl;
TH1F *hist_zvv = (TH1F*)generateTemplate(lMet, (RooFormulaVar*)wout->function("ratio_Zvv") , *(wout->var(mvamet.GetName())), (RooDataSet*) wout->data("DY")
//TH1F *hist_zvv = (TH1F*)generateTemplate(lMet, (RooFormulaVar*)wout->function("") , *(wout->var(mvamet.GetName())), (RooDataSet*) wout->data("DY")
, 1 /*run correction*/
, 1 /*brscaleFactorZvv*datayield/mcyield*/ /*additional weight*/);
hist_zvv->Write();
std::cout << " DataCardInfo ---------------- " << std::endl;
std::cout << Form(" Zvv_norm gmN %d %g ",(int)datayield,hist_zvv->Integral()/datayield) << std::endl;
std::cout << " ----------------------------- " << std::endl;
// Also correct normalization data why not?
hist_zvv = (TH1F*)generateTemplate(lMet, (RooFormulaVar*)wout->function("ratio_Zvv") , *(wout->var(mvamet.GetName())), (RooDataSet*) wout->data("Zvv_control_mc")
, 1 /*run correction*/
, 1. /*additional weight*/);
hist_zvv->Write();
// Single muon
mcyield = wout->data("W")->sumEntries();
datayield = wout->var("num_Wlv")->getVal(); // post fit number of data W->munu in control
std::cout << "sfactor" << brscaleFactorWlv*datayield/mcyield << std::endl;
TH1F *hist_wlv = (TH1F*)generateTemplate(lMet, (RooFormulaVar*)wout->function("ratio_Wlv") , *(wout->var(mvamet.GetName())), (RooDataSet*) wout->data("W")
, 1 /*run correction*/
, 1./*brscaleFactorWlv*datayield/mcyield*/ /*additional weight*/);
hist_wlv->Write();
std::cout << " DataCardInfo ---------------- " << std::endl;
std::cout << Form(" Wlv_norm gmN %d %g ",(int)datayield,hist_wlv->Integral()/datayield) << std::endl;
std::cout << " ----------------------------- " << std::endl;
// Also correct normalization data why not?
hist_wlv = (TH1F*)generateTemplate(lMet, (RooFormulaVar*)wout->function("ratio_Wlv") , *(wout->var(mvamet.GetName())), (RooDataSet*) wout->data("Wlv_control_mc")
, 1 /*run correction*/
, 1. /*additional weight*/);
hist_wlv->Write();
//buildAndFitModels(fout,wout,mvamet,"Wlv");
//TH1F *hist_wlv = (TH1F*)generateTemplate(lMet, (RooFormulaVar*)wout->function("ratio_Wlv"), &mvamet, (RooDataSet*) wout->data(""));
//hist_wlv->Write();
#endif
// Since the W came in 2 parts, we can make the histogram based on the dataset (called uncorrected)
TH1F *hist_wlv_uc = (TH1F*)generateTemplate(lMet, (RooFormulaVar*)wout->function("") , *(wout->var(mvamet.GetName())), (RooDataSet*) wout->data("W")
, 1 /*run correction forwards*/
, 1./*brscaleFactorWlv*datayield/mcyield*/ /*additional weight*/);
hist_wlv_uc->Write();
#ifdef RUN_BKGSYS
// Load and run systematics from fit model
std::vector<TH1F> v_th1f_Z;
generateVariations(lMet,(RooFitResult*)fout->Get("fitResult_Zvv_control"),(RooFormulaVar*)wout->function("ratio_Zvv"),wout->var(mvamet.GetName()),v_th1f_Z,wout,"DY");
std::vector<TH1F> v_th1f_W;
generateVariations(lMet,(RooFitResult*)fout->Get("fitResult_Wlv_control"),(RooFormulaVar*)wout->function("ratio_Wlv"),wout->var(mvamet.GetName()),v_th1f_W,wout,"W");
// Nice plots
hist_zvv = (TH1F*)fout->Get("th1f_corrected_DY");
hist_wlv = (TH1F*)fout->Get("th1f_corrected_W");
double norm = hist_zvv->Integral();
int colit=2, styleit=1;
TCanvas *can_zvv_systs = new TCanvas("can_zvv_systs","can_zvv_systs",800,600);
TLegend *leg = new TLegend(0.6,0.4,0.89,0.89); leg->SetFillColor(0); leg->SetTextFont(42);
hist_zvv->SetLineColor(1);hist_zvv->SetLineWidth(3); hist_zvv->Draw();
for (std::vector<TH1F>::iterator hit=v_th1f_Z.begin();hit!=v_th1f_Z.end();hit++){
hit->SetLineColor(colit);
hit->SetLineWidth(3);
hit->SetLineStyle(styleit%2+1);
leg->AddEntry(&(*hit),hit->GetName(),"L");
hit->Scale(norm/hit->Integral());
hit->Draw("same");
hit->Write();
styleit++;
if (styleit%2==1) colit++;
}
leg->Draw();
can_zvv_systs->Write();
norm = hist_wlv->Integral();
styleit=1; colit=2;
TCanvas *can_wlv_systs = new TCanvas("can_wlv_systs","can_wlv_systs",800,600);
TLegend *leg_2 = new TLegend(0.6,0.4,0.89,0.89); leg_2->SetFillColor(0); leg_2->SetTextFont(42);
hist_wlv->SetLineColor(1);hist_wlv->SetLineWidth(3); hist_wlv->Draw();
for (std::vector<TH1F>::iterator hit=v_th1f_W.begin();hit!=v_th1f_W.end();hit++){
hit->SetLineColor(colit);
hit->SetLineWidth(3);
hit->SetLineStyle(styleit%2+1);
leg_2->AddEntry(&(*hit),hit->GetName(),"L");
hit->Scale(norm/hit->Integral());
hit->Draw("same");
hit->Write();
styleit++;
if (styleit%2==1) colit++;
}
leg_2->Draw();
can_wlv_systs->Write();
#endif
// Save the work
fout->cd();
wout->Write();
// Done!
}
