software/JCompass/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.
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

int main	(	int	argc,
		char **	argv
	)

Definition at line 65 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;
  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['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;


  typedef pair<double, JQuaternion3D>   element_type;
  typedef vector<element_type>          buffer_type;
  typedef JHashMap<int, buffer_type>    map_type;               // container for temporary storage of data

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

    // 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());

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

      // 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());

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

    STATUS(endl);
  }

  JMultipleFileScanner<JMeta> io(inputFile);

  io >> outputFile;

  outputFile.close();
}