#include #include #include #include #include #include #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 -S \.\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 * Finally, data within a given time window can be averaged (option -T \.\n) * \author mdejong */ int main(int argc, char **argv) { using namespace std; using namespace JPP; JMultipleFileScanner_t inputFile; counter_type numberOfEvents; JFileRecorder::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(getARCAMagneticDeclination) : isORCADetector(detector) ? static_cast(getORCAMagneticDeclination) : static_cast(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 element_type; typedef vector buffer_type; typedef JHashMap map_type; // container for temporary storage of data JLegendre 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 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& 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& t1 = make_array(p, q, &element_type::first); const array_type& 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 io(inputFile); io >> outputFile; outputFile.close(); }