software/JCompass/JBallarat.cc

Go to the documentation of this file.

#include <string>
#include <iostream>
#include <iomanip>
#include <algorithm>
#include <utility>
#include <vector>
 
#include "km3net-dataformat/definitions/module_status.hh"
 
#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 "JDetector/JDetector.hh"
#include "JDetector/JDetectorToolkit.hh"
#include "JDetector/JModuleRouter.hh"
#include "JDetector/JCompass.hh"
 
#include "JGeometry3D/JQuaternion3D.hh"
#include "JGeometry3D/JEigen3D.hh"
 
#include "JTools/JHashMap.hh"
#include "JLang/JVectorize.hh"
#include "JLang/JComparator.hh"
#include "JMath/JLegendre.hh"
 
#include "JCompass/JEvt.hh"
#include "JCompass/JEvtToolkit.hh"
#include "JCompass/JSupport.hh"
#include "JCompass/JNOAA.hh"
#include "JCompass/JPolicy.hh"
#include "JCompass/JCompassSupportkit.hh"
 
#include "Jeep/JPrint.hh"
#include "Jeep/JParser.hh"
#include "Jeep/JMessage.hh"
 
namespace {
 
  static const int NUMBER_OF_POINTS  = 20;    //!< number of points  for interpolation
  static const int NUMBER_OF_DEGREES =  1;    //!< number of degrees for interpolation
 
}
 
 
/**
 * \file
 *
 * Auxiliary program to process AHRS data.
 *
 * The AHRS calibration as well as the quaternion calibration are applied to the data.\n
 * Possible outliers are removed according the specified maximal angle (option <tt>-S <angle_deg></tt>.\n
 * For this, interpolations of the data are made based on Legendre polynomials with 
 * the number of points and the number of degrees\n
 * set to NUMBER_OF_POINTS and NUMBER_OF_DEGREES, respectively.\n
 * A policy for missing compass data is applied in which data from neighbouring compasses are used.\n
 * The number of floors that will be covered can be specified with option <tt>-N <npy></tt>.\n
 * Finally, data within a given time window can be averaged (option <tt>-T <T_s></tt>.\n)
 * \author mdejong
 */
