examples/JDB/JCompass.cc

Go to the documentation of this file.

#include <iostream>
#include <iomanip>
#include <vector>
#include <map>
 
#include "TROOT.h"
#include "TFile.h"
#include "TGraph.h"
 
#include "JDB/JAHRS.hh"
#include "JDB/JAHRSCalibration_t.hh"
#include "JDB/JSupport.hh"
#include "JDB/JAHRSToolkit.hh"
 
#include "JGeometry3D/JEigen3D.hh" //JAverage
 
#include "JROOT/JGraph.hh"
 
#include "JDetector/JDetector.hh"
#include "JDetector/JDetectorToolkit.hh"
#include "JDetector/JDetectorSupportkit.hh"
#include "JDetector/JModuleRouter.hh"
#include "JDetector/JCompass.hh"
 
#include "JGeometry3D/JQuaternion3D.hh"
#include "JGeometry3D/JDirection3D.hh"
 
#include "JLang/JComparator.hh"
#include "JSupport/JMultipleFileScanner.hh"
 
#include "Jeep/JPrint.hh"
#include "Jeep/JParser.hh"
#include "Jeep/JMessage.hh"
 
 
/**
 * \file
 *
 * Example program to monitor AHRS data of the first floor and check offsets vs detector file
 * \author mdejong, vkulikovskiy
 */
int main(int argc, char **argv)
{
  using namespace std;
  using namespace JPP;
  
  JMultipleFileScanner<JAHRS> inputFile;
  JLimit_t&       numberOfEvents = inputFile.getLimit();
  string          outputFile;
  string          detectorFile;
  double          Tmax_s;
  string          ahrsFile;
  int             debug;
  JRange<int>     frange;
 
  try {
 
    JParser<> zap("Example program to monitor AHRS data.");
    
    zap['f'] = make_field(inputFile);
    zap['n'] = make_field(numberOfEvents)              = JLimit::max();
    zap['o'] = make_field(outputFile)                  = "ahrs.root";
    zap['a'] = make_field(detectorFile);
    zap['T'] = make_field(Tmax_s)                      = 600.0;   // [s]
    zap['R'] = make_field(frange,   "floor range")     = JRange<int>(1,1);
    zap['c'] = make_field(ahrsFile,     "output of JAHRSCalibration");
    zap['d'] = make_field(debug)               = 2;
    
 
    zap(argc, argv);
  }
  catch(const exception &error) {
    FATAL(error.what() << endl);
  }
 
  JDetector detector;
 
  try {
    load(detectorFile, detector);
  }
  catch(const JException& error) {
    FATAL(error);
  }
 
  const JModuleRouter router(detector);
 
  const JAHRSCalibration_t calibration(ahrsFile.c_str());
  const JAHRSValidity      is_valid;
 
  typedef vector<JAHRS>          buffer_type;    // data type
  typedef map<JObjectID, buffer_type>  map_type;       // DOM -> data
 
  map_type  data;
 
  for (int counter = 0; inputFile.hasNext(); ++counter) {
 
    STATUS("counter: " << setw(8) << counter << '\r' << flush); DEBUG(endl);
 
    const JAHRS* parameters = inputFile.next();
 
    if (is_valid(*parameters) && router.hasModule(parameters->DOMID) && calibration.has(parameters->DOMID)) {
 
      const JLocation location = router.getModule(parameters->DOMID);
      
      if (frange(location.getFloor())) {
        data[parameters->DOMID].push_back(*parameters);
      }
    }
  }
 
  map <JObjectID, double> DOM_heading_map;
  const JDetectorBuilder& demo = getDetectorBuilder(detector.getID());
  for (JDetector::const_iterator module = detector.begin(); module != detector.end(); ++module) {
    if (frange(module->getFloor())) {
      JModule buffer = demo.getModule(module->getID(), module->getLocation());
      const JQuaternion3D Q = getRotation(buffer, *module);
      const JQuaternion3D::decomposition q1(Q, JVector3Z_t);
      const double phi = (JVector3Z_t.getDot(q1.twist) >= 0.0 ? +1.0 : -1.0) * q1.twist.getAngle();
      DOM_heading_map[module->getID()] = phi;
      cout << "Heading from detector file:  " << module->getID() << " " << module->getString() << " " << module->getFloor() << " " << phi/M_PI*180. << " [deg]" << endl; 
    }
  }
 
  STATUS(endl);
 
 
  map<JObjectID, JGraph_t>  ZO;
  map<JObjectID, JGraph_t>  ZA;
  map<JObjectID, JGraph_t>  PD;
  map<JObjectID, JGraph_t>  XO;
 
  JGraph_t PD_all;
 
  for (map_type::iterator module = data.begin(); module != data.end(); ++module) {
 
    if (!module->second.empty()) {
      const JLocation location = router.getModule(module->first);
 
      sort(module->second.begin(), module->second.end(), make_comparator(&JAHRS::UNIXTIME));
 
      for (buffer_type::const_iterator p = module->second.begin(); p != module->second.end(); ) {
 
        set<int> buffer;
 
        buffer_type::const_iterator q = p;
        JAverage<JQuaternion3D> Q;
 
        for ( ; q != module->second.end() && q->UNIXTIME - p->UNIXTIME <= Tmax_s * 1.0e3; ++q) {
          Q.put(JCompass(*q, calibration.get(q->DOMID)).getQuaternion());
        }
 
        JDirection3D u(0,0,1);
        JDirection3D x(1,0,0);
        JDirection3D d(cos(DOM_heading_map[module->first]),sin(DOM_heading_map[module->first]),0);
 
        u.rotate(Q);
        x.rotate(Q);
        d.rotate_back(Q);
 
        const double t1  =  p->UNIXTIME * 1.0e-3  +  0.5*Tmax_s;
 
        ZO[module->first].put(t1, atan2(u.getDY(), u.getDX()));
        ZA[module->first].put(t1, sqrt(u.getDX()*u.getDX() + u.getDY()* u.getDY()));
        PD[module->first].put(t1, atan2(d.getDY(), d.getDX()));
        PD_all.put(location.getString(), atan2(d.getDY(), d.getDX()));
        XO[module->first].put(t1, atan2(x.getDY(), x.getDX()));
 
        p = q;
      }
    }
  }
 
 
  TFile out(outputFile.c_str(), "recreate");
 
  for (map<JObjectID, JGraph_t>::const_iterator i = ZO.begin(); i != ZO.end(); ++i) {
    const JLocation location = router.getModule(i->first);
    out << JGraph(i->second, MAKE_CSTRING("G[" << location.getString() << ","<< location.getFloor()  << "].orientation"));
  }
 
  for (map<JObjectID, JGraph_t>::const_iterator i = ZA.begin(); i != ZA.end(); ++i) {
    const JLocation location = router.getModule(i->first);
    out << JGraph(i->second, MAKE_CSTRING("G[" << location.getString() << ","<< location.getFloor() << "].amplitude"));
  }
 
  for (map<JObjectID, JGraph_t>::const_iterator i = PD.begin(); i != PD.end(); ++i) {
    const JLocation location = router.getModule(i->first);
    out << JGraph(i->second, MAKE_CSTRING("G[" << location.getString() << ","<< location.getFloor() << "].offset"));
  }
 
  for (map<JObjectID, JGraph_t>::const_iterator i = XO.begin(); i != XO.end(); ++i) {
    const JLocation location = router.getModule(i->first);
    out << JGraph(i->second, MAKE_CSTRING("G[" << location.getString() << ","<< location.getFloor() << "].xorientation"));
  }
  out << JGraph(PD_all, "all_off");
 
 
  out.Write();
  out.Close();
}