software/JCompass/JCompass.cc

Go to the documentation of this file.

#include <iostream>
#include <iomanip>
#include <vector>
#include <map>

#include "TROOT.h"
#include "TFile.h"
#include "TH1D.h"
#include "TH2D.h"

#include "JDB/JSupport.hh"
#include "JDB/JAHRS.hh"
#include "JDB/JAHRSCalibration_t.hh"
#include "JDB/JAHRSToolkit.hh"

#include "JSupport/JMultipleFileScanner.hh"
#include "JSupport/JFileRecorder.hh"
#include "JSupport/JMeta.hh"

#include "JROOT/JManager.hh"
#include "JFit/JSimplex.hh"
#include "JLang/JComparator.hh"
#include "JSystem/JStat.hh"

#include "JDetector/JDetector.hh"
#include "JDetector/JDetectorToolkit.hh"
#include "JDetector/JModuleRouter.hh"
#include "JDetector/JStringRouter.hh"
#include "JDetector/JCompass.hh"

#include "JCompass/JHit.hh"
#include "JCompass/JModel.hh"
#include "JCompass/JEvt.hh"
#include "JCompass/JEvtToolkit.hh"
#include "JCompass/JSupport.hh"

#include "Jeep/JProperties.hh"
#include "Jeep/JContainer.hh"
#include "Jeep/JPrint.hh"
#include "Jeep/JParser.hh"
#include "Jeep/JMessage.hh"


/**
 * \file
 *
 * Program to calibrate in situ AHRS.
 * \author mdejong
 */