int main(int argc, char **argv)
{
  using namespace std;
  using namespace JPP;
 
  JMultipleFileScanner_t inputFile;
  counter_type    numberOfEvents;
  JFileRecorder<JTYPELIST<JOrientation, JMeta>::typelist>  outputFile;
  string          detectorFile;
  string          ahrsFile;
  double          angle_deg;
  double          T_s;
  size_t          npy;
  int             debug;
 
  try {
 
    JParser<> zap("Auxiliary program to process AHRS data.");
 
    zap['f'] = make_field(inputFile,    "output of JConvertDB -q ahrs");
    zap['n'] = make_field(numberOfEvents)                                    = JLimit::max();
    zap['a'] = make_field(detectorFile);
    zap['c'] = make_field(ahrsFile,     "output of JAHRSCalibration");
    zap['o'] = make_field(outputFile,   "ROOT formatted orientations file");
    zap['S'] = make_field(angle_deg,    "maximal angle w.r.t. fit")          = 0.0;
    zap['T'] = make_field(T_s,          "time window for averaging")         = 0.0;
    zap['N'] = make_field(npy,          "number of floors for missing data policy") = 1;
    zap['d'] = make_field(debug)         = 1;
 
    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 JNOAAFunction1D_t& getMagneticDeclination = (isARCADetector(detector) ? static_cast<const JNOAAFunction1D_t&>(getARCAMagneticDeclination) :
                                                     isORCADetector(detector) ? static_cast<const JNOAAFunction1D_t&>(getORCAMagneticDeclination) :
                                                     static_cast<const JNOAAFunction1D_t&>(getZEROMagneticDeclination));
  const double             meridian               = (isARCADetector(detector) ? ARCA_MERIDIAN_CONVERGENCE_ANGLE_RAD :
                                                     isORCADetector(detector) ? ORCA_MERIDIAN_CONVERGENCE_ANGLE_RAD :
                                                     0.0);
 
  NOTICE("Magnetic declination " << getMagneticDeclination << endl);
  NOTICE("Meridian angle [rad] " << FIXED(5,3) << meridian << endl);
 
  const JAHRSCalibration_t calibration(ahrsFile.c_str());
  const JAHRSValidity      is_valid;
 
 
  struct  element_type;
  typedef vector<element_type>          buffer_type;
  typedef JHashMap<int, buffer_type>    map_type;               // container for temporary storage of data
 
 
  struct element_type {
    /**
     * Default constructor.
     */
    element_type() :
      first(0.0),
      second(),
      ns(0),
      policy(false)
    {}
 
    /**
     * Constructor.
     *
     * \param  t1         time [s]
     * \param  Q          quaternion
     * \param  ns         number of points
     * \param  policy     true if based on policy
     */
    element_type(const double         t1,
                 const JQuaternion3D& Q,
                 const size_t         ns,
                 const bool           policy) :
      first(t1),
      second(Q),
      ns(ns),
      policy(policy)
    {}
 
    /**
     * Get common policy.
     *
     * \param  __begin    begin of elements
     * \param  __end      end   of elements
     * \return            true if one or more elements are based on policy; else false
     */
    static inline bool getOR(buffer_type::const_iterator __begin,
                             buffer_type::const_iterator __end)
    {
      for (buffer_type::const_iterator i = __begin; i != __end; ++i) {
        if (i->policy) {
          return true;
        }
      }
 
      return false;
    }
 
    /**
     * Get common policy.
     *
     * \param  __begin    begin of elements
     * \param  __end      end   of elements
     * \return            true if all elements are based on policy; else false
     */
    static inline bool getAND(buffer_type::const_iterator __begin,
                              buffer_type::const_iterator __end)
    {
      for (buffer_type::const_iterator i = __begin; i != __end; ++i) {
        if (!i->policy) {
          return false;
        }
      }
 
      return true;
    }
 
    double         first;          // time [s]
    JQuaternion3D  second;         // quaternion 
    size_t         ns;             // number of points
    bool           policy;         // true if based on policy; else false
  };
 
 
  JLegendre<JQuaternion3D, NUMBER_OF_DEGREES>  f1;              // interpolator for outlier removal
 
 
  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 << "... " << flush);
 
    // collect data
 
    map_type data;
 
    for (JMultipleFileScanner<JAHRS> in(*file_name); in.hasNext() && counter != numberOfEvents; ++counter) {
 
      const JAHRS* parameters = in.next();
      const int    id         = parameters->DOMID; 
 
      if (is_valid(*parameters) && calibration.has(id) && router.hasModule(id)) {
 
        const JModule& module = router.getModule(id);
 
        if (module.getFloor() != 0 && !module.has(COMPASS_DISABLE)) {
 
          JCompass compass(*parameters, calibration.get(id));
 
          compass.correct(getMagneticDeclination(parameters->UNIXTIME * 1.0e-3), meridian);
 
          const JQuaternion3D Q = compass.getQuaternion();
 
          data[id].push_back(element_type(parameters->UNIXTIME * 1.0e-3, Q, 1, false));
        }
      }
    }
 
    // remove outliers
 
    if (angle_deg > 0.0) {
 
      for (map_type::iterator module = data.begin(); module != data.end(); ++module) {
 
        if (!module->second.empty()) {
 
          sort(module->second.begin(), module->second.end(), make_comparator(&element_type::first));
 
          buffer_type::iterator       out = module->second.begin();       // output
          buffer_type::const_iterator in  = module->second.begin();       // input
          buffer_type::const_iterator p   = module->second.begin();       // begin interpolation data
          buffer_type::const_iterator q   = module->second.begin();       // end   interpolation data
      
          for (int i = 0; i != NUMBER_OF_POINTS && q != module->second.end(); ++i, ++q) {}
 
          for (int i = 0; i != NUMBER_OF_POINTS/2 && in != q; ++i, ++in) {
 
            f1.set(p, in, q);
 
            if (getAngle(in->second, f1(in->first)) <= angle_deg) {
              *out = *in;
              ++out;
            }
          }
 
          for (++p; q++ != module->second.end(); ++p, ++in) {
 
            f1.set(p, in, q);
 
            if (getAngle(in->second, f1(in->first)) <= angle_deg) {
              *out = *in;
              ++out;
            }
          }
 
          for ( ; in != module->second.end(); ++in) {
 
            f1.set(p, in, q);
 
            if (getAngle(in->second, f1(in->first)) <= angle_deg) {
              *out = *in;
              ++out;
            }
          }
 
          module->second.erase(out, module->second.end());                // remove leftovers
        }
      }
    }
 
 
    // apply policy for missing modules and write output
    
    const array_type<map_type::key_type>& buffer = make_array(data.begin(), data.end(), &map_type::value_type::first);
 
    const JPolicy policy(router, buffer.begin(), buffer.end(), npy);
 
    for (JPolicy::const_iterator module = policy.begin(); module != policy.end(); ++module) {
      
      buffer_type buffer;
 
      for (JPolicy::mapped_type::const_iterator in = module->second.begin(); in != module->second.end(); ++in) {
        for (map_type::mapped_type::const_iterator i = data[*in].begin(); i != data[*in].end(); ++i) {
          buffer.push_back(element_type(i->first, i->second, 1, true));
        }
      }
 
      // average
 
      if (T_s > 0.0) {
 
        sort(buffer.begin(), buffer.end(), make_comparator(&element_type::first));
 
        buffer_type::iterator       out = buffer.begin();                 // output
        buffer_type::const_iterator p   = buffer.begin();                 // begin averaging data
        buffer_type::const_iterator q   = buffer.begin();                 // end   averaging data
 
        for (p = buffer.begin(); p != buffer.end(); p = q) {
 
          for (q = p; q != buffer.end() && q->first - p->first < T_s; ++q) {}
 
          if (distance(p,q) > 1) {
 
            const array_type<double>&        t1 = make_array(p, q, &element_type::first);
            const array_type<JQuaternion3D>& q1 = make_array(p, q, &element_type::second);
 
            out->first  = getAverage(t1.begin(), t1.end());
            out->second = getAverage(q1.begin(), q1.end());
            out->ns     = distance(p,q);
            out->policy = element_type::getOR(p,q);
 
          } else {
 
            *out = *p;
          }
 
          ++out;
        }
 
        buffer.erase(out, buffer.end());                                  // remove leftovers
      }
 
      for (buffer_type::const_iterator i = buffer.begin(); i != buffer.end(); ++i) {
        outputFile.put(JOrientation(module->first, i->first, getQuaternion(i->second), i->ns, i->policy));
      }
    }
 
    STATUS(endl);
  }
 
  JMultipleFileScanner<JMeta> io(inputFile);
 
  io >> outputFile;
 
  outputFile.close();
}