int main (int argc, char *argv[]){
if (argc != 2){
printf("Usage : %s <nombre de fils voulus>", argv[0]);
return 1;
}
observation(atoi(argv[1]));
return 0;
}
static void read(const cv::FileNode& node, observation& x,
const observation& default_value = observation()) {
if (node.empty()) {
x = default_value;
} else {
x.cam_id = (int) node["cam_id"];
x.point_id = (int) node["point_id"];
x.coord[0] = (float) node["coord0"];
x.coord[1] = (float) node["coord1"];
}
}
void
MultiviewRecognizerWithChangeDetection<PointT>::findChangedPoints(
Cloud observation_unposed,
Eigen::Affine3f pose,
Cloud &removed_points,
Cloud &added_points) {
CloudPtr observation(new Cloud);
pcl::transformPointCloud(observation_unposed, *observation, pose);
observation = v4r::downsampleCloud<PointT>(observation);
if(!changing_scene->empty()) {
v4r::ChangeDetector<PointT> detector;
detector.detect(changing_scene, observation, pose, param_.tolerance_for_cloud_diff_);
v4r::ChangeDetector<PointT>::removePointsFrom(changing_scene, detector.getRemoved());
removed_points += *(detector.getRemoved());
added_points += *(detector.getAdded());
*changing_scene += added_points;
} else {
added_points += *observation;
*changing_scene += *observation;
}
}
// data list
// ---------------------------------------------------------------------------
void gvf_list(t_gvf *x,const t_symbol *sss, int argc, t_atom *argv)
{
if(argc == 0)
{
post("invalid format, points have at least 1 coordinate");
return;
}
vector<float> observation(argc);
for (int k=0; k<argc; k++)
observation[k] = atom_getfloat(&argv[k]);
switch (x->bubi->getState())
{
case GVF::STATE_LEARNING:
{
x->bubi->addObservation(observation);
break;
}
case GVF::STATE_FOLLOWING:
{
if (x->bubi->getNumberOfGestureTemplates()>0)
{
x->outcomes = x->bubi->update(observation);
int numberOfTemplates = x->bubi->getNumberOfGestureTemplates();
t_atom *outAtoms = new t_atom[numberOfTemplates];
for(int j = 0; j < numberOfTemplates; j++)
SETFLOAT(&outAtoms[j],x->outcomes.alignments[j]);
outlet_list(x->Position, &s_list, numberOfTemplates, outAtoms);
delete[] outAtoms;
int dynamicsDimension = x->bubi->getDynamicsVariance().size();
int scalingsDimension = x->bubi->getScalingsVariance().size();
int rotationDimension = 0;
outAtoms = new t_atom[numberOfTemplates * dynamicsDimension];
for(int j = 0; j < numberOfTemplates; j++)
for(int jj = 0; jj < dynamicsDimension; jj++)
SETFLOAT(&outAtoms[j*dynamicsDimension+jj],x->outcomes.dynamics[j][jj]);
outlet_list(x->Vitesse, &s_list, numberOfTemplates * dynamicsDimension, outAtoms);
delete[] outAtoms;
outAtoms = new t_atom[numberOfTemplates * scalingsDimension];
for(int j = 0; j < numberOfTemplates; j++)
for(int jj = 0; jj < scalingsDimension; jj++)
SETFLOAT(&outAtoms[j*scalingsDimension+jj],x->outcomes.scalings[j][jj]);
outlet_list(x->Scaling, &s_list, numberOfTemplates * scalingsDimension, outAtoms);
delete[] outAtoms;
outAtoms = new t_atom[numberOfTemplates * rotationDimension];
for(int j = 0; j < numberOfTemplates; j++)
for(int jj = 0; jj < rotationDimension; jj++)
SETFLOAT(&outAtoms[j*rotationDimension+jj],x->outcomes.rotations[j][jj]);
outlet_list(x->Rotation, &s_list, numberOfTemplates * rotationDimension, outAtoms);
delete[] outAtoms;
outAtoms = new t_atom[numberOfTemplates];
for(int j = 0; j < numberOfTemplates; j++)
SETFLOAT(&outAtoms[j],x->outcomes.likelihoods[j]);
outlet_list(x->Likelihoods, &s_list, numberOfTemplates, outAtoms);
delete[] outAtoms;
}
break;
}
default:
break;
}
}
void fitSignal(){
std::string shapes_file = "mlfit.root";
std::string data_file = "param_ws.root";
std::string channel = "ch1";
TFile *dfile = TFile::Open(data_file.c_str());
TFile *sfile = TFile::Open(shapes_file.c_str());
TH1F *bkg = (TH1F*)sfile->Get(Form("shapes_fit_b/%s/total",channel.c_str()));
TH1F *data = (TH1F*)dfile->Get("SR_data"); // TH1 for data
TH1F *signal= (TH1F*)dfile->Get("SR_signal"); // TH1 for signal
TH2F *covar = (TH2F*)sfile->Get(Form("shapes_fit_b/%s/total_covar",channel.c_str()));
// bkg and covariance defined as pdf / GeV, so scale by bin widhts
//
int nbins = data->GetNbinsX();
if (!isTH1Input){
for (int b=1;b<=nbins;b++){
double bw = bkg->GetBinWidth(b);
bkg->SetBinContent(b,bkg->GetBinContent(b)*bw);
for (int j=1;j<=nbins;j++){
covar->SetBinContent(b,j,covar->GetBinContent(b,j)*bw*bw);
}
}
}
RooArgList xlist_;
RooArgList olist_;
RooArgList mu_;
bkg->Print() ;
covar->Print() ;
signal->Print() ;
data->Print() ;
// Make a dataset (simultaneous)
RooCategory sampleType("bin_number","Bin Number");
RooRealVar observation("observed","Observed Events bin",1);
// You have to define the samples types first!, because RooFit suuuuucks!
for (int b=1;b<=nbins;b++){
sampleType.defineType(Form("%d",b-1),b-1);
sampleType.setIndex(b-1);
}
RooArgSet obsargset(observation,sampleType);
RooDataSet obsdata("combinedData","Data in all Bins",obsargset);
//obsdata.add(RooArgSet(observation,sampleType));
for (int b=1;b<=nbins;b++){
observation.setVal(data->GetBinContent(b));
sampleType.setIndex(b-1);
std::cout << sampleType.getLabel() << ", " << sampleType.getIndex() << std::endl;
//RooArgSet localset(observation,sampleType);
//obsdata.add(localset);
obsdata.add(RooArgSet(observation,sampleType));
std::cout << " Observed at " << b << ", " << observation.getVal() << std::endl;
}
// make a constraint term for the background, and a RooRealVar for bkg
for (int b=1;b<=nbins;b++){
double bkgy = (double)bkg->GetBinContent(b);
RooRealVar *mean_ = new RooRealVar(Form("exp_bin_%d_In",b),Form("expected bin %d",b),bkgy);
mean_->setConstant(true);
RooRealVar *x_ = new RooRealVar(Form("exp_bin_%d",b),Form("bkg bin %d",b),bkgy,0.2*bkgy,bkgy*4);
std::cout << " Exp background At " << b << ", " << x_->getVal() << std::endl;
xlist_.add(*x_);
mu_.add(*mean_);
}
// constraint PDF for background
// Convert TH2 -> TMatrix
TMatrixDSym Tcovar(nbins);
for (int i=0;i<nbins;i++){
for (int j=0;j<nbins;j++){
//if (i==j)Tcovar[i][j] = covar->GetBinContent(i+1,j+1);
//else Tcovar[i][j] = 0;
Tcovar[i][j] = covar->GetBinContent(i+1,j+1);
}
}
std::cout<< "Made Covariance" << std::endl;
RooMultiVarGaussian constraint_pdf("constraint_pdf","Constraint for background pdf",xlist_,mu_,Tcovar);
std::cout<< "Made Covariance Gauss" << std::endl;
// Make the signal component
RooRealVar r("r","r",1,-5,5);
RooArgList signals_;
for (int b=1;b<=nbins;b++) {
RooProduct *sigF = new RooProduct(Form("signal_%d",b),"signal nominal",RooArgSet(r,RooFit::RooConst(signal->GetBinContent(b))));
std::cout << " Signal At " << b << ", " << sigF->getVal() << std::endl;
signals_.add(*sigF);
}
RooArgList plist_;
RooArgList slist_;
sampleType.setIndex(1);
RooSimultaneous combined_pdf("combined_pdf","combined_pdf",sampleType);
for (int b=1;b<=nbins;b++){
RooAddition *sum = new RooAddition(Form("splusb_bin_%d",b),Form("Signal plus background in bin %d",b),RooArgList(*((RooRealVar*)(signals_.at(b-1))),*((RooRealVar*)(xlist_.at(b-1)))));
RooPoisson *pois = new RooPoisson(Form("pdf_bin_%d",b),Form("Poisson in bin %d",b),observation,(*sum));
// Who cares about poissons?
//RooGaussian *pois = new RooGaussian(Form("pdf_bin_%d",b),Form("Poisson in bin %d",b),observation,(*sum),RooFit::RooConst(TMath::Sqrt(sum->getVal())));
combined_pdf.addPdf(*pois,Form("%d",b-1));
slist_.add(*sum);
plist_.add(*pois);
}
combined_pdf.Print("v");
obsdata.Print("v");
// Make a prodpdf instread
RooProdPdf combinedpdfprod("maybefinalpdf","finalpdf",RooArgList(combined_pdf,constraint_pdf));
RooAbsReal *nll_ = combined_pdf.createNLL(obsdata,RooFit::ExternalConstraints(RooArgList(constraint_pdf)));
//
RooMinimizer m(*nll_);
m.minimize("Minuit2","minimize");
// constrained fit!
r.setConstant(true);
double nllMin = nll_->getVal();
TFile *fout = new TFile("simple.root","RECREATE");
TTree *tree = new TTree("limit","limit");
float deltaNLL_;
float r_;
tree->Branch("r",&r_,"r/F");
tree->Branch("deltaNLL",&deltaNLL_,"deltaNLL/F");
RooMinimizer mc(*nll_);
//combinedpdfprod.fitTo(obsdata);
//
for(float rv=-2;rv<=2;rv+=0.2){
r.setVal(rv);
r_=rv;
mc.minimize("Minuit2","minimize");
deltaNLL_ = nll_->getVal() - nllMin;
tree->Fill();
}
fout->cd();
tree->Write();
fout->Close();
/*
RooAbsReal *nll_ = combinedpdfprod.createNLL(obsdata);
nll_->Print("v");
// Minimize
RooMinimizer m(*nll_);
r.setConstant(true);
std::cout << combinedpdfprod.getVal() << std::endl;
std::cout << constraint_pdf.getVal() << std::endl;
//m.Print();
m.minimize("Minuit2","minimize");
*/
}
Configuration::Configuration(int & argc, char ** argv)
: runNormally_(true),
observationToCheck_(0),
logLevel_(milog::DEBUG),
port_(0) {
const char * USER = std::getenv("USER");
const std::string databaseUser = USER ? USER : "******";
options_description commandLine("Command-line options");
options_description observation("Single observation");
observation.add_options()("station,s", value<int>(), "Check the given station")(
"obstime,o", value<std::string>(), "Check the given obstime")(
"typeid,t", value<int>(), "Check the given typeid")(
"qcx",
value<std::string>(),
"Only run the given check. This also means that control flags will not be reset before checks are run")(
"model-source", value<std::string>(&modelDataName_)->default_value("yr"),
"Get model data from the given source (from model table in database");
options_description logging("Logging");
logging.add_options()(
"runloglevel", value<std::string>()->default_value("info"),
"Set loglevel (debug_all, debug, info, warn, error or fatal")(
"runlogfile",
value<std::string>(&runLogFile_)->default_value(
kvalobs::kvPath(kvalobs::logdir) + "/kvQabased.log"),
"Set file name for run log")(
"logdir",
value<std::string>(&baseLogDir_)->default_value(
kvalobs::kvPath(kvalobs::logdir) + "/checks/"),
"Use the given directory as base directory for script logs");
options_description database("Database");
database.add_options()(
"database,d", value<std::string>(&databaseName_)->default_value("kvalobs"),
"Name of database to connect to")("host,h", value<std::string>(&host_),
"Hostname of database")(
"port,p", value<int>(&port_), "Port of database")(
"user,U", value<std::string>(&user_)->default_value(databaseUser),
"Database user");
options_description generic("Generic");
generic.add_options()
("process-count", value<unsigned>(& processCount_)->default_value(4), "Run the given number of processes")
("config", value<std::string>(), "Read configuration from the given file")
("version", "Produce version information")
("help", "Produce help message");
commandLine.add(observation).add(logging).add(database).add(generic);
parsed_options parsed = command_line_parser(argc, argv).options(commandLine).
//allow_unregistered().
run();
variables_map vm;
store(parsed, vm);
notify(vm);
if (vm.count("version")) {
version(std::cout);
runNormally_ = false;
return;
}
if (vm.count("help")) {
help(std::cout, commandLine);
runNormally_ = false;
return;
}
options_description configFileOptions;
configFileOptions.add(logging).add(database);
if (vm.count("config"))
parse(vm["config"].as<std::string>(), vm, configFileOptions);
boost::filesystem::path defaultConfig = defaultConfigFile();
if (not defaultConfig.empty())
parse(defaultConfig, vm, configFileOptions);
if (vm.count("qcx")) {
std::string qcxFilter = vm["qcx"].as<std::string>();
qcxFilter_.push_back(qcxFilter);
}
if (vm.count("runloglevel")) {
std::string wantedLevel = vm["runloglevel"].as<std::string>();
if (wantedLevel == "debug_all")
logLevel_ = milog::DEBUG1;
else if (wantedLevel == "debug")
logLevel_ = milog::DEBUG;
else if (wantedLevel == "info")
logLevel_ = milog::INFO;
else if (wantedLevel == "warn")
logLevel_ = milog::WARN;
else if (wantedLevel == "error")
logLevel_ = milog::ERROR;
else if (wantedLevel == "fatal")
logLevel_ = milog::FATAL;
else
throw std::runtime_error(wantedLevel + " is not a valid loglevel");
}
//milog::Logger::logger().logLevel(logLevel_);
observationToCheck_ = getStationInfo(vm);
}
void System::get_kalman_path( vector<State>& obs, vector<double>& obs_times, list<State>& kalman_path)
{
#if 0
kalman_path.clear();
kalman_path.push_back(init_state);
Matrix3d Q;
Q << init_var[0], 0, 0, 0, init_var[1], 0, 0, 0, init_var[2];
Vector3d curr_state;
curr_state(0) = init_state.x[0];
curr_state(1) = init_state.x[1];
curr_state(2) = init_state.x[2];
MatrixXd Rk(3,3);
Rk << obs_noise[0], 0, 0, 0, obs_noise[1], 0, 0, 0, obs_noise[2];
Matrix3d Cd = Matrix3d::Identity();
double prev_time = 0;
for(unsigned int i=0; i< obs.size(); i++)
{
State& next_obs = obs[i];
Vector3d noisy_obs;
noisy_obs(0) = next_obs.x[0];
noisy_obs(1) = next_obs.x[1];
noisy_obs(2) = next_obs.x[2];
double delta_t = obs_times[i] - prev_time;
//cout<<"delta_t: "<< delta_t << endl;
State stmp1;
stmp1.x[0] = curr_state(0);
stmp1.x[1] = curr_state(1);
stmp1.x[2] = curr_state(2);
State next_state = integrate(stmp1, delta_t, true);
State clean_obs = observation(next_state, true);
Matrix3d Ad;
Ad << 0, 0, -sin(stmp1.x[2]), 0, 0, cos(stmp1.x[2]), 0, 0, 0;
Matrix3d Wk;
Wk << process_noise[0]*delta_t, 0, 0, 0, process_noise[1]*delta_t, 0, 0, 0, process_noise[2]*delta_t;
Matrix3d Q_new = Ad * Q * Ad.transpose() + Wk;
Matrix3d Lk = Q_new*Cd.transpose()*(Cd*Q_new*Cd.transpose() + Rk).inverse();
Vector3d Sk;
curr_state(0) = next_state.x[0];
curr_state(1) = next_state.x[1];
curr_state(2) = next_state.x[2];
Sk = noisy_obs - Cd*curr_state;
Vector3d estimate = curr_state + Lk*Sk;
Matrix3d covar = (Matrix3d::Identity() - Lk*Cd)*Q_new;
Q = covar;
curr_state = estimate;
State stmp2;
stmp2.x[0] = curr_state(0);
stmp2.x[1] = curr_state(1);
stmp2.x[2] = curr_state(2);
kalman_path.push_back(stmp2);
prev_time = obs_times[i];
}
#endif
}