int main(int argc, char **argv)
{
  using namespace std;
  using namespace JPP;

  JMultipleFileScanner_t inputFile;
  counter_type    numberOfEvents;
  JFileRecorder<JTYPELIST<JEvt, JMeta, TH1D, TH2D>::typelist>  outputFile;
  string          detectorFile;
  long long int   Tmax_s           = 600;                                // time window data collection [s]
  int             mestimator       = EM_LORENTZIAN;                      // M-estimator fit
  double          sigma_deg        = 1.0;                                // resolution [deg]
  double          stdev            = numeric_limits<double>::max();      // number of standard deviations
  int             numberOfOutliers = 0;
  string          ahrsFile;
  bool            overwriteDetector;
  int             debug;

  try {

    JProperties properties;

    properties.insert(gmake_property(Tmax_s));
    properties.insert(gmake_property(mestimator));
    properties.insert(gmake_property(sigma_deg));
    properties.insert(gmake_property(stdev));
    properties.insert(gmake_property(numberOfOutliers));

    JParser<> zap("Program to calibrate in situ AHRS.");
    
    zap['f'] = make_field(inputFile,    "output of JConvertDB -q ahrs");
    zap['n'] = make_field(numberOfEvents)                                    = JLimit::max();
    zap['a'] = make_field(detectorFile);
    zap['@'] = make_field(properties)                                        = JPARSER::initialised();
    zap['c'] = make_field(ahrsFile,     "output of JAHRSCalibration");
    zap['A'] = make_field(overwriteDetector);
    zap['o'] = make_field(outputFile)    = "compass.root";
    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 floor_range   range = getRangeOfFloors(detector);

  const JModuleRouter router(detector);
  const JStringRouter string(detector);
  

  map<int, JAverage<JQuaternion3D> > compass;                     // output compass calibration


  const JAHRSCalibration_t calibration(ahrsFile.c_str());
  const JAHRSValidity      is_valid;


  JSimplex<JModel> simplex;

  JSimplex<JModel>::MAXIMUM_ITERATIONS = 10000;

  simplex.debug = debug;

  const JChi2 getChi2(mestimator);


  typedef JManager<int, TH1D>  JManager_t;

  JManager_t H0(new TH1D("%.twist", NULL, 100,  0.0,   5.0));
  JManager_t H1(new TH1D("%.swing", NULL, 250,  0.0,   2.5));
  JManager_t HN(new TH1D("%.count", NULL, 100, -0.5,  99.5));

  TH2D  h2("h2", NULL,
           getNumberOfStrings(detector), -0.5, getNumberOfStrings(detector) - 0.5,
           range.getLength(), range.getLowerLimit() - 0.5, range.getUpperLimit() + 0.5);

  for (Int_t i = 1; i <= h2.GetXaxis()->GetNbins(); ++i) {
    h2.GetXaxis()->SetBinLabel(i, MAKE_CSTRING(string.at(i-1)));
  }
  for (Int_t i = 1; i <= h2.GetYaxis()->GetNbins(); ++i) {
    h2.GetYaxis()->SetBinLabel(i, MAKE_CSTRING(i));
  }

  TH2D* h1 = (TH2D*) h2.Clone("h1");


  outputFile.open();

  outputFile.put(JMeta(argc, argv));

  counter_type counter = 0;
  
  for (JMultipleFileScanner_t::const_iterator file_name = inputFile.begin(); file_name != inputFile.end(); ++file_name) {

    STATUS("processing file " << *file_name << endl);
      
    map<int, vector<JAHRS> > data;                                // AHRS data per string
    
    for (JMultipleFileScanner<JAHRS> in(*file_name); in.hasNext() && counter != numberOfEvents; ++counter) {

      const JAHRS* parameters = in.next();

      if (is_valid(*parameters) && router.hasModule(parameters->DOMID)) {
        data[router.getModule(parameters->DOMID).getString()].push_back(*parameters);
      }
    }
    
    for (map<int, vector<JAHRS> >::iterator i = data.begin(); i != data.end(); ++i) {

      sort(i->second.begin(), i->second.end(), make_comparator(&JAHRS::UNIXTIME));
    
      for (vector<JAHRS>::const_iterator p = i->second.begin(); p != i->second.end(); ) {

        long long int t1 = p->UNIXTIME;
        long long int t2 = t1;

        vector<JHit> buffer;                                      // calibrated quaternion data

        for ( ; p != i->second.end() && p->UNIXTIME < t1 + Tmax_s * 1000; t2 = (p++)->UNIXTIME) {

          if (calibration.has(p->DOMID)) {

            const JModule& module = router.getModule(p->DOMID);

            if (module.getFloor() != 0) {

              const JCompass compass(*p, calibration.get(p->DOMID));

              const JQuaternion3D Q = module.getQuaternion() * compass.getQuaternion();

              buffer.push_back(JHit(p->DOMID, module.getZ(), Q, sigma_deg));
            }
          }
        }

        if (buffer.size() > JModel::NUMBER_OF_PARAMETERS) {

          for (vector<JHit>::const_iterator hit = buffer.begin(); hit != buffer.end(); ++hit) {

            const JLocation& location = router.getModule(hit->getID());

            h1->Fill((double) string.getIndex(location.getString()), (double) location.getFloor());
          }
          
          JModel result(buffer.begin(), buffer.end());            // prefit

          vector<JHit>::iterator __end = buffer.end();

          for (int ns = 0; ns != numberOfOutliers; ++ns) {        // outlier removal

            double                 xmax = 0.0;
            vector<JHit>::iterator out  = __end;
          
            for (vector<JHit>::iterator hit = buffer.begin(); hit != __end; ++hit) {

              const JQuaternion3D Q1 = result(hit->getZ());       // fitted
              const JQuaternion3D Q2 = hit->getQuaternion();      // measured

              const double x = getAngle(Q1, Q2);

              if (x > xmax) {
                xmax = x;
                out  = hit;
              }
            }

            if (xmax > stdev * sigma_deg) {

              const JLocation& location = router.getModule(out->getID());

              h2.Fill((double) string.getIndex(location.getString()), (double) location.getFloor());

              if (debug >= debug_t) {
              
                const JQuaternion3D Q1 = result(out->getZ());     // fitted
                const JQuaternion3D Q2 = out->getQuaternion();    // measured

                const JQuaternion3D::decomposition q1(Q1, JVector3Z_t);
                const JQuaternion3D::decomposition q2(Q2, JVector3Z_t);

                cout << "remove "  << location                     << ' '
                     << FIXED(5,2) << getAngle(Q1,Q2)              << ' '
                     << FIXED(5,2) << getAngle(q1.twist, q2.twist) << ' '
                     << FIXED(5,2) << getAngle(q1.swing, q2.swing) << endl;
              }
            
              swap(*out, *--__end);

              result = JModel(buffer.begin(), __end);             // refit

            } else {

              break;
            }
          }

          simplex.value = result;                                 // start value

          simplex.step.resize(4);

          simplex.step[0] = JModel(JQuaternion3X(5.0e-1 * PI / 180.0), JQuaternion3D::getIdentity());
          simplex.step[1] = JModel(JQuaternion3Y(5.0e-1 * PI / 180.0), JQuaternion3D::getIdentity());
          simplex.step[2] = JModel(JQuaternion3Z(5.0e-1 * PI / 180.0), JQuaternion3D::getIdentity());
          simplex.step[3] = JModel(JQuaternion3D::getIdentity(), JQuaternion3Z(5.0e-2 * PI / 180.0));

          const double chi2 = simplex(getChi2, buffer.begin(), __end);
          const int    ndf  = distance(buffer.begin(), __end) * 4 - simplex.step.size();

          result = simplex.value;                                 // final value


          outputFile.put(getEvt(JHead(t1, t2, i->first, ndf, chi2), result));


          for (vector<JHit>::const_iterator hit = buffer.begin(); hit != buffer.end(); ++hit) {

            const JQuaternion3D Q1 = result(hit->getZ());         // fitted
            const JQuaternion3D Q2 = hit->getQuaternion();        // measured

            const JQuaternion3D::decomposition q1(Q1, JVector3Z_t);
            const JQuaternion3D::decomposition q2(Q2, JVector3Z_t);

            compass[hit->getID()].put(Q1 * Q2.getConjugate());
          
            H0[hit->getID()]->Fill(getAngle(q1.twist, q2.twist));
            H1[hit->getID()]->Fill(getAngle(q1.swing, q2.swing));
          }

          map<int, int> count;

          for (vector<JHit>::const_iterator hit = buffer.begin(); hit != __end; ++hit) {
            count[hit->getID()] += 1;
          }

          for (map<int, int>::const_iterator i = count.begin(); i != count.end(); ++i) {
            HN[i->first]->Fill(i->second);
          }
        }
      }
    }
  }

  
  h2.Divide(h1);

  outputFile.put(h2);

  for (JManager_t* p : { &H0, &H1, &HN }) {
    for (JManager_t::iterator i = p->begin(); i != p->end(); ++i) {
      outputFile.put(*(i->second));
    }
  }

  outputFile.close();


  if (overwriteDetector) {

    NOTICE("Store calibration data on file " << detectorFile << endl);

    if (detector.setToLatestVersion()) {
      NOTICE("Set detector version to " << detector.getVersion() << endl);
    }

    detector.comment.add(JMeta(argc, argv));

    for (map<int, JAverage<JQuaternion3D> >::const_iterator i = compass.begin(); i != compass.end(); ++i) {

      JModule& module = detector[router.getIndex(i->first)];

      JQuaternion3D Q(i->second * module.getQuaternion());

      module.setQuaternion(Q.normalise());
    }

    store(detectorFile, detector);
  }
}