JBallarat.cc File Reference

Auxiliary program to process AHRS data. More...

#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"

Go to the source code of this file.

Functions
int	main (int argc, char **argv)

Detailed Description

Auxiliary program to process AHRS data.

The AHRS calibration as well as the quaternion calibration are applied to the data.
Possible outliers are removed according the specified maximal angle (option -S <angle_deg>.
For this, interpolations of the data are made based on Legendre polynomials with the number of points and the number of degrees
set to NUMBER_OF_POINTS and NUMBER_OF_DEGREES, respectively.
A policy for missing compass data is applied in which data from neighbouring compasses are used.
The number of floors that will be covered can be specified with option -N <npy>.
Finally, data within a given time window can be averaged (option -T <T_s>.
)

Author

mdejong

Definition in file software/JCompass/JBallarat.cc.

Function Documentation

main()

int main	(	int	argc,
		char **	argv )

Default constructor.

Constructor.

Parameters

t1	time [s]
Q	quaternion
ns	number of points
policy	true if based on policy

Get common policy.

Parameters

__begin	begin of elements
__end	end of elements

Returns

true if one or more elements are based on policy; else false

Get common policy.

Parameters

__begin	begin of elements
__end	end of elements

Returns

true if all elements are based on policy; else false

Definition at line 66 of file software/JCompass/JBallarat.cc.

{
  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();
}