//-------------------------------------------------------------------------
//
// Converts pixel coordinates in given zoom level of pyramid to EPSG:900913
//
inline static void PixelsToMeters( const double pixelX, const double pixelY, const MagnificationType magnification, double& meterX, double& meterY )
//-------------------------------------------------------------------------
{
double res = Resolution( magnification );
meterX = pixelX * res - OriginShift;
meterY = pixelY * res - OriginShift;
}
Timestamp::TimeVal Timestamp::CurrentTime() {
TimeVal ts;
#if defined(UKN_OS_WINDOWS)
FILETIME ft;
GetSystemTimeAsFileTime(&ft);
ULARGE_INTEGER epoch; // UNIX epoch (1970-01-01 00:00:00) expressed in Windows NT FILETIME
epoch.LowPart = 0xD53E8000;
epoch.HighPart = 0x019DB1DE;
ULARGE_INTEGER tts;
tts.LowPart = ft.dwLowDateTime;
tts.HighPart = ft.dwHighDateTime;
tts.QuadPart -= epoch.QuadPart;
ts = tts.QuadPart/10;
#else
struct timeval tv;
if (gettimeofday(&tv, NULL))
throw ("cannot get time of day");
ts = TimeVal(tv.tv_sec)*Resolution() + tv.tv_usec;
#endif
return ts;
}
Resolution OptionsWindowSize::convert(std::string text)
{
auto resolution = Resolution(0, 0);
resolution.height = atoi((text.substr(text.find("x") + 1)).c_str());
resolution.width = atoi((text.substr(0, text.find("x") - 1)).c_str());
return resolution;
}
//-------------------------------------------------------------------------
//
// Converts EPSG:900913 to pyramid pixel coordinates in given zoom level
//
inline static void MetersToPixels( const double meterX, const double meterY, const MagnificationType magnification, double& pixelX, double& pixelY )
//-------------------------------------------------------------------------
{
double res = Resolution( magnification );
pixelX = ( meterX + OriginShift ) / res;
pixelY = ( meterY + OriginShift ) / res;
}
void setResolution(MapGenerator* pGenerator, int res)
{
if (pGenerator == NULL)
{
displayMessage("Stream does not exist!");
return;
}
XnMapOutputMode Mode;
pGenerator->GetMapOutputMode(Mode);
Mode.nXRes = Resolution((XnResolution)res).GetXResolution();
Mode.nYRes = Resolution((XnResolution)res).GetYResolution();
XnStatus nRetVal = pGenerator->SetMapOutputMode(Mode);
if (nRetVal != XN_STATUS_OK)
{
displayMessage("Failed to set resolution: %s", xnGetStatusString(nRetVal));
}
}
// --------------------------------
// Code
// --------------------------------
void initConstants()
{
// Primary Streams
int nIndex = 0;
g_PrimaryStream.pValues[nIndex++] = "Any";
g_PrimaryStream.pValues[nIndex++] = xnProductionNodeTypeToString(XN_NODE_TYPE_DEPTH);
g_PrimaryStream.pValues[nIndex++] = xnProductionNodeTypeToString(XN_NODE_TYPE_IMAGE);
g_PrimaryStream.pValues[nIndex++] = xnProductionNodeTypeToString(XN_NODE_TYPE_IR);
g_PrimaryStream.pValues[nIndex++] = xnProductionNodeTypeToString(XN_NODE_TYPE_AUDIO);
g_PrimaryStream.nValuesCount = nIndex;
// Registration
nIndex = 0;
g_Registration.pValues[nIndex++] = FALSE;
g_Registration.pValueToName[FALSE] = "Off";
g_Registration.pValues[nIndex++] = TRUE;
g_Registration.pValueToName[TRUE] = "Depth -> Image";
g_Registration.nValuesCount = nIndex;
// Resolutions
nIndex = 0;
g_Resolution.pValues[nIndex++] = XN_RES_QVGA;
g_Resolution.pValueToName[XN_RES_QVGA] = Resolution(XN_RES_QVGA).GetName();
g_Resolution.pValues[nIndex++] = XN_RES_VGA;
g_Resolution.pValueToName[XN_RES_VGA] = Resolution(XN_RES_VGA).GetName();
g_Resolution.pValues[nIndex++] = XN_RES_SXGA;
g_Resolution.pValueToName[XN_RES_SXGA] = Resolution(XN_RES_SXGA).GetName();
g_Resolution.pValues[nIndex++] = XN_RES_UXGA;
g_Resolution.pValueToName[XN_RES_UXGA] = Resolution(XN_RES_UXGA).GetName();
g_Resolution.nValuesCount = nIndex;
}
int main(int argc, char** argv) {
ros::init(argc, argv, "Cam_Setup");
//cv::initModule_nonfree(); //initialize nonfree opencv modules
BaxterCamera right_cam("right_hand_camera");
right_cam.display();
ros::spinOnce();
std::cout<<"Setting up right cam...\n";
right_cam.resolution(Resolution(320,200));
right_cam.half_resolution(false);
right_cam.window(Resolution(560,190)); // (570, 150)
right_cam.gain(15);
right_cam.exposure(10);
BaxterCamera left_cam("left_hand_camera");
left_cam.display();
ros::spinOnce();
/*std::cout<<"Setting up left_cam...\n";
left_cam.resolution(Resolution(320,200));
left_cam.half_resolution(false);
left_cam.window(Resolution(555,220)); // (570, 150)
left_cam.gain(15);
*/
std::cout<<"Starting trackbar...\n";
cv::namedWindow(window_name, CV_WINDOW_AUTOSIZE);
cv::createTrackbar("Threshold", window_name, &thresh_val, max_value, threshold);
threshold(0,0);
while(ros::ok()) {
left = left_cam.cvImage();
right = right_cam.cvImage();
ros::spinOnce();
}
return 0;
}
void Resolution::scanPossibleResolutions()
{
SDL_Rect** modes = SDL_ListModes(NULL, SDL_OPENGL|SDL_FULLSCREEN);
for ( int curModeNr=0; modes[curModeNr]; ++curModeNr ) {
Resolution tryRes = Resolution( modes[curModeNr]->w, modes[curModeNr]->h, 24, true );
if ( checkResolutionAndAdjustBpp( tryRes ) ) {
if ( tryRes.width >= 800 && tryRes.height >= 600 ) {
possibleResolutions.push_back( tryRes );
tryRes.fullscreen = false;
possibleResolutions.push_back( tryRes );
}
}
}
}
Node::Node()
: scan_(parameters_)
{
parameters_("map_size", Size(1024, 1024, 1024));
parameters_("map_resolution", Resolution(0.05, 0.05, 0.05));
parameters_("map_frame", p_map_frame_ = "map");
parameters_("base_frame", p_base_frame_ = "base_link");
parameters_("odom_frame", p_odom_frame_ = "base_link");
parameters_("use_tf_scan_transformation", p_use_tf_scan_transformation_ = true);
parameters_("use_tf_pose_start_estimate", p_use_tf_pose_start_estimate_ = false);
parameters_("pub_map_odom_transform_", p_pub_map_odom_transform_ = true);
parameters_("advertise_map_service", p_advertise_map_service_ = true);
parameters_("map_update_distance_thresh", p_map_update_translational_threshold_ = 0.4);
parameters_("map_update_angle_thresh", p_map_update_angular_threshold_ = 0.9);
}
void CheckParams(int again)
{
char *textsample;
char *textsample2;
int four;
int nbblocks;
int squaresize;
four = 4;
textsample2 = ft_AssignBlocks(ft_CpySample());
textsample = ft_CpySample();
nbblocks = ft_NbBlocks(textsample2);
if (CheckTxt(textsample, nbblocks))
{
squaresize = ft_SizeFinalSquare(nbblocks, four, again);
Resolution(textsample2, four, nbblocks, squaresize);
}
}
int main( int argc, char *argv[] ) {
Game mGame;
Window mWindow( Resolution( 1024, 768 ) );
mGame.Create();
int currentTime = SDL_GetTicks();
while( mGame.isRunning() ) {
int newTime = SDL_GetTicks();
int frameTime = newTime - currentTime;
currentTime = newTime;
SDL_Event event;
while ( SDL_PollEvent( &event ) ) {
switch( event.type ) {
case SDL_KEYDOWN:
mGame.KeyPress( &event.key.keysym );
break;
case SDL_KEYUP:
mGame.KeyRelease( &event.key.keysym );
break;
case SDL_MOUSEBUTTONDOWN:
mGame.MousePress( event.button.button, event.button.x, event.button.y );
break;
case SDL_MOUSEBUTTONUP:
mGame.MouseRelease( event.button.button, event.button.x, event.button.y );
break;
default:
break;
}
}
mGame.Logic( frameTime );
mGame.Render();
}
mGame.Destroy();
return 0;
}
void GraphicsManager::Initialize()
{
if( !this->Initialized ) {
this->TheEntresol->GetScheduler().AddWorkUnitMonopoly(this->RenderWork, "RenderWork");
if( !this->OgreBeenInitialized ) {
this->InitOgreRenderSystem();
}
Ogre::ConfigOptionMap& CurrentRendererOptions = Ogre::Root::getSingleton().getRenderSystem()->getConfigOptions();
for( Ogre::ConfigOptionMap::iterator configItr = CurrentRendererOptions.begin() ; configItr != CurrentRendererOptions.end() ; configItr++ )
{
if( (configItr)->first == "Video Mode" ) {
for( Whole X = 0 ; X < (configItr)->second.possibleValues.size() ; X++ )
{
String NewRes = (configItr)->second.possibleValues[X];
String ResWidth = NewRes.substr(0,NewRes.find_first_of('x'));
String ResHeight = NewRes.substr(NewRes.find_first_of('x')+1);
StringTools::Trim( ResWidth );
StringTools::Trim( ResHeight );
this->SupportedResolutions.push_back( Resolution( StringTools::ConvertToWhole( ResWidth ) , StringTools::ConvertToWhole( ResHeight ) ) );
}
std::sort(this->SupportedResolutions.begin(),this->SupportedResolutions.end());
}else if( (configItr)->first == "Rendering Device" ) {
for( Whole Y = 0 ; Y < (configItr)->second.possibleValues.size() ; Y++ )
{ this->SupportedDevices.push_back( (configItr)->second.possibleValues[Y] ); }
}
}
if( this->AutoGenFiles ) {
this->SaveAllSettings();
}
this->Initialized = true;
}
}
void Global::loadDefaultResolutions()
{
_resolutions.clear();
_resolutions.push_back(Resolution("176x144-QCIF(11:9)", false));
_resolutions.push_back(Resolution("180x144-PAL D-1(video)(5:4)", false));
_resolutions.push_back(Resolution("240x180-PAL(video)(4:3)", false));
_resolutions.push_back(Resolution("240x192-PAL D-1(video)(5:4)", false));
_resolutions.push_back(Resolution("256x188-70mm IMAX(cine)(64:47)", false));
_resolutions.push_back(Resolution("256x256-6cmx6cm(2 1/4\" x 2 1/4)(slide)(11:9)", false));
_resolutions.push_back(Resolution("320x180-NTSC HDTV(16:9)", true));
_resolutions.push_back(Resolution("320x200-(8:5)", false));
_resolutions.push_back(Resolution("320x240-(4:3)", false));
_resolutions.push_back(Resolution("320x400-(4:5)", false));
_resolutions.push_back(Resolution("320x480-(2:3)", false));
_resolutions.push_back(Resolution("352x288-CIF(11:9)", false));
_resolutions.push_back(Resolution("360x200-(9:5)", false));
_resolutions.push_back(Resolution("360x240-NTSC DV(video)50%, VistaVision, 35mm(24mmx36mm)(slide)(3:2)", false));
_resolutions.push_back(Resolution("360x288-PAL D-1(video) 50% (5:4)", false));
_resolutions.push_back(Resolution("360x400-(9:10)", false));
_resolutions.push_back(Resolution("360x480-(3:4)", false));
_resolutions.push_back(Resolution("360x200-(8:5)", false));
_resolutions.push_back(Resolution("400x300-(4:3)", true));
_resolutions.push_back(Resolution("440x200-700mm Panavision(cine)(11:5)", false));
_resolutions.push_back(Resolution("480x360-PAL(video)(4:3)", false));
_resolutions.push_back(Resolution("480x384-PAL D-1(video)(5:4)", false));
_resolutions.push_back(Resolution("512x384-4\"x5\" or 8\"x10\"(slide) (4:3)", false));
_resolutions.push_back(Resolution("640x360-(16:9)", true));
_resolutions.push_back(Resolution("640x400-(8:5)", false));
_resolutions.push_back(Resolution("640x480-(4:3)", true));
_resolutions.push_back(Resolution("720x480-NTSC DV(video)(3:2)", false));
_resolutions.push_back(Resolution("720x576-PAL D-1(video)(5:4)", false));
_resolutions.push_back(Resolution("768x432-(16:9)", false));
_resolutions.push_back(Resolution("768x576-PAL(video)(4:3)", false));
_resolutions.push_back(Resolution("800x600-(4:3)", true));
_resolutions.push_back(Resolution("854x480-WVGA(427:240)", false));
_resolutions.push_back(Resolution("960x600-(8:5)", false));
_resolutions.push_back(Resolution("1024x436-35mm Anamorphic(2.35:1)", false));
_resolutions.push_back(Resolution("1088x612-(16:9)", false));
_resolutions.push_back(Resolution("1120x640-35mm(cine)(7:4)", false));
_resolutions.push_back(Resolution("1280x720-HDTV(video)(16:9)", true));
_resolutions.push_back(Resolution("1280:800-WXGA(16:10)", true));
_resolutions.push_back(Resolution("1280x960-SXGA-(UVGA)(4:3)", true));
_resolutions.push_back(Resolution("1366x768-HD(16:9)", true));
_resolutions.push_back(Resolution("1440x900-WXGA+(16:10)", true));
_resolutions.push_back(Resolution("1600x900-HD+(16:9)", true));
_resolutions.push_back(Resolution("1600x1200-UXGA(4:3)", true));
_resolutions.push_back(Resolution("1680x1050-WSXGA+(16:10)", true));
_resolutions.push_back(Resolution("1920x1080-FullHD(16:9)", true));
_resolutions.push_back(Resolution("1920x1200-WUXGA(16:10)", false));
_resolutions.push_back(Resolution("2560x1440-WQHD(16:9)", false));
_resolutions.push_back(Resolution("2560x1600-WQXGA(16:10)", false));
}
bool FParse::Resolution( const TCHAR* InResolution, uint32& OutX, uint32& OutY )
{
int32 WindowModeDummy;
return Resolution(InResolution, OutX, OutY, WindowModeDummy);
}
int StopWatch::ElapsedSeconds() const
{
return int(Elapsed()/Resolution());
}
void draw_tof_singleparticle(const string ConfigDir="../..", const string InFileBase = "../../output/SingleParticle_rv02-00-01_sv02-00-01_mILD_l5_o1_v02/", const string OutFileBase = "../../output/SP") {
string plot_config_path = ConfigDir + "/PlottingSettings.cnf";
string ptype_config_path = ConfigDir + "/ParticleTypes.cnf";
string res_config_path = ConfigDir + "/TimeResolutions.cnf";
float min_p = 0, max_p = 100;
float min_beta = 0.5, max_beta = 1.05;
ConfigReader plot_settings_reader(plot_config_path);
auto beta_p_settings = plot_settings_reader["BetaPPlots"];
int bins_p = stoi(beta_p_settings["NbinsBeta"]);
int bins_beta = stoi(beta_p_settings["NbinsP"]);
int every_nth_bin = stoi(beta_p_settings["BinJumpsPerSlice"]); // Consider only every n-th bin (=slice) in the momentum distribution
ConfigReader ptype_reader(ptype_config_path);
ParticleVec particle_types {};
auto ptype_names = ptype_reader.get_keys();
for (auto & ptype_name: ptype_names) {
auto ptype_sec = ptype_reader[ptype_name]; // Section
particle_types.push_back( ParticleType(stoi(ptype_sec["pdgID"]), ptype_sec["name_s"], ptype_name, stod(ptype_sec["mass"]), stoi(ptype_sec["colour"])) );
}
int N_ptypes = particle_types.size();
ConfigReader res_reader(res_config_path);
ResVec resolutions {};
auto res_values = res_reader.get_keys();
for (auto & res_value: res_values) {
auto res_sec = res_reader[res_value];
resolutions.push_back( Resolution(stof(res_value), stoi(res_sec["colour"])) );
}
int N_ress = resolutions.size();
HistoMap beta_histos {};
for ( auto &ptype : particle_types ) {
for ( auto &res : resolutions) {
beta_histos.addHisto( ptype, res );
}
}
// TODO Also loop over different time estimators?? (Here: CH -> Closest Hit)
unique_ptr<TCanvas> c_dummy {new TCanvas("dummy","",1000,800)}; // To write histos to that don't need to be written to current canvas
unique_ptr<TCanvas> c_beta_p {new TCanvas("c_beta_p","#beta_{CH} vs momentum",400*N_ress,400*N_ptypes)};
unique_ptr<TCanvas> c_beta_p_average {new TCanvas("c_beta_p_average","#beta_{CH}^{average} vs momentum",400*N_ress,400*N_ptypes)};
c_beta_p->Clear();
c_beta_p_average->Clear();
c_beta_p->Divide(resolutions.size(), particle_types.size());
c_beta_p_average->Divide(resolutions.size(), particle_types.size());
int i_pad=0;
vector<TFile*> closables {}; // Because ROOT... To collect files that I need to close later
unique_ptr<TFile> file_beta_p {new TFile ( ( OutFileBase + "_beta_p.root" ).c_str() ,"recreate")};
// Get the beta vs p histograms and save for each particle type
for ( auto & ptype : particle_types ) {
TFile* file = new TFile ( ( InFileBase + to_string(ptype.pdg_id) + "_lctuple.root" ).c_str() ,"r");
TTree* tree = (TTree*) file->Get("MyLCTuple");
TF1* beta_func = new TF1( ("beta_func_" + ptype.name_s).c_str(), "x / sqrt(x^2 + [0]^2)", min_p, max_p);
beta_func->SetParameter(0, ptype.mass);
beta_func->SetLineWidth(1);
beta_func->SetLineColorAlpha(6, 0.5);
file_beta_p->WriteTObject(beta_func);
for (auto &res : resolutions) {
i_pad++;
c_beta_p->cd(i_pad);
string h_beta_name = "h_beta_" + ptype.name_s + "_" + res.str();
TH2F *h_beta = new TH2F( h_beta_name.c_str(),
("#beta_{CH} vs momentum, " + ptype.name_l + ", " + res.w_unit() + "; p [GeV/c] ; #beta ").c_str(),
bins_p, min_p, max_p, bins_beta, min_beta, max_beta
);
string tof_res = "tof" + res.str();
string draw_string = tof_res + "len/" + tof_res + "ch/299.8:sqrt(rcmox*rcmox+rcmoy*rcmoy+rcmoz*rcmoz)>>" + h_beta_name;
string cut_string = tof_res + "len>1.&&" + tof_res + "ch>0.1&&(" + tof_res + "len/" + tof_res + "ch/299.8)<1.1 && sqrt(rcmox*rcmox+rcmoy*rcmoy+rcmoz*rcmoz)<100.&&abs(rccha)>0.005 && abs(rcmoz)/sqrt(rcmox*rcmox+rcmoy*rcmoy+rcmoz*rcmoz) < 0.792573 && sqrt(rcmox*rcmox+rcmoy*rcmoy) > 0.948543 && nrec==1";
tree->Draw( draw_string.c_str(), cut_string.c_str(), "colz" );
file_beta_p->WriteTObject(h_beta);
beta_func->Draw("same");
c_dummy->cd();
beta_histos.getHisto(res, ptype)->histo = h_beta;
beta_histos.getHisto(res, ptype)->slice_histo(every_nth_bin);
c_beta_p_average->cd(i_pad);
string h_beta_average_name = "h_beta_average_" + ptype.name_s + "_" + res.str();
TH1F* h_beta_average = new TH1F( h_beta_average_name.c_str(),
("#beta_{CH}^{average} vs momentum, " + ptype.name_l + ", " + res.w_unit() + ", error = #sigma_{gaus}; p [GeV/c] ; #beta ").c_str(),
bins_p, min_p, max_p
);
for ( auto & slice: beta_histos.getHisto(res, ptype)->slices ) {
int bin = h_beta_average->FindBin(slice.p);
if (slice.fit_successful) {
h_beta_average->SetBinContent(bin, slice.mean);
h_beta_average->SetBinError( bin, slice.sigma);
}
}
h_beta_average->SetMarkerSize(0);
h_beta_average->SetFillColor(2);
h_beta_average->Draw("E5");
beta_func->Draw("same");
file_beta_p->WriteTObject(h_beta_average);
file->cd();
}
closables.push_back(file);
}
c_beta_p->Print( (OutFileBase + "_beta_p.pdf").c_str() );
c_beta_p_average->Print( (OutFileBase + "_beta_p_average.pdf").c_str() );
file_beta_p->Close();
// Save the sliced histograms
for ( auto & res: resolutions ) {
unique_ptr<TFile> file_slices { new TFile( (OutFileBase + "_slices_" + res.w_unit() + ".root").c_str(), "RECREATE" ) };
for ( auto & histo: beta_histos.getHistosToRes(res) ) {
for ( auto & slice: histo->slices ) {
slice.histo->Write();
}
}
file_slices->Close();
}
unique_ptr<TFile> file_sep_pwr { new TFile( (OutFileBase + "_separation_powers.root").c_str(), "RECREATE" ) };
// Find separation power for all combinations
for (int i=0; i<N_ptypes-1; i++) {
if ( 1 == N_ptypes ) {
cout << "Only one particle type given, not creating separation power plots." << endl;
break;
}
for (int j=i+1; j<N_ptypes; j++) { // Loop avoids dublicats
ParticleType ptype1 = particle_types[i];
ParticleType ptype2 = particle_types[j];
unique_ptr<TCanvas> c_sep_pwr {new TCanvas( ("c_sep_pwr_" + ptype1.name_s + "_" + ptype2.name_s).c_str() ,
"Separation power vs momentum",
800*resolutions.size(),800)};
c_sep_pwr->Divide(resolutions.size(), 1);
int i_seppwr_pad = 1;
vector<pair<Resolution,TH1D*>> sep_pwr_hists {};
for ( auto & res: resolutions ) {
auto slices_p1 = beta_histos.getHisto(res, ptype1)->slices;
auto slices_p2 = beta_histos.getHisto(res, ptype2)->slices;
int N_slices = slices_p1.size();
TH1D* h_seppwr = new TH1D(("h_seppwr_" + ptype1.name_s + ptype2.name_s + res.str()).c_str(),
(ptype1.name_l + " - " + ptype2.name_l + " TOF-separation, " + res.w_unit() + "; p [GeV/c] ; Separation Power [#sigma]").c_str(),
N_slices, min_p, max_p);
for (int i_slice=0; i_slice<N_slices; i_slice++) {
auto slice_p1 = slices_p1[i_slice];
auto slice_p2 = slices_p2[i_slice];
double p = slice_p1.p;
if ( ! (slice_p1.fit_successful && slice_p2.fit_successful) ) {
cout << ptype1.name_l << " - " << ptype2.name_l << ", " << res.w_unit() <<
" : Fit in slice p=" << p << " not successful, point won't be drawn." << endl;
continue;
}
double diff_mean = abs(slice_p1.mean - slice_p2.mean);
double sigma_sqr_sum = pow(slice_p1.sigma, 2) + pow(slice_p2.sigma, 2);
double ms_sigma = sigma_sqr_sum / 2.0; // ms = mean squared
double rms_sigma = sqrt( ms_sigma ); // rms = root mean squared
double sep_pwr = diff_mean / rms_sigma;
double sep_pwr_err = sqrt( (pow(slice_p1.mean_err,2) + pow(slice_p2.mean_err,2))/ms_sigma + pow(diff_mean,2) * (pow(slice_p1.sigma,2)*pow(slice_p1.sigma_err,2) + pow(slice_p2.sigma,2)*pow(slice_p2.sigma_err,2)) / (4*pow(ms_sigma,3)) );
// Check that sep pwr is not NaN
if ( sep_pwr != sep_pwr) { sep_pwr = sep_pwr_err = 0; }
h_seppwr->SetBinContent(h_seppwr->FindBin(p), sep_pwr);
h_seppwr->SetBinError( h_seppwr->FindBin(p), sep_pwr_err);
}
h_seppwr->Write();
c_sep_pwr->cd(i_seppwr_pad++);
h_seppwr->Draw("pe"); // TODO Better drawing style: Set some better markers, REMOVE X ERROR
h_seppwr->SetMinimum(0);
h_seppwr->SetMaximum(40);
sep_pwr_hists.push_back(make_pair(res,h_seppwr));
}
c_sep_pwr->Write();
unique_ptr<TCanvas> c_sep_pwr_comb { new TCanvas( ("c_sep_pwr_comb_" + ptype1.name_s + "_" + ptype2.name_s).c_str() ,
"Separation power vs momentum",
800,800)
};
unique_ptr<THStack> h_sep_pwr_comb { new THStack(
("h_seppwr_comb_" + ptype1.name_s + ptype2.name_s).c_str(),
(ptype1.name_l + " - " + ptype2.name_l + " TOF-separation; p [GeV/c] ; Separation Power [#sigma]").c_str())
};
unique_ptr<TLegend> leg_sep_pwr_comb { new TLegend(0.48,0.7,0.8,0.9) };
for (auto &res_hist: sep_pwr_hists) {
Resolution res = res_hist.first;
TH1D* h_seppwr = res_hist.second;
leg_sep_pwr_comb->AddEntry(h_seppwr, res.w_unit().c_str(), "lep");
h_sep_pwr_comb->Add(h_seppwr);
h_seppwr->SetLineColor(res.colour_index);
}
h_sep_pwr_comb->Draw("nostack");
h_sep_pwr_comb->SetMinimum(0);
if( h_sep_pwr_comb->GetMaximum() > 120 ) { h_sep_pwr_comb->SetMaximum(100); }
gPad->Modified();
gPad->Update();
leg_sep_pwr_comb->Draw();
c_sep_pwr_comb->Write();
}
}
file_sep_pwr->Close();
for ( auto & closable : closables ) {
closable->Close();
delete closable;
}
}
void addPossibleResolution( int width, int height, int bpp, bool fullscreen )
{
possibleResolutions.push_back( Resolution( width, height, bpp, fullscreen ) );
}
time_t TimeStamp::EpochTime() const
{
time_t tv = m_ts/Resolution();
return tv;
}
bool FParse::Resolution( const TCHAR* InResolution, uint32& OutX, uint32& OutY )
{
bool bFullScreen;
return Resolution(InResolution, OutX, OutY, bFullScreen);
}
void Node::onInit()
{
// map_size parameter
XmlRpc::XmlRpcValue p_map_size;
if (getPrivateNodeHandle().getParam("map_size", p_map_size)) {
Size &map_size = parameters_("map_size", Size(0, 0, 0));
if (p_map_size.getType() == XmlRpc::XmlRpcValue::TypeInt) {
map_size.x() = map_size.y() = static_cast<int>(p_map_size);
} else if (p_map_size.getType() == XmlRpc::XmlRpcValue::TypeArray) {
if (p_map_size.size() >= 2) {
map_size.x() = static_cast<int>(p_map_size[0]);
map_size.y() = static_cast<int>(p_map_size[1]);
}
if (p_map_size.size() >= 3) {
map_size.z() = static_cast<int>(p_map_size[2]);
}
}
}
// map_resolution parameter
XmlRpc::XmlRpcValue p_map_resolution;
if (getPrivateNodeHandle().getParam("map_resolution", p_map_resolution)) {
Resolution &resolution = parameters_("map_resolution", Resolution(0.0, 0.0, 0.0));
if (p_map_resolution.getType() == XmlRpc::XmlRpcValue::TypeDouble) {
resolution.x() = resolution.y() = static_cast<double>(p_map_resolution);
} else if (p_map_resolution.getType() == XmlRpc::XmlRpcValue::TypeArray) {
if (p_map_resolution.size() >= 2) {
resolution.x() = static_cast<double>(p_map_resolution[0]);
resolution.y() = static_cast<double>(p_map_resolution[1]);
}
if (p_map_resolution.size() >= 3) {
resolution.z() = static_cast<double>(p_map_resolution[2]);
}
}
}
// map_offset parameter
XmlRpc::XmlRpcValue p_map_offset;
if (getPrivateNodeHandle().getParam("map_offset", p_map_offset)) {
Point &offset = parameters_("map_offset", Point(0.0, 0.0, 0.0));
if (p_map_offset.getType() == XmlRpc::XmlRpcValue::TypeArray) {
if (p_map_offset.size() >= 2) {
offset.x() = static_cast<double>(p_map_offset[0]);
offset.y() = static_cast<double>(p_map_offset[1]);
}
if (p_map_offset.size() >= 3) {
offset.z() = static_cast<double>(p_map_offset[2]);
}
}
}
// map_type parameter
std::string p_map_type = "OccupancyGridMap2D";
getPrivateNodeHandle().getParam("map_type", p_map_type);
map_ = MapFactory(parameters_).create<OccupancyGridMapBase>(p_map_type);
if (!map_) {
ROS_FATAL("Unknown map type: %s.\n\nAvailable map types:\n%s", p_map_type.c_str(), MapFactory::getMapTypes().c_str());
ros::shutdown();
return;
}
// scan matcher parameters
ScanMatcherParameters &matcher_parameters = parameters_("matcher", ScanMatcherParameters());
getPrivateNodeHandle().getParam("match_level_minimum", matcher_parameters.match_level_minimum());
getPrivateNodeHandle().getParam("match_level_maximum", matcher_parameters.match_level_maximum());
getPrivateNodeHandle().getParam("occupied_space_residual_weight", matcher_parameters.occupied_space_residual_weight());
getPrivateNodeHandle().getParam("free_space_residual_weight", matcher_parameters.free_space_residual_weight());
getPrivateNodeHandle().getParam("motion_residual_weight", matcher_parameters.motion_residual_weight());
getPrivateNodeHandle().getParam("function_tolerance", matcher_parameters.function_tolerance());
getPrivateNodeHandle().getParam("gradient_tolerance", matcher_parameters.gradient_tolerance());
getPrivateNodeHandle().getParam("parameter_tolerance", matcher_parameters.parameter_tolerance());
getPrivateNodeHandle().getParam("max_num_iterations", matcher_parameters.max_num_iterations());
getPrivateNodeHandle().getParam("max_solver_time_in_seconds", matcher_parameters.max_solver_time_in_seconds());
// get occupancy parameters
OccupancyParameters &occupancy_parameters = parameters_("occupancy", OccupancyParameters::Default());
double p_update_factor_free, p_update_factor_occupied;
// private_nh_.param("update_factor_free", p_update_factor_free_, 0.4);
// private_nh_.param("update_factor_occupied", p_update_factor_occupied_, 0.9);
if (getPrivateNodeHandle().getParam("update_factor_free", p_update_factor_free))
occupancy_parameters.step_free() = occupancy_parameters.getOccupancy(p_update_factor_free);
if (getPrivateNodeHandle().getParam("update_factor_occupied", p_update_factor_occupied))
occupancy_parameters.step_occupied() = occupancy_parameters.getOccupancy(p_update_factor_occupied);
// get scan parameters
ScanParameters &scan_parameters = parameters_("scan", ScanParameters());
getPrivateNodeHandle().getParam("laser_min_dist", scan_parameters.min_distance());
getPrivateNodeHandle().getParam("laser_max_dist", scan_parameters.max_distance());
getPrivateNodeHandle().getParam("laser_z_min_value", scan_parameters.min_z());
getPrivateNodeHandle().getParam("laser_z_max_value", scan_parameters.max_z());
// get other parameters
getPrivateNodeHandle().getParam("map_frame", p_map_frame_);
getPrivateNodeHandle().getParam("base_frame", p_base_frame_);
getPrivateNodeHandle().getParam("odom_frame", p_odom_frame_);
getPrivateNodeHandle().getParam("use_tf_scan_transformation", p_use_tf_scan_transformation_);
getPrivateNodeHandle().getParam("use_tf_pose_start_estimate", p_use_tf_pose_start_estimate_);
getPrivateNodeHandle().getParam("pub_map_odom_transform", p_pub_map_odom_transform_);
getPrivateNodeHandle().getParam("advertise_map_service", p_advertise_map_service_);
getPrivateNodeHandle().getParam("map_update_distance_thresh", p_map_update_translational_threshold_);
getPrivateNodeHandle().getParam("map_update_angle_thresh", p_map_update_angular_threshold_);
// private_nh_.param("pub_drawings", p_pub_drawings, false);
// private_nh_.param("pub_debug_output", p_pub_debug_output_, false);
// private_nh_.param("pub_map_odom_transform", p_pub_map_odom_transform_,true);
// private_nh_.param("pub_odometry", p_pub_odometry_,false);
// private_nh_.param("advertise_map_service", p_advertise_map_service_,true);
// private_nh_.param("scan_subscriber_queue_size", p_scan_subscriber_queue_size_, 5);
// private_nh_.param("map_resolution", p_map_resolution_, 0.025);
// private_nh_.param("map_size", p_map_size_, 1024);
// private_nh_.param("map_start_x", p_map_start_x_, 0.5);
// private_nh_.param("map_start_y", p_map_start_y_, 0.5);
// private_nh_.param("map_multi_res_levels", p_map_multi_res_levels_, 3);
// private_nh_.param("update_factor_free", p_update_factor_free_, 0.4);
// private_nh_.param("update_factor_occupied", p_update_factor_occupied_, 0.9);
// private_nh_.param("map_update_distance_thresh", p_map_update_distance_threshold_, 0.4);
// private_nh_.param("map_update_angle_thresh", p_map_update_angle_threshold_, 0.9);
// private_nh_.param("scan_topic", p_scan_topic_, std::string("scan"));
// private_nh_.param("sys_msg_topic", p_sys_msg_topic_, std::string("syscommand"));
// private_nh_.param("pose_update_topic", p_pose_update_topic_, std::string("poseupdate"));
// private_nh_.param("use_tf_scan_transformation", p_use_tf_scan_transformation_,true);
// private_nh_.param("use_tf_pose_start_estimate", p_use_tf_pose_start_estimate_,false);
// private_nh_.param("map_with_known_poses", p_map_with_known_poses_, false);
// private_nh_.param("base_frame", p_base_frame_, std::string("base_link"));
// private_nh_.param("map_frame", p_map_frame_, std::string("map"));
// private_nh_.param("odom_frame", p_odom_frame_, std::string("odom"));
// private_nh_.param("pub_map_scanmatch_transform", p_pub_map_scanmatch_transform_,true);
// private_nh_.param("tf_map_scanmatch_transform_frame_name", p_tf_map_scanmatch_transform_frame_name_, std::string("scanmatcher_frame"));
// private_nh_.param("output_timing", p_timing_output_,false);
// private_nh_.param("map_pub_period", p_map_pub_period_, 2.0);
// double tmp = 0.0;
// private_nh_.param("laser_min_dist", tmp, 0.4);
// p_sqr_laser_min_dist_ = static_cast<float>(tmp*tmp);
// private_nh_.param("laser_max_dist", tmp, 30.0);
// p_sqr_laser_max_dist_ = static_cast<float>(tmp*tmp);
// private_nh_.param("laser_z_min_value", tmp, -1.0);
// p_laser_z_min_value_ = static_cast<float>(tmp);
// private_nh_.param("laser_z_max_value", tmp, 1.0);
// p_laser_z_max_value_ = static_cast<float>(tmp);
// initialize scan and scan matcher
if (p_use_tf_scan_transformation_) scan_.setTransformer(getTransformListener(), p_base_frame_);
matcher_ = ScanMatcher::Factory(parameters_);
// subscribe scan
scan_subscriber_ = getNodeHandle().subscribe<sensor_msgs::LaserScan>("scan", 10, &Node::scanCallback, this);
cloud_subscriber_ = getNodeHandle().subscribe<sensor_msgs::PointCloud2>("point_cloud", 10, &Node::cloudCallback, this);
// initial pose subscriber
initial_pose_subscriber_ = getNodeHandle().subscribe<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 10, &Node::initialPoseCallback, this);
// static map subscriber
static_map_subscriber_ = getNodeHandle().subscribe<nav_msgs::OccupancyGrid>("static_map", 10, &Node::staticMapCallback, this);
// subscribe syscommand (reset)
syscommand_subscriber_ = getNodeHandle().subscribe<std_msgs::String>("syscommand", 10, &Node::syscommandCallback, this);
// advertise map
map_publisher_ = getNodeHandle().advertise<nav_msgs::OccupancyGrid>("map", 1);
map_metadata_publisher_ = getNodeHandle().advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
ROS_INFO("Advertised map as %s", map_publisher_.getTopic().c_str());
// advertise map service
if (p_advertise_map_service_) {
map_service_ = getNodeHandle().advertiseService("map", &Node::mapServiceCallback, this);
}
// advertise pose
pose_with_covariance_publisher_ = getNodeHandle().advertise<geometry_msgs::PoseWithCovarianceStamped>("poseupdate", 1);
pose_publisher_ = getPrivateNodeHandle().advertise<geometry_msgs::PoseStamped>("pose", 1);
covariance_publisher_ = getPrivateNodeHandle().advertise<visualization_msgs::Marker>("covariance", 1);
// advertise tf
if (p_pub_map_odom_transform_) {
getTransformListener();
getTransformBroadcaster();
}
// advertise scan cloud
bool p_publish_scan_cloud = true;
getPrivateNodeHandle().getParam("publish_scan_cloud", p_publish_scan_cloud);
if (p_publish_scan_cloud) scan_.advertisePointCloud(getPrivateNodeHandle());
// setup map publish thread
double p_map_publish_period = 1.0;
getPrivateNodeHandle().getParam("map_publish_period", p_map_publish_period);
if (p_map_publish_period > 0.0) {
map_publish_thread_ = boost::thread(boost::bind(&Node::mapPublishThread, this, ros::Rate(ros::Duration(p_map_publish_period))));
}
// advertise timing information
#ifdef USE_HECTOR_TIMING
timing_publisher_ = getPrivateNodeHandle().advertise<hector_diagnostics::TimingInfo>("timing", 1);
#endif
// reset
reset();
}
Timestamp Timestamp::FromEpochTime(std::time_t t) {
return Timestamp(TimeVal(t)*Resolution());
}