void CalibrateSpectrum(TString filename,double A[256][256],double B[256][256],double C[256][256],double T[256][256],TH1D *histo,double offset=0){
TFile * file = TFile::Open(filename,"open");
TTreeReader myReader("pixels", file);
TTreeReaderValue<Int_t> col(myReader, "col");
TTreeReaderValue<Int_t> row(myReader, "row");
TTreeReaderValue<Int_t> tot(myReader, "tot");
int cnt = 0 ;
while (myReader.Next()) {
cnt++;
if (cnt%1000==0) cout << "Event " << cnt << endl;
//if (cnt==1000000) break;
if(A[*col][*row]!=0){
histo->Fill(TOTtoE(*tot-offset,A[*col][*row],B[*col][*row],C[*col][*row],T[*col][*row]));
}
else {
histo->Fill(TOTtoE(*tot-offset,29.8,534.1,1817,0.7));
}
}
}
bool
OrderedTask::UpdateIdle(const AircraftState &state,
const GlidePolar &glide_polar)
{
bool retval = AbstractTask::UpdateIdle(state, glide_polar);
if (HasStart() && task_behaviour.optimise_targets_range &&
positive(GetOrderedTaskBehaviour().aat_min_time)) {
CalcMinTarget(state, glide_polar,
GetOrderedTaskBehaviour().aat_min_time + fixed(task_behaviour.optimise_targets_margin));
if (task_behaviour.optimise_targets_bearing) {
if (task_points[active_task_point]->GetType() == TaskPoint::AAT) {
AATPoint *ap = (AATPoint *)task_points[active_task_point];
// very nasty hack
TaskOptTarget tot(task_points, active_task_point, state,
task_behaviour.glide, glide_polar,
*ap, task_projection, taskpoint_start);
tot.search(fixed(0.5));
}
}
retval = true;
}
return retval;
}
int main()
{
double h, t, d;
double x, y;
char a, b;
while (scanf("%c %lf %c %lf\r\n", &a, &x, &b, &y) && a != 'E')
{
h = t = d = MAX;
if (a == 'T')
t = x;
else if (a == 'D')
d = x;
else
h = x;
if (b == 'T')
t = y;
else if (b == 'D')
d = y;
else
h = y;
if (h == MAX)
h = toh(t, d);
else if (t == MAX)
t = tot(d, h);
else
d = tod(t, h);
printf("T %.1f D %.1f H %.1f\r\n", t, d, h);
}
}
main()
{
char line[MAX];
char to[MAX];
tot(to);
escape(line,to);
printf("%s\n",line);
return 0;
}
bool cxy_CAD_helper::filterPointsWithInnerNormals(pcl::PointCloud<pcl::PointXYZ>::Ptr &in_cloud,
pcl::PointCloud<pcl::PointXYZ>::Ptr &in_normals,
pcl::PointCloud<pcl::PointXYZ>::Ptr &out_cloud,
pcl::PointCloud<pcl::PointXYZ>::Ptr &out_normals)
{
Eigen::Vector3d tot(0.0f, 0.0f, 0.0f);
Eigen::Vector3d centroid;
pcl::PointCloud<pcl::PointXYZ>::iterator it, it2;
// Check if in_cloud == NULL
if (in_cloud == NULL)
return false;
if (in_normals == NULL)
return false;
if (out_cloud == NULL)
out_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
if (out_normals == NULL)
out_normals = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
// Compute centroid
for (it = in_cloud->points.begin(); it < in_cloud->points.end(); it++)
tot = tot + Eigen::Vector3d(it->x, it->y, it->z);
centroid = tot / in_cloud->points.size();
it = in_cloud->points.begin();
for (it2 = in_normals->points.begin(); it2 < in_normals->points.end(); it2++, it++)
{
pcl::PointXYZ pt(*it);
pcl::PointXYZ norm(*it2);
Eigen::Vector3d v((pt.x - centroid(0)),(pt.y - centroid(1)),(pt.z - centroid(2)));
Eigen::Vector3d e_norm(norm.x, norm.y, norm.z);
double dot = v.dot(e_norm);
if (dot > 0)
{
out_cloud->points.push_back(pt);
out_normals->points.push_back(norm);
}
}
out_cloud->header = in_cloud->header;
out_cloud->width = out_cloud->points.size();
out_cloud->height = 1;
out_normals->header = in_normals->header;
out_normals->width = out_normals->points.size();
out_normals->height = 1;
return true;
}
vectorc vectorc::operator-(const vectorc& vec) const
{
try
{
if (size_m!=vec.size_m)
throw("\nCan't add vectors of different sizes.");
}
catch(char* str)
{
std::cout << "Exception raised: " << str << '\n';
}
vectorc tot(*this);
gsl_vector_complex_sub(tot.v_m, vec.v_m);
return tot;
}
bool TrivialDetector::compute_centroid_sensor(DataFrame& frame, cv::Mat sensor_silhouette, Vector3& retval)
{
Vector3 tot(0,0,0);
int count = 0;
for(int row=0; row<sensor_silhouette.rows; row++){
for(int col=0; col<sensor_silhouette.cols; col++){
if(sensor_silhouette.at<uchar>(row,col)<125)
continue;
Vector3 p_sensor = frame.point_at_pixel(col,row,camera);
tot += p_sensor;
count++;
}
}
if(count==0){
retval = Vector3(0,0,0);
return false;
} else {
retval = tot / count;
return true;
}
}
bool TrivialDetector::compute_centroid_render(cv::Mat color_render, cv::Mat depth_render, Vector3& retval)
{
Vector3 tot(0,0,0);
int count = 0;
for(int row = 0; row < depth_render.rows; row++){
for(int col = 0; col < depth_render.cols; col++){
if(color_render.at<uchar>(row,col)==255)
continue;
cv::Vec4f p = depth_render.at<cv::Vec4f>(row, col);
tot += Vector3(p[0], p[1], p[2]);
count++;
}
}
if(count==0){
retval = Vector3(0,0,0);
return false;
} else {
retval = tot / count;
return true;
}
}
int main() {
std::vector<double> tot(8,0), dtot(8,0); bool dens=true;
unsigned nmols=200, dir; std::vector<double> com(3), dist(3), a1(3), a2(3), len(nmols);
for(unsigned i=0;i<20;++i){
if(dens) std::cout<<1+nmols<<std::endl;
else std::cout<<1+2*nmols<<std::endl;
std::cout<<10.0<<" "<<10.0<<" "<<10.0<<std::endl;
std::cout<<"Ar "<<5.0<<" "<<5.0<<" "<<5.0<<std::endl;
for(unsigned j=0;j<nmols;++j){
com[0]=10.0*randomn(); com[1]=10.0*randomn(); com[2]=10.0*randomn();
for(unsigned k=0;k<3;++k) dist[k]=fabs(com[k]-5.0);
len[j]=randomn(); dir=floor( 3*randomn() );
if( dist[0]<1.0 ){ tot[1]+=1.0; dtot[1]+=len[j]; }
if( dist[1]<1.0 ){ tot[2]+=1.0; dtot[2]+=len[j]; }
if( dist[2]<1.0 ){ tot[3]+=1.0; dtot[3]+=len[j]; }
if( dist[0]<1.0 && dist[1]<1.0 ){ tot[4]+=1.0; dtot[4]+=len[j]; }
if( dist[0]<1.0 && dist[2]<1.0 ){ tot[5]+=1.0; dtot[5]+=len[j]; }
if( dist[1]<1.0 && dist[2]<1.0 ){ tot[6]+=1.0; dtot[6]+=len[j]; }
if( dist[0]<1.0 && dist[1]<1.0 && dist[2]<1.0 ){ tot[7]+=1.0; dtot[7]+=len[j]; }
a1[0]=com[0]; a1[1]=com[1]; a1[2]=com[2];
a2[0]=com[0]; a2[1]=com[1]; a2[2]=com[2];
a1[dir]-=0.5*len[j]; a2[dir]+=0.5*len[j];
if(dens){
std::cout<<"Ar "<<com[0]<<" "<<com[1]<<" "<<com[2]<<std::endl;
} else {
std::cout<<"Ar "<<a1[0]<<" "<<a1[1]<<" "<<a1[2]<<std::endl;
std::cout<<"Ar "<<a2[0]<<" "<<a2[1]<<" "<<a2[2]<<std::endl;
}
}
for(unsigned i=1;i<8;++i){
if(dens) std::cerr<<"CV "<<i<<" "<<tot[i]<<std::endl;
else std::cerr<<"CV "<<i<<dtot[i] / static_cast<double>( nmols )<<std::endl;
tot[i]=dtot[i]=0.0;
}
}
return 1;
}
GLint DataHandler::addObject(myGLWidget *m)
{
GLint tot(-1);
size_t tmp;
if (!m)
return tot;
if ((tot = this->findObject(m)) < 0) {
if ((tmp = m->getColors().size()) > 0) {
col_objects.push_back(std::pair<myGLWidget*, GLuint>(m, col_objects.size()));
tot = (GLint)col_objects.size() - 1;
col_pos.insert(col_pos.end(), m->getVertices().begin(), m->getVertices().end());
col_datas.insert(col_datas.end(), m->getColors().begin(), m->getColors().end());
return tot;
}
tex_objects.push_back(std::pair<myGLWidget*, GLuint>(m, tex_objects.size()));
tot = (GLint)tex_objects.size() - 1;
tex_pos.insert(tex_pos.end(), m->getVertices().begin(), m->getVertices().end());
tex_datas.insert(tex_datas.end(), m->getTextures().begin(), m->getTextures().end());
}
return tot;
}
void CalibrateSpectrumGlobal(TString filename,double A,double B,double C,double T,TH1D *histo,double offset=0){
TFile * file = TFile::Open(filename,"open");
TTreeReader myReader("pixels", file);
TTreeReaderValue<Int_t> col(myReader, "col");
TTreeReaderValue<Int_t> row(myReader, "row");
TTreeReaderValue<Int_t> tot(myReader, "tot");
int cnt = 0 ;
while (myReader.Next()) {
cnt++;
if (cnt%1000==0) cout << "Event " << cnt << endl;
//if (cnt==1000000) break;
histo->Fill(TOTtoE(*tot-offset,A,B,C,T));
}
}
std::vector<float> get_spectrum_1d(const cv::Mat_<float> &im, const int axis, const bool windowing)
{
if(axis >= im.dims)
throw std::invalid_argument("Matrix dimension is too small to compute the spectrum along this axis");
assert(im.isContinuous());
Convolver co(im.size[axis]);
if(windowing)
co.set_hanning();
std::vector<float> spectrum(co.fourier_size());
std::vector<double> tot(co.fourier_size(), 0.0);
std::vector<std::complex<double> > totf(co.fourier_size(), 0.0);
unsigned long int step = im.step1(axis);
//whatever the real dimension, we fall back to a 3d situation where the axis of interest is y
//and either x or z can be of size 1
int nbplanes = 1;
for(int d=0; d<axis; ++d)
nbplanes *= im.size[d];
int planestep = im.total()/nbplanes;
//for each plane
for(int i=0; i<nbplanes; ++i)
{
//for each line
for(size_t j=0; j<step; ++j)
{
co.spectrum(reinterpret_cast<float* const>(im.data) + i*planestep + j, step, &spectrum[0]);
for(size_t u=0; u<spectrum.size(); ++u)
tot[u] += spectrum[u];
for(size_t u=0; u<spectrum.size(); ++u)
totf[u] += co.get_fourier()[u];
}
}
const double icount = 1.0 / (nbplanes * step);
for(size_t i=0; i<tot.size(); ++i)
spectrum[i] = tot[i]*icount - std::norm(totf[i]*icount);
return spectrum;
}
void convert_raw_to_XR-calc() {
/*** Creates a CERN ROOT file that contains a TTree that holds all the calculated data needed for analysis. Used when source is: Fe55, X-rays.
Organized in the following branches: (name of branch, root type descriptor)
- XR-raw_num, i
- event_num, L (NOT same numbers as indicated by h5/raw files; goes by 1, 2, 3, etc)
- num_hits, i
- sum_tots, i
- mean_x, D
- mean_y, D
- mean_z, D
- R_xy, D
- R_xyz
The formatting was based off of Kelsey Oliver-Mallory's organization and scripts that were used with the older software, STControl.
Author: Dennis Feng
***/
gROOT->Reset();
// Setting up TTreeReader for input file
TFile *in_file = new TFile("/home/pixel/pybar/tags/2.0.1/host/pybar/module_test/111_module_test_stop_mode_ext_trigger_scan_interpreted_raw.root");
TTreeReader tr("Table", in_file);
TTreeReaderValue<UInt_t> h5_file_num(tr, "h5_file_num");
TTreeReaderValue<ULong64_t> event_number(tr, "event_number");
TTreeReaderValue<UChar_t> tot(tr, "tot");
TTreeReaderValue<UChar_t> relative_BCID(tr, "relative_BCID");
TTreeReaderValue<Double_t> x(tr, "x");
TTreeReaderValue<Double_t> y(tr, "y");
TTreeReaderValue<Double_t> z(tr, "z");
// Create XR-calc file and TTree
TFile *out_file = new TFile("test_XR-calc.root","RECREATE");
TTree *t = new TTree("Table","XR-calc Data");
UInt_t XR-raw_num;
ULong64_t event_num;
UInt_t num_hits;
UInt_t sum_tots;
Double_t mean_x;
Double_t mean_y;
Double_t mean_z;
Double_t R_xy;
Double_t R_xyz;
Double_t RMS_xy;
Double_t RMS_xyz;
t->Branch("XR-raw_num", &XR-raw_num, "XR-raw_num/i");
t->Branch("event_num", &event_num, "event_num/L");
t->Branch("num_hits", &num_hits, "num_hits/i");
t->Branch("sum_tots", &sum_tots, "sum_tots/i");
t->Branch("mean_x", &mean_x, "mean_x/D");
t->Branch("mean_y", &mean_y, "mean_y/D");
t->Branch("mean_z", &mean_z, "mean_z/D");
t->Branch("R_xy", &R_xy, "R_xy/D");
t->Branch("R_xyz", &R_xyz, "R_xyz/D");
t->Branch("RMS_xy", &RMS_xy, "RMS_xy/D");
t->Branch("RMS_xyz", &RMS_xyz, "RMS_xyz/D");
// Calculations:
// @@@ finish these calculations some other time
}
void ExploreFrontier::findFrontiers(Costmap2DROS& costmap_) {
frontiers_.clear();
Costmap2D costmap;
costmap_.getCostmapCopy(costmap);
int idx;
int w = costmap.getSizeInCellsX();
int h = costmap.getSizeInCellsY();
int size = (w * h);
int pts = 0;
map_.info.width = w;
map_.info.height = h;
map_.set_data_size(size);
map_.info.resolution = costmap.getResolution();
map_.info.origin.position.x = costmap.getOriginX();
map_.info.origin.position.y = costmap.getOriginY();
// Find all frontiers (open cells next to unknown cells).
const unsigned char* map = costmap.getCharMap();
for (idx = 0; idx < size; idx++) {
//get the world point for the index
unsigned int mx, my;
costmap.indexToCells(idx, mx, my);
geometry_msgs::Point p;
costmap.mapToWorld(mx, my, p.x, p.y);
//check if the point has valid potential and is next to unknown space
//bool valid_point = planner_->validPointPotential(p);
//bool valid_point = planner_->computePotential(p) && planner_->validPointPotential(p);
//bool valid_point = (map[idx] < LETHAL_OBSTACLE);
//bool valid_point = (map[idx] < INSCRIBED_INFLATED_OBSTACLE );
bool valid_point = (map[idx] == FREE_SPACE);
if ((valid_point && map) &&
(((idx+1 < size) && (map[idx+1] == NO_INFORMATION)) ||
((idx-1 >= 0) && (map[idx-1] == NO_INFORMATION)) ||
((idx+w < size) && (map[idx+w] == NO_INFORMATION)) ||
((idx-w >= 0) && (map[idx-w] == NO_INFORMATION))))
{
map_.data[idx] = -128;
//ROS_INFO("Candidate Point %d.", pts++ );
} else {
map_.data[idx] = -127;
}
}
// Clean up frontiers detected on separate rows of the map
// I don't know what this means -- Travis.
idx = map_.info.height - 1;
for (unsigned int y=0; y < map_.info.width; y++) {
map_.data[idx] = -127;
idx += map_.info.height;
}
// Group adjoining map_ pixels
int segment_id = 127;
std::vector< std::vector<FrontierPoint> > segments;
for (int i = 0; i < size; i++) {
if (map_.data[i] == -128) {
std::vector<int> neighbors, candidates;
std::vector<FrontierPoint> segment;
neighbors.push_back(i);
int points_in_seg = 0;
unsigned int mx, my;
geometry_msgs::Point p_init, p;
costmap.indexToCells(i, mx, my);
costmap.mapToWorld(mx, my, p_init.x, p_init.y);
// claim all neighbors
while (neighbors.size() > 0) {
int idx = neighbors.back();
neighbors.pop_back();
btVector3 tot(0,0,0);
int c = 0;
if ((idx+1 < size) && (map[idx+1] == NO_INFORMATION)) {
tot += btVector3(1,0,0);
c++;
}
if ((idx-1 >= 0) && (map[idx-1] == NO_INFORMATION)) {
tot += btVector3(-1,0,0);
c++;
}
if ((idx+w < size) && (map[idx+w] == NO_INFORMATION)) {
tot += btVector3(0,1,0);
c++;
}
if ((idx-w >= 0) && (map[idx-w] == NO_INFORMATION)) {
tot += btVector3(0,-1,0);
c++;
}
assert(c > 0);
// Only adjust the map and add idx to segment when its near enough to start point.
// This should prevent greedy approach where all cells belong to one frontier!
costmap.indexToCells(idx, mx, my);
costmap.mapToWorld(mx, my, p.x, p.y);
float dist;
dist = sqrt( pow( p.x - p_init.x, 2 ) + pow( p.y - p_init.y, 2 ));
if ( dist <= 0.8 ){
map_.data[idx] = segment_id; // This used to be up before "assert" block above.
points_in_seg += 1;
//ROS_INFO( "Dist to start cell: %3.2f", dist );
segment.push_back(FrontierPoint(idx, tot / c));
// consider 8 neighborhood
if (((idx-1)>0) && (map_.data[idx-1] == -128))
neighbors.push_back(idx-1);
if (((idx+1)<size) && (map_.data[idx+1] == -128))
neighbors.push_back(idx+1);
if (((idx-map_.info.width)>0) && (map_.data[idx-map_.info.width] == -128))
neighbors.push_back(idx-map_.info.width);
if (((idx-map_.info.width+1)>0) && (map_.data[idx-map_.info.width+1] == -128))
neighbors.push_back(idx-map_.info.width+1);
if (((idx-map_.info.width-1)>0) && (map_.data[idx-map_.info.width-1] == -128))
neighbors.push_back(idx-map_.info.width-1);
if (((idx+(int)map_.info.width)<size) && (map_.data[idx+map_.info.width] == -128))
neighbors.push_back(idx+map_.info.width);
if (((idx+(int)map_.info.width+1)<size) && (map_.data[idx+map_.info.width+1] == -128))
neighbors.push_back(idx+map_.info.width+1);
if (((idx+(int)map_.info.width-1)<size) && (map_.data[idx+map_.info.width-1] == -128))
neighbors.push_back(idx+map_.info.width-1);
}
}
segments.push_back(segment);
ROS_INFO( "Segment %d has %d costmap cells", segment_id, points_in_seg );
segment_id--;
if (segment_id < -127)
break;
}
}
int num_segments = 127 - segment_id;
ROS_INFO("Segments: %d", num_segments );
if (num_segments <= 0)
return;
for (unsigned int i=0; i < segments.size(); i++) {
Frontier frontier;
std::vector<FrontierPoint>& segment = segments[i];
uint size = segment.size();
//we want to make sure that the frontier is big enough for the robot to fit through
// This seems like a goofy heuristic rather than fact. What happens if we don't do this...?
if (size * costmap.getResolution() < costmap.getInscribedRadius()){
ROS_INFO( "Discarding segment... too small?" );
//continue;
}
float x = 0, y = 0;
btVector3 d(0,0,0);
for (uint j=0; j<size; j++) {
d += segment[j].d;
int idx = segment[j].idx;
x += (idx % map_.info.width);
y += (idx / map_.info.width);
// x = (idx % map_.info.width);
// y = (idx / map_.info.width);
}
d = d / size;
frontier.pose.position.x = map_.info.origin.position.x + map_.info.resolution * (x / size);
frontier.pose.position.y = map_.info.origin.position.y + map_.info.resolution * (y / size);
// frontier.pose.position.x = map_.info.origin.position.x + map_.info.resolution * (x);
// frontier.pose.position.y = map_.info.origin.position.y + map_.info.resolution * (y);
frontier.pose.position.z = 0.0;
frontier.pose.orientation = tf::createQuaternionMsgFromYaw(btAtan2(d.y(), d.x()));
frontier.size = size;
geometry_msgs::Point p;
p.x = map_.info.origin.position.x + map_.info.resolution * (x); // frontier.pose.position
p.y = map_.info.origin.position.y + map_.info.resolution * (y);
if (!planner_->validPointPotential(p)){
ROS_INFO( "Discarding segment... can't reach?" );
//continue;
}
frontiers_.push_back(frontier);
}
}
int main()
{
//char buf[80];
ifstream fin("cis.log");
int ln= 1, gotit= 0;
Assoc<int> tot("", 0);
Regexp reet("(..):(..):(..)"), remnth("^(..)/../..");
Regexp r1("^(../../..) (..:..).. (.*)");
PerlString s;
fin >> s; // eat first line
while(fin >> s){
ln++;
// cout << "line " << ln << ": <" << s << ">" << endl;
PerlStringList l;
//05/20/92 10:48PM CIS 2400 988-5366
//05/21/92 09:24PM OFFLINE 00:00:06
if(s.m(r1, l)){
if(l.scalar() < 4){
cerr << "Didn't match all expressions" << endl;
exit(1);
}
// cout << "Expressions matched: " << endl << l << endl;
PerlString a= l[3];
if(a.m("^CIS")) gotit= 1;
else if(a.m("^OFFLINE") && gotit){ // extract Elapsed time
PerlStringList et, mnth;
int hr, mn, sc, tm;
if(a.m(reet, et) != 4){
cerr << "Failed to extract Elapsed time" << endl;
exit(1);
}
hr= atoi(et[1]); mn= atoi(et[2]); sc= atoi(et[3]);
tm= (hr*60) + mn + ((sc >= 30) ? 1 : 0);
gotit= 0;
// extract month
if(l[1].m(remnth, mnth) != 2){
cerr << "Failed to extract Month" << endl;
exit(1);
}
// cout << "Month: " << mnth[1] << " Elapsed Time = " << tm << " mins" << endl;
tot(mnth[1]) += tm;
}else gotit= 0;
}else{
cerr << "Didn't match any expressions" << endl;
exit(1);
}
};
// cout << "tot = " << endl << tot << endl;
Assoc<PerlString> months;
months("01")= "January"; months("02")= "February"; months("03")= "March";
months("04")= "April"; months("05")= "May"; months("06")= "June";
months("07")= "July"; months("08")= "August"; months("09")= "September";
months("10")= "October"; months("11")= "November"; months("12")= "December";
for(int i=0;i<tot.scalar();i++)
cout << months(tot[i].key()) << ": " << tot[i].value() << " mins $"
<< tot[i].value() * (12.50/60.0) << endl;
exit(0);
}
void ExploreFrontier::findFrontiers() {
frontiers_.clear();
const size_t w = costmap_->getSizeInCellsX();
const size_t h = costmap_->getSizeInCellsY();
const size_t size = w * h;
map_.info.width = w;
map_.info.height = h;
map_.data.resize(size);
map_.info.resolution = costmap_->getResolution();
map_.info.origin.position.x = costmap_->getOriginX();
map_.info.origin.position.y = costmap_->getOriginY();
// lock as we are accessing raw underlying map
auto *mutex = costmap_->getMutex();
std::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*mutex);
// Find all frontiers (open cells next to unknown cells).
const unsigned char* map = costmap_->getCharMap();
ROS_DEBUG_COND(!map, "no map available");
for (size_t i = 0; i < size; ++i) {
// //get the world point for the index
// unsigned int mx, my;
// costmap.indexToCells(i, mx, my);
// geometry_msgs::Point p;
// costmap.mapToWorld(mx, my, p.x, p.y);
//
//check if the point has valid potential and is next to unknown space
// bool valid_point = planner_->validPointPotential(p);
bool valid_point = (map[i] < costmap_2d::LETHAL_OBSTACLE);
// ROS_DEBUG_COND(!valid_point, "invalid point %u", map[i]);
if ((valid_point && map) &&
(((i+1 < size) && (map[i+1] == costmap_2d::NO_INFORMATION)) ||
((i-1 < size) && (map[i-1] == costmap_2d::NO_INFORMATION)) ||
((i+w < size) && (map[i+w] == costmap_2d::NO_INFORMATION)) ||
((i-w < size) && (map[i-w] == costmap_2d::NO_INFORMATION))))
{
ROS_DEBUG_THROTTLE(30, "found suitable point");
map_.data[i] = -128;
} else {
map_.data[i] = -127;
}
}
// Clean up frontiers detected on separate rows of the map
for (size_t y = 0, i = h-1; y < w; ++y) {
ROS_DEBUG_THROTTLE(30, "cleaning cell %lu", i);
map_.data[i] = -127;
i += h;
}
// Group adjoining map_ pixels
signed char segment_id = 127;
std::vector<std::vector<FrontierPoint>> segments;
for (size_t i = 0; i < size; i++) {
if (map_.data[i] == -128) {
ROS_DEBUG_THROTTLE(30, "adjoining on %lu", i);
std::vector<size_t> neighbors;
std::vector<FrontierPoint> segment;
neighbors.push_back(i);
// claim all neighbors
while (!neighbors.empty()) {
ROS_DEBUG_THROTTLE(30, "got %lu neighbors", neighbors.size());
size_t idx = neighbors.back();
neighbors.pop_back();
map_.data[idx] = segment_id;
tf::Vector3 tot(0,0,0);
size_t c = 0;
if (idx+1 < size && map[idx+1] == costmap_2d::NO_INFORMATION) {
tot += tf::Vector3(1,0,0);
++c;
}
if (idx-1 < size && map[idx-1] == costmap_2d::NO_INFORMATION) {
tot += tf::Vector3(-1,0,0);
++c;
}
if (idx+w < size && map[idx+w] == costmap_2d::NO_INFORMATION) {
tot += tf::Vector3(0,1,0);
++c;
}
if (idx-w < size && map[idx-w] == costmap_2d::NO_INFORMATION) {
tot += tf::Vector3(0,-1,0);
++c;
}
if(!(c > 0)) {
ROS_ERROR("assertion failed. corrupted costmap?");
ROS_DEBUG("c is %lu", c);
continue;
}
segment.push_back(FrontierPoint(idx, tot / c));
// consider 8 neighborhood
if (idx-1 < size && map_.data[idx-1] == -128)
neighbors.push_back(idx-1);
if (idx+1 < size && map_.data[idx+1] == -128)
neighbors.push_back(idx+1);
if (idx-w < size && map_.data[idx-w] == -128)
neighbors.push_back(idx-w);
if (idx-w+1 < size && map_.data[idx-w+1] == -128)
neighbors.push_back(idx-w+1);
if (idx-w-1 < size && map_.data[idx-w-1] == -128)
neighbors.push_back(idx-w-1);
if (idx+w < size && map_.data[idx+w] == -128)
neighbors.push_back(idx+w);
if (idx+w+1 < size && map_.data[idx+w+1] == -128)
neighbors.push_back(idx+w+1);
if (idx+w-1 < size && map_.data[idx+w-1] == -128)
neighbors.push_back(idx+w-1);
}
segments.push_back(std::move(segment));
segment_id--;
if (segment_id < -127)
break;
}
}
// no longer accessing map
lock.unlock();
int num_segments = 127 - segment_id;
if (num_segments <= 0) {
ROS_DEBUG("#segments is <0, no frontiers found");
return;
}
for (auto& segment : segments) {
Frontier frontier;
size_t segment_size = segment.size();
//we want to make sure that the frontier is big enough for the robot to fit through
if (segment_size * costmap_->getResolution() < costmap_client_->getInscribedRadius()) {
ROS_DEBUG_THROTTLE(30, "some frontiers were small");
continue;
}
float x = 0, y = 0;
tf::Vector3 d(0,0,0);
for (const auto& frontier_point : segment) {
d += frontier_point.d;
size_t idx = frontier_point.idx;
x += (idx % w);
y += (idx / w);
}
d = d / segment_size;
frontier.pose.position.x = map_.info.origin.position.x + map_.info.resolution * (x / segment_size);
frontier.pose.position.y = map_.info.origin.position.y + map_.info.resolution * (y / segment_size);
frontier.pose.position.z = 0.0;
frontier.pose.orientation = tf::createQuaternionMsgFromYaw(atan2(d.y(), d.x()));
frontier.size = segment_size;
frontiers_.push_back(std::move(frontier));
}
}
void robustpn(int nout, mxArray* pout[], int nin, const mxArray* pin[])
{
/*
*********************************************************
* Check inputs and construct the energy
**********************************************************
*/
int nLabel, nVar, nPair, nHigher, ii;
int hop_fields_indices[HOP_N_OF_FIELDS]; // indices to fields in hop struct
// check pin[0] is sparse
if ( ! mxIsSparse(pin[0]) || ! mxIsDouble(pin[0]) || mxIsComplex(pin[0]))
mexErrMsgIdAndTxt("robustpn:inputs","sparseG must be a sparse double matrix");
// check pin[0] is square
const mwSize *spd = mxGetDimensions(pin[0]);
if (spd[0] != spd[1])
mexErrMsgIdAndTxt("robustpn:inputs","sparseG must be a square matrix");
nVar = spd[0];
nPair = 0;
// read the sparse matrix
double* Pr = mxGetPr(pin[0]);
mwIndex *ir = mxGetIr(pin[0]);
mwIndex *jc = mxGetJc(pin[0]);
mwIndex col, starting_row_index, stopping_row_index, current_row_index, tot(0);
mwSize max_npair = mxGetNzmax(pin[0]);
int * pairs = new int[2 * max_npair]; // will be de-alocate on ~Energy
termType* sc = new termType[max_npair]; // will be de-alocate on ~Energy
// mexPrintf("Preparing to read sG\n");
// traverse the sparseG matrix - pick only connections from the upper tri of the matrix
// (since its symmetric and we don't want to count each potential twice).
for (col=0; col<nVar; col++) {
starting_row_index = jc[col];
stopping_row_index = jc[col+1];
if (starting_row_index == stopping_row_index)
continue;
else {
for (current_row_index = starting_row_index;
current_row_index < stopping_row_index ;
current_row_index++) {
if ( ir[current_row_index] >= col ) { // ignore lower tri of matrix
pairs[nPair*2] = ir[current_row_index]; // from
pairs[nPair*2 + 1] = col; // to
sc[nPair] = (termType)Pr[tot]; // potential weight
nPair++;
}
tot++;
}
}
}
// mexPrintf("Done reading sG got %d pairs\n", nPair);
// check pin[1] has enough columns (=#nodes)
const mwSize *sdc = mxGetDimensions(pin[1]);
if (sdc[1] != spd[0])
mexErrMsgIdAndTxt("robustpn:inputs","Dc must have %d columns to match graph structure", spd[0]);
nLabel = sdc[0];
// check pin[2] is struct array with proper feilds
if ( mxGetClassID(pin[2]) != mxSTRUCT_CLASS )
mexErrMsgIdAndTxt("robustpn:inputs","hop must be a struct array");
nHigher = mxGetNumberOfElements(pin[2]);
// expecting HOP_N_OF_FIELDS fieds
if ( mxGetNumberOfFields(pin[2]) != HOP_N_OF_FIELDS )
mexErrMsgIdAndTxt("robustpn:inputs","hop must have %d fields", HOP_N_OF_FIELDS);
// chack that we have the right fields
for ( ii = 0; ii < HOP_N_OF_FIELDS ; ii++ ) {
hop_fields_indices[ii] = mxGetFieldNumber(pin[2], HOP_FIELDS[ii]);
if ( hop_fields_indices[ii] < 0 )
mexErrMsgIdAndTxt("robustpn:inputs","hop is missing %s field", HOP_FIELDS[ii]);
}
Energy<termType> *energy = new Energy<termType>(nLabel, nVar, nPair, nHigher);
energy->SetUnaryCost( (termType*)mxGetData(pin[1]) );
energy->SetPairCost(pairs, sc);
delete[] pairs; // were copied into energy
delete[] sc;
// Add the HO potentials
mxArray *xind, *xw, *xgamma, *xQ;
int * ind, n;
termType* w;
termType* gamma;
termType Q;
for ( ii = 0 ; ii < nHigher; ii++ ) {
xind = mxGetFieldByNumber(pin[2], ii, hop_fields_indices[0]);
n = mxGetNumberOfElements(xind);
ind = new int[n]; // allocation for energy
GetArr(xind, ind, -1); // bias = -1 convert from 1-ind of matlab to 0-ind of C
xw = mxGetFieldByNumber(pin[2], ii, hop_fields_indices[1]);
if ( mxGetNumberOfElements(xw) != n ) {
delete energy;
delete[] ind;
mexErrMsgIdAndTxt("robustpn:inputs","hop %d: number of indices is different than number of weights", ii);
}
w = new termType[n]; // allocation for energy
GetArr(xw, w);
xgamma = mxGetFieldByNumber(pin[2], ii, hop_fields_indices[2]);
if ( mxGetNumberOfElements(xgamma) != nLabel+1 ) {
delete energy;
delete[] ind;
delete[] w;
mexErrMsgIdAndTxt("robustpn:inputs","hop %d: must have exactly %d gamma values", ii, nLabel+1);
}
gamma = new termType[nLabel+1];
GetArr(xgamma, gamma);
xQ = mxGetFieldByNumber(pin[2], ii, hop_fields_indices[3]);
Q = (termType)mxGetScalar(xQ);
if ( energy->SetOneHOP(n, ind, w, gamma, Q) < 0 ) {
delete energy;
delete[] gamma;
mexErrMsgIdAndTxt("robustpn:inputs","failed to load hop #%d", ii);
}
delete[] gamma; // this array is being allocated inside energy
// mexPrintf("Done reading hop(%d) / %d\n", ii, nHigher);
}
// mexPrintf("Done reading hops\n");
/*
*********************************************************
* Minimize energy
**********************************************************
*/
//initialize alpha expansion - max MAX_ITER iterations
AExpand<termType> *expand = new AExpand<termType>(energy, MAX_ITER);
// must have at least one output for the labels
pout[0] = mxCreateNumericMatrix(1, nVar, mxINT32_CLASS, mxREAL);
int *solution = (int*)mxGetData(pout[0]);
// Do we have an initial guess of labeling ?
if ( nin == 4 ) {
if ( mxGetNumberOfElements(pin[3]) != nVar )
mexErrMsgIdAndTxt("robustpn:inputs","Initial guess of labeling must have exactly %d elements", nVar);
GetArr(pin[3], solution, -1); // convert Matlab's 1-ind labeling to C's 0-ind labeling
} else {
// default initial labeling
memset(solution, 0, nVar*sizeof(int));
}
termType ee[3];
termType E(0);
E = expand->minimize(solution, ee);
if (nout>1) {
pout[1] = mxCreateNumericMatrix(1, 4, mxGetClassID(pin[1]), mxREAL);
termType *pE = (termType*)mxGetData(pout[1]);
pE[0] = ee[0];
pE[1] = ee[1];
pE[2] = ee[2];
pE[3] = E;
}
// de-allocate
delete expand;
delete energy;
}
vectorc vectorc::operator-() const
{
vectorc tot(size_m);
gsl_vector_complex_sub(tot.v_m, v_m);
return tot;
}
void view_individual_events() {
/***
Displays an 3D occupancy plot for each SM Event (stop mode event). The h5_file_num chosen must have working Hits and EventsCR files (_Hits.root and _EventsCR.root files).
Can choose which SM event to start at. (find "CHOOSE THIS" in this script)
***/
gROOT->Reset();
// Setting up files, treereaders, histograms
string file_kind = "aggr"; // string that is either "aggr" or "non_aggr" to indicate whether or not its an aggregate file pair or not.
int file_num_input = 19;
string view_option = "1"; // choose what to view:
// "1" or "3d": view the events with their 3d reconstruction and line fit
// "2" or "SM_rel_BCID": numHits per SMRelBCID with the 3d reconstruction
TFile *fileHits;
TFile *fileEventsCR;
if (file_kind.compare("non_aggr") == 0) {
fileHits = new TFile(("/home/pixel/pybar/tags/2.0.2_new/pyBAR-master/pybar/module_202_new/" + to_string(file_num_input) + "_module_202_new_stop_mode_ext_trigger_scan_interpreted_Hits.root").c_str());
fileEventsCR = new TFile(("/home/pixel/pybar/tags/2.0.2_new/pyBAR-master/pybar/module_202_new/" + to_string(file_num_input) + "_module_202_new_stop_mode_ext_trigger_scan_interpreted_EventsCR.root").c_str());
} else if (file_kind.compare("aggr") == 0) {
// fileHits = new TFile("/home/pixel/pybar/tags/2.0.2_new/pyBAR-master/pybar/homemade_scripts/aggregate_data/1_module_202_new_AggrHits.root");
fileHits = new TFile(("/home/pixel/pybar/tags/2.0.2_new/pyBAR-master/pybar/homemade_scripts/aggregate_data/" + to_string(file_num_input) + "_module_202_new_AggrHits.root").c_str());
// fileEventsCR = new TFile("/home/pixel/pybar/tags/2.0.2_new/pyBAR-master/pybar/homemade_scripts/aggregate_data/1_module_202_new_AggrEventsCR.root");
fileEventsCR = new TFile(("/home/pixel/pybar/tags/2.0.2_new/pyBAR-master/pybar/homemade_scripts/aggregate_data/" + to_string(file_num_input) + "_module_202_new_AggrEventsCR.root").c_str());
} else {
cout << "Error: Input file_kind is not valid.";
}
TTreeReader *readerHits = new TTreeReader("Table", fileHits);
TTreeReaderValue<UInt_t> h5_file_num_Hits(*readerHits, "h5_file_num");
TTreeReaderValue<Long64_t> event_number(*readerHits, "event_number");
TTreeReaderValue<UChar_t> tot(*readerHits, "tot");
TTreeReaderValue<UChar_t> relative_BCID(*readerHits, "relative_BCID");
TTreeReaderValue<Long64_t> SM_event_num_Hits(*readerHits, "SM_event_num");
TTreeReaderValue<UInt_t> SM_rel_BCID(*readerHits, "SM_rel_BCID");
TTreeReaderValue<Double_t> x(*readerHits, "x");
TTreeReaderValue<Double_t> y(*readerHits, "y");
TTreeReaderValue<Double_t> z(*readerHits, "z");
TTreeReaderValue<Double_t> s(*readerHits, "s");
TTreeReader *readerEventsCR = new TTreeReader("Table", fileEventsCR);
TTreeReaderValue<UInt_t> h5_file_num_EventsCR(*readerEventsCR, "h5_file_num");
TTreeReaderValue<Long64_t> SM_event_num_EventsCR(*readerEventsCR, "SM_event_num");
TTreeReaderValue<UInt_t> num_hits(*readerEventsCR, "num_hits");
TTreeReaderValue<UInt_t> sum_tots(*readerEventsCR, "sum_tots");
TTreeReaderValue<Double_t> mean_x(*readerEventsCR, "mean_x");
TTreeReaderValue<Double_t> mean_y(*readerEventsCR, "mean_y");
TTreeReaderValue<Double_t> mean_z(*readerEventsCR, "mean_z");
TTreeReaderValue<Double_t> line_fit_param0(*readerEventsCR, "line_fit_param0");
TTreeReaderValue<Double_t> line_fit_param1(*readerEventsCR, "line_fit_param1");
TTreeReaderValue<Double_t> line_fit_param2(*readerEventsCR, "line_fit_param2");
TTreeReaderValue<Double_t> line_fit_param3(*readerEventsCR, "line_fit_param3");
TTreeReaderValue<Double_t> sum_of_squares(*readerEventsCR, "sum_of_squares");
TTreeReaderValue<UInt_t> event_status(*readerEventsCR, "event_status");
TTreeReaderValue<Double_t> fraction_inside_sphere(*readerEventsCR, "fraction_inside_sphere");
TTreeReaderValue<Double_t> length_track(*readerEventsCR, "length_track");
TTreeReaderValue<Double_t> sum_tots_div_by_length_track(*readerEventsCR, "sum_tots_div_by_length_track");
TTreeReaderValue<Double_t> sum_squares_div_by_DoF(*readerEventsCR, "sum_squares_div_by_DoF");
TTreeReaderValue<Double_t> zenith_angle(*readerEventsCR, "zenith_angle");
TTreeReaderValue<UInt_t> duration(*readerEventsCR, "duration");
// Initialize the canvas and graph_3d
TCanvas *c1 = new TCanvas("c1","3D Occupancy for Specified SM Event", 1000, 10, 900, 1000);
// c1->SetRightMargin(0.25);
TPad *pad1 = new TPad("pad1", "pad1", 0, 0.5, 1, 1.0); // upper pad
pad1->SetRightMargin(0.25);
TPad *pad2 = new TPad("pad2", "pad2", 0, 0, 1, 0.5); // lower pad
// pad2->SetRightMargin(0.35);
c1->cd();
TH2F *h_2d_occupancy = new TH2F("h_2d_occupancy", "2D Occupancy", 80, 0, 20, 336, -16.8, 0);
h_2d_occupancy->GetXaxis()->SetTitle("x (mm)");
h_2d_occupancy->GetYaxis()->SetTitle("y (mm)");
h_2d_occupancy->GetZaxis()->SetTitle("SM Relative BCID (BCIDs)");
TH1F *h_SM_rel_BCID = new TH1F("h_SM_rel_BCID", "Num Hits per SMRelBCID", 256, 0, 256);
h_SM_rel_BCID->GetXaxis()->SetTitle("Stop Mode Relative BCID (BCIDs)");
h_SM_rel_BCID->GetYaxis()->SetTitle("Count (hits)");
bool quit = false; // if pressed q
// Main Loop (loops for every entry in readerEventsCR)
while (readerEventsCR->Next() && !quit) {
if (readerEventsCR->GetCurrentEntry() == 0 && file_kind.compare("non_aggr") == 0) {
continue; // skip the entry num 0, because it probably contains no data
}
// Get startEntryNum_Hits and endEntryNum_Hits (for readerHits)
vector<int> entryNumRange_include(2);
entryNumRange_include = getEntryNumRangeWithH5FileNumAndSMEventNum(readerHits, *h5_file_num_EventsCR, *SM_event_num_EventsCR);
if (entryNumRange_include[0] == -1) {
cout << "Error: h5_file_num and SM_event_num should be able to be found in the Hits file.\n";
}
// If statement for choosing which graph_3d/h_2d_occupancy to view
TGraph2D *graph_3d = new TGraph2D(); // create a new TGraph to refresh; the graph_3d is the 3d plot, the h_2d_occupancy is the 2d plot.
h_2d_occupancy->Reset(); // must do reset for histograms, cannot create a new hist to refresh it
h_SM_rel_BCID->Reset();
// Fill graph_3d and h_2d_occupancy with points and set title and axes
readerHits->SetEntry(entryNumRange_include[0]);
for (int i = 0; i < entryNumRange_include[1] - entryNumRange_include[0] + 1; i++) {
graph_3d->SetPoint(i, (*x - 0.001), (*y + 0.001), (*z - 0.001));
h_2d_occupancy->Fill(*x, *y, *SM_rel_BCID);
h_SM_rel_BCID->Fill(*SM_rel_BCID);
readerHits->Next();
}
string graphTitle = "3D Reconstruction and Line Fit for h5FileNum " + to_string(*h5_file_num_EventsCR) + ", SMEventNum " + to_string(*SM_event_num_EventsCR);
// graphTitle.append(to_string(*SM_event_num_EventsCR));
graph_3d->SetTitle(graphTitle.c_str());
graph_3d->GetXaxis()->SetTitle("x (mm)");
graph_3d->GetYaxis()->SetTitle("y (mm)");
graph_3d->GetZaxis()->SetTitle("z (mm)");
graph_3d->GetXaxis()->SetLimits(0, 20); // ROOT is buggy, x and y use setlimits()
graph_3d->GetYaxis()->SetLimits(-16.8, 0); // but z uses setrangeuser()
graph_3d->GetZaxis()->SetRangeUser(0, 40.96);
c1->SetTitle(graphTitle.c_str());
// Draw the graph_3d on pad1 (upper pad)
c1->cd();
pad1->Draw();
pad1->cd();
graph_3d->SetMarkerStyle(8);
graph_3d->SetMarkerSize(0.5);
graph_3d->Draw("pcol");
// Draw other histogram on pad2
c1->cd();
pad2->Draw();
pad2->cd();
if (view_option.compare("3d") == 0 || view_option.compare("1") == 0) {
pad2->SetRightMargin(0.35);
h_2d_occupancy->Draw("COLZ");
} else if (view_option.compare("SM_rel_BCID") == 0 || view_option.compare("2") == 0) {
pad2->SetRightMargin(0.25);
h_SM_rel_BCID->Draw("COLZ");
} else {
cout << "Error: Input view_option is not valid.\n";
}
pad1->cd();
// Display results, draw graph_3d and line fit
if (file_kind.compare("aggr") == 0) {
cout << "Aggr EventsCR Entry Num: " << readerEventsCR->GetCurrentEntry();
}
cout << " h5 Event Num: " << *h5_file_num_EventsCR << " SM Event Num: " << *SM_event_num_EventsCR << "\n";
// cout << " Number of hits: " << *num_hits << "\n";
// Draw the fitted line only if fit did not fail.
if (*event_status != 1) {
double fitParams[4];
fitParams[0] = *line_fit_param0;
fitParams[1] = *line_fit_param1;
fitParams[2] = *line_fit_param2;
fitParams[3] = *line_fit_param3;
int n = 1000;
double t0 = 0; // t is the z coordinate
double dt = 40.96;
TPolyLine3D *l = new TPolyLine3D(n);
for (int i = 0; i <n;++i) {
double t = t0+ dt*i/n;
double x,y,z;
line(t,fitParams,x,y,z);
l->SetPoint(i,x,y,z);
}
l->SetLineColor(kRed);
l->Draw("same");
cout << "Sum of squares div by DoF: " << *sum_squares_div_by_DoF;
} else {
cout << "Sum of squares div by DoF: FIT FAILED";
}
cout << " Zenith angle: " << *zenith_angle << "\n";
cout << "Duration: " << *duration << "\n";
// cout << "Fraction inside sphere (1 mm radius): " << *fraction_inside_sphere << "\n";
cout << "Length of track: " << *length_track << "\n";
cout << "SumTots/Length: " << *sum_tots_div_by_length_track << "\n";
// if (view_option.compare("3d") == 0 || view_option.compare("1") == 0) {
// } else if (view_option.compare("SM_rel_BCID") == 0 || view_option.compare("2") == 0) {
// // // Reset histogram
// // h_SM_rel_BCID->Reset();
// // // For every hit, fill in the histogram with the SM_rel_BCID
// // readerHits->SetEntry(entryNumRange_include[0]);
// // for (int i = 0; i < entryNumRange_include[1] - entryNumRange_include[0] + 1; i++) {
// // h_SM_rel_BCID->Fill(*SM_rel_BCID);
// // readerHits->Next();
// // }
// // // Draw the hist
// // c1->cd();
// // pad1->Draw();
// // pad1->cd();
// // h_SM_rel_BCID->Draw();
// // // Print info lines
// // if (file_kind.compare("aggr") == 0) {
// // cout << "Aggr EventsCR Entry Num: " << readerEventsCR->GetCurrentEntry();
// // }
// // cout << " h5 Event Num: " << *h5_file_num_EventsCR << " SM Event Num: " << *SM_event_num_EventsCR << "\n";
// } else {
// cout << "Error: Input view_option is not valid.\n";
// }
// Ask for input
if (true) { // won't show drawings or ask for input unless its a good event // CHOOSE THIS to show all events or only good events
c1->Update(); // show all the drawings
// handle input
string inString = "";
bool inStringValid = false;
do {
cout << "<Enter>: next; 'b': previous; [number]: the nth SMEvent in the EventsCR file; 'q': quit.\n"; // b is for back
getline(cin, inString);
// Handles behavior according to input
if (inString.empty()) { // <Enter>
// leave things be
inStringValid = true;
} else if (inString.compare("b") == 0) {
readerEventsCR->SetEntry(readerEventsCR->GetCurrentEntry() - 2);
// smEventNum -= 2; // because it gets incremented once at the end of this do while loop
inStringValid = true;
} else if (inString.compare("q") == 0 || inString.compare(".q") == 0) {
quit = true;
inStringValid = true;
} else if (canConvertStringToPosInt(inString)) {
readerEventsCR->SetEntry(convertStringToPosInt(inString) - 1);
// smEventNum = convertStringToPosInt(inString) - 1; // -1 because it gets incremented once at the end of this do while loop
inStringValid = true;
} // else, leave inStringValid as false, so that it asks for input again
} while (!inStringValid);
} else {
cout << "\n";
}
}
cout << "Exiting program.\n";
}
void view_SMEvents_3D_from_Hits() {
/*** Displays an 3D occupancy plot for each SM Event. (stop mode event)
Can choose which SM event to start at. (find "CHOOSE THIS" in this script)
Input file must be a Hits file (_interpreted_Hits.root file).
***/
gROOT->Reset();
// Setting up file, treereader, histogram
TFile *f = new TFile("/home/pixel/pybar/tags/2.0.2_new/pyBAR-master/pybar/module_202_new/101_module_202_new_stop_mode_ext_trigger_scan_interpreted_Hits.root");
if (!f) { // if we cannot open the file, print an error message and return immediately
cout << "Error: cannot open the root file!\n";
//return;
}
TTreeReader *reader = new TTreeReader("Table", f);
TTreeReaderValue<UInt_t> h5_file_num(*reader, "h5_file_num");
TTreeReaderValue<Long64_t> event_number(*reader, "event_number");
TTreeReaderValue<UChar_t> tot(*reader, "tot");
TTreeReaderValue<UChar_t> relative_BCID(*reader, "relative_BCID");
TTreeReaderValue<Long64_t> SM_event_num(*reader, "SM_event_num");
TTreeReaderValue<Double_t> x(*reader, "x");
TTreeReaderValue<Double_t> y(*reader, "y");
TTreeReaderValue<Double_t> z(*reader, "z");
// Initialize the canvas and graph
TCanvas *c1 = new TCanvas("c1","3D Occupancy for Specified SM Event", 1000, 10, 900, 550);
c1->SetRightMargin(0.25);
TGraph2D *graph = new TGraph2D();
// Variables used to loop the main loop
bool endOfReader = false; // if reached end of the reader
bool quit = false; // if pressed q
int smEventNum = 1; // the current SM-event CHOOSE THIS to start at desired SM event number
// Main Loop (loops for every smEventNum)
while (!endOfReader && !quit) {
// Variables used in this main loop
int startEntryNum = 0;
int endEntryNum = 0;
string histTitle = "3D Occupancy for SM Event ";
string inString = "";
bool fitFailed = false; // true if the 3D fit failed
bool lastEvent = false;
// Declaring some important output values for the current graph and/or line fit
int numEntries = 0;
double sumSquares = 0;
// Get startEntryNum and endEntryNum
startEntryNum = getEntryNumWithSMEventNum(reader, smEventNum);
endEntryNum = getEntryNumWithSMEventNum(reader, smEventNum + 1);
if (startEntryNum == -2) { // can't find the smEventNum
cout << "Error: There should not be any SM event numbers that are missing." << "\n";
} else if (startEntryNum == -3) {
endOfReader = true;
break;
} else if (endEntryNum == -3) { // assuming no SM event nums are skipped
endEntryNum = reader->GetEntries(false);
lastEvent = true;
}
// Fill TGraph with points and set title and axes
graph = new TGraph2D(); // create a new TGraph to refresh
reader->SetEntry(startEntryNum);
for (int i = 0; i < endEntryNum - startEntryNum; i++) {
graph->SetPoint(i, (*x - 0.001), (*y + 0.001), (*z - 0.001));
endOfReader = !(reader->Next());
}
histTitle.append(to_string(smEventNum));
graph->SetTitle(histTitle.c_str());
graph->GetXaxis()->SetTitle("x (mm)");
graph->GetYaxis()->SetTitle("y (mm)");
graph->GetZaxis()->SetTitle("z (mm)");
graph->GetXaxis()->SetLimits(0, 20); // ROOT is buggy, x and y use setlimits()
graph->GetYaxis()->SetLimits(-16.8, 0); // but z uses setrangeuser()
graph->GetZaxis()->SetRangeUser(0, 40.96);
c1->SetTitle(histTitle.c_str());
// 3D Fit, display results, draw graph and line fit, only accept "good" events, get input
if (!endOfReader || lastEvent) {
// Display some results
numEntries = graph->GetN();
cout << "Current SM Event Number: " << smEventNum << "\n";
cout << "Number of entries: " << numEntries << "\n";
// Starting the fit. First, get decent starting parameters for the fit - do two 2D fits (one for x vs z, one for y vs z)
TGraph *graphZX = new TGraph();
TGraph *graphZY = new TGraph();
reader->SetEntry(startEntryNum);
for (int i = 0; i < endEntryNum - startEntryNum; i++) {
graphZX->SetPoint(i, (*z - 0.001), (*x + 0.001));
graphZY->SetPoint(i, (*z - 0.001), (*y + 0.001));
reader->Next();
}
TFitResultPtr fitZX = graphZX->Fit("pol1", "WQS"); // w for ignore error of each pt, q for quiet (suppress results output), s for return a tfitresultptr
TFitResultPtr fitZY = graphZY->Fit("pol1", "WQS");
Double_t param0 = fitZX->GetParams()[0];
Double_t param1 = fitZX->GetParams()[1];
Double_t param2 = fitZY->GetParams()[0];
Double_t param3 = fitZY->GetParams()[1];
// // Draw the lines for the two 2D fits
// int n = 2;
// TPolyLine3D *lineZX = new TPolyLine3D(n);
// TPolyLine3D *lineZY = new TPolyLine3D(n);
// lineZX->SetPoint(0, param0, 0, 0);
// lineZX->SetPoint(1, param0 + param1 * 40.96, 0, 40.96);
// lineZX->SetLineColor(kBlue);
// lineZX->Draw("same");
// lineZY->SetPoint(0, 0, param2, 0);
// lineZY->SetPoint(1, 0, param2 + param3 * 40.96, 40.96);
// lineZY->SetLineColor(kGreen);
// lineZY->Draw("same");
// 3D FITTING CODE (based on line3Dfit.C), draw graph and line fit
ROOT::Fit::Fitter fitter;
SumDistance2 sdist(graph);
#ifdef __CINT__
ROOT::Math::Functor fcn(&sdist,4,"SumDistance2");
#else
ROOT::Math::Functor fcn(sdist,4);
#endif
// set the function and the initial parameter values
double pStart[4] = {param0,param1,param2,param3};
fitter.SetFCN(fcn,pStart);
// set step sizes different than default ones (0.3 times parameter values)
for (int i = 0; i < 4; ++i) fitter.Config().ParSettings(i).SetStepSize(0.01);
bool ok = fitter.FitFCN();
if (!ok) {
Error("line3Dfit","Line3D Fit failed");
fitFailed = true;
} else {
const ROOT::Fit::FitResult & result = fitter.Result();
const double * fitParams = result.GetParams();
sumSquares = result.MinFcnValue();
std::cout << "Sum of distance squares: " << sumSquares << std::endl;
std::cout << "Sum of distance squares divided by numEntries: " << sumSquares/numEntries << std::endl;
std::cout << "Theta : " << TMath::ATan(sqrt(pow(fitParams[1], 2) + pow(fitParams[3], 2))) << std::endl;
// result.Print(std::cout); // (un)suppress results output
// Draw the graph
graph->SetMarkerStyle(8);
graph->SetMarkerSize(0.5);
graph->Draw("pcol");
// Draw the fitted line
int n = 1000;
double t0 = 0; // t is the z coordinate
double dt = 40.96;
TPolyLine3D *l = new TPolyLine3D(n);
for (int i = 0; i <n;++i) {
double t = t0+ dt*i/n;
double x,y,z;
line(t,fitParams,x,y,z);
l->SetPoint(i,x,y,z);
}
l->SetLineColor(kRed);
l->Draw("same");
// Access fit params and minfcnvalue
// cout << "FIT1: " << fitParams[1] << "\n";
// cout << "FIT2: " << result.MinFcnValue() << "\n";
}
// Criteria to be a good event (if not good entry, then don't show)
bool isGoodEvent = false;
// the following block of code finds the mean X, Y ans Z values
double meanX = 0;
double meanY = 0;
double meanZ = 0;
reader->SetEntry(startEntryNum);
for (int i = 0; i < endEntryNum - startEntryNum; i++) {
meanX += graph->GetX()[i];
meanY += graph->GetY()[i];
meanZ += graph->GetZ()[i];
reader->Next();
}
meanX /= endEntryNum - startEntryNum;
meanY /= endEntryNum - startEntryNum;
meanZ /= endEntryNum - startEntryNum;
// the following code block calculates the fraction of the hits in the smEvent that are inside a sphere, centered at the mean XYZ, of radius 'radius' (larger fraction means the track is less like a long streak and more like a dense blob)
double radius = 1; // length in mm
double fractionInsideSphere = 0;
reader->SetEntry(startEntryNum);
for (int i = 0; i < endEntryNum - startEntryNum; i++) {
double distanceFromMeanXYZ = sqrt(pow(graph->GetX()[i] - meanX, 2) + pow(graph->GetY()[i] - meanY, 2) + pow(graph->GetZ()[i] - meanZ, 2));
if (distanceFromMeanXYZ <= 2) {
fractionInsideSphere += 1;
}
reader->Next();
}
fractionInsideSphere /= endEntryNum - startEntryNum;
cout << "fraction inside sphere: " << fractionInsideSphere << "\n";
// if (numEntries >= 50
// && sumSquares/numEntries < 2.0
// && fractionInsideSphere < 0.8) {
// isGoodEvent = true;
// }
isGoodEvent = true;
if (isGoodEvent) { // won't show drawings or ask for input unless its a good event
c1->Update(); // show all the drawings
// handle input
bool inStringValid = false;
do {
cout << "<Enter>: next event; 'b': previous SM event; [number]: specific SM event number; 'q': quit.\n";
getline(cin, inString);
// Handles behavior according to input
if (inString.empty()) { // <Enter>
// leave things be
inStringValid = true;
} else if (inString.compare("b") == 0) {
smEventNum -= 2; // because it gets incremented once at the end of this do while loop
inStringValid = true;
} else if (inString.compare("q") == 0 || inString.compare(".q") == 0) {
quit = true;
inStringValid = true;
} else if (canConvertStringToPosInt(inString)) {
smEventNum = convertStringToPosInt(inString) - 1; // -1 because it gets incremented once at the end of this do while loop
inStringValid = true;
} // else, leave inStringValid as false, so that it asks for input again
} while (!inStringValid);
} else {
cout << "\n";
}
}
smEventNum++;
}
cout << "Exiting program.\n";
}
void Colorsel::paintBar()
{
QPainter painter(this);
painter.setWindow(0, 0, 200, 200);
painter.setRenderHint(QPainter::Antialiasing);
painter.setPen(QPen(Qt::black, 1, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
painter.setBrush(Qt::black);
QLinearGradient linGrad1(30, 90, 35, 90);
linGrad1.setColorAt(0, Qt::gray);
linGrad1.setColorAt(1, Qt::black);
linGrad1.setSpread(QGradient::PadSpread);
painter.setBrush(linGrad1);
QRectF gradRect1(30, 30, 20, 127.5);
painter.drawRect(gradRect1);
QLinearGradient redGrad(40, 142.5, 40, 30);
redGrad.setColorAt(0, Qt::red);
redGrad.setColorAt(1, Qt::black);
redGrad.setSpread(QGradient::PadSpread);
painter.setOpacity(0.75);
painter.setBrush(redGrad);
QRectF red(30, 30, 20, redyp);
painter.drawRect(red);
QLinearGradient linGrad2(60, 90, 65, 90);
linGrad2.setColorAt(0, Qt::gray);
linGrad2.setColorAt(1, Qt::black);
linGrad2.setSpread(QGradient::PadSpread);
painter.setBrush(linGrad2);
QRectF gradRect2(60, 30, 20, 127.5);
painter.drawRect(gradRect2);
QLinearGradient greenGrad(70, 142.5, 70, 30);
greenGrad.setColorAt(0, Qt::green);
greenGrad.setColorAt(1, Qt::black);
greenGrad.setSpread(QGradient::PadSpread);
painter.setOpacity(0.75);
painter.setBrush(greenGrad);
QRectF green(60, 30, 20, greenyp);
painter.drawRect(green);
QLinearGradient linGrad3(90, 90, 95, 90);
linGrad3.setColorAt(0, Qt::gray);
linGrad3.setColorAt(1, Qt::black);
linGrad3.setSpread(QGradient::PadSpread);
painter.setBrush(linGrad3);
QRectF gradRect3(90, 30, 20, 127.5);
painter.drawRect(gradRect3);
QLinearGradient blueGrad(100, 142.5, 100, 30);
blueGrad.setColorAt(0, Qt::blue);
blueGrad.setColorAt(1, Qt::black);
blueGrad.setSpread(QGradient::PadSpread);
painter.setOpacity(0.75);
painter.setBrush(blueGrad);
QRectF blue(90, 30, 20, blueyp);
painter.drawRect(blue);
QLinearGradient linGrad4(120, 90, 125, 90);
linGrad4.setColorAt(0, Qt::gray);
linGrad4.setColorAt(1, Qt::black);
linGrad4.setSpread(QGradient::PadSpread);
painter.setBrush(linGrad4);
QRectF gradRect4(120, 30, 20, 127.5);
painter.drawRect(gradRect4);
QLinearGradient opGrad(130, 142.5, 130, 30);
opGrad.setColorAt(0, Qt::white);
opGrad.setColorAt(1, Qt::black);
opGrad.setSpread(QGradient::PadSpread);
painter.setOpacity(0.75);
painter.setBrush(opGrad);
QRectF op(120, 30, 20, opyp);
painter.drawRect(op);
painter.setOpacity(1);
QColor color(redY, greenY, blueY, opY);
painter.setBrush(color);
QRectF tot(150, 30, 30, 150.5);
painter.drawRect(tot);
}
