diff --git a/README.md b/README.md index ed8ccea..d34471e 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,119 @@ -# scancontext_tro -scancontext++ (TRO 2022) codes + + +# Scan Context + +## NEWS (Dec, 2023): Recent updates +- The previous official repository `irapkaist/scancontext` will be unavailable soon because the owner `irapkaist` wille be closed (the lab moved to [SNU](https://rpm.snu.ac.kr/)). Therefore, scancontext repository is moving to here. +- T-RO journal version codes have been added. + - See `tro2022` directory. + - Particularly, `tro2022/scancontext/desc/ptcloud2cartcontext.m` is the journal version's variation. + - This directory contains whole helper functions and batch scripts for doing evaluation during the T-RO revision. Thus, the Matlab codes may not be runnable directly, so please use this for reference in understanding the paper. + +## NEWS (Oct, 2021): Scan Context++ is accepted for T-RO! +- Our extended study named Scan Context++ is accepted for T-RO. + - Scan Context++: Structural Place Recognition Robust to Rotation and Lateral Variations in Urban Environments + - [Paper](https://arxiv.org/pdf/2109.13494.pdf), [Summary](https://threadreaderapp.com/thread/1443044133937942533.html), [Video](https://youtu.be/ZWEqwYKQIeg) +- The additional evaluation codes (e.g., lateral evaluations on Oxford Radar RobotCar dataset) with the new metric (we call it recall-distribution based on KL-D) will be added soon. + +## Note +- Scan Context can be easily integrated with any LiDAR odometry algorithms or any LiDAR sensors. Examples are: + - Integrated with A-LOAM: [SC-A-LOAM](https://github.com/gisbi-kim/SC-A-LOAM) + - Integrated with LeGO-LOAM: [SC-LeGO-LOAM](https://github.com/irapkaist/SC-LeGO-LOAM) + - Integrated with LIO-SAM: [SC-LIO-SAM](https://github.com/gisbi-kim/SC-LIO-SAM) + - Integrated with FAST-LIO2: [FAST_LIO_SLAM](https://github.com/gisbi-kim/FAST_LIO_SLAM) + - Integrated with a basic ICP odometry: [PyICP-SLAM](https://github.com/gisbi-kim/PyICP-SLAM) + - This implementation is fully python-based so slow but educational purpose. + - If you find a fast python API for Scan Context, use [https://github.com/gisbi-kim/scancontext-pybind](https://github.com/gisbi-kim/scancontext-pybind) +- Scan Context also works for radar. + - Integrated with yeti-radar-odometry for radar SLAM: [navtech-radar-slam](https://github.com/gisbi-kim/navtech-radar-slam) + - p.s. please see the ``fast_evaluator_radar`` directory for the radar place recognition evaluation (radar scan context was introduced in [MulRan dataset](https://sites.google.com/view/mulran-pr/home) paper). + +## NEWS (April, 2020): C++ implementation +- C++ implementation released! + - See the directory `cpp/module/Scancontext` + - Features + - Light-weight: a single header and cpp file named "Scancontext.h" and "Scancontext.cpp" + - Our module has KDtree and we used nanoflann. nanoflann is an also single-header-program and that file is in our directory. + - Easy to use: A user just remembers and uses only two API functions; `makeAndSaveScancontextAndKeys` and `detectLoopClosureID`. + - Fast: tested the loop detector runs at 10-15Hz (for 20 x 60 size, 10 candidates) + - Example: Real-time LiDAR SLAM + - We integrated the C++ implementation within the recent popular LiDAR odometry codes (e.g., LeGO-LOAM and A-LOAM). + - That is, LiDAR SLAM = LiDAR Odometry (LeGO-LOAM) + Loop detection (Scan Context) and closure (GTSAM) + - For details, see `cpp/example/lidar_slam` or refer these repositories: SC-LeGO-LOAM or SC-A-LOAM. +--- + + +- Scan Context is a global descriptor for LiDAR point cloud, which is proposed in this paper and details are easily summarized in this video . + +``` +@ARTICLE { gskim-2021-tro, + AUTHOR = { Giseop Kim and Sunwook Choi and Ayoung Kim }, + TITLE = { Scan Context++: Structural Place Recognition Robust to Rotation and Lateral Variations in Urban Environments }, + JOURNAL = { IEEE Transactions on Robotics }, + YEAR = { 2021 }, + NOTE = { Accepted. To appear. }, +} + +@INPROCEEDINGS { gkim-2018-iros, + author = {Kim, Giseop and Kim, Ayoung}, + title = { Scan Context: Egocentric Spatial Descriptor for Place Recognition within {3D} Point Cloud Map }, + booktitle = { Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems }, + year = { 2018 }, + month = { Oct. }, + address = { Madrid } +} +``` +- This point cloud descriptor is used for place retrieval problem such as place +recognition and long-term localization. + + +## What is Scan Context? + +- Scan Context is a global descriptor for LiDAR point cloud, which is especially designed for a sparse and noisy point cloud acquired in outdoor environment. +- It encodes egocentric visible information as below: +

+ +- A user can vary the resolution of a Scan Context. Below is the example of Scan Contexts' various resolutions for the same point cloud. +

+ + +## How to use?: example cases +- The structure of this repository is composed of 3 example use cases. +- Most of the codes are written in Matlab. +- A directory _matlab_ contains main functions including Scan Context generation and the distance function. +- A directory _example_ contains a full example code for a few applications. We provide a total 3 examples. + 1. _**basics**_ contains a literally basic codes such as generation and can be a start point to understand Scan Context. + + 2. _**place recognition**_ is an example directory for our IROS18 paper. The example is conducted using KITTI sequence 00 and PlaceRecognizer.m is the main code. You can easily grasp the full pipeline of Scan Context-based place recognition via watching and following the PlaceRecognizer.m code. Our Scan Context-based place recognition system consists of two steps; description and search. The search step is then composed of two hierarchical stages (1. ring key-based KD tree for fast candidate proposal, 2. candidate to query pairwise comparison-based nearest search). We note that our coarse yaw aligning-based pairwise distance enables reverse-revisit detection well, unlike others. The pipeline is below. +

+ + 3. _**long-term localization**_ is an example directory for our RAL19 paper. For the separation of mapping and localization, there are separated train and test steps. The main training and test codes are written in python and Keras, only excluding data generation and performance evaluation codes (they are written in Matlab), and those python codes are provided using jupyter notebook. We note that some path may not directly work for your environment but the evaluation codes (e.g., makeDataForPRcurveForSCIresult.m) will help you understand how this classification-based SCI-localization system works. The figure below depicts our long-term localization pipeline.

More details of our long-term localization pipeline is found in the below paper and we also recommend you to watch this video . +``` +@ARTICLE{ gkim-2019-ral, + author = {G. {Kim} and B. {Park} and A. {Kim}}, + journal = {IEEE Robotics and Automation Letters}, + title = {1-Day Learning, 1-Year Localization: Long-Term LiDAR Localization Using Scan Context Image}, + year = {2019}, + volume = {4}, + number = {2}, + pages = {1948-1955}, + month = {April} +} +``` + + 4. _**SLAM**_ directory contains the practical use case of Scan Context for SLAM pipeline. The details are maintained in the related other repository _[PyICP SLAM](https://github.com/kissb2/PyICP-SLAM)_; the full-python LiDAR SLAM codes using Scan Context as a loop detector. + +## Acknowledgment +This work is supported by the Korea Agency for Infrastructure Technology Advancement (KAIA) grant funded by the Ministry of Land, Infrastructure and Transport of Korea (19CTAP-C142170-02), and [High-Definition Map Based Precise Vehicle Localization Using Cameras and LIDARs] project funded by NAVER LABS Corporation. + +## Contact +If you have any questions, contact here please + ``` + paulgkim@kaist.ac.kr + ``` + +## License + Creative Commons License
This work is licensed under a Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License. + +### Copyright +- All codes on this page are copyrighted by KAIST and Naver Labs and published under the Creative Commons Attribution-NonCommercial-ShareAlike 4.0 License. You must attribute the work in the manner specified by the author. You may not use the work for commercial purposes, and you may only distribute the resulting work under the same license if you alter, transform, or create the work. diff --git a/cpp/example/lidar_slam/README.md b/cpp/example/lidar_slam/README.md new file mode 100644 index 0000000..92c3002 --- /dev/null +++ b/cpp/example/lidar_slam/README.md @@ -0,0 +1,2 @@ +# Go to +- https://github.com/irapkaist/SC-LeGO-LOAM diff --git a/cpp/module/Scancontext/KDTreeVectorOfVectorsAdaptor.h b/cpp/module/Scancontext/KDTreeVectorOfVectorsAdaptor.h new file mode 100644 index 0000000..0a56d6e --- /dev/null +++ b/cpp/module/Scancontext/KDTreeVectorOfVectorsAdaptor.h @@ -0,0 +1,117 @@ +/*********************************************************************** + * Software License Agreement (BSD License) + * + * Copyright 2011-16 Jose Luis Blanco (joseluisblancoc@gmail.com). + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR + * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *************************************************************************/ + +#pragma once + +#include + +#include + +// ===== This example shows how to use nanoflann with these types of containers: ======= +//typedef std::vector > my_vector_of_vectors_t; +//typedef std::vector my_vector_of_vectors_t; // This requires #include +// ===================================================================================== + + +/** A simple vector-of-vectors adaptor for nanoflann, without duplicating the storage. + * The i'th vector represents a point in the state space. + * + * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations. + * \tparam num_t The type of the point coordinates (typically, double or float). + * \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. + * \tparam IndexType The type for indices in the KD-tree index (typically, size_t of int) + */ +template +struct KDTreeVectorOfVectorsAdaptor +{ + typedef KDTreeVectorOfVectorsAdaptor self_t; + typedef typename Distance::template traits::distance_t metric_t; + typedef nanoflann::KDTreeSingleIndexAdaptor< metric_t,self_t,DIM,IndexType> index_t; + + index_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index. + + /// Constructor: takes a const ref to the vector of vectors object with the data points + KDTreeVectorOfVectorsAdaptor(const size_t /* dimensionality */, const VectorOfVectorsType &mat, const int leaf_max_size = 10) : m_data(mat) + { + assert(mat.size() != 0 && mat[0].size() != 0); + const size_t dims = mat[0].size(); + if (DIM>0 && static_cast(dims) != DIM) + throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument"); + index = new index_t( static_cast(dims), *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size ) ); + index->buildIndex(); + } + + ~KDTreeVectorOfVectorsAdaptor() { + delete index; + } + + const VectorOfVectorsType &m_data; + + /** Query for the \a num_closest closest points to a given point (entered as query_point[0:dim-1]). + * Note that this is a short-cut method for index->findNeighbors(). + * The user can also call index->... methods as desired. + * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface. + */ + inline void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq, const int nChecks_IGNORED = 10) const + { + nanoflann::KNNResultSet resultSet(num_closest); + resultSet.init(out_indices, out_distances_sq); + index->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); + } + + /** @name Interface expected by KDTreeSingleIndexAdaptor + * @{ */ + + const self_t & derived() const { + return *this; + } + self_t & derived() { + return *this; + } + + // Must return the number of data points + inline size_t kdtree_get_point_count() const { + return m_data.size(); + } + + // Returns the dim'th component of the idx'th point in the class: + inline num_t kdtree_get_pt(const size_t idx, const size_t dim) const { + return m_data[idx][dim]; + } + + // Optional bounding-box computation: return false to default to a standard bbox computation loop. + // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again. + // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds) + template + bool kdtree_get_bbox(BBOX & /*bb*/) const { + return false; + } + + /** @} */ + +}; // end of KDTreeVectorOfVectorsAdaptor diff --git a/cpp/module/Scancontext/Scancontext.cpp b/cpp/module/Scancontext/Scancontext.cpp new file mode 100644 index 0000000..5376c69 --- /dev/null +++ b/cpp/module/Scancontext/Scancontext.cpp @@ -0,0 +1,340 @@ +#include "Scancontext.h" + +// namespace SC2 +// { + +void coreImportTest (void) +{ + cout << "scancontext lib is successfully imported." << endl; +} // coreImportTest + + +float rad2deg(float radians) +{ + return radians * 180.0 / M_PI; +} + +float deg2rad(float degrees) +{ + return degrees * M_PI / 180.0; +} + + +float xy2theta( const float & _x, const float & _y ) +{ + if ( _x >= 0 & _y >= 0) + return (180/M_PI) * atan(_y / _x); + + if ( _x < 0 & _y >= 0) + return 180 - ( (180/M_PI) * atan(_y / (-_x)) ); + + if ( _x < 0 & _y < 0) + return 180 + ( (180/M_PI) * atan(_y / _x) ); + + if ( _x >= 0 & _y < 0) + return 360 - ( (180/M_PI) * atan((-_y) / _x) ); +} // xy2theta + + +MatrixXd circshift( MatrixXd &_mat, int _num_shift ) +{ + // shift columns to right direction + assert(_num_shift >= 0); + + if( _num_shift == 0 ) + { + MatrixXd shifted_mat( _mat ); + return shifted_mat; // Early return + } + + MatrixXd shifted_mat = MatrixXd::Zero( _mat.rows(), _mat.cols() ); + for ( int col_idx = 0; col_idx < _mat.cols(); col_idx++ ) + { + int new_location = (col_idx + _num_shift) % _mat.cols(); + shifted_mat.col(new_location) = _mat.col(col_idx); + } + + return shifted_mat; + +} // circshift + + +std::vector eig2stdvec( MatrixXd _eigmat ) +{ + std::vector vec( _eigmat.data(), _eigmat.data() + _eigmat.size() ); + return vec; +} // eig2stdvec + + +double SCManager::distDirectSC ( MatrixXd &_sc1, MatrixXd &_sc2 ) +{ + int num_eff_cols = 0; // i.e., to exclude all-nonzero sector + double sum_sector_similarity = 0; + for ( int col_idx = 0; col_idx < _sc1.cols(); col_idx++ ) + { + VectorXd col_sc1 = _sc1.col(col_idx); + VectorXd col_sc2 = _sc2.col(col_idx); + + if( col_sc1.norm() == 0 | col_sc2.norm() == 0 ) + continue; // don't count this sector pair. + + double sector_similarity = col_sc1.dot(col_sc2) / (col_sc1.norm() * col_sc2.norm()); + + sum_sector_similarity = sum_sector_similarity + sector_similarity; + num_eff_cols = num_eff_cols + 1; + } + + double sc_sim = sum_sector_similarity / num_eff_cols; + return 1.0 - sc_sim; + +} // distDirectSC + + +int SCManager::fastAlignUsingVkey( MatrixXd & _vkey1, MatrixXd & _vkey2) +{ + int argmin_vkey_shift = 0; + double min_veky_diff_norm = 10000000; + for ( int shift_idx = 0; shift_idx < _vkey1.cols(); shift_idx++ ) + { + MatrixXd vkey2_shifted = circshift(_vkey2, shift_idx); + + MatrixXd vkey_diff = _vkey1 - vkey2_shifted; + + double cur_diff_norm = vkey_diff.norm(); + if( cur_diff_norm < min_veky_diff_norm ) + { + argmin_vkey_shift = shift_idx; + min_veky_diff_norm = cur_diff_norm; + } + } + + return argmin_vkey_shift; + +} // fastAlignUsingVkey + + +std::pair SCManager::distanceBtnScanContext( MatrixXd &_sc1, MatrixXd &_sc2 ) +{ + // 1. fast align using variant key (not in original IROS18) + MatrixXd vkey_sc1 = makeSectorkeyFromScancontext( _sc1 ); + MatrixXd vkey_sc2 = makeSectorkeyFromScancontext( _sc2 ); + int argmin_vkey_shift = fastAlignUsingVkey( vkey_sc1, vkey_sc2 ); + + const int SEARCH_RADIUS = round( 0.5 * SEARCH_RATIO * _sc1.cols() ); // a half of search range + std::vector shift_idx_search_space { argmin_vkey_shift }; + for ( int ii = 1; ii < SEARCH_RADIUS + 1; ii++ ) + { + shift_idx_search_space.push_back( (argmin_vkey_shift + ii + _sc1.cols()) % _sc1.cols() ); + shift_idx_search_space.push_back( (argmin_vkey_shift - ii + _sc1.cols()) % _sc1.cols() ); + } + std::sort(shift_idx_search_space.begin(), shift_idx_search_space.end()); + + // 2. fast columnwise diff + int argmin_shift = 0; + double min_sc_dist = 10000000; + for ( int num_shift: shift_idx_search_space ) + { + MatrixXd sc2_shifted = circshift(_sc2, num_shift); + double cur_sc_dist = distDirectSC( _sc1, sc2_shifted ); + if( cur_sc_dist < min_sc_dist ) + { + argmin_shift = num_shift; + min_sc_dist = cur_sc_dist; + } + } + + return make_pair(min_sc_dist, argmin_shift); + +} // distanceBtnScanContext + + +MatrixXd SCManager::makeScancontext( pcl::PointCloud & _scan_down ) +{ + TicToc t_making_desc; + + int num_pts_scan_down = _scan_down.points.size(); + + // main + const int NO_POINT = -1000; + MatrixXd desc = NO_POINT * MatrixXd::Ones(PC_NUM_RING, PC_NUM_SECTOR); + + SCPointType pt; + float azim_angle, azim_range; // wihtin 2d plane + int ring_idx, sctor_idx; + for (int pt_idx = 0; pt_idx < num_pts_scan_down; pt_idx++) + { + pt.x = _scan_down.points[pt_idx].x; + pt.y = _scan_down.points[pt_idx].y; + pt.z = _scan_down.points[pt_idx].z + LIDAR_HEIGHT; // naive adding is ok (all points should be > 0). + + // xyz to ring, sector + azim_range = sqrt(pt.x * pt.x + pt.y * pt.y); + azim_angle = xy2theta(pt.x, pt.y); + + // if range is out of roi, pass + if( azim_range > PC_MAX_RADIUS ) + continue; + + ring_idx = std::max( std::min( PC_NUM_RING, int(ceil( (azim_range / PC_MAX_RADIUS) * PC_NUM_RING )) ), 1 ); + sctor_idx = std::max( std::min( PC_NUM_SECTOR, int(ceil( (azim_angle / 360.0) * PC_NUM_SECTOR )) ), 1 ); + + // taking maximum z + if ( desc(ring_idx-1, sctor_idx-1) < pt.z ) // -1 means cpp starts from 0 + desc(ring_idx-1, sctor_idx-1) = pt.z; // update for taking maximum value at that bin + } + + // reset no points to zero (for cosine dist later) + for ( int row_idx = 0; row_idx < desc.rows(); row_idx++ ) + for ( int col_idx = 0; col_idx < desc.cols(); col_idx++ ) + if( desc(row_idx, col_idx) == NO_POINT ) + desc(row_idx, col_idx) = 0; + + t_making_desc.toc("PolarContext making"); + + return desc; +} // SCManager::makeScancontext + + +MatrixXd SCManager::makeRingkeyFromScancontext( Eigen::MatrixXd &_desc ) +{ + /* + * summary: rowwise mean vector + */ + Eigen::MatrixXd invariant_key(_desc.rows(), 1); + for ( int row_idx = 0; row_idx < _desc.rows(); row_idx++ ) + { + Eigen::MatrixXd curr_row = _desc.row(row_idx); + invariant_key(row_idx, 0) = curr_row.mean(); + } + + return invariant_key; +} // SCManager::makeRingkeyFromScancontext + + +MatrixXd SCManager::makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc ) +{ + /* + * summary: columnwise mean vector + */ + Eigen::MatrixXd variant_key(1, _desc.cols()); + for ( int col_idx = 0; col_idx < _desc.cols(); col_idx++ ) + { + Eigen::MatrixXd curr_col = _desc.col(col_idx); + variant_key(0, col_idx) = curr_col.mean(); + } + + return variant_key; +} // SCManager::makeSectorkeyFromScancontext + + +void SCManager::makeAndSaveScancontextAndKeys( pcl::PointCloud & _scan_down ) +{ + Eigen::MatrixXd sc = makeScancontext(_scan_down); // v1 + Eigen::MatrixXd ringkey = makeRingkeyFromScancontext( sc ); + Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext( sc ); + std::vector polarcontext_invkey_vec = eig2stdvec( ringkey ); + + polarcontexts_.push_back( sc ); + polarcontext_invkeys_.push_back( ringkey ); + polarcontext_vkeys_.push_back( sectorkey ); + polarcontext_invkeys_mat_.push_back( polarcontext_invkey_vec ); + + // cout < SCManager::detectLoopClosureID ( void ) +{ + int loop_id { -1 }; // init with -1, -1 means no loop (== LeGO-LOAM's variable "closestHistoryFrameID") + + auto curr_key = polarcontext_invkeys_mat_.back(); // current observation (query) + auto curr_desc = polarcontexts_.back(); // current observation (query) + + /* + * step 1: candidates from ringkey tree_ + */ + if( polarcontext_invkeys_mat_.size() < NUM_EXCLUDE_RECENT + 1) + { + std::pair result {loop_id, 0.0}; + return result; // Early return + } + + // tree_ reconstruction (not mandatory to make everytime) + if( tree_making_period_conter % TREE_MAKING_PERIOD_ == 0) // to save computation cost + { + TicToc t_tree_construction; + + polarcontext_invkeys_to_search_.clear(); + polarcontext_invkeys_to_search_.assign( polarcontext_invkeys_mat_.begin(), polarcontext_invkeys_mat_.end() - NUM_EXCLUDE_RECENT ) ; + + polarcontext_tree_.reset(); + polarcontext_tree_ = std::make_unique(PC_NUM_RING /* dim */, polarcontext_invkeys_to_search_, 10 /* max leaf */ ); + // tree_ptr_->index->buildIndex(); // inernally called in the constructor of InvKeyTree (for detail, refer the nanoflann and KDtreeVectorOfVectorsAdaptor) + t_tree_construction.toc("Tree construction"); + } + tree_making_period_conter = tree_making_period_conter + 1; + + double min_dist = 10000000; // init with somthing large + int nn_align = 0; + int nn_idx = 0; + + // knn search + std::vector candidate_indexes( NUM_CANDIDATES_FROM_TREE ); + std::vector out_dists_sqr( NUM_CANDIDATES_FROM_TREE ); + + TicToc t_tree_search; + nanoflann::KNNResultSet knnsearch_result( NUM_CANDIDATES_FROM_TREE ); + knnsearch_result.init( &candidate_indexes[0], &out_dists_sqr[0] ); + polarcontext_tree_->index->findNeighbors( knnsearch_result, &curr_key[0] /* query */, nanoflann::SearchParams(10) ); + t_tree_search.toc("Tree search"); + + /* + * step 2: pairwise distance (find optimal columnwise best-fit using cosine distance) + */ + TicToc t_calc_dist; + for ( int candidate_iter_idx = 0; candidate_iter_idx < NUM_CANDIDATES_FROM_TREE; candidate_iter_idx++ ) + { + MatrixXd polarcontext_candidate = polarcontexts_[ candidate_indexes[candidate_iter_idx] ]; + std::pair sc_dist_result = distanceBtnScanContext( curr_desc, polarcontext_candidate ); + + double candidate_dist = sc_dist_result.first; + int candidate_align = sc_dist_result.second; + + if( candidate_dist < min_dist ) + { + min_dist = candidate_dist; + nn_align = candidate_align; + + nn_idx = candidate_indexes[candidate_iter_idx]; + } + } + t_calc_dist.toc("Distance calc"); + + /* + * loop threshold check + */ + if( min_dist < SC_DIST_THRES ) + { + loop_id = nn_idx; + + // std::cout.precision(3); + cout << "[Loop found] Nearest distance: " << min_dist << " btn " << polarcontexts_.size()-1 << " and " << nn_idx << "." << endl; + cout << "[Loop found] yaw diff: " << nn_align * PC_UNIT_SECTORANGLE << " deg." << endl; + } + else + { + std::cout.precision(3); + cout << "[Not loop] Nearest distance: " << min_dist << " btn " << polarcontexts_.size()-1 << " and " << nn_idx << "." << endl; + cout << "[Not loop] yaw diff: " << nn_align * PC_UNIT_SECTORANGLE << " deg." << endl; + } + + // To do: return also nn_align (i.e., yaw diff) + float yaw_diff_rad = deg2rad(nn_align * PC_UNIT_SECTORANGLE); + std::pair result {loop_id, yaw_diff_rad}; + + return result; + +} // SCManager::detectLoopClosureID + +// } // namespace SC2 \ No newline at end of file diff --git a/cpp/module/Scancontext/Scancontext.h b/cpp/module/Scancontext/Scancontext.h new file mode 100644 index 0000000..ccccc28 --- /dev/null +++ b/cpp/module/Scancontext/Scancontext.h @@ -0,0 +1,110 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "nanoflann.hpp" +#include "KDTreeVectorOfVectorsAdaptor.h" + +#include "tictoc.h" + +using namespace Eigen; +using namespace nanoflann; + +using std::cout; +using std::endl; +using std::make_pair; + +using std::atan2; +using std::cos; +using std::sin; + +using SCPointType = pcl::PointXYZI; // using xyz only. but a user can exchange the original bin encoding function (i.e., max hegiht) to max intensity (for detail, refer 20 ICRA Intensity Scan Context) +using KeyMat = std::vector >; +using InvKeyTree = KDTreeVectorOfVectorsAdaptor< KeyMat, float >; + + +// namespace SC2 +// { + +void coreImportTest ( void ); + + +// sc param-independent helper functions +float xy2theta( const float & _x, const float & _y ); +MatrixXd circshift( MatrixXd &_mat, int _num_shift ); +std::vector eig2stdvec( MatrixXd _eigmat ); + + +class SCManager +{ +public: + SCManager( ) = default; // reserving data space (of std::vector) could be considered. but the descriptor is lightweight so don't care. + + Eigen::MatrixXd makeScancontext( pcl::PointCloud & _scan_down ); + Eigen::MatrixXd makeRingkeyFromScancontext( Eigen::MatrixXd &_desc ); + Eigen::MatrixXd makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc ); + + int fastAlignUsingVkey ( MatrixXd & _vkey1, MatrixXd & _vkey2 ); + double distDirectSC ( MatrixXd &_sc1, MatrixXd &_sc2 ); // "d" (eq 5) in the original paper (IROS 18) + std::pair distanceBtnScanContext ( MatrixXd &_sc1, MatrixXd &_sc2 ); // "D" (eq 6) in the original paper (IROS 18) + + // User-side API + void makeAndSaveScancontextAndKeys( pcl::PointCloud & _scan_down ); + std::pair detectLoopClosureID( void ); // int: nearest node index, float: relative yaw + +public: + // hyper parameters () + const double LIDAR_HEIGHT = 2.0; // lidar height : add this for simply directly using lidar scan in the lidar local coord (not robot base coord) / if you use robot-coord-transformed lidar scans, just set this as 0. + + const int PC_NUM_RING = 20; // 20 in the original paper (IROS 18) + const int PC_NUM_SECTOR = 60; // 60 in the original paper (IROS 18) + const double PC_MAX_RADIUS = 80.0; // 80 meter max in the original paper (IROS 18) + const double PC_UNIT_SECTORANGLE = 360.0 / double(PC_NUM_SECTOR); + const double PC_UNIT_RINGGAP = PC_MAX_RADIUS / double(PC_NUM_RING); + + // tree + const int NUM_EXCLUDE_RECENT = 50; // simply just keyframe gap, but node position distance-based exclusion is ok. + const int NUM_CANDIDATES_FROM_TREE = 10; // 10 is enough. (refer the IROS 18 paper) + + // loop thres + const double SEARCH_RATIO = 0.1; // for fast comparison, no Brute-force, but search 10 % is okay. // not was in the original conf paper, but improved ver. + const double SC_DIST_THRES = 0.13; // empirically 0.1-0.2 is fine (rare false-alarms) for 20x60 polar context (but for 0.15 <, DCS or ICP fit score check (e.g., in LeGO-LOAM) should be required for robustness) + // const double SC_DIST_THRES = 0.5; // 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold / if not, recommend 0.1-0.15 + + // config + const int TREE_MAKING_PERIOD_ = 50; // i.e., remaking tree frequency, to avoid non-mandatory every remaking, to save time cost / if you want to find a very recent revisits use small value of it (it is enough fast ~ 5-50ms wrt N.). + int tree_making_period_conter = 0; + + // data + std::vector polarcontexts_timestamp_; // optional. + std::vector polarcontexts_; + std::vector polarcontext_invkeys_; + std::vector polarcontext_vkeys_; + + KeyMat polarcontext_invkeys_mat_; + KeyMat polarcontext_invkeys_to_search_; + std::unique_ptr polarcontext_tree_; + +}; // SCManager + +// } // namespace SC2 diff --git a/cpp/module/Scancontext/nanoflann.hpp b/cpp/module/Scancontext/nanoflann.hpp new file mode 100644 index 0000000..a8e4667 --- /dev/null +++ b/cpp/module/Scancontext/nanoflann.hpp @@ -0,0 +1,2040 @@ +/*********************************************************************** + * Software License Agreement (BSD License) + * + * Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved. + * Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved. + * Copyright 2011-2016 Jose Luis Blanco (joseluisblancoc@gmail.com). + * All rights reserved. + * + * THE BSD LICENSE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR + * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *************************************************************************/ + +/** \mainpage nanoflann C++ API documentation + * nanoflann is a C++ header-only library for building KD-Trees, mostly + * optimized for 2D or 3D point clouds. + * + * nanoflann does not require compiling or installing, just an + * #include in your code. + * + * See: + * - C++ API organized by modules + * - Online README + * - Doxygen + * documentation + */ + +#ifndef NANOFLANN_HPP_ +#define NANOFLANN_HPP_ + +#include +#include +#include +#include // for abs() +#include // for fwrite() +#include // for abs() +#include +#include // std::reference_wrapper +#include +#include + +/** Library version: 0xMmP (M=Major,m=minor,P=patch) */ +#define NANOFLANN_VERSION 0x132 + +// Avoid conflicting declaration of min/max macros in windows headers +#if !defined(NOMINMAX) && \ + (defined(_WIN32) || defined(_WIN32_) || defined(WIN32) || defined(_WIN64)) +#define NOMINMAX +#ifdef max +#undef max +#undef min +#endif +#endif + +namespace nanoflann { +/** @addtogroup nanoflann_grp nanoflann C++ library for ANN + * @{ */ + +/** the PI constant (required to avoid MSVC missing symbols) */ +template T pi_const() { + return static_cast(3.14159265358979323846); +} + +/** + * Traits if object is resizable and assignable (typically has a resize | assign + * method) + */ +template struct has_resize : std::false_type {}; + +template +struct has_resize().resize(1), 0)> + : std::true_type {}; + +template struct has_assign : std::false_type {}; + +template +struct has_assign().assign(1, 0), 0)> + : std::true_type {}; + +/** + * Free function to resize a resizable object + */ +template +inline typename std::enable_if::value, void>::type +resize(Container &c, const size_t nElements) { + c.resize(nElements); +} + +/** + * Free function that has no effects on non resizable containers (e.g. + * std::array) It raises an exception if the expected size does not match + */ +template +inline typename std::enable_if::value, void>::type +resize(Container &c, const size_t nElements) { + if (nElements != c.size()) + throw std::logic_error("Try to change the size of a std::array."); +} + +/** + * Free function to assign to a container + */ +template +inline typename std::enable_if::value, void>::type +assign(Container &c, const size_t nElements, const T &value) { + c.assign(nElements, value); +} + +/** + * Free function to assign to a std::array + */ +template +inline typename std::enable_if::value, void>::type +assign(Container &c, const size_t nElements, const T &value) { + for (size_t i = 0; i < nElements; i++) + c[i] = value; +} + +/** @addtogroup result_sets_grp Result set classes + * @{ */ +template +class KNNResultSet { +public: + typedef _DistanceType DistanceType; + typedef _IndexType IndexType; + typedef _CountType CountType; + +private: + IndexType *indices; + DistanceType *dists; + CountType capacity; + CountType count; + +public: + inline KNNResultSet(CountType capacity_) + : indices(0), dists(0), capacity(capacity_), count(0) {} + + inline void init(IndexType *indices_, DistanceType *dists_) { + indices = indices_; + dists = dists_; + count = 0; + if (capacity) + dists[capacity - 1] = (std::numeric_limits::max)(); + } + + inline CountType size() const { return count; } + + inline bool full() const { return count == capacity; } + + /** + * Called during search to add an element matching the criteria. + * @return true if the search should be continued, false if the results are + * sufficient + */ + inline bool addPoint(DistanceType dist, IndexType index) { + CountType i; + for (i = count; i > 0; --i) { +#ifdef NANOFLANN_FIRST_MATCH // If defined and two points have the same + // distance, the one with the lowest-index will be + // returned first. + if ((dists[i - 1] > dist) || + ((dist == dists[i - 1]) && (indices[i - 1] > index))) { +#else + if (dists[i - 1] > dist) { +#endif + if (i < capacity) { + dists[i] = dists[i - 1]; + indices[i] = indices[i - 1]; + } + } else + break; + } + if (i < capacity) { + dists[i] = dist; + indices[i] = index; + } + if (count < capacity) + count++; + + // tell caller that the search shall continue + return true; + } + + inline DistanceType worstDist() const { return dists[capacity - 1]; } +}; + +/** operator "<" for std::sort() */ +struct IndexDist_Sorter { + /** PairType will be typically: std::pair */ + template + inline bool operator()(const PairType &p1, const PairType &p2) const { + return p1.second < p2.second; + } +}; + +/** + * A result-set class used when performing a radius based search. + */ +template +class RadiusResultSet { +public: + typedef _DistanceType DistanceType; + typedef _IndexType IndexType; + +public: + const DistanceType radius; + + std::vector> &m_indices_dists; + + inline RadiusResultSet( + DistanceType radius_, + std::vector> &indices_dists) + : radius(radius_), m_indices_dists(indices_dists) { + init(); + } + + inline void init() { clear(); } + inline void clear() { m_indices_dists.clear(); } + + inline size_t size() const { return m_indices_dists.size(); } + + inline bool full() const { return true; } + + /** + * Called during search to add an element matching the criteria. + * @return true if the search should be continued, false if the results are + * sufficient + */ + inline bool addPoint(DistanceType dist, IndexType index) { + if (dist < radius) + m_indices_dists.push_back(std::make_pair(index, dist)); + return true; + } + + inline DistanceType worstDist() const { return radius; } + + /** + * Find the worst result (furtherest neighbor) without copying or sorting + * Pre-conditions: size() > 0 + */ + std::pair worst_item() const { + if (m_indices_dists.empty()) + throw std::runtime_error("Cannot invoke RadiusResultSet::worst_item() on " + "an empty list of results."); + typedef + typename std::vector>::const_iterator + DistIt; + DistIt it = std::max_element(m_indices_dists.begin(), m_indices_dists.end(), + IndexDist_Sorter()); + return *it; + } +}; + +/** @} */ + +/** @addtogroup loadsave_grp Load/save auxiliary functions + * @{ */ +template +void save_value(FILE *stream, const T &value, size_t count = 1) { + fwrite(&value, sizeof(value), count, stream); +} + +template +void save_value(FILE *stream, const std::vector &value) { + size_t size = value.size(); + fwrite(&size, sizeof(size_t), 1, stream); + fwrite(&value[0], sizeof(T), size, stream); +} + +template +void load_value(FILE *stream, T &value, size_t count = 1) { + size_t read_cnt = fread(&value, sizeof(value), count, stream); + if (read_cnt != count) { + throw std::runtime_error("Cannot read from file"); + } +} + +template void load_value(FILE *stream, std::vector &value) { + size_t size; + size_t read_cnt = fread(&size, sizeof(size_t), 1, stream); + if (read_cnt != 1) { + throw std::runtime_error("Cannot read from file"); + } + value.resize(size); + read_cnt = fread(&value[0], sizeof(T), size, stream); + if (read_cnt != size) { + throw std::runtime_error("Cannot read from file"); + } +} +/** @} */ + +/** @addtogroup metric_grp Metric (distance) classes + * @{ */ + +struct Metric {}; + +/** Manhattan distance functor (generic version, optimized for + * high-dimensionality data sets). Corresponding distance traits: + * nanoflann::metric_L1 \tparam T Type of the elements (e.g. double, float, + * uint8_t) \tparam _DistanceType Type of distance variables (must be signed) + * (e.g. float, double, int64_t) + */ +template +struct L1_Adaptor { + typedef T ElementType; + typedef _DistanceType DistanceType; + + const DataSource &data_source; + + L1_Adaptor(const DataSource &_data_source) : data_source(_data_source) {} + + inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size, + DistanceType worst_dist = -1) const { + DistanceType result = DistanceType(); + const T *last = a + size; + const T *lastgroup = last - 3; + size_t d = 0; + + /* Process 4 items with each loop for efficiency. */ + while (a < lastgroup) { + const DistanceType diff0 = + std::abs(a[0] - data_source.kdtree_get_pt(b_idx, d++)); + const DistanceType diff1 = + std::abs(a[1] - data_source.kdtree_get_pt(b_idx, d++)); + const DistanceType diff2 = + std::abs(a[2] - data_source.kdtree_get_pt(b_idx, d++)); + const DistanceType diff3 = + std::abs(a[3] - data_source.kdtree_get_pt(b_idx, d++)); + result += diff0 + diff1 + diff2 + diff3; + a += 4; + if ((worst_dist > 0) && (result > worst_dist)) { + return result; + } + } + /* Process last 0-3 components. Not needed for standard vector lengths. */ + while (a < last) { + result += std::abs(*a++ - data_source.kdtree_get_pt(b_idx, d++)); + } + return result; + } + + template + inline DistanceType accum_dist(const U a, const V b, const size_t) const { + return std::abs(a - b); + } +}; + +/** Squared Euclidean distance functor (generic version, optimized for + * high-dimensionality data sets). Corresponding distance traits: + * nanoflann::metric_L2 \tparam T Type of the elements (e.g. double, float, + * uint8_t) \tparam _DistanceType Type of distance variables (must be signed) + * (e.g. float, double, int64_t) + */ +template +struct L2_Adaptor { + typedef T ElementType; + typedef _DistanceType DistanceType; + + const DataSource &data_source; + + L2_Adaptor(const DataSource &_data_source) : data_source(_data_source) {} + + inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size, + DistanceType worst_dist = -1) const { + DistanceType result = DistanceType(); + const T *last = a + size; + const T *lastgroup = last - 3; + size_t d = 0; + + /* Process 4 items with each loop for efficiency. */ + while (a < lastgroup) { + const DistanceType diff0 = a[0] - data_source.kdtree_get_pt(b_idx, d++); + const DistanceType diff1 = a[1] - data_source.kdtree_get_pt(b_idx, d++); + const DistanceType diff2 = a[2] - data_source.kdtree_get_pt(b_idx, d++); + const DistanceType diff3 = a[3] - data_source.kdtree_get_pt(b_idx, d++); + result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3; + a += 4; + if ((worst_dist > 0) && (result > worst_dist)) { + return result; + } + } + /* Process last 0-3 components. Not needed for standard vector lengths. */ + while (a < last) { + const DistanceType diff0 = *a++ - data_source.kdtree_get_pt(b_idx, d++); + result += diff0 * diff0; + } + return result; + } + + template + inline DistanceType accum_dist(const U a, const V b, const size_t) const { + return (a - b) * (a - b); + } +}; + +/** Squared Euclidean (L2) distance functor (suitable for low-dimensionality + * datasets, like 2D or 3D point clouds) Corresponding distance traits: + * nanoflann::metric_L2_Simple \tparam T Type of the elements (e.g. double, + * float, uint8_t) \tparam _DistanceType Type of distance variables (must be + * signed) (e.g. float, double, int64_t) + */ +template +struct L2_Simple_Adaptor { + typedef T ElementType; + typedef _DistanceType DistanceType; + + const DataSource &data_source; + + L2_Simple_Adaptor(const DataSource &_data_source) + : data_source(_data_source) {} + + inline DistanceType evalMetric(const T *a, const size_t b_idx, + size_t size) const { + DistanceType result = DistanceType(); + for (size_t i = 0; i < size; ++i) { + const DistanceType diff = a[i] - data_source.kdtree_get_pt(b_idx, i); + result += diff * diff; + } + return result; + } + + template + inline DistanceType accum_dist(const U a, const V b, const size_t) const { + return (a - b) * (a - b); + } +}; + +/** SO2 distance functor + * Corresponding distance traits: nanoflann::metric_SO2 + * \tparam T Type of the elements (e.g. double, float) + * \tparam _DistanceType Type of distance variables (must be signed) (e.g. + * float, double) orientation is constrained to be in [-pi, pi] + */ +template +struct SO2_Adaptor { + typedef T ElementType; + typedef _DistanceType DistanceType; + + const DataSource &data_source; + + SO2_Adaptor(const DataSource &_data_source) : data_source(_data_source) {} + + inline DistanceType evalMetric(const T *a, const size_t b_idx, + size_t size) const { + return accum_dist(a[size - 1], data_source.kdtree_get_pt(b_idx, size - 1), + size - 1); + } + + /** Note: this assumes that input angles are already in the range [-pi,pi] */ + template + inline DistanceType accum_dist(const U a, const V b, const size_t) const { + DistanceType result = DistanceType(); + DistanceType PI = pi_const(); + result = b - a; + if (result > PI) + result -= 2 * PI; + else if (result < -PI) + result += 2 * PI; + return result; + } +}; + +/** SO3 distance functor (Uses L2_Simple) + * Corresponding distance traits: nanoflann::metric_SO3 + * \tparam T Type of the elements (e.g. double, float) + * \tparam _DistanceType Type of distance variables (must be signed) (e.g. + * float, double) + */ +template +struct SO3_Adaptor { + typedef T ElementType; + typedef _DistanceType DistanceType; + + L2_Simple_Adaptor distance_L2_Simple; + + SO3_Adaptor(const DataSource &_data_source) + : distance_L2_Simple(_data_source) {} + + inline DistanceType evalMetric(const T *a, const size_t b_idx, + size_t size) const { + return distance_L2_Simple.evalMetric(a, b_idx, size); + } + + template + inline DistanceType accum_dist(const U a, const V b, const size_t idx) const { + return distance_L2_Simple.accum_dist(a, b, idx); + } +}; + +/** Metaprogramming helper traits class for the L1 (Manhattan) metric */ +struct metric_L1 : public Metric { + template struct traits { + typedef L1_Adaptor distance_t; + }; +}; +/** Metaprogramming helper traits class for the L2 (Euclidean) metric */ +struct metric_L2 : public Metric { + template struct traits { + typedef L2_Adaptor distance_t; + }; +}; +/** Metaprogramming helper traits class for the L2_simple (Euclidean) metric */ +struct metric_L2_Simple : public Metric { + template struct traits { + typedef L2_Simple_Adaptor distance_t; + }; +}; +/** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */ +struct metric_SO2 : public Metric { + template struct traits { + typedef SO2_Adaptor distance_t; + }; +}; +/** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */ +struct metric_SO3 : public Metric { + template struct traits { + typedef SO3_Adaptor distance_t; + }; +}; + +/** @} */ + +/** @addtogroup param_grp Parameter structs + * @{ */ + +/** Parameters (see README.md) */ +struct KDTreeSingleIndexAdaptorParams { + KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10) + : leaf_max_size(_leaf_max_size) {} + + size_t leaf_max_size; +}; + +/** Search options for KDTreeSingleIndexAdaptor::findNeighbors() */ +struct SearchParams { + /** Note: The first argument (checks_IGNORED_) is ignored, but kept for + * compatibility with the FLANN interface */ + SearchParams(int checks_IGNORED_ = 32, float eps_ = 0, bool sorted_ = true) + : checks(checks_IGNORED_), eps(eps_), sorted(sorted_) {} + + int checks; //!< Ignored parameter (Kept for compatibility with the FLANN + //!< interface). + float eps; //!< search for eps-approximate neighbours (default: 0) + bool sorted; //!< only for radius search, require neighbours sorted by + //!< distance (default: true) +}; +/** @} */ + +/** @addtogroup memalloc_grp Memory allocation + * @{ */ + +/** + * Allocates (using C's malloc) a generic type T. + * + * Params: + * count = number of instances to allocate. + * Returns: pointer (of type T*) to memory buffer + */ +template inline T *allocate(size_t count = 1) { + T *mem = static_cast(::malloc(sizeof(T) * count)); + return mem; +} + +/** + * Pooled storage allocator + * + * The following routines allow for the efficient allocation of storage in + * small chunks from a specified pool. Rather than allowing each structure + * to be freed individually, an entire pool of storage is freed at once. + * This method has two advantages over just using malloc() and free(). First, + * it is far more efficient for allocating small objects, as there is + * no overhead for remembering all the information needed to free each + * object or consolidating fragmented memory. Second, the decision about + * how long to keep an object is made at the time of allocation, and there + * is no need to track down all the objects to free them. + * + */ + +const size_t WORDSIZE = 16; +const size_t BLOCKSIZE = 8192; + +class PooledAllocator { + /* We maintain memory alignment to word boundaries by requiring that all + allocations be in multiples of the machine wordsize. */ + /* Size of machine word in bytes. Must be power of 2. */ + /* Minimum number of bytes requested at a time from the system. Must be + * multiple of WORDSIZE. */ + + size_t remaining; /* Number of bytes left in current block of storage. */ + void *base; /* Pointer to base of current block of storage. */ + void *loc; /* Current location in block to next allocate memory. */ + + void internal_init() { + remaining = 0; + base = NULL; + usedMemory = 0; + wastedMemory = 0; + } + +public: + size_t usedMemory; + size_t wastedMemory; + + /** + Default constructor. Initializes a new pool. + */ + PooledAllocator() { internal_init(); } + + /** + * Destructor. Frees all the memory allocated in this pool. + */ + ~PooledAllocator() { free_all(); } + + /** Frees all allocated memory chunks */ + void free_all() { + while (base != NULL) { + void *prev = + *(static_cast(base)); /* Get pointer to prev block. */ + ::free(base); + base = prev; + } + internal_init(); + } + + /** + * Returns a pointer to a piece of new memory of the given size in bytes + * allocated from the pool. + */ + void *malloc(const size_t req_size) { + /* Round size up to a multiple of wordsize. The following expression + only works for WORDSIZE that is a power of 2, by masking last bits of + incremented size to zero. + */ + const size_t size = (req_size + (WORDSIZE - 1)) & ~(WORDSIZE - 1); + + /* Check whether a new block must be allocated. Note that the first word + of a block is reserved for a pointer to the previous block. + */ + if (size > remaining) { + + wastedMemory += remaining; + + /* Allocate new storage. */ + const size_t blocksize = + (size + sizeof(void *) + (WORDSIZE - 1) > BLOCKSIZE) + ? size + sizeof(void *) + (WORDSIZE - 1) + : BLOCKSIZE; + + // use the standard C malloc to allocate memory + void *m = ::malloc(blocksize); + if (!m) { + fprintf(stderr, "Failed to allocate memory.\n"); + return NULL; + } + + /* Fill first word of new block with pointer to previous block. */ + static_cast(m)[0] = base; + base = m; + + size_t shift = 0; + // int size_t = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) & + // (WORDSIZE-1))) & (WORDSIZE-1); + + remaining = blocksize - sizeof(void *) - shift; + loc = (static_cast(m) + sizeof(void *) + shift); + } + void *rloc = loc; + loc = static_cast(loc) + size; + remaining -= size; + + usedMemory += size; + + return rloc; + } + + /** + * Allocates (using this pool) a generic type T. + * + * Params: + * count = number of instances to allocate. + * Returns: pointer (of type T*) to memory buffer + */ + template T *allocate(const size_t count = 1) { + T *mem = static_cast(this->malloc(sizeof(T) * count)); + return mem; + } +}; +/** @} */ + +/** @addtogroup nanoflann_metaprog_grp Auxiliary metaprogramming stuff + * @{ */ + +/** Used to declare fixed-size arrays when DIM>0, dynamically-allocated vectors + * when DIM=-1. Fixed size version for a generic DIM: + */ +template struct array_or_vector_selector { + typedef std::array container_t; +}; +/** Dynamic size version */ +template struct array_or_vector_selector<-1, T> { + typedef std::vector container_t; +}; + +/** @} */ + +/** kd-tree base-class + * + * Contains the member functions common to the classes KDTreeSingleIndexAdaptor + * and KDTreeSingleIndexDynamicAdaptor_. + * + * \tparam Derived The name of the class which inherits this class. + * \tparam DatasetAdaptor The user-provided adaptor (see comments above). + * \tparam Distance The distance metric to use, these are all classes derived + * from nanoflann::Metric \tparam DIM Dimensionality of data points (e.g. 3 for + * 3D points) \tparam IndexType Will be typically size_t or int + */ + +template +class KDTreeBaseClass { + +public: + /** Frees the previously-built index. Automatically called within + * buildIndex(). */ + void freeIndex(Derived &obj) { + obj.pool.free_all(); + obj.root_node = NULL; + obj.m_size_at_index_build = 0; + } + + typedef typename Distance::ElementType ElementType; + typedef typename Distance::DistanceType DistanceType; + + /*--------------------- Internal Data Structures --------------------------*/ + struct Node { + /** Union used because a node can be either a LEAF node or a non-leaf node, + * so both data fields are never used simultaneously */ + union { + struct leaf { + IndexType left, right; //!< Indices of points in leaf node + } lr; + struct nonleaf { + int divfeat; //!< Dimension used for subdivision. + DistanceType divlow, divhigh; //!< The values used for subdivision. + } sub; + } node_type; + Node *child1, *child2; //!< Child nodes (both=NULL mean its a leaf node) + }; + + typedef Node *NodePtr; + + struct Interval { + ElementType low, high; + }; + + /** + * Array of indices to vectors in the dataset. + */ + std::vector vind; + + NodePtr root_node; + + size_t m_leaf_max_size; + + size_t m_size; //!< Number of current points in the dataset + size_t m_size_at_index_build; //!< Number of points in the dataset when the + //!< index was built + int dim; //!< Dimensionality of each data point + + /** Define "BoundingBox" as a fixed-size or variable-size container depending + * on "DIM" */ + typedef + typename array_or_vector_selector::container_t BoundingBox; + + /** Define "distance_vector_t" as a fixed-size or variable-size container + * depending on "DIM" */ + typedef typename array_or_vector_selector::container_t + distance_vector_t; + + /** The KD-tree used to find neighbours */ + + BoundingBox root_bbox; + + /** + * Pooled memory allocator. + * + * Using a pooled memory allocator is more efficient + * than allocating memory directly when there is a large + * number small of memory allocations. + */ + PooledAllocator pool; + + /** Returns number of points in dataset */ + size_t size(const Derived &obj) const { return obj.m_size; } + + /** Returns the length of each point in the dataset */ + size_t veclen(const Derived &obj) { + return static_cast(DIM > 0 ? DIM : obj.dim); + } + + /// Helper accessor to the dataset points: + inline ElementType dataset_get(const Derived &obj, size_t idx, + int component) const { + return obj.dataset.kdtree_get_pt(idx, component); + } + + /** + * Computes the inde memory usage + * Returns: memory used by the index + */ + size_t usedMemory(Derived &obj) { + return obj.pool.usedMemory + obj.pool.wastedMemory + + obj.dataset.kdtree_get_point_count() * + sizeof(IndexType); // pool memory and vind array memory + } + + void computeMinMax(const Derived &obj, IndexType *ind, IndexType count, + int element, ElementType &min_elem, + ElementType &max_elem) { + min_elem = dataset_get(obj, ind[0], element); + max_elem = dataset_get(obj, ind[0], element); + for (IndexType i = 1; i < count; ++i) { + ElementType val = dataset_get(obj, ind[i], element); + if (val < min_elem) + min_elem = val; + if (val > max_elem) + max_elem = val; + } + } + + /** + * Create a tree node that subdivides the list of vecs from vind[first] + * to vind[last]. The routine is called recursively on each sublist. + * + * @param left index of the first vector + * @param right index of the last vector + */ + NodePtr divideTree(Derived &obj, const IndexType left, const IndexType right, + BoundingBox &bbox) { + NodePtr node = obj.pool.template allocate(); // allocate memory + + /* If too few exemplars remain, then make this a leaf node. */ + if ((right - left) <= static_cast(obj.m_leaf_max_size)) { + node->child1 = node->child2 = NULL; /* Mark as leaf node. */ + node->node_type.lr.left = left; + node->node_type.lr.right = right; + + // compute bounding-box of leaf points + for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { + bbox[i].low = dataset_get(obj, obj.vind[left], i); + bbox[i].high = dataset_get(obj, obj.vind[left], i); + } + for (IndexType k = left + 1; k < right; ++k) { + for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { + if (bbox[i].low > dataset_get(obj, obj.vind[k], i)) + bbox[i].low = dataset_get(obj, obj.vind[k], i); + if (bbox[i].high < dataset_get(obj, obj.vind[k], i)) + bbox[i].high = dataset_get(obj, obj.vind[k], i); + } + } + } else { + IndexType idx; + int cutfeat; + DistanceType cutval; + middleSplit_(obj, &obj.vind[0] + left, right - left, idx, cutfeat, cutval, + bbox); + + node->node_type.sub.divfeat = cutfeat; + + BoundingBox left_bbox(bbox); + left_bbox[cutfeat].high = cutval; + node->child1 = divideTree(obj, left, left + idx, left_bbox); + + BoundingBox right_bbox(bbox); + right_bbox[cutfeat].low = cutval; + node->child2 = divideTree(obj, left + idx, right, right_bbox); + + node->node_type.sub.divlow = left_bbox[cutfeat].high; + node->node_type.sub.divhigh = right_bbox[cutfeat].low; + + for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { + bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low); + bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high); + } + } + + return node; + } + + void middleSplit_(Derived &obj, IndexType *ind, IndexType count, + IndexType &index, int &cutfeat, DistanceType &cutval, + const BoundingBox &bbox) { + const DistanceType EPS = static_cast(0.00001); + ElementType max_span = bbox[0].high - bbox[0].low; + for (int i = 1; i < (DIM > 0 ? DIM : obj.dim); ++i) { + ElementType span = bbox[i].high - bbox[i].low; + if (span > max_span) { + max_span = span; + } + } + ElementType max_spread = -1; + cutfeat = 0; + for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { + ElementType span = bbox[i].high - bbox[i].low; + if (span > (1 - EPS) * max_span) { + ElementType min_elem, max_elem; + computeMinMax(obj, ind, count, i, min_elem, max_elem); + ElementType spread = max_elem - min_elem; + ; + if (spread > max_spread) { + cutfeat = i; + max_spread = spread; + } + } + } + // split in the middle + DistanceType split_val = (bbox[cutfeat].low + bbox[cutfeat].high) / 2; + ElementType min_elem, max_elem; + computeMinMax(obj, ind, count, cutfeat, min_elem, max_elem); + + if (split_val < min_elem) + cutval = min_elem; + else if (split_val > max_elem) + cutval = max_elem; + else + cutval = split_val; + + IndexType lim1, lim2; + planeSplit(obj, ind, count, cutfeat, cutval, lim1, lim2); + + if (lim1 > count / 2) + index = lim1; + else if (lim2 < count / 2) + index = lim2; + else + index = count / 2; + } + + /** + * Subdivide the list of points by a plane perpendicular on axe corresponding + * to the 'cutfeat' dimension at 'cutval' position. + * + * On return: + * dataset[ind[0..lim1-1]][cutfeat]cutval + */ + void planeSplit(Derived &obj, IndexType *ind, const IndexType count, + int cutfeat, DistanceType &cutval, IndexType &lim1, + IndexType &lim2) { + /* Move vector indices for left subtree to front of list. */ + IndexType left = 0; + IndexType right = count - 1; + for (;;) { + while (left <= right && dataset_get(obj, ind[left], cutfeat) < cutval) + ++left; + while (right && left <= right && + dataset_get(obj, ind[right], cutfeat) >= cutval) + --right; + if (left > right || !right) + break; // "!right" was added to support unsigned Index types + std::swap(ind[left], ind[right]); + ++left; + --right; + } + /* If either list is empty, it means that all remaining features + * are identical. Split in the middle to maintain a balanced tree. + */ + lim1 = left; + right = count - 1; + for (;;) { + while (left <= right && dataset_get(obj, ind[left], cutfeat) <= cutval) + ++left; + while (right && left <= right && + dataset_get(obj, ind[right], cutfeat) > cutval) + --right; + if (left > right || !right) + break; // "!right" was added to support unsigned Index types + std::swap(ind[left], ind[right]); + ++left; + --right; + } + lim2 = left; + } + + DistanceType computeInitialDistances(const Derived &obj, + const ElementType *vec, + distance_vector_t &dists) const { + assert(vec); + DistanceType distsq = DistanceType(); + + for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { + if (vec[i] < obj.root_bbox[i].low) { + dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].low, i); + distsq += dists[i]; + } + if (vec[i] > obj.root_bbox[i].high) { + dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].high, i); + distsq += dists[i]; + } + } + return distsq; + } + + void save_tree(Derived &obj, FILE *stream, NodePtr tree) { + save_value(stream, *tree); + if (tree->child1 != NULL) { + save_tree(obj, stream, tree->child1); + } + if (tree->child2 != NULL) { + save_tree(obj, stream, tree->child2); + } + } + + void load_tree(Derived &obj, FILE *stream, NodePtr &tree) { + tree = obj.pool.template allocate(); + load_value(stream, *tree); + if (tree->child1 != NULL) { + load_tree(obj, stream, tree->child1); + } + if (tree->child2 != NULL) { + load_tree(obj, stream, tree->child2); + } + } + + /** Stores the index in a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when + * loading the index object it must be constructed associated to the same + * source of data points used while building it. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void saveIndex_(Derived &obj, FILE *stream) { + save_value(stream, obj.m_size); + save_value(stream, obj.dim); + save_value(stream, obj.root_bbox); + save_value(stream, obj.m_leaf_max_size); + save_value(stream, obj.vind); + save_tree(obj, stream, obj.root_node); + } + + /** Loads a previous index from a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the + * index object must be constructed associated to the same source of data + * points used while building the index. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void loadIndex_(Derived &obj, FILE *stream) { + load_value(stream, obj.m_size); + load_value(stream, obj.dim); + load_value(stream, obj.root_bbox); + load_value(stream, obj.m_leaf_max_size); + load_value(stream, obj.vind); + load_tree(obj, stream, obj.root_node); + } +}; + +/** @addtogroup kdtrees_grp KD-tree classes and adaptors + * @{ */ + +/** kd-tree static index + * + * Contains the k-d trees and other information for indexing a set of points + * for nearest-neighbor matching. + * + * The class "DatasetAdaptor" must provide the following interface (can be + * non-virtual, inlined methods): + * + * \code + * // Must return the number of data poins + * inline size_t kdtree_get_point_count() const { ... } + * + * + * // Must return the dim'th component of the idx'th point in the class: + * inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... } + * + * // Optional bounding-box computation: return false to default to a standard + * bbox computation loop. + * // Return true if the BBOX was already computed by the class and returned + * in "bb" so it can be avoided to redo it again. + * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 + * for point clouds) template bool kdtree_get_bbox(BBOX &bb) const + * { + * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits + * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits + * ... + * return true; + * } + * + * \endcode + * + * \tparam DatasetAdaptor The user-provided adaptor (see comments above). + * \tparam Distance The distance metric to use: nanoflann::metric_L1, + * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM + * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will + * be typically size_t or int + */ +template +class KDTreeSingleIndexAdaptor + : public KDTreeBaseClass< + KDTreeSingleIndexAdaptor, + Distance, DatasetAdaptor, DIM, IndexType> { +public: + /** Deleted copy constructor*/ + KDTreeSingleIndexAdaptor( + const KDTreeSingleIndexAdaptor + &) = delete; + + /** + * The dataset used by this index + */ + const DatasetAdaptor &dataset; //!< The source of our data + + const KDTreeSingleIndexAdaptorParams index_params; + + Distance distance; + + typedef typename nanoflann::KDTreeBaseClass< + nanoflann::KDTreeSingleIndexAdaptor, + Distance, DatasetAdaptor, DIM, IndexType> + BaseClassRef; + + typedef typename BaseClassRef::ElementType ElementType; + typedef typename BaseClassRef::DistanceType DistanceType; + + typedef typename BaseClassRef::Node Node; + typedef Node *NodePtr; + + typedef typename BaseClassRef::Interval Interval; + /** Define "BoundingBox" as a fixed-size or variable-size container depending + * on "DIM" */ + typedef typename BaseClassRef::BoundingBox BoundingBox; + + /** Define "distance_vector_t" as a fixed-size or variable-size container + * depending on "DIM" */ + typedef typename BaseClassRef::distance_vector_t distance_vector_t; + + /** + * KDTree constructor + * + * Refer to docs in README.md or online in + * https://github.com/jlblancoc/nanoflann + * + * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 + * for 3D points) is determined by means of: + * - The \a DIM template parameter if >0 (highest priority) + * - Otherwise, the \a dimensionality parameter of this constructor. + * + * @param inputData Dataset with the input features + * @param params Basically, the maximum leaf node size + */ + KDTreeSingleIndexAdaptor(const int dimensionality, + const DatasetAdaptor &inputData, + const KDTreeSingleIndexAdaptorParams ¶ms = + KDTreeSingleIndexAdaptorParams()) + : dataset(inputData), index_params(params), distance(inputData) { + BaseClassRef::root_node = NULL; + BaseClassRef::m_size = dataset.kdtree_get_point_count(); + BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; + BaseClassRef::dim = dimensionality; + if (DIM > 0) + BaseClassRef::dim = DIM; + BaseClassRef::m_leaf_max_size = params.leaf_max_size; + + // Create a permutable array of indices to the input vectors. + init_vind(); + } + + /** + * Builds the index + */ + void buildIndex() { + BaseClassRef::m_size = dataset.kdtree_get_point_count(); + BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; + init_vind(); + this->freeIndex(*this); + BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; + if (BaseClassRef::m_size == 0) + return; + computeBoundingBox(BaseClassRef::root_bbox); + BaseClassRef::root_node = + this->divideTree(*this, 0, BaseClassRef::m_size, + BaseClassRef::root_bbox); // construct the tree + } + + /** \name Query methods + * @{ */ + + /** + * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored + * inside the result object. + * + * Params: + * result = the result object in which the indices of the + * nearest-neighbors are stored vec = the vector for which to search the + * nearest neighbors + * + * \tparam RESULTSET Should be any ResultSet + * \return True if the requested neighbors could be found. + * \sa knnSearch, radiusSearch + */ + template + bool findNeighbors(RESULTSET &result, const ElementType *vec, + const SearchParams &searchParams) const { + assert(vec); + if (this->size(*this) == 0) + return false; + if (!BaseClassRef::root_node) + throw std::runtime_error( + "[nanoflann] findNeighbors() called before building the index."); + float epsError = 1 + searchParams.eps; + + distance_vector_t + dists; // fixed or variable-sized container (depending on DIM) + auto zero = static_cast(0); + assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim), + zero); // Fill it with zeros. + DistanceType distsq = this->computeInitialDistances(*this, vec, dists); + searchLevel(result, vec, BaseClassRef::root_node, distsq, dists, + epsError); // "count_leaf" parameter removed since was neither + // used nor returned to the user. + return result.full(); + } + + /** + * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. + * Their indices are stored inside the result object. \sa radiusSearch, + * findNeighbors \note nChecks_IGNORED is ignored but kept for compatibility + * with the original FLANN interface. \return Number `N` of valid points in + * the result set. Only the first `N` entries in `out_indices` and + * `out_distances_sq` will be valid. Return may be less than `num_closest` + * only if the number of elements in the tree is less than `num_closest`. + */ + size_t knnSearch(const ElementType *query_point, const size_t num_closest, + IndexType *out_indices, DistanceType *out_distances_sq, + const int /* nChecks_IGNORED */ = 10) const { + nanoflann::KNNResultSet resultSet(num_closest); + resultSet.init(out_indices, out_distances_sq); + this->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); + return resultSet.size(); + } + + /** + * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius. + * The output is given as a vector of pairs, of which the first element is a + * point index and the second the corresponding distance. Previous contents of + * \a IndicesDists are cleared. + * + * If searchParams.sorted==true, the output list is sorted by ascending + * distances. + * + * For a better performance, it is advisable to do a .reserve() on the vector + * if you have any wild guess about the number of expected matches. + * + * \sa knnSearch, findNeighbors, radiusSearchCustomCallback + * \return The number of points within the given radius (i.e. indices.size() + * or dists.size() ) + */ + size_t + radiusSearch(const ElementType *query_point, const DistanceType &radius, + std::vector> &IndicesDists, + const SearchParams &searchParams) const { + RadiusResultSet resultSet(radius, IndicesDists); + const size_t nFound = + radiusSearchCustomCallback(query_point, resultSet, searchParams); + if (searchParams.sorted) + std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter()); + return nFound; + } + + /** + * Just like radiusSearch() but with a custom callback class for each point + * found in the radius of the query. See the source of RadiusResultSet<> as a + * start point for your own classes. \sa radiusSearch + */ + template + size_t radiusSearchCustomCallback( + const ElementType *query_point, SEARCH_CALLBACK &resultSet, + const SearchParams &searchParams = SearchParams()) const { + this->findNeighbors(resultSet, query_point, searchParams); + return resultSet.size(); + } + + /** @} */ + +public: + /** Make sure the auxiliary list \a vind has the same size than the current + * dataset, and re-generate if size has changed. */ + void init_vind() { + // Create a permutable array of indices to the input vectors. + BaseClassRef::m_size = dataset.kdtree_get_point_count(); + if (BaseClassRef::vind.size() != BaseClassRef::m_size) + BaseClassRef::vind.resize(BaseClassRef::m_size); + for (size_t i = 0; i < BaseClassRef::m_size; i++) + BaseClassRef::vind[i] = i; + } + + void computeBoundingBox(BoundingBox &bbox) { + resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim)); + if (dataset.kdtree_get_bbox(bbox)) { + // Done! It was implemented in derived class + } else { + const size_t N = dataset.kdtree_get_point_count(); + if (!N) + throw std::runtime_error("[nanoflann] computeBoundingBox() called but " + "no data points found."); + for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { + bbox[i].low = bbox[i].high = this->dataset_get(*this, 0, i); + } + for (size_t k = 1; k < N; ++k) { + for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { + if (this->dataset_get(*this, k, i) < bbox[i].low) + bbox[i].low = this->dataset_get(*this, k, i); + if (this->dataset_get(*this, k, i) > bbox[i].high) + bbox[i].high = this->dataset_get(*this, k, i); + } + } + } + } + + /** + * Performs an exact search in the tree starting from a node. + * \tparam RESULTSET Should be any ResultSet + * \return true if the search should be continued, false if the results are + * sufficient + */ + template + bool searchLevel(RESULTSET &result_set, const ElementType *vec, + const NodePtr node, DistanceType mindistsq, + distance_vector_t &dists, const float epsError) const { + /* If this is a leaf node, then do check and return. */ + if ((node->child1 == NULL) && (node->child2 == NULL)) { + // count_leaf += (node->lr.right-node->lr.left); // Removed since was + // neither used nor returned to the user. + DistanceType worst_dist = result_set.worstDist(); + for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right; + ++i) { + const IndexType index = BaseClassRef::vind[i]; // reorder... : i; + DistanceType dist = distance.evalMetric( + vec, index, (DIM > 0 ? DIM : BaseClassRef::dim)); + if (dist < worst_dist) { + if (!result_set.addPoint(dist, BaseClassRef::vind[i])) { + // the resultset doesn't want to receive any more points, we're done + // searching! + return false; + } + } + } + return true; + } + + /* Which child branch should be taken first? */ + int idx = node->node_type.sub.divfeat; + ElementType val = vec[idx]; + DistanceType diff1 = val - node->node_type.sub.divlow; + DistanceType diff2 = val - node->node_type.sub.divhigh; + + NodePtr bestChild; + NodePtr otherChild; + DistanceType cut_dist; + if ((diff1 + diff2) < 0) { + bestChild = node->child1; + otherChild = node->child2; + cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx); + } else { + bestChild = node->child2; + otherChild = node->child1; + cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx); + } + + /* Call recursively to search next level down. */ + if (!searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError)) { + // the resultset doesn't want to receive any more points, we're done + // searching! + return false; + } + + DistanceType dst = dists[idx]; + mindistsq = mindistsq + cut_dist - dst; + dists[idx] = cut_dist; + if (mindistsq * epsError <= result_set.worstDist()) { + if (!searchLevel(result_set, vec, otherChild, mindistsq, dists, + epsError)) { + // the resultset doesn't want to receive any more points, we're done + // searching! + return false; + } + } + dists[idx] = dst; + return true; + } + +public: + /** Stores the index in a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when + * loading the index object it must be constructed associated to the same + * source of data points used while building it. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); } + + /** Loads a previous index from a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the + * index object must be constructed associated to the same source of data + * points used while building the index. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); } + +}; // class KDTree + +/** kd-tree dynamic index + * + * Contains the k-d trees and other information for indexing a set of points + * for nearest-neighbor matching. + * + * The class "DatasetAdaptor" must provide the following interface (can be + * non-virtual, inlined methods): + * + * \code + * // Must return the number of data poins + * inline size_t kdtree_get_point_count() const { ... } + * + * // Must return the dim'th component of the idx'th point in the class: + * inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... } + * + * // Optional bounding-box computation: return false to default to a standard + * bbox computation loop. + * // Return true if the BBOX was already computed by the class and returned + * in "bb" so it can be avoided to redo it again. + * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 + * for point clouds) template bool kdtree_get_bbox(BBOX &bb) const + * { + * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits + * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits + * ... + * return true; + * } + * + * \endcode + * + * \tparam DatasetAdaptor The user-provided adaptor (see comments above). + * \tparam Distance The distance metric to use: nanoflann::metric_L1, + * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM + * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will + * be typically size_t or int + */ +template +class KDTreeSingleIndexDynamicAdaptor_ + : public KDTreeBaseClass, + Distance, DatasetAdaptor, DIM, IndexType> { +public: + /** + * The dataset used by this index + */ + const DatasetAdaptor &dataset; //!< The source of our data + + KDTreeSingleIndexAdaptorParams index_params; + + std::vector &treeIndex; + + Distance distance; + + typedef typename nanoflann::KDTreeBaseClass< + nanoflann::KDTreeSingleIndexDynamicAdaptor_, + Distance, DatasetAdaptor, DIM, IndexType> + BaseClassRef; + + typedef typename BaseClassRef::ElementType ElementType; + typedef typename BaseClassRef::DistanceType DistanceType; + + typedef typename BaseClassRef::Node Node; + typedef Node *NodePtr; + + typedef typename BaseClassRef::Interval Interval; + /** Define "BoundingBox" as a fixed-size or variable-size container depending + * on "DIM" */ + typedef typename BaseClassRef::BoundingBox BoundingBox; + + /** Define "distance_vector_t" as a fixed-size or variable-size container + * depending on "DIM" */ + typedef typename BaseClassRef::distance_vector_t distance_vector_t; + + /** + * KDTree constructor + * + * Refer to docs in README.md or online in + * https://github.com/jlblancoc/nanoflann + * + * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 + * for 3D points) is determined by means of: + * - The \a DIM template parameter if >0 (highest priority) + * - Otherwise, the \a dimensionality parameter of this constructor. + * + * @param inputData Dataset with the input features + * @param params Basically, the maximum leaf node size + */ + KDTreeSingleIndexDynamicAdaptor_( + const int dimensionality, const DatasetAdaptor &inputData, + std::vector &treeIndex_, + const KDTreeSingleIndexAdaptorParams ¶ms = + KDTreeSingleIndexAdaptorParams()) + : dataset(inputData), index_params(params), treeIndex(treeIndex_), + distance(inputData) { + BaseClassRef::root_node = NULL; + BaseClassRef::m_size = 0; + BaseClassRef::m_size_at_index_build = 0; + BaseClassRef::dim = dimensionality; + if (DIM > 0) + BaseClassRef::dim = DIM; + BaseClassRef::m_leaf_max_size = params.leaf_max_size; + } + + /** Assignment operator definiton */ + KDTreeSingleIndexDynamicAdaptor_ + operator=(const KDTreeSingleIndexDynamicAdaptor_ &rhs) { + KDTreeSingleIndexDynamicAdaptor_ tmp(rhs); + std::swap(BaseClassRef::vind, tmp.BaseClassRef::vind); + std::swap(BaseClassRef::m_leaf_max_size, tmp.BaseClassRef::m_leaf_max_size); + std::swap(index_params, tmp.index_params); + std::swap(treeIndex, tmp.treeIndex); + std::swap(BaseClassRef::m_size, tmp.BaseClassRef::m_size); + std::swap(BaseClassRef::m_size_at_index_build, + tmp.BaseClassRef::m_size_at_index_build); + std::swap(BaseClassRef::root_node, tmp.BaseClassRef::root_node); + std::swap(BaseClassRef::root_bbox, tmp.BaseClassRef::root_bbox); + std::swap(BaseClassRef::pool, tmp.BaseClassRef::pool); + return *this; + } + + /** + * Builds the index + */ + void buildIndex() { + BaseClassRef::m_size = BaseClassRef::vind.size(); + this->freeIndex(*this); + BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; + if (BaseClassRef::m_size == 0) + return; + computeBoundingBox(BaseClassRef::root_bbox); + BaseClassRef::root_node = + this->divideTree(*this, 0, BaseClassRef::m_size, + BaseClassRef::root_bbox); // construct the tree + } + + /** \name Query methods + * @{ */ + + /** + * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored + * inside the result object. + * + * Params: + * result = the result object in which the indices of the + * nearest-neighbors are stored vec = the vector for which to search the + * nearest neighbors + * + * \tparam RESULTSET Should be any ResultSet + * \return True if the requested neighbors could be found. + * \sa knnSearch, radiusSearch + */ + template + bool findNeighbors(RESULTSET &result, const ElementType *vec, + const SearchParams &searchParams) const { + assert(vec); + if (this->size(*this) == 0) + return false; + if (!BaseClassRef::root_node) + return false; + float epsError = 1 + searchParams.eps; + + // fixed or variable-sized container (depending on DIM) + distance_vector_t dists; + // Fill it with zeros. + assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim), + static_cast(0)); + DistanceType distsq = this->computeInitialDistances(*this, vec, dists); + searchLevel(result, vec, BaseClassRef::root_node, distsq, dists, + epsError); // "count_leaf" parameter removed since was neither + // used nor returned to the user. + return result.full(); + } + + /** + * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. + * Their indices are stored inside the result object. \sa radiusSearch, + * findNeighbors \note nChecks_IGNORED is ignored but kept for compatibility + * with the original FLANN interface. \return Number `N` of valid points in + * the result set. Only the first `N` entries in `out_indices` and + * `out_distances_sq` will be valid. Return may be less than `num_closest` + * only if the number of elements in the tree is less than `num_closest`. + */ + size_t knnSearch(const ElementType *query_point, const size_t num_closest, + IndexType *out_indices, DistanceType *out_distances_sq, + const int /* nChecks_IGNORED */ = 10) const { + nanoflann::KNNResultSet resultSet(num_closest); + resultSet.init(out_indices, out_distances_sq); + this->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); + return resultSet.size(); + } + + /** + * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius. + * The output is given as a vector of pairs, of which the first element is a + * point index and the second the corresponding distance. Previous contents of + * \a IndicesDists are cleared. + * + * If searchParams.sorted==true, the output list is sorted by ascending + * distances. + * + * For a better performance, it is advisable to do a .reserve() on the vector + * if you have any wild guess about the number of expected matches. + * + * \sa knnSearch, findNeighbors, radiusSearchCustomCallback + * \return The number of points within the given radius (i.e. indices.size() + * or dists.size() ) + */ + size_t + radiusSearch(const ElementType *query_point, const DistanceType &radius, + std::vector> &IndicesDists, + const SearchParams &searchParams) const { + RadiusResultSet resultSet(radius, IndicesDists); + const size_t nFound = + radiusSearchCustomCallback(query_point, resultSet, searchParams); + if (searchParams.sorted) + std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter()); + return nFound; + } + + /** + * Just like radiusSearch() but with a custom callback class for each point + * found in the radius of the query. See the source of RadiusResultSet<> as a + * start point for your own classes. \sa radiusSearch + */ + template + size_t radiusSearchCustomCallback( + const ElementType *query_point, SEARCH_CALLBACK &resultSet, + const SearchParams &searchParams = SearchParams()) const { + this->findNeighbors(resultSet, query_point, searchParams); + return resultSet.size(); + } + + /** @} */ + +public: + void computeBoundingBox(BoundingBox &bbox) { + resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim)); + + if (dataset.kdtree_get_bbox(bbox)) { + // Done! It was implemented in derived class + } else { + const size_t N = BaseClassRef::m_size; + if (!N) + throw std::runtime_error("[nanoflann] computeBoundingBox() called but " + "no data points found."); + for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { + bbox[i].low = bbox[i].high = + this->dataset_get(*this, BaseClassRef::vind[0], i); + } + for (size_t k = 1; k < N; ++k) { + for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { + if (this->dataset_get(*this, BaseClassRef::vind[k], i) < bbox[i].low) + bbox[i].low = this->dataset_get(*this, BaseClassRef::vind[k], i); + if (this->dataset_get(*this, BaseClassRef::vind[k], i) > bbox[i].high) + bbox[i].high = this->dataset_get(*this, BaseClassRef::vind[k], i); + } + } + } + } + + /** + * Performs an exact search in the tree starting from a node. + * \tparam RESULTSET Should be any ResultSet + */ + template + void searchLevel(RESULTSET &result_set, const ElementType *vec, + const NodePtr node, DistanceType mindistsq, + distance_vector_t &dists, const float epsError) const { + /* If this is a leaf node, then do check and return. */ + if ((node->child1 == NULL) && (node->child2 == NULL)) { + // count_leaf += (node->lr.right-node->lr.left); // Removed since was + // neither used nor returned to the user. + DistanceType worst_dist = result_set.worstDist(); + for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right; + ++i) { + const IndexType index = BaseClassRef::vind[i]; // reorder... : i; + if (treeIndex[index] == -1) + continue; + DistanceType dist = distance.evalMetric( + vec, index, (DIM > 0 ? DIM : BaseClassRef::dim)); + if (dist < worst_dist) { + if (!result_set.addPoint( + static_cast(dist), + static_cast( + BaseClassRef::vind[i]))) { + // the resultset doesn't want to receive any more points, we're done + // searching! + return; // false; + } + } + } + return; + } + + /* Which child branch should be taken first? */ + int idx = node->node_type.sub.divfeat; + ElementType val = vec[idx]; + DistanceType diff1 = val - node->node_type.sub.divlow; + DistanceType diff2 = val - node->node_type.sub.divhigh; + + NodePtr bestChild; + NodePtr otherChild; + DistanceType cut_dist; + if ((diff1 + diff2) < 0) { + bestChild = node->child1; + otherChild = node->child2; + cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx); + } else { + bestChild = node->child2; + otherChild = node->child1; + cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx); + } + + /* Call recursively to search next level down. */ + searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError); + + DistanceType dst = dists[idx]; + mindistsq = mindistsq + cut_dist - dst; + dists[idx] = cut_dist; + if (mindistsq * epsError <= result_set.worstDist()) { + searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError); + } + dists[idx] = dst; + } + +public: + /** Stores the index in a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when + * loading the index object it must be constructed associated to the same + * source of data points used while building it. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); } + + /** Loads a previous index from a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the + * index object must be constructed associated to the same source of data + * points used while building the index. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); } +}; + +/** kd-tree dynaimic index + * + * class to create multiple static index and merge their results to behave as + * single dynamic index as proposed in Logarithmic Approach. + * + * Example of usage: + * examples/dynamic_pointcloud_example.cpp + * + * \tparam DatasetAdaptor The user-provided adaptor (see comments above). + * \tparam Distance The distance metric to use: nanoflann::metric_L1, + * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM + * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will + * be typically size_t or int + */ +template +class KDTreeSingleIndexDynamicAdaptor { +public: + typedef typename Distance::ElementType ElementType; + typedef typename Distance::DistanceType DistanceType; + +protected: + size_t m_leaf_max_size; + size_t treeCount; + size_t pointCount; + + /** + * The dataset used by this index + */ + const DatasetAdaptor &dataset; //!< The source of our data + + std::vector treeIndex; //!< treeIndex[idx] is the index of tree in which + //!< point at idx is stored. treeIndex[idx]=-1 + //!< means that point has been removed. + + KDTreeSingleIndexAdaptorParams index_params; + + int dim; //!< Dimensionality of each data point + + typedef KDTreeSingleIndexDynamicAdaptor_ + index_container_t; + std::vector index; + +public: + /** Get a const ref to the internal list of indices; the number of indices is + * adapted dynamically as the dataset grows in size. */ + const std::vector &getAllIndices() const { return index; } + +private: + /** finds position of least significant unset bit */ + int First0Bit(IndexType num) { + int pos = 0; + while (num & 1) { + num = num >> 1; + pos++; + } + return pos; + } + + /** Creates multiple empty trees to handle dynamic support */ + void init() { + typedef KDTreeSingleIndexDynamicAdaptor_ + my_kd_tree_t; + std::vector index_( + treeCount, my_kd_tree_t(dim /*dim*/, dataset, treeIndex, index_params)); + index = index_; + } + +public: + Distance distance; + + /** + * KDTree constructor + * + * Refer to docs in README.md or online in + * https://github.com/jlblancoc/nanoflann + * + * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 + * for 3D points) is determined by means of: + * - The \a DIM template parameter if >0 (highest priority) + * - Otherwise, the \a dimensionality parameter of this constructor. + * + * @param inputData Dataset with the input features + * @param params Basically, the maximum leaf node size + */ + KDTreeSingleIndexDynamicAdaptor(const int dimensionality, + const DatasetAdaptor &inputData, + const KDTreeSingleIndexAdaptorParams ¶ms = + KDTreeSingleIndexAdaptorParams(), + const size_t maximumPointCount = 1000000000U) + : dataset(inputData), index_params(params), distance(inputData) { + treeCount = static_cast(std::log2(maximumPointCount)); + pointCount = 0U; + dim = dimensionality; + treeIndex.clear(); + if (DIM > 0) + dim = DIM; + m_leaf_max_size = params.leaf_max_size; + init(); + const size_t num_initial_points = dataset.kdtree_get_point_count(); + if (num_initial_points > 0) { + addPoints(0, num_initial_points - 1); + } + } + + /** Deleted copy constructor*/ + KDTreeSingleIndexDynamicAdaptor( + const KDTreeSingleIndexDynamicAdaptor &) = delete; + + /** Add points to the set, Inserts all points from [start, end] */ + void addPoints(IndexType start, IndexType end) { + size_t count = end - start + 1; + treeIndex.resize(treeIndex.size() + count); + for (IndexType idx = start; idx <= end; idx++) { + int pos = First0Bit(pointCount); + index[pos].vind.clear(); + treeIndex[pointCount] = pos; + for (int i = 0; i < pos; i++) { + for (int j = 0; j < static_cast(index[i].vind.size()); j++) { + index[pos].vind.push_back(index[i].vind[j]); + if (treeIndex[index[i].vind[j]] != -1) + treeIndex[index[i].vind[j]] = pos; + } + index[i].vind.clear(); + index[i].freeIndex(index[i]); + } + index[pos].vind.push_back(idx); + index[pos].buildIndex(); + pointCount++; + } + } + + /** Remove a point from the set (Lazy Deletion) */ + void removePoint(size_t idx) { + if (idx >= pointCount) + return; + treeIndex[idx] = -1; + } + + /** + * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored + * inside the result object. + * + * Params: + * result = the result object in which the indices of the + * nearest-neighbors are stored vec = the vector for which to search the + * nearest neighbors + * + * \tparam RESULTSET Should be any ResultSet + * \return True if the requested neighbors could be found. + * \sa knnSearch, radiusSearch + */ + template + bool findNeighbors(RESULTSET &result, const ElementType *vec, + const SearchParams &searchParams) const { + for (size_t i = 0; i < treeCount; i++) { + index[i].findNeighbors(result, &vec[0], searchParams); + } + return result.full(); + } +}; + +/** An L2-metric KD-tree adaptor for working with data directly stored in an + * Eigen Matrix, without duplicating the data storage. Each row in the matrix + * represents a point in the state space. + * + * Example of usage: + * \code + * Eigen::Matrix mat; + * // Fill out "mat"... + * + * typedef KDTreeEigenMatrixAdaptor< Eigen::Matrix > + * my_kd_tree_t; const int max_leaf = 10; my_kd_tree_t mat_index(mat, max_leaf + * ); mat_index.index->buildIndex(); mat_index.index->... \endcode + * + * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality + * for the points in the data set, allowing more compiler optimizations. \tparam + * Distance The distance metric to use: nanoflann::metric_L1, + * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. + */ +template +struct KDTreeEigenMatrixAdaptor { + typedef KDTreeEigenMatrixAdaptor self_t; + typedef typename MatrixType::Scalar num_t; + typedef typename MatrixType::Index IndexType; + typedef + typename Distance::template traits::distance_t metric_t; + typedef KDTreeSingleIndexAdaptor + index_t; + + index_t *index; //! The kd-tree index for the user to call its methods as + //! usual with any other FLANN index. + + /// Constructor: takes a const ref to the matrix object with the data points + KDTreeEigenMatrixAdaptor(const size_t dimensionality, + const std::reference_wrapper &mat, + const int leaf_max_size = 10) + : m_data_matrix(mat) { + const auto dims = mat.get().cols(); + if (size_t(dims) != dimensionality) + throw std::runtime_error( + "Error: 'dimensionality' must match column count in data matrix"); + if (DIM > 0 && int(dims) != DIM) + throw std::runtime_error( + "Data set dimensionality does not match the 'DIM' template argument"); + index = + new index_t(static_cast(dims), *this /* adaptor */, + nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size)); + index->buildIndex(); + } + +public: + /** Deleted copy constructor */ + KDTreeEigenMatrixAdaptor(const self_t &) = delete; + + ~KDTreeEigenMatrixAdaptor() { delete index; } + + const std::reference_wrapper m_data_matrix; + + /** Query for the \a num_closest closest points to a given point (entered as + * query_point[0:dim-1]). Note that this is a short-cut method for + * index->findNeighbors(). The user can also call index->... methods as + * desired. \note nChecks_IGNORED is ignored but kept for compatibility with + * the original FLANN interface. + */ + inline void query(const num_t *query_point, const size_t num_closest, + IndexType *out_indices, num_t *out_distances_sq, + const int /* nChecks_IGNORED */ = 10) const { + nanoflann::KNNResultSet resultSet(num_closest); + resultSet.init(out_indices, out_distances_sq); + index->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); + } + + /** @name Interface expected by KDTreeSingleIndexAdaptor + * @{ */ + + const self_t &derived() const { return *this; } + self_t &derived() { return *this; } + + // Must return the number of data points + inline size_t kdtree_get_point_count() const { + return m_data_matrix.get().rows(); + } + + // Returns the dim'th component of the idx'th point in the class: + inline num_t kdtree_get_pt(const IndexType idx, size_t dim) const { + return m_data_matrix.get().coeff(idx, IndexType(dim)); + } + + // Optional bounding-box computation: return false to default to a standard + // bbox computation loop. + // Return true if the BBOX was already computed by the class and returned in + // "bb" so it can be avoided to redo it again. Look at bb.size() to find out + // the expected dimensionality (e.g. 2 or 3 for point clouds) + template bool kdtree_get_bbox(BBOX & /*bb*/) const { + return false; + } + + /** @} */ + +}; // end of KDTreeEigenMatrixAdaptor + /** @} */ + +/** @} */ // end of grouping +} // namespace nanoflann + +#endif /* NANOFLANN_HPP_ */ diff --git a/cpp/module/Scancontext/tictoc.h b/cpp/module/Scancontext/tictoc.h new file mode 100644 index 0000000..937abf8 --- /dev/null +++ b/cpp/module/Scancontext/tictoc.h @@ -0,0 +1,47 @@ +// Author: Tong Qin qintonguav@gmail.com +// Shaozu Cao saozu.cao@connect.ust.hk + +#pragma once + +#include +#include +#include +#include +#include + +class TicToc +{ +public: + TicToc() + { + tic(); + } + + TicToc( bool _disp ) + { + disp_ = _disp; + tic(); + } + + void tic() + { + start = std::chrono::system_clock::now(); + } + + void toc( std::string _about_task ) + { + end = std::chrono::system_clock::now(); + std::chrono::duration elapsed_seconds = end - start; + double elapsed_ms = elapsed_seconds.count() * 1000; + + if( disp_ ) + { + std::cout.precision(3); // 10 for sec, 3 for ms + std::cout << _about_task << ": " << elapsed_ms << " msec." << std::endl; + } + } + +private: + std::chrono::time_point start, end; + bool disp_ = false; +}; diff --git a/example/SLAM/README.md b/example/SLAM/README.md new file mode 100644 index 0000000..2b29331 --- /dev/null +++ b/example/SLAM/README.md @@ -0,0 +1,2 @@ +# Scan Context for LiDAR SLAM +- Go to [PyICP SLAM](https://github.com/kissb2/PyICP-SLAM) diff --git a/example/basic/basic.m b/example/basic/basic.m new file mode 100755 index 0000000..b13d07b --- /dev/null +++ b/example/basic/basic.m @@ -0,0 +1,81 @@ +clear; clc; +addpath(genpath('../../matlab/')); + + +%% Parameters +data_dir = '../../sample_data/KITTI/00/velodyne/'; + +basic_max_range = 80; % meter +basic_num_sectors = 60; +basic_num_rings = 20; + + +%% Visualization of ScanContext +bin_path = [data_dir, '000094.bin']; +ptcloud = KITTIbin2Ptcloud(bin_path); +sc = Ptcloud2ScanContext(ptcloud, basic_num_sectors, basic_num_rings, basic_max_range); + +h1 = figure(1); clf; +imagesc(sc); +set(gcf, 'Position', [10 10 800 300]); +xlabel('sector'); ylabel('ring'); + +% for vivid visualization +colormap jet; +caxis([0, 4]); % KITTI00 is usually in z: [0, 4] + + +%% Making Ringkey and maintaining kd-tree +% Read the PlaceRecognizer.m + + +%% ScanContext with different resolution + +figure(2); clf; +pcshow(ptcloud); colormap jet; caxis([0 4]); + +res = [0.25, 0.5, 1, 2, 3]; + +h2=figure(3); clf; +set(gcf, 'Position', [10 10 500 1000]); + +for i = 1:length(res) + num_sectors = basic_num_sectors * res(i); + num_rings = basic_num_rings * res(i); + + sc = Ptcloud2ScanContext(ptcloud, num_sectors, num_rings, basic_max_range); + + subplot(length(res), 1, i); + imagesc(sc); hold on; + colormap jet; + caxis([0, 4]); % KITTI00 is usually in z: [0, 4] +end + + + +%% Comparison btn two scan contexts + +KITTI_bin1a_path = [data_dir, '000094.bin']; +KITTI_bin1b_path = [data_dir, '000095.bin']; +KITTI_bin2a_path = [data_dir, '000198.bin']; +KITTI_bin2b_path = [data_dir, '000199.bin']; + +ptcloud_KITTI1a = KITTIbin2Ptcloud(KITTI_bin1a_path); +ptcloud_KITTI1b = KITTIbin2Ptcloud(KITTI_bin1b_path); +ptcloud_KITTI2a = KITTIbin2Ptcloud(KITTI_bin2a_path); +ptcloud_KITTI2b = KITTIbin2Ptcloud(KITTI_bin2b_path); + +sc_KITTI1a = Ptcloud2ScanContext(ptcloud_KITTI1a, basic_num_sectors, basic_num_rings, basic_max_range); +sc_KITTI1b = Ptcloud2ScanContext(ptcloud_KITTI1b, basic_num_sectors, basic_num_rings, basic_max_range); +sc_KITTI2a = Ptcloud2ScanContext(ptcloud_KITTI2a, basic_num_sectors, basic_num_rings, basic_max_range); +sc_KITTI2b = Ptcloud2ScanContext(ptcloud_KITTI2b, basic_num_sectors, basic_num_rings, basic_max_range); + +dist_1a_1b = DistanceBtnScanContexts(sc_KITTI1a, sc_KITTI1b); +dist_1b_2a = DistanceBtnScanContexts(sc_KITTI1b, sc_KITTI2a); +dist_1a_2a = DistanceBtnScanContexts(sc_KITTI1a, sc_KITTI2a); +dist_2a_2b = DistanceBtnScanContexts(sc_KITTI2a, sc_KITTI2b); + +disp([dist_1a_1b, dist_1b_2a, dist_1a_2a, dist_2a_2b]); + + + diff --git a/example/basic/ptcloud.png b/example/basic/ptcloud.png new file mode 100644 index 0000000..182444e Binary files /dev/null and b/example/basic/ptcloud.png differ diff --git a/example/basic/scmaking.gif b/example/basic/scmaking.gif new file mode 100644 index 0000000..88fc5d9 Binary files /dev/null and b/example/basic/scmaking.gif differ diff --git a/example/basic/various_res.png b/example/basic/various_res.png new file mode 100755 index 0000000..d6391c8 Binary files /dev/null and b/example/basic/various_res.png differ diff --git a/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/Main.m b/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/Main.m new file mode 100755 index 0000000..9aaa74c --- /dev/null +++ b/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/Main.m @@ -0,0 +1,112 @@ +%% information +% main for Sampling places + +%% +clear; clc; +addpath(genpath('../../../../../matlab/')); +addpath(genpath('./helper')); + +SaveDirectoryList +Parameters + +%% Preparation 1: make pre-determined Grid Cell index +PlaceIndexAndGridCenters_10m = makeGridCellIndex(xRange, yRange, 10); + +%% Preparation 2: get scan times +SequenceDate = '2012-01-15'; % ### Change this part to your date +ScanBaseDir = 'F:\NCLT/'; % ### Change this part to your path + +ScanDir = strcat(ScanBaseDir, SequenceDate, '/velodyne_sync/'); +Scans = dir(ScanDir); Scans(1:2, :) = []; Scans = {Scans(:).name}; +ScanTimes = getNCLTscanInformation(Scans); + +%% Preparation 3: load GT pose (for calc moving diff and location) +GroundTruthPosePath = strcat(ScanBaseDir, SequenceDate, '/groundtruth_', SequenceDate, '.csv'); +GroundTruthPoseData = csvread(GroundTruthPosePath); + +GroundTruthPoseTime = GroundTruthPoseData(:, 1); +GroundTruthPoseXYZ = GroundTruthPoseData(:, 2:4); + +nGroundTruthPoses = length(GroundTruthPoseData); + +%% logger +TrajectoryInformationWRT10mCell = []; + +nTotalSampledPlaces = 0; + +%% Main: Sampling + +MoveCounter = 0; % Reset 0 again for every SamplingGap reached. +for ii = 1000:nGroundTruthPoses % just quite large number 1000 for avoiding first several NaNs + curTime = GroundTruthPoseTime(ii, 1); + + prvPose = GroundTruthPoseXYZ(ii-1, :); + curPose = GroundTruthPoseXYZ(ii, :); + + curMove = norm(curPose - prvPose); + MoveCounter = MoveCounter + curMove; + + if(MoveCounter >= SamplingGap) + nTotalSampledPlaces = nTotalSampledPlaces + 1; + curSamplingCounter = nTotalSampledPlaces; + + % Returns the index of the cell, where the current pose is closest to the cell's center coordinates. + PlaceIdx_10m = getPlaceIdx(curPose, PlaceIndexAndGridCenters_10m); % 2nd argument is cell's size + + % load current point cloud + curPtcloud = getNearestPtcloud( ScanTimes, curTime, Scans, ScanDir); + + %% Save data + % log + TrajectoryInformationWRT10mCell = [TrajectoryInformationWRT10mCell; curTime, curPose, nTotalSampledPlaces, PlaceIdx_10m]; + + % scan context + ScanContextForward = Ptcloud2ScanContext(curPtcloud, nSectors, nRings, Lmax); + + % SCI gray (1 channel) + ScanContextForwardRanged = ScaleSc2Img(ScanContextForward, NCLTminHeight, NCLTmaxHeight); + ScanContextForwardScaled = ScanContextForwardRanged./maxColor; + + SCIforwardGray = round(ScanContextForwardScaled*255); + SCIforwardGray = ind2gray(SCIforwardGray, gray(255)); + + % SCI jet (color, 3 channel) + SCIforwardColor = round(ScanContextForwardScaled*255); + SCIforwardColor = ind2rgb(SCIforwardColor, jet(255)); + + saveSCIcolor(SCIforwardColor, DIR_SCIcolor, curSamplingCounter, PlaceIdx_10m, '10', 'f'); + + % SCI jet + Backward dataAug + ScanContextBackwardScaled = circshift(ScanContextForwardScaled, nSectors/2, 2); + SCIbackwardColor = round(ScanContextBackwardScaled*255); + SCIbackwardColor = ind2rgb(SCIbackwardColor, jet(255)); + + saveSCIcolor(SCIforwardColor, DIR_SCIcolorAlsoBack, curSamplingCounter, PlaceIdx_10m, '10', 'f'); + saveSCIcolor(SCIbackwardColor, DIR_SCIcolorAlsoBack, curSamplingCounter, PlaceIdx_10m, '10', 'b'); + + % End: Reset counter + MoveCounter = 0; + + % Tracking progress message + if(rem(curSamplingCounter, 100) == 0) + message = strcat(num2str(curSamplingCounter), "th sample is saved." ); + disp(message) + end + + end + +end + + +%% save Trajectory Information +% 10m +filepath = strcat(DIR_SampledPlacesInformation, '/TrajectoryInformation.csv'); +TrajectoryInformation = TrajectoryInformationWRT10mCell; +dlmwrite(filepath, TrajectoryInformation, 'precision','%.6f') + + + + + + + diff --git a/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/Parameters.m b/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/Parameters.m new file mode 100755 index 0000000..0533ed8 --- /dev/null +++ b/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/Parameters.m @@ -0,0 +1,29 @@ +% flag for train/test +IF_TRAINING = 1; + +% sampling gap +SamplingGap = 1; % in meter + +% place resolution +PlaceCellSize = 10; % in meter + +% NCLT region +xRange = [-350, 130]; +yRange = [-730, 120]; + + +% scan context +nRings = 40; +nSectors = 120; +Lmax = 80; + +% scan context image +NCLTminHeight = 0; +NCLTmaxHeight = 15; + +SCI_HEIGHT_RANGE = [NCLTminHeight, NCLTmaxHeight]; + +minColor = 0; +maxColor = 255; +rangeColor = maxColor - minColor; + diff --git a/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/SaveDirectoryList.m b/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/SaveDirectoryList.m new file mode 100755 index 0000000..8c7ec11 --- /dev/null +++ b/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/SaveDirectoryList.m @@ -0,0 +1,6 @@ + +%% Save Directories + +DIR_SampledPlacesInformation = './data/SampledPlacesInformation/'; +DIR_SCIcolor = './data/SCI_jet0to15/'; +DIR_SCIcolorAlsoBack = './data/SCI_jet0to15_BackAug/'; diff --git a/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/helper/ScaleSc2Img.m b/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/helper/ScaleSc2Img.m new file mode 100755 index 0000000..baf3041 --- /dev/null +++ b/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/helper/ScaleSc2Img.m @@ -0,0 +1,33 @@ +function [ scScaled ] = ScaleSc2Img( scOriginal, minHeight, maxHeight ) + +maxColor = 255; + +rangeHeight = maxHeight - minHeight; + +nRows = size(scOriginal, 1); +nCols = size(scOriginal, 2); + +scOriginalRangeCut = scOriginal; +% cut into range +for ithRow = 1:nRows + for jthCol = 1:nCols + + ithPixel = scOriginal(ithRow, jthCol); + + if(ithPixel >= maxHeight) + scOriginalRangeCut(ithRow, jthCol) = maxHeight; + end + + if(ithPixel <= minHeight) + scOriginalRangeCut(ithRow, jthCol) = minHeight; + end + + scOriginalRangeCut(ithRow, jthCol) = round(scOriginalRangeCut(ithRow, jthCol) * (maxColor/rangeHeight)); + end +end + +scScaled = scOriginalRangeCut; + + +end + diff --git a/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/helper/getNCLTscanInformation.m b/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/helper/getNCLTscanInformation.m new file mode 100755 index 0000000..58e40e3 --- /dev/null +++ b/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/helper/getNCLTscanInformation.m @@ -0,0 +1,15 @@ +function [ ScanTimes ] = getNCLTscanInformation( Scans ) + +nScans = length(Scans); + +TIME_LENGTH = 16; % Dont Change this +ScanTimes = zeros(nScans, 1); +for i=1:nScans + ithScanName = Scans{i}; + ithScanTime = str2double(ithScanName(1:TIME_LENGTH)); + ScanTimes(i) = ithScanTime; +end + + +end + diff --git a/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/helper/getNearestPtcloud.m b/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/helper/getNearestPtcloud.m new file mode 100755 index 0000000..a6349e4 --- /dev/null +++ b/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/helper/getNearestPtcloud.m @@ -0,0 +1,9 @@ +function [ Ptcloud ] = getNearestPtcloud( ScanTimes, curTime, Scans, ScanDir) + +[MinTimeDelta, ArgminIdx] = min(abs(ScanTimes-curTime)); +ArgminBinName = Scans{ArgminIdx}; +ArgminBinPath = strcat(ScanDir, ArgminBinName); +Ptcloud = NCLTbin2Ptcloud(ArgminBinPath); + +end + diff --git a/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/helper/getPlaceIdx.m b/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/helper/getPlaceIdx.m new file mode 100755 index 0000000..ec662d2 --- /dev/null +++ b/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/helper/getPlaceIdx.m @@ -0,0 +1,23 @@ +function [ PlaceIdx ] = getPlaceIdx(curPose, PlaceIndexAndGridCenters) + +%% load meta file + +%% Main +curX = curPose(1); +curY = curPose(2); + +PlaceCellCenters = PlaceIndexAndGridCenters(:, 2:3); +nPlaces = length(PlaceCellCenters); + +Dists = zeros(nPlaces, 1); +for ii=1:nPlaces + Dist = norm(PlaceCellCenters(ii, :) - [curX, curY]); + Dists(ii) = Dist; +end + +[NearestDist, NearestIdx] = min(Dists); + +PlaceIdx = NearestIdx; + +end + diff --git a/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/helper/getThetaFromXY.m b/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/helper/getThetaFromXY.m new file mode 100755 index 0000000..17046b9 --- /dev/null +++ b/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/helper/getThetaFromXY.m @@ -0,0 +1,17 @@ +function [ theta ] = getThetaFromXY( x, y ) + + if (x >= 0 && y >= 0) + theta = 180/pi * atan(y/x); + end + if (x < 0 && y >= 0) + theta = 180 - ((180/pi) * atan(y/(-x))); + end + if (x < 0 && y < 0) + theta = 180 + ((180/pi) * atan(y/x)); + end + if ( x >= 0 && y < 0) + theta = 360 - ((180/pi) * atan((-y)/x)); + end + +end + diff --git a/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/helper/makeGridCellIndex.m b/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/helper/makeGridCellIndex.m new file mode 100755 index 0000000..033b50d --- /dev/null +++ b/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/helper/makeGridCellIndex.m @@ -0,0 +1,28 @@ +function [ PlaceIndexAndGridCenters ] = makeGridCellIndex ( xRange, yRange, PlaceCellSize ) + +xSize = xRange(2) - xRange(1); +ySize = yRange(2) - yRange(1); + +nGridX = round(xSize/PlaceCellSize); +nGridY = round(ySize/PlaceCellSize); + +xGridBoundaries = linspace(xRange(1), xRange(2), nGridX+1); +yGridBoundaries = linspace(yRange(1), yRange(2), nGridY+1); + +nTotalIndex = nGridX * nGridY; + +curAssignedIndex = 1; +PlaceIndexAndGridCenters = zeros(nTotalIndex, 3); +for ii=1:nGridX + xGridCenter = (xGridBoundaries(ii+1) + xGridBoundaries(ii))/2; + for jj=1:nGridY + yGridCenter = (yGridBoundaries(jj+1) + yGridBoundaries(jj))/2; + + PlaceIndexAndGridCenters(curAssignedIndex, :) = [curAssignedIndex, xGridCenter, yGridCenter]; + curAssignedIndex = curAssignedIndex + 1; + end + +end + +end + diff --git a/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/helper/saveSCIcolor.m b/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/helper/saveSCIcolor.m new file mode 100755 index 0000000..9d39388 --- /dev/null +++ b/example/longterm_localization/NCLT/2012-01-15/1_DataMaker/helper/saveSCIcolor.m @@ -0,0 +1,22 @@ +function [ ] = saveSCIcolor(curSCIcolor, DIR_SCIcolor, SamplingCounter, PlaceIdx, CellSize, ForB) + +SCIcolor = curSCIcolor; + +curSamplingCounter = num2str(SamplingCounter,'%0#6.f'); +curSamplingCounter = curSamplingCounter(1:end-1); + +curPlaceIdx = num2str(PlaceIdx,'%0#6.f'); +curPlaceIdx = curPlaceIdx(1:end-1); + +saveName = strcat(curSamplingCounter, '_', curPlaceIdx); +saveDir = strcat(DIR_SCIcolor, CellSize, '/'); +if( ~exist(saveDir)) + mkdir(saveDir) +end + +savePath = strcat(saveDir, saveName, ForB, '.png'); + +imwrite(SCIcolor, savePath); + +end + diff --git a/example/longterm_localization/NCLT/2012-01-15/2_Train/.ipynb_checkpoints/train-checkpoint.ipynb b/example/longterm_localization/NCLT/2012-01-15/2_Train/.ipynb_checkpoints/train-checkpoint.ipynb new file mode 100755 index 0000000..66a93e2 --- /dev/null +++ b/example/longterm_localization/NCLT/2012-01-15/2_Train/.ipynb_checkpoints/train-checkpoint.ipynb @@ -0,0 +1,302 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 9, + "metadata": {}, + "outputs": [], + "source": [ + "import os\n", + "import numpy as np \n", + "import pandas as pd \n", + "import tensorflow as tf\n", + "import keras\n", + "import matplotlib.pyplot as plt" + ] + }, + { + "cell_type": "code", + "execution_count": 115, + "metadata": {}, + "outputs": [], + "source": [ + "# Data info \n", + "rootDir = '..your_data_path/'\n", + "\n", + "Dataset = 'NCLT'\n", + "TrainOrTest = '/Train/'\n", + "SequenceDate = '2012-01-15'\n", + "\n", + "SCImiddlePath = '/5. SCI_jet0to15_BackAug/'\n", + "\n", + "GridCellSize = '10'" + ] + }, + { + "cell_type": "code", + "execution_count": 116, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "/media/gskim/IRAP-ADV1/Data/ICRA2019/NCLT/Train/2012-01-15/5. SCI_jet0to15_BackAug/10/\n" + ] + } + ], + "source": [ + "DataPath = rootDir + Dataset + TrainOrTest + SequenceDate + SCImiddlePath + GridCellSize + '/'\n", + "print(DataPath)" + ] + }, + { + "cell_type": "code", + "execution_count": 181, + "metadata": {}, + "outputs": [], + "source": [ + "def getTrainingDataNCLT(DataPath, SequenceDate): \n", + "\n", + " # info\n", + " WholeData = os.listdir(DataPath)\n", + " nWholeData = len(WholeData)\n", + " print(str(nWholeData) + ' data exist in ' + SequenceDate)\n", + " \n", + " # read \n", + " X = []\n", + " y = []\n", + " for ii in range(nWholeData):\n", + " dataName = WholeData[ii]\n", + " dataPath = DataPath + dataName\n", + " \n", + " dataTrajNodeOrder = int(dataName[0:5])\n", + "\n", + " SCI = plt.imread(dataPath)\n", + " dataPlaceIndex = int(dataName[6:11])\n", + " \n", + " X.append(SCI)\n", + " y.append(dataPlaceIndex)\n", + " \n", + " # progress message \n", + " if ii%1000==0:\n", + " print(str(format((ii/nWholeData)*100, '.1f')), '% loaded.')\n", + " \n", + " \n", + " dataShape = SCI.shape\n", + " \n", + " # X\n", + " X_nd = np.zeros(shape=(nWholeData, dataShape[0], dataShape[1], dataShape[2]))\n", + " for jj in range(len(X)):\n", + " X_nd[jj, :, :] = X[jj]\n", + " X_nd = X_nd.astype('float32')\n", + " \n", + " # y (one-hot encoded)\n", + " from sklearn.preprocessing import LabelEncoder\n", + " lbl_enc = LabelEncoder()\n", + " lbl_enc.fit(y)\n", + " \n", + " ClassesTheSequenceHave = lbl_enc.classes_\n", + " nClassesTheSequenceHave = len(ClassesTheSequenceHave)\n", + " \n", + " y = lbl_enc.transform(y)\n", + " y_nd = keras.utils.np_utils.to_categorical(y, num_classes=nClassesTheSequenceHave)\n", + "\n", + " # log message \n", + " print('Data size: %s' % nWholeData)\n", + " print(' ')\n", + " print('Data shape:', X_nd.shape)\n", + " print('Label shape:', y_nd.shape)\n", + " \n", + " return X_nd, y_nd, lbl_enc\n", + " " + ] + }, + { + "cell_type": "code", + "execution_count": 182, + "metadata": { + "scrolled": false + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "15078 data exist in 2012-01-15\n", + "0.0 % loaded.\n", + "6.6 % loaded.\n", + "13.3 % loaded.\n", + "19.9 % loaded.\n", + "26.5 % loaded.\n", + "33.2 % loaded.\n", + "39.8 % loaded.\n", + "46.4 % loaded.\n", + "53.1 % loaded.\n", + "59.7 % loaded.\n", + "66.3 % loaded.\n", + "73.0 % loaded.\n", + "79.6 % loaded.\n", + "86.2 % loaded.\n", + "92.9 % loaded.\n", + "99.5 % loaded.\n", + "Data size: 15078\n", + "Data shape: (15078, 40, 120, 3)\n", + "Label shape: (15078, 579)\n" + ] + } + ], + "source": [ + "[X, y, lbl_enc] = getTrainingDataNCLT(DataPath, SequenceDate)" + ] + }, + { + "cell_type": "code", + "execution_count": 172, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "(40, 120, 3)\n", + "(579,)\n" + ] + } + ], + "source": [ + "dataShape = X[0].shape\n", + "labelShape = y[0].shape\n", + "\n", + "print(dataShape)\n", + "print(labelShape)" + ] + }, + { + "cell_type": "code", + "execution_count": 173, + "metadata": {}, + "outputs": [], + "source": [ + "# Model \n", + "from keras import backend as K\n", + "K.clear_session()\n", + "\n", + "ModelName = 'my_model'\n", + "\n", + "Drop1 = 0.7\n", + "Drop2 = 0.7\n", + "\n", + "KernelSize = 5\n", + "\n", + "nConv1Filter = 64\n", + "nConv2Filter = 128\n", + "nConv3Filter = 256\n", + "\n", + "nFCN1 = 64\n", + "\n", + "inputs = keras.layers.Input(shape=(dataShape[0], dataShape[1], dataShape[2]))\n", + "x = keras.layers.Conv2D(filters=nConv1Filter, kernel_size=KernelSize, activation='relu', padding='same')(inputs)\n", + "x = keras.layers.MaxPooling2D(pool_size=(2, 2), strides=None, padding='valid')(x)\n", + "x = keras.layers.BatchNormalization()(x)\n", + "x = keras.layers.Conv2D(filters=nConv2Filter, kernel_size=KernelSize, activation='relu', padding='same')(x)\n", + "x = keras.layers.MaxPool2D()(x)\n", + "x = keras.layers.BatchNormalization()(x)\n", + "x = keras.layers.Conv2D(filters=nConv3Filter, kernel_size=KernelSize, activation='relu', padding='same')(x)\n", + "x = keras.layers.MaxPool2D()(x)\n", + "x = keras.layers.Flatten()(x)\n", + "x = keras.layers.Dropout(rate=Drop1)(x)\n", + "x = keras.layers.Dense(units=nFCN1)(x)\n", + "x = keras.layers.Dropout(rate=Drop2)(x)\n", + "outputs = keras.layers.Dense(units=labelShape[0], activation='softmax')(x)\n", + "\n", + "model = keras.models.Model(inputs=inputs, outputs=outputs)" + ] + }, + { + "cell_type": "code", + "execution_count": 174, + "metadata": {}, + "outputs": [], + "source": [ + "# Model Compile \n", + "model.compile(optimizer='adam', loss='categorical_crossentropy', metrics=['acc'])\n", + "model.build(None,)" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "scrolled": true + }, + "outputs": [], + "source": [ + "#Train\n", + "X_train = X\n", + "y_train = y\n", + "\n", + "nEpoch = 200\n", + "\n", + "model.fit(X_train,\n", + " y_train,\n", + " epochs=nEpoch,\n", + " batch_size=64,\n", + " verbose=1\n", + " )" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "#Train\n", + "X_train = X\n", + "y_train = y\n", + "\n", + "nEpoch = 200\n", + "\n", + "model.fit(X_train,\n", + " y_train,\n", + " epochs=nEpoch,\n", + " batch_size=64,\n", + " verbose=1\n", + " )" + ] + }, + { + "cell_type": "code", + "execution_count": 186, + "metadata": {}, + "outputs": [], + "source": [ + "# model save \n", + "modelName = 'ModelWS_' + GridCellSize + 'm_' + ModelName + '.h5'\n", + "model.save(modelName)\n" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.5.2" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/example/longterm_localization/NCLT/2012-01-15/2_Train/train.ipynb b/example/longterm_localization/NCLT/2012-01-15/2_Train/train.ipynb new file mode 100755 index 0000000..66a93e2 --- /dev/null +++ b/example/longterm_localization/NCLT/2012-01-15/2_Train/train.ipynb @@ -0,0 +1,302 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 9, + "metadata": {}, + "outputs": [], + "source": [ + "import os\n", + "import numpy as np \n", + "import pandas as pd \n", + "import tensorflow as tf\n", + "import keras\n", + "import matplotlib.pyplot as plt" + ] + }, + { + "cell_type": "code", + "execution_count": 115, + "metadata": {}, + "outputs": [], + "source": [ + "# Data info \n", + "rootDir = '..your_data_path/'\n", + "\n", + "Dataset = 'NCLT'\n", + "TrainOrTest = '/Train/'\n", + "SequenceDate = '2012-01-15'\n", + "\n", + "SCImiddlePath = '/5. SCI_jet0to15_BackAug/'\n", + "\n", + "GridCellSize = '10'" + ] + }, + { + "cell_type": "code", + "execution_count": 116, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "/media/gskim/IRAP-ADV1/Data/ICRA2019/NCLT/Train/2012-01-15/5. SCI_jet0to15_BackAug/10/\n" + ] + } + ], + "source": [ + "DataPath = rootDir + Dataset + TrainOrTest + SequenceDate + SCImiddlePath + GridCellSize + '/'\n", + "print(DataPath)" + ] + }, + { + "cell_type": "code", + "execution_count": 181, + "metadata": {}, + "outputs": [], + "source": [ + "def getTrainingDataNCLT(DataPath, SequenceDate): \n", + "\n", + " # info\n", + " WholeData = os.listdir(DataPath)\n", + " nWholeData = len(WholeData)\n", + " print(str(nWholeData) + ' data exist in ' + SequenceDate)\n", + " \n", + " # read \n", + " X = []\n", + " y = []\n", + " for ii in range(nWholeData):\n", + " dataName = WholeData[ii]\n", + " dataPath = DataPath + dataName\n", + " \n", + " dataTrajNodeOrder = int(dataName[0:5])\n", + "\n", + " SCI = plt.imread(dataPath)\n", + " dataPlaceIndex = int(dataName[6:11])\n", + " \n", + " X.append(SCI)\n", + " y.append(dataPlaceIndex)\n", + " \n", + " # progress message \n", + " if ii%1000==0:\n", + " print(str(format((ii/nWholeData)*100, '.1f')), '% loaded.')\n", + " \n", + " \n", + " dataShape = SCI.shape\n", + " \n", + " # X\n", + " X_nd = np.zeros(shape=(nWholeData, dataShape[0], dataShape[1], dataShape[2]))\n", + " for jj in range(len(X)):\n", + " X_nd[jj, :, :] = X[jj]\n", + " X_nd = X_nd.astype('float32')\n", + " \n", + " # y (one-hot encoded)\n", + " from sklearn.preprocessing import LabelEncoder\n", + " lbl_enc = LabelEncoder()\n", + " lbl_enc.fit(y)\n", + " \n", + " ClassesTheSequenceHave = lbl_enc.classes_\n", + " nClassesTheSequenceHave = len(ClassesTheSequenceHave)\n", + " \n", + " y = lbl_enc.transform(y)\n", + " y_nd = keras.utils.np_utils.to_categorical(y, num_classes=nClassesTheSequenceHave)\n", + "\n", + " # log message \n", + " print('Data size: %s' % nWholeData)\n", + " print(' ')\n", + " print('Data shape:', X_nd.shape)\n", + " print('Label shape:', y_nd.shape)\n", + " \n", + " return X_nd, y_nd, lbl_enc\n", + " " + ] + }, + { + "cell_type": "code", + "execution_count": 182, + "metadata": { + "scrolled": false + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "15078 data exist in 2012-01-15\n", + "0.0 % loaded.\n", + "6.6 % loaded.\n", + "13.3 % loaded.\n", + "19.9 % loaded.\n", + "26.5 % loaded.\n", + "33.2 % loaded.\n", + "39.8 % loaded.\n", + "46.4 % loaded.\n", + "53.1 % loaded.\n", + "59.7 % loaded.\n", + "66.3 % loaded.\n", + "73.0 % loaded.\n", + "79.6 % loaded.\n", + "86.2 % loaded.\n", + "92.9 % loaded.\n", + "99.5 % loaded.\n", + "Data size: 15078\n", + "Data shape: (15078, 40, 120, 3)\n", + "Label shape: (15078, 579)\n" + ] + } + ], + "source": [ + "[X, y, lbl_enc] = getTrainingDataNCLT(DataPath, SequenceDate)" + ] + }, + { + "cell_type": "code", + "execution_count": 172, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "(40, 120, 3)\n", + "(579,)\n" + ] + } + ], + "source": [ + "dataShape = X[0].shape\n", + "labelShape = y[0].shape\n", + "\n", + "print(dataShape)\n", + "print(labelShape)" + ] + }, + { + "cell_type": "code", + "execution_count": 173, + "metadata": {}, + "outputs": [], + "source": [ + "# Model \n", + "from keras import backend as K\n", + "K.clear_session()\n", + "\n", + "ModelName = 'my_model'\n", + "\n", + "Drop1 = 0.7\n", + "Drop2 = 0.7\n", + "\n", + "KernelSize = 5\n", + "\n", + "nConv1Filter = 64\n", + "nConv2Filter = 128\n", + "nConv3Filter = 256\n", + "\n", + "nFCN1 = 64\n", + "\n", + "inputs = keras.layers.Input(shape=(dataShape[0], dataShape[1], dataShape[2]))\n", + "x = keras.layers.Conv2D(filters=nConv1Filter, kernel_size=KernelSize, activation='relu', padding='same')(inputs)\n", + "x = keras.layers.MaxPooling2D(pool_size=(2, 2), strides=None, padding='valid')(x)\n", + "x = keras.layers.BatchNormalization()(x)\n", + "x = keras.layers.Conv2D(filters=nConv2Filter, kernel_size=KernelSize, activation='relu', padding='same')(x)\n", + "x = keras.layers.MaxPool2D()(x)\n", + "x = keras.layers.BatchNormalization()(x)\n", + "x = keras.layers.Conv2D(filters=nConv3Filter, kernel_size=KernelSize, activation='relu', padding='same')(x)\n", + "x = keras.layers.MaxPool2D()(x)\n", + "x = keras.layers.Flatten()(x)\n", + "x = keras.layers.Dropout(rate=Drop1)(x)\n", + "x = keras.layers.Dense(units=nFCN1)(x)\n", + "x = keras.layers.Dropout(rate=Drop2)(x)\n", + "outputs = keras.layers.Dense(units=labelShape[0], activation='softmax')(x)\n", + "\n", + "model = keras.models.Model(inputs=inputs, outputs=outputs)" + ] + }, + { + "cell_type": "code", + "execution_count": 174, + "metadata": {}, + "outputs": [], + "source": [ + "# Model Compile \n", + "model.compile(optimizer='adam', loss='categorical_crossentropy', metrics=['acc'])\n", + "model.build(None,)" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "scrolled": true + }, + "outputs": [], + "source": [ + "#Train\n", + "X_train = X\n", + "y_train = y\n", + "\n", + "nEpoch = 200\n", + "\n", + "model.fit(X_train,\n", + " y_train,\n", + " epochs=nEpoch,\n", + " batch_size=64,\n", + " verbose=1\n", + " )" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "#Train\n", + "X_train = X\n", + "y_train = y\n", + "\n", + "nEpoch = 200\n", + "\n", + "model.fit(X_train,\n", + " y_train,\n", + " epochs=nEpoch,\n", + " batch_size=64,\n", + " verbose=1\n", + " )" + ] + }, + { + "cell_type": "code", + "execution_count": 186, + "metadata": {}, + "outputs": [], + "source": [ + "# model save \n", + "modelName = 'ModelWS_' + GridCellSize + 'm_' + ModelName + '.h5'\n", + "model.save(modelName)\n" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.5.2" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/.ipynb_checkpoints/inference-checkpoint.ipynb b/example/longterm_localization/NCLT/2012-01-15/3_Inference/.ipynb_checkpoints/inference-checkpoint.ipynb new file mode 100755 index 0000000..bbacf87 --- /dev/null +++ b/example/longterm_localization/NCLT/2012-01-15/3_Inference/.ipynb_checkpoints/inference-checkpoint.ipynb @@ -0,0 +1,303 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [], + "source": [ + "import os\n", + "import numpy as np \n", + "import pandas as pd \n", + "import tensorflow as tf\n", + "import keras\n", + "import matplotlib.pyplot as plt" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "# Data info \n", + "rootDir = '..your_path'\n", + "\n", + "Dataset = 'NCLT'\n", + "TrainOrTest = '/Test/'\n", + "SequenceDate = '2013-04-05'\n", + "\n", + "SCImiddlePath = '/4. SCI_jet0to15/'\n", + "\n", + "GridCellSize = '10'\n", + "\n", + "DataPath = rootDir + Dataset + TrainOrTest + SequenceDate + SCImiddlePath + GridCellSize + '/'\n", + "print(DataPath)" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], + "source": [ + "DataPath = ICRArootDir + Dataset + TrainOrTest + SequenceDate + SCImiddlePath + GridCellSize + '/'\n", + "print(DataPath)" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [], + "source": [ + "def getTestDataNCLT(DataPath, SequenceDate, lbl_enc_train): \n", + "\n", + " # load training label encoding information \n", + " from sklearn.preprocessing import LabelEncoder\n", + " ClassesTrainingSequenceHave = lbl_enc_train.classes_\n", + " nClassesTrainingSequenceHave = len(ClassesTrainingSequenceHave)\n", + "\n", + " # info\n", + " WholeData = os.listdir(DataPath)\n", + " nWholeData = len(WholeData)\n", + " print(str(nWholeData) + ' data exist in ' + SequenceDate)\n", + " \n", + " # read \n", + " X_seen = []\n", + " y_seen = []\n", + " X_unseen = []\n", + " y_unseen = []\n", + " \n", + " for ii in range(nWholeData):\n", + " dataName = WholeData[ii]\n", + " dataPath = DataPath + dataName\n", + " \n", + " dataTrajNodeOrder = int(dataName[0:5])\n", + "\n", + " SCI = plt.imread(dataPath)\n", + " dataPlaceIndex = int(dataName[6:11])\n", + " \n", + " # if label is in the train, then save into the seen (seen is only subset to be tested)\n", + " if dataPlaceIndex in ClassesTrainingSequenceHave:\n", + " X_seen.append(SCI)\n", + " y_seen.append(dataPlaceIndex)\n", + " else:\n", + " X_unseen.append(SCI)\n", + " y_unseen.append(dataPlaceIndex)\n", + " \n", + " # progress message \n", + " if ii%1000==0:\n", + " print(str(format((ii/nWholeData)*100, '.1f')), '% loaded.')\n", + " \n", + " dataShape = SCI.shape\n", + " \n", + " # X\n", + " nSeenData = len(X_seen)\n", + " X_nd = np.zeros(shape=(nSeenData, dataShape[0], dataShape[1], dataShape[2]))\n", + " for jj in range(nSeenData):\n", + " X_nd[jj, :, :] = X_seen[jj]\n", + " X_nd = X_nd.astype('float32')\n", + " \n", + " # y (one-hot encoded) \n", + " y_seen = lbl_enc_train.transform(y_seen)\n", + " y_nd = keras.utils.np_utils.to_categorical(y_seen, num_classes=nClassesTrainingSequenceHave)\n", + "\n", + " # log message \n", + " print('Data size: %s' % nWholeData)\n", + " print('- Seen data: %s' % len(X_seen))\n", + " print('- Uneen data: %s' % len(X_unseen))\n", + " print(' ')\n", + " print('Data shape:', X_nd.shape)\n", + " print('Label shape:', y_nd.shape)\n", + " \n", + " return X_nd, y_nd, X_unseen, y_unseen\n", + " " + ] + }, + { + "cell_type": "code", + "execution_count": 63, + "metadata": {}, + "outputs": [], + "source": [ + "# load training label encoding information for discriminate seen/unseen of test \n", + "import pickle\n", + "TrainingDate = '2012-01-15'\n", + "TrainingDataPath = 'data_pickle/Train_' + TrainingDate + '_SCI_color.pkl'\n", + "\n", + "with open(TrainingDataPath, 'rb') as f: # Python 3: open(..., 'rb')\n", + " X_train, y_train, lbl_enc_train = pickle.load(f)" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [], + "source": [ + "# Load test data using training label encoder information \n", + "[X_seen, y_seen, X_unseen, y_unseen] = getTestDataNCLT(DataPath, SequenceDate, lbl_enc_train)" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "/home/gskim/anaconda3/envs/tfkeras/lib/python3.5/site-packages/h5py/__init__.py:36: FutureWarning: Conversion of the second argument of issubdtype from `float` to `np.floating` is deprecated. In future, it will be treated as `np.float64 == np.dtype(float).type`.\n", + " from ._conv import register_converters as _register_converters\n", + "Using TensorFlow backend.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "_________________________________________________________________\n", + "Layer (type) Output Shape Param # \n", + "=================================================================\n", + "input_1 (InputLayer) (None, 40, 120, 3) 0 \n", + "_________________________________________________________________\n", + "conv2d_1 (Conv2D) (None, 40, 120, 64) 4864 \n", + "_________________________________________________________________\n", + "max_pooling2d_1 (MaxPooling2 (None, 20, 60, 64) 0 \n", + "_________________________________________________________________\n", + "batch_normalization_1 (Batch (None, 20, 60, 64) 256 \n", + "_________________________________________________________________\n", + "conv2d_2 (Conv2D) (None, 20, 60, 128) 204928 \n", + "_________________________________________________________________\n", + "max_pooling2d_2 (MaxPooling2 (None, 10, 30, 128) 0 \n", + "_________________________________________________________________\n", + "batch_normalization_2 (Batch (None, 10, 30, 128) 512 \n", + "_________________________________________________________________\n", + "conv2d_3 (Conv2D) (None, 10, 30, 256) 819456 \n", + "_________________________________________________________________\n", + "max_pooling2d_3 (MaxPooling2 (None, 5, 15, 256) 0 \n", + "_________________________________________________________________\n", + "flatten_1 (Flatten) (None, 19200) 0 \n", + "_________________________________________________________________\n", + "dropout_1 (Dropout) (None, 19200) 0 \n", + "_________________________________________________________________\n", + "dense_1 (Dense) (None, 64) 1228864 \n", + "_________________________________________________________________\n", + "dropout_2 (Dropout) (None, 64) 0 \n", + "_________________________________________________________________\n", + "dense_2 (Dense) (None, 579) 37635 \n", + "=================================================================\n", + "Total params: 2,296,515\n", + "Trainable params: 2,296,131\n", + "Non-trainable params: 384\n", + "_________________________________________________________________\n" + ] + } + ], + "source": [ + "from keras.models import load_model\n", + "modelName = 'pre_trained_model/base0.h5'\n", + "testModel = load_model(modelName)\n", + "\n", + "testModel.summary()" + ] + }, + { + "cell_type": "code", + "execution_count": 13, + "metadata": { + "scrolled": false + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "5170/5170 [==============================] - 1s 182us/step\n", + "2012-02-04\n", + "- Test score: 1.008079155962518\n", + "- Test accuracy: 82.76595741913904\n", + " \n" + ] + } + ], + "source": [ + "# Load Trained net \n", + "from keras.models import load_model\n", + "modelName = 'model/base0.h5'\n", + "testModel = load_model(modelName)\n", + "\n", + "# Predict \n", + "scores_TEST = testModel.evaluate(X_seen, y_seen, verbose=1, batch_size=1000)\n", + "print(SequenceDate)\n", + "print('- Test score:', scores_TEST[0])\n", + "print('- Test accuracy:', scores_TEST[1]*100)\n", + "print(' ')" + ] + }, + { + "cell_type": "code", + "execution_count": 21, + "metadata": {}, + "outputs": [], + "source": [ + "# save prediction (for later top N analysis )\n", + "y_seen_predicted = testModel.predict(X_seen)\n", + "\n", + "# save \n", + "filename_y_seen_predicted = 'results_predictionvectors/base0/' + SequenceDate + '_seen_predicted'\n", + "np.save(filename_y_seen_predicted, y_seen_predicted)\n", + "\n", + "# save prediction (for later top N analysis )\n", + "X_unseen = np.array(X_unseen)\n", + "y_unseen_predicted = testModel.predict(X_unseen)\n", + "\n", + "# save \n", + "filename_y_unseen_predicted = 'results_predictionvectors/base0/' + SequenceDate + '_unseen_predicted'\n", + "np.save(filename_y_unseen_predicted, y_unseen_predicted)\n" + ] + }, + { + "cell_type": "code", + "execution_count": 72, + "metadata": { + "scrolled": true + }, + "outputs": [], + "source": [ + "### save GT also \n", + "\n", + "# seen \n", + "filename_y_seen_GT = 'results_predictionvectors/base0/' + SequenceDate + '_seen_GT'\n", + "np.save(filename_y_seen_GT, y_seen)\n", + "\n", + "# unseen\n", + "filename_y_unseen_GT = 'results_predictionvectors/base0/' + SequenceDate + '_unseen_GT'\n", + "np.save(filename_y_unseen_GT, y_unseen)\n" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.5.2" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/drawPRcurve_NCLT_SCI.m b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/drawPRcurve_NCLT_SCI.m new file mode 100755 index 0000000..689799c --- /dev/null +++ b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/drawPRcurve_NCLT_SCI.m @@ -0,0 +1,100 @@ +clear + +%% Path info +Dataset = 'NCLT'; +Method = 'SCI'; +% ResultsDir = strcat('Result/', Dataset, '/', Method, '/'); + +ResultsDir = 'Result/'; + +%% Params +FigIdx = 1; +figure(FigIdx); clf; + +TopNindexes = [25]; +nTopNindexes = length(TopNindexes); + +%% Main +SequenceNames = dir(ResultsDir); SequenceNames(1:2, :) = []; SequenceNames = {SequenceNames(:).name}; +nSequences = length(SequenceNames); + +for ithTopN = 1:nTopNindexes + + TopNidx = TopNindexes(ithTopN); + + for ithSeq = 1:nSequences + + % seq info + ithSeqName = SequenceNames{ithSeq}; + ithSeqPath = strcat(ResultsDir, ithSeqName, '/'); + ithSeqPRcurveData = dir(ithSeqPath); ithSeqPRcurveData(1:2, :) = []; ithSeqPRcurveData = {ithSeqPRcurveData(:).name}; + + % load + nCorrectRejectionsAll = load(strcat(ithSeqPath, ithSeqPRcurveData{1})); + nCorrectRejectionsAll = nCorrectRejectionsAll.nCorrectRejections; + nCorrectRejectionsForThisTopN = nCorrectRejectionsAll(TopNidx, :); + + nFalseAlarmsAll = load(strcat(ithSeqPath, ithSeqPRcurveData{2})); + nFalseAlarmsAll = nFalseAlarmsAll.nFalseAlarms; + nFalseAlarmsForThisTopN = nFalseAlarmsAll(TopNidx, :); + + nHitsAll = load(strcat(ithSeqPath, ithSeqPRcurveData{3})); + nHitsAll = nHitsAll.nHits; + nHitsForThisTopN = nHitsAll(TopNidx, :); + + nMissesAll = load(strcat(ithSeqPath, ithSeqPRcurveData{4})); + nMissesAll = nMissesAll.nMisses; + nMissesForThisTopN = nMissesAll(TopNidx, :); + + % info + nTopNs = size(nCorrectRejectionsAll, 1); + nThres = size(nCorrectRejectionsAll, 2); + + % main + Precisions = []; + Recalls = []; + Accuracies = []; + for ithThres = 1:nThres + nCorrectRejections = nCorrectRejectionsForThisTopN(ithThres); + nFalseAlarms = nFalseAlarmsForThisTopN(ithThres); + nHits = nHitsForThisTopN(ithThres); + nMisses = nMissesForThisTopN(ithThres); + + nTotalTestPlaces = nCorrectRejections + nFalseAlarms + nHits + nMisses; + + Precision = nHits / (nHits + nFalseAlarms); + Recall = nHits / (nHits + nMisses); + Acc = (nHits + nCorrectRejections)/nTotalTestPlaces; + + Precisions = [Precisions; Precision]; + Recalls = [Recalls; Recall]; + Accuracies = [Accuracies; Acc]; + end + + % draw + figure(FigIdx); + plot(Recalls, Precisions, 'LineWidth', 2); % commonly x axis is recall + title('SCI at NCLT'); + xlabel('Recall'); ylabel('Precision'); + % axis equal; + xlim([0, 1]); ylim([0,1]); + grid on; grid minor; + hold on; + + end + + lgd = legend(SequenceNames, 'Location', 'best'); + lgd.FontSize = 9; + lgd.FontWeight = 'bold'; + + + %% save + % fileName = strcat('./results/', testDate, '_PRcurveWithEntropyThresVarying.png'); + % saveas(gcf, fileName) + % + % fileName = strcat('./results/', testDate, '_EntireWorkSpace.mat'); + % save(fileName) + +end + + diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/makeDataForPRcurveForSCIresult.m b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/makeDataForPRcurveForSCIresult.m new file mode 100755 index 0000000..b8e624d --- /dev/null +++ b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/makeDataForPRcurveForSCIresult.m @@ -0,0 +1,142 @@ +clear +addpath(genpath('./')); + +%% Setup +Dataset = 'NCLT'; +Method = 'LearningSCI'; +ResultDir = strcat('/media/gskim/IRAP-ADV1/Data/ICRA2019/Experiments/#. trunk/', Dataset, '/', Method, '/10m/results_predictionvectors/base0/'); + +NCLTTestDateNames = {'2012-02-04', '2012-03-17', '2012-05-26', '2012-06-15', '2012-08-20', '2012-09-28', '2012-10-28', '2012-11-16', '2013-02-23', '2013-04-05'}; +nNCLTTestDates = length(NCLTTestDateNames); + + +%% Main +for ithDate = 1:nNCLTTestDates + + TestDateName = NCLTTestDateNames{ithDate}; + + % path + SeenGTs_Path = strcat(ResultDir, TestDateName, '_seen_GT.npy'); + SeenPredictions_Path = strcat(ResultDir, TestDateName, '_seen_predicted.npy'); + + UnseenGTs_Path = strcat(ResultDir, TestDateName, '_unseen_GT.npy'); + UnseenPredictions_Path = strcat(ResultDir, TestDateName, '_unseen_predicted.npy'); + + % load + SeenGTs = double(readNPY(SeenGTs_Path)); + SeenPredictions = double(readNPY(SeenPredictions_Path)); + UnseenGTs = double(readNPY(UnseenGTs_Path)); + UnseenPredictions = double(readNPY(UnseenPredictions_Path)); + + % info + nSeenPlaces = size(SeenGTs, 1); + nUnseenPlaces = size(UnseenGTs, 1); + + % concate seen and unseen for convenience +% TotalGTs = [SeenGTs; UnseenGTs]; + TotalPredictions = [SeenPredictions; UnseenPredictions]; + TotalSeenFlags = [ ones(nSeenPlaces, 1); zeros(nUnseenPlaces, 1)]; % seen (1) or unseen (0) + nTotalTestPlaces = nSeenPlaces + nUnseenPlaces; + nTestData = nTotalTestPlaces; + + + %% Main + + % policy (top N) + TopN = 25; + TopNs = linspace(1, TopN, TopN); + nTopNs = length(TopNs); + + % Entropy thresholds + Thresholds = linspace(0, 1, 200); + nThresholds = length(Thresholds); + + % Main variables to store the result for drawing PR curve + nHits = zeros(nTopNs, nThresholds); + nFalseAlarms = zeros(nTopNs, nThresholds); + nCorrectRejections = zeros(nTopNs, nThresholds); + nMisses = zeros(nTopNs, nThresholds); + + for ith=1:nTestData + tic + + % Flag: Seen(1) or Unseen(0) + SeenOrUnseen = TotalSeenFlags(ith); + + % GT place idx (in this code, use it only for the seen case. thus knowing unseen place index is unnecessary in this case for PR curve analysis + if(SeenOrUnseen == 1) + [dummy, ithTruthPlaceIdx] = max(SeenGTs(ith, :)); % but this idx is not the real place index, plz refer the scikitlearn label maker + else + ithTruthPlaceIdx = nan; + end + + % ith prediction vector + ithPrediction = TotalPredictions(ith, :); + + % entropy + EntropyOfPrediction = NormalizedEntropyOfVector(ithPrediction); % Entropy for module 1 (new place detection) + + % top N predictions + [NearestSoftmaxOutputs, idxs] = sort(ithPrediction, 'descend'); + NearestPlaceIdxs = idxs(1:TopN); % as mentioned above: this idx is not the real place index, plz refer the scikitlearn label maker + + + for ithTopN = 1:nTopNs + + ithNearestPlaceIdxs = NearestPlaceIdxs(1:ithTopN); + + for ithThres = 1:nThresholds + + ithDistThreshold = Thresholds(ithThres); + + % main + if(EntropyOfPrediction >= ithDistThreshold) + % if over the theshold, it is considered unseen. + % for unseen considered, no step 2 (recognition of place index), and just quit now. + if(SeenOrUnseen == 0) + % TN: Correct Rejection + nCorrectRejections(ithTopN, ithThres) = nCorrectRejections(ithTopN, ithThres) + 1; + else + % FN: MISS + nMisses(ithTopN, ithThres) = nMisses(ithTopN, ithThres) + 1; + end + else + % if under the theshold, it is considered seen. + % and then check the correctness + if( ismember(ithTruthPlaceIdx, ithNearestPlaceIdxs) ) + % TP: Hit + nHits(ithTopN, ithThres) = nHits(ithTopN, ithThres) + 1; + else + % FP: False Alarm + nFalseAlarms(ithTopN, ithThres) = nFalseAlarms(ithTopN, ithThres) + 1; + end + end + + end + + end + + message = strcat(num2str(ith), "/", num2str(nTestData), " of ", TestDateName); + disp(message); + toc + + end + + + % save the results (PR curve is for the later.) + savePath = strcat('Result/', TestDateName, '/'); + if(~(7==exist(savePath,'dir'))) + mkdir(savePath); + end + save(strcat(savePath, 'nCorrectRejections.mat'), 'nCorrectRejections'); + save(strcat(savePath, 'nMisses.mat'), 'nMisses'); + save(strcat(savePath, 'nHits.mat'), 'nHits'); + save(strcat(savePath, 'nFalseAlarms.mat'), 'nFalseAlarms'); + +end + + + + + + diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-02-04/nCorrectRejections.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-02-04/nCorrectRejections.mat new file mode 100755 index 0000000..4925c37 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-02-04/nCorrectRejections.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-02-04/nFalseAlarms.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-02-04/nFalseAlarms.mat new file mode 100755 index 0000000..c1ea8e9 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-02-04/nFalseAlarms.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-02-04/nHits.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-02-04/nHits.mat new file mode 100755 index 0000000..481bdb7 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-02-04/nHits.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-02-04/nMisses.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-02-04/nMisses.mat new file mode 100755 index 0000000..173cad1 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-02-04/nMisses.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-03-17/nCorrectRejections.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-03-17/nCorrectRejections.mat new file mode 100755 index 0000000..1db6f20 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-03-17/nCorrectRejections.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-03-17/nFalseAlarms.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-03-17/nFalseAlarms.mat new file mode 100755 index 0000000..c262b8b Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-03-17/nFalseAlarms.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-03-17/nHits.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-03-17/nHits.mat new file mode 100755 index 0000000..1944e3d Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-03-17/nHits.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-03-17/nMisses.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-03-17/nMisses.mat new file mode 100755 index 0000000..f2389bd Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-03-17/nMisses.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-05-26/nCorrectRejections.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-05-26/nCorrectRejections.mat new file mode 100755 index 0000000..cccd9e5 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-05-26/nCorrectRejections.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-05-26/nFalseAlarms.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-05-26/nFalseAlarms.mat new file mode 100755 index 0000000..3c44336 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-05-26/nFalseAlarms.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-05-26/nHits.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-05-26/nHits.mat new file mode 100755 index 0000000..b917103 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-05-26/nHits.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-05-26/nMisses.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-05-26/nMisses.mat new file mode 100755 index 0000000..9ed9b90 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-05-26/nMisses.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-06-15/nCorrectRejections.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-06-15/nCorrectRejections.mat new file mode 100755 index 0000000..028ffe4 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-06-15/nCorrectRejections.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-06-15/nFalseAlarms.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-06-15/nFalseAlarms.mat new file mode 100755 index 0000000..b61821f Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-06-15/nFalseAlarms.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-06-15/nHits.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-06-15/nHits.mat new file mode 100755 index 0000000..827780d Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-06-15/nHits.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-06-15/nMisses.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-06-15/nMisses.mat new file mode 100755 index 0000000..3b2bffc Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-06-15/nMisses.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-08-20/nCorrectRejections.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-08-20/nCorrectRejections.mat new file mode 100755 index 0000000..c02f316 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-08-20/nCorrectRejections.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-08-20/nFalseAlarms.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-08-20/nFalseAlarms.mat new file mode 100755 index 0000000..1aa3202 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-08-20/nFalseAlarms.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-08-20/nHits.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-08-20/nHits.mat new file mode 100755 index 0000000..191a63a Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-08-20/nHits.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-08-20/nMisses.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-08-20/nMisses.mat new file mode 100755 index 0000000..09124b0 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-08-20/nMisses.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-09-28/nCorrectRejections.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-09-28/nCorrectRejections.mat new file mode 100755 index 0000000..2400539 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-09-28/nCorrectRejections.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-09-28/nFalseAlarms.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-09-28/nFalseAlarms.mat new file mode 100755 index 0000000..a0f0f60 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-09-28/nFalseAlarms.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-09-28/nHits.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-09-28/nHits.mat new file mode 100755 index 0000000..6e7de1a Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-09-28/nHits.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-09-28/nMisses.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-09-28/nMisses.mat new file mode 100755 index 0000000..36a4375 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-09-28/nMisses.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-10-28/nCorrectRejections.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-10-28/nCorrectRejections.mat new file mode 100755 index 0000000..e11cd66 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-10-28/nCorrectRejections.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-10-28/nFalseAlarms.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-10-28/nFalseAlarms.mat new file mode 100755 index 0000000..247f4b0 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-10-28/nFalseAlarms.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-10-28/nHits.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-10-28/nHits.mat new file mode 100755 index 0000000..4a5d3f3 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-10-28/nHits.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-10-28/nMisses.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-10-28/nMisses.mat new file mode 100755 index 0000000..16f9153 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-10-28/nMisses.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-11-16/nCorrectRejections.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-11-16/nCorrectRejections.mat new file mode 100755 index 0000000..330d001 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-11-16/nCorrectRejections.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-11-16/nFalseAlarms.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-11-16/nFalseAlarms.mat new file mode 100755 index 0000000..99c6cb0 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-11-16/nFalseAlarms.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-11-16/nHits.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-11-16/nHits.mat new file mode 100755 index 0000000..71100cf Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-11-16/nHits.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-11-16/nMisses.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-11-16/nMisses.mat new file mode 100755 index 0000000..21cefff Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2012-11-16/nMisses.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2013-02-23/nCorrectRejections.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2013-02-23/nCorrectRejections.mat new file mode 100755 index 0000000..51dd321 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2013-02-23/nCorrectRejections.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2013-02-23/nFalseAlarms.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2013-02-23/nFalseAlarms.mat new file mode 100755 index 0000000..96b7001 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2013-02-23/nFalseAlarms.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2013-02-23/nHits.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2013-02-23/nHits.mat new file mode 100755 index 0000000..2b3fbd2 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2013-02-23/nHits.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2013-02-23/nMisses.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2013-02-23/nMisses.mat new file mode 100755 index 0000000..2ca2d4c Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2013-02-23/nMisses.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2013-04-05/nCorrectRejections.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2013-04-05/nCorrectRejections.mat new file mode 100755 index 0000000..a228241 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2013-04-05/nCorrectRejections.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2013-04-05/nFalseAlarms.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2013-04-05/nFalseAlarms.mat new file mode 100755 index 0000000..7c48989 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2013-04-05/nFalseAlarms.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2013-04-05/nHits.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2013-04-05/nHits.mat new file mode 100755 index 0000000..4743791 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2013-04-05/nHits.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2013-04-05/nMisses.mat b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2013-04-05/nMisses.mat new file mode 100755 index 0000000..2594a0a Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/result/2013-04-05/nMisses.mat differ diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/src/EntropyOfVector.m b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/src/EntropyOfVector.m new file mode 100755 index 0000000..9571468 --- /dev/null +++ b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/src/EntropyOfVector.m @@ -0,0 +1,16 @@ +function H = EntropyOfVector(vec) + +len = length(vec); + +H = 0; +for ith=1:len + pith = vec(ith); + if(pith == 0) + H = H + 0; % 0log0 = 0 + else + H = H + ( -1 * (pith*log2(pith)) ); + end +end + +end + diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/src/NormalizedEntropyOfVector.m b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/src/NormalizedEntropyOfVector.m new file mode 100755 index 0000000..f1d0bfa --- /dev/null +++ b/example/longterm_localization/NCLT/2012-01-15/3_Inference/PRcurve/src/NormalizedEntropyOfVector.m @@ -0,0 +1,22 @@ +function NormalizedH = NormalizedEntropyOfVector(vec) + +len = length(vec); + +% max entropy +maxEntropy = -len*(1/len * log2(1/len)); + +H = 0; +for ith=1:len + pith = vec(ith); + if(pith == 0) + H = H + 0; % 0log0 = 0 + else + H = H + ( -1 * (pith*log2(pith)) ); + end +end + +% return +NormalizedH = H/maxEntropy; + +end + diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/inference.ipynb b/example/longterm_localization/NCLT/2012-01-15/3_Inference/inference.ipynb new file mode 100755 index 0000000..bbacf87 --- /dev/null +++ b/example/longterm_localization/NCLT/2012-01-15/3_Inference/inference.ipynb @@ -0,0 +1,303 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [], + "source": [ + "import os\n", + "import numpy as np \n", + "import pandas as pd \n", + "import tensorflow as tf\n", + "import keras\n", + "import matplotlib.pyplot as plt" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "# Data info \n", + "rootDir = '..your_path'\n", + "\n", + "Dataset = 'NCLT'\n", + "TrainOrTest = '/Test/'\n", + "SequenceDate = '2013-04-05'\n", + "\n", + "SCImiddlePath = '/4. SCI_jet0to15/'\n", + "\n", + "GridCellSize = '10'\n", + "\n", + "DataPath = rootDir + Dataset + TrainOrTest + SequenceDate + SCImiddlePath + GridCellSize + '/'\n", + "print(DataPath)" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], + "source": [ + "DataPath = ICRArootDir + Dataset + TrainOrTest + SequenceDate + SCImiddlePath + GridCellSize + '/'\n", + "print(DataPath)" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [], + "source": [ + "def getTestDataNCLT(DataPath, SequenceDate, lbl_enc_train): \n", + "\n", + " # load training label encoding information \n", + " from sklearn.preprocessing import LabelEncoder\n", + " ClassesTrainingSequenceHave = lbl_enc_train.classes_\n", + " nClassesTrainingSequenceHave = len(ClassesTrainingSequenceHave)\n", + "\n", + " # info\n", + " WholeData = os.listdir(DataPath)\n", + " nWholeData = len(WholeData)\n", + " print(str(nWholeData) + ' data exist in ' + SequenceDate)\n", + " \n", + " # read \n", + " X_seen = []\n", + " y_seen = []\n", + " X_unseen = []\n", + " y_unseen = []\n", + " \n", + " for ii in range(nWholeData):\n", + " dataName = WholeData[ii]\n", + " dataPath = DataPath + dataName\n", + " \n", + " dataTrajNodeOrder = int(dataName[0:5])\n", + "\n", + " SCI = plt.imread(dataPath)\n", + " dataPlaceIndex = int(dataName[6:11])\n", + " \n", + " # if label is in the train, then save into the seen (seen is only subset to be tested)\n", + " if dataPlaceIndex in ClassesTrainingSequenceHave:\n", + " X_seen.append(SCI)\n", + " y_seen.append(dataPlaceIndex)\n", + " else:\n", + " X_unseen.append(SCI)\n", + " y_unseen.append(dataPlaceIndex)\n", + " \n", + " # progress message \n", + " if ii%1000==0:\n", + " print(str(format((ii/nWholeData)*100, '.1f')), '% loaded.')\n", + " \n", + " dataShape = SCI.shape\n", + " \n", + " # X\n", + " nSeenData = len(X_seen)\n", + " X_nd = np.zeros(shape=(nSeenData, dataShape[0], dataShape[1], dataShape[2]))\n", + " for jj in range(nSeenData):\n", + " X_nd[jj, :, :] = X_seen[jj]\n", + " X_nd = X_nd.astype('float32')\n", + " \n", + " # y (one-hot encoded) \n", + " y_seen = lbl_enc_train.transform(y_seen)\n", + " y_nd = keras.utils.np_utils.to_categorical(y_seen, num_classes=nClassesTrainingSequenceHave)\n", + "\n", + " # log message \n", + " print('Data size: %s' % nWholeData)\n", + " print('- Seen data: %s' % len(X_seen))\n", + " print('- Uneen data: %s' % len(X_unseen))\n", + " print(' ')\n", + " print('Data shape:', X_nd.shape)\n", + " print('Label shape:', y_nd.shape)\n", + " \n", + " return X_nd, y_nd, X_unseen, y_unseen\n", + " " + ] + }, + { + "cell_type": "code", + "execution_count": 63, + "metadata": {}, + "outputs": [], + "source": [ + "# load training label encoding information for discriminate seen/unseen of test \n", + "import pickle\n", + "TrainingDate = '2012-01-15'\n", + "TrainingDataPath = 'data_pickle/Train_' + TrainingDate + '_SCI_color.pkl'\n", + "\n", + "with open(TrainingDataPath, 'rb') as f: # Python 3: open(..., 'rb')\n", + " X_train, y_train, lbl_enc_train = pickle.load(f)" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [], + "source": [ + "# Load test data using training label encoder information \n", + "[X_seen, y_seen, X_unseen, y_unseen] = getTestDataNCLT(DataPath, SequenceDate, lbl_enc_train)" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "/home/gskim/anaconda3/envs/tfkeras/lib/python3.5/site-packages/h5py/__init__.py:36: FutureWarning: Conversion of the second argument of issubdtype from `float` to `np.floating` is deprecated. In future, it will be treated as `np.float64 == np.dtype(float).type`.\n", + " from ._conv import register_converters as _register_converters\n", + "Using TensorFlow backend.\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "_________________________________________________________________\n", + "Layer (type) Output Shape Param # \n", + "=================================================================\n", + "input_1 (InputLayer) (None, 40, 120, 3) 0 \n", + "_________________________________________________________________\n", + "conv2d_1 (Conv2D) (None, 40, 120, 64) 4864 \n", + "_________________________________________________________________\n", + "max_pooling2d_1 (MaxPooling2 (None, 20, 60, 64) 0 \n", + "_________________________________________________________________\n", + "batch_normalization_1 (Batch (None, 20, 60, 64) 256 \n", + "_________________________________________________________________\n", + "conv2d_2 (Conv2D) (None, 20, 60, 128) 204928 \n", + "_________________________________________________________________\n", + "max_pooling2d_2 (MaxPooling2 (None, 10, 30, 128) 0 \n", + "_________________________________________________________________\n", + "batch_normalization_2 (Batch (None, 10, 30, 128) 512 \n", + "_________________________________________________________________\n", + "conv2d_3 (Conv2D) (None, 10, 30, 256) 819456 \n", + "_________________________________________________________________\n", + "max_pooling2d_3 (MaxPooling2 (None, 5, 15, 256) 0 \n", + "_________________________________________________________________\n", + "flatten_1 (Flatten) (None, 19200) 0 \n", + "_________________________________________________________________\n", + "dropout_1 (Dropout) (None, 19200) 0 \n", + "_________________________________________________________________\n", + "dense_1 (Dense) (None, 64) 1228864 \n", + "_________________________________________________________________\n", + "dropout_2 (Dropout) (None, 64) 0 \n", + "_________________________________________________________________\n", + "dense_2 (Dense) (None, 579) 37635 \n", + "=================================================================\n", + "Total params: 2,296,515\n", + "Trainable params: 2,296,131\n", + "Non-trainable params: 384\n", + "_________________________________________________________________\n" + ] + } + ], + "source": [ + "from keras.models import load_model\n", + "modelName = 'pre_trained_model/base0.h5'\n", + "testModel = load_model(modelName)\n", + "\n", + "testModel.summary()" + ] + }, + { + "cell_type": "code", + "execution_count": 13, + "metadata": { + "scrolled": false + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "5170/5170 [==============================] - 1s 182us/step\n", + "2012-02-04\n", + "- Test score: 1.008079155962518\n", + "- Test accuracy: 82.76595741913904\n", + " \n" + ] + } + ], + "source": [ + "# Load Trained net \n", + "from keras.models import load_model\n", + "modelName = 'model/base0.h5'\n", + "testModel = load_model(modelName)\n", + "\n", + "# Predict \n", + "scores_TEST = testModel.evaluate(X_seen, y_seen, verbose=1, batch_size=1000)\n", + "print(SequenceDate)\n", + "print('- Test score:', scores_TEST[0])\n", + "print('- Test accuracy:', scores_TEST[1]*100)\n", + "print(' ')" + ] + }, + { + "cell_type": "code", + "execution_count": 21, + "metadata": {}, + "outputs": [], + "source": [ + "# save prediction (for later top N analysis )\n", + "y_seen_predicted = testModel.predict(X_seen)\n", + "\n", + "# save \n", + "filename_y_seen_predicted = 'results_predictionvectors/base0/' + SequenceDate + '_seen_predicted'\n", + "np.save(filename_y_seen_predicted, y_seen_predicted)\n", + "\n", + "# save prediction (for later top N analysis )\n", + "X_unseen = np.array(X_unseen)\n", + "y_unseen_predicted = testModel.predict(X_unseen)\n", + "\n", + "# save \n", + "filename_y_unseen_predicted = 'results_predictionvectors/base0/' + SequenceDate + '_unseen_predicted'\n", + "np.save(filename_y_unseen_predicted, y_unseen_predicted)\n" + ] + }, + { + "cell_type": "code", + "execution_count": 72, + "metadata": { + "scrolled": true + }, + "outputs": [], + "source": [ + "### save GT also \n", + "\n", + "# seen \n", + "filename_y_seen_GT = 'results_predictionvectors/base0/' + SequenceDate + '_seen_GT'\n", + "np.save(filename_y_seen_GT, y_seen)\n", + "\n", + "# unseen\n", + "filename_y_unseen_GT = 'results_predictionvectors/base0/' + SequenceDate + '_unseen_GT'\n", + "np.save(filename_y_unseen_GT, y_unseen)\n" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.5.2" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/example/longterm_localization/NCLT/2012-01-15/3_Inference/pre_trained_model/base0.h5 b/example/longterm_localization/NCLT/2012-01-15/3_Inference/pre_trained_model/base0.h5 new file mode 100755 index 0000000..6669c79 Binary files /dev/null and b/example/longterm_localization/NCLT/2012-01-15/3_Inference/pre_trained_model/base0.h5 differ diff --git a/example/longterm_localization/sci_pipeline.png b/example/longterm_localization/sci_pipeline.png new file mode 100644 index 0000000..9950598 Binary files /dev/null and b/example/longterm_localization/sci_pipeline.png differ diff --git a/example/place_recognition/KITTI/00/DistBtn2Dpose.m b/example/place_recognition/KITTI/00/DistBtn2Dpose.m new file mode 100755 index 0000000..ee83fcf --- /dev/null +++ b/example/place_recognition/KITTI/00/DistBtn2Dpose.m @@ -0,0 +1,3 @@ +function dist = DistBtn2Dpose(pose1, pose2) + dist = sqrt( (pose1(1) - pose2(1))^2 + (pose1(2) - pose2(2))^2); +end diff --git a/example/place_recognition/KITTI/00/GTposes.mat b/example/place_recognition/KITTI/00/GTposes.mat new file mode 100755 index 0000000..64b3948 Binary files /dev/null and b/example/place_recognition/KITTI/00/GTposes.mat differ diff --git a/example/place_recognition/KITTI/00/KITTIbin2PtcloudWithIndex.m b/example/place_recognition/KITTI/00/KITTIbin2PtcloudWithIndex.m new file mode 100755 index 0000000..844dc0c --- /dev/null +++ b/example/place_recognition/KITTI/00/KITTIbin2PtcloudWithIndex.m @@ -0,0 +1,21 @@ +function ptcloud = KITTIbin2PtcloudWithIndex(base_dir, index) + +%% File path +if( length(num2str(index)) == 4 ) + bin_path = strcat(base_dir, '00', num2str(index), '.bin'); +elseif (length(num2str(index)) == 3) + bin_path = strcat(base_dir, '000', num2str(index), '.bin'); +elseif (length(num2str(index)) == 2) + bin_path = strcat(base_dir, '0000', num2str(index), '.bin'); +elseif (length(num2str(index)) == 1) + bin_path = strcat(base_dir, '00000', num2str(index), '.bin'); +end + +%% Read +fid = fopen(bin_path, 'rb'); raw_data = fread(fid, [4 inf], 'single'); fclose(fid); +points = raw_data(1:3,:)'; +points(:, 3) = points(:, 3) + 1.9; % z in car coord. + +ptcloud = pointCloud(points); + +end % end of function diff --git a/example/place_recognition/KITTI/00/PlaceRecognizer.m b/example/place_recognition/KITTI/00/PlaceRecognizer.m new file mode 100755 index 0000000..a6f8446 --- /dev/null +++ b/example/place_recognition/KITTI/00/PlaceRecognizer.m @@ -0,0 +1,105 @@ +clear; clc; +addpath(genpath('../../../../matlab/')); + +%% Change this to your path +% vel_dir = '/media/gskim/Data/KITTI odo/data_odometry_velodyne/dataset/sequences/00/velodyne/'; +vel_dir = 'your_pointcloud_files_path'; + + +%% Params +% below 3 parameters: same setting as the original paper (G. Kim, 18 IROS) +max_range = 80; % meter +num_sectors = 60; +num_rings = 20; + +num_candidates = 50; % means Scan Context-50 in the paper. + +loop_thres = 0.2; % ### this is a user parameter ### + +num_enough_node_diff = 50; + + +%% Main +load('GTposes.mat'); + +ringkeys = []; +scancontexts = {}; + +loop_log = []; +num_nodes = length(GTposes); +for ith_node = 1:num_nodes-1 + + % information + idx_query = ith_node; + ptcloud = KITTIbin2PtcloudWithIndex(vel_dir, idx_query); + + sc_query = Ptcloud2ScanContext(ptcloud, num_sectors, num_rings, max_range); + scancontexts{end+1} = sc_query; % save into database + + % ringkey tree + ringkey = ScanContext2RingKey(sc_query); + ringkeys = [ringkeys; ringkey]; + tree = createns(ringkeys, 'NSMethod', 'kdtree'); % Create object to use in k-nearest neighbor search + + % try loop-detection after enough moving + if(ith_node < num_candidates) + continue; + end + + % find nearest candidates + candidates = knnsearch(tree, ringkey, 'K', num_candidates); + + % check dist btn candidates is lower than the given threshold. + idx_nearest = 0; + min_dist = inf; % initialization + for ith_candidate = 1:length(candidates) + + idx_candidate = candidates(ith_candidate); + sc_candidate = scancontexts{idx_candidate}; + + % skip condition: do not check nearest measurements + if( abs(idx_query - idx_candidate) < num_enough_node_diff) + continue; + end + + % Main + distance_to_query = DistanceBtnScanContexts(sc_query, sc_candidate); + if( distance_to_query > loop_thres) + continue; + end + + if( distance_to_query < min_dist) + idx_nearest = idx_candidate; + min_dist = distance_to_query; + end + + end + + % log the result + if( ~isequal(min_dist, inf)) % that is, when any loop (satisfied under acceptance theshold) occured. + pose_dist_real = DistBtn2Dpose(GTposes(idx_query,:), GTposes(idx_nearest,:)); + loop_log = [loop_log; ... + idx_query, idx_nearest, min_dist, pose_dist_real]; + end + + %% Log Message: procedure and LoopFound Event + if( rem(idx_query, 100) == 0) + disp( strcat(num2str(idx_query), '.bin processed') ); + end + if( ~isequal(min_dist, inf) ) + disp( strcat("Loop found: ", num2str(idx_query), " <-> ", num2str(idx_nearest), " with dist ", num2str(min_dist))); + end + +end + + +%% Save the result into csv file +save_dir = strcat('./result/', num2str(num_candidates),'/', num2str(loop_thres), '/'); +save_file_name = strcat(save_dir, '/LogLoopFound.csv'); +if( ~exist(save_dir)) + mkdir(save_dir) +end +csvwrite(save_file_name, loop_log); + + + diff --git a/example/place_recognition/KITTI/00/result/50/0.2/LogLoopFound.csv b/example/place_recognition/KITTI/00/result/50/0.2/LogLoopFound.csv new file mode 100755 index 0000000..b611beb --- /dev/null +++ b/example/place_recognition/KITTI/00/result/50/0.2/LogLoopFound.csv @@ -0,0 +1,757 @@ +1568,119,0.18125,2.3459 +1569,121,0.15028,2.0697 +1570,123,0.18087,1.8398 +1571,123,0.17516,1.7656 +1572,126,0.1617,1.6084 +1573,126,0.12506,1.5125 +1574,126,0.13887,1.6024 +1575,129,0.14479,1.2933 +1576,130,0.11332,1.2109 +1577,131,0.10118,1.1562 +1578,132,0.11642,1.1173 +1579,133,0.14223,1.0887 +1580,134,0.13837,1.0587 +1581,136,0.15035,0.96174 +1582,136,0.13031,1.0202 +1583,138,0.14318,0.84406 +1584,140,0.1356,1.0254 +1585,140,0.1024,0.75281 +1586,141,0.11191,0.71942 +1587,142,0.11754,0.68515 +1588,143,0.1062,0.6584 +1589,144,0.098269,0.62726 +1590,145,0.093211,0.60696 +1591,146,0.088597,0.59144 +1592,147,0.11178,0.58298 +1593,148,0.088569,0.58158 +1594,149,0.090449,0.5925 +1595,150,0.075444,0.6126 +1596,151,0.071824,0.64588 +1597,152,0.091617,0.68911 +1598,153,0.082827,0.73222 +1599,154,0.11145,0.78655 +1600,156,0.10101,0.51769 +1601,156,0.085753,0.89851 +1602,157,0.074695,0.95616 +1603,159,0.060621,0.57845 +1604,160,0.036375,0.59372 +1605,161,0.030381,0.60652 +1606,162,0.048665,0.61569 +1607,163,0.062883,0.61811 +1608,164,0.043436,0.61427 +1609,165,0.090096,0.60787 +1610,166,0.11956,0.60152 +1611,167,0.10092,0.59354 +1612,168,0.12087,0.59199 +1613,170,0.098452,0.61371 +1614,170,0.11376,0.59625 +1615,172,0.10143,0.51073 +1616,172,0.10065,0.63149 +1617,174,0.10071,0.40156 +1618,175,0.066769,0.35817 +1619,176,0.088933,0.33 +1620,177,0.11702,0.34074 +1621,178,0.10745,0.39436 +1622,180,0.13317,0.57002 +1623,181,0.13514,0.49705 +1624,182,0.13071,0.47461 +1625,183,0.12965,0.52818 +1626,185,0.12495,0.67878 +1627,186,0.10545,0.63886 +1628,187,0.12124,0.6919 +1629,189,0.092797,0.73359 +1630,191,0.093877,0.8266 +1631,193,0.11648,0.88208 +1632,194,0.088575,0.62795 +1633,196,0.059866,0.54343 +1634,198,0.056973,0.49308 +1635,199,0.11314,0.12862 +1636,201,0.14971,0.59225 +1637,200,0.19476,1.3008 +2442,390,0.16356,1.4154 +2443,390,0.14665,1.0374 +2444,391,0.14361,0.82121 +2445,392,0.16731,0.65271 +2446,393,0.16125,0.51617 +2447,394,0.12566,0.40868 +2448,395,0.17469,0.34193 +2449,396,0.092845,0.29563 +2450,397,0.10571,0.25573 +2451,397,0.17995,0.44304 +2452,398,0.19878,0.50376 +2453,400,0.19093,0.12883 +2454,401,0.1413,0.13343 +2455,403,0.088901,0.46873 +2456,404,0.095194,0.32575 +2457,405,0.16198,0.18097 +2458,408,0.19245,0.9109 +2459,409,0.11232,0.65213 +2460,410,0.15126,0.38529 +2461,412,0.094598,0.62195 +2462,412,0.1364,0.30654 +2463,414,0.17397,0.50585 +3285,2347,0.19181,1.7119 +3286,2348,0.18886,1.4355 +3287,2348,0.19508,1.3702 +3288,2350,0.15239,1.0513 +3289,2350,0.15637,0.82514 +3290,2351,0.10586,0.66724 +3291,2351,0.13678,0.69258 +3292,2352,0.13644,0.45817 +3293,2353,0.087613,0.31482 +3294,2353,0.15271,0.57803 +3295,2354,0.10603,0.35358 +3296,2355,0.045707,0.15249 +3297,2356,0.083127,0.1477 +3298,2356,0.12741,0.53191 +3299,2357,0.15497,0.3257 +3300,2358,0.14542,0.11591 +3301,2359,0.13331,0.082783 +3302,2359,0.10406,0.63607 +3303,2360,0.092642,0.46669 +3304,2361,0.094625,0.30149 +3305,2362,0.081233,0.14712 +3306,2363,0.071525,0.01429 +3307,2364,0.079,0.14063 +3308,2365,0.10557,0.28989 +3309,2365,0.086534,0.559 +3310,2366,0.078621,0.44814 +3311,2367,0.045815,0.35524 +3312,2368,0.055051,0.27569 +3313,2369,0.059123,0.22411 +3314,2370,0.066648,0.20413 +3315,2371,0.079022,0.21808 +3316,2372,0.081073,0.2491 +3317,2373,0.084914,0.29572 +3318,2374,0.1,0.34206 +3319,2375,0.090124,0.39653 +3320,2376,0.098208,0.44958 +3321,2377,0.085425,0.49533 +3322,2378,0.074547,0.53816 +3323,2379,0.08426,0.58167 +3324,2380,0.08894,0.62385 +3325,2381,0.078235,0.66161 +3326,2382,0.091711,0.69853 +3327,2383,0.058422,0.72976 +3328,2384,0.066054,0.74093 +3329,2385,0.064545,0.74904 +3330,2386,0.067067,0.73265 +3331,2387,0.061414,0.71872 +3332,2388,0.090268,0.70308 +3333,2389,0.078915,0.69906 +3334,2390,0.083742,0.69499 +3335,2391,0.058813,0.67748 +3336,2392,0.089391,0.66885 +3337,2393,0.052395,0.66004 +3338,2394,0.066151,0.65691 +3339,2395,0.080086,0.65148 +3340,2396,0.059779,0.65392 +3341,2397,0.062375,0.65045 +3342,2398,0.073409,0.64607 +3343,2399,0.071161,0.64021 +3344,2400,0.050122,0.64122 +3345,2401,0.038577,0.6421 +3346,2402,0.054643,0.65593 +3347,2403,0.068992,0.66919 +3348,2404,0.062084,0.70059 +3349,2405,0.068393,0.72263 +3350,2406,0.094639,0.75104 +3351,2407,0.083902,0.77198 +3352,2408,0.08078,0.79738 +3353,2409,0.085166,0.81228 +3354,2410,0.079796,0.83729 +3355,2411,0.070817,0.85167 +3356,2412,0.099746,0.84575 +3357,2413,0.078259,0.83719 +3358,2414,0.087562,0.83061 +3359,2415,0.09848,0.8226 +3360,2416,0.080679,0.81034 +3361,2416,0.087786,1.0043 +3362,2417,0.085017,0.93943 +3363,2418,0.077789,0.85353 +3364,2419,0.098126,0.78161 +3365,2420,0.11023,0.70018 +3366,2420,0.11876,0.9617 +3367,2421,0.089966,0.84369 +3368,2422,0.1494,0.73867 +3369,2422,0.11137,1.1043 +3370,2423,0.11026,0.99525 +3371,2424,0.089247,0.89416 +3372,2425,0.10631,0.78985 +3373,2425,0.11679,1.1914 +3374,2426,0.091945,1.0832 +3375,2427,0.086268,0.97989 +3376,2428,0.049368,0.88783 +3377,2429,0.072314,0.80638 +3378,2430,0.052582,0.69987 +3379,2431,0.094895,0.59817 +3380,2431,0.10076,1.0012 +3381,2432,0.1007,0.9295 +3382,2433,0.049914,0.85069 +3383,2434,0.045934,0.74765 +3384,2435,0.069366,0.69214 +3385,2436,0.066831,0.61126 +3386,2437,0.096679,0.53902 +3387,2438,0.097736,0.45893 +3388,2438,0.099658,0.96359 +3389,2439,0.088638,0.91281 +3390,2440,0.081316,0.88109 +3391,2441,0.079838,0.78725 +3392,2442,0.12346,0.70026 +3393,2443,0.087868,0.62774 +3394,2443,0.10694,1.1528 +3395,2444,0.0867,1.0204 +3396,2445,0.08955,0.87821 +3397,2446,0.086738,0.74263 +3398,2447,0.097434,0.58631 +3399,2447,0.067854,1.094 +3400,2448,0.054073,0.94282 +3401,2449,0.082468,0.78407 +3402,396,0.078349,0.9965 +3403,2450,0.10656,1.1245 +3404,2451,0.10494,0.96157 +3405,2452,0.089806,0.78443 +3406,2453,0.11585,0.60537 +3407,2453,0.082687,1.1265 +3408,2454,0.070972,0.94199 +3409,2455,0.083759,0.75355 +3410,2455,0.070209,1.285 +3411,2456,0.10177,1.098 +3412,405,0.12,0.89414 +3413,406,0.07472,0.91874 +3414,407,0.12934,0.94777 +3415,2459,0.095186,1.0739 +3416,409,0.14754,1.0207 +3417,411,0.12847,0.60378 +3418,2461,0.11527,1.2465 +3419,413,0.10909,0.69339 +3420,414,0.095259,0.71018 +3421,415,0.084456,0.70203 +3422,416,0.10397,0.7228 +3423,417,0.07364,0.73688 +3424,418,0.10099,0.76916 +3425,419,0.094113,0.79142 +3426,422,0.11695,0.11838 +3427,421,0.11265,0.86118 +3428,424,0.12608,0.2286 +3429,424,0.098531,0.52433 +3430,425,0.074338,0.58793 +3431,426,0.066058,0.66997 +3432,427,0.07593,0.68653 +3433,430,0.076777,0.43547 +3434,431,0.10626,0.50459 +3435,431,0.090603,0.57493 +3436,432,0.084591,0.62021 +3437,433,0.06518,0.65516 +3438,434,0.088447,0.67053 +3439,435,0.084382,0.69085 +3440,437,0.08864,0.5711 +3441,438,0.071889,0.58659 +3442,439,0.06133,0.59776 +3443,439,0.076491,0.7442 +3444,441,0.083076,0.59403 +3445,442,0.053194,0.60042 +3446,443,0.043822,0.58572 +3447,444,0.06208,0.57422 +3448,445,0.052252,0.56499 +3449,446,0.054012,0.5492 +3450,447,0.047266,0.54206 +3451,448,0.043386,0.537 +3452,449,0.046498,0.53628 +3453,450,0.051607,0.55481 +3454,451,0.04954,0.61125 +3455,452,0.055524,0.67392 +3456,453,0.049512,0.72452 +3457,454,0.060479,0.78121 +3458,455,0.048154,0.82682 +3459,456,0.037072,0.86692 +3460,457,0.04674,0.90879 +3461,458,0.031691,0.94834 +3462,459,0.039648,0.99626 +3463,460,0.047547,1.0273 +3464,461,0.057568,1.0677 +3465,463,0.061845,0.90718 +3466,463,0.064674,1.1651 +3467,465,0.072989,0.88811 +3468,466,0.075313,0.89524 +3469,467,0.044292,0.92334 +3470,468,0.032245,0.98216 +3471,469,0.053995,1.0397 +3472,471,0.040517,0.85185 +3473,472,0.059152,0.82555 +3474,473,0.039805,0.85778 +3475,474,0.037291,0.92548 +3476,475,0.063179,1.0448 +3477,477,0.05835,0.79219 +3478,478,0.017437,0.81236 +3479,479,0.064769,0.8788 +3480,481,0.067249,0.75607 +3481,482,0.049923,0.71111 +3482,483,0.04944,0.73929 +3483,484,0.084984,0.83942 +3484,486,0.068865,0.66955 +3485,487,0.080969,0.645 +3486,488,0.096104,0.70935 +3487,490,0.079273,0.62396 +3488,491,0.070285,0.55952 +3489,492,0.081309,0.59844 +3490,494,0.079061,0.59827 +3491,495,0.038021,0.50946 +3492,496,0.041706,0.52806 +3493,498,0.074656,0.64311 +3494,499,0.069889,0.54968 +3495,500,0.073375,0.54568 +3496,501,0.066413,0.62479 +3497,502,0.066331,0.75863 +3498,504,0.04612,0.5742 +3499,505,0.083456,0.61658 +3500,507,0.072278,0.64292 +3501,508,0.06483,0.59317 +3502,509,0.045953,0.62332 +3503,511,0.071982,0.63206 +3504,512,0.051929,0.57741 +3505,513,0.062702,0.59741 +3506,514,0.097461,0.67465 +3507,516,0.08499,0.54534 +3508,517,0.08652,0.56008 +3509,519,0.10472,0.50263 +3510,520,0.1377,0.46874 +3511,521,0.11436,0.50328 +3512,522,0.13331,0.57529 +3513,524,0.14193,0.40398 +3514,524,0.12945,0.75625 +3515,526,0.16611,0.51414 +3516,528,0.13532,0.33883 +3517,529,0.11522,0.38343 +3518,530,0.097582,0.44403 +3519,530,0.10195,0.77263 +3520,533,0.10225,0.39314 +3521,533,0.087222,0.67149 +3522,551,0.093044,0.22042 +3523,551,0.077817,0.26192 +3524,561,0.10284,0.25655 +3525,563,0.092164,0.30675 +3526,566,0.089572,0.18999 +3527,567,0.095544,0.21718 +3528,569,0.099764,0.17982 +3529,570,0.10202,0.1727 +3530,571,0.12059,0.17599 +3531,572,0.088523,0.18541 +3532,573,0.083859,0.19611 +3533,573,0.092266,0.22882 +3534,574,0.065916,0.2085 +3535,572,0.075181,0.86744 +3536,573,0.086694,0.86023 +3537,577,0.09177,0.16811 +3538,575,0.07192,0.88213 +3539,579,0.098566,0.20077 +3540,577,0.11658,0.94366 +3541,579,0.10608,0.68274 +3542,581,0.10241,0.39468 +3543,582,0.079983,0.42449 +3544,583,0.09799,0.46025 +3545,584,0.065836,0.46168 +3546,585,0.060715,0.53953 +3547,586,0.078283,0.58405 +3548,587,0.074035,0.62879 +3549,588,0.092114,0.67884 +3550,589,0.099951,0.73316 +3551,590,0.092961,0.78744 +3552,592,0.11666,0.37682 +3553,592,0.11467,0.87151 +3554,594,0.089423,0.48545 +3555,595,0.048698,0.56237 +3556,596,0.030646,0.5912 +3557,597,0.092709,0.65082 +3558,599,0.076415,0.3088 +3559,600,0.064109,0.32145 +3560,601,0.092458,0.33833 +3561,602,0.087178,0.35998 +3562,603,0.065431,0.39903 +3563,604,0.079738,0.45687 +3564,605,0.068067,0.52619 +3565,606,0.065775,0.60701 +3566,608,0.069198,0.29107 +3567,609,0.042854,0.26908 +3568,609,0.073597,0.90412 +3569,610,0.07436,1.0097 +3570,612,0.027189,0.38187 +3571,613,0.03542,0.46832 +3572,614,0.054128,0.57312 +3573,615,0.058842,0.69129 +3574,616,0.088969,0.82029 +3575,618,0.068361,0.27177 +3576,619,0.07929,0.3252 +3577,620,0.061089,0.4271 +3578,621,0.087537,0.55955 +3579,622,0.10045,0.70594 +3580,624,0.090376,0.28564 +3581,625,0.055574,0.24113 +3582,626,0.060658,0.30453 +3583,627,0.088106,0.42489 +3584,628,0.10668,0.57663 +3585,629,0.11436,0.73438 +3586,631,0.092673,0.27996 +3587,632,0.070837,0.24312 +3588,633,0.071837,0.31331 +3589,634,0.10559,0.45099 +3590,635,0.055948,0.61128 +3591,636,0.10764,0.78426 +3592,638,0.099881,0.28908 +3593,639,0.058087,0.26122 +3594,640,0.049617,0.3359 +3595,641,0.033961,0.4745 +3596,642,0.076609,0.6363 +3597,644,0.077513,0.39055 +3598,645,0.052841,0.27914 +3599,646,0.080118,0.27985 +3600,647,0.06745,0.40313 +3601,648,0.068639,0.57203 +3602,650,0.10875,0.381 +3603,651,0.064039,0.23013 +3604,652,0.06631,0.25651 +3605,653,0.06431,0.43451 +3606,655,0.089584,0.43654 +3607,656,0.095025,0.2244 +3608,657,0.046494,0.16451 +3609,658,0.062422,0.34744 +3610,660,0.065911,0.47125 +3611,661,0.087185,0.23454 +3612,662,0.043045,0.094604 +3613,663,0.051207,0.29061 +3614,664,0.07003,0.53155 +3615,666,0.058302,0.27346 +3616,667,0.044426,0.075985 +3617,668,0.060829,0.22237 +3618,669,0.082533,0.44624 +3619,671,0.095735,0.3995 +3620,672,0.074451,0.19306 +3621,673,0.03985,0.04715 +3622,674,0.065908,0.22087 +3623,675,0.052044,0.40935 +3624,677,0.068861,0.48659 +3625,677,0.065753,0.78468 +3626,679,0.090192,0.12168 +3627,680,0.044919,0.089761 +3628,681,0.05526,0.26014 +3629,682,0.090563,0.44568 +3630,683,0.083276,0.62779 +3631,685,0.090358,0.29193 +3632,686,0.047125,0.15728 +3633,687,0.06156,0.17626 +3634,688,0.059814,0.31989 +3635,690,0.087951,0.63475 +3636,690,0.088435,0.6424 +3637,692,0.064857,0.32143 +3638,693,0.061375,0.22204 +3639,694,0.03735,0.24184 +3640,695,0.076527,0.34438 +3641,696,0.071587,0.46863 +3642,697,0.092114,0.59412 +3643,699,0.043643,0.38307 +3644,700,0.050312,0.2936 +3645,701,0.060029,0.25528 +3646,702,0.049366,0.29442 +3647,703,0.064904,0.38212 +3648,704,0.072242,0.49863 +3649,706,0.075988,0.45218 +3650,707,0.0769,0.37363 +3651,708,0.072505,0.34137 +3652,709,0.07811,0.36967 +3653,710,0.078036,0.44525 +3654,712,0.073206,0.49596 +3655,713,0.068118,0.41128 +3656,714,0.050477,0.3862 +3657,715,0.064732,0.44054 +3658,717,0.076224,0.48555 +3659,717,0.084693,0.67737 +3660,719,0.078924,0.45158 +3661,720,0.10023,0.53984 +3662,721,0.10665,0.66459 +3663,723,0.096093,0.51577 +3664,724,0.14278,0.62171 +3665,725,0.14953,0.74938 +3666,728,0.13766,0.70935 +3667,728,0.13118,0.76398 +3668,729,0.13124,0.88244 +3669,732,0.15585,1.0328 +3670,733,0.17714,1.125 +3679,741,0.19688,1.8353 +3689,749,0.17713,1.7566 +3690,749,0.17868,1.7155 +3691,751,0.1978,1.595 +3692,752,0.16191,1.5302 +3693,753,0.15975,1.4976 +3694,754,0.18196,1.4574 +3695,755,0.17917,1.4232 +3696,756,0.15793,1.376 +3697,756,0.15715,1.289 +3698,757,0.18161,1.2367 +3699,758,0.19893,1.2009 +3701,761,0.19814,1.1338 +3702,762,0.19765,1.0772 +3704,764,0.15639,0.96618 +3705,765,0.14924,0.91483 +3706,767,0.16049,1.1076 +3707,768,0.14661,1.0304 +3708,769,0.16173,0.95107 +3709,770,0.17248,0.88029 +3710,771,0.15391,0.80607 +3711,772,0.12899,0.74643 +3712,773,0.14391,0.69723 +3713,774,0.10839,0.66246 +3714,775,0.13295,0.62635 +3715,777,0.11932,0.99003 +3716,777,0.11136,0.60216 +3717,778,0.11118,0.61244 +3718,780,0.084911,0.75093 +3719,781,0.084824,0.65949 +3720,782,0.093516,0.57417 +3721,783,0.083243,0.49306 +3722,784,0.075382,0.42461 +3723,786,0.08921,0.97408 +3724,787,0.063303,0.83625 +3725,788,0.055663,0.69642 +3726,789,0.032825,0.55779 +3727,790,0.033788,0.42876 +3728,791,0.056153,0.33518 +3729,792,0.077125,0.32126 +3730,794,0.098905,0.74103 +3731,795,0.093547,0.58313 +3732,796,0.072227,0.44562 +3733,797,0.069637,0.35809 +3734,798,0.072181,0.36877 +3735,800,0.1012,0.73319 +3736,801,0.088988,0.59052 +3737,802,0.081669,0.49135 +3738,803,0.088109,0.45904 +3739,805,0.14143,0.87342 +3740,806,0.088275,0.72476 +3741,807,0.081838,0.60473 +3742,808,0.061756,0.5184 +3743,809,0.07685,0.49552 +3744,810,0.083131,0.54533 +3745,812,0.07936,0.65237 +3746,813,0.11602,0.52991 +3747,814,0.092008,0.46097 +3748,815,0.11438,0.46278 +3749,817,0.11669,0.69024 +3750,818,0.12787,0.54126 +3751,819,0.1183,0.4249 +3752,820,0.11498,0.36484 +3753,821,0.12448,0.3726 +3754,822,0.12017,0.43852 +3755,824,0.075637,0.46155 +3756,825,0.067569,0.33571 +3757,826,0.086052,0.26541 +3758,828,0.054262,0.62002 +3759,829,0.059315,0.43646 +3760,830,0.054124,0.28347 +3761,832,0.045085,0.61395 +3762,833,0.039748,0.38973 +3763,835,0.054577,0.68335 +3764,836,0.034231,0.44815 +3765,837,0.051626,0.29632 +3766,839,0.048617,0.49034 +3767,840,0.035521,0.32485 +3768,842,0.02213,0.51578 +3769,843,0.038473,0.3297 +3770,845,0.03616,0.5392 +3771,846,0.069148,0.3313 +3772,848,0.058476,0.58235 +3773,849,0.033742,0.36661 +3774,850,0.068282,0.27062 +3775,852,0.055623,0.43044 +3776,853,0.081876,0.28355 +3777,855,0.085668,0.57836 +3778,856,0.079642,0.39534 +3779,857,0.077434,0.30271 +3780,859,0.075026,0.5724 +3781,860,0.052223,0.38786 +3782,861,0.083688,0.32685 +3783,863,0.093958,0.66109 +3784,864,0.071433,0.48336 +3785,865,0.097187,0.38096 +3786,867,0.081609,0.7876 +3787,868,0.064721,0.63527 +3788,869,0.043929,0.50521 +3789,870,0.047902,0.43407 +3790,872,0.07444,0.84348 +3791,873,0.061356,0.67386 +3792,874,0.051737,0.54037 +3793,875,0.050476,0.44854 +3794,876,0.068758,0.44399 +3795,878,0.10942,0.80398 +3796,879,0.05869,0.64458 +3797,880,0.04027,0.52199 +3798,881,0.013119,0.45485 +3799,882,0.066674,0.46733 +3800,884,0.078149,0.85938 +3801,885,0.075164,0.72029 +3802,886,0.068032,0.60417 +3803,887,0.056921,0.52924 +3804,888,0.032784,0.51512 +3805,889,0.068174,0.55505 +3806,891,0.10477,0.80946 +3807,892,0.095494,0.67578 +3808,893,0.062951,0.57035 +3809,894,0.062536,0.51395 +3810,895,0.072229,0.51577 +3811,896,0.095976,0.58189 +3812,898,0.07504,0.64844 +3813,899,0.079638,0.52944 +3814,900,0.026548,0.45961 +3815,901,0.096918,0.4666 +3816,902,0.13836,0.53983 +3817,903,0.10539,0.66673 +3818,905,0.099527,0.54633 +3819,906,0.060867,0.48184 +3820,907,0.074188,0.49242 +3821,908,0.094226,0.56528 +3822,910,0.08159,0.66476 +3823,911,0.074229,0.54405 +3824,912,0.081451,0.46848 +3825,913,0.072537,0.46444 +3826,914,0.13248,0.5335 +3827,916,0.1075,0.56961 +3828,917,0.089515,0.42794 +3829,918,0.10344,0.35664 +3830,919,0.11292,0.39764 +3831,921,0.11373,0.5034 +3832,922,0.11061,0.31533 +3833,923,0.12234,0.25132 +3834,925,0.095064,0.46486 +3835,926,0.11477,0.2108 +3836,928,0.1148,0.55532 +3837,929,0.13395,0.19798 +3838,931,0.089244,0.42762 +3839,932,0.10884,0.061937 +3840,934,0.09651,0.16994 +3841,936,0.11766,0.27467 +3842,939,0.15204,0.86704 +3843,940,0.10821,0.35631 +3844,943,0.10591,0.92646 +3845,945,0.11257,1.0528 +3846,947,0.14007,1.2791 +4086,4026,0.19647,71.785 +4087,4029,0.17798,69.611 +4098,4039,0.1933,71.937 +4104,4032,0.19953,87.231 +4107,4035,0.18493,87.513 +4108,4035,0.16613,88.736 +4109,4022,0.18817,104.65 +4113,4056,0.19922,70.563 +4114,4027,0.19434,105.02 +4117,4058,0.19995,73.055 +4119,4058,0.17109,75.484 +4120,4059,0.17764,75.503 +4122,4063,0.18719,73.119 +4155,4089,0.17875,80.876 +4156,4090,0.18251,80.874 +4167,4104,0.19689,77.014 +4168,4081,0.1964,106.63 +4174,4054,0.19218,146.25 +4175,4053,0.18144,148.57 +4176,4053,0.1844,149.74 +4177,4057,0.18942,146.28 +4178,4056,0.19632,148.61 +4179,4058,0.17388,147.46 +4180,4058,0.14882,148.64 +4181,4058,0.15282,149.81 +4182,4058,0.16478,150.98 +4183,4058,0.16937,152.15 +4184,4063,0.16278,147.48 +4185,4060,0.16456,152.18 +4186,4062,0.17425,151.01 +4188,4063,0.173,152.2 +4189,4090,0.18209,120.6 +4192,4068,0.1988,151.06 +4212,4042,0.19878,204.79 +4213,4042,0.19257,206.02 +4346,4232,0.19848,147.54 +4449,1,0.13053,1.2687 +4450,1,0.065646,1.6334 +4451,2,0.11856,1.3391 +4452,2,0.12076,1.885 +4453,4,0.07551,0.85034 +4454,5,0.069256,0.65579 +4455,6,0.12955,0.48 +4456,6,0.14076,1.1592 +4457,8,0.1402,0.20551 +4458,8,0.12864,0.92316 +4459,9,0.1212,0.83523 +4460,10,0.091014,0.76869 +4461,11,0.094309,0.71579 +4462,12,0.09095,0.68183 +4463,13,0.086022,0.66145 +4464,14,0.12145,0.66037 +4465,15,0.11576,0.67138 +4466,16,0.10502,0.6999 +4467,18,0.13693,0.16018 +4468,19,0.12991,0.14666 +4469,19,0.1361,0.83053 +4470,21,0.11473,0.13735 +4471,22,0.12905,0.1519 +4472,23,0.11426,0.17233 +4473,24,0.11572,0.2005 +4474,25,0.094085,0.24349 +4475,26,0.11117,0.2909 +4476,27,0.069105,0.35066 +4477,28,0.12759,0.42191 +4478,29,0.12165,0.50369 +4479,30,0.12076,0.59271 +4480,31,0.1146,0.68602 +4481,32,0.11854,0.78384 +4482,34,0.10691,0.11035 +4483,35,0.095109,0.04609 +4484,36,0.1005,0.12729 +4485,37,0.082428,0.23604 +4486,38,0.10448,0.33445 +4487,39,0.080922,0.433 +4488,40,0.11181,0.51503 +4489,41,0.10755,0.55844 +4490,43,0.14419,0.45751 +4491,44,0.091988,0.38999 +4492,45,0.068163,0.32214 +4493,46,0.096962,0.24878 +4494,47,0.076357,0.19398 +4495,48,0.067854,0.17365 +4496,49,0.081921,0.21149 +4497,50,0.11233,0.29649 +4498,52,0.079512,0.64139 +4499,53,0.074202,0.49544 +4500,54,0.055416,0.35099 +4501,55,0.04999,0.20826 +4502,56,0.076014,0.096245 +4503,57,0.076982,0.19117 +4504,59,0.10069,0.60803 +4505,60,0.066785,0.41546 +4506,61,0.060471,0.21604 +4507,62,0.078096,0.010787 +4508,64,0.11174,0.74511 +4509,65,0.072687,0.51088 +4510,66,0.085603,0.26699 +4511,67,0.068949,0.066393 +4512,69,0.084442,0.63513 +4513,70,0.085138,0.33661 +4514,71,0.076727,0.12166 +4515,73,0.076458,0.5219 +4516,75,0.091597,0.97262 +4517,76,0.11259,0.61147 +4518,78,0.11267,0.98103 +4519,79,0.10949,0.58178 +4520,81,0.085893,0.82734 +4521,83,0.090288,1.028 +4522,84,0.14685,0.59582 +4523,87,0.13908,1.1813 +4524,89,0.10489,1.1831 +4525,91,0.068849,1.1217 +4526,92,0.098152,0.61523 +4527,96,0.13071,1.2383 +4528,97,0.078189,0.58424 +4529,100,0.052122,0.6118 +4530,102,0.095975,0.30614 +4531,104,0.17613,0.50631 +4536,1557,0.11952,2.3321 +4537,1555,0.090415,2.0274 +4538,1555,0.16563,0.95465 diff --git a/example/place_recognition/sc_pipeline.png b/example/place_recognition/sc_pipeline.png new file mode 100644 index 0000000..4dcf8d5 Binary files /dev/null and b/example/place_recognition/sc_pipeline.png differ diff --git a/fast_evaluator/00.csv b/fast_evaluator/00.csv new file mode 100644 index 0000000..7c911bc --- /dev/null +++ b/fast_evaluator/00.csv @@ -0,0 +1,4541 @@ +1,9.04368E-12,2.326809E-11,5.551115E-17,9.043683E-12,1,2.39237E-10,3.330669E-16,2.32681E-11,2.39237E-10,0.9999999,-4.440892E-16 +0.9999978,0.0005272628,-0.002066935,-0.04690294,-0.0005296506,0.9999992,-0.001154865,-0.02839928,0.002066324,0.001155958,0.9999971,0.8586941 +0.999991,0.001048972,-0.004131348,-0.09374345,-0.001058514,0.9999968,-0.002308104,-0.05676064,0.004128913,0.002312456,0.9999887,1.716275 +0.9999796,0.001566466,-0.006198571,-0.1406429,-0.001587952,0.9999927,-0.003462706,-0.08515762,0.006193102,0.003472479,0.9999747,2.574964 +0.9999637,0.002078471,-0.008263498,-0.1874858,-0.002116664,0.9999871,-0.004615826,-0.1135202,0.008253797,0.004633149,0.9999551,3.432648 +0.9999433,0.002586172,-0.01033094,-0.2343818,-0.002645881,0.9999798,-0.005770163,-0.141915,0.01031581,0.00579717,0.9999299,4.291335 +0.9999184,0.003088363,-0.01239599,-0.2812195,-0.00317435,0.999971,-0.006922975,-0.1702743,0.01237425,0.006961759,0.9998991,5.148987 +0.999889,0.003586305,-0.01446384,-0.3281178,-0.003703403,0.9999605,-0.008077186,-0.1986703,0.0144343,0.008129853,0.9998627,6.007777 +0.9998551,0.004078705,-0.01652913,-0.3749547,-0.004231669,0.9999484,-0.009229794,-0.227029,0.01649063,0.009298401,0.9998207,6.865477 +0.9998167,0.004566671,-0.01859652,-0.4218367,-0.004760342,0.9999347,-0.01038342,-0.2554151,0.01854788,0.01047004,0.9997731,7.724036 +0.9997738,0.005049868,-0.02066463,-0.4687329,-0.005289072,0.9999194,-0.0115373,-0.2838096,0.0206047,0.01164399,0.9997198,8.582886 +0.9997264,0.005527315,-0.02272922,-0.5155474,-0.005816781,0.9999025,-0.01268908,-0.3121547,0.02265686,0.01281782,0.9996611,9.440275 +0.9996745,0.00600054,-0.02479692,-0.562431,-0.00634516,0.999884,-0.01384246,-0.3405416,0.02471098,0.0139953,0.9995966,10.29896 +0.9996182,0.006468772,-0.0268644,-0.6093087,-0.006873365,0.9998639,-0.01499561,-0.368925,0.02676374,0.01517453,0.9995266,11.15757 +0.9995562,0.00705845,-0.02894213,-0.6562052,-0.007530449,0.9998399,-0.01623192,-0.3973964,0.02882292,0.01644266,0.9994492,12.01541 +0.9995095,0.005595311,-0.0308145,-0.7018788,-0.006093682,0.9998517,-0.01610315,-0.4239119,0.03071983,0.01628303,0.9993953,12.86965 +0.9994226,0.007842768,-0.03306167,-0.7498241,-0.008456089,0.999794,-0.01845198,-0.4540039,0.03291014,0.01872089,0.9992829,13.73146 +0.9993587,0.008704386,-0.034734,-0.7992511,-0.009439421,0.9997338,-0.02105422,-0.484077,0.03454149,0.02136859,0.9991747,14.60026 +0.9992482,0.01235688,-0.03674849,-0.8546642,-0.01320676,0.9996488,-0.02297477,-0.515507,0.03645169,0.02344282,0.9990604,15.47957 +0.9991826,0.01352279,-0.03809783,-0.9072868,-0.01439161,0.9996404,-0.02262373,-0.5464705,0.03777819,0.02315352,0.9990178,16.3694 +0.9991241,0.01328777,-0.03967996,-0.9609163,-0.01412604,0.9996813,-0.02092052,-0.5783595,0.03938933,0.02146271,0.9989934,17.26896 +0.9990604,0.01351433,-0.04118029,-1.011589,-0.01432436,0.9997084,-0.01943894,-0.6092403,0.04090558,0.02001055,0.9989626,18.17318 +0.9990174,0.01304822,-0.04235654,-1.066256,-0.01386115,0.9997242,-0.01895579,-0.6387363,0.04209752,0.01952427,0.9989227,19.08411 +0.9989383,0.01325999,-0.04411963,-1.118651,-0.01413494,0.9997084,-0.0195787,-0.6686219,0.04384715,0.02018154,0.9988343,19.99722 +0.998912,0.01152857,-0.04518863,-1.17184,-0.01247777,0.9997062,-0.02077977,-0.6990778,0.04493579,0.02132101,0.9987623,20.91368 +0.99888,0.01128892,-0.04595003,-1.224279,-0.01218132,0.9997417,-0.0191876,-0.7287421,0.04572155,0.01972584,0.9987594,21.84042 +0.9988593,0.01299421,-0.04594914,-1.280807,-0.01370812,0.9997896,-0.0152561,-0.7604717,0.04574123,0.01586857,0.9988272,22.77432 +0.9988876,0.0137903,-0.04509495,-1.334841,-0.01432558,0.9998304,-0.0115685,-0.7929673,0.04492776,0.01220164,0.9989157,23.70953 +0.9989649,0.01285173,-0.0436358,-1.385747,-0.01325687,0.9998715,-0.009007757,-0.8209244,0.04351442,0.009576906,0.9990068,24.65175 +0.9990298,0.01265253,-0.04218321,-1.436633,-0.01299403,0.9998849,-0.007831284,-0.8456208,0.04207926,0.008371815,0.9990791,25.59694 +0.9990746,0.01289592,-0.04103309,-1.487044,-0.01333334,0.999857,-0.01040438,-0.8702127,0.04089305,0.01094186,0.9991036,26.54471 +0.9990967,0.0139101,-0.04015465,-1.538025,-0.01454998,0.9997711,-0.01568743,-0.8971121,0.03992724,0.01625751,0.9990703,27.49627 +0.9991533,0.01320645,-0.03896502,-1.586551,-0.01396325,0.9997178,-0.01921474,-0.9254151,0.03870026,0.01974255,0.9990558,28.45466 +0.9992105,0.01079096,-0.03823584,-1.633363,-0.01148611,0.9997718,-0.01800793,-0.955563,0.03803279,0.01843289,0.9991064,29.42293 +0.9992215,0.009086312,-0.03839229,-1.680971,-0.009596057,0.9998679,-0.01311391,-0.9838377,0.03826806,0.01347211,0.9991766,30.39839 +0.9992105,0.005838117,-0.03929763,-1.728893,-0.006198498,0.9999398,-0.00905494,-1.011772,0.03924239,0.009291377,0.9991865,31.37765 +0.999183,-0.001840283,-0.04037463,-1.768976,0.001460769,0.9999545,-0.009427292,-1.036363,0.04039014,0.009360611,0.9991401,32.35714 +0.9991063,-0.004894794,-0.04198475,-1.815834,0.004337365,0.9999014,-0.01335777,-1.059789,0.04204599,0.01316373,0.9990289,33.33631 +0.9990442,-0.004076714,-0.0435224,-1.870663,0.003306848,0.999837,-0.01774634,-1.088016,0.04358766,0.01758545,0.9988948,34.32021 +0.998962,-0.003501086,-0.04541814,-1.92978,0.002639355,0.9998156,-0.0190194,-1.12988,0.04547635,0.01887978,0.9987869,35.33097 +0.9989126,-0.003564847,-0.046487,-1.993748,0.002782576,0.9998536,-0.01688161,-1.194548,0.04654037,0.0167339,0.9987762,36.38751 +0.9988708,-0.0002386257,-0.04750925,-2.061112,-0.0004151975,0.9999053,-0.01375168,-1.249881,0.04750803,0.01375587,0.9987761,37.4285 +0.9988085,2.160348E-06,-0.04880145,-2.126953,-0.0006257569,0.9999183,-0.01276297,-1.305052,0.04879743,0.0127783,0.9987269,38.47151 +0.9987592,0.0008809214,-0.04979299,-2.190967,-0.001403287,0.9999443,-0.01045676,-1.349142,0.04978101,0.01051366,0.9987048,39.50365 +0.9987569,0.0004894213,-0.04984526,-2.256115,-0.0008210333,0.9999776,-0.006632571,-1.393047,0.0498409,0.00666525,0.9987349,40.53855 +0.9987132,0.0064665,-0.05030151,-2.32994,-0.006722014,0.9999653,-0.004912121,-1.426813,0.050268,0.005243927,0.9987219,41.56111 +0.9987143,0.006952179,-0.05021498,-2.399728,-0.007192513,0.9999635,-0.004606971,-1.460731,0.05018112,0.004962219,0.9987278,42.58303 +0.9987083,0.006676282,-0.0503716,-2.465214,-0.007006772,0.999955,-0.00638728,-1.489501,0.05032669,0.006731971,0.9987101,43.59302 +0.9987013,0.008983148,-0.05015069,-2.535885,-0.00945617,0.9999129,-0.009202712,-1.52285,0.05006365,0.009664992,0.9986992,44.5979 +0.9986535,0.008808468,-0.05112439,-2.600016,-0.009296065,0.9999135,-0.009307503,-1.559885,0.05103798,0.009770225,0.9986489,45.59882 +0.9986012,0.006863746,-0.05242779,-2.661881,-0.00728016,0.9999434,-0.007755765,-1.593756,0.05237159,0.008126598,0.9985945,46.59803 +0.998554,0.007061951,-0.05329345,-2.728914,-0.007431486,0.9999497,-0.006738977,-1.629817,0.05324317,0.007125281,0.9985561,47.59275 +0.9985154,0.009830435,-0.05357609,-2.800794,-0.01032218,0.999907,-0.008909522,-1.664529,0.05348352,0.009449317,0.998524,48.57856 +0.9984686,0.0102379,-0.05436634,-2.871336,-0.01103255,0.9998363,-0.01433651,-1.697621,0.05421067,0.01491435,0.9984181,49.556 +0.9984385,0.01064895,-0.05483888,-2.937935,-0.0114946,0.9998195,-0.01512829,-1.729013,0.05466788,0.01573501,0.9983805,50.53773 +0.9984274,0.01026893,-0.05511221,-3.002463,-0.01093885,0.9998697,-0.01186763,-1.766742,0.05498316,0.01245183,0.9984096,51.51845 +0.9984356,0.007566176,-0.0554007,-3.065844,-0.008064009,0.999929,-0.008767995,-1.798994,0.05533043,0.009201029,0.9984256,52.49501 +0.9984285,0.006348755,-0.05568046,-3.130185,-0.006799312,0.9999456,-0.007906116,-1.830051,0.05562724,0.008272279,0.9984173,53.46498 +0.9984326,0.003282949,-0.05587138,-3.19152,-0.00377969,0.9999542,-0.00878744,-1.858398,0.05583997,0.008984842,0.9983992,54.42804 +0.9984068,-0.0001460363,-0.05642638,-3.248811,-0.0004508913,0.9999441,-0.010566,-1.886487,0.05642476,0.01057461,0.9983508,55.39178 +0.9983367,-0.001591214,-0.05763218,-3.310922,0.001036802,0.9999529,-0.009648457,-1.913841,0.05764481,0.009572654,0.9982912,56.35039 +0.9982641,-0.004382863,-0.05873397,-3.371514,0.004075329,0.9999773,-0.005354817,-1.938229,0.05875611,0.005106161,0.9982593,57.31251 +0.9982268,-0.00596474,-0.05922694,-3.438994,0.005986946,0.999982,0.0001974663,-1.956763,0.0592247,-0.0005517038,0.9982445,58.274 +0.9982022,-0.00440776,-0.05977459,-3.511313,0.004547914,0.9999872,0.002208854,-1.975962,0.05976408,-0.002476732,0.9982094,59.2254 +0.9981161,-0.002919496,-0.06128511,-3.579193,0.002920554,0.9999957,-7.231076E-05,-1.998111,0.06128506,-0.0001068112,0.9981202,60.16633 +0.9980242,-0.001762043,-0.06280674,-3.641784,0.001227247,0.9999627,-0.008552514,-2.021539,0.06281946,0.008458535,0.997989,61.09125 +0.9979547,-0.0009592105,-0.0639183,-3.703384,5.024105E-05,0.9998989,-0.01422089,-2.049159,0.06392547,0.0141886,0.9978538,62.00632 +0.9978643,0.004204986,-0.0651861,-3.776035,-0.005092497,0.9998965,-0.01345487,-2.078109,0.06512277,0.01375809,0.9977824,62.91678 +0.9977905,0.007492613,-0.06601519,-3.84394,-0.0082063,0.9999107,-0.01054641,-2.112094,0.06593028,0.01106485,0.9977628,63.81519 +0.9976764,0.01025448,-0.06735521,-3.912602,-0.01087073,0.9999023,-0.008789172,-2.145353,0.06725849,0.009500949,0.9976903,64.69786 +0.9975909,0.01034502,-0.0685962,-3.978228,-0.0110335,0.9998924,-0.009665437,-2.173835,0.06848883,0.01039901,0.9975976,65.56672 +0.9974616,0.01183547,-0.07021661,-4.046625,-0.01248174,0.9998836,-0.008772233,-2.201409,0.07010461,0.00962639,0.9974931,66.42163 +0.9973396,0.0131838,-0.07169363,-4.114442,-0.01380046,0.9998718,-0.008112825,-2.230822,0.07157748,0.009080646,0.9973937,67.26455 +0.9972258,0.01296774,-0.07329823,-4.179994,-0.0135235,0.9998834,-0.007090915,-2.258366,0.07319772,0.008062491,0.9972848,68.09328 +0.9971241,0.01256612,-0.07473754,-4.244856,-0.01307757,0.9998942,-0.006357728,-2.28331,0.07464974,0.007316829,0.9971829,68.90994 +0.9969894,0.01489953,-0.07609372,-4.313021,-0.01525772,0.9998751,-0.004127928,-2.307845,0.07602271,0.005276517,0.9970921,69.71424 +0.9968772,0.01677181,-0.07716622,-4.381935,-0.01677976,0.999859,0.0005454462,-2.329476,0.07716449,0.0007510882,0.997018,70.50799 +0.9966886,0.01928865,-0.07899331,-4.44551,-0.01916694,0.9998136,0.002298764,-2.355391,0.07902292,-0.0007770902,0.9968724,71.28437 +0.9964961,0.02232345,-0.08060564,-4.514875,-0.02246983,0.9997471,-0.0009091952,-2.382091,0.08056495,0.002717205,0.9967456,72.03735 +0.9963707,0.02284027,-0.08199948,-4.58021,-0.02361073,0.9996856,-0.008438421,-2.409731,0.08178096,0.01034386,0.9965966,72.76791 +0.9962563,0.02164989,-0.08369451,-4.644343,-0.02291184,0.9996374,-0.01414702,-2.432982,0.08335788,0.01601165,0.996391,73.48065 +0.9962035,0.02236698,-0.0841333,-4.709494,-0.02381956,0.9995833,-0.01630112,-2.459523,0.08373364,0.01824325,0.9963211,74.17539 +0.9960873,0.02480834,-0.08482128,-4.77573,-0.02599143,0.9995793,-0.01287207,-2.490277,0.08446625,0.01502633,0.996313,74.85147 +0.9959529,0.02606958,-0.08601342,-4.837515,-0.02697536,0.999592,-0.009385091,-2.525955,0.08573366,0.01166735,0.9962497,75.50744 +0.9959075,0.02291063,-0.08742661,-4.891889,-0.02409575,0.9996312,-0.01252425,-2.556654,0.08710742,0.01457961,0.9960922,76.13828 +0.9960426,0.01503549,-0.08759638,-4.937677,-0.01662384,0.9997098,-0.0174314,-2.585469,0.08730887,0.0188186,0.9960035,76.7501 +0.9961635,0.01044107,-0.08688679,-4.985837,-0.01249941,0.9996532,-0.02317957,-2.616742,0.08661463,0.02417667,0.9959484,77.34409 +0.9963973,0.01211154,-0.08393973,-5.039038,-0.01414999,0.9996182,-0.02373245,-2.646048,0.08362024,0.02483469,0.9961881,77.92534 +0.9966394,0.01770654,-0.07997749,-5.092732,-0.01941041,0.9995998,-0.02057726,-2.675699,0.07958113,0.0220605,0.9965842,78.49464 +0.9970121,0.01664848,-0.07543122,-5.133801,-0.01801913,0.999684,-0.01752686,-2.70184,0.07511558,0.01883369,0.9969969,79.04953 +0.9974633,0.01217037,-0.07013516,-5.159772,-0.01315774,0.9998205,-0.01363335,-2.729207,0.06995665,0.01452158,0.9974443,79.5923 +0.998024,0.007887781,-0.06233726,-5.183357,-0.008483336,0.9999208,-0.009294846,-2.7568,0.062259,0.009805306,0.9980118,80.12304 +0.9985955,0.01068615,-0.05189267,-5.208269,-0.01122407,0.9998861,-0.01008572,-2.780022,0.05177898,0.010654,0.9986017,80.63455 +0.9991827,0.01624761,-0.03701415,-5.235209,-0.01664974,0.9998054,-0.01058172,-2.801727,0.03683502,0.01118935,0.9992587,81.13538 +0.9995878,0.02136933,-0.01917552,-5.248892,-0.02156744,0.9997155,-0.01018473,-2.822088,0.01895242,0.0105941,0.9997642,81.62286 +0.9997985,0.01993349,0.002394767,-5.236828,-0.01990872,0.9997522,-0.009958356,-2.839863,-0.002592678,0.009908671,0.9999475,82.09701 +0.9994836,0.01619025,0.02775764,-5.207469,-0.01594,0.9998305,-0.009213406,-2.859306,-0.0279021,0.00876619,0.9995722,82.55594 +0.9982814,0.01472502,0.05672377,-5.163543,-0.01430176,0.9998668,-0.007860647,-2.878004,-0.05683196,0.007035887,0.9983589,83.00846 +0.9959138,0.01420072,0.08918593,-5.10562,-0.0136705,0.9998851,-0.006553178,-2.895751,-0.08926873,0.005307184,0.9959934,83.45454 +0.9920007,0.01389449,0.1254661,-5.02956,-0.01324753,0.9998943,-0.005989348,-2.911918,-0.125536,0.00427932,0.9920798,83.8853 +0.9859899,0.01389745,0.1662252,-4.934649,-0.01302694,0.9998951,-0.006326123,-2.926167,-0.1662957,0.004072087,0.9860675,84.31338 +0.9775146,0.01346779,0.2104374,-4.817297,-0.01236226,0.999902,-0.006568159,-2.942364,-0.2105053,0.003818988,0.9775852,84.72869 +0.9660755,0.01536344,0.2578024,-4.680938,-0.01364848,0.9998712,-0.008440597,-2.961855,-0.2578989,0.004635642,0.9661607,85.1182 +0.9507397,0.01607587,0.3095733,-4.519193,-0.013199,0.999848,-0.0113854,-2.980545,-0.3097093,0.006738488,0.9508074,85.50171 +0.9313823,0.01599263,0.3636913,-4.336344,-0.01189395,0.999838,-0.0135066,-2.995233,-0.3638483,0.00825408,0.9314216,85.85373 +0.9076306,0.01642664,0.4194484,-4.130089,-0.01094447,0.9998204,-0.01547307,-3.006846,-0.4196272,0.009453184,0.9076472,86.20001 +0.8794214,0.01652575,0.4757573,-3.903866,-0.009195858,0.9998005,-0.01773051,-3.018353,-0.4759554,0.01121759,0.8793978,86.52588 +0.847002,0.0146349,0.5313884,-3.65638,-0.004922887,0.999794,-0.01968841,-3.030941,-0.531567,0.01406015,0.8468995,86.8121 +0.8111527,0.01400217,0.5846668,-3.393066,-0.002514888,0.9997876,-0.0204548,-3.048142,-0.584829,0.01512159,0.8110156,87.09558 +0.7722941,0.01213857,0.6351492,-3.115393,-7.665609E-05,0.9998192,-0.01901471,-3.063358,-0.6352652,0.01463625,0.7721553,87.34503 +0.7306835,0.0113909,0.6826214,-2.817803,-0.0002276654,0.9998648,-0.01644104,-3.074584,-0.6827163,0.01185778,0.7305872,87.59401 +0.6869081,0.0100239,0.7266752,-2.508137,0.001642222,0.9998809,-0.01534491,-3.08446,-0.7267424,0.0117339,0.6868098,87.8201 +0.6415606,0.00860591,0.7670241,-2.182602,0.0058016,0.999854,-0.01607086,-3.093149,-0.7670504,0.01476039,0.641417,88.00813 +0.5961032,0.005416591,0.8028896,-1.847638,0.01299442,0.9997812,-0.01639257,-3.103538,-0.8028026,0.02020475,0.5959023,88.18799 +0.5506435,0.006831839,0.8347126,-1.511055,0.01267694,0.9997827,-0.01654562,-3.116572,-0.8346443,0.01969233,0.5504372,88.34349 +0.5045001,0.009750747,0.8633566,-1.165582,0.008934955,0.9998237,-0.01651313,-3.127427,-0.8633654,0.01604492,0.504324,88.50204 +0.4602983,0.01148164,0.8876901,-0.8110714,0.00870464,0.9998099,-0.0174455,-3.138031,-0.8877216,0.01575715,0.4601108,88.64159 +0.4188673,0.009627242,0.9079964,-0.4500223,0.01561927,0.9997194,-0.01780508,-3.146259,-0.9079131,0.0216402,0.4185994,88.74795 +0.3806252,0.001378826,0.9247284,-0.07268361,0.03173796,0.9993902,-0.01455374,-3.150458,-0.9241845,0.03488851,0.3803494,88.83995 +0.3461082,-0.002337058,0.9381917,0.3020916,0.03764494,0.9992261,-0.0113985,-3.156586,-0.937439,0.03926327,0.3459283,88.93047 +0.3138409,0.001002432,0.9494751,0.676345,0.03611653,0.9992631,-0.01299301,-3.166835,-0.9487884,0.03836947,0.3135734,89.02315 +0.2833112,0.008752363,0.9589881,1.050891,0.02492263,0.9995534,-0.01648542,-3.185174,-0.9587041,0.028571,0.2829665,89.12503 +0.2551133,0.01327093,0.9668201,1.434583,0.01946154,0.9996327,-0.01885662,-3.205997,-0.9667153,0.02362638,0.2547613,89.21056 +0.2298762,0.01514276,0.9731021,1.83291,0.02080734,0.9995739,-0.02047003,-3.222114,-0.9729974,0.02495324,0.2294632,89.27715 +0.206735,0.01580712,0.9782693,2.240743,0.0274705,0.9993815,-0.02195352,-3.234217,-0.9780112,0.0314121,0.2061729,89.3295 +0.1870149,0.02174817,0.9821163,2.66275,0.036317,0.9989184,-0.02903574,-3.232193,-0.9816855,0.04109762,0.1860228,89.3721 +0.1677054,0.02973015,0.9853888,3.089762,0.0389192,0.9985663,-0.03675147,-3.233114,-0.9850686,0.04451395,0.1663079,89.41899 +0.1501195,0.03105637,0.98818,3.53677,0.03662397,0.9986458,-0.03694904,-3.247013,-0.9879892,0.04173784,0.1487788,89.47222 +0.1346625,0.02612444,0.9905471,4.002128,0.03409387,0.9989383,-0.03098074,-3.269703,-0.9903048,0.03794352,0.1336289,89.52059 +0.1200339,0.02244059,0.9925162,4.475771,0.03438479,0.9990507,-0.02674681,-3.292077,-0.9921741,0.03733797,0.1191484,89.55606 +0.106645,0.0201568,0.9940929,4.962461,0.03689254,0.9990258,-0.02421462,-3.311658,-0.9936124,0.03925697,0.1057975,89.58341 +0.0948166,0.01910255,0.9953115,5.460049,0.04091002,0.9988965,-0.02306858,-3.327578,-0.9946538,0.04290548,0.09393048,89.60332 +0.08451437,0.01970806,0.9962274,5.974572,0.04433756,0.9987397,-0.02351912,-3.344693,-0.9954353,0.04615798,0.08353405,89.62101 +0.0761114,0.02033196,0.996892,6.504623,0.04662512,0.9986258,-0.02392709,-3.36274,-0.9960086,0.04830132,0.07505883,89.63826 +0.07011217,0.02055161,0.9973274,7.050381,0.04845902,0.9985372,-0.02398322,-3.379767,-0.9963613,0.05001101,0.0690137,89.65473 +0.0643362,0.02169879,0.9976924,7.614093,0.04999818,0.9984379,-0.02493915,-3.394968,-0.9966749,0.05148728,0.06315079,89.67153 +0.05805224,0.02266708,0.9980562,8.197957,0.05045707,0.9983978,-0.0256097,-3.409306,-0.9970376,0.05184568,0.05681551,89.68785 +0.05340714,0.02263529,0.9983163,8.800912,0.05292689,0.9982736,-0.02546577,-3.4262,-0.9971692,0.05419782,0.05211692,89.70167 +0.04996154,0.0224389,0.9984991,9.42023,0.05465687,0.998188,-0.02516676,-3.442284,-0.9972544,0.05583219,0.04864457,89.70786 +0.04649404,0.02206005,0.998675,10.05978,0.05576839,0.9981395,-0.02464457,-3.460292,-0.9973606,0.05684031,0.04517728,89.71666 +0.04330615,0.02144658,0.9988317,10.71458,0.05731411,0.9980697,-0.02391518,-3.477648,-0.9974165,0.05828281,0.04199336,89.71954 +0.04207275,0.0210856,0.9988921,11.38837,0.0619778,0.9977967,-0.02367295,-3.495197,-0.9971903,0.0629051,0.04067321,89.72145 +0.04227179,0.02071974,0.9988913,12.07462,0.06595748,0.997546,-0.02348308,-3.511618,-0.9969266,0.06687701,0.04080144,89.72018 +0.0441034,0.02054931,0.9988156,12.7819,0.06780199,0.9974216,-0.02351448,-3.529809,-0.9967235,0.06875874,0.0425964,89.72729 +0.04733313,0.02077681,0.9986631,13.50501,0.0702074,0.9972418,-0.02407483,-3.549541,-0.9964088,0.07125306,0.04574389,89.73868 +0.0510726,0.02062413,0.998482,14.24395,0.0709717,0.997184,-0.02422755,-3.570045,-0.9961699,0.07210131,0.04946505,89.75239 +0.0548512,0.0187366,0.9983188,15.0026,0.07237751,0.9971191,-0.02269077,-3.595307,-0.9958678,0.07350043,0.05333707,89.77631 +0.05870203,0.01722669,0.9981269,15.77368,0.07228987,0.9971527,-0.02146141,-3.621398,-0.9956546,0.07341427,0.05728957,89.80526 +0.06263966,0.01506436,0.9979225,16.56055,0.06951846,0.9973916,-0.01942003,-3.645849,-0.9956121,0.07059048,0.06142902,89.84339 +0.0665983,0.01396403,0.9976822,17.35473,0.06848,0.9974803,-0.01853246,-3.669719,-0.9954271,0.06955549,0.06547424,89.88363 +0.07057071,0.01479215,0.9973971,18.15651,0.06595721,0.9976326,-0.01946245,-3.696455,-0.9953237,0.067159,0.06942799,89.92948 +0.07390613,0.01816167,0.9970998,18.95996,0.06387777,0.9976948,-0.0229072,-3.726996,-0.9952173,0.06538548,0.07257563,89.97884 +0.07513596,0.02442768,0.9968741,19.76819,0.05827767,0.9978836,-0.0288449,-3.758629,-0.9954688,0.06026278,0.07355335,90.03338 +0.07474794,0.02749119,0.9968235,20.58193,0.05566893,0.997946,-0.03169656,-3.789412,-0.9956473,0.05786133,0.073064,90.08278 +0.07476584,0.02757564,0.9968198,21.39947,0.05324229,0.9980814,-0.03160395,-3.828201,-0.9957787,0.05543585,0.07315419,90.13241 +0.07451993,0.02494887,0.9969074,22.21735,0.05043155,0.9983135,-0.02875388,-3.869642,-0.9959434,0.05241831,0.07313604,90.18269 +0.07437195,0.02433854,0.9969336,23.03432,0.05132069,0.998284,-0.02820007,-3.908789,-0.9959091,0.0532606,0.07299525,90.22769 +0.07447614,0.0232276,0.9969523,23.85148,0.04903232,0.9984342,-0.02692503,-3.948711,-0.9960166,0.05088814,0.07322062,90.27374 +0.07410208,0.02327013,0.9969792,24.6666,0.04734121,0.9985185,-0.02682478,-3.991884,-0.9961263,0.04918596,0.07289066,90.31732 +0.07412396,0.02412341,0.9969572,25.47837,0.04481095,0.998617,-0.02749528,-4.031552,-0.9962417,0.04671265,0.07294045,90.365 +0.07273598,0.02538877,0.9970281,26.2888,0.04182689,0.9987188,-0.02848322,-4.070675,-0.9964737,0.04377433,0.07158085,90.41153 +0.06992106,0.0254814,0.9972271,27.10009,0.03897825,0.9988405,-0.02825561,-4.110043,-0.9967907,0.04084581,0.06884676,90.45782 +0.06591466,0.0241763,0.9975324,27.90992,0.03637502,0.9989837,-0.02661506,-4.149285,-0.997162,0.03803957,0.06496825,90.49922 +0.06089788,0.02167742,0.9979086,28.7218,0.03431028,0.9991278,-0.02379771,-4.189372,-0.9975541,0.03568775,0.06010101,90.53737 +0.05454557,0.01931204,0.9983245,29.53344,0.03223633,0.9992577,-0.0210914,-4.227712,-0.9979907,0.03333275,0.05388253,90.56802 +0.04700058,0.01930296,0.9987084,30.34032,0.03172229,0.9992801,-0.02080691,-4.26442,-0.998391,0.03265924,0.04635441,90.58889 +0.0381621,0.02033872,0.9990646,31.14204,0.03120822,0.9992809,-0.02153522,-4.298961,-0.9987841,0.03200084,0.03749992,90.60259 +0.02806847,0.02112184,0.9993829,31.94324,0.03180308,0.9992517,-0.02201229,-4.332533,-0.9990999,0.03240129,0.02737573,90.60409 +0.01781185,0.02023573,0.9996366,32.74069,0.03296529,0.9992397,-0.02081509,-4.3668,-0.9992977,0.03332406,0.01713123,90.59833 +0.006968081,0.01874427,0.9998001,33.53267,0.03197931,0.9993087,-0.01895795,-4.400925,-0.9994642,0.03210501,0.006363835,90.58651 +-0.004213761,0.01806636,0.9998279,34.32424,0.0333777,0.9992822,-0.01791584,-4.430514,-0.9994339,0.03329646,-0.004813748,90.56241 +-0.01536171,0.01808549,0.9997185,35.11039,0.03379189,0.9992746,-0.01755822,-4.460151,-0.9993108,0.03351264,-0.01596171,90.52962 +-0.02648072,0.01872506,0.999474,35.88915,0.03400306,0.9992628,-0.01782022,-4.490465,-0.9990708,0.03351327,-0.0270979,90.49003 +-0.03725612,0.01985515,0.9991085,36.66363,0.03374649,0.9992573,-0.01859973,-4.51981,-0.9987357,0.03302344,-0.03789849,90.44253 +-0.04758921,0.02088992,0.9986486,37.43083,0.03458039,0.9992164,-0.01925393,-4.548257,-0.9982682,0.03361737,-0.0482743,90.38673 +-0.05718399,0.02164931,0.9981289,38.18958,0.03548667,0.9991771,-0.01963898,-4.577147,-0.9977327,0.03429723,-0.05790519,90.32361 +-0.06683215,0.0220867,0.9975198,38.94049,0.03617941,0.9991511,-0.01969887,-4.607065,-0.997108,0.03477315,-0.0675745,90.25165 +-0.07637784,0.02219893,0.9968318,39.68363,0.03679438,0.9991339,-0.019431,-4.635152,-0.9963998,0.0351937,-0.07712847,90.17629 +-0.08502155,0.02197809,0.9961367,40.41876,0.03826025,0.9990913,-0.01877773,-4.66402,-0.9956442,0.03651592,-0.08578518,90.09176 +-0.09242347,0.02068885,0.9955049,41.14574,0.03988263,0.9990587,-0.01705998,-4.69221,-0.9949207,0.0381266,-0.09316159,90.0029 +-0.09778459,0.0192455,0.9950215,41.86324,0.04272773,0.9989723,-0.01512291,-4.72176,-0.9942899,0.04103621,-0.0985064,89.91031 +-0.1017231,0.01749021,0.994659,42.57483,0.04579707,0.9988677,-0.01288059,-4.746579,-0.993758,0.0442422,-0.102409,89.81611 +-0.1045887,0.01626385,0.9943826,43.27463,0.04754502,0.9988048,-0.01133542,-4.770015,-0.9933784,0.04609237,-0.1052369,89.72399 +-0.1064238,0.01554435,0.9941994,43.95946,0.04803206,0.9987908,-0.01047457,-4.793116,-0.99316,0.04663869,-0.1070417,89.63442 +-0.1074152,0.01566224,0.9940909,44.63116,0.04845889,0.99877,-0.01049981,-4.815263,-0.9930325,0.04704469,-0.108042,89.54649 +-0.1066568,0.01685791,0.994153,45.28497,0.0494948,0.9987067,-0.01162513,-4.83706,-0.9930632,0.04796549,-0.1073532,89.46022 +-0.1039437,0.01811139,0.9944183,45.9228,0.05064114,0.9986336,-0.0128948,-4.858683,-0.9932931,0.04901813,-0.1047188,89.37856 +-0.09900439,0.01888729,0.9949078,46.54259,0.05067771,0.9986181,-0.01391474,-4.880596,-0.9937957,0.04904201,-0.09982473,89.3065 +-0.09158105,0.01937642,0.9956091,47.14397,0.04996096,0.9986409,-0.01483978,-4.902936,-0.9945435,0.04838254,-0.09242464,89.24451 +-0.08067749,0.01959419,0.9965477,47.72631,0.04891325,0.99868,-0.01567626,-4.926346,-0.9955393,0.04747965,-0.08152941,89.19325 +-0.06719239,0.01967589,0.997546,48.28879,0.04841075,0.9986922,-0.01643767,-4.950044,-0.9965648,0.04718745,-0.06805703,89.1528 +-0.0507211,0.02086047,0.998495,48.83147,0.04714178,0.9987174,-0.01847044,-4.973119,-0.9975996,0.04613398,-0.05163944,89.12695 +-0.03001449,0.02349433,0.9992733,49.36647,0.04565938,0.9987123,-0.0221097,-5.001409,-0.998506,0.04496258,-0.03104857,89.12113 +-0.00601273,0.02652506,0.9996301,49.89376,0.04359308,0.9987047,-0.02623831,-5.029978,-0.9990312,0.04341918,-0.007161249,89.13018 +0.02161174,0.02824599,0.9993674,50.4167,0.04145875,0.9987156,-0.02912415,-5.056479,-0.9989064,0.04206193,0.02041294,89.14842 +0.05426707,0.02797547,0.9981345,50.93941,0.03943816,0.9987674,-0.03013741,-5.083698,-0.9977473,0.04100004,0.05309688,89.19154 +0.09163408,0.02556617,0.9954645,51.45679,0.03803664,0.9988509,-0.02915448,-5.111122,-0.995066,0.04053566,0.09055633,89.25586 +0.1328286,0.02208643,0.9908929,51.96384,0.03578779,0.9989928,-0.02706431,-5.139956,-0.9904927,0.03905678,0.1319044,89.33556 +0.1770684,0.01742295,0.9840444,52.46407,0.03263936,0.9991893,-0.02356422,-5.168307,-0.9836571,0.03629105,0.1763561,89.45093 +0.2245365,0.01412619,0.9743633,52.95984,0.0280314,0.9993875,-0.02094867,-5.197886,-0.9740624,0.0320165,0.224003,89.59271 +0.2747903,0.0126181,0.9614214,53.43056,0.02372844,0.9995203,-0.01990011,-5.228395,-0.9612113,0.02828138,0.2743591,89.75037 +0.3285181,0.01239074,0.9444164,53.88945,0.02179716,0.9995481,-0.02069628,-5.254058,-0.9442461,0.02738469,0.3280996,89.94526 +0.3855813,0.01285007,0.9225844,54.33154,0.02342307,0.9994444,-0.02370995,-5.281038,-0.9223765,0.03075187,0.3850661,90.16419 +0.444398,0.01583459,0.8956895,54.73889,0.02326734,0.9993024,-0.02921047,-5.307765,-0.8955272,0.03382138,0.4437196,90.3886 +0.5031692,0.01924202,0.8639737,55.13319,0.02368969,0.9990692,-0.03604743,-5.33687,-0.8638631,0.03860521,0.502245,90.66097 +0.5605451,0.02352431,0.8277897,55.49149,0.01882847,0.998976,-0.04113899,-5.362113,-0.8279098,0.03864626,0.5595281,90.9416 +0.6156356,0.0268268,0.7875742,55.83593,0.01377101,0.9989015,-0.04478976,-5.389737,-0.7879106,0.03841985,0.6145899,91.27112 +0.6681238,0.02787792,0.7435277,56.15845,0.01109601,0.9988134,-0.04742037,-5.418165,-0.7439674,0.03993286,0.6670216,91.6203 +0.7170289,0.02626286,0.6965485,56.44855,0.009622649,0.9988218,-0.04756544,-5.448053,-0.696977,0.04080842,0.7159313,91.96615 +0.7622181,0.02264837,0.646924,56.72244,0.006903411,0.9990465,-0.04310967,-5.479109,-0.6472835,0.03732494,0.7613349,92.35602 +0.8038727,0.01798655,0.5945294,56.96793,0.004662333,0.9993214,-0.03653692,-5.510534,-0.5947831,0.03214292,0.8032433,92.76492 +0.8408561,0.0152942,0.5410427,57.17528,0.00169998,0.9995211,-0.03089649,-5.539453,-0.5412561,0.02689926,0.8404274,93.17127 +0.8725465,0.01221869,0.4883783,57.37005,-0.0004511298,0.9997069,-0.02420557,-5.564449,-0.4885309,0.02090016,0.8722962,93.61062 +0.8986353,0.01248378,0.4385189,57.53992,-0.00475736,0.9998135,-0.01871374,-5.590987,-0.4386707,0.01473063,0.8985271,94.0612 +0.9196509,0.01403544,0.392486,57.68438,-0.00862986,0.9998421,-0.01553372,-5.612666,-0.392642,0.0108985,0.9196268,94.50798 +0.9354383,0.02009299,0.3529185,57.81242,-0.01677232,0.9997816,-0.01246502,-5.628018,-0.3530919,0.005740989,0.935571,94.9816 +0.9478455,0.02467478,0.3177737,57.9272,-0.02141611,0.9996761,-0.01374446,-5.638494,-0.3180099,0.006222142,0.9480669,95.44688 +0.9575689,0.02703186,0.286934,58.04176,-0.02398075,0.9996123,-0.01414319,-5.648502,-0.2872051,0.006662179,0.9578459,95.92488 +0.9641795,0.02129999,0.2643943,58.16224,-0.01820981,0.9997343,-0.01413344,-5.661977,-0.2646251,0.008812601,0.964311,96.40217 +0.9684686,0.01824212,0.2484671,58.27731,-0.0154248,0.9997928,-0.01328105,-5.680139,-0.2486579,0.009029716,0.9685492,96.88437 +0.9715089,0.02227795,0.2359539,58.38327,-0.01962485,0.9997151,-0.01358695,-5.692607,-0.2361893,0.008569285,0.9716692,97.38035 +0.9736636,0.02774803,0.2262947,58.48694,-0.02542298,0.9995898,-0.01318291,-5.69871,-0.2265677,0.007082631,0.9739696,97.88806 +0.9747832,0.03487426,0.2204121,58.58946,-0.03249051,0.9993678,-0.01443214,-5.712881,-0.220776,0.006906903,0.9753,98.40345 +0.9751968,0.04130161,0.2174521,58.69673,-0.03824103,0.9991016,-0.01826602,-5.73231,-0.2180111,0.009497371,0.9759,98.9283 +0.9753059,0.04290914,0.2166502,58.81399,-0.03946071,0.9990165,-0.0202201,-5.756138,-0.2173047,0.01117161,0.9760398,99.46087 +0.974846,0.0420688,0.2188736,58.94036,-0.03790111,0.9990119,-0.02320746,-5.778487,-0.2196337,0.01432814,0.9754771,100.0024 +0.9740428,0.04025106,0.2227568,59.07451,-0.03435475,0.9989508,-0.0302834,-5.797891,-0.223742,0.02184457,0.9744035,100.5505 +0.9731349,0.04014621,0.2267087,59.20976,-0.0329407,0.9988274,-0.03547901,-5.818562,-0.2278672,0.02705792,0.9733161,101.1106 +0.9718871,0.04493798,0.2311193,59.34323,-0.03793231,0.9986787,-0.03466905,-5.84477,-0.2323718,0.02492751,0.9723075,101.6926 +0.9703452,0.048448,0.2368188,59.47979,-0.04230693,0.9986252,-0.03094804,-5.872668,-0.2379925,0.02001121,0.9710608,102.2853 +0.969278,0.0454103,0.2417398,59.63546,-0.04019101,0.998841,-0.02648059,-5.900971,-0.2426622,0.01595128,0.9699797,102.8883 +0.9685083,0.04085678,0.2456065,59.7988,-0.0369093,0.9991051,-0.02065604,-5.928075,-0.2462306,0.01094038,0.9691495,103.4998 +0.967706,0.03817011,0.2491753,59.96454,-0.03530703,0.9992492,-0.01595117,-5.953554,-0.249597,0.006638405,0.968327,104.1244 +0.9670771,0.03374565,0.2522366,60.13424,-0.03096203,0.999408,-0.01499784,-5.97519,-0.2525933,0.006694307,0.9675493,104.7544 +0.967077,0.03012062,0.2526952,60.30914,-0.02652167,0.9994926,-0.01763722,-5.99825,-0.2530982,0.01035465,0.9673851,105.3923 +0.9672372,0.02935759,0.2521715,60.48079,-0.02413441,0.9994258,-0.0237816,-6.023068,-0.2527249,0.01691644,0.9673902,106.0354 +0.9674264,0.03108975,0.2512362,60.64973,-0.02389677,0.9992139,-0.03163137,-6.049018,-0.2520221,0.02459729,0.9674088,106.6923 +0.9679171,0.03485791,0.2488402,60.81912,-0.02666631,0.9989881,-0.03621546,-6.075562,-0.2498508,0.02841791,0.9678672,107.3637 +0.9685648,0.03339321,0.2465099,60.9951,-0.02633527,0.9991449,-0.03187395,-6.103151,-0.2473634,0.02438008,0.9686159,108.0547 +0.9690741,0.02877873,0.2450863,61.17722,-0.02348928,0.9994243,-0.02447838,-6.135437,-0.2456496,0.01796446,0.9691922,108.7607 +0.9696494,0.02911215,0.2427603,61.35508,-0.02561797,0.9995179,-0.01753858,-6.168814,-0.2431539,0.01078724,0.9699277,109.4778 +0.9700047,0.0323765,0.2409204,61.52759,-0.02883405,0.9994182,-0.01821556,-6.198021,-0.24137,0.01072247,0.9703739,110.2002 +0.9702632,0.03845449,0.2389783,61.69627,-0.03397645,0.9991618,-0.02283122,-6.225332,-0.2396559,0.01403265,0.9707564,110.9295 +0.9711702,0.03945438,0.2350997,61.87414,-0.03375909,0.999032,-0.02820239,-6.252301,-0.2359848,0.01945256,0.971562,111.6692 +0.9721966,0.03867136,0.2309511,62.05312,-0.03231967,0.9989897,-0.03122404,-6.282018,-0.2319252,0.02289163,0.9724642,112.4214 +0.9731405,0.03672904,0.2272633,62.23417,-0.03078898,0.9990867,-0.02962862,-6.31524,-0.2281439,0.0218356,0.9733825,113.1891 +0.9741028,0.03235463,0.223779,62.41758,-0.02673172,0.9992473,-0.02811183,-6.351409,-0.2245201,0.02140181,0.9742344,113.9649 +0.9745476,0.03082445,0.2220517,62.60024,-0.02588369,0.9993491,-0.02512708,-6.387404,-0.2226817,0.01874001,0.974711,114.7551 +0.9749759,0.02776787,0.2205697,62.78483,-0.02339025,0.9994746,-0.02243445,-6.419328,-0.2210768,0.01671386,0.9751131,115.5643 +0.9752499,0.02247719,0.2199602,62.97372,-0.01826607,0.9996092,-0.02116033,-6.450506,-0.2203499,0.0166188,0.9752793,116.3732 +0.9753752,0.02305642,0.2193439,63.15739,-0.01920219,0.9996217,-0.01968763,-6.483943,-0.2197148,0.01499094,0.9754489,117.1926 +0.9751303,0.02794458,0.219864,63.33463,-0.0237535,0.9994826,-0.02168323,-6.517585,-0.2203562,0.01592143,0.9752895,118.0121 +0.9748174,0.03407297,0.2203864,63.51246,-0.02930179,0.9992608,-0.02488311,-6.551031,-0.2210713,0.01779877,0.9750952,118.8364 +0.9743646,0.04180173,0.2210574,63.6913,-0.03686288,0.9989709,-0.02642226,-6.583319,-0.2219344,0.01759609,0.9749028,119.6659 +0.9734172,0.04884577,0.2237701,63.87033,-0.04420333,0.9986916,-0.02571202,-6.619743,-0.2247332,0.01513714,0.9743027,120.5026 +0.9724994,0.05172831,0.2270885,64.05736,-0.0477212,0.9985934,-0.02310429,-6.658079,-0.2279642,0.01163197,0.9736,121.3427 +0.9719058,0.05218735,0.2295117,64.25325,-0.04802517,0.9985652,-0.0236874,-6.695256,-0.2304186,0.01199958,0.9730176,122.1809 +0.9714761,0.0520124,0.2313632,64.44981,-0.04701719,0.9985276,-0.02705593,-6.728166,-0.2324297,0.01540614,0.9724911,123.0215 +0.9716103,0.04466236,0.2323333,64.65586,-0.03970179,0.9988736,-0.0259859,-6.752867,-0.2332322,0.01602412,0.972289,123.8767 +0.9718221,0.03802642,0.2326284,64.8645,-0.03279916,0.9991159,-0.02629888,-6.784235,-0.2334227,0.01792781,0.97221,124.7241 +0.971516,0.03673359,0.2341097,65.06629,-0.03144821,0.99916,-0.02627103,-6.818192,-0.2348781,0.01816039,0.9718551,125.5712 +0.9711204,0.04003934,0.2352064,65.26209,-0.03523136,0.9990761,-0.0246101,-6.849345,-0.2359744,0.01561273,0.9716338,126.4226 +0.970795,0.04129353,0.2363301,65.45904,-0.03782412,0.9991,-0.01919733,-6.878751,-0.2369101,0.009697688,0.9714831,127.2746 +0.9703421,0.04468201,0.2375705,65.65809,-0.04268596,0.9989967,-0.01354208,-6.913045,-0.2379372,0.002999522,0.9712758,128.1205 +0.9701141,0.0443127,0.2385687,65.86477,-0.04089052,0.9989776,-0.01927718,-6.955017,-0.2391791,0.008945864,0.9709342,128.941 +0.9702979,0.04107535,0.2384005,66.08066,-0.03412526,0.9988656,-0.03320919,-7.000709,-0.2394942,0.02408732,0.9705989,129.7485 +0.9706399,0.03734387,0.2376208,66.29836,-0.02778327,0.9986688,-0.04345839,-7.046839,-0.2389273,0.03558056,0.9703853,130.5573 +0.9708707,0.03562799,0.2369405,66.51051,-0.02737139,0.998901,-0.03804652,-7.092137,-0.2380356,0.03045286,0.9707789,131.3828 +0.9711965,0.02941011,0.2364583,66.72265,-0.02649658,0.9995289,-0.01549055,-7.134533,-0.2368024,0.008779029,0.9715181,132.2254 +0.9714734,0.02591697,0.2357283,66.93208,-0.02747143,0.9996171,0.003311926,-7.177765,-0.2355522,-0.009693239,0.9718133,133.0601 +0.9715736,0.02883383,0.2349753,67.13546,-0.02883399,0.9995783,-0.003435808,-7.203471,-0.2349753,-0.003437134,0.9719952,133.8675 +0.9716942,0.03680518,0.2333577,67.33397,-0.0332056,0.9992614,-0.01933645,-7.221705,-0.233897,0.01104033,0.9721986,134.6626 +0.9719789,0.03798216,0.2319793,67.53946,-0.0322059,0.9990709,-0.02863797,-7.240417,-0.2328515,0.0203644,0.972299,135.4599 +0.9725903,0.03392131,0.2300382,67.74822,-0.02784568,0.9991736,-0.02960745,-7.275568,-0.2308524,0.02239035,0.9727311,136.2625 +0.9732069,0.03360055,0.227463,67.9399,-0.02840108,0.9992559,-0.02609403,-7.307651,-0.2281706,0.01893469,0.973437,137.0723 +0.9740244,0.0331691,0.2240007,68.12659,-0.02766976,0.9992347,-0.02764591,-7.329594,-0.2247463,0.02072974,0.9741967,137.8726 +0.9751147,0.03373626,0.2191191,68.31148,-0.0278567,0.9991656,-0.02986795,-7.347445,-0.2199439,0.02302074,0.9752408,138.6688 +0.9756333,0.0383979,0.2160215,68.48738,-0.0330003,0.9990478,-0.02853953,-7.375994,-0.2169116,0.02071534,0.9759714,139.4818 +0.9758225,0.04488478,0.2139062,68.65535,-0.03922057,0.9987603,-0.03065279,-7.400654,-0.2150169,0.02152215,0.9763731,140.2662 +0.9761098,0.04665322,0.2122104,68.8308,-0.04167268,0.9987421,-0.02788476,-7.431426,-0.2132444,0.0183752,0.976826,141.0669 +0.9764697,0.04461131,0.2109903,69.01055,-0.04067802,0.9989087,-0.02294787,-7.461305,-0.2117838,0.01382523,0.9772187,141.8662 +0.9767943,0.04343929,0.2097286,69.18789,-0.03956073,0.9989603,-0.02265516,-7.492154,-0.2104946,0.01383241,0.9774971,142.6598 +0.977464,0.0415636,0.2069702,69.35983,-0.03690635,0.9989722,-0.02631422,-7.522327,-0.2078512,0.01808269,0.9779932,143.451 +0.9785757,0.03715735,0.2025067,69.53255,-0.03191867,0.9990675,-0.02907493,-7.548491,-0.2033982,0.02198827,0.9788491,144.2346 +0.9800402,0.03397379,0.195875,69.69416,-0.02847149,0.9991187,-0.03083931,-7.579826,-0.1967501,0.02464691,0.9801438,145.0284 +0.9813433,0.03290257,0.1894277,69.84462,-0.02849114,0.9992567,-0.02596524,-7.612956,-0.1901412,0.0200838,0.9815513,145.8298 +0.9825216,0.03087594,0.1835705,69.99277,-0.02824613,0.9994577,-0.01692414,-7.64599,-0.1839935,0.01144318,0.9828608,146.6342 +0.9833609,0.02646368,0.1797248,70.1315,-0.02542474,0.999644,-0.008082177,-7.668862,-0.1798747,0.00337824,0.9836837,147.4395 +0.9838322,0.03363794,0.1759058,70.25738,-0.0335154,0.9994314,-0.003668401,-7.690397,-0.1759292,-0.002286463,0.9844001,148.2407 +0.9845163,0.03687818,0.1713701,70.37981,-0.03585879,0.9993159,-0.00904122,-7.71208,-0.1715862,0.002756104,0.9851652,149.033 +0.9857542,0.03716732,0.1640347,70.50797,-0.03448744,0.9992215,-0.01915602,-7.73271,-0.1646189,0.01322599,0.9862685,149.8199 +0.9869512,0.03436872,0.1573096,70.63407,-0.03068028,0.9991958,-0.02581629,-7.751909,-0.1580703,0.02065311,0.9872118,150.6153 +0.9880243,0.03297858,0.150733,70.75332,-0.02956337,0.9992541,-0.024843,-7.779012,-0.1514399,0.02008931,0.9882622,151.422 +0.9888875,0.03991791,0.1432067,70.85703,-0.03731589,0.9990868,-0.0208108,-7.812429,-0.1439066,0.01523565,0.9894739,152.237 +0.9897183,0.04586975,0.1354761,70.95263,-0.04336929,0.9988309,-0.02135247,-7.841404,-0.1362971,0.01525743,0.9905505,153.0488 +0.9909259,0.04849401,0.1253563,71.05219,-0.04546236,0.9986028,-0.02693473,-7.868414,-0.1264873,0.02099133,0.991746,153.8549 +0.9920486,0.04708364,0.1167168,71.14828,-0.04394991,0.9986045,-0.02928025,-7.891249,-0.1179326,0.02391773,0.9927335,154.6649 +0.9929566,0.04708196,0.108722,71.23668,-0.04431524,0.9986327,-0.02772649,-7.916918,-0.1098787,0.02271316,0.9936854,155.4826 +0.9935935,0.05028742,0.1012092,71.30611,-0.04847236,0.9986179,-0.02031541,-7.939937,-0.1020909,0.01527941,0.9946577,156.3078 +0.994188,0.05348146,0.09343488,71.36837,-0.05238072,0.9985263,-0.01419561,-7.966304,-0.09405638,0.009218918,0.9955241,157.136 +0.9949035,0.05253998,0.08606248,71.43075,-0.05185407,0.9986027,-0.01018773,-7.990297,-0.08647748,0.00567312,0.9962376,157.9586 +0.9954223,0.05249755,0.07986537,71.4907,-0.05197701,0.9986114,-0.008584225,-8.012612,-0.08020511,0.004393765,0.9967686,158.78 +0.9958028,0.05184121,0.07542759,71.54565,-0.05127576,0.9986401,-0.009415322,-8.036337,-0.07581312,0.005508196,0.9971068,159.6018 +0.9958975,0.05644942,0.07072225,71.59206,-0.05546812,0.998336,-0.01576487,-8.062682,-0.07149448,0.01177736,0.9973714,160.4205 +0.9957357,0.06336806,0.06704397,71.62754,-0.06207913,0.9978473,-0.02113905,-8.085177,-0.06823918,0.01688687,0.997526,161.2383 +0.99586,0.0643766,0.06417679,71.66501,-0.06289692,0.9977114,-0.02481823,-8.105358,-0.06562762,0.02067896,0.9976298,162.0549 +0.995925,0.0661254,0.06132616,71.70556,-0.06512064,0.9977106,-0.01824263,-8.124046,-0.06239206,0.01417469,0.997951,162.8891 +0.9958828,0.06825485,0.05965502,71.73958,-0.06785743,0.9976574,-0.008664995,-8.140372,-0.06010669,0.004581283,0.9981814,163.7286 +0.9960001,0.06906583,0.05669062,71.7754,-0.06916226,0.9976054,-0.00026162,-8.158203,-0.05657294,-0.003660277,0.9983917,164.5638 +0.9964269,0.06676599,0.05172834,71.81125,-0.06671062,0.9977684,-0.00279817,-8.173449,-0.05179972,-0.0006626575,0.9986572,165.3868 +0.9973506,0.05623046,0.04615245,71.85566,-0.05577477,0.9983816,-0.01110362,-8.188372,-0.04670211,0.008500062,0.9988726,166.201 +0.9979389,0.05110589,0.03881003,71.88912,-0.05043294,0.9985629,-0.01812578,-8.197125,-0.03968059,0.01613112,0.9990821,167.0187 +0.9979639,0.05517487,0.03199794,71.90149,-0.05468822,0.998377,-0.0158903,-8.209715,-0.03282275,0.01410803,0.9993616,167.8465 +0.9982641,0.05341943,0.02480255,71.91447,-0.0531693,0.9985288,-0.01063788,-8.225974,-0.02533433,0.009300678,0.9996357,168.6811 +0.9986378,0.04912776,0.01758187,71.93066,-0.04903568,0.9987811,-0.005630728,-8.240659,-0.01783706,0.004760919,0.9998295,169.5173 +0.9990975,0.0407869,0.01185966,71.9442,-0.04075076,0.999164,-0.003273653,-8.251269,-0.01198327,0.002787409,0.9999243,170.3582 +0.999326,0.03624932,0.005790517,71.95036,-0.03623788,0.999341,-0.002069112,-8.265898,-0.005861705,0.001857882,0.999981,171.2012 +0.9994607,0.0328324,-0.0005780755,71.94712,-0.03283589,0.9994309,-0.007716679,-8.281895,0.0003243895,0.007731499,0.99997,172.0379 +0.9996479,0.0252859,-0.008050398,71.94423,-0.02542603,0.9995181,-0.01780696,-8.295895,0.007596253,0.01800538,0.999809,172.867 +0.999799,0.01295244,-0.01530398,71.9461,-0.01333594,0.9995928,-0.02522779,-8.314995,0.01497098,0.02542681,0.9995645,173.6993 +0.9996436,0.01605209,-0.02133065,71.92242,-0.01640104,0.9997328,-0.01628577,-8.332767,0.02106353,0.0166298,0.9996398,174.5517 +0.9993113,0.02637007,-0.02610724,71.88077,-0.02645667,0.9996455,-0.002976997,-8.350218,0.02601948,0.003665657,0.9996547,175.4112 +0.9990013,0.03251977,-0.03064244,71.83806,-0.03236068,0.9994601,0.005673991,-8.369208,0.03081041,-0.004676713,0.9995143,176.2661 +0.9988864,0.03211212,-0.03456697,71.80048,-0.03201024,0.9994814,0.003496713,-8.380943,0.03466132,-0.002386321,0.9993962,177.1104 +0.9987805,0.03298445,-0.03673766,71.7593,-0.03325968,0.9994229,-0.006905757,-8.392488,0.03648868,0.008119217,0.999301,177.9477 +0.9982576,0.04360448,-0.03975662,71.70504,-0.04429609,0.9988791,-0.01668374,-8.408732,0.03898457,0.01841573,0.99907,178.7853 +0.9981639,0.04382976,-0.04180851,71.66001,-0.04470847,0.9987934,-0.02031881,-8.427651,0.0408675,0.0221507,0.998919,179.6316 +0.998319,0.03709972,-0.04452873,71.62938,-0.03785446,0.9991515,-0.01622741,-8.451315,0.04388891,0.01788574,0.9988762,180.4893 +0.9982984,0.03770733,-0.0444814,71.58675,-0.03805675,0.9992508,-0.007034446,-8.471918,0.04418282,0.008715292,0.9989854,181.3511 +0.9982447,0.03981282,-0.04384633,71.53819,-0.03987331,0.9992046,-0.0005053899,-8.495182,0.04379133,0.002252801,0.9990381,182.2141 +0.9982172,0.03836984,-0.04571952,71.49582,-0.03851832,0.9992551,-0.00237055,-8.51899,0.0455945,0.004127362,0.9989514,183.0694 +0.9981548,0.03713477,-0.0480433,71.45164,-0.03766269,0.9992391,-0.01012985,-8.543212,0.04763058,0.0119206,0.9987938,183.918 +0.9983567,0.02690662,-0.05059681,71.41963,-0.02803423,0.9993712,-0.02171003,-8.567846,0.04998085,0.02309279,0.9984831,184.7628 +0.9984731,0.01547705,-0.05302878,71.38986,-0.0169001,0.9995061,-0.02649288,-8.596086,0.05259256,0.02734862,0.9982414,185.611 +0.998389,0.01720848,-0.05406808,71.34131,-0.01842183,0.9995877,-0.02202345,-8.625507,0.0536668,0.022984,0.9982943,186.4714 +0.9982214,0.02292478,-0.05503216,71.28399,-0.02362423,0.9996477,-0.0120931,-8.65918,0.05473554,0.01337168,0.9984113,187.3332 +0.9980787,0.02529689,-0.05656048,71.2292,-0.02571834,0.9996465,-0.006735662,-8.688115,0.05637009,0.008177361,0.9983764,188.1818 +0.9980717,0.02293392,-0.05768076,71.18264,-0.02358163,0.999666,-0.0105736,-8.712856,0.057419,0.01191341,0.998279,189.0202 +0.9980532,0.02327322,-0.05786424,71.13685,-0.02430321,0.9995573,-0.0171604,-8.736466,0.05743924,0.01853328,0.9981769,189.8562 +0.9978588,0.03003041,-0.05810388,71.07577,-0.03115242,0.9993434,-0.01850161,-8.761375,0.05751011,0.02027207,0.998139,190.6947 +0.9977393,0.03329039,-0.05837976,71.01964,-0.03417387,0.999315,-0.01420047,-8.788561,0.05786702,0.01616343,0.9981934,191.5357 +0.9975674,0.03693481,-0.0591201,70.96425,-0.03766129,0.9992275,-0.01122112,-8.81671,0.05865998,0.01342036,0.9981878,192.3732 +0.9974328,0.04009581,-0.05933112,70.90819,-0.04066337,0.9991377,-0.008389275,-8.838886,0.05894358,0.01078034,0.998203,193.2083 +0.9972334,0.04411351,-0.05982968,70.84733,-0.04469685,0.9989649,-0.008446293,-8.861377,0.05939515,0.01109712,0.9981728,194.04 +0.9972752,0.04218303,-0.06052129,70.79115,-0.0429119,0.9990205,-0.01079394,-8.884907,0.06000668,0.01336161,0.9981085,194.8635 +0.9971203,0.04409215,-0.06170116,70.73224,-0.04521326,0.9988345,-0.01689265,-8.906076,0.06088441,0.01963371,0.9979516,195.6763 +0.9969926,0.04514267,-0.06299117,70.67083,-0.0463303,0.9987725,-0.01752171,-8.923746,0.06212287,0.02038741,0.9978602,196.4897 +0.997178,0.04511427,-0.06000663,70.62911,-0.04628586,0.998761,-0.01827907,-8.938874,0.05910763,0.02100494,0.9980305,197.1957 +0.9973545,0.04508963,-0.0570171,70.58733,-0.046241,0.9987489,-0.01903726,-8.954024,0.05608737,0.02162342,0.9981916,197.9026 +0.9975218,0.0450688,-0.0540291,70.54557,-0.04619582,0.9987362,-0.01979462,-8.969162,0.05306869,0.02224148,0.9983431,198.6091 +0.99768,0.04505178,-0.0510401,70.5038,-0.04615029,0.998723,-0.02055182,-8.984301,0.05004903,0.02285965,0.9984851,199.3157 +0.997829,0.04503855,-0.04805043,70.46202,-0.04610441,0.9987093,-0.02130877,-8.999441,0.0470287,0.02347784,0.9986175,200.0222 +0.9979689,0.04502913,-0.04506211,70.42027,-0.04605821,0.998695,-0.02206497,-9.01457,0.04400973,0.02409563,0.9987404,200.7284 +0.9980995,0.0450235,-0.04207462,70.37852,-0.04601169,0.9986802,-0.02282056,-9.029691,0.04099163,0.02471311,0.9988538,201.4342 +0.9982214,0.04502167,-0.03907941,70.33667,-0.04596471,0.9986648,-0.0235777,-9.044849,0.03796572,0.02533204,0.9989578,202.1417 +0.9983338,0.04502363,-0.03609049,70.29491,-0.04591749,0.9986488,-0.02433286,-9.059972,0.03494617,0.0259495,0.9990522,202.8477 +0.9984373,0.0450294,-0.03309705,70.25308,-0.04586987,0.9986323,-0.02508878,-9.075116,0.03192205,0.02656773,0.9991371,203.5546 +0.9985315,0.04503897,-0.03010522,70.21128,-0.04582193,0.9986152,-0.02584392,-9.090248,0.02889955,0.02718544,0.9992125,204.261 +0.9986166,0.04505231,-0.02711672,70.16952,-0.04577371,0.9985977,-0.02659784,-9.105362,0.0258804,0.02780227,0.9992783,204.9666 +0.9986926,0.04506947,-0.02412324,70.12768,-0.04572507,0.9985795,-0.02735266,-9.120498,0.0228562,0.02841993,0.9993347,205.6732 +0.9987594,0.0450904,-0.02113323,70.0859,-0.04567615,0.9985608,-0.02810624,-9.135614,0.01983549,0.02903665,0.9993815,206.379 +0.9988171,0.04511514,-0.01813963,70.04406,-0.04562684,0.9985415,-0.02886037,-9.150747,0.01681113,0.02965389,0.9994188,207.0856 +0.9988567,0.04517153,-0.01565232,69.99905,-0.04562219,0.9985159,-0.02974193,-9.16529,0.0142856,0.03042201,0.999435,207.7971 +0.998961,0.0448615,-0.008029166,69.98606,-0.04507788,0.9985573,-0.02917563,-9.185709,0.00670872,0.02950725,0.999542,208.4453 +0.999135,0.04154088,-0.001912342,69.98306,-0.04157468,0.9988516,-0.02381015,-9.208113,0.0009210518,0.02386905,0.9997146,209.0931 +0.9994026,0.03430069,0.004241041,69.99464,-0.03422306,0.9992663,-0.01719175,-9.235771,-0.004827618,0.01703633,0.9998432,209.74 +0.99956,0.02799323,0.009815266,70.00778,-0.02783387,0.9994843,-0.01601343,-9.261371,-0.01025847,0.01573319,0.9998235,210.3757 +0.9995027,0.02847329,0.01355659,70.01564,-0.02827301,0.9994915,-0.01474345,-9.283511,-0.01396949,0.01435283,0.9997993,211.0139 +0.9993972,0.03154472,0.01450058,70.01951,-0.03133748,0.9994065,-0.01430386,-9.30223,-0.01494319,0.01384082,0.9997925,211.6498 +0.9993906,0.03173357,0.01454168,70.02608,-0.03154475,0.9994173,-0.01303544,-9.317034,-0.01494687,0.01256878,0.9998092,212.2959 +0.9993983,0.03193726,0.0135355,70.03039,-0.0318025,0.9994435,-0.0100572,-9.333416,-0.01384917,0.009620679,0.9998578,212.9445 +0.9993448,0.03345592,0.01380943,70.03031,-0.03335812,0.9994171,-0.007253049,-9.346935,-0.01404404,0.006787639,0.9998783,213.6019 +0.999017,0.04213031,0.01378874,70.01762,-0.04197964,0.9990574,-0.01103998,-9.361494,-0.01424087,0.01045028,0.9998439,214.2524 +0.9988099,0.04710767,0.01263911,70.00789,-0.04684375,0.9986934,-0.02042305,-9.378064,-0.01358467,0.01980668,0.9997115,214.9028 +0.9989009,0.04575401,0.01018246,70.00644,-0.04544509,0.9985532,-0.02874302,-9.397152,-0.01148284,0.02824868,0.9995349,215.5565 +0.9992198,0.03894455,0.00656922,70.01323,-0.03869261,0.9986417,-0.03489495,-9.415421,-0.007919264,0.03461354,0.9993693,216.2172 +0.9991864,0.04023503,0.002775105,70.00612,-0.04014078,0.9987944,-0.02825507,-9.429329,-0.003908602,0.02812068,0.9995968,216.8933 +0.9988357,0.04823388,-0.0008642295,69.98281,-0.04824156,0.9987046,-0.01617815,-9.452115,8.277524E-05,0.016201,0.9998687,217.5788 +0.9989962,0.04456999,-0.004497923,69.97211,-0.0445797,0.9990036,-0.002080989,-9.473975,0.004400691,0.002279416,0.9999877,218.2707 +0.9992283,0.03837875,-0.008362817,69.96763,-0.03830045,0.9992227,0.009330271,-9.487565,0.0087144,-0.009002769,0.9999214,218.9643 +0.9993067,0.035389,-0.01156541,69.95651,-0.03531205,0.9993532,0.006791368,-9.495411,0.01179826,-0.006378259,0.99991,219.6463 +0.9990883,0.04036912,-0.01389429,69.93436,-0.04034287,0.9991835,0.002164684,-9.510323,0.01397033,-0.001602174,0.9999011,220.3174 +0.9990959,0.0383368,-0.0183781,69.94019,-0.03847501,0.9992334,-0.007226487,-9.540496,0.01808697,0.00792705,0.9998049,220.9704 +0.9993118,0.03052516,-0.02107665,69.96054,-0.0308139,0.9994338,-0.01351338,-9.567751,0.02065221,0.01415353,0.9996865,221.6247 +0.9995047,0.02253595,-0.02196621,69.97285,-0.02273633,0.9997017,-0.008915106,-9.591872,0.02175874,0.00941012,0.9997189,222.3055 +0.9996015,0.01534279,-0.02369614,69.98119,-0.01543214,0.9998744,-0.00359259,-9.615886,0.02363804,0.003956841,0.9997127,222.9918 +0.9996094,0.01212864,-0.02518004,69.9843,-0.01213886,0.9999263,-0.0002531299,-9.650603,0.02517511,0.0005586885,0.9996828,223.6723 +0.9996062,0.009568085,-0.02638256,69.98163,-0.009648184,0.9999492,-0.002910422,-9.68375,0.02635337,0.003163819,0.9996476,224.3537 +0.9996142,0.003849148,-0.02751025,69.98542,-0.004102379,0.9999497,-0.009154463,-9.716608,0.02747363,0.009263787,0.9995795,225.0278 +0.9995754,-0.004678655,-0.02876161,69.98607,0.004383364,0.9999371,-0.01032134,-9.742245,0.02880809,0.01019089,0.999533,225.7134 +0.9995281,-0.007405982,-0.02981397,69.98187,0.007180519,0.9999448,-0.007662325,-9.771343,0.02986907,0.007444628,0.999526,226.3991 +0.9995051,-0.008326333,-0.03033594,69.96949,0.00824386,0.999962,-0.002842732,-9.792795,0.03035846,0.00259124,0.9995357,227.0898 +0.999467,-0.00899109,-0.0313853,69.96118,0.008999502,0.9999595,0.0001267407,-9.818999,0.03138288,-0.0004091245,0.9995073,227.7732 +0.9994428,-0.01020901,-0.03178101,69.94919,0.0101548,0.9999467,-0.001866652,-9.840708,0.03179837,0.001542882,0.9994931,228.4545 +0.9994092,-0.01323742,-0.03171797,69.93862,0.01297005,0.9998787,-0.008620756,-9.863524,0.03182824,0.008204279,0.9994596,229.1258 +0.99938,-0.01491247,-0.0318952,69.92429,0.01432713,0.9997261,-0.01850238,-9.884742,0.03216238,0.01803394,0.9993199,229.7927 +0.9993829,-0.01412937,-0.03216079,69.90717,0.01335457,0.9996184,-0.02417986,-9.909781,0.03249016,0.02373544,0.9991901,230.4543 +0.999443,-0.01049593,-0.03167951,69.88635,0.009741052,0.9996671,-0.02388948,-9.937611,0.0319197,0.02356758,0.9992125,231.1122 +0.9994183,-0.01238982,-0.03177627,69.87145,0.01184129,0.9997786,-0.01739299,-9.964503,0.03198473,0.0170066,0.9993436,231.769 +0.9993685,-0.01429766,-0.0325299,69.85983,0.01401429,0.999862,-0.008922511,-9.992889,0.03265298,0.008460992,0.9994309,232.4182 +0.9993512,-0.01492646,-0.03277765,69.84446,0.0147921,0.9998812,-0.004337658,-10.01409,0.03283849,0.003849993,0.9994532,233.0405 +0.999353,-0.01359056,-0.0332998,69.8271,0.01357282,0.9999076,-0.0007585932,-10.03357,0.03330703,0.0003061307,0.9994451,233.65 +0.9992501,-0.01502281,-0.03568876,69.8102,0.01510574,0.9998838,0.002054942,-10.04766,0.03565374,-0.002592505,0.9993608,234.2476 +0.9989381,-0.02157956,-0.04070775,69.80096,0.0216815,0.9997628,0.002064218,-10.05898,0.04065355,-0.00294463,0.9991689,234.8182 +0.9984666,-0.02652601,-0.0485884,69.78279,0.02653046,0.9996478,-0.0005535584,-10.06888,0.04858597,-0.0007363619,0.9988187,235.3659 +0.9978259,-0.02916094,-0.05910287,69.75496,0.02896311,0.9995716,-0.004201377,-10.07841,0.05920007,0.00248044,0.998243,235.8968 +0.9968748,-0.02993489,-0.07310717,69.71456,0.02942164,0.9995343,-0.008087593,-10.08536,0.07331523,0.005911384,0.9972912,236.4174 +0.9953043,-0.03131877,-0.09158957,69.66405,0.03043549,0.9994759,-0.01102519,-10.08899,0.09188686,0.008185844,0.9957358,236.9317 +0.9929139,-0.03522832,-0.1134947,69.60296,0.0342721,0.9993588,-0.010366,-10.09296,0.1137871,0.006402846,0.9934845,237.4372 +0.9899005,-0.03275755,-0.1379279,69.52087,0.03196483,0.9994573,-0.007959044,-10.09963,0.1381138,0.003469819,0.9904102,237.9427 +0.9856528,-0.03228651,-0.1656692,69.42542,0.03177041,0.9994785,-0.005765007,-10.1059,0.1657689,0.0004189171,0.9861645,238.4416 +0.9797313,-0.03394986,-0.1974186,69.315,0.03353493,0.9994227,-0.005445507,-10.11569,0.1974895,-0.001285283,0.9803041,238.9201 +0.9719757,-0.03229428,-0.2328528,69.18511,0.03113808,0.9994777,-0.008640477,-10.12556,0.2330102,0.001147745,0.9724735,239.3949 +0.961972,-0.02748637,-0.2717618,69.02217,0.02486868,0.9996052,-0.01307228,-10.13488,0.2720138,0.00581681,0.9622757,239.8316 +0.949156,-0.02367706,-0.3139146,68.84642,0.0194374,0.9996728,-0.01662934,-10.14043,0.3142056,0.009682159,0.9493055,240.2766 +0.9334436,-0.01890486,-0.3582258,68.64775,0.01334384,0.9997491,-0.01798977,-10.14643,0.358476,0.01201233,0.9334616,240.7091 +0.914751,-0.008804964,-0.4039222,68.41495,0.001345196,0.9998233,-0.01874838,-10.15095,0.4040159,0.01660675,0.9146011,241.0925 +0.8926575,0.004783842,-0.4507104,68.16609,-0.01452077,0.9997298,-0.01814807,-10.16774,0.4505018,0.02274467,0.8924857,241.4794 +0.8668268,0.01766589,-0.4982963,67.89906,-0.02937527,0.9994456,-0.01566773,-10.18148,0.4977433,0.0282188,0.8668652,241.8486 +0.8370897,0.02722037,-0.5463881,67.60937,-0.04034109,0.9991135,-0.01202968,-10.18159,0.5455762,0.03211181,0.8374457,242.1616 +0.8037197,0.03418704,-0.5940252,67.31827,-0.04804963,0.9988166,-0.007528043,-10.18322,0.5930648,0.03459312,0.8044112,242.4928 +0.7676768,0.04234926,-0.6394364,67.0089,-0.05366195,0.9985577,0.001709552,-10.18416,0.6385865,0.03300102,0.768842,242.8099 +0.7298087,0.05429701,-0.6814919,66.67291,-0.05888986,0.9981288,0.01645961,-10.17111,0.6811104,0.0281206,0.7316405,243.0702 +0.6901781,0.06672397,-0.7205569,66.33224,-0.06457326,0.9974463,0.03051326,-10.16386,0.7207527,0.02546912,0.6927241,243.3442 +0.6485969,0.0763725,-0.7572908,65.98247,-0.06981509,0.9967283,0.04072516,-10.15157,0.7579234,0.02645611,0.6518068,243.5912 +0.6054692,0.08074954,-0.7917618,65.62105,-0.07027296,0.9963781,0.04787927,-10.13305,0.7927602,0.02665002,0.6089506,243.7845 +0.559859,0.08423029,-0.8242956,65.26011,-0.07033104,0.9960603,0.05401338,-10.11788,0.8255976,0.02773368,0.5635773,243.9919 +0.5123307,0.08230045,-0.8548356,64.89784,-0.06435976,0.9962777,0.0573451,-10.09595,0.8563732,0.02563736,0.5157205,244.1844 +0.4625216,0.07638311,-0.8833116,64.52042,-0.05635113,0.9968002,0.05669019,-10.07158,0.8848154,0.02355517,0.4653459,244.32 +0.4108591,0.06909842,-0.9090766,64.1403,-0.04771502,0.9973869,0.05424594,-10.04135,0.9104494,0.02108918,0.4130824,244.4743 +0.3578203,0.06312724,-0.9316543,63.73047,-0.0431374,0.9977646,0.051039,-10.01139,0.9327935,0.02192636,0.3597435,244.5719 +0.3070666,0.06126535,-0.949714,63.3085,-0.04192552,0.9978277,0.05081356,-9.982249,0.950764,0.02421411,0.3089682,244.6831 +0.258365,0.06223652,-0.9640406,62.86751,-0.04237868,0.9976921,0.05305142,-9.956809,0.9651173,0.02714814,0.2604062,244.7727 +0.2119977,0.06361948,-0.9751972,62.40525,-0.04089409,0.9975822,0.05618988,-9.929802,0.9764141,0.02796767,0.2140868,244.8251 +0.1692436,0.06258446,-0.9835852,61.93966,-0.03816279,0.9976495,0.05691277,-9.901649,0.9848351,0.02790424,0.1712342,244.8844 +0.1290802,0.0616505,-0.9897159,61.45894,-0.03658882,0.997682,0.05737475,-9.873647,0.9909589,0.02880659,0.1310367,244.926 +0.09339541,0.06112447,-0.9937511,60.96195,-0.035776,0.9976751,0.05800353,-9.844318,0.9949861,0.03013517,0.09536506,244.9414 +0.0640376,0.05829931,-0.9962432,60.4615,-0.03642959,0.9977633,0.05604662,-9.81181,0.9972823,0.03270364,0.06601818,244.9539 +0.03857869,0.05591526,-0.9976899,59.94742,-0.0360955,0.9978595,0.05452904,-9.779054,0.9986034,0.03390845,0.0405144,244.952 +0.01584437,0.05443454,-0.9983917,59.42184,-0.03716079,0.9978592,0.05381578,-9.749152,0.9991836,0.03624835,0.01783327,244.9457 +-0.003072926,0.05313575,-0.9985826,58.88559,-0.03932738,0.9978083,0.05321559,-9.72066,0.9992216,0.03943516,-0.0009765028,244.9283 +-0.01815342,0.05229163,-0.9984669,58.33537,-0.0421253,0.9977046,0.05301761,-9.69156,0.9989474,0.04302316,-0.01590895,244.9021 +-0.02862872,0.05060338,-0.9983084,57.77118,-0.04374597,0.9976975,0.05182693,-9.660943,0.9986324,0.0451557,-0.02634911,244.8764 +-0.03531846,0.04863119,-0.9981922,57.19241,-0.04399184,0.9977715,0.05016724,-9.62949,0.9984074,0.04568413,-0.03310038,244.8503 +-0.03900133,0.04809847,-0.9980809,56.59598,-0.0443245,0.9977744,0.04981575,-9.598446,0.9982556,0.04618231,-0.03678258,244.824 +-0.04089244,0.04857652,-0.9979821,55.98115,-0.04394532,0.9977635,0.05036655,-9.567377,0.9981966,0.04591625,-0.03866627,244.7978 +-0.04214587,0.04899243,-0.9979096,55.34817,-0.0436339,0.9977538,0.05082763,-9.534743,0.9981582,0.04568486,-0.03991346,244.7709 +-0.04211898,0.04942055,-0.9978896,54.69404,-0.04335224,0.9977448,0.0512432,-9.501856,0.9981716,0.04541906,-0.0398815,244.7438 +-0.04129111,0.04970212,-0.9979102,54.02129,-0.04265036,0.9977639,0.05145961,-9.467967,0.9982364,0.04468605,-0.03907896,244.7179 +-0.04120315,0.04791619,-0.9980012,53.33773,-0.04132406,0.997913,0.04961806,-9.428068,0.9982958,0.04328588,-0.03913707,244.6758 +-0.04156077,0.04509514,-0.9981178,52.65042,-0.03886575,0.9981518,0.04671503,-9.376197,0.9983797,0.04073411,-0.0397313,244.6048 +-0.04172997,0.04367408,-0.998174,51.94411,-0.0379923,0.9982522,0.04526583,-9.321575,0.9984063,0.03981186,-0.03999775,244.5279 +-0.04177101,0.04275942,-0.9982118,51.20668,-0.03860585,0.9982686,0.04437736,-9.271451,0.998381,0.0403905,-0.04004792,244.4614 +-0.04144332,0.04258583,-0.9982329,50.45197,-0.03928489,0.9982492,0.04421751,-9.222874,0.9983682,0.04104799,-0.03969778,244.3907 +-0.04068132,0.04334719,-0.9982315,49.66726,-0.03774082,0.9982789,0.04488733,-9.176746,0.9984591,0.03950015,-0.03897534,244.3343 +-0.0397613,0.04369248,-0.9982535,48.86814,-0.03523235,0.9983609,0.04510052,-9.139656,0.9985878,0.03696407,-0.03815674,244.2845 +-0.03861235,0.04372656,-0.9982971,48.05807,-0.0323609,0.9984633,0.04498551,-9.108985,0.9987301,0.03404279,-0.03713799,244.235 +-0.03772664,0.0438267,-0.9983266,47.22454,-0.03113691,0.9985011,0.04501103,-9.074933,0.9988028,0.03278292,-0.03630546,244.1891 +-0.03715853,0.04606859,-0.998247,46.38186,-0.03041643,0.9984218,0.04720889,-9.03982,0.9988463,0.03211732,-0.03569864,244.141 +-0.03668499,0.05022321,-0.9980641,45.51505,-0.02858509,0.9982749,0.05128451,-9.002662,0.9989179,0.03041113,-0.03518607,244.1009 +-0.03616752,0.05496909,-0.9978328,44.65008,-0.027487,0.9980536,0.05597756,-8.966351,0.9989676,0.029452,-0.03458619,244.0565 +-0.0358849,0.05657377,-0.9977533,43.78198,-0.02660232,0.9979885,0.05754389,-8.926032,0.9990018,0.02860751,-0.03430772,244.016 +-0.03699745,0.05340815,-0.9978872,42.92179,-0.02705227,0.9981513,0.05442528,-8.888812,0.9989491,0.02900871,-0.03548424,243.9692 +-0.03848837,0.04919261,-0.9980475,42.06014,-0.02626256,0.9983927,0.05022242,-8.851693,0.9989138,0.02814427,-0.03713458,243.9267 +-0.04063027,0.04763625,-0.9980381,41.19342,-0.02332629,0.9985454,0.04861009,-8.814679,0.9989019,0.02525557,-0.03945999,243.8841 +-0.04328305,0.05235258,-0.9976903,40.31659,-0.0209308,0.9983594,0.05329574,-8.777811,0.9988435,0.02318926,-0.04211626,243.8403 +-0.04653298,0.05881119,-0.997184,39.44834,-0.01901639,0.9980323,0.05974862,-8.740525,0.9987357,0.02174312,-0.04532304,243.7937 +-0.0496293,0.0636234,-0.9967392,38.57904,-0.01729574,0.9977646,0.06455004,-8.699355,0.9986179,0.02044292,-0.04841794,243.7424 +-0.05168669,0.06301279,-0.9966734,37.70694,-0.0146346,0.9978524,0.06384628,-8.641674,0.9985561,0.01788592,-0.05065352,243.7224 +-0.05359497,0.06149026,-0.9966677,36.83197,-0.01382364,0.9979609,0.06231341,-8.584315,0.998467,0.01711726,-0.05263566,243.7018 +-0.05595537,0.05846574,-0.99672,35.96532,-0.01594662,0.9981043,0.05944219,-8.533757,0.9983059,0.01922043,-0.05491696,243.6638 +-0.05780494,0.05654442,-0.9967253,35.09489,-0.01698818,0.9981944,0.057613,-8.481095,0.9981833,0.02026286,-0.05673998,243.6298 +-0.05928177,0.05599217,-0.9966698,34.22783,-0.01645021,0.9982353,0.05705859,-8.428708,0.9981057,0.01977796,-0.05825607,243.5838 +-0.06055939,0.05494748,-0.9966511,33.34194,-0.02093509,0.9981941,0.05630464,-8.373309,0.997945,0.02427476,-0.05929969,243.5406 +-0.06131504,0.05187698,-0.9967694,32.47295,-0.02518677,0.99825,0.05350338,-8.324969,0.9978006,0.02838596,-0.05990112,243.489 +-0.06184715,0.04853574,-0.9969048,31.59247,-0.02759989,0.9983518,0.05031847,-8.284474,0.9977039,0.03062651,-0.06040563,243.4415 +-0.06115626,0.04728597,-0.9970075,30.71981,-0.0292683,0.9983627,0.04914556,-8.245801,0.9976989,0.03218627,-0.05967215,243.3929 +-0.05931853,0.04758689,-0.9971042,29.85195,-0.0302169,0.9983198,0.04944254,-8.2145,0.9977816,0.03306226,-0.05778093,243.3482 +-0.05738838,0.04803069,-0.9971959,28.98709,-0.02947892,0.998325,0.04978158,-8.189127,0.9979166,0.03225314,-0.05587635,243.315 +-0.05529084,0.04884164,-0.997275,28.12453,-0.02810314,0.998331,0.05045146,-8.151992,0.9980747,0.03081606,-0.05382595,243.2849 +-0.05309253,0.04992647,-0.9973408,27.2642,-0.0259884,0.998342,0.05136007,-8.118056,0.9982513,0.02864613,-0.05170699,243.2585 +-0.05123993,0.05223599,-0.9973194,26.41452,-0.02135718,0.9983455,0.05338702,-8.084825,0.9984579,0.02403548,-0.05003954,243.2298 +-0.04922633,0.05241439,-0.9974114,25.56309,-0.01550442,0.9984616,0.0532348,-8.045766,0.9986673,0.01808484,-0.04833795,243.2081 +-0.04730535,0.05068884,-0.9975936,24.72405,-0.01582281,0.9985483,0.05148767,-8.010156,0.9987551,0.01822038,-0.04643463,243.1749 +-0.04553925,0.0502875,-0.997696,23.88069,-0.01697085,0.9985491,0.05110513,-7.985757,0.9988183,0.01925904,-0.04461975,243.1391 +-0.04473834,0.04976063,-0.9977587,23.04725,-0.01250247,0.9986526,0.05036582,-7.958451,0.9989205,0.01472774,-0.04405593,243.1145 +-0.04473087,0.05407644,-0.9975344,22.21039,-0.01015789,0.9984576,0.05458199,-7.933477,0.9989474,0.01257435,-0.04411257,243.091 +-0.04526291,0.05848194,-0.9972618,21.38123,-0.0119897,0.9981812,0.05908004,-7.910159,0.9989031,0.01463101,-0.0444794,243.0551 +-0.0469014,0.0605289,-0.997064,20.55536,-0.01360045,0.9980312,0.06122739,-7.887252,0.9988069,0.01643217,-0.04598583,243.0172 +-0.04940591,0.05985638,-0.9969836,19.73633,-0.0149218,0.9980469,0.06065968,-7.865476,0.9986673,0.01787374,-0.04841625,242.9795 +-0.05227617,0.0563115,-0.9970438,18.92549,-0.01560376,0.9982409,0.05719725,-7.84405,0.9985107,0.01854769,-0.05130554,242.9405 +-0.05591956,0.05302971,-0.997026,18.11645,-0.01638986,0.9984052,0.05402233,-7.824628,0.9983007,0.01936203,-0.05496123,242.898 +-0.06045013,0.0511202,-0.9968614,17.31107,-0.0170411,0.9984893,0.05223708,-7.802514,0.9980257,0.02014535,-0.05948766,242.8504 +-0.06554325,0.05080544,-0.9965555,16.50231,-0.01848901,0.9984697,0.05211905,-7.778565,0.9976784,0.02184138,-0.0645036,242.7995 +-0.07047798,0.0509072,-0.9962135,15.70147,-0.02042675,0.9984138,0.05246475,-7.748232,0.9973041,0.02404701,-0.06932632,242.7426 +-0.07512578,0.05232973,-0.9958001,14.90179,-0.02230864,0.9982839,0.0541433,-7.72312,0.9969244,0.0262825,-0.07382945,242.6828 +-0.07951903,0.05522067,-0.9953027,14.10837,-0.02472708,0.9980479,0.05734854,-7.697842,0.9965266,0.02917123,-0.07799836,242.6121 +-0.08371244,0.05651182,-0.9948863,13.3226,-0.02894793,0.9978314,0.05911488,-7.672823,0.9960694,0.03374855,-0.08189499,242.5362 +-0.08689516,0.05609011,-0.9946372,12.542,-0.03297897,0.9977045,0.05914426,-7.650837,0.9956714,0.03794146,-0.0848459,242.4553 +-0.08907174,0.05483452,-0.9945147,11.77083,-0.03726923,0.9976007,0.05834262,-7.627257,0.9953277,0.04226147,-0.08681438,242.3767 +-0.0906727,0.05368439,-0.9944328,11.00912,-0.04247981,0.9974286,0.05771946,-7.604613,0.9949743,0.04747689,-0.08815904,242.2959 +-0.09164312,0.05389024,-0.9943327,10.25808,-0.04632776,0.9972226,0.0583167,-7.58108,0.9947136,0.05140952,-0.08889196,242.2181 +-0.09246109,0.0545742,-0.9942196,9.515117,-0.04877149,0.9970501,0.05926526,-7.565821,0.9945211,0.0539693,-0.08952667,242.143 +-0.09280224,0.05437444,-0.9941988,8.787789,-0.05120081,0.9969261,0.05930289,-7.550347,0.9943672,0.05640722,-0.08973295,242.0695 +-0.09259478,0.05442722,-0.9942152,8.077281,-0.05444942,0.996734,0.05963619,-7.534341,0.994214,0.05965644,-0.08932884,241.9973 +-0.09220125,0.05416512,-0.9942661,7.381882,-0.05695575,0.9965977,0.05957383,-7.513435,0.9941101,0.06212195,-0.08880253,241.929 +-0.09102842,0.05504401,-0.9943259,6.700094,-0.05837182,0.9964596,0.06050595,-7.496876,0.994136,0.06354836,-0.08749312,241.8648 +-0.08861615,0.05621136,-0.9944785,6.032146,-0.05918043,0.9963455,0.06159036,-7.478381,0.9943062,0.06431156,-0.08496568,241.8054 +-0.08579543,0.0568146,-0.9946916,5.376557,-0.0604408,0.9962371,0.06211611,-7.462505,0.9944777,0.06544923,-0.08203867,241.7488 +-0.08303764,0.05576182,-0.9949851,4.739786,-0.06223855,0.996194,0.06102377,-7.443521,0.9946009,0.0669937,-0.07925105,241.6958 +-0.08006015,0.05455034,-0.9952963,4.11442,-0.06244703,0.9962655,0.05962662,-7.425036,0.994832,0.066927,-0.07635465,241.6471 +-0.07729197,0.05380904,-0.9955554,3.50502,-0.06059704,0.9964429,0.05856159,-7.403702,0.9951652,0.06485405,-0.07375637,241.6056 +-0.07423508,0.05345577,-0.9958071,2.907815,-0.05842191,0.9966141,0.05785432,-7.383011,0.995528,0.06247177,-0.07086074,241.5665 +-0.07093472,0.05380028,-0.996029,2.325909,-0.0587341,0.9965865,0.0580133,-7.36385,0.9957502,0.06261601,-0.06753267,241.5237 +-0.06703682,0.05524507,-0.9962199,1.760074,-0.05951483,0.9964666,0.05926359,-7.344447,0.9959739,0.0632627,-0.06351205,241.4841 +-0.0630589,0.0553194,-0.9964755,1.213038,-0.06050102,0.9964143,0.05914463,-7.327049,0.9961742,0.06401737,-0.05948591,241.4491 +-0.05858339,0.05501398,-0.9967655,0.6848531,-0.06033977,0.9964596,0.05854349,-7.310814,0.9964572,0.06357427,-0.05505645,241.4221 +-0.0541218,0.05544775,-0.9969937,0.1703349,-0.05914283,0.9965261,0.05863232,-7.299518,0.9967813,0.0621383,-0.05065445,241.3995 +-0.04981643,0.0563095,-0.9971698,-0.3247848,-0.05872758,0.9965167,0.05920653,-7.285113,0.9970302,0.06151082,-0.04633599,241.3788 +-0.04550638,0.05687983,-0.9973434,-0.8046972,-0.05675787,0.9966177,0.05942817,-7.27066,0.9973503,0.05931145,-0.04212408,241.3618 +-0.04120044,0.05798503,-0.997467,-1.267287,-0.05680532,0.9965639,0.06027889,-7.253762,0.9975348,0.05914494,-0.03776501,241.3461 +-0.03610771,0.05992338,-0.9975497,-1.717128,-0.05736686,0.9964303,0.06193262,-7.238023,0.9977,0.05946254,-0.0325412,241.3305 +-0.03140207,0.06112176,-0.9976363,-2.147061,-0.05786746,0.996343,0.062864,-7.227306,0.9978302,0.05970473,-0.02775027,241.3224 +-0.02624658,0.06176553,-0.9977456,-2.559693,-0.05693015,0.9963771,0.06317843,-7.217072,0.9980331,0.05846002,-0.02263517,241.3183 +-0.02046943,0.06252511,-0.9978335,-2.957329,-0.05622369,0.9963912,0.06358811,-7.203941,0.9982083,0.05740349,-0.01688017,241.3176 +-0.01446951,0.06200308,-0.9979711,-3.335045,-0.05472316,0.9965306,0.06270702,-7.191538,0.9983967,0.05551947,-0.0110263,241.3211 +-0.008663444,0.06079635,-0.9981126,-3.693693,-0.05283777,0.9967278,0.06117064,-7.183816,0.9985655,0.05326799,-0.005422752,241.329 +-0.002829753,0.06025885,-0.9981788,-4.038108,-0.05192025,0.9968276,0.06032448,-7.172216,0.9986472,0.05199639,0.0003078767,241.3375 +0.002518108,0.05938084,-0.9982323,-4.362696,-0.05116412,0.9969356,0.05917465,-7.164628,0.998687,0.05092466,0.005548558,241.3477 +0.007348508,0.05940952,-0.9982067,-4.663567,-0.05110519,0.9969514,0.0589586,-7.156169,0.9986662,0.05058028,0.01036224,241.3569 +0.0118547,0.0600161,-0.998127,-4.943153,-0.05106244,0.9969311,0.05933774,-7.153234,0.9986251,0.05026337,0.01488288,241.3655 +0.01591373,0.05996155,-0.9980739,-5.195608,-0.05100457,0.9969493,0.05908076,-7.148566,0.9985716,0.04996612,0.01892349,241.3751 +0.01967919,0.05830293,-0.998105,-5.423479,-0.04946181,0.9971327,0.05727093,-7.145945,0.9985821,0.04824103,0.02250653,241.3865 +0.02295032,0.05335208,-0.998312,-5.61774,-0.04762497,0.9974996,0.05221382,-7.146376,0.9986016,0.04634625,0.02543382,241.3993 +0.02570316,0.04958938,-0.9984389,-5.787975,-0.04611513,0.9977644,0.04836873,-7.145318,0.9986054,0.04479991,0.02793251,241.411 +0.02777035,0.04973839,-0.9983762,-5.935611,-0.04596377,0.9977684,0.04842962,-7.14152,0.998557,0.04454422,0.02999454,241.4211 +0.02924553,0.05138035,-0.9982509,-6.055183,-0.04628213,0.9976765,0.04999488,-7.140534,0.9985002,0.04473904,0.03155556,241.4288 +0.0301448,0.05060538,-0.9982637,-6.148943,-0.04636141,0.9977135,0.04917751,-7.141362,0.9984697,0.04479846,0.032422,241.4357 +0.03082369,0.04809906,-0.9983669,-6.224444,-0.0459713,0.9978526,0.04665497,-7.140925,0.9984671,0.04445814,0.03296867,241.4423 +0.03139705,0.04642569,-0.9984282,-6.292219,-0.04561437,0.9979464,0.04496889,-7.144182,0.9984656,0.04413078,0.03345025,241.4481 +0.03143924,0.04563628,-0.9984633,-6.347609,-0.04544415,0.9979893,0.04418369,-7.147305,0.998472,0.04398521,0.03344992,241.4535 +0.03145683,0.04593749,-0.9984489,-6.391909,-0.04562791,0.9979678,0.04447782,-7.151484,0.9984631,0.04415801,0.03348893,241.4576 +0.03162515,0.04589291,-0.9984457,-6.421901,-0.04583707,0.9979609,0.04441877,-7.154462,0.9984482,0.04436107,0.03366426,241.4586 +0.03170023,0.04558599,-0.9984573,-6.441034,-0.04586583,0.9979733,0.0441077,-7.157278,0.9984445,0.04439685,0.03372682,241.4574 +0.03177846,0.04565542,-0.9984517,-6.44834,-0.04594093,0.9979671,0.04417107,-7.158324,0.9984385,0.0444661,0.03381131,241.4564 +0.03185787,0.04526446,-0.998467,-6.445881,-0.04603004,0.9979805,0.04377375,-7.158426,0.9984319,0.04456493,0.03387705,241.4562 +0.03185098,0.04399996,-0.9985237,-6.44144,-0.04631928,0.9980221,0.04250037,-7.1594,0.9984187,0.04489721,0.03382603,241.4561 +0.03195735,0.04223728,-0.9985964,-6.441119,-0.04659687,0.9980833,0.04072438,-7.161211,0.9984024,0.04523002,0.03386422,241.4563 +0.0318599,0.04174043,-0.9986204,-6.441162,-0.04668088,0.9980994,0.04022936,-7.161957,0.9984016,0.04533477,0.03374783,241.4591 +0.03178882,0.04223003,-0.9986021,-6.439927,-0.04638389,0.9980929,0.04073195,-7.160925,0.9984177,0.04502423,0.03368698,241.4626 +0.03170929,0.04185072,-0.9986206,-6.435729,-0.04602516,0.9981243,0.04036849,-7.162727,0.9984368,0.04468161,0.033576,241.4683 +0.03147722,0.04154018,-0.9986409,-6.435232,-0.04613712,0.9981313,0.04006475,-7.164249,0.998439,0.04481329,0.03333494,241.4733 +0.03114961,0.04115968,-0.9986669,-6.44044,-0.04610469,0.9981474,0.03970021,-7.166143,0.9984508,0.04480658,0.03298955,241.4828 +0.03108701,0.04056081,-0.9986934,-6.449325,-0.04603399,0.9981741,0.0391068,-7.162294,0.998456,0.04475812,0.03289742,241.488 +0.03105899,0.04042378,-0.9986998,-6.462353,-0.04593484,0.9981838,0.03897436,-7.157896,0.9984614,0.04466461,0.03285944,241.4935 +0.03098859,0.04053219,-0.9986976,-6.480607,-0.04600033,0.9981765,0.0390837,-7.15323,0.9984606,0.04472926,0.03279657,241.4981 +0.03108834,0.04037203,-0.998701,-6.502415,-0.04593963,0.9981857,0.03892116,-7.152502,0.9984603,0.04466995,0.03288661,241.5018 +0.03114357,0.04018303,-0.9987069,-6.528339,-0.04592933,0.9981936,0.03873013,-7.151255,0.9984591,0.04466374,0.03293289,241.506 +0.03118041,0.03939027,-0.9987373,-6.560264,-0.04592693,0.9982242,0.03793621,-7.15051,0.998458,0.04468607,0.03293411,241.5077 +0.03151021,0.03709605,-0.9988148,-6.603855,-0.04590655,0.9983101,0.03562907,-7.149296,0.9984486,0.04472946,0.03315991,241.509 +0.03214448,0.03434114,-0.9988931,-6.662736,-0.04586604,0.9984074,0.03284847,-7.149164,0.9984302,0.04475937,0.03366837,241.5123 +0.03337074,0.0322451,-0.9989228,-6.742488,-0.04570207,0.9984831,0.03070416,-7.14747,0.9983975,0.04462822,0.03479378,241.5161 +0.03551186,0.03107362,-0.9988861,-6.841205,-0.04549815,0.9985304,0.02944504,-7.146762,0.998333,0.04440181,0.03687346,241.5229 +0.03904896,0.03013454,-0.9987828,-6.960563,-0.04542079,0.9985655,0.0283522,-7.146072,0.9982044,0.04425838,0.04036168,241.5303 +0.04439619,0.02837773,-0.9986109,-7.099251,-0.04503181,0.9986373,0.02637646,-7.145097,0.9979985,0.04379824,0.04561359,241.5417 +0.05169972,0.0269851,-0.9982981,-7.256774,-0.04495861,0.9986842,0.02466724,-7.142968,0.9976501,0.0436068,0.05284491,241.556 +0.06120976,0.02643534,-0.9977748,-7.433982,-0.04555554,0.9986814,0.02366471,-7.138124,0.9970847,0.04400565,0.06233333,241.575 +0.07270826,0.02614764,-0.9970105,-7.627305,-0.04679329,0.9986448,0.02277805,-7.130703,0.9962549,0.04499724,0.07383326,241.5979 +0.08656549,0.02535974,-0.9959234,-7.83498,-0.04844174,0.9986006,0.02121737,-7.122577,0.9950677,0.04640756,0.08767281,241.6234 +0.102698,0.02596047,-0.9943738,-8.055734,-0.04900017,0.9985778,0.02100954,-7.117187,0.9935049,0.04656684,0.103824,241.6612 +0.121641,0.03071668,-0.9920988,-8.293171,-0.04812427,0.998528,0.02501525,-7.114535,0.9914068,0.04470115,0.1229401,241.705 +0.1441195,0.03574823,-0.9889144,-8.540863,-0.04914636,0.9983726,0.0289278,-7.111017,0.9883391,0.04443248,0.1456418,241.7596 +0.167681,0.03643599,-0.9851678,-8.793327,-0.05046545,0.9983238,0.02833307,-7.102833,0.9845487,0.04496602,0.1692387,241.8206 +0.193752,0.03543131,-0.9804106,-9.050632,-0.04849689,0.9984717,0.02649992,-7.098001,0.9798511,0.04241244,0.1951742,241.8908 +0.2236658,0.03505221,-0.9740354,-9.316724,-0.04638919,0.9986034,0.02528408,-7.092475,0.9735613,0.03952952,0.2249795,241.985 +0.256623,0.0349091,-0.965881,-9.588231,-0.04294145,0.9987725,0.02468885,-7.088248,0.9655571,0.0351406,0.257807,242.0972 +0.2919401,0.03517341,-0.9557897,-9.863606,-0.03836837,0.9989498,0.02504234,-7.085354,0.9556667,0.02936123,0.292983,242.2077 +0.3313972,0.03834766,-0.9427117,-10.14807,-0.03663018,0.9989433,0.02775822,-7.081455,0.94278,0.0253327,0.3324517,242.3512 +0.3732099,0.04373391,-0.9267156,-10.43713,-0.03828661,0.9987634,0.03171512,-7.076031,0.9269565,0.0236444,0.3744228,242.4854 +0.4172784,0.0480695,-0.9075066,-10.72766,-0.04334647,0.9985163,0.03295915,-7.06956,0.9077443,0.02558407,0.4187429,242.6616 +0.4633272,0.05101507,-0.8847177,-11.01459,-0.04799635,0.9983209,0.03243002,-7.064329,0.8848866,0.02743751,0.4649977,242.8586 +0.5098119,0.05221632,-0.8586998,-11.29588,-0.04801255,0.9983275,0.03220174,-7.059169,0.8589451,0.02481153,0.5114662,243.0471 +0.5569376,0.05164291,-0.8289473,-11.56617,-0.04545139,0.9984645,0.03166671,-7.058609,0.8293098,0.02004042,0.5584296,243.2941 +0.6044473,0.04887129,-0.7951447,-11.82781,-0.04278162,0.9986675,0.02885883,-7.059708,0.7954955,0.01657394,0.6057327,243.565 +0.6510547,0.04512475,-0.7576884,-12.07917,-0.0423491,0.9988358,0.02309742,-7.054224,0.7578485,0.01704974,0.6522077,243.81 +0.6971811,0.03975702,-0.7157918,-12.31767,-0.04205941,0.9990095,0.01452185,-7.048075,0.7156602,0.01998142,0.6981627,244.1218 +0.7418464,0.03796311,-0.6694944,-12.54841,-0.04460439,0.9989786,0.007221468,-7.040201,0.6690847,0.02450517,0.7427819,244.4066 +0.7845385,0.03766814,-0.618935,-12.76645,-0.04577573,0.9989479,0.002772012,-7.039227,0.6183882,0.02615745,0.7854373,244.7706 +0.8233369,0.03776799,-0.566295,-12.96987,-0.04441218,0.9990112,0.00205629,-7.04187,0.5658127,0.02345738,0.8242,245.1641 +0.857375,0.03416242,-0.5135573,-13.15225,-0.03927378,0.9992281,0.0009029155,-7.045521,0.5131917,0.0193952,0.8580548,245.5349 +0.8871323,0.03008649,-0.4605336,-13.31828,-0.03384144,0.9994272,0.000102976,-7.056461,0.4602728,0.01549377,0.8876422,245.9798 +0.9126407,0.02660723,-0.407896,-13.46958,-0.02995712,0.9995495,-0.001826035,-7.066833,0.4076636,0.0138859,0.9130265,246.4412 +0.9337846,0.02360728,-0.3570561,-13.60271,-0.0272091,0.9996169,-0.005066996,-7.074069,0.3567997,0.01444666,0.9340691,246.8869 +0.9507319,0.02143287,-0.3092726,-13.72235,-0.02617588,0.9995947,-0.0111942,-7.081737,0.3089073,0.01873816,0.9509075,247.3819 +0.9645302,0.0208814,-0.2631455,-13.82811,-0.02573414,0.9995561,-0.01500775,-7.091706,0.2627153,0.02124725,0.9646394,247.8773 +0.9750521,0.02050081,-0.2210276,-13.91896,-0.02448105,0.9995834,-0.01528326,-7.107983,0.2206222,0.02031296,0.9751477,248.4226 +0.9827972,0.02010763,-0.1835902,-13.99361,-0.02322719,0.9996198,-0.01485716,-7.126064,0.1832216,0.01886586,0.9828905,248.9891 +0.9883304,0.01935405,-0.1510909,-14.05752,-0.02179091,0.9996575,-0.01448928,-7.143097,0.1507587,0.0176126,0.9884136,249.558 +0.9921809,0.01665338,-0.1236926,-14.10353,-0.01858093,0.999723,-0.01444609,-7.161301,0.1234178,0.01663145,0.9922154,250.1686 +0.9946502,0.01350806,-0.1024142,-14.14237,-0.01510162,0.9997764,-0.01480062,-7.177194,0.1021914,0.01626806,0.9946317,250.776 +0.9962842,0.01150535,-0.08535498,-14.17621,-0.01278396,0.9998139,-0.01444852,-7.19411,0.08517285,0.01548601,0.9962458,251.4059 +0.9973124,0.01063516,-0.07249081,-14.21087,-0.01169431,0.9998307,-0.01420202,-7.21106,0.0723275,0.01501158,0.9972679,252.0577 +0.9979777,0.01000854,-0.0627731,-14.24036,-0.0109663,0.9998284,-0.01493162,-7.227459,0.06261288,0.01558981,0.9979161,252.7243 +0.9984713,0.009784261,-0.05440052,-14.26469,-0.01067759,0.9998125,-0.01615498,-7.244408,0.05423225,0.01671115,0.9983884,253.405 +0.9988298,0.008648028,-0.04758561,-14.28735,-0.009487898,0.9998027,-0.01745218,-7.262488,0.04742529,0.01788324,0.9987146,254.1031 +0.9990223,0.008114487,-0.04345983,-14.30712,-0.008919116,0.9997918,-0.01835252,-7.282421,0.04330186,0.01872219,0.9988865,254.8169 +0.9991434,0.007492461,-0.04070019,-14.32889,-0.008231782,0.9998036,-0.01802794,-7.302451,0.04055712,0.01834752,0.9990087,255.5442 +0.999227,0.007280994,-0.03863134,-14.34971,-0.007940312,0.9998249,-0.01694101,-7.323118,0.03850122,0.01723466,0.9991099,256.2902 +0.9992676,0.007770527,-0.03746952,-14.37576,-0.008333282,0.9998544,-0.01488628,-7.341575,0.03734839,0.01518762,0.9991868,257.0548 +0.9992972,0.007176542,-0.03679222,-14.39965,-0.007624023,0.9998985,-0.01203652,-7.358411,0.0367021,0.01230856,0.9992504,257.8376 +0.9992876,0.00667624,-0.03714609,-14.4226,-0.007101514,0.9999106,-0.01132853,-7.37266,0.03706713,0.01158425,0.9992456,258.6323 +0.9992276,0.005891745,-0.0388542,-14.44732,-0.006384081,0.9999007,-0.01255948,-7.385382,0.03877635,0.01279783,0.9991659,259.4409 +0.9991628,0.004458685,-0.04066817,-14.47138,-0.005111776,0.9998594,-0.0159692,-7.396035,0.04059125,0.01616371,0.999045,260.2632 +0.9990942,0.006010951,-0.04212781,-14.49886,-0.006936102,0.9997372,-0.02184894,-7.409775,0.0419854,0.02212134,0.9988732,261.0941 +0.9990658,0.008135047,-0.04244319,-14.52874,-0.009250828,0.999615,-0.02615897,-7.427194,0.04221405,0.02652717,0.9987563,261.9434 +0.999043,0.01004219,-0.04257084,-14.56159,-0.01116962,0.9995909,-0.02632906,-7.448031,0.04228902,0.02677936,0.9987464,262.8078 +0.9990513,0.01056088,-0.04224966,-14.59164,-0.01140066,0.9997412,-0.01968541,-7.46886,0.04203083,0.02014841,0.9989131,263.6919 +0.9990818,0.01032153,-0.04158202,-14.62306,-0.01071466,0.9998999,-0.009242434,-7.489532,0.04148246,0.009679484,0.9990923,264.5856 +0.9991022,0.008576964,-0.04148808,-14.65282,-0.008756423,0.999953,-0.004145755,-7.507641,0.04145058,0.00450532,0.9991303,265.4917 +0.9991063,0.004707581,-0.04200629,-14.68155,-0.004957871,0.9999705,-0.005856188,-7.522891,0.04197748,0.006059215,0.9991001,266.4038 +0.9990583,0.001247512,-0.04337041,-14.70869,-0.001804954,0.9999162,-0.01281627,-7.535572,0.04335078,0.01288249,0.9989768,267.323 +0.9990023,-0.001132359,-0.04464555,-14.73976,0.000214814,0.9997888,-0.02055121,-7.552162,0.04465939,0.02052111,0.9987914,268.2544 +0.998942,-0.001462056,-0.04596511,-14.77556,0.0003091421,0.9996854,-0.02507948,-7.575616,0.04598732,0.02503874,0.9986281,269.1985 +0.9989125,-0.001191959,-0.0466099,-14.81424,-2.887654E-05,0.9996571,-0.02618319,-7.605036,0.04662513,0.02615605,0.9985699,270.1618 +0.9988735,0.00136425,-0.04743263,-14.85834,-0.00252323,0.9996995,-0.02438295,-7.636316,0.04738511,0.02447516,0.9985767,271.1348 +0.9988723,0.002905,-0.04738863,-14.90154,-0.003788932,0.9998203,-0.01857367,-7.666176,0.04732616,0.01873227,0.9987038,272.1219 +0.9988801,0.004315237,-0.04711669,-14.944,-0.004830234,0.9999298,-0.01082186,-7.692098,0.04706668,0.01103732,0.9988307,273.1257 +0.9989308,0.004024903,-0.04605644,-14.97353,-0.004345222,0.999967,-0.006856915,-7.71705,0.04602732,0.007049708,0.9989152,274.1347 +0.9989498,0.002119031,-0.04577001,-15.00587,-0.002515547,0.9999598,-0.008607377,-7.737837,0.04574993,0.008713473,0.9989149,275.1423 +0.998964,0.0003271503,-0.0455081,-15.03534,-0.0009187433,0.9999153,-0.01297942,-7.76151,0.0455,0.01300778,0.9988796,276.1557 +0.9989413,-0.0006527915,-0.04599968,-15.06955,-0.0001360693,0.999853,-0.01714405,-7.787226,0.04600411,0.01713216,0.9987943,277.1757 +0.9989253,-0.002385309,-0.04628885,-15.10295,0.001525131,0.9998256,-0.01860927,-7.816972,0.04632517,0.01851867,0.9987547,278.2078 +0.9988876,-0.002298772,-0.04709969,-15.14094,0.0014327,0.9998294,-0.01841358,-7.847445,0.04713399,0.01832561,0.9987204,279.2453 +0.9988294,-0.003910977,-0.04821364,-15.17521,0.003173488,0.9998769,-0.01536335,-7.874001,0.04826778,0.01519236,0.9987188,280.2936 +0.9987399,-0.003697907,-0.05005105,-15.21559,0.003108383,0.9999249,-0.01185118,-7.902476,0.05009111,0.01168066,0.9986763,281.3463 +0.9986454,-0.004742608,-0.05181604,-15.25743,0.004087801,0.9999105,-0.01273583,-7.930494,0.0518718,0.01250676,0.9985754,282.3977 +0.9985229,-0.007975316,-0.05374395,-15.29723,0.006955752,0.9997928,-0.01913121,-7.955939,0.05388539,0.01872912,0.9983714,283.4464 +0.9984444,-0.007380052,-0.05526746,-15.34119,0.006075483,0.9996998,-0.02373559,-7.990759,0.05542604,0.02336289,0.9981894,284.4978 +0.9983812,-0.006912109,-0.05645685,-15.38588,0.005516813,0.9996764,-0.02483296,-8.031229,0.05661022,0.02448129,0.9980961,285.5557 +0.9983049,-0.005105044,-0.05797659,-15.43659,0.003769235,0.9997254,-0.02312653,-8.06989,0.05807873,0.0228688,0.99805,286.6196 +0.9982427,-0.003686731,-0.05914424,-15.48438,0.002558879,0.9998136,-0.01913395,-8.105104,0.05920376,0.01894898,0.998066,287.6852 +0.9981689,-0.00161007,-0.06046705,-15.53562,0.0007429468,0.9998966,-0.01436018,-8.139637,0.06048392,0.01428896,0.9980668,288.7506 +0.9981017,4.768442E-05,-0.06158783,-15.58579,-0.0007090934,0.9999423,-0.01071747,-8.171223,0.06158377,0.01074079,0.9980441,289.8109 +0.9980578,0.002519387,-0.06224415,-15.63834,-0.00313149,0.9999477,-0.009738314,-8.199249,0.06221636,0.009914316,0.9980134,290.8664 +0.9980532,0.002632571,-0.06231318,-15.68736,-0.003297814,0.9999386,-0.01057536,-8.224551,0.06228152,0.01076027,0.9980006,291.9147 +0.998076,0.001323292,-0.06198871,-15.73624,-0.002109184,0.9999182,-0.01261426,-8.251678,0.06196694,0.01272074,0.9979971,292.9596 +0.998077,0.001955753,-0.06195659,-15.78736,-0.002953217,0.9998674,-0.01601192,-8.281877,0.06191706,0.0161641,0.9979503,293.998 +0.9980657,0.0003718679,-0.06216768,-15.83486,-0.001613933,0.9998,-0.01993026,-8.313716,0.06214783,0.01999204,0.9978667,295.0365 +0.9980551,0.0006113599,-0.06233626,-15.88531,-0.001963118,0.9997642,-0.021626,-8.348776,0.06230834,0.02170631,0.9978208,296.0719 +0.9980904,0.001530942,-0.06175197,-15.93564,-0.002765562,0.9997979,-0.0199127,-8.385496,0.061709,0.02004545,0.9978928,297.107 +0.9981177,0.003726375,-0.06121435,-15.99106,-0.004769569,0.9998457,-0.01690439,-8.423964,0.06114191,0.01716453,0.9979814,298.1391 +0.9981435,0.005930896,-0.060617,-16.04697,-0.006860732,0.9998618,-0.01514292,-8.461541,0.0605188,0.01553068,0.9980462,299.1656 +0.9981552,0.007384158,-0.06026477,-16.10293,-0.008263692,0.9998627,-0.01435831,-8.494666,0.06015047,0.01482983,0.9980791,300.1912 +0.9981795,0.009191397,-0.05961014,-16.16193,-0.009862368,0.9998912,-0.01097155,-8.522851,0.0595028,0.01153948,0.9981614,301.2168 +0.9982047,0.011629,-0.05875564,-16.21966,-0.01199174,0.9999111,-0.005824913,-8.545531,0.05868267,0.006519037,0.9982553,302.2463 +0.9982063,0.01422968,-0.05815372,-16.27711,-0.01454371,0.9998818,-0.004980213,-8.564683,0.05807598,0.00581705,0.9982952,303.2743 +0.998185,0.01709697,-0.05774464,-16.33433,-0.01771294,0.9997913,-0.01017203,-8.585233,0.05755868,0.01117639,0.9982795,304.2939 +0.9981657,0.01886348,-0.05752832,-16.38971,-0.01977332,0.9996876,-0.01528736,-8.606509,0.05722198,0.01639684,0.9982268,305.3141 +0.9981468,0.02150165,-0.05692666,-16.44631,-0.02253866,0.9995904,-0.01763756,-8.631852,0.0565241,0.01888792,0.9982225,306.3356 +0.9981021,0.02478627,-0.05637325,-16.50343,-0.02584925,0.9995,-0.01820552,-8.662702,0.05589382,0.01962817,0.9982437,307.3565 +0.9980666,0.02695554,-0.05600554,-16.55969,-0.02811222,0.9994053,-0.01996868,-8.692187,0.05543396,0.0215045,0.9982307,308.3783 +0.9980493,0.02768966,-0.05595444,-16.61304,-0.02878223,0.9994086,-0.01881517,-8.722646,0.05540036,0.02038896,0.998256,309.3998 +0.9980319,0.0281705,-0.05602515,-16.66632,-0.02913385,0.9994401,-0.01645301,-8.754727,0.05553029,0.01805286,0.9982937,310.4297 +0.9980327,0.02744582,-0.05637045,-16.70935,-0.02830643,0.9994937,-0.01452562,-8.785016,0.05594324,0.01609268,0.9983042,311.4589 +0.99802,0.02710993,-0.05675649,-16.75029,-0.02793765,0.9995138,-0.0138413,-8.813019,0.05635366,0.01539954,0.998292,312.4903 +0.9980008,0.02806362,-0.05663033,-16.79627,-0.02890272,0.9994834,-0.01405281,-8.839964,0.0562067,0.01566148,0.9982963,313.5254 +0.9980613,0.02765939,-0.05575568,-16.8393,-0.02849716,0.9994917,-0.01428705,-8.86701,0.05533217,0.01584823,0.9983422,314.5633 +0.9981009,0.02783125,-0.05495629,-16.88389,-0.02864324,0.999491,-0.01404317,-8.894774,0.05453748,0.01559063,0.9983899,315.6082 +0.9981614,0.026954,-0.05428964,-16.92691,-0.02777838,0.9995091,-0.01448769,-8.922586,0.05387248,0.01596913,0.9984201,316.6562 +0.9981692,0.02755228,-0.05384389,-16.97227,-0.02838637,0.9994876,-0.01478765,-8.954986,0.05340886,0.01628901,0.9984398,317.7092 +0.99819,0.02806323,-0.05319097,-17.02224,-0.0288113,0.9994957,-0.01334943,-8.983779,0.05278951,0.01485776,0.9984951,318.7691 +0.9982522,0.02697717,-0.05258238,-17.06905,-0.02776246,0.9995128,-0.01426161,-9.017426,0.05217202,0.0156965,0.9985147,319.8317 +0.9982316,0.02808579,-0.05239269,-17.1184,-0.0291176,0.9993947,-0.0190352,-9.048416,0.05182636,0.02052708,0.9984451,320.8931 +0.998231,0.02804119,-0.05242845,-17.17214,-0.02926733,0.9993123,-0.02276701,-9.08471,0.05175398,0.02426117,0.9983651,321.966 +0.998275,0.02633589,-0.05247431,-17.22368,-0.02739219,0.9994343,-0.01951331,-9.122043,0.05193072,0.02091703,0.9984316,323.0455 +0.9982835,0.02685334,-0.05204802,-17.2817,-0.02763333,0.9995155,-0.01432442,-9.164148,0.05163814,0.01573809,0.9985418,324.1272 +0.9982801,0.02795495,-0.05153154,-17.33875,-0.02858648,0.9995245,-0.0115591,-9.20251,0.05118389,0.01301232,0.9986044,325.2114 +0.9983401,0.02681858,-0.0509693,-17.39586,-0.027341,0.9995802,-0.00957994,-9.238189,0.05069098,0.01095759,0.9986542,326.294 +0.9983392,0.02756337,-0.05058741,-17.45438,-0.02798429,0.9995792,-0.007631242,-9.272285,0.05035578,0.00903422,0.9986904,327.3754 +0.9984003,0.02571505,-0.0503563,-17.50894,-0.02610599,0.9996338,-0.007121138,-9.305503,0.05015474,0.008424346,0.9987059,328.4521 +0.9984555,0.02281474,-0.05065657,-17.56163,-0.02329993,0.9996879,-0.009008098,-9.339167,0.05043524,0.01017448,0.9986755,329.5244 +0.9984733,0.02147258,-0.05089227,-17.61419,-0.02210281,0.9996854,-0.01185325,-9.373015,0.05062174,0.01296001,0.9986338,330.5939 +0.9984995,0.01930106,-0.05124836,-17.66713,-0.02003001,0.9997048,-0.01374839,-9.40848,0.05096787,0.01475426,0.9985912,331.6621 +0.9985037,0.01822851,-0.05155769,-17.71901,-0.01887459,0.999749,-0.01207225,-9.442686,0.05132469,0.01302731,0.998597,332.7313 +0.9985493,0.01574748,-0.0514914,-17.77586,-0.01624307,0.9998255,-0.009220505,-9.476537,0.05133721,0.01004351,0.9986308,333.8025 +0.9985773,0.01325286,-0.05165107,-17.83594,-0.0135646,0.9998918,-0.005689473,-9.508603,0.05157007,0.006382004,0.9986489,334.8728 +0.9985967,0.01064236,-0.0518794,-17.90043,-0.01084865,0.9999343,-0.003696252,-9.541916,0.05183665,0.004253886,0.9986465,335.9393 +0.9986036,0.007811883,-0.05224871,-17.95961,-0.008174264,0.9999439,-0.006725582,-9.572257,0.05219324,0.007143284,0.9986114,336.9952 +0.9986097,0.004252037,-0.05254244,-18.01787,-0.004852603,0.9999243,-0.01130783,-9.602235,0.05249038,0.01154708,0.9985546,338.0469 +0.9985969,0.002612049,-0.0528908,-18.07196,-0.003293556,0.9999126,-0.0128021,-9.634547,0.05285274,0.01295833,0.9985182,339.098 +0.9985694,0.001754677,-0.05344295,-18.12844,-0.002379828,0.9999294,-0.01163614,-9.671642,0.05341876,0.01174668,0.998503,340.1343 +0.9985231,-5.164091E-05,-0.05432863,-18.18459,-0.0004450667,0.9999582,-0.009130511,-9.708592,0.05432683,0.009141205,0.9984813,341.1635 +0.99846,-0.001904697,-0.05544415,-18.23973,0.001540662,0.999977,-0.006607799,-9.742058,0.05545545,0.006512202,0.9984399,342.1813 +0.998408,-0.003499733,-0.05629613,-18.29564,0.003306891,0.9999883,-0.003518292,-9.773282,0.05630778,0.003326525,0.9984079,343.1919 +0.9983343,-0.005552358,-0.05742732,-18.34619,0.005432087,0.9999827,-0.00225022,-9.800677,0.05743882,0.001934522,0.9983471,344.1923 +0.998258,-0.007305779,-0.05854621,-18.39821,0.00712175,0.999969,-0.003351354,-9.82937,0.05856887,0.002928565,0.998279,345.1844 +0.9981817,-0.009071191,-0.05959162,-18.45308,0.008779304,0.9999481,-0.005158145,-9.855773,0.05963532,0.004625593,0.9982095,346.169 +0.998117,-0.009075693,-0.060665,-18.51181,0.008679338,0.9999392,-0.006793847,-9.882062,0.06072297,0.006254521,0.998135,347.1442 +0.9980733,-0.007636205,-0.06157438,-18.57248,0.007199344,0.9999473,-0.007313618,-9.908893,0.06162698,0.006856232,0.9980756,348.1059 +0.9980269,-0.007307747,-0.06236224,-18.63261,0.006856186,0.9999487,-0.007451883,-9.935871,0.0624135,0.007009612,0.9980257,349.0562 +0.9979955,-0.006581295,-0.06294219,-18.69263,0.006201206,0.9999613,-0.006232169,-9.964349,0.06298076,0.005829359,0.9979977,349.9968 +0.9979777,-0.004895669,-0.06337752,-18.75506,0.004584373,0.9999767,-0.005056268,-9.993081,0.06340079,0.004755496,0.9979768,350.925 +0.9979606,-0.002742424,-0.0637742,-18.81771,0.002345222,0.9999774,-0.006302295,-10.02007,0.06379004,0.006139877,0.9979444,351.8411 +0.9979539,-0.002004775,-0.06390638,-18.87668,0.001486504,0.9999656,-0.008156372,-10.04361,0.06392053,0.008044686,0.9979225,352.7376 +0.9979927,-0.00265064,-0.06327444,-18.93231,0.002054009,0.9999528,-0.009492455,-10.06499,0.06329661,0.009343433,0.997951,353.6193 +0.9980688,-0.00220087,-0.06208002,-18.98645,0.001649294,0.9999587,-0.008934767,-10.09002,0.06209712,0.008815123,0.9980311,354.4884 +0.998174,-0.001421829,-0.06038857,-19.03898,0.0009237563,0.9999653,-0.008274912,-10.11649,0.06039824,0.008204016,0.9981406,355.3388 +0.9982707,-0.002958885,-0.0587109,-19.08444,0.002558607,0.999973,-0.006891801,-10.14361,0.0587297,0.006729665,0.9982512,356.172 +0.9983501,-0.005015791,-0.05720178,-19.12487,0.004652761,0.9999682,-0.006477911,-10.17245,0.05723245,0.006201076,0.9983416,356.9824 +0.9984377,-0.005386841,-0.05561598,-19.1617,0.005084692,0.9999715,-0.005572844,-10.2038,0.05564442,0.005281347,0.9984366,357.7702 +0.9985141,-0.007980842,-0.05390679,-19.19084,0.007796447,0.999963,-0.003630072,-10.23706,0.05393376,0.003204396,0.9985393,358.5447 +0.9985803,-0.009202805,-0.05246779,-19.22093,0.009115056,0.9999566,-0.00191151,-10.26248,0.05248311,0.001430549,0.9986207,359.2895 +0.9986795,-0.007186209,-0.05087034,-19.25385,0.00717926,0.9999742,-0.000319345,-10.28902,0.05087132,-4.62874E-05,0.9987052,360.0121 +0.9987562,-0.007251884,-0.04933017,-19.28152,0.007257127,0.9999736,-7.282749E-05,-10.31346,0.04932939,-0.0002852576,0.9987825,360.7091 +0.9988048,-0.007620574,-0.04828057,-19.31045,0.007470628,0.9999667,-0.003285431,-10.34004,0.04830399,0.002920818,0.9988284,361.3777 +0.998864,-0.007962988,-0.04698197,-19.33791,0.007556177,0.9999324,-0.008830138,-10.36798,0.04704911,0.008465103,0.9988567,362.0224 +0.9989318,-0.007630449,-0.04557471,-19.36488,0.007101791,0.9999057,-0.01175049,-10.39159,0.04566007,0.01141427,0.9988918,362.6482 +0.9989751,-0.0071816,-0.04469035,-19.39005,0.006700128,0.999918,-0.010914,-10.41291,0.04476506,0.01060338,0.9989412,363.254 +0.9989926,-0.007481992,-0.04424791,-19.41263,0.007100457,0.9999363,-0.008773571,-10.43387,0.04431073,0.008450552,0.998982,363.8417 +0.9989841,-0.007408061,-0.04445172,-19.43538,0.007196113,0.999962,-0.004926187,-10.46051,0.04448652,0.004601302,0.9989993,364.4048 +0.9989025,-0.007148054,-0.0462901,-19.45834,0.007005405,0.9999702,-0.003243141,-10.48547,0.0463119,0.002915301,0.9989227,364.9459 +0.9987303,-0.005839828,-0.05003797,-19.48219,0.005561817,0.9999683,-0.005693446,-10.50845,0.05006963,0.005407914,0.998731,365.4565 +0.9984418,-0.004930242,-0.05558556,-19.51293,0.004420359,0.999947,-0.009292167,-10.52766,0.05562842,0.009031979,0.9984106,365.9466 +0.9979648,-0.004067365,-0.06363771,-19.55244,0.003304268,0.9999214,-0.01209194,-10.54957,0.06368189,0.01185705,0.9978998,366.4174 +0.9971397,-0.004126093,-0.07546833,-19.59573,0.003147394,0.9999094,-0.01308269,-10.57038,0.07551548,0.01280774,0.9970623,366.8759 +0.9958645,-0.00374372,-0.09077401,-19.64989,0.002475095,0.9998977,-0.01408419,-10.59195,0.09081745,0.01380127,0.9957719,367.3221 +0.9940304,-0.001950481,-0.109086,-19.71691,0.0003369825,0.9998903,-0.01480755,-10.61419,0.1091029,0.01468239,0.993922,367.7556 +0.991455,0.001298631,-0.1304433,-19.79756,-0.003145821,0.9998976,-0.0139558,-10.63599,0.1304118,0.0142469,0.9913575,368.193 +0.9878196,0.004390917,-0.1555418,-19.89223,-0.006258518,0.999914,-0.01151939,-10.65635,0.1554778,0.01235253,0.9877621,368.6282 +0.982785,0.004520839,-0.1846978,-19.99201,-0.005948586,0.9999565,-0.007176795,-10.67197,0.1846573,0.008151937,0.9827691,369.0442 +0.9758239,0.003941942,-0.2185231,-20.11145,-0.00517786,0.9999737,-0.005083399,-10.69258,0.2184973,0.006091984,0.9758185,369.4708 +0.9663979,0.004343598,-0.2570142,-20.25209,-0.005620668,0.9999752,-0.00423444,-10.71488,0.2569895,0.005536746,0.9663983,369.8915 +0.9543646,0.007912236,-0.2985393,-20.4131,-0.008955059,0.9999576,-0.002125312,-10.73262,0.2985098,0.004701761,0.9543949,370.2672 +0.9398804,0.0126234,-0.3412706,-20.59635,-0.01277685,0.9999167,0.001798107,-10.74732,0.3412649,0.002670359,0.9399633,370.6619 +0.9228212,0.01690747,-0.3848573,-20.79175,-0.01539227,0.9998569,0.007017531,-10.75976,0.3849209,-0.0005520978,0.9229493,371.0542 +0.9027482,0.01751854,-0.4298126,-20.99448,-0.01391458,0.9998367,0.01152669,-10.76923,0.4299443,-0.004425029,0.9028445,371.3905 +0.8796044,0.02149342,-0.4752202,-21.22593,-0.01553767,0.9997438,0.01645744,-10.77923,0.4754522,-0.00709222,0.8797129,371.7584 +0.8523648,0.02856964,-0.5221668,-21.48733,-0.02085011,0.9995692,0.02065516,-10.79175,0.5225319,-0.006718489,0.8525932,372.1192 +0.8212983,0.03651026,-0.5693296,-21.76792,-0.02673728,0.9993168,0.0255143,-10.78903,0.5698721,-0.005732523,0.8217133,372.412 +0.7867686,0.04160532,-0.6158443,-22.07385,-0.02766828,0.9991,0.03214992,-10.78485,0.6166277,-0.008255185,0.7872116,372.7666 +0.7486532,0.04278519,-0.6615799,-22.40006,-0.02352688,0.9990014,0.03798332,-10.77442,0.6625443,-0.01287142,0.7489121,373.1138 +0.7068168,0.04118838,-0.7061965,-22.74041,-0.01666484,0.9989959,0.04158621,-10.76969,0.7072003,-0.01762518,0.7067935,373.3813 +0.6614981,0.04179807,-0.7487811,-23.11657,-0.009999883,0.9988484,0.04692298,-10.76253,0.7498801,-0.02355173,0.6611543,373.6999 +0.6128729,0.04327422,-0.7889957,-23.51416,-0.004725865,0.9986821,0.05110401,-10.74819,0.7901674,-0.02759156,0.6122696,373.9226 +0.5598859,0.03951947,-0.8276267,-23.9314,0.003706369,0.9987324,0.05019718,-10.72974,0.8285614,-0.03117217,0.5590297,374.1995 +0.5031833,0.03489118,-0.8634751,-24.37041,0.01238171,0.9987909,0.04757436,-10.71168,0.864091,-0.03462991,0.5021429,374.4566 +0.4432234,0.03354891,-0.8957832,-24.82872,0.01370107,0.9989291,0.04419107,-10.70388,0.8963065,-0.03185969,0.442289,374.5891 +0.3802574,0.03290759,-0.9242951,-25.3119,0.01358916,0.9990601,0.04116007,-10.6949,0.9247808,-0.02821181,0.3794528,374.7754 +0.3156375,0.02922396,-0.9484297,-25.80661,0.017383,0.9991798,0.03657279,-10.68257,0.9487206,-0.02803029,0.3148706,374.9355 +0.2501886,0.02461508,-0.9678842,-26.31378,0.03032354,0.9989871,0.03324443,-10.66974,0.9677221,-0.03766704,0.2491887,374.9978 +0.1858934,0.029085,-0.9821394,-26.84913,0.03368512,0.9987856,0.03595368,-10.65755,0.9819923,-0.03976702,0.1846879,375.1027 +0.1257519,0.03922354,-0.991286,-27.40599,0.0273888,0.9986999,0.04299137,-10.64986,0.9916835,-0.03255637,0.1245141,375.1528 +0.07102653,0.0482788,-0.9963054,-27.96897,0.01710392,0.9986222,0.04961041,-10.63684,0.9973277,-0.02056437,0.07010291,375.108 +0.02305901,0.05233677,-0.9983633,-28.54132,0.007376844,0.9985926,0.05251919,-10.62462,0.9997068,-0.008575804,0.02264047,375.0885 +-0.01792318,0.05391266,-0.9983848,-29.12195,0.002374449,0.9985446,0.05387868,-10.60557,0.9998365,-0.001404931,-0.01802511,375.0327 +-0.04827881,0.05930858,-0.9970716,-29.72223,-0.0005899366,0.9982337,0.05940629,-10.58472,0.9988337,0.003456278,-0.04815854,374.9866 +-0.07000877,0.06580012,-0.9953739,-30.33161,-0.001597988,0.9978134,0.0660738,-10.56036,0.9975451,0.006216345,-0.06975054,374.9304 +-0.08720875,0.06846012,-0.9938349,-30.94654,-0.003293942,0.9976106,0.06900926,-10.53899,0.9961846,0.009291849,-0.08677486,374.8624 +-0.09738643,0.06444002,-0.9931583,-31.5615,-0.00852214,0.9978111,0.06557758,-10.51891,0.9952101,0.0148502,-0.09662409,374.7889 +-0.1031194,0.05304976,-0.9932533,-32.17858,-0.01235733,0.9984313,0.05460926,-10.49498,0.9945922,0.01790524,-0.1023021,374.7144 +-0.1059933,0.04028742,-0.9935504,-32.8073,-0.01301408,0.9990371,0.04189827,-10.46742,0.9942816,0.01737108,-0.1053669,374.6461 +-0.105944,0.03481156,-0.9937626,-33.45603,-0.009943746,0.9992999,0.03606564,-10.44538,0.9943223,0.01370266,-0.1055237,374.5824 +-0.1044871,0.03834089,-0.9937869,-34.1246,-0.00676189,0.9992061,0.03926092,-10.4301,0.9945032,0.01082214,-0.1041449,374.5193 +-0.1034141,0.04456558,-0.9936395,-34.80652,-0.00451677,0.9989644,0.0452745,-10.4195,0.9946281,0.009170066,-0.1031057,374.4543 +-0.1026641,0.04646491,-0.9936303,-35.49402,-0.005460093,0.998867,0.04727396,-10.40427,0.9947011,0.01027865,-0.1022941,374.3853 +-0.1017117,0.04631928,-0.993735,-36.19251,-0.00807354,0.9988441,0.04738378,-10.38614,0.9947811,0.01284245,-0.1012201,374.312 +-0.1005378,0.04528225,-0.9939023,-36.90223,-0.01181186,0.998839,0.046702,-10.36462,0.9948631,0.01643515,-0.09988617,374.2372 +-0.09752465,0.04322706,-0.9942939,-37.62426,-0.01683617,0.9988416,0.04507614,-10.34508,0.9950907,0.02113614,-0.0966839,374.1631 +-0.09296835,0.04170676,-0.9947952,-38.36035,-0.01995138,0.9988436,0.04374105,-10.32578,0.9954691,0.02391407,-0.09202874,374.089 +-0.08863652,0.04189795,-0.9951825,-39.11122,-0.02284854,0.9987665,0.04408387,-10.30501,0.9958019,0.0266459,-0.08756987,374.017 +-0.08436246,0.0444428,-0.9954436,-39.88459,-0.02433195,0.998615,0.0466465,-10.28617,0.996138,0.02815629,-0.08316424,373.9571 +-0.08018097,0.04803644,-0.9956222,-40.67409,-0.02744073,0.9983531,0.05037811,-10.26296,0.9964025,0.03135997,-0.07873076,373.8909 +-0.07676122,0.05010214,-0.9957899,-41.47522,-0.02919341,0.9981955,0.05247358,-10.24075,0.996622,0.03309844,-0.07516005,373.8277 +-0.07383626,0.04858678,-0.9960861,-42.29422,-0.02994891,0.9982539,0.05091253,-10.20926,0.9968205,0.03359089,-0.07225221,373.7678 +-0.07058995,0.04572886,-0.9964567,-43.12535,-0.02838747,0.9984519,0.04783143,-10.17746,0.9971014,0.03166331,-0.06918254,373.7174 +-0.06697657,0.04619894,-0.9966844,-43.97811,-0.02602609,0.9985066,0.04803235,-10.14326,0.997415,0.02915684,-0.06567417,373.6686 +-0.06389866,0.05055652,-0.996675,-44.85127,-0.02613449,0.9982886,0.05231391,-10.10567,0.9976141,0.02939038,-0.06246804,373.6186 +-0.06060847,0.05610264,-0.9965837,-45.73389,-0.02551888,0.9980057,0.05773466,-10.07084,0.9978353,0.02893091,-0.05905592,373.5706 +-0.05771309,0.06146979,-0.996439,-46.62626,-0.02393362,0.9977305,0.0629357,-10.02954,0.9980462,0.02748061,-0.05611092,373.5291 +-0.05477673,0.06289815,-0.9965156,-47.52284,-0.02269734,0.9976776,0.06421914,-9.991003,0.9982406,0.02613597,-0.05322189,373.4909 +-0.05204313,0.06247196,-0.9966889,-48.42205,-0.02477983,0.9976533,0.06382632,-9.953142,0.9983373,0.02801951,-0.05037295,373.4503 +-0.05006292,0.05792578,-0.9970649,-49.32645,-0.02674703,0.9978808,0.05931617,-9.914375,0.9983878,0.02963806,-0.04840749,373.4089 +-0.04895235,0.05440657,-0.9973182,-50.23736,-0.02614553,0.9981033,0.05573273,-9.876888,0.9984588,0.02880366,-0.04743702,373.372 +-0.0484408,0.05342693,-0.9973962,-51.15358,-0.02516498,0.9981861,0.05469145,-9.841602,0.998509,0.02774875,-0.04700845,373.3369 +-0.04862114,0.05432673,-0.9973388,-52.07555,-0.02479428,0.9981463,0.05557947,-9.806627,0.9985095,0.02743063,-0.04718402,373.2968 +-0.04949872,0.0554054,-0.9972363,-53.00011,-0.02392873,0.9981078,0.05664156,-9.773933,0.9984875,0.02666628,-0.04807927,373.2575 +-0.05049479,0.05646806,-0.9971267,-53.91919,-0.02318753,0.9980649,0.05769543,-9.738669,0.9984551,0.02603423,-0.04908772,373.2173 +-0.05177777,0.05708943,-0.9970255,-54.83668,-0.02481405,0.9979829,0.05843291,-9.701954,0.9983503,0.02776577,-0.05025671,373.1742 +-0.05312263,0.05794277,-0.9969056,-55.75461,-0.02731211,0.9978573,0.0594535,-9.666145,0.9982144,0.03038592,-0.05142626,373.1277 +-0.0542467,0.05810904,-0.9968353,-56.66814,-0.02844995,0.99781,0.05971408,-9.629948,0.9981221,0.03159921,-0.05247469,373.0824 +-0.05504347,0.05865725,-0.9967596,-57.58298,-0.02781843,0.9977953,0.06025441,-9.595395,0.9980963,0.0310449,-0.05329036,373.0404 +-0.05634955,0.05780837,-0.9967362,-58.48949,-0.02953307,0.997789,0.05953906,-9.561552,0.9979742,0.03279167,-0.0545177,372.9954 +-0.05783411,0.05596459,-0.9967564,-59.39958,-0.02987117,0.9978834,0.05776108,-9.525374,0.9978792,0.03311484,-0.05603997,372.9496 +-0.06063863,0.05432452,-0.9966804,-60.30854,-0.02818048,0.9980267,0.05611242,-9.48765,0.9977619,0.03148951,-0.05898807,372.905 +-0.06411494,0.05616216,-0.996361,-61.21711,-0.02896349,0.9978898,0.05811213,-9.447759,0.9975221,0.03258395,-0.06235299,372.854 +-0.06804216,0.06096702,-0.9958179,-62.12827,-0.02986373,0.9975594,0.06311418,-9.413426,0.9972353,0.03403326,-0.06605539,372.7971 +-0.07190997,0.06371124,-0.9953742,-63.03109,-0.03129255,0.9973224,0.06609665,-9.379269,0.9969201,0.0359008,-0.06972373,372.7365 +-0.07535459,0.06172806,-0.9952444,-63.93282,-0.03609309,0.9972592,0.06458581,-9.341901,0.9965033,0.04078828,-0.0729201,372.6685 +-0.07801495,0.05909321,-0.9951993,-64.82727,-0.03865197,0.9973119,0.06224864,-9.302409,0.9962026,0.04332273,-0.07552116,372.5998 +-0.0804508,0.05580634,-0.9951951,-65.72199,-0.03927851,0.9974784,0.05910963,-9.267669,0.9959843,0.04384519,-0.07805594,372.5323 +-0.08191513,0.05392768,-0.9951793,-66.61519,-0.03649405,0.997703,0.05706835,-9.234586,0.9959709,0.04099288,-0.07975893,372.4709 +-0.08305142,0.05333808,-0.9951169,-67.5092,-0.03622612,0.997745,0.05650235,-9.200919,0.9958866,0.04074182,-0.08093191,372.4021 +-0.0834745,0.05660145,-0.9949012,-68.40522,-0.0385695,0.997454,0.05998277,-9.171513,0.9957632,0.04337987,-0.08107888,372.3333 +-0.0831029,0.05896421,-0.994795,-69.30001,-0.03638643,0.9974028,0.06215843,-9.143158,0.9958764,0.04136258,-0.08074156,372.2717 +-0.08177674,0.0553196,-0.9951142,-70.18617,-0.03767985,0.9975729,0.05855276,-9.112672,0.9959381,0.04228401,-0.07949382,372.2087 +-0.0785567,0.05236842,-0.9955332,-71.07138,-0.03926376,0.9976819,0.05557973,-9.081827,0.9961361,0.04345453,-0.07631841,372.1463 +-0.07545427,0.05287978,-0.9957462,-71.95849,-0.04253972,0.997513,0.05619713,-9.051563,0.9962414,0.04659907,-0.07301712,372.0834 +-0.0710678,0.05549648,-0.9959265,-72.84534,-0.04331506,0.9973375,0.05866602,-9.021905,0.9965305,0.04730788,-0.06847474,372.0284 +-0.06643211,0.05727894,-0.9961455,-73.73148,-0.04448647,0.9971881,0.06030567,-8.989193,0.9967987,0.04832122,-0.06369717,371.9759 +-0.06115884,0.0582584,-0.9964264,-74.61312,-0.04498354,0.9971199,0.06105996,-8.955145,0.9971138,0.04855714,-0.05836203,371.9277 +-0.05591674,0.05938125,-0.9966681,-75.49532,-0.04501014,0.997065,0.06193015,-8.920653,0.9974203,0.0483231,-0.05307987,371.8861 +-0.05081605,0.05971275,-0.9969213,-76.36962,-0.0445436,0.9970821,0.06199291,-8.885715,0.9977141,0.0475567,-0.04800796,371.8493 +-0.04531908,0.05881432,-0.9972397,-77.24379,-0.04366453,0.9971947,0.06079599,-8.853528,0.9980178,0.04629922,-0.04262384,371.8176 +-0.0402374,0.05691804,-0.9975677,-78.11605,-0.04420633,0.9972972,0.0586857,-8.82199,0.9982117,0.04646016,-0.03761251,371.7873 +-0.03499781,0.05499364,-0.9978732,-78.98474,-0.04491381,0.9973894,0.05654222,-8.793073,0.9983776,0.04679714,-0.03243647,371.7602 +-0.02986458,0.05393778,-0.9980976,-79.84974,-0.04541275,0.9974386,0.05526099,-8.767265,0.9985218,0.0469767,-0.02733862,371.7354 +-0.02462415,0.05467083,-0.9982008,-80.71785,-0.04546706,0.997409,0.05574908,-8.739272,0.9986623,0.04675803,-0.02207463,371.7185 +-0.01968438,0.05569423,-0.9982538,-81.58408,-0.04569083,0.997354,0.056545,-8.709843,0.9987616,0.0467241,-0.01708758,371.7068 +-0.01502749,0.05761819,-0.9982256,-82.44873,-0.04662108,0.9972121,0.05826154,-8.680237,0.9987996,0.04741388,-0.01229937,371.6974 +-0.01072362,0.06007042,-0.9981366,-83.31178,-0.04623057,0.9970967,0.06050454,-8.648349,0.9988732,0.04679325,-0.007915395,371.6948 +-0.007057561,0.06168402,-0.9980708,-84.16878,-0.04410449,0.9971051,0.06193622,-8.617964,0.999002,0.04445652,-0.004316589,371.698 +-0.004087821,0.06197835,-0.9980692,-85.01559,-0.04162482,0.9972019,0.06209499,-8.589614,0.9991249,0.04179828,-0.001496546,371.7041 +-0.001760038,0.06103921,-0.9981338,-85.84271,-0.03964425,0.9973464,0.06106097,-8.562016,0.9992123,0.03967774,0.0006644851,371.7099 +-0.000318394,0.05851355,-0.9982866,-86.65213,-0.03639312,0.9976246,0.05848637,-8.5347,0.9993375,0.03634938,0.001811852,371.7181 +0.0007352782,0.05793684,-0.99832,-87.44548,-0.03345229,0.9977629,0.05787988,-8.511473,0.99944,0.03335353,0.002671752,371.7253 +0.001306633,0.05803989,-0.9983134,-88.21569,-0.03165525,0.9978163,0.05796957,-8.488287,0.999498,0.03152611,0.003141046,371.7341 +0.001243637,0.05583075,-0.9984395,-88.96671,-0.02876484,0.9980291,0.05577198,-8.4681,0.9995854,0.02865059,0.002847147,371.742 +0.0003983406,0.05534356,-0.9984673,-89.69543,-0.02694129,0.9981055,0.05531277,-8.448239,0.9996369,0.02687796,0.001888611,371.7483 +-0.001761077,0.05446575,-0.9985141,-90.40266,-0.02387835,0.9982286,0.0544923,-8.426609,0.9997133,0.02393883,-0.0004574064,371.7553 +-0.004984478,0.05643605,-0.9983938,-91.08715,-0.02194172,0.9981596,0.05653236,-8.410589,0.9997468,0.02218826,-0.003737002,371.7554 +-0.009015765,0.05941652,-0.9981926,-91.74421,-0.0192355,0.9980381,0.05958107,-8.396913,0.9997743,0.0197379,-0.007855171,371.7538 +-0.0141781,0.0603301,-0.9980778,-92.37686,-0.01802747,0.9980004,0.06058152,-8.377804,0.9997369,0.01885175,-0.01306215,371.7472 +-0.02005889,0.06023929,-0.9979824,-92.99091,-0.01575382,0.9980402,0.06055943,-8.359815,0.9996746,0.01693679,-0.01907058,371.7379 +-0.02649183,0.05783823,-0.9979744,-93.58457,-0.01469932,0.9981943,0.05824119,-8.343593,0.9995409,0.01621246,-0.02559381,371.7213 +-0.03357685,0.05543715,-0.9978975,-94.18117,-0.01368763,0.9983413,0.05592237,-8.327124,0.9993424,0.01553655,-0.03276235,371.7 +-0.04160326,0.05552193,-0.9975904,-94.7746,-0.01405422,0.9983235,0.05614886,-8.309292,0.9990353,0.01635634,-0.04075319,371.6732 +-0.04893424,0.05683927,-0.9971834,-95.36688,-0.01275195,0.9982625,0.05752656,-8.292758,0.9987206,0.01553106,-0.0481244,371.6455 +-0.05632171,0.05739264,-0.9967618,-95.95417,-0.01322561,0.9982159,0.05822369,-8.278958,0.998325,0.01646204,-0.05546217,371.6101 +-0.06340252,0.05498277,-0.9964723,-96.5378,-0.01385944,0.9983364,0.05596747,-8.263858,0.9978918,0.01735902,-0.062535,371.5706 +-0.07036542,0.05106428,-0.9962134,-97.116,-0.0151696,0.9985186,0.05225392,-8.247622,0.9974059,0.01878903,-0.06948655,371.5277 +-0.07669274,0.04779096,-0.9959088,-97.6929,-0.01476679,0.9986866,0.04906143,-8.229449,0.9969454,0.01846903,-0.07588628,371.484 +-0.08328609,0.0451469,-0.9955025,-98.26993,-0.01430732,0.9988161,0.04649417,-8.213451,0.9964229,0.01811529,-0.08254155,371.4388 +-0.08895906,0.04273338,-0.9951182,-98.84636,-0.01434183,0.9989207,0.04417877,-8.199034,0.995932,0.01820192,-0.08825016,371.389 +-0.09452557,0.04149222,-0.9946574,-99.42884,-0.01634106,0.9989318,0.04322348,-8.186657,0.9953883,0.02033949,-0.09374656,371.3324 +-0.09922676,0.04069118,-0.9942325,-100.0168,-0.01818838,0.9989224,0.04269838,-8.171096,0.9948986,0.0223203,-0.09837972,371.2717 +-0.1040151,0.03997827,-0.9937719,-100.6101,-0.01775479,0.9989579,0.04204524,-8.156426,0.9944172,0.02201755,-0.1031969,371.2114 +-0.1084053,0.04003776,-0.9933002,-101.2124,-0.01644231,0.9989797,0.04206116,-8.141451,0.9939708,0.0208918,-0.1076364,371.1501 +-0.112151,0.03932628,-0.9929127,-101.8198,-0.01767161,0.9989796,0.04156262,-8.127155,0.993534,0.02220765,-0.1113416,371.0817 +-0.1155328,0.03695243,-0.9926161,-102.4309,-0.02230033,0.9989594,0.03978416,-8.112895,0.9930533,0.02673204,-0.1145885,371.0055 +-0.1178769,0.03551224,-0.9923931,-103.0491,-0.02583621,0.9989124,0.03881438,-8.096046,0.992692,0.03021499,-0.1168311,370.9299 +-0.1193593,0.03462679,-0.9922472,-103.6768,-0.02589438,0.9989431,0.03797535,-8.07829,0.9925134,0.03022633,-0.1183365,370.8586 +-0.1199653,0.03437181,-0.9921829,-104.3134,-0.02424181,0.9990011,0.03753911,-8.060977,0.992482,0.0285557,-0.1190122,370.7887 +-0.1201938,0.03558811,-0.9921124,-104.9637,-0.02512899,0.998928,0.03887696,-8.04239,0.9924323,0.02960355,-0.1191706,370.7129 +-0.1195223,0.03957517,-0.9920425,-105.6283,-0.0252561,0.9987607,0.04288607,-8.019412,0.9925102,0.03018096,-0.1183746,370.6343 +-0.1193852,0.04476975,-0.9918381,-106.3014,-0.02834621,0.9984219,0.0484789,-7.995103,0.9924432,0.03390251,-0.1179277,370.5532 +-0.1182758,0.05654201,-0.9913697,-106.9999,-0.02965854,0.9977309,0.06044325,-7.961767,0.9925377,0.03655155,-0.1163305,370.4698 +-0.1165393,0.06727038,-0.9909053,-107.7095,-0.03336337,0.9968753,0.07159951,-7.927113,0.9926255,0.0414041,-0.1139308,370.3837 +-0.1137649,0.07017563,-0.9910262,-108.4259,-0.03556553,0.9965753,0.07465132,-7.892215,0.9928709,0.04373906,-0.1108794,370.2999 +-0.1107229,0.06308715,-0.991847,-109.1474,-0.03977229,0.9969025,0.06784863,-7.863187,0.9930551,0.04696042,-0.1078708,370.2185 +-0.1064261,0.04665078,-0.9932257,-109.8735,-0.04240981,0.9977768,0.05140885,-7.822793,0.9934157,0.04759375,-0.1042111,370.1404 +-0.1024481,0.03848327,-0.9939937,-110.6253,-0.04362888,0.998116,0.04313958,-7.775619,0.9937811,0.0477864,-0.1005761,370.0636 +-0.09832008,0.05068146,-0.9938635,-111.4139,-0.04315178,0.9975458,0.05513813,-7.727221,0.9942188,0.04830816,-0.09589178,369.9905 +-0.09406416,0.07002775,-0.9931003,-112.2248,-0.04478078,0.9962159,0.07448898,-7.690621,0.9945585,0.05147854,-0.0905723,369.9157 +-0.09014759,0.07570782,-0.9930467,-113.0391,-0.04360401,0.9958503,0.07987989,-7.648582,0.9949734,0.0505018,-0.08647234,369.8495 +-0.08662695,0.06646987,-0.9940209,-113.8504,-0.04123745,0.9966773,0.07024128,-7.59755,0.9953869,0.04707567,-0.08359806,369.7875 +-0.08284837,0.04926372,-0.9953438,-114.671,-0.04247035,0.9976954,0.05291518,-7.551143,0.9956567,0.04665653,-0.08056519,369.7225 +-0.07889538,0.03753908,-0.9961759,-115.5125,-0.0447382,0.9981506,0.04115669,-7.506696,0.9958785,0.04781418,-0.07707004,369.6572 +-0.07491892,0.04034314,-0.9963732,-116.3815,-0.05025876,0.9977586,0.04417828,-7.470759,0.9959223,0.05338627,-0.0727234,369.5879 +-0.06962103,0.05160105,-0.9962381,-117.2754,-0.05722763,0.99681,0.05562998,-7.432281,0.9959306,0.06088536,-0.06644593,369.5175 +-0.06336864,0.0564306,-0.9963935,-118.1783,-0.0620147,0.996248,0.06036638,-7.392513,0.9960615,0.06561638,-0.05963135,369.4551 +-0.05637116,0.04730667,-0.9972885,-119.079,-0.06461708,0.9966098,0.05092693,-7.352691,0.9963166,0.06731268,-0.05312322,369.402 +-0.04872278,0.03298185,-0.9982677,-119.9904,-0.06185812,0.9974364,0.03597353,-7.316884,0.996895,0.06350368,-0.04655768,369.3637 +-0.04161567,0.02354608,-0.9988562,-120.9176,-0.05483414,0.9981617,0.02581429,-7.287875,0.9976278,0.0558457,-0.04024803,369.3375 +-0.03450527,0.02873525,-0.9989914,-121.8705,-0.05062995,0.9982528,0.03046278,-7.268012,0.9981212,0.05163,-0.03299012,369.3124 +-0.0273327,0.04100511,-0.998785,-122.8443,-0.04785905,0.9979588,0.04228091,-7.242979,0.99848,0.04895655,-0.02531444,369.291 +-0.02097537,0.05238267,-0.9984068,-123.822,-0.04573789,0.9975306,0.05329761,-7.210357,0.9987332,0.04678296,-0.0185277,369.2685 +-0.01558509,0.05377397,-0.9984315,-124.798,-0.04462857,0.9975202,0.05442153,-7.171402,0.998882,0.04540673,-0.01314659,369.251 +-0.01189366,0.04843143,-0.9987557,-125.7739,-0.04266151,0.9978923,0.04889761,-7.12972,0.9990187,0.04318999,-0.009802436,369.236 +-0.009853041,0.04559385,-0.9989115,-126.7573,-0.04165551,0.9980741,0.04596652,-7.094259,0.9990834,0.04206307,-0.007934831,369.2286 +-0.009012415,0.04558744,-0.9989197,-127.7492,-0.03952559,0.9981633,0.04590953,-7.067306,0.9991779,0.03989664,-0.007193992,369.222 +-0.008681528,0.04748294,-0.9988344,-128.7442,-0.03689769,0.9981765,0.04777238,-7.039616,0.9992813,0.03726941,-0.006913688,369.2162 +-0.009113077,0.04856194,-0.9987786,-129.7392,-0.03710233,0.9981159,0.04886826,-7.010053,0.9992699,0.03750236,-0.007294146,369.2059 +-0.01110359,0.0515961,-0.9986063,-130.7381,-0.038446,0.9979074,0.05198749,-6.978782,0.9991989,0.03896967,-0.00909669,369.1913 +-0.0138441,0.05578262,-0.998347,-131.7386,-0.03814968,0.9976862,0.05627473,-6.944658,0.9991761,0.03886569,-0.01168398,369.1754 +-0.01717664,0.05655699,-0.9982516,-132.737,-0.03856448,0.9976185,0.0571847,-6.909152,0.9991084,0.03947929,-0.01495465,369.1613 +-0.02014464,0.04928275,-0.9985817,-133.7249,-0.03541369,0.9981224,0.0499745,-6.873,0.9991696,0.03637018,-0.01836153,369.151 +-0.02332779,0.04079338,-0.9988953,-134.7107,-0.03070672,0.9986665,0.04150116,-6.836596,0.9992561,0.03164093,-0.02204405,369.142 +-0.02659783,0.03897954,-0.998886,-135.6995,-0.02505098,0.9988996,0.03964713,-6.801852,0.9993322,0.0260776,-0.02559208,369.1313 +-0.02996479,0.04514121,-0.9985311,-136.6932,-0.02204334,0.9987069,0.04581066,-6.767486,0.9993078,0.02338366,-0.02893098,369.1167 +-0.03287829,0.05156356,-0.9981284,-137.686,-0.02158186,0.9983988,0.05228844,-6.733132,0.9992263,0.02326062,-0.0317128,369.0914 +-0.03520728,0.05307285,-0.9979698,-138.6679,-0.02262992,0.9982905,0.05388827,-6.69739,0.9991237,0.02448124,-0.03394606,369.063 +-0.03748825,0.05000773,-0.998045,-139.6402,-0.02654743,0.9983447,0.05101992,-6.665761,0.9989443,0.02840818,-0.03609862,369.0262 +-0.03983027,0.04611387,-0.9981418,-140.6078,-0.03099335,0.9983968,0.04736243,-6.63286,0.9987256,0.03282222,-0.03833719,368.984 +-0.04207209,0.04314398,-0.9981826,-141.5778,-0.03267945,0.9984732,0.04453395,-6.602139,0.9985799,0.0344937,-0.04059793,368.9429 +-0.04405978,0.04245006,-0.9981266,-142.5454,-0.03173202,0.9985332,0.04386809,-6.57532,0.9985248,0.03360539,-0.04264813,368.9094 +-0.04589568,0.04412076,-0.9979714,-143.5151,-0.02994613,0.9985144,0.04552197,-6.547679,0.9984972,0.03197465,-0.04450625,368.8734 +-0.04807395,0.05004634,-0.9975893,-144.486,-0.02626054,0.9983354,0.05134928,-6.517102,0.9984985,0.0286658,-0.04667968,368.839 +-0.05114879,0.05688625,-0.9970696,-145.4529,-0.02457189,0.9980025,0.0582,-6.483563,0.9983887,0.02747675,-0.04964881,368.8004 +-0.05415381,0.06143589,-0.9966409,-146.4204,-0.02202613,0.9977891,0.0627035,-6.445217,0.9982896,0.02534778,-0.05268089,368.758 +-0.05766881,0.06033428,-0.996511,-147.3774,-0.0238293,0.9978045,0.06179163,-6.403585,0.9980513,0.02730961,-0.05610447,368.7095 +-0.06084897,0.05239532,-0.9967709,-148.326,-0.0256645,0.998209,0.05403764,-6.362759,0.9978169,0.02886976,-0.05939529,368.6537 +-0.0636864,0.04699773,-0.9968627,-149.2752,-0.02801701,0.9984126,0.04886073,-6.32144,0.9975766,0.03104087,-0.06226857,368.5981 +-0.06568723,0.04667322,-0.9967481,-150.2278,-0.02595952,0.9984874,0.04846545,-6.288175,0.9975025,0.02905866,-0.06437626,368.5407 +-0.06725695,0.05207556,-0.9963758,-151.1845,-0.02506258,0.9982337,0.05386443,-6.256123,0.9974208,0.0285945,-0.065833,368.4837 +-0.06868069,0.0569711,-0.9960107,-152.1373,-0.02330961,0.9980039,0.05869246,-6.21918,0.9973663,0.02724766,-0.06721561,368.425 +-0.06992206,0.0592605,-0.9957907,-153.0839,-0.02060872,0.997935,0.06083522,-6.175756,0.9973395,0.02477569,-0.06855638,368.3679 +-0.07063564,0.06136347,-0.995613,-154.0295,-0.02036505,0.9978093,0.06294368,-6.138328,0.9972942,0.02472177,-0.06923122,368.3067 +-0.07063519,0.06221153,-0.9955604,-154.9685,-0.02085577,0.997743,0.06382765,-6.101396,0.9972841,0.02527165,-0.0691783,368.2417 +-0.07031087,0.06142404,-0.9956322,-155.9016,-0.0233709,0.9977269,0.06320372,-6.064642,0.9972513,0.02771273,-0.06871551,368.1752 +-0.0695106,0.05965373,-0.995796,-156.834,-0.02697629,0.997733,0.06165283,-6.028879,0.9972164,0.03114841,-0.06774378,368.1108 +-0.06847529,0.05766241,-0.9959851,-157.7614,-0.03023284,0.9977498,0.05984314,-5.992298,0.9971946,0.03420923,-0.0665779,368.0449 +-0.06703008,0.05437833,-0.996268,-158.6835,-0.02993901,0.9979545,0.05648472,-5.959485,0.9973016,0.03361345,-0.06526493,367.9845 +-0.06581856,0.05169557,-0.9964916,-159.6016,-0.02817943,0.9981624,0.05364351,-5.927697,0.9974336,0.0316113,-0.06424086,367.9298 +-0.06419495,0.05018196,-0.9966749,-160.5196,-0.02768902,0.9982608,0.05204525,-5.894364,0.9975531,0.03093799,-0.0626938,367.8737 +-0.06303622,0.05440678,-0.9965272,-161.4371,-0.0268719,0.9980584,0.0561902,-5.861145,0.9976494,0.03032059,-0.06145181,367.8205 +-0.06215397,0.06171511,-0.9961567,-162.3529,-0.02501014,0.9976767,0.06336976,-5.826577,0.9977531,0.0288527,-0.06046606,367.7697 +-0.06113681,0.06506562,-0.9960064,-163.2549,-0.02335634,0.9975065,0.06659728,-5.789504,0.997856,0.02733461,-0.05946467,367.7206 +-0.05982877,0.06497859,-0.9960915,-164.1449,-0.0239106,0.9974994,0.0665066,-5.750163,0.9979222,0.02779616,-0.05812548,367.6707 +-0.05841149,0.06273405,-0.9963195,-165.0253,-0.02311541,0.9976709,0.06417435,-5.709019,0.9980249,0.02677885,-0.05682532,367.6228 +-0.05732252,0.0602254,-0.9965375,-165.8914,-0.02352658,0.9978201,0.06165621,-5.66698,0.9980784,0.02697941,-0.05578066,367.5742 +-0.05642917,0.06061396,-0.996565,-166.7434,-0.02484652,0.997761,0.06209362,-5.627809,0.9980973,0.02826506,-0.05479678,367.5248 +-0.05589303,0.06415437,-0.9963735,-167.5871,-0.02637749,0.9974903,0.06570597,-5.591398,0.9980882,0.02995434,-0.05406053,367.4736 +-0.05526132,0.06704275,-0.9962186,-168.4147,-0.02552006,0.9973224,0.06853267,-5.560208,0.9981457,0.02921076,-0.05340241,367.4265 +-0.05464152,0.06385069,-0.9964625,-169.2138,-0.02113899,0.9976557,0.06508633,-5.530577,0.9982822,0.02462063,-0.05316368,367.3848 +-0.05403882,0.05696162,-0.9969129,-169.9897,-0.01773033,0.9981595,0.05799395,-5.497707,0.9983814,0.02080952,-0.05292941,367.3471 +-0.05403008,0.05219054,-0.9971745,-170.7472,-0.01663346,0.9984475,0.05315843,-5.465282,0.9984007,0.01945861,-0.05307809,367.3055 +-0.05437363,0.05665159,-0.9969123,-171.4949,-0.01285263,0.9982668,0.05742959,-5.43615,0.9984379,0.0159356,-0.05355126,367.2694 +-0.05520551,0.06563741,-0.9963153,-172.2298,-0.007400959,0.9977826,0.06614417,-5.408734,0.9984475,0.01102522,-0.05459732,367.2374 +-0.05703222,0.06879741,-0.9959991,-172.9313,-0.007073998,0.99757,0.06931099,-5.384605,0.9983472,0.01099866,-0.05640695,367.2013 +-0.05856718,0.06543314,-0.9961368,-173.6069,-0.007827707,0.9977888,0.06600189,-5.362451,0.9982527,0.01166301,-0.05792548,367.1608 +-0.0618947,0.05660624,-0.9964762,-174.255,-0.0102315,0.9983019,0.05734548,-5.340548,0.9980302,0.01374483,-0.06121043,367.1185 +-0.06447237,0.05006024,-0.9966631,-174.8867,-0.01120022,0.9986417,0.05088415,-5.317998,0.9978566,0.01444347,-0.06382411,367.076 +-0.06737246,0.04833993,-0.9965562,-175.5043,-0.01532907,0.9986575,0.04947819,-5.29466,0.9976101,0.01860975,-0.06654101,367.0285 +-0.07134877,0.05066223,-0.996164,-176.1135,-0.01799863,0.9984812,0.05206922,-5.27718,0.997289,0.02164466,-0.07032855,366.9763 +-0.076687,0.05522832,-0.9955245,-176.7064,-0.01945003,0.9981918,0.05687457,-5.259521,0.9968654,0.02372452,-0.07547414,366.9279 +-0.08250596,0.05802894,-0.9948997,-177.2879,-0.01898789,0.9980306,0.05978621,-5.242102,0.9964096,0.02382377,-0.08124162,366.8763 +-0.09101204,0.05891668,-0.9941055,-177.8516,-0.01791488,0.99799,0.06078705,-5.224067,0.9956886,0.02334164,-0.08977361,366.8251 +-0.1032542,0.05832202,-0.9929437,-178.4058,-0.01554857,0.9980628,0.06023958,-5.200726,0.9945334,0.02165884,-0.1021474,366.7708 +-0.1191298,0.0572044,-0.9912294,-178.9483,-0.01247272,0.9981739,0.0591042,-5.178101,0.9928003,0.0194044,-0.1181988,366.7006 +-0.1395858,0.05562312,-0.9886465,-179.4822,-0.01038177,0.9982839,0.05763114,-5.157679,0.9901555,0.01830839,-0.1387688,366.6191 +-0.1649366,0.0549302,-0.9847734,-180.0128,-0.00819843,0.9983371,0.05705992,-5.137863,0.98627,0.01748487,-0.164212,366.5239 +-0.1947555,0.0542035,-0.979353,-180.5245,-0.007047529,0.9983688,0.05665745,-5.114952,0.9808265,0.01793637,-0.1940558,366.3922 +-0.2293444,0.05458947,-0.9718134,-181.041,-0.006346743,0.9983209,0.0575763,-5.091005,0.9733246,0.01937265,-0.2286128,366.2572 +-0.2684077,0.05532174,-0.9617156,-181.5484,-0.003612893,0.9982847,0.05843369,-5.065694,0.9632986,0.01915863,-0.2677474,366.1069 +-0.3116168,0.05559536,-0.9485801,-182.0234,-0.0005501312,0.9982762,0.05868874,-5.042477,0.9502077,0.01881024,-0.311049,365.8942 +-0.3588189,0.05633877,-0.9317054,-182.5069,0.003899528,0.9982585,0.05886136,-5.016582,0.933399,0.01748736,-0.3584137,365.7009 +-0.4082375,0.05754307,-0.9110604,-182.9767,0.00705041,0.9981803,0.05988641,-4.990208,0.9128485,0.01802453,-0.4079003,365.4859 +-0.4584834,0.05859806,-0.886769,-183.3992,0.0108955,0.9981194,0.06032289,-4.960498,0.8886361,0.01799525,-0.4582596,365.1915 +-0.5086892,0.05913891,-0.8589167,-183.8309,0.0134766,0.9980627,0.06073807,-4.931494,0.8608447,0.01932152,-0.5085007,364.9227 +-0.5589647,0.0596891,-0.8270404,-184.2427,0.01409014,0.9979454,0.06250068,-4.906417,0.8290717,0.02328256,-0.5586572,364.6328 +-0.6098841,0.05953893,-0.790251,-184.596,0.01470181,0.9978522,0.06383373,-4.880452,0.7923542,0.02731306,-0.6094495,364.2445 +-0.6603689,0.06003445,-0.7485378,-184.9679,0.01597157,0.9976966,0.06592728,-4.85677,0.7507714,0.03158101,-0.6598066,363.9019 +-0.7092737,0.05937761,-0.702428,-185.318,0.01844311,0.9976682,0.06571199,-4.831999,0.7046919,0.03365283,-0.7087149,363.538 +-0.7555738,0.05943099,-0.6523621,-185.6,0.02222471,0.9976283,0.06514427,-4.805931,0.6546864,0.03472274,-0.7551026,363.0652 +-0.7988709,0.05907796,-0.5985943,-185.9006,0.02394102,0.9974994,0.06649655,-4.775507,0.6010259,0.0387912,-0.7982876,362.6453 +-0.8383895,0.05550605,-0.5422381,-186.1231,0.02322727,0.997536,0.06619933,-4.744508,0.5445765,0.0429061,-0.8376129,362.1165 +-0.8738963,0.05274897,-0.483242,-186.3646,0.02341367,0.9975088,0.06654318,-4.71322,0.4855482,0.04683737,-0.8729543,361.6515 +-0.9051966,0.05198891,-0.4218014,-186.5757,0.02608472,0.9974148,0.06695741,-4.68742,0.4241919,0.04960704,-0.9042125,361.1633 +-0.9315783,0.05654804,-0.359116,-186.7261,0.03403483,0.9970558,0.06871169,-4.654975,0.3619442,0.05178787,-0.9307601,360.5806 +-0.9527563,0.05901103,-0.2979483,-186.8836,0.04017523,0.9968103,0.06895698,-4.625626,0.3010671,0.05372905,-0.9520881,360.0514 +-0.9686751,0.05929647,-0.2411486,-186.9996,0.04422759,0.9967431,0.06743229,-4.592235,0.2443617,0.05465455,-0.9681426,359.4979 +-0.9804789,0.05597044,-0.1884899,-187.0639,0.04447969,0.996916,0.06465307,-4.5607,0.1915272,0.05500699,-0.9799446,358.8838 +-0.9884014,0.05102838,-0.1430345,-187.1247,0.04242581,0.9971388,0.06256298,-4.531234,0.1458178,0.05576897,-0.9877383,358.2971 +-0.9934441,0.04535166,-0.1049386,-187.1523,0.0391646,0.9974124,0.06028746,-4.505623,0.1074011,0.05578233,-0.9926496,357.6756 +-0.9963187,0.04145223,-0.07503889,-187.1797,0.03689873,0.9974504,0.06108384,-4.473612,0.07737963,0.05809012,-0.9953079,357.0554 +-0.9977972,0.04047053,-0.05256361,-187.1955,0.03712769,0.9973172,0.06308685,-4.44088,0.05497574,0.06099631,-0.9966228,356.4193 +-0.9985377,0.04133062,-0.03484636,-187.206,0.03914782,0.9973604,0.06115309,-4.412544,0.03728188,0.0596995,-0.9975199,355.7735 +-0.9989044,0.04266642,-0.01922528,-187.2126,0.04153044,0.9975639,0.05604879,-4.393112,0.02156984,0.05518894,-0.9982429,355.1263 +-0.998932,0.04597104,-0.004651654,-187.2097,0.04569286,0.997783,0.04838651,-4.369286,0.00686572,0.04812228,-0.9988178,354.4658 +-0.9986796,0.05038595,0.01001768,-187.1974,0.05074252,0.9979394,0.03926943,-4.339156,-0.008018405,0.03972589,-0.9991784,353.8009 +-0.9984338,0.04978318,0.0255271,-187.1702,0.05064785,0.9981231,0.03442465,-4.316768,-0.02376542,0.03566362,-0.9990812,353.1204 +-0.9981428,0.04624168,0.03965786,-187.1252,0.04763379,0.9982547,0.03490667,-4.29869,-0.0379745,0.03673089,-0.9986034,352.4225 +-0.9976692,0.04479935,0.05147053,-187.0783,0.04668419,0.99826,0.0360198,-4.279837,-0.04976731,0.0383387,-0.9980247,351.7129 +-0.9970587,0.04622445,0.06113338,-187.0304,0.04839278,0.9982332,0.03447629,-4.261203,-0.05943172,0.03733329,-0.997534,350.9938 +-0.9967822,0.04408104,0.06694989,-186.971,0.04644513,0.9983361,0.03417423,-4.242069,-0.06533205,0.03717375,-0.9971709,350.2608 +-0.9966352,0.03992878,0.07158207,-186.9043,0.04247851,0.998503,0.0344578,-4.221325,-0.07009905,0.03738255,-0.9968393,349.5157 +-0.996433,0.03752733,0.07558415,-186.8369,0.04014029,0.9986372,0.03335233,-4.199482,-0.07422952,0.03626733,-0.9965814,348.7591 +-0.9961127,0.03731923,0.07979282,-186.7699,0.04016267,0.9986031,0.03433169,-4.177337,-0.07840013,0.03740292,-0.99622,347.9857 +-0.9956153,0.03861368,0.0852015,-186.7002,0.0417239,0.9985149,0.03502987,-4.153622,-0.08372233,0.0384312,-0.9957477,347.1995 +-0.9952306,0.03680483,0.0903417,-186.6213,0.04045359,0.9984241,0.0388946,-4.129835,-0.08876782,0.04236373,-0.995151,346.3977 +-0.9949026,0.03255063,0.09544259,-186.5329,0.03657758,0.9984996,0.04075038,-4.104362,-0.09397293,0.04403371,-0.9946004,345.585 +-0.9944347,0.03108847,0.1006637,-186.4415,0.03538405,0.9985253,0.04117163,-4.07709,-0.09923532,0.04450438,-0.9940682,344.7617 +-0.9940038,0.02946047,0.1053025,-186.343,0.03383768,0.9986255,0.04002556,-4.043981,-0.1039786,0.04334874,-0.9936344,343.9351 +-0.9937319,0.02822859,0.1081676,-186.2422,0.03232519,0.9988177,0.03630788,-4.015366,-0.1070148,0.03957683,-0.9934694,343.0956 +-0.9936779,0.02574361,0.1092779,-186.1381,0.02941187,0.9990521,0.03208984,-3.987286,-0.1083482,0.03510103,-0.9934931,342.2456 +-0.9937461,0.02418959,0.1090119,-186.036,0.02752673,0.9991941,0.02921214,-3.960565,-0.1082174,0.03203018,-0.9936111,341.3784 +-0.9938599,0.02444263,0.1079124,-185.9369,0.02790306,0.9991399,0.03067411,-3.936593,-0.1070698,0.03349685,-0.993687,340.4954 +-0.9940648,0.02269373,0.1063967,-185.8362,0.02646425,0.9990659,0.03416118,-3.912968,-0.1055221,0.03677413,-0.9937367,339.6007 +-0.9944083,0.0196358,0.1037621,-185.7407,0.02343103,0.9990955,0.03548468,-3.893331,-0.1029715,0.03771751,-0.9939689,338.6963 +-0.9946803,0.01831302,0.1013701,-185.6499,0.02202904,0.9991211,0.03566057,-3.86895,-0.100628,0.03770394,-0.9942094,337.7892 +-0.9949748,0.01651522,0.09875441,-185.5588,0.02023026,0.9991202,0.03673653,-3.84331,-0.09806081,0.03854974,-0.9944334,336.87 +-0.9953039,0.01463971,0.095686,-185.4699,0.01839618,0.9990894,0.03849465,-3.819756,-0.09503532,0.04007413,-0.9946669,335.9488 +-0.9955645,0.0136556,0.09308602,-185.3826,0.01756023,0.9989942,0.0412571,-3.79503,-0.092429,0.04270871,-0.9948029,335.0194 +-0.9957762,0.01331149,0.09084416,-185.3002,0.01752134,0.9988014,0.0457023,-3.764975,-0.09012691,0.04710096,-0.9948158,334.0884 +-0.9959957,0.01275509,0.08848706,-185.2184,0.01745002,0.998469,0.05248873,-3.733635,-0.08768208,0.05382264,-0.9946934,333.1545 +-0.996183,0.01186678,0.08647927,-185.1342,0.01690448,0.9981876,0.05775568,-3.698286,-0.08563715,0.0589971,-0.9945781,332.2208 +-0.996383,0.0102593,0.08435512,-185.0534,0.01504292,0.9983023,0.05626942,-3.662061,-0.08363462,0.05733483,-0.9948456,331.2957 +-0.9966026,0.00786137,0.08198488,-184.9741,0.01215374,0.9985737,0.05198858,-3.627279,-0.08145924,0.05280837,-0.9952766,330.3728 +-0.9967791,0.008640203,0.07972968,-184.8995,0.0126901,0.998647,0.05042916,-3.589469,-0.07918608,0.0512785,-0.99554,329.4393 +-0.9969232,0.007588653,0.07801657,-184.8257,0.01161914,0.9986137,0.05133846,-3.554183,-0.07751882,0.05208698,-0.9956293,328.5131 +-0.9970723,0.008984319,0.07593565,-184.7565,0.01295678,0.9985639,0.05198373,-3.522351,-0.07535955,0.0528154,-0.9957567,327.5735 +-0.9971547,0.01145367,0.07450714,-184.6927,0.01516954,0.9986589,0.04949952,-3.489816,-0.07384026,0.05048891,-0.9959912,326.6419 +-0.9972948,0.0109164,0.07269077,-184.6267,0.01448511,0.998706,0.04874956,-3.45773,-0.07206453,0.04967061,-0.9961623,325.7067 +-0.997479,0.00704121,0.070613,-184.5562,0.01050092,0.9987561,0.04874441,-3.428208,-0.07018194,0.04936302,-0.996312,324.7739 +-0.997628,0.002278638,0.06879934,-184.4861,0.005514911,0.998885,0.04688597,-3.397748,-0.06861579,0.04715416,-0.9965281,323.835 +-0.9977938,-0.001132186,0.06638078,-184.4181,0.001922755,0.9989424,0.04593939,-3.364575,-0.06636259,0.04596566,-0.9967362,322.8956 +-0.99793,-0.00503211,0.06411358,-184.348,-0.00196167,0.9988519,0.04786373,-3.332388,-0.06428083,0.04763887,-0.9967941,321.9491 +-0.9980669,-0.009151789,0.06147257,-184.2803,-0.006068217,0.9987226,0.05016228,-3.30081,-0.06185312,0.04969227,-0.9968474,321.0006 +-0.998198,-0.009592838,0.05923543,-184.2193,-0.006496391,0.9986131,0.05224648,-3.265983,-0.05965446,0.0517675,-0.9968758,320.0498 +-0.9983672,-0.003834383,0.05699468,-184.1701,-0.0008478517,0.9986294,0.05233211,-3.231575,-0.05711722,0.05219833,-0.9970019,319.0934 +-0.9984642,0.003865386,0.0552666,-184.1277,0.006588943,0.998768,0.04918324,-3.199115,-0.0550084,0.04947185,-0.9972595,318.1419 +-0.9985477,0.005238518,0.0536197,-184.0779,0.007690445,0.998929,0.04562427,-3.164447,-0.05332327,0.04597036,-0.9975185,317.1871 +-0.9986099,0.003144079,0.05261652,-184.0244,0.005787295,0.9987245,0.05015863,-3.127592,-0.0523917,0.05039341,-0.9973543,316.2249 +-0.9986151,0.003929795,0.05246509,-183.9789,0.007010536,0.9982531,0.05866544,-3.092866,-0.05214289,0.05895199,-0.996898,315.26 +-0.9986216,0.005750679,0.05217223,-183.9332,0.008951732,0.9980773,0.06133073,-3.055122,-0.05171923,0.06171321,-0.996753,314.3008 +-0.9986156,0.0001170781,0.05260237,-183.8728,0.003117758,0.9983712,0.05696598,-3.008214,-0.05251002,0.0570511,-0.9969893,313.3504 +-0.9985475,-0.00490734,0.05365609,-183.8124,-0.002275125,0.9987958,0.04900845,-2.968608,-0.05383197,0.04881518,-0.997356,312.4073 +-0.9984639,-0.001452966,0.05538791,-183.7616,0.0009891994,0.9990293,0.04403898,-2.932355,-0.05539813,0.04402611,-0.9974932,311.4602 +-0.9983835,-0.0006416411,0.05683359,-183.71,0.001790703,0.9990848,0.04273624,-2.900357,-0.05680899,0.04276892,-0.9974685,310.5136 +-0.9983398,-0.001109504,0.0575892,-183.6542,0.001462512,0.9990038,0.04459994,-2.86823,-0.05758132,0.04461011,-0.9973436,309.5622 +-0.9983209,-0.0004420059,0.05792403,-183.6018,0.002269527,0.9989046,0.04673759,-2.83886,-0.05788123,0.04679056,-0.9972263,308.6162 +-0.9983784,-0.0002405604,0.05692603,-183.5521,0.002406885,0.9989185,0.04643348,-2.809138,-0.05687563,0.04649519,-0.997298,307.6738 +-0.998508,-0.001651476,0.05458224,-183.5027,0.0008158484,0.9989798,0.04515048,-2.77948,-0.05460112,0.04512763,-0.9974879,306.7371 +-0.9986559,-0.001211054,0.05181624,-183.4581,0.001108385,0.9989993,0.04471047,-2.749605,-0.05181854,0.0447078,-0.9976552,305.7949 +-0.9987961,6.82466E-05,0.04905484,-183.4195,0.002375459,0.9988932,0.04697639,-2.718396,-0.04899734,0.04703636,-0.9976907,304.8598 +-0.9989324,4.791895E-05,0.0461965,-183.3786,0.00225904,0.9988538,0.04781219,-2.6898,-0.04614125,0.0478655,-0.9977874,303.9271 +-0.9990585,-0.001822606,0.04334551,-183.3431,8.849913E-05,0.9990294,0.04404711,-2.662548,-0.04338372,0.04400947,-0.9980886,303.0002 +-0.9991392,0.001987768,0.04143727,-183.3166,0.003641517,0.9991981,0.0398723,-2.628647,-0.04132478,0.03998886,-0.9983452,302.0756 +-0.9991699,0.002507305,0.04066007,-183.2895,0.004265357,0.9990569,0.04320867,-2.597211,-0.04051338,0.04334622,-0.9982383,301.1445 +-0.9991948,0.001178021,0.04010582,-183.2613,0.003098074,0.9988499,0.04784604,-2.570727,-0.04000333,0.04793176,-0.9980492,300.2119 +-0.9991897,0.003534238,0.04009399,-183.2372,0.005529962,0.9987451,0.04977476,-2.538212,-0.03986776,0.04995613,-0.9979553,299.2862 +-0.9991814,0.004771053,0.0401721,-183.2087,0.006585816,0.9989579,0.04516405,-2.505074,-0.03991475,0.04539163,-0.9981715,298.3688 +-0.9991223,0.006492765,0.04138362,-183.1788,0.008218533,0.9990977,0.04166875,-2.47137,-0.04107573,0.04197228,-0.998274,297.45 +-0.9989448,0.01137894,0.04449668,-183.1531,0.01319183,0.9990858,0.04066278,-2.438103,-0.0439933,0.04120686,-0.9981816,296.5249 +-0.9987075,0.01498792,0.04856636,-183.1228,0.01709077,0.9989212,0.04317644,-2.411142,-0.04786685,0.04395066,-0.9978863,295.6017 +-0.9984145,0.01939432,0.05284353,-183.0905,0.02167306,0.9988444,0.04289606,-2.384435,-0.05195052,0.04397332,-0.997681,294.6849 +-0.9982328,0.01602972,0.05722203,-183.0403,0.01835986,0.9990136,0.04043021,-2.355767,-0.0565175,0.04140934,-0.9975424,293.7625 +-0.9981255,0.01022608,0.06034023,-182.982,0.01252178,0.9992072,0.03779125,-2.328667,-0.05990593,0.03847597,-0.9974622,292.8436 +-0.9980013,0.00970687,0.06244442,-182.9273,0.01234232,0.9990431,0.04195821,-2.297451,-0.06197738,0.04264505,-0.997166,291.9083 +-0.9979644,0.009687798,0.06303416,-182.8746,0.01264318,0.998831,0.04665659,-2.267893,-0.06250846,0.04735856,-0.9969201,290.973 +-0.9979881,0.006695272,0.06304701,-182.8171,0.009879059,0.9986841,0.05032295,-2.238052,-0.06262712,0.05084454,-0.996741,290.0341 +-0.997884,0.009969184,0.06425098,-182.7666,0.01291541,0.9988761,0.04560386,-2.207091,-0.06372413,0.04633718,-0.9968912,289.0992 +-0.9977281,0.01711478,0.0651603,-182.7218,0.01952573,0.9991412,0.03654488,-2.176152,-0.06447888,0.03773415,-0.9972053,288.1653 +-0.9976307,0.02047487,0.06567957,-182.6726,0.02266837,0.9992039,0.0328272,-2.147003,-0.06495515,0.03423827,-0.9973006,287.222 +-0.9977185,0.01804456,0.06505557,-182.6138,0.02053092,0.9990761,0.03775513,-2.123569,-0.06431419,0.03900463,-0.9971671,286.2673 +-0.9977665,0.01524609,0.06503609,-182.5528,0.01824805,0.9987832,0.04581673,-2.097813,-0.06425842,0.04690117,-0.9968305,285.3066 +-0.9978139,0.01408128,0.06457023,-182.4948,0.01728231,0.9986351,0.0492868,-2.065947,-0.06378808,0.05029496,-0.9966952,284.3463 +-0.9978981,0.01599587,0.0627983,-182.4419,0.01910188,0.9986075,0.04917532,-2.031942,-0.06192424,0.05027151,-0.996814,283.3856 +-0.9980483,0.01745674,0.05995739,-182.3929,0.02020818,0.9987556,0.04559427,-1.996433,-0.05908685,0.04671691,-0.997159,282.4284 +-0.9981824,0.01969323,0.05695762,-182.3488,0.02215084,0.9988362,0.0428434,-1.963862,-0.0560476,0.04402717,-0.9974568,281.4646 +-0.9983191,0.02046627,0.05422425,-182.3004,0.02290008,0.9987402,0.04464966,-1.925579,-0.05324213,0.04581634,-0.99753,280.4906 +-0.9984121,0.0232506,0.05130993,-182.2509,0.02584629,0.9983886,0.05051854,-1.885551,-0.05005266,0.05176449,-0.9974042,279.5142 +-0.9985371,0.02295253,0.04895808,-182.2003,0.02564514,0.9981515,0.05509824,-1.841502,-0.04760293,0.05627316,-0.9972799,278.5289 +-0.9986328,0.02259906,0.04713739,-182.1499,0.025107,0.9982623,0.05330939,-1.800893,-0.04585073,0.05441997,-0.9974648,277.557 +-0.9986745,0.02006255,0.04740023,-182.0977,0.02248367,0.9984399,0.05110954,-1.766355,-0.04630089,0.05210752,-0.9975675,276.5833 +-0.9985658,0.02323133,0.0482354,-182.0509,0.02559945,0.9984671,0.04907194,-1.732915,-0.04702145,0.05023636,-0.9976298,275.6086 +-0.9984366,0.02448874,0.05024697,-182.0014,0.02703532,0.9983508,0.05064366,-1.699389,-0.04892389,0.05192291,-0.9974519,274.6278 +-0.9983341,0.02274934,0.05302463,-181.9452,0.02543443,0.9983988,0.05052612,-1.6645,-0.05179029,0.05179059,-0.9973141,273.6526 +-0.9981534,0.02108874,0.0569663,-181.8851,0.02419256,0.9982284,0.0543566,-1.628436,-0.05571907,0.05563438,-0.9968952,272.6701 +-0.9979778,0.02079746,0.06006579,-181.8235,0.02421887,0.9980914,0.05680632,-1.591806,-0.05876972,0.05814616,-0.9965767,271.689 +-0.9979101,0.02042248,0.06130634,-181.7618,0.02383613,0.9981754,0.05547675,-1.554957,-0.06006151,0.0568221,-0.996576,270.7124 +-0.9978628,0.02091333,0.06190728,-181.6986,0.02393974,0.9985336,0.0485549,-1.518421,-0.06080105,0.04993317,-0.9969001,269.743 +-0.9977658,0.02223882,0.06299947,-181.6385,0.02495187,0.9987801,0.04261014,-1.484133,-0.06197501,0.04408688,-0.9971035,268.7678 +-0.9976767,0.0191225,0.06538792,-181.5688,0.02190007,0.9988763,0.04202873,-1.45015,-0.06451075,0.04336308,-0.9969744,267.7765 +-0.9975603,0.01997411,0.06689226,-181.5024,0.02313895,0.9986326,0.04687673,-1.413471,-0.06586447,0.04831017,-0.9966584,266.7855 +-0.9975459,0.01325752,0.06874939,-181.4246,0.01694154,0.9984357,0.053283,-1.374923,-0.06793544,0.05431695,-0.99621,265.7878 +-0.9974531,0.01020604,0.07059136,-181.3532,0.01391147,0.9985397,0.05220025,-1.335469,-0.06995551,0.05304933,-0.9961385,264.8055 +-0.9972573,0.01346563,0.07277754,-181.2862,0.01681368,0.9988188,0.04558866,-1.295565,-0.07207769,0.04668728,-0.9963057,263.8162 +-0.9970797,0.01637935,0.07459169,-181.2198,0.01886034,0.9992879,0.0326788,-1.264596,-0.07400331,0.03399018,-0.9966785,262.8382 +-0.9968337,0.0203156,0.07687648,-181.1524,0.02203389,0.9995245,0.02156928,-1.242004,-0.07640173,0.02319487,-0.9968072,261.8529 +-0.9965898,0.02407933,0.07892418,-181.0843,0.02585502,0.9994333,0.02155424,-1.224198,-0.07836044,0.02352132,-0.9966475,260.859 +-0.9964449,0.02442469,0.08062938,-181.0093,0.02715063,0.9990902,0.0328865,-1.208518,-0.07975278,0.03495872,-0.9962014,259.8449 +-0.9964428,0.02200869,0.08134822,-180.9292,0.02572248,0.9986607,0.04489033,-1.182226,-0.08025129,0.04682311,-0.9956743,258.832 +-0.9964881,0.0205022,0.08118572,-180.8483,0.02480883,0.998318,0.0523982,-1.149494,-0.07997489,0.05422829,-0.9953207,257.8248 +-0.996579,0.01950259,0.08031188,-180.7704,0.02375694,0.9983455,0.05236253,-1.114211,-0.07915779,0.05409136,-0.9953934,256.828 +-0.996684,0.02019145,0.07882554,-180.6952,0.02394794,0.9986075,0.04700483,-1.078985,-0.07776668,0.04873666,-0.9957796,255.8384 +-0.9967383,0.02420343,0.07698721,-180.6275,0.02749534,0.9987396,0.04199042,-1.046652,-0.07587386,0.04397024,-0.9961474,254.8491 +-0.9968376,0.02579074,0.07516397,-180.561,0.02887392,0.9987733,0.04022523,-1.019212,-0.07403432,0.04226829,-0.9963595,253.8604 +-0.9968944,0.02916869,0.07314901,-180.4967,0.03187311,0.9988405,0.03608033,-0.9941325,-0.07201177,0.03829976,-0.9966681,252.875 +-0.9969335,0.03201212,0.07140657,-180.4354,0.03461487,0.9987695,0.03551471,-0.9705894,-0.0701818,0.03787753,-0.9968148,251.8909 +-0.9970181,0.03281358,0.06984412,-180.3727,0.03555275,0.9986319,0.0383431,-0.94663,-0.06849039,0.04071191,-0.9968207,250.9082 +-0.9971064,0.03318144,0.06839496,-180.312,0.03615929,0.9984303,0.0427706,-0.9223052,-0.06686841,0.04511994,-0.996741,249.9236 +-0.9972231,0.03274316,0.06688805,-180.2514,0.03571254,0.9984066,0.04369057,-0.8975132,-0.0653509,0.04595798,-0.9968034,248.9474 +-0.9973897,0.0313077,0.06506686,-180.191,0.0341088,0.9985186,0.04239394,-0.8712488,-0.06364321,0.04450262,-0.9969799,247.9795 +-0.9975282,0.02965914,0.06370145,-180.1307,0.03230687,0.998639,0.04094469,-0.8440752,-0.06240037,0.04290147,-0.9971287,247.0169 +-0.9975574,0.03098137,0.06260632,-180.0771,0.03364212,0.9985552,0.04190194,-0.8176461,-0.06121768,0.04390579,-0.9971582,246.0599 +-0.997591,0.03167595,0.06171639,-180.0256,0.0344533,0.9984164,0.04446943,-0.7912622,-0.06021004,0.04648863,-0.9971025,245.1048 +-0.9976438,0.0320805,0.06064431,-179.9748,0.03493899,0.9982988,0.04667761,-0.7643523,-0.05904369,0.04868647,-0.9970674,244.1585 +-0.9974256,0.03864042,0.06040897,-179.932,0.04171567,0.9978523,0.05050289,-0.733112,-0.05832777,0.05289286,-0.9968952,243.2222 +-0.997235,0.04269384,0.06082441,-179.89,0.04596165,0.997516,0.05337918,-0.7015608,-0.05839436,0.05602717,-0.9967201,242.3011 +-0.9972952,0.04122611,0.06085018,-179.8396,0.04436305,0.997706,0.05113395,-0.6755522,-0.05860253,0.05369514,-0.9968362,241.3981 +-0.9969309,0.04944306,0.06069838,-179.8009,0.05235558,0.9975044,0.04736878,-0.6438624,-0.05820484,0.05040129,-0.9970315,240.5085 +-0.9965951,0.05564877,0.06084034,-179.7651,0.05848959,0.9972298,0.04595323,-0.6101037,-0.05811456,0.04935528,-0.9970891,239.6292 +-0.9971871,0.04473263,0.06014123,-179.7063,0.04744974,0.9978803,0.04453591,-0.5888265,-0.05802153,0.04726431,-0.9971958,238.7631 +-0.9977473,0.03403087,0.05781342,-179.645,0.03633938,0.9985641,0.03935922,-0.5708078,-0.05639097,0.04137145,-0.9975512,237.9148 +-0.9978335,0.03599311,0.05507191,-179.6034,0.03780243,0.9987672,0.03217209,-0.5482957,-0.05384604,0.03418423,-0.9979639,237.0777 +-0.9981272,0.03504328,0.05014095,-179.5652,0.03686669,0.9986746,0.03591484,-0.5273764,-0.04881591,0.03769611,-0.9980961,236.2422 +-0.9985646,0.02791231,0.04571292,-179.5221,0.0298632,0.9986473,0.0425649,-0.5092179,-0.04446299,0.04386893,-0.9980473,235.4177 +-0.9988258,0.02471756,0.04166671,-179.4866,0.02663554,0.9985806,0.04612259,-0.4890252,-0.04046753,0.04717824,-0.9980664,234.6106 +-0.9989512,0.02466021,0.03858079,-179.4618,0.02647848,0.9985273,0.04735008,-0.4651856,-0.0373563,0.04832197,-0.9981329,233.8196 +-0.999013,0.02674164,0.03546875,-179.4435,0.02841845,0.9984599,0.04764572,-0.4403554,-0.03414,0.04860665,-0.9982343,233.0489 +-0.9991033,0.02761392,0.03209627,-179.4262,0.02914539,0.998409,0.04826883,-0.4184439,-0.03071231,0.049161,-0.9983185,232.2935 +-0.9992291,0.02688436,0.02861148,-179.41,0.02826954,0.9983906,0.04916359,-0.396176,-0.0272437,0.04993451,-0.9983808,231.5589 +-0.9993375,0.02635335,0.02510282,-179.3952,0.0275698,0.9983978,0.04941269,-0.3719122,-0.02376041,0.05007202,-0.9984629,230.8454 +-0.9993958,0.02688265,0.02203485,-179.3848,0.02794833,0.9983793,0.04957379,-0.3491587,-0.02066646,0.05015966,-0.9985273,230.153 +-0.9993643,0.02925682,0.0203768,-179.3801,0.03024438,0.9982926,0.04997207,-0.3282832,-0.01887999,0.05055657,-0.9985427,229.4817 +-0.9992959,0.03197517,0.01963204,-179.377,0.0329225,0.9982067,0.04999374,-0.3078274,-0.01799827,0.05060487,-0.9985565,228.8304 +-0.9992307,0.03397066,0.01959563,-179.3729,0.03488981,0.9982066,0.04864478,-0.2870117,-0.01790799,0.04929104,-0.9986238,228.203 +-0.9991488,0.03566889,0.02072602,-179.3672,0.0366298,0.9981756,0.04799675,-0.2672289,-0.01897621,0.04871507,-0.9986324,227.5967 +-0.9989674,0.03886522,0.02352914,-179.3616,0.03994257,0.9980868,0.04719477,-0.2485969,-0.02164988,0.04808585,-0.9986085,227.0094 +-0.9987144,0.04201232,0.02836476,-179.3532,0.0432923,0.9979969,0.04612985,-0.231539,-0.02636991,0.04729852,-0.9985326,226.4456 +-0.9984593,0.04302391,0.03504259,-179.3378,0.04461946,0.9979398,0.04609908,-0.2153403,-0.03298703,0.04759163,-0.998322,225.9028 +-0.9981595,0.04234893,0.04340903,-179.3138,0.04436803,0.9979252,0.0466559,-0.1992051,-0.04134314,0.048496,-0.9979673,225.3829 +-0.9976709,0.04184389,0.0538699,-179.2845,0.044369,0.9979294,0.04656399,-0.1833889,-0.05180994,0.04884568,-0.9974616,224.8897 +-0.996796,0.04297559,0.06745994,-179.2524,0.04612492,0.9978832,0.04584198,-0.1699047,-0.06534705,0.04880668,-0.9966682,224.4216 +-0.9953559,0.04628344,0.08440667,-179.2118,0.05008996,0.9977947,0.04355054,-0.1577349,-0.08220486,0.04757621,-0.9954792,223.9655 +-0.9932889,0.04737817,0.1055105,-179.1595,0.05186348,0.9978456,0.04017896,-0.1468091,-0.1033796,0.04538145,-0.9936061,223.5204 +-0.9902613,0.04679603,0.1311212,-179.0907,0.05215381,0.9979261,0.03772771,-0.1355985,-0.1290837,0.04419875,-0.9906481,223.0843 +-0.9857494,0.04560044,0.1619224,-178.9955,0.05209185,0.9979906,0.03607087,-0.1229871,-0.1599522,0.04399167,-0.986144,222.6423 +-0.9789343,0.04546082,0.1990502,-178.8929,0.05346045,0.9979565,0.03499788,-0.1104466,-0.1970524,0.04490194,-0.9793641,222.2223 +-0.9696995,0.04495261,0.2401297,-178.7547,0.05471419,0.9979183,0.03413684,-0.1001665,-0.2380953,0.04624097,-0.9701404,221.793 +-0.9579931,0.04207432,0.2836882,-178.6098,0.05364381,0.9980102,0.03313421,-0.09114754,-0.2817296,0.04696045,-0.9583439,221.3982 +-0.9437424,0.03627054,0.3286865,-178.4408,0.04936241,0.9982818,0.03157163,-0.08341023,-0.3269766,0.04602023,-0.9439112,221.0195 +-0.9270163,0.02981371,0.3738342,-178.2271,0.04357582,0.998646,0.02841404,-0.08023768,-0.3724809,0.0426304,-0.9270602,220.632 +-0.9080299,0.02583418,0.418108,-178.0215,0.04009846,0.9988737,0.02536548,-0.07354154,-0.4169818,0.0397981,-0.9080431,220.2778 +-0.8860333,0.02591017,0.4628971,-177.7993,0.04042793,0.9989518,0.02146798,-0.06734474,-0.4618556,0.03773531,-0.886152,219.9124 +-0.8613422,0.02659761,0.5073286,-177.5389,0.04176415,0.9989555,0.01853508,-0.05918083,-0.5063057,0.03715318,-0.8615533,219.5279 +-0.8339021,0.02570727,0.5513135,-177.2817,0.04226276,0.9989559,0.01734504,-0.05065815,-0.550292,0.03776408,-0.8341178,219.1907 +-0.8033851,0.02262152,0.59503,-176.9931,0.04169966,0.9989621,0.01832317,-0.04192789,-0.593998,0.0395331,-0.8034945,218.8687 +-0.7691809,0.0205546,0.6387005,-176.6523,0.04340114,0.998855,0.0201225,-0.03061677,-0.6375556,0.04319817,-0.7691922,218.5186 +-0.7313145,0.02024783,0.6817399,-176.3154,0.04764827,0.9986337,0.02145348,-0.01959867,-0.680374,0.04817296,-0.73128,218.205 +-0.6902558,0.0208627,0.7232647,-175.9537,0.05022648,0.9985546,0.0191307,-0.008613545,-0.7218201,0.0495321,-0.6903059,217.9086 +-0.6461045,0.02114756,0.762956,-175.5342,0.04967208,0.998662,0.01438363,0.001303197,-0.7616309,0.04719092,-0.6462904,217.6015 +-0.5989,0.02034696,0.8005653,-175.108,0.04812566,0.9987848,0.01061777,0.008884163,-0.7993764,0.04488671,-0.5991514,217.3396 +-0.5500654,0.02032669,0.8348742,-174.6464,0.04556859,0.9989449,0.005701999,0.01392669,-0.8338774,0.0411805,-0.5504112,217.0988 +-0.5003583,0.02474633,0.8654648,-174.1229,0.046541,0.998915,-0.001654963,0.01612814,-0.8645666,0.03945151,-0.5009671,216.8298 +-0.4506962,0.02898178,0.8922068,-173.5983,0.04783918,0.9988207,-0.008279107,0.01603695,-0.8913946,0.03895107,-0.4515512,216.6318 +-0.4000276,0.02866854,0.9160546,-173.0375,0.047097,0.9988331,-0.01069259,0.01526395,-0.9152922,0.03886609,-0.400911,216.4235 +-0.3485587,0.02464741,0.9369628,-172.474,0.04444381,0.9989643,-0.009744902,0.01245219,-0.9362326,0.03824552,-0.3492931,216.2595 +-0.2963673,0.02040642,0.954856,-171.8842,0.03855601,0.9992123,-0.009387394,0.01226133,-0.9542954,0.03403331,-0.2969206,216.1281 +-0.2447217,0.01766386,0.9694325,-171.2424,0.03118455,0.9994601,-0.01033883,0.006738538,-0.9690917,0.02770117,-0.2451404,215.9877 +-0.1965382,0.01272851,0.9804136,-170.6006,0.02523249,0.9996502,-0.007920038,-0.0006340179,-0.9801714,0.02318167,-0.1967906,215.8959 +-0.1543085,0.007278431,0.9879959,-169.9389,0.02371097,0.9997121,-0.003661491,-0.007785711,-0.9877381,0.02286133,-0.1544366,215.8249 +-0.1185439,0.007015819,0.9929241,-169.2517,0.02303227,0.9997254,-0.00431409,-0.01961535,-0.9926816,0.02235788,-0.1186729,215.7481 +-0.08853081,0.0117477,0.9960042,-168.5656,0.01983274,0.999753,-0.01002907,-0.03513643,-0.9958759,0.0188656,-0.08874193,215.7053 +-0.0642177,0.01800979,0.9977734,-167.8625,0.01532884,0.999737,-0.01705866,-0.04809223,-0.9978181,0.01419924,-0.06447687,215.6708 +-0.04477947,0.02512077,0.998681,-167.1609,0.01179472,0.9996274,-0.02461573,-0.05831758,-0.9989272,0.01067688,-0.04505908,215.6513 +-0.02908638,0.0253378,0.9992557,-166.4434,0.01122628,0.9996239,-0.02502037,-0.07436568,-0.9995138,0.01049017,-0.02935988,215.6216 +-0.01873924,0.01827257,0.9996574,-165.7179,0.01263633,0.9997574,-0.01803753,-0.09615531,-0.9997445,0.01229399,-0.01896559,215.5997 +-0.01235537,0.01235622,0.9998474,-164.9838,0.01354171,0.999834,-0.01218873,-0.1188212,-0.9998319,0.01338904,-0.01252064,215.5792 +-0.00726048,0.01067545,0.9999167,-164.246,0.01512135,0.9998298,-0.01056473,-0.138904,-0.9998593,0.01504338,-0.00742067,215.5684 +-0.003185157,0.01326303,0.999907,-163.5009,0.01741937,0.999761,-0.01320561,-0.1556038,-0.9998432,0.01737568,-0.003415428,215.5524 +0.0008592044,0.02293189,0.9997367,-162.7532,0.01972934,0.999542,-0.02294439,-0.1745427,-0.9998049,0.01974385,0.0004063811,215.5422 +0.003731294,0.03081124,0.9995183,-161.9912,0.01921589,0.9993384,-0.03087744,-0.1972969,-0.9998084,0.01932183,0.003136761,215.5327 +0.005863838,0.03318676,0.999432,-161.2119,0.02102289,0.9992241,-0.03330322,-0.2213943,-0.9997618,0.02120623,0.005161607,215.5241 +0.008073372,0.02852556,0.9995605,-160.4196,0.0232848,0.9993166,-0.02870668,-0.2475348,-0.9996962,0.02350632,0.007403643,215.5174 +0.01008213,0.01819469,0.9997837,-159.6089,0.0238938,0.9995446,-0.0184313,-0.2795917,-0.9996636,0.02407444,0.009642803,215.5124 +0.01219213,0.01489025,0.9998148,-158.7983,0.02615664,0.9995422,-0.01520516,-0.3074569,-0.9995835,0.02633718,0.01179707,215.5063 +0.01451325,0.01561239,0.9997728,-157.9869,0.02950628,0.9994359,-0.01603547,-0.3292413,-0.9994592,0.02973229,0.0140444,215.4944 +0.01695341,0.01836247,0.9996877,-157.1687,0.03207913,0.9993066,-0.0188995,-0.3490628,-0.9993415,0.03238951,0.0163526,215.4909 +0.01889659,0.0199229,0.999623,-156.3334,0.03362382,0.9992232,-0.02055056,-0.3744766,-0.9992559,0.03399947,0.01821203,215.4749 +0.02043865,0.0202884,0.9995853,-155.4939,0.03465424,0.9991789,-0.02098874,-0.3999312,-0.9991903,0.03506884,0.01971879,215.4766 +0.02199562,0.02159573,0.9995248,-154.6509,0.03134288,0.9992603,-0.02227975,-0.4260836,-0.9992666,0.03181804,0.02130248,215.4909 +0.02294006,0.02251953,0.9994832,-153.798,0.03022963,0.9992735,-0.02320864,-0.4491774,-0.9992797,0.03074641,0.02224263,215.5215 +0.02446594,0.02227928,0.9994524,-152.9246,0.03099251,0.9992542,-0.02303354,-0.4765393,-0.9992201,0.03153906,0.0237572,215.5288 +0.02540903,0.01744622,0.9995249,-152.043,0.03392248,0.9992568,-0.01830389,-0.5017483,-0.9991014,0.03437144,0.02479833,215.5373 +0.02646854,0.01295087,0.9995658,-151.1495,0.03538576,0.9992773,-0.01388415,-0.5295159,-0.9990231,0.03573788,0.02599113,215.545 +0.02835642,0.01228304,0.9995224,-150.2503,0.03596512,0.9992645,-0.0133002,-0.5601175,-0.9989506,0.03632508,0.0278938,215.5562 +0.03147828,0.01497887,0.9993922,-149.3558,0.04054759,0.9990454,-0.01625082,-0.5823925,-0.9986816,0.04103448,0.03084087,215.5771 +0.03613727,0.0165291,0.9992102,-148.4648,0.04473946,0.9988339,-0.01814092,-0.6008344,-0.9983448,0.04535968,0.03535562,215.6092 +0.04041012,0.01371443,0.9990891,-147.5671,0.04564691,0.9988365,-0.01555725,-0.6198851,-0.9981399,0.04623399,0.03973708,215.6569 +0.04491279,0.01033987,0.9989374,-146.6647,0.04472494,0.998923,-0.01235058,-0.6483958,-0.9979892,0.0452321,0.04440196,215.6906 +0.04976907,0.009341506,0.9987171,-145.7612,0.04437813,0.998948,-0.01155517,-0.6795057,-0.9977743,0.04489627,0.04930215,215.7217 +0.05504587,0.009230824,0.9984412,-144.8559,0.04651039,0.9988481,-0.01179879,-0.7108617,-0.9973999,0.04708736,0.05455313,215.7519 +0.06038061,0.005292635,0.9981614,-143.9508,0.04518336,0.9989464,-0.008030026,-0.7389006,-0.9971522,0.04558513,0.06007785,215.7977 +0.06523002,0.001276598,0.9978695,-143.0473,0.0454714,0.9989566,-0.004250427,-0.7706459,-0.9968336,0.04565177,0.06510391,215.8453 +0.07038445,0.001445692,0.9975189,-142.1458,0.0443384,0.9990061,-0.004576348,-0.8040035,-0.996534,0.04455048,0.07025039,215.8968 +0.07518003,0.01083875,0.9971111,-141.2479,0.0394954,0.9991239,-0.0138385,-0.8410487,-0.9963875,0.04042167,0.07468608,215.9462 +0.07997385,0.01697851,0.9966524,-140.3543,0.03694618,0.9991174,-0.01998516,-0.8685023,-0.996112,0.03842078,0.07927597,216.0153 +0.08455566,0.01490481,0.9963073,-139.4551,0.03748894,0.9991326,-0.01812873,-0.8905654,-0.9957132,0.03888338,0.08392355,216.0874 +0.08833661,0.00993368,0.9960412,-138.5557,0.03406495,0.9993352,-0.01298768,-0.9204463,-0.995508,0.03507737,0.08793949,216.1647 +0.09023938,0.005326776,0.9959059,-137.6571,0.02809989,0.9995739,-0.007892541,-0.9524096,-0.9955236,0.02869705,0.09005124,216.2474 +0.09116475,0.00638773,0.9958154,-136.7684,0.02662971,0.9996062,-0.008849944,-0.9745783,-0.9954797,0.02732506,0.09095874,216.3308 +0.09108455,0.008534801,0.9958066,-135.8805,0.0254784,0.9996159,-0.01089792,-0.9967056,-0.9955171,0.02636418,0.09083211,216.4147 +0.09024122,0.009460066,0.995875,-134.9897,0.02256011,0.9996789,-0.01154049,-1.024534,-0.9956643,0.02350847,0.08999881,216.4855 +0.08939503,0.009507446,0.9959509,-134.1037,0.01865576,0.999763,-0.01121836,-1.047336,-0.9958215,0.01958308,0.08919647,216.5623 +0.08827417,0.01144954,0.9960304,-133.2288,0.01100799,0.9998616,-0.01246918,-1.0716,-0.9960353,0.01206499,0.08813592,216.6465 +0.08633051,0.01330099,0.9961778,-132.3612,0.008337092,0.9998662,-0.01407275,-1.094509,-0.9962316,0.009520128,0.08620806,216.7257 +0.08483742,0.01871565,0.996219,-131.5054,0.007252038,0.9997855,-0.01940024,-1.117805,-0.9963684,0.008870479,0.08468349,216.803 +0.0824228,0.02001024,0.9963966,-130.6583,0.001937441,0.9997933,-0.02023872,-1.143989,-0.9965955,0.003598587,0.08236698,216.8839 +0.08108829,0.02048022,0.9964965,-129.8199,0.001424722,0.9997854,-0.02066376,-1.168113,-0.9967059,0.003095316,0.08104171,216.9542 +0.07907886,0.01820206,0.9967022,-128.9928,0.003361164,0.9998227,-0.01852573,-1.194745,-0.9968627,0.004815068,0.07900366,217.0182 +0.07633885,0.01626123,0.9969494,-128.1705,0.002200203,0.9998618,-0.01647722,-1.23286,-0.9970795,0.003451338,0.07629252,217.0757 +0.0734223,0.01586447,0.9971748,-127.3692,-0.0005401203,0.9998739,-0.01586765,-1.27213,-0.9973008,0.0006264412,0.07342161,217.1305 +0.07050436,0.01591698,0.9973845,-126.5849,-0.004223094,0.9998685,-0.0156581,-1.306515,-0.9975025,-0.003108086,0.07056231,217.191 +0.06662052,0.01555635,0.9976571,-125.8229,-0.006457742,0.9998642,-0.01515955,-1.342639,-0.9977574,-0.005432678,0.06671193,217.25 +0.06171212,0.01568753,0.9979707,-125.076,-0.005548005,0.9998664,-0.01537426,-1.381566,-0.9980785,-0.004587971,0.06179091,217.289 +0.05572867,0.01809429,0.998282,-124.3571,-0.006040366,0.9998236,-0.01778504,-1.421077,-0.9984276,-0.005038854,0.05582813,217.3301 +0.0489649,0.02049042,0.9985903,-123.6574,-0.005455572,0.9997801,-0.02024733,-1.453254,-0.9987856,-0.004456476,0.04906591,217.3657 +0.04310026,0.02075216,0.9988552,-122.9783,-0.003520074,0.9997812,-0.02061952,-1.493099,-0.9990645,-0.002627341,0.04316388,217.3942 +0.03776879,0.01981509,0.9990901,-122.3261,-0.001107404,0.9998036,-0.01978739,-1.535093,-0.9992858,-0.0003590538,0.03778332,217.4189 +0.03210016,0.02033701,0.9992778,-121.6962,0.002442903,0.9997884,-0.02042588,-1.572983,-0.9994816,0.003096808,0.03204368,217.4395 +0.02605659,0.02176843,0.9994235,-121.0747,0.005880197,0.9997422,-0.02192868,-1.610957,-0.9996431,0.006448189,0.02592187,217.452 +0.02032068,0.02379152,0.9995104,-120.4635,0.005416513,0.9996995,-0.02390615,-1.644178,-0.9997788,0.005899646,0.0201857,217.4662 +0.01409916,0.02666648,0.999545,-119.8581,0.008048893,0.9996089,-0.02678173,-1.678712,-0.9998682,0.008422825,0.01387901,217.4794 +0.008621016,0.02835402,0.9995608,-119.2538,0.00850543,0.9995597,-0.02842735,-1.71198,-0.9999266,0.008746762,0.008376057,217.487 +0.003515091,0.02853458,0.9995867,-118.6536,0.006907448,0.9995682,-0.02855836,-1.74273,-0.9999699,0.007004973,0.003316473,217.4972 +-0.001628374,0.02984565,0.9995532,-118.0529,0.005409591,0.9995401,-0.02983646,-1.765239,-0.999984,0.005358585,-0.001789077,217.5009 +-0.005179471,0.02569942,0.9996563,-117.4528,0.00671555,0.999648,-0.02566442,-1.789865,-0.999964,0.006580309,-0.005350232,217.5015 +-0.006471473,0.01843254,0.9998092,-116.8496,0.00914475,0.9997894,-0.01837299,-1.815262,-0.9999372,0.0090241,-0.006638669,217.4959 +-0.007568888,0.01213899,0.9998977,-116.2506,0.0105329,0.9998718,-0.01205895,-1.8409,-0.9999158,0.01044054,-0.007695775,217.4919 +-0.007038889,0.008675549,0.9999376,-115.661,0.01094106,0.9999032,-0.008598238,-1.856427,-0.9999153,0.01087985,-0.007133125,217.5002 +-0.004737082,0.002515654,0.9999856,-115.0678,0.007560565,0.9999683,-0.0024798,-1.864826,-0.9999602,0.007548704,-0.00475595,217.514 +-0.002149378,-0.002889002,0.9999935,-114.4769,0.006545977,0.9999743,0.002903012,-1.869443,-0.9999762,0.00655217,-0.00213041,217.5287 +0.003025416,-0.001089564,0.9999949,-113.8982,0.01168629,0.9999311,0.001054134,-1.885694,-0.9999271,0.01168303,0.003037942,217.5304 +0.01111706,0.001428958,0.9999372,-113.324,0.01999914,0.9997986,-0.00165111,-1.905071,-0.9997381,0.02001623,0.01108624,217.5353 +0.02266651,0.006675049,0.9997208,-112.7521,0.0245037,0.9996736,-0.007230308,-1.91705,-0.9994427,0.02466073,0.02249555,217.5543 +0.03911843,0.01623123,0.9991028,-112.188,0.03053812,0.9993816,-0.01743144,-1.923584,-0.9987678,0.0311926,0.03859857,217.5872 +0.05957786,0.01941745,0.9980348,-111.616,0.03533581,0.9991431,-0.0215484,-1.935585,-0.997598,0.03655017,0.05884067,217.6231 +0.08199681,0.0159289,0.9965053,-111.0473,0.03694496,0.9991364,-0.01901095,-1.945084,-0.9959475,0.03837467,0.0813375,217.6962 +0.105673,0.01716449,0.9942528,-110.4838,0.03735413,0.9990768,-0.02121791,-1.964158,-0.9936991,0.0393816,0.1049343,217.7775 +0.1312508,0.0209986,0.9911268,-109.9277,0.0372845,0.9989637,-0.02610207,-1.97678,-0.9906478,0.04037957,0.1303319,217.8753 +0.1610889,0.02084459,0.9867198,-109.3774,0.03592609,0.9989905,-0.026969,-1.987256,-0.9862858,0.03979338,0.1601774,218.0015 +0.1942726,0.01835305,0.9807759,-108.8327,0.03626557,0.9990071,-0.02587771,-1.995466,-0.9802769,0.04059572,0.1934141,218.1458 +0.2298126,0.01656791,0.9730939,-108.2909,0.03397316,0.9991091,-0.02503419,-2.004646,-0.9726417,0.03881223,0.229045,218.3013 +0.2679109,0.01205567,0.9633683,-107.764,0.02793854,0.999404,-0.02027628,-2.019178,-0.9630385,0.03234733,0.2674143,218.4985 +0.3096707,0.007249328,0.9508163,-107.2563,0.02372564,0.9996007,-0.01534847,-2.021587,-0.9505478,0.02731169,0.309375,218.7282 +0.3536117,0.00742192,0.9353629,-106.7495,0.0224119,0.9996142,-0.01640451,-2.025929,-0.9351238,0.02676408,0.3533089,218.9454 +0.3986985,0.007783227,0.9170491,-106.2729,0.02113521,0.9996204,-0.01767283,-2.029648,-0.9168385,0.02642815,0.3983826,219.2153 +0.4452357,0.007557993,0.8953815,-105.7926,0.01968046,0.9996402,-0.01822432,-2.038583,-0.8951971,0.02573563,0.4449267,219.4762 +0.4922363,0.009732946,0.8704072,-105.3492,0.01766807,0.9996198,-0.02116953,-2.045845,-0.8702823,0.02579882,0.4918771,219.7938 +0.5386538,0.01318341,0.8424241,-104.9255,0.01787414,0.9994737,-0.02707003,-2.060392,-0.8423375,0.02963898,0.5381347,220.1308 +0.583928,0.01797787,0.8116064,-104.5035,0.01688669,0.9992694,-0.03428429,-2.078284,-0.8116298,0.03372489,0.5831978,220.4538 +0.6280457,0.02106081,0.7778914,-104.1155,0.01379795,0.9991751,-0.03819194,-2.105153,-0.7780541,0.03471958,0.6272371,220.8349 +0.6699816,0.02135417,0.7420706,-103.7453,0.01414829,0.9990374,-0.04152262,-2.129794,-0.7422429,0.03831841,0.6690345,221.2334 +0.7095755,0.0150544,0.7044687,-103.3709,0.01959886,0.9989632,-0.04108865,-2.158418,-0.7043568,0.04296227,0.7085447,221.616 +0.7461192,0.007924154,0.6657653,-103.036,0.02160188,0.9991146,-0.03610088,-2.189144,-0.6654619,0.04131733,0.7452873,222.0512 +0.7795734,0.003816531,0.6262993,-102.7308,0.02121189,0.9992468,-0.03249226,-2.232675,-0.6259516,0.03861508,0.7789052,222.5002 +0.8104792,0.004235603,0.5857521,-102.4376,0.0176274,0.9993446,-0.03161657,-2.273704,-0.5855021,0.03594985,0.8098733,222.9327 +0.8391007,0.00236203,0.5439711,-102.1791,0.0191138,0.999245,-0.03382284,-2.315727,-0.5436403,0.03877811,0.838422,223.4001 +0.8652185,0.002006879,0.5013911,-101.9309,0.01863895,0.999172,-0.03616335,-2.34715,-0.5010485,0.04063459,0.8644647,223.8528 +0.8882291,0.003490833,0.4593876,-101.7237,0.01510501,0.9992085,-0.03679852,-2.383609,-0.4591524,0.03962456,0.8874733,224.3458 +0.9085359,0.007092991,0.4177468,-101.5432,0.008964448,0.9992948,-0.03646351,-2.415078,-0.4177108,0.03687327,0.9078315,224.8487 +0.9263823,0.0123038,0.3763835,-101.3752,0.0006735876,0.9994104,-0.03432813,-2.444928,-0.3765839,0.03205449,0.9258277,225.3389 +0.9409404,0.01635955,0.3381769,-101.2366,-0.005611613,0.9994483,-0.03273532,-2.472231,-0.3385258,0.02890427,0.940513,225.8512 +0.9521679,0.02134674,0.3048288,-101.1157,-0.0131229,0.9994932,-0.02900225,-2.49977,-0.3052934,0.02361477,0.9519654,226.3838 +0.9603605,0.02881652,0.2772676,-101.0069,-0.02235077,0.9994002,-0.02645255,-2.526393,-0.2778636,0.01920684,0.9604285,226.9101 +0.9657553,0.04070631,0.2562416,-100.9157,-0.03565333,0.9990678,-0.02433635,-2.553482,-0.2569933,0.01436709,0.9663063,227.4386 +0.9692204,0.05063334,0.2409319,-100.8306,-0.04662699,0.9986633,-0.02230435,-2.570641,-0.2417392,0.0103839,0.9702856,227.9686 +0.9717072,0.05214659,0.2303605,-100.7342,-0.04831,0.9985841,-0.02226766,-2.585013,-0.2311955,0.01050893,0.9728505,228.4882 +0.9734555,0.05283127,0.2226958,-100.6347,-0.04872709,0.9985264,-0.02388812,-2.600567,-0.2236296,0.0124027,0.9745952,229.003 +0.9744748,0.05195808,0.2184017,-100.5311,-0.04756034,0.9985466,-0.02534884,-2.625163,-0.2194013,0.01431455,0.9755296,229.5011 +0.9743505,0.05548929,0.2180874,-100.4466,-0.05122549,0.9983701,-0.02516092,-2.640569,-0.2191281,0.01334392,0.9756048,230.0159 +0.9737176,0.05964601,0.2198101,-100.3557,-0.05532738,0.998136,-0.02575674,-2.656963,-0.2209367,0.01291827,0.9752025,230.5209 +0.9730356,0.0558926,0.2237807,-100.2521,-0.05085892,0.9983076,-0.02819939,-2.672563,-0.2249781,0.01605776,0.9742314,231.0179 +0.9710135,0.05148934,0.233413,-100.1357,-0.04553357,0.9984867,-0.03083685,-2.68848,-0.2346475,0.01931486,0.9718886,231.5079 +0.9672846,0.04711881,0.2492797,-100.0122,-0.04065866,0.9986919,-0.03100408,-2.703396,-0.2504145,0.01985439,0.9679351,231.9873 +0.9619036,0.04635601,0.2694303,-99.88399,-0.03977828,0.9987633,-0.02982522,-2.718972,-0.2704797,0.01797151,0.9625579,232.4635 +0.9538843,0.04783415,0.2963387,-99.75006,-0.04175051,0.9987678,-0.02682763,-2.728487,-0.2972568,0.01321816,0.954706,232.9331 +0.943051,0.04628199,0.3294129,-99.59956,-0.0417725,0.9989115,-0.02075819,-2.739823,-0.330015,0.005815627,0.9439577,233.3861 +0.93002,0.03943292,0.3653874,-99.42555,-0.03755768,0.9992195,-0.01224113,-2.754618,-0.3655849,-0.00233861,0.930775,233.8241 +0.9147938,0.01963845,0.4034436,-99.20992,-0.01710494,0.9998048,-0.009882732,-2.762877,-0.4035589,0.00213978,0.9149511,234.1986 +0.8956362,0.00794624,0.4447165,-98.99629,-0.001542821,0.9998899,-0.01475898,-2.777882,-0.4447848,0.01253255,0.8955497,234.5599 +0.8716996,0.007796969,0.4899787,-98.77588,0.004330226,0.9997118,-0.023612,-2.79773,-0.4900216,0.02270429,0.8714145,234.8729 +0.8436311,0.01892265,0.5365898,-98.56661,-0.001449129,0.9994554,-0.0329671,-2.811248,-0.5369214,0.02703448,0.8431989,235.2085 +0.8108383,0.03265384,0.5843586,-98.34855,-0.0126884,0.9991884,-0.03822843,-2.820092,-0.5851326,0.02358249,0.8105946,235.5401 +0.7736779,0.03805772,0.6324351,-98.0861,-0.01856547,0.9991274,-0.03741226,-2.826192,-0.6333071,0.01720357,0.7737093,235.8063 +0.7344721,0.02984541,0.6779823,-97.80747,-0.01146224,0.9994355,-0.0315788,-2.837866,-0.678542,0.01542254,0.7343996,236.0797 +0.6932109,0.01608133,0.7205554,-97.49142,0.002527732,0.9996906,-0.02474288,-2.846074,-0.7207303,0.0189734,0.6929558,236.2814 +0.6493595,0.01094814,0.7604028,-97.17427,0.009988631,0.9996873,-0.02292329,-2.873917,-0.760416,0.02248083,0.649047,236.4687 +0.6032829,0.0107093,0.7974554,-96.87881,0.01427774,0.9996045,-0.02422528,-2.878181,-0.7973994,0.02600055,0.6028914,236.6544 +0.5559095,0.01195141,0.8311569,-96.56356,0.01846361,0.9994724,-0.02672083,-2.881999,-0.8310377,0.03020051,0.5553955,236.7815 +0.5092309,0.01366507,0.8605215,-96.25415,0.02157604,0.9993569,-0.02863784,-2.886378,-0.8603594,0.03314991,0.5086085,236.9311 +0.4655531,0.01536932,0.8848865,-95.93153,0.02154589,0.999356,-0.02869315,-2.889154,-0.8847576,0.03242385,0.4649221,237.0532 +0.4248062,0.01627647,0.905138,-95.61348,0.01885866,0.9994623,-0.02682353,-2.892253,-0.9050878,0.02846448,0.4242708,237.2126 +0.3842488,0.01725238,0.9230684,-95.28315,0.01582146,0.9995555,-0.025268,-2.893774,-0.923094,0.02431349,0.383805,237.3588 +0.3431131,0.01670352,0.9391456,-94.93043,0.01613095,0.9995896,-0.02367195,-2.895213,-0.9391555,0.02327146,0.3427028,237.4685 +0.3038695,0.01257638,0.9526307,-94.56472,0.01977829,0.9996141,-0.01950552,-2.904768,-0.9525083,0.02476853,0.3035035,237.5781 +0.2673359,0.008349815,0.9635672,-94.18508,0.02410965,0.9995914,-0.01535106,-2.914728,-0.9633017,0.02733515,0.2670254,237.6748 +0.2330441,0.007533274,0.972437,-93.78494,0.02767581,0.9995136,-0.01437553,-2.924129,-0.9720722,0.03026311,0.2327222,237.7469 +0.2036372,0.009433099,0.979001,-93.37827,0.030736,0.9993991,-0.01602289,-2.929392,-0.9785638,0.03335343,0.2032248,237.8266 +0.180307,0.01292666,0.9835255,-92.95531,0.03277506,0.9992794,-0.01914228,-2.934146,-0.9830641,0.03568658,0.1797533,237.8988 +0.1611995,0.01757504,0.9867654,-92.51885,0.03218672,0.9992159,-0.02305487,-2.940151,-0.9863968,0.03547716,0.1605074,237.9685 +0.1449885,0.02184479,0.9891922,-92.0658,0.03094499,0.999167,-0.02660077,-2.94732,-0.9889493,0.03446734,0.1441917,238.0376 +0.1298539,0.02601172,0.9911919,-91.59685,0.02954899,0.9991103,-0.03009067,-2.957697,-0.9910927,0.0331961,0.1289698,238.1027 +0.1145835,0.02898269,0.9929908,-91.10748,0.02919914,0.9990442,-0.03252874,-2.970966,-0.9929844,0.03272172,0.1136277,238.1588 +0.09991095,0.03003109,0.9945431,-90.60052,0.03199429,0.9989306,-0.0333777,-2.985924,-0.9944818,0.03515449,0.09884328,238.2071 +0.0884914,0.02928028,0.9956465,-90.07469,0.03633721,0.9988076,-0.03260284,-3.000677,-0.9954139,0.03906407,0.08732192,238.2483 +0.0807732,0.02799201,0.9963394,-89.53204,0.03987548,0.9987145,-0.03129145,-3.015435,-0.9959345,0.04225701,0.07955317,238.2882 +0.07730756,0.025407,0.9966835,-88.96618,0.04244549,0.998685,-0.0287503,-3.031384,-0.9961033,0.04452733,0.07612749,238.3336 +0.0768665,0.02248064,0.996788,-88.38745,0.04319333,0.9987321,-0.02585531,-3.040495,-0.9961053,0.04504199,0.07579802,238.3917 +0.07641245,0.01901453,0.996895,-87.76457,0.04261013,0.9988425,-0.02231777,-3.050019,-0.9961654,0.04418316,0.07551379,238.447 +0.0745106,0.01839796,0.9970505,-87.13484,0.04230253,0.9988715,-0.02159288,-3.068087,-0.9963225,0.04378665,0.07364823,238.488 +0.07304468,0.01818219,0.9971629,-86.48629,0.04140328,0.9989166,-0.02124707,-3.071277,-0.9964688,0.04283779,0.07221273,238.5401 +0.07181538,0.01845204,0.9972473,-85.81893,0.04085014,0.9989355,-0.02142505,-3.089892,-0.996581,0.04227633,0.07098516,238.5774 +0.06951287,0.0203727,0.997373,-85.13954,0.04086682,0.998894,-0.02325203,-3.10953,-0.9967436,0.04237577,0.06860342,238.6147 +0.06708308,0.02279986,0.9974869,-84.44349,0.04074323,0.9988424,-0.02557092,-3.131581,-0.9969151,0.0423562,0.06607648,238.6508 +0.06447766,0.02478679,0.9976113,-83.73651,0.03983907,0.9988306,-0.02739197,-3.141617,-0.9971236,0.04151006,0.06341477,238.689 +0.06193632,0.02616636,0.9977371,-82.99599,0.03952459,0.9988078,-0.02864801,-3.164465,-0.9972972,0.04120949,0.06082827,238.7136 +0.05985783,0.02588744,0.9978712,-82.25386,0.03787593,0.9988848,-0.02818575,-3.18894,-0.997488,0.03948243,0.05881056,238.7468 +0.05768897,0.02528313,0.9980144,-81.501,0.03686986,0.9989433,-0.02743788,-3.193174,-0.9976535,0.0383795,0.05669582,238.7936 +0.05556419,0.02359773,0.9981762,-80.73125,0.03730068,0.9989737,-0.02569296,-3.204779,-0.9977581,0.03866025,0.05462695,238.8359 +0.05332103,0.02221788,0.9983303,-79.94014,0.0379823,0.9989838,-0.02426107,-3.188593,-0.9978548,0.0392125,0.05242296,238.8723 +0.05033489,0.02129089,0.9985055,-79.14001,0.03843736,0.9989907,-0.02323888,-3.199573,-0.9979924,0.03954963,0.04946572,238.9059 +0.04754663,0.02035523,0.9986616,-78.3389,0.03883068,0.9989989,-0.02221085,-3.201263,-0.9981139,0.03983475,0.04670862,238.9574 +0.0454186,0.02108926,0.9987454,-77.53959,0.03934008,0.9989638,-0.0228829,-3.204957,-0.9981931,0.04033002,0.04454188,239.0111 +0.04354915,0.02059931,0.9988389,-76.72966,0.04213669,0.9988599,-0.0224369,-3.214693,-0.9981623,0.04306486,0.04263151,239.0557 +0.04228548,0.02055011,0.9988942,-75.91108,0.04355843,0.9987999,-0.0223921,-3.224093,-0.9981556,0.04445711,0.0413396,239.1026 +0.04067563,0.02143888,0.9989424,-75.08744,0.04330933,0.9987923,-0.02319917,-3.239402,-0.9982333,0.04420715,0.039698,239.1507 +0.0392125,0.02325049,0.9989604,-74.25346,0.04279786,0.9987727,-0.02492609,-3.253523,-0.9983139,0.04373077,0.03816931,239.2005 +0.03780913,0.02688977,0.9989232,-73.41834,0.04231995,0.9986979,-0.02848552,-3.272699,-0.9983884,0.04335138,0.03662193,239.247 +0.03618408,0.02876116,0.9989312,-72.57493,0.04050586,0.9987221,-0.03022238,-3.292981,-0.9985239,0.04155613,0.03497284,239.3139 +0.03468729,0.03072415,0.9989259,-71.72614,0.03818142,0.9987569,-0.0320448,-3.314227,-0.9986686,0.03925194,0.03347108,239.3683 +0.0333227,0.03014757,0.9989899,-70.86221,0.03898065,0.9987452,-0.03144044,-3.33866,-0.9986841,0.03998894,0.03210571,239.4117 +0.03311988,0.02821177,0.9990532,-69.99191,0.04009829,0.9987592,-0.02953279,-3.366787,-0.9986466,0.04103844,0.03194754,239.4464 +0.03408332,0.0248059,0.9991111,-69.11572,0.04201732,0.9987725,-0.02623087,-3.390705,-0.9985353,0.042874,0.0329992,239.481 +0.03659189,0.02210989,0.9990857,-68.23645,0.04515142,0.9986977,-0.02375499,-3.412566,-0.9983097,0.04597937,0.03554594,239.513 +0.03987932,0.02059004,0.9989924,-67.35162,0.04873321,0.9985578,-0.02252649,-3.440482,-0.9980153,0.04958243,0.03881838,239.5488 +0.04322326,0.02045162,0.9988561,-66.45635,0.05064293,0.9984603,-0.02263498,-3.487813,-0.997781,0.05156334,0.04212098,239.5791 +0.04660282,0.02112581,0.9986901,-65.56589,0.05146178,0.9983979,-0.02352105,-3.534768,-0.997587,0.0524905,0.04544099,239.621 +0.04970645,0.023695,0.9984828,-64.66636,0.05098192,0.998355,-0.02622996,-3.602912,-0.9974618,0.05220835,0.04841667,239.6577 +0.05247616,0.02580241,0.9982888,-63.77679,0.04984492,0.9983524,-0.02842421,-3.651501,-0.9973774,0.0512512,0.05110358,239.7007 +0.05440976,0.02620447,0.9981748,-62.88482,0.04973583,0.9983436,-0.02891997,-3.696764,-0.9972792,0.05121857,0.05301633,239.754 +0.05575086,0.02684014,0.9980839,-61.99803,0.04724734,0.9984478,-0.02948907,-3.746013,-0.9973261,0.04880083,0.05439619,239.8088 +0.05676508,0.02650235,0.9980358,-61.10321,0.0455696,0.998537,-0.02910752,-3.809388,-0.997347,0.04713237,0.05547433,239.8603 +0.05821248,0.02335632,0.998031,-60.20898,0.04457672,0.9986683,-0.02597128,-3.87313,-0.9973084,0.04600079,0.05709381,239.9033 +0.05993401,0.01452023,0.9980968,-59.31175,0.04058046,0.9990321,-0.01697063,-3.939659,-0.9973771,0.04152033,0.05928676,239.9526 +0.06154102,0.008979593,0.9980642,-58.4305,0.04653743,0.9988462,-0.01185615,-3.980641,-0.997019,0.04717697,0.06105212,240.0059 +0.06394604,0.01213018,0.9978797,-57.56113,0.04725105,0.9987678,-0.01516892,-4.020154,-0.9968341,0.04812084,0.06329408,240.0565 +0.06452445,0.02318654,0.9976467,-56.70195,0.04431491,0.9986772,-0.02607664,-4.060627,-0.9969316,0.04589319,0.06341159,240.1091 +0.06575963,0.03168409,0.9973324,-55.85398,0.04459527,0.9984037,-0.03465855,-4.101583,-0.9968384,0.04675543,0.0642417,240.16 +0.06674302,0.03215427,0.997252,-55.00292,0.04769386,0.9982353,-0.03537799,-4.143154,-0.9966296,0.04992402,0.06509167,240.2089 +0.06634772,0.02574414,0.9974644,-54.15113,0.05022662,0.9983136,-0.02910696,-4.172782,-0.9965316,0.05203044,0.06494279,240.2676 +0.06558209,0.01700928,0.9977022,-53.30353,0.04852038,0.9986176,-0.02021429,-4.205374,-0.9966668,0.04973457,0.06466613,240.3305 +0.06470055,0.01304684,0.9978195,-52.46197,0.04727867,0.9987516,-0.01612468,-4.242766,-0.9967841,0.04821884,0.06400293,240.3855 +0.06471257,0.01720553,0.9977556,-51.62594,0.0505381,0.9985118,-0.02049639,-4.290144,-0.9966233,0.05175103,0.06374673,240.4271 +0.0653146,0.02167297,0.9976294,-50.79621,0.05343501,0.9982537,-0.02518492,-4.332237,-0.9964329,0.05495326,0.06404244,240.4708 +0.06599738,0.02168398,0.9975842,-49.96026,0.05559544,0.9981309,-0.02537391,-4.376671,-0.9962697,0.05713573,0.06466849,240.5163 +0.06718694,0.01832064,0.9975722,-49.12739,0.05502092,0.9982419,-0.02203863,-4.421564,-0.9962221,0.05636804,0.0660608,240.5643 +0.06808073,0.01583371,0.9975542,-48.29973,0.05370469,0.9983662,-0.01951182,-4.464891,-0.9962333,0.0549017,0.06711915,240.6143 +0.06921989,0.01685799,0.997459,-47.4783,0.05360339,0.9983499,-0.02059293,-4.504143,-0.9961602,0.05489261,0.06820202,240.6616 +0.07046255,0.0199085,0.9973158,-46.66587,0.05337431,0.9982933,-0.02369903,-4.538667,-0.9960854,0.05490092,0.06927968,240.7127 +0.07145922,0.02186448,0.9972039,-45.85726,0.05435565,0.9981887,-0.02578118,-4.571703,-0.9959613,0.05604595,0.07014132,240.7706 +0.07267546,0.02174184,0.9971187,-45.04822,0.05484924,0.9981622,-0.02576231,-4.610806,-0.9958462,0.05656348,0.07134938,240.8271 +0.07360728,0.02015605,0.9970836,-44.24057,0.05550483,0.9981633,-0.02427539,-4.651036,-0.9957415,0.05712979,0.07235332,240.8863 +0.07487035,0.01792003,0.9970323,-43.44076,0.05546826,0.9982157,-0.0221066,-4.687337,-0.9956493,0.05695876,0.07374276,240.9476 +0.07615531,0.01668653,0.9969564,-42.65454,0.05534835,0.9982476,-0.02093609,-4.714944,-0.9955586,0.05677427,0.07509828,241.0187 +0.07735897,0.01602126,0.9968746,-41.87896,0.05559801,0.9982456,-0.02035779,-4.728352,-0.9954518,0.05699909,0.0763325,241.0889 +0.07874542,0.01604372,0.9967657,-41.11857,0.05539489,0.9982552,-0.02044395,-4.737337,-0.9953544,0.05682558,0.07771928,241.162 +0.07972552,0.01609303,0.996687,-40.37476,0.05313762,0.9983794,-0.02037087,-4.751432,-0.9953995,0.05458564,0.07874116,241.236 +0.0806234,0.01598065,0.9966165,-39.64902,0.05011546,0.9985418,-0.02006573,-4.769963,-0.9954839,0.05156365,0.07970495,241.3128 +0.08172832,0.0169103,0.9965112,-38.93985,0.04724897,0.9986661,-0.02082197,-4.793176,-0.995534,0.04878586,0.0808203,241.3846 +0.08261518,0.01805425,0.996418,-38.24412,0.04376309,0.9988057,-0.02172601,-4.801646,-0.9956201,0.04540122,0.0817264,241.4615 +0.08263138,0.01968892,0.9963857,-37.55243,0.0403095,0.9989206,-0.02308193,-4.809079,-0.9957646,0.04207109,0.08174853,241.5394 +0.08219274,0.02106272,0.9963939,-36.8629,0.03773431,0.998994,-0.0242304,-4.824751,-0.9959018,0.03958978,0.08131526,241.6097 +0.08150379,0.02114477,0.9964487,-36.17526,0.03639746,0.9990449,-0.02417697,-4.839722,-0.9960082,0.03823871,0.08065632,241.6749 +0.08077332,0.02167639,0.9964968,-35.48874,0.03259883,0.9991712,-0.02437695,-4.855125,-0.9961992,0.03445362,0.07999975,241.7431 +0.07964735,0.02006328,0.9966212,-34.80489,0.03159797,0.9992442,-0.02264131,-4.857816,-0.9963221,0.03329452,0.07895318,241.8164 +0.07867938,0.01797641,0.9967379,-34.12746,0.03246389,0.9992609,-0.02058452,-4.863699,-0.9963712,0.03397756,0.07803764,241.8815 +0.07735895,0.01576701,0.9968787,-33.4567,0.03466775,0.9992277,-0.01849443,-4.876482,-0.9964003,0.03599024,0.07675259,241.939 +0.07517046,0.01558807,0.9970489,-32.79782,0.03592306,0.9991864,-0.01832984,-4.885648,-0.9965234,0.0371949,0.07454933,241.9976 +0.07304583,0.01694724,0.9971846,-32.15573,0.03585347,0.9991647,-0.01960724,-4.889678,-0.9966839,0.03718475,0.07237719,242.0644 +0.07102186,0.01934385,0.9972872,-31.53309,0.03574376,0.9991204,-0.02192491,-4.898054,-0.9968341,0.03720393,0.07026797,242.1248 +0.06835045,0.02180431,0.9974231,-30.92804,0.03543928,0.9990771,-0.02426903,-4.908945,-0.9970317,0.03700674,0.06751464,242.1843 +0.06519191,0.02211371,0.9976277,-30.3363,0.0357253,0.9990618,-0.02448004,-4.922557,-0.997233,0.03723644,0.06434072,242.2396 +0.06126871,0.02144594,0.9978909,-29.76456,0.03421169,0.9991365,-0.02357325,-4.942867,-0.9975348,0.03558383,0.06048211,242.2879 +0.05641752,0.0205985,0.9981948,-29.21431,0.03380813,0.9991744,-0.02252954,-4.965689,-0.9978347,0.03501815,0.05567454,242.3367 +0.05116057,0.01835047,0.9985219,-28.68168,0.03268423,0.9992648,-0.02003875,-4.986696,-0.9981554,0.0336611,0.05052318,242.373 +0.04490137,0.01672152,0.9988515,-28.17242,0.03138189,0.9993428,-0.01814046,-5.007234,-0.9984984,0.03216037,0.04434711,242.4081 +0.03756281,0.01708595,0.9991482,-27.68212,0.03085268,0.9993573,-0.01824943,-5.025891,-0.9988178,0.03151189,0.03701152,242.432 +0.02986987,0.01770445,0.999397,-27.20634,0.03067144,0.999356,-0.01862043,-5.041519,-0.9990831,0.03120912,0.02930762,242.4537 +0.02163067,0.01734513,0.9996156,-26.74717,0.03233434,0.9993143,-0.0180396,-5.053168,-0.999243,0.03271211,0.02105499,242.4689 +0.01284501,0.01667668,0.9997785,-26.30096,0.03358374,0.9992896,-0.01710001,-5.053968,-0.9993533,0.03379593,0.01227582,242.4792 +0.003463876,0.01665669,0.9998553,-25.87211,0.03470988,0.9992567,-0.01676697,-5.06017,-0.9993914,0.03476293,0.002883151,242.4793 +-0.005956811,0.01722856,0.9998339,-25.46026,0.03510783,0.9992388,-0.01700915,-5.070177,-0.9993657,0.03500067,-0.006557132,242.4733 +-0.01542424,0.01682896,0.9997394,-25.06402,0.03592944,0.9992219,-0.01626592,-5.07954,-0.9992352,0.03566918,-0.0160169,242.4658 +-0.02543958,0.01546335,0.9995568,-24.68451,0.0360602,0.9992438,-0.01454075,-5.088412,-0.9990257,0.03567429,-0.02597796,242.456 +-0.03580316,0.01392939,0.9992618,-24.31871,0.03556374,0.9992873,-0.01265551,-5.104554,-0.9987258,0.03508437,-0.03627302,242.4374 +-0.0466297,0.01230011,0.9988365,-23.9679,0.03614151,0.9992902,-0.01061848,-5.119929,-0.9982582,0.03560431,-0.04704115,242.4093 +-0.0579055,0.01332491,0.9982332,-23.63681,0.04062757,0.999114,-0.01097995,-5.143724,-0.997495,0.03991998,-0.05839555,242.3685 +-0.06958454,0.0155684,0.9974546,-23.3242,0.04693663,0.9988219,-0.01231535,-5.163532,-0.9964712,0.04596019,-0.07023329,242.3223 +-0.08148075,0.01551686,0.9965541,-23.02391,0.05325803,0.998518,-0.01119294,-5.190213,-0.9952509,0.05216249,-0.08218639,242.2745 +-0.09336097,0.01321402,0.9955447,-22.74508,0.05511753,0.9984471,-0.008083696,-5.20646,-0.9941055,0.05411725,-0.0939443,242.2312 +-0.1057725,0.01102888,0.9943292,-22.48378,0.05521546,0.9984609,-0.005201133,-5.221438,-0.9928562,0.05435219,-0.1062186,242.1869 +-0.1187021,0.009999348,0.9928796,-22.24922,0.05228095,0.9986251,-0.003806854,-5.23478,-0.9915525,0.05145679,-0.1190617,242.1494 +-0.1320468,0.00906089,0.9912021,-22.03471,0.05041384,0.9987255,-0.002413597,-5.248313,-0.9899606,0.04965158,-0.1323353,242.1124 +-0.1453125,0.008694113,0.9893476,-21.84323,0.05015977,0.9987402,-0.001409339,-5.259766,-0.9881135,0.04942064,-0.1455655,242.0695 +-0.158377,0.01079704,0.9873197,-21.67523,0.05157588,0.9986655,-0.002647779,-5.265402,-0.9860307,0.05050252,-0.1587225,242.0269 +-0.172129,0.0148589,0.9849624,-21.51618,0.05257529,0.9985996,-0.005876736,-5.267841,-0.9836704,0.05077311,-0.1726691,241.9847 +-0.1850382,0.01843142,0.9825585,-21.35727,0.05483779,0.9984599,-0.00840251,-5.27407,-0.9812001,0.05232653,-0.1857639,241.9371 +-0.1980443,0.01883505,0.9800121,-21.19397,0.05901298,0.9982308,-0.00725966,-5.291182,-0.978415,0.05639568,-0.1988054,241.8882 +-0.2125056,0.01764831,0.9770005,-21.0326,0.06212656,0.998058,-0.004515656,-5.299016,-0.9751828,0.05973806,-0.2131894,241.8351 +-0.2271283,0.01768008,0.9737044,-20.87495,0.06378608,0.9979583,-0.003241609,-5.301724,-0.9717736,0.06137251,-0.2277923,241.791 +-0.2410412,0.01878098,0.9703332,-20.71927,0.06363221,0.9979672,-0.003508926,-5.30205,-0.9684266,0.06089864,-0.2417463,241.7434 +-0.2559619,0.02010973,0.9664777,-20.56416,0.06301497,0.9980042,-0.004076838,-5.302307,-0.9646308,0.05985903,-0.2567183,241.6928 +-0.2714485,0.02135033,0.9622162,-20.41009,0.06195239,0.9980682,-0.004668608,-5.302005,-0.9604569,0.05834429,-0.2722468,241.641 +-0.2868997,0.02263699,0.9576931,-20.25737,0.06072074,0.9981401,-0.005402698,-5.299092,-0.9560342,0.05660179,-0.2877407,241.5861 +-0.3038683,0.02583471,0.9523637,-20.10375,0.05896141,0.998226,-0.008266157,-5.291935,-0.9508878,0.05364087,-0.3048525,241.5245 +-0.3231923,0.03055241,0.94584,-19.93858,0.05830801,0.9982226,-0.01232071,-5.286906,-0.9445352,0.05116808,-0.3243993,241.4532 +-0.3445536,0.03408879,0.9381475,-19.75489,0.05834479,0.9981861,-0.01484208,-5.289635,-0.9369518,0.04962212,-0.3459175,241.3621 +-0.3693305,0.03560528,0.9286158,-19.55122,0.05830265,0.998185,-0.01508452,-5.29894,-0.9274674,0.04856958,-0.370736,241.2652 +-0.39747,0.03632026,0.9168961,-19.33089,0.05907909,0.9981561,-0.01392866,-5.318294,-0.9157113,0.04863315,-0.3988829,241.1505 +-0.4295416,0.03713322,0.9022833,-19.09778,0.06018823,0.9981097,-0.01242369,-5.342702,-0.901039,0.04897033,-0.4309646,241.0007 +-0.464145,0.03779531,0.8849525,-18.85305,0.06070113,0.9980976,-0.01079074,-5.382576,-0.8836768,0.04870914,-0.4655562,240.8454 +-0.4990937,0.03806856,0.8657114,-18.60355,0.06120557,0.9980881,-0.008603883,-5.40465,-0.8643838,0.0486922,-0.5004695,240.6741 +-0.5354343,0.03880701,0.8436849,-18.34637,0.06143977,0.9980868,-0.006917066,-5.427006,-0.8423392,0.04813216,-0.5367941,240.4655 +-0.5739593,0.0386809,0.8179698,-18.08613,0.05884817,0.9982494,-0.005913111,-5.451539,-0.8167665,0.04474213,-0.5752308,240.2581 +-0.6133838,0.03698406,0.7889186,-17.81755,0.05460115,0.9984987,-0.004356707,-5.481176,-0.7878953,0.04040352,-0.6144823,240.0003 +-0.6526185,0.03467073,0.756893,-17.5544,0.05052589,0.9987203,-0.002182926,-5.502023,-0.7560001,0.03681806,-0.6535351,239.7443 +-0.6911038,0.03462877,0.7219255,-17.29903,0.04978988,0.9987597,-0.0002435898,-5.512958,-0.7210385,0.03577623,-0.6919707,239.4659 +-0.7294523,0.03662226,0.6830507,-17.04794,0.05097496,0.9986995,0.0008918126,-5.517479,-0.6821297,0.035469,-0.7303704,239.13 +-0.7669016,0.03878138,0.6405919,-16.81073,0.05262823,0.9986109,0.002549445,-5.516022,-0.6396032,0.03566838,-0.7678772,238.8014 +-0.8029273,0.04284191,0.5945354,-16.58565,0.05498521,0.9984845,0.002307899,-5.514193,-0.5935355,0.03454372,-0.8040661,238.4559 +-0.8371323,0.04474184,0.5451676,-16.37144,0.05471324,0.9984999,0.002068134,-5.510814,-0.5442573,0.03155918,-0.8383245,238.0411 +-0.8688056,0.04463225,0.4931379,-16.16855,0.05325569,0.9985749,0.003447679,-5.520163,-0.4922812,0.02925775,-0.8699443,237.6457 +-0.896638,0.04295866,0.4406756,-15.9803,0.05125953,0.9986612,0.006944043,-5.524629,-0.4397873,0.02881511,-0.8976395,237.1817 +-0.9202281,0.045603,0.3887168,-15.81914,0.05351473,0.9985214,0.00954466,-5.521756,-0.3877067,0.02958533,-0.9213078,236.7315 +-0.9392864,0.05267972,0.3390664,-15.68153,0.06026755,0.9981115,0.0118804,-5.51225,-0.3378002,0.03159379,-0.9406874,236.2441 +-0.9548918,0.05616821,0.2915936,-15.55169,0.0633794,0.9978716,0.01533568,-5.508345,-0.2901116,0.03312493,-0.9564193,235.7056 +-0.9676208,0.05507723,0.246326,-15.43172,0.06211042,0.9978511,0.02086847,-5.507429,-0.2446472,0.03549217,-0.9689623,235.1643 +-0.9772346,0.05274359,0.2055011,-15.32649,0.05936071,0.9978936,0.02616449,-5.502579,-0.2036883,0.03776753,-0.978307,234.595 +-0.9839256,0.05253744,0.1706758,-15.23713,0.05857235,0.9978167,0.03051452,-5.494907,-0.1687,0.04002089,-0.9848546,233.988 +-0.9883645,0.05421274,0.1421151,-15.16093,0.05954473,0.9976621,0.03353537,-5.481781,-0.1399648,0.04160737,-0.9892818,233.3669 +-0.991358,0.05598539,0.1186384,-15.09436,0.0604419,0.9975821,0.03430192,-5.468998,-0.1164311,0.0411762,-0.9923448,232.7201 +-0.9934117,0.05733969,0.09922383,-15.03583,0.06102452,0.9975396,0.03450632,-5.450609,-0.09700112,0.04033406,-0.9944666,232.0487 +-0.9950471,0.05682338,0.08156199,-14.98543,0.05992686,0.9975491,0.03611878,-5.428723,-0.0793097,0.04082764,-0.9960135,231.3598 +-0.9963821,0.05489789,0.06487673,-14.94039,0.05738157,0.9976642,0.0370593,-5.407624,-0.06269072,0.04064794,-0.9972048,230.6501 +-0.9972026,0.05496372,0.05065623,-14.90294,0.05696563,0.9976156,0.03896042,-5.38828,-0.04839404,0.04173709,-0.9979559,229.9214 +-0.9975121,0.05812147,0.03989373,-14.87945,0.05977783,0.9973418,0.04166365,-5.367708,-0.03736613,0.04394475,-0.9983349,229.1661 +-0.9976001,0.06140476,0.03199403,-14.86457,0.0627487,0.9971092,0.04284647,-5.343652,-0.02927056,0.04475122,-0.9985692,228.3904 +-0.9976451,0.06360688,0.02566043,-14.85183,0.06469025,0.9969408,0.04386502,-5.322829,-0.02279181,0.0454217,-0.9987078,227.5959 +-0.9977316,0.06375142,0.02162314,-14.83457,0.06466194,0.9969185,0.0444095,-5.296103,-0.01872534,0.04570695,-0.9987793,226.7838 +-0.997712,0.06478573,0.01933163,-14.82178,0.06560289,0.9968238,0.04514945,-5.263447,-0.01634519,0.04631435,-0.9987931,225.9532 +-0.9976308,0.06649962,0.01762439,-14.81286,0.06724168,0.9966976,0.0455246,-5.232088,-0.01453881,0.04660183,-0.9988077,225.1006 +-0.9975689,0.06761777,0.01686084,-14.80591,0.06832551,0.9966161,0.04569292,-5.202901,-0.01371413,0.04673386,-0.9988132,224.2273 +-0.9975169,0.0683684,0.016909,-14.79638,0.06908045,0.9965548,0.04589512,-5.167708,-0.01371297,0.04694923,-0.9988031,223.3371 +-0.9974062,0.06976417,0.01771528,-14.79054,0.0705015,0.9964908,0.04511724,-5.127437,-0.01450554,0.04624916,-0.9988246,222.4301 +-0.9973462,0.07029188,0.01896398,-14.77615,0.07107343,0.9964875,0.04428471,-5.09385,-0.01578452,0.04551502,-0.9988389,221.5037 +-0.9971564,0.07276255,0.01961861,-14.76205,0.073551,0.9963634,0.04301457,-5.057931,-0.01641742,0.04433521,-0.9988817,220.5627 +-0.997085,0.07347211,0.02057883,-14.74402,0.07426374,0.9964042,0.04078531,-5.024081,-0.01750825,0.04219467,-0.9989559,219.6008 +-0.9969265,0.07519607,0.02198199,-14.72619,0.07601142,0.9963453,0.03896476,-4.990431,-0.01897165,0.04051588,-0.9989987,218.6252 +-0.9966278,0.0783932,0.02424171,-14.70817,0.0792836,0.9961178,0.03825448,-4.958863,-0.02114871,0.04004745,-0.9989739,217.6308 +-0.9964774,0.07913667,0.02775339,-14.68116,0.08017596,0.9960338,0.03857951,-4.929982,-0.02459026,0.04066876,-0.99887,216.6246 +-0.9964381,0.07817124,0.03163049,-14.64681,0.07933262,0.9961502,0.0372972,-4.902737,-0.02859315,0.03967368,-0.9988034,215.6024 +-0.9964322,0.07648872,0.03567037,-14.60524,0.07777771,0.9963108,0.0362669,-4.87412,-0.03276477,0.03891186,-0.9987053,214.5685 +-0.9964564,0.07423901,0.03953891,-14.55998,0.07570552,0.9964442,0.03698104,-4.8425,-0.03665288,0.0398433,-0.9985334,213.5193 +-0.9964861,0.07196444,0.04285538,-14.50969,0.07361086,0.9965565,0.03816448,-4.811522,-0.03996132,0.04118499,-0.998352,212.4596 +-0.9964379,0.07092381,0.04562324,-14.45902,0.07265404,0.9966537,0.03745324,-4.783106,-0.04281424,0.04063453,-0.9982563,211.3863 +-0.9963237,0.07087906,0.04811786,-14.40526,0.07270085,0.9966587,0.03722791,-4.757659,-0.0453184,0.04058925,-0.9981476,210.299 +-0.9961282,0.07194233,0.0505267,-14.35348,0.07392284,0.9965211,0.0384857,-4.732652,-0.04758217,0.04207176,-0.9979809,209.2013 +-0.9959165,0.07324016,0.05278576,-14.30092,0.07539234,0.9963512,0.04000204,-4.705401,-0.0496634,0.04381833,-0.9978043,208.0965 +-0.9957023,0.07442569,0.05511606,-14.24429,0.07667328,0.9962595,0.0398511,-4.677533,-0.05194395,0.04390575,-0.9976843,206.9852 +-0.9955777,0.07440811,0.0573458,-14.17974,0.07671066,0.9962883,0.03905213,-4.652377,-0.05422715,0.04327846,-0.9975902,205.8696 +-0.9955268,0.07346709,0.05940558,-14.10941,0.07585453,0.9963566,0.03898243,-4.627366,-0.05632521,0.04331423,-0.9974724,204.7443 +-0.9955752,0.07173969,0.06069279,-14.0364,0.07418114,0.9964828,0.03897503,-4.606409,-0.05768326,0.04330482,-0.9973952,203.6142 +-0.9955987,0.07076558,0.06144599,-13.96238,0.0732594,0.9965373,0.0393257,-4.581422,-0.05845031,0.0436541,-0.9973353,202.4788 +-0.9955844,0.0703802,0.06211542,-13.88728,0.07292393,0.9965479,0.03967897,-4.560121,-0.05910837,0.04403346,-0.9972799,201.336 +-0.9955256,0.07063052,0.06277018,-13.8127,0.0732069,0.9965248,0.03973643,-4.535794,-0.05974543,0.04415384,-0.9972366,200.1913 +-0.995439,0.07129491,0.06338987,-13.73912,0.07389359,0.996478,0.03963932,-4.511093,-0.06034052,0.04414262,-0.9972013,199.0451 +-0.9953862,0.07186251,0.06357821,-13.6658,0.07449041,0.9964202,0.03997353,-4.484373,-0.06047801,0.04452506,-0.9971759,197.8919 +-0.9954351,0.07121114,0.06354601,-13.5915,0.07389669,0.9964244,0.04095963,-4.461457,-0.06040201,0.04546849,-0.997138,196.7326 +-0.9955612,0.06994617,0.06297265,-13.51493,0.0726532,0.996482,0.04177335,-4.432709,-0.05982922,0.04616309,-0.9971406,195.5722 +-0.99571,0.06833713,0.06238295,-13.43473,0.07104753,0.996575,0.04231351,-4.406072,-0.0592777,0.04656414,-0.9971549,194.4028 +-0.9959128,0.06632279,0.06131195,-13.35421,0.06899516,0.9967093,0.04254637,-4.38168,-0.0582884,0.04660269,-0.9972114,193.2295 +-0.9960885,0.06494764,0.05991293,-13.27544,0.06750041,0.996851,0.04161449,-4.356643,-0.0570215,0.04549586,-0.9973357,192.0549 +-0.9962335,0.06417577,0.05831182,-13.19833,0.06660458,0.9969487,0.04070791,-4.332718,-0.05552143,0.04443841,-0.997468,190.8763 +-0.9963532,0.06379072,0.05666649,-13.12607,0.06616707,0.9969618,0.04109736,-4.30667,-0.0538727,0.04469693,-0.9975469,189.69 +-0.9964024,0.06434789,0.05515069,-13.052,0.06680852,0.9967953,0.04399722,-4.285346,-0.05214282,0.04752346,-0.9975082,188.4996 +-0.9964679,0.06439028,0.0539048,-12.98336,0.06699077,0.9966026,0.0479107,-4.253796,-0.05063667,0.05135259,-0.997396,187.2995 +-0.9965055,0.06501694,0.05243767,-12.91699,0.06763714,0.9964641,0.04984429,-4.220079,-0.04901153,0.05321683,-0.9973794,186.094 +-0.9965331,0.06594263,0.05072826,-12.85457,0.06849215,0.9963839,0.0502777,-4.17898,-0.04722938,0.05357787,-0.9974461,184.8847 +-0.9966056,0.06602274,0.04917591,-12.79407,0.06843616,0.9964452,0.04912557,-4.140603,-0.0457577,0.05232422,-0.9975812,183.6729 +-0.9966002,0.06738282,0.04741049,-12.7319,0.06962713,0.9964466,0.047395,-4.10399,-0.04404841,0.05053491,-0.9977504,182.4501 +-0.9965373,0.06923428,0.04604472,-12.67514,0.07136241,0.9963752,0.04630206,-4.065229,-0.04267213,0.04942758,-0.9978657,181.2202 +-0.9964261,0.07174208,0.04458941,-12.62118,0.07376311,0.9962389,0.0454641,-4.027475,-0.04116002,0.04859066,-0.9979703,179.9874 +-0.9962973,0.07431542,0.04323151,-12.56693,0.07621845,0.9961121,0.04417433,-3.991746,-0.0397806,0.0473058,-0.9980879,178.7452 +-0.9962323,0.07572461,0.04227339,-12.50803,0.07751599,0.9960857,0.0424785,-3.960138,-0.03889125,0.04559531,-0.9982026,177.502 +-0.9962535,0.0759441,0.04137134,-12.45081,0.07763843,0.9961376,0.04101309,-3.92774,-0.03809684,0.04407143,-0.9983017,176.2461 +-0.9963806,0.07484723,0.04029417,-12.39605,0.07650981,0.9962076,0.04143279,-3.895038,-0.03704022,0.04436572,-0.9983284,174.9872 +-0.99657,0.07267309,0.03958545,-12.34119,0.07440862,0.9962438,0.0442905,-3.859344,-0.03621803,0.04708407,-0.9982341,173.7187 +-0.9967878,0.06994415,0.03901255,-12.28401,0.07173826,0.9963316,0.0466577,-3.82379,-0.035606,0.04930651,-0.9981488,172.4501 +-0.9968291,0.06959328,0.03858233,-12.22752,0.07130737,0.9964393,0.0449887,-3.78913,-0.03531404,0.04759724,-0.9982421,171.1789 +-0.9967983,0.07014577,0.038378,-12.17501,0.07172342,0.9965649,0.0414027,-3.753055,-0.03534194,0.04402273,-0.9984051,169.898 +-0.9967383,0.07092261,0.03850635,-12.12392,0.07243228,0.9965973,0.03933729,-3.718784,-0.03558542,0.04199808,-0.9984837,168.615 +-0.9966624,0.07176217,0.03891486,-12.07148,0.07326825,0.9965586,0.03876355,-3.687339,-0.03599918,0.04148539,-0.9984903,167.326 +-0.9965866,0.07250476,0.0394759,-12.01825,0.07413206,0.9963867,0.04144863,-3.654372,-0.03632804,0.04423357,-0.9983604,166.0281 +-0.9965516,0.07264605,0.04009448,-11.9612,0.07441859,0.9962273,0.04464387,-3.624176,-0.03670001,0.04747368,-0.998198,164.728 +-0.9964585,0.07364473,0.04058204,-11.90192,0.0754238,0.9961713,0.04420445,-3.591375,-0.03717123,0.04710875,-0.9981979,163.4318 +-0.9963913,0.07421798,0.04118428,-11.84135,0.07598927,0.9961701,0.043252,-3.554551,-0.03781647,0.04622547,-0.9982149,162.1341 +-0.9963561,0.0743864,0.04172768,-11.77885,0.0761629,0.9961792,0.04273363,-3.523863,-0.03838944,0.045756,-0.9982147,160.8345 +-0.996288,0.07494299,0.04235494,-11.72026,0.07672619,0.9961599,0.04217127,-3.495831,-0.03903185,0.04526446,-0.9982122,159.5304 +-0.9962547,0.07500587,0.04302109,-11.65518,0.07680993,0.9961634,0.04193594,-3.475071,-0.03971059,0.04508331,-0.9981936,158.2205 +-0.9962885,0.07416001,0.04369938,-11.5911,0.07603687,0.9961784,0.04297618,-3.44908,-0.04034526,0.04613943,-0.9981199,156.9119 +-0.9962911,0.07369567,0.04441888,-11.52805,0.07564548,0.9961662,0.04393995,-3.427408,-0.0410104,0.04713706,-0.9980462,155.6018 +-0.9963031,0.07297865,0.04532553,-11.46658,0.07499232,0.9961934,0.04443854,-3.404632,-0.04190993,0.04767332,-0.9979833,154.2873 +-0.9962748,0.07279099,0.04623878,-11.40471,0.07489538,0.9961511,0.04553618,-3.37976,-0.04274619,0.04882961,-0.9978919,152.9761 +-0.9962278,0.07287466,0.04711133,-11.34344,0.07505487,0.9961042,0.04629416,-3.361264,-0.04355412,0.04965545,-0.9978162,151.6424 +-0.9961849,0.07285137,0.04804677,-11.28131,0.07508169,0.9960985,0.04637312,-3.336424,-0.04448096,0.04980362,-0.997768,150.32 +-0.9961416,0.07292889,0.04881951,-11.21753,0.075163,0.9961278,0.04560633,-3.31279,-0.04530445,0.04909977,-0.9977658,148.9982 +-0.9960832,0.07324312,0.04953564,-11.14296,0.07546769,0.9961487,0.04463524,-3.265873,-0.04607564,0.04819875,-0.9977744,147.6896 +-0.9960388,0.07327843,0.05036998,-11.05762,0.07554981,0.9961363,0.04477304,-3.207848,-0.04689447,0.04840112,-0.9977265,146.3791 +-0.9959557,0.07396547,0.05100468,-10.97994,0.07627722,0.9960719,0.04497192,-3.16496,-0.04747796,0.04868052,-0.9976853,145.0648 +-0.9959725,0.07318364,0.05179774,-10.89652,0.0755072,0.9961554,0.04441888,-3.120438,-0.04834786,0.04815108,-0.9976692,143.7492 +-0.9960276,0.07178739,0.05268533,-10.81524,0.07413313,0.9962769,0.04400659,-3.079246,-0.04933006,0.0477375,-0.997641,142.4436 +-0.9960538,0.07087358,0.05342194,-10.73215,0.07335144,0.9962474,0.04594267,-3.035455,-0.04996534,0.04967994,-0.9975145,141.1379 +-0.9961426,0.06913243,0.05404403,-10.65026,0.0719544,0.9960441,0.05214031,-2.995802,-0.05022565,0.05582788,-0.9971763,139.8272 +-0.9960495,0.06973046,0.05498337,-10.57055,0.072814,0.9957593,0.05622763,-2.953729,-0.05082942,0.06000905,-0.9969028,138.5179 +-0.9959282,0.07056879,0.05609982,-10.49042,0.07370374,0.9957107,0.05592734,-2.907447,-0.05191247,0.05983437,-0.9968575,137.2165 +-0.9958169,0.07156016,0.05681466,-10.41078,0.07461969,0.9957657,0.05369004,-2.860157,-0.05273202,0.05770493,-0.99694,135.9273 +-0.995763,0.0716102,0.05768947,-10.33398,0.07467311,0.9958086,0.05281123,-2.818003,-0.05366585,0.05689531,-0.9969367,134.6391 +-0.9956926,0.0718524,0.05859737,-10.26756,0.07499671,0.9957556,0.0533509,-2.782459,-0.05451527,0.05751569,-0.996855,133.3539 +-0.9955485,0.07307127,0.05953011,-10.19703,0.07634218,0.9955824,0.05465886,-2.76977,-0.05527313,0.05896019,-0.9967289,132.0608 +-0.9954176,0.07415226,0.060377,-10.12324,0.07752538,0.9954397,0.05558409,-2.741915,-0.05597997,0.06001012,-0.9966268,130.7858 +-0.9952523,0.07597763,0.06083024,-10.05361,0.07944774,0.9952185,0.05681686,-2.722968,-0.05622257,0.06137993,-0.9965297,129.5177 +-0.9952297,0.07600679,0.06116269,-9.983675,0.07946414,0.9952504,0.05623136,-2.704977,-0.05659822,0.06082335,-0.9965425,128.2716 +-0.9952049,0.07641744,0.06105434,-9.91435,0.07971245,0.9953818,0.05348805,-2.693324,-0.05668495,0.05809835,-0.9967002,127.0275 +-0.9953209,0.07534886,0.06048868,-9.842207,0.07847462,0.9956068,0.0510769,-2.676072,-0.05637435,0.05558473,-0.9968612,125.7959 +-0.9955321,0.07262602,0.06034348,-9.768507,0.0757054,0.995855,0.05041391,-2.668058,-0.05643199,0.05475698,-0.9969037,124.5864 +-0.9956184,0.07193217,0.0597474,-9.696148,0.07493906,0.9959484,0.04970876,-2.675813,-0.05592966,0.05396837,-0.996975,123.3957 +-0.9957201,0.07142144,0.05865536,-9.626526,0.07438912,0.9959714,0.05007235,-2.673222,-0.05484281,0.05422136,-0.9970217,122.2269 +-0.9958299,0.07075025,0.05759566,-9.557928,0.07363915,0.9960466,0.0496826,-2.676765,-0.05385291,0.0537167,-0.9971029,121.0553 +-0.995862,0.07110237,0.05659893,-9.493383,0.07390922,0.9960532,0.04914614,-2.664828,-0.05288114,0.05312595,-0.9971866,119.9144 +-0.9959578,0.07065617,0.05546017,-9.430385,0.07342022,0.9960723,0.04949069,-2.656282,-0.05174551,0.05336253,-0.9972335,118.791 +-0.9960527,0.07026569,0.0542383,-9.369618,0.07295424,0.9961165,0.04929064,-2.640186,-0.05056422,0.05305298,-0.9973106,117.6937 +-0.9961715,0.0695039,0.05302564,-9.31028,0.07208599,0.9962213,0.0484431,-2.629674,-0.04945828,0.05208003,-0.9974174,116.6077 +-0.9961917,0.07005536,0.05190701,-9.254439,0.0725416,0.9962255,0.04766941,-2.616277,-0.04837159,0.05125328,-0.9975135,115.5322 +-0.9962258,0.07034187,0.0508558,-9.200535,0.07277434,0.9962083,0.04767399,-2.603835,-0.04730949,0.05119504,-0.9975674,114.4721 +-0.996188,0.07172591,0.0496483,-9.151332,0.07409011,0.9961178,0.04753843,-2.588856,-0.04604582,0.05103565,-0.9976347,113.4423 +-0.9961531,0.07289656,0.04863228,-9.105417,0.07521117,0.9960312,0.04759325,-2.57026,-0.04496988,0.05106784,-0.9976822,112.4364 +-0.9961733,0.07332033,0.04757049,-9.061229,0.07562751,0.995948,0.0486614,-2.551094,-0.04380986,0.05207281,-0.9976818,111.4494 +-0.9962187,0.07340937,0.04647002,-9.017561,0.07568432,0.9959148,0.04924961,-2.534579,-0.0426648,0.05258043,-0.9977048,110.482 +-0.9962591,0.07354235,0.04538023,-8.973774,0.07579749,0.9958611,0.05015305,-2.518585,-0.04150404,0.05340513,-0.99771,109.5389 +-0.9963828,0.07264441,0.04409188,-8.931383,0.07484917,0.9959102,0.05060113,-2.50091,-0.04023567,0.05371832,-0.9977451,108.631 +-0.9965194,0.07152238,0.0428218,-8.88896,0.07370262,0.9959389,0.05170601,-2.499823,-0.03894976,0.05468211,-0.9977438,107.7448 +-0.9965622,0.0715685,0.04173568,-8.850383,0.07373292,0.9958762,0.05285779,-2.490803,-0.03778062,0.05575336,-0.9977295,106.8841 +-0.9966037,0.07157097,0.04072852,-8.810192,0.07374684,0.9957759,0.05469652,-2.484177,-0.03664179,0.05751434,-0.997672,106.0361 +-0.9965274,0.0728689,0.04029183,-8.774269,0.07507494,0.9955897,0.05625643,-2.471963,-0.03601478,0.05908597,-0.997603,105.2229 +-0.9963119,0.0752076,0.04130905,-8.743349,0.07752694,0.9953162,0.05775139,-2.45648,-0.03677222,0.06074095,-0.9974759,104.4293 +-0.9958914,0.07849169,0.04515929,-8.714586,0.08103601,0.9950467,0.0575775,-2.438815,-0.04041625,0.06100046,-0.9973191,103.6667 +-0.9951967,0.08263177,0.05249461,-8.679794,0.08558221,0.994717,0.05668933,-2.425206,-0.04753294,0.06090963,-0.9970108,102.918 +-0.9941419,0.08671496,0.06451678,-8.637154,0.09037707,0.9943213,0.05618827,-2.411937,-0.05927805,0.06168994,-0.9963335,102.1865 +-0.9927004,0.08903931,0.08135159,-8.579986,0.09370763,0.9940524,0.0554856,-2.39846,-0.07592734,0.06270383,-0.9951398,101.4891 +-0.9906796,0.09047763,0.1018221,-8.504861,0.09642566,0.9938147,0.05508541,-2.388482,-0.09620829,0.06439024,-0.9932763,100.8128 +-0.9877818,0.09109262,0.1264489,-8.410845,0.09849788,0.9936933,0.053589,-2.378517,-0.1207698,0.06538917,-0.9905245,100.1642 +-0.9833359,0.09348022,0.155923,-8.297513,0.1024351,0.9934619,0.05040344,-2.373526,-0.1501918,0.06553548,-0.9864823,99.5295 +-0.9776056,0.09280749,0.188876,-8.163715,0.1035149,0.9934874,0.04761647,-2.36755,-0.1832268,0.06610159,-0.9808458,98.91972 +-0.970136,0.09043999,0.2250708,-8.004362,0.1027903,0.9937405,0.04374899,-2.365664,-0.2197053,0.06557753,-0.9733597,98.33881 +-0.9609776,0.08558626,0.2630533,-7.822238,0.09957581,0.9942138,0.04029245,-2.363865,-0.2580828,0.06491388,-0.9639395,97.78786 +-0.9499461,0.07969838,0.3020771,-7.618518,0.0953543,0.99474,0.03741516,-2.365779,-0.2975063,0.06434673,-0.9525489,97.24366 +-0.9360883,0.07451328,0.3437827,-7.391983,0.09209469,0.9951323,0.03507498,-2.368503,-0.3394957,0.06449383,-0.9383939,96.72212 +-0.9195393,0.06644626,0.3873401,-7.139675,0.08604633,0.9957291,0.03346023,-2.371668,-0.3834625,0.06409718,-0.9213294,96.22046 +-0.9001217,0.05680061,0.4319198,-6.858427,0.07819436,0.996427,0.03191973,-2.377152,-0.4285634,0.06250532,-0.901347,95.72524 +-0.8770849,0.0469913,0.4780313,-6.558989,0.07063488,0.9970018,0.03159283,-2.382497,-0.4751135,0.06147527,-0.8777744,95.26713 +-0.8504074,0.03805485,0.5247468,-6.241788,0.06375191,0.9974848,0.03097865,-2.3914,-0.522248,0.05979807,-0.8506945,94.82915 +-0.8208033,0.0304972,0.5703963,-5.899838,0.05886994,0.9977727,0.03136653,-2.396299,-0.5681693,0.05932493,-0.8207705,94.39238 +-0.7883662,0.02270087,0.6147874,-5.538427,0.05370952,0.998043,0.03202133,-2.402837,-0.6128573,0.05826445,-0.7880426,93.99153 +-0.7519757,0.01698164,0.6589721,-5.158326,0.05106535,0.9981647,0.03254981,-2.408911,-0.6572099,0.0581273,-0.7514627,93.62076 +-0.7112647,0.01414579,0.702782,-4.759829,0.05104987,0.9981968,0.03157407,-2.41455,-0.7010681,0.05833444,-0.7107043,93.2539 +-0.6682819,0.01388988,0.7437785,-4.350995,0.05322919,0.9981557,0.02918587,-2.417305,-0.7420013,0.05909511,-0.6677886,92.93117 +-0.6228704,0.01465697,0.7821878,-3.910527,0.05456684,0.9982034,0.02474779,-2.412408,-0.7804197,0.05809617,-0.6225511,92.63366 +-0.5752228,0.01473772,0.8178641,-3.450087,0.05406313,0.9983365,0.02003404,-2.40785,-0.8162083,0.05574031,-0.5750626,92.34876 +-0.5261793,0.01216376,0.8502867,-2.971838,0.05327719,0.9984049,0.01868662,-2.408021,-0.8487031,0.05513339,-0.525988,92.10569 +-0.475088,0.009326973,0.8798889,-2.475933,0.05099495,0.998555,0.0169494,-2.410053,-0.8784594,0.05292234,-0.4748771,91.87573 +-0.4226279,0.006764292,0.9062781,-1.973871,0.04659141,0.998812,0.01427218,-2.417222,-0.9051049,0.04825658,-0.422441,91.69173 +-0.3702129,0.006034494,0.9289274,-1.461824,0.04572878,0.9988849,0.0117357,-2.425884,-0.9278207,0.04682341,-0.370076,91.527 +-0.3190705,0.00794832,0.9476977,-0.9535855,0.04679551,0.9988772,0.007377527,-2.435403,-0.9465749,0.04670193,-0.3190842,91.37699 +-0.2707671,0.01132841,0.9625783,-0.4368032,0.04725783,0.9988815,0.001537664,-2.446763,-0.9614842,0.0459057,-0.2709996,91.26186 +-0.2261652,0.01067749,0.9740305,0.08493472,0.03975518,0.9992079,-0.001722539,-2.456413,-0.9732773,0.03833317,-0.2264105,91.18058 +-0.1860257,0.008681791,0.9825066,0.6102987,0.03606891,0.9993473,-0.002001399,-2.466161,-0.9818826,0.03506562,-0.1862174,91.09177 +-0.1504744,0.01003583,0.988563,1.138073,0.03389616,0.9994129,-0.004986469,-2.48012,-0.9880326,0.03275815,-0.1507262,91.02634 +-0.1206036,0.01253365,0.9926217,1.669864,0.03609708,0.9993144,-0.008232369,-2.501031,-0.9920442,0.03483788,-0.1209733,90.96038 +-0.0964984,0.01447502,0.9952279,2.20844,0.03664238,0.9992681,-0.0109809,-2.524745,-0.9946584,0.03540787,-0.09695816,90.91501 +-0.07720147,0.01815333,0.9968503,2.751248,0.03393141,0.9993028,-0.01557017,-2.544208,-0.9964379,0.03262248,-0.07776361,90.88197 +-0.06177846,0.02661506,0.997735,3.299026,0.03328086,0.9991434,-0.02459193,-2.55768,-0.9975348,0.03168622,-0.06261131,90.84272 +-0.05011074,0.02866767,0.9983322,3.858709,0.03535214,0.9990125,-0.02691273,-2.572712,-0.9981178,0.03394455,-0.05107471,90.8059 +-0.04104311,0.0255638,0.9988303,4.431705,0.03833365,0.9989769,-0.02399238,-2.590718,-0.9984217,0.03730408,-0.04198107,90.76614 +-0.03414277,0.02062906,0.9992041,5.016199,0.03918087,0.999046,-0.019287,-2.612554,-0.9986486,0.03849116,-0.03491846,90.73841 +-0.02960733,0.01776081,0.9994038,5.610331,0.03861348,0.9991161,-0.01661178,-2.632315,-0.9988155,0.03809862,-0.03026696,90.71049 +-0.02786768,0.01757846,0.9994571,6.211745,0.0386676,0.999116,-0.01649431,-2.650403,-0.9988634,0.03818693,-0.02852275,90.68357 +-0.02722461,0.01829836,0.9994619,6.821468,0.04119832,0.9990035,-0.01716776,-2.665408,-0.99878,0.04070875,-0.02795134,90.64843 +-0.02616227,0.01900282,0.9994771,7.446192,0.04497527,0.9988292,-0.01781324,-2.681728,-0.9986454,0.04448571,-0.0269863,90.6185 +-0.0256702,0.02032244,0.9994639,8.08109,0.04852036,0.9986403,-0.0190595,-2.703699,-0.9984922,0.04800508,-0.02662134,90.57853 +-0.02617453,0.02102096,0.9994364,8.731175,0.05109047,0.9985004,-0.01966326,-2.727805,-0.9983509,0.05054698,-0.02720924,90.54284 +-0.02633504,0.02105336,0.9994315,9.399524,0.05266188,0.9984191,-0.0196444,-2.754209,-0.998265,0.05211459,-0.02740212,90.50118 +-0.02610918,0.02061157,0.9994466,10.07658,0.05422092,0.9983449,-0.01917241,-2.773656,-0.9981875,0.05369032,-0.02718354,90.46928 +-0.02549808,0.02042503,0.9994662,10.77161,0.05726642,0.9981793,-0.01893777,-2.793484,-0.9980332,0.05675296,-0.02662132,90.4367 +-0.02421406,0.02064081,0.9994937,11.47973,0.062724,0.9978484,-0.01908727,-2.81297,-0.9977371,0.06223005,-0.02545663,90.40048 +-0.02149124,0.02091953,0.9995502,12.20589,0.06682984,0.9975749,-0.01944129,-2.834519,-0.9975329,0.06638194,-0.02283717,90.36536 +-0.01700505,0.02095003,0.9996359,12.95106,0.07041597,0.9973231,-0.0197037,-2.856224,-0.9973727,0.07005525,-0.01843474,90.33131 +-0.01058114,0.02102745,0.9997229,13.71186,0.07352996,0.9970885,-0.0201938,-2.879219,-0.9972368,0.0732959,-0.01209648,90.30931 +-0.002234786,0.0208229,0.9997807,14.48981,0.07655804,0.9968525,-0.02059079,-2.901943,-0.9970626,0.07649522,-0.00382191,90.29031 +0.007417937,0.01924772,0.9997873,15.28482,0.0792827,0.996656,-0.01977569,-2.923415,-0.9968245,0.0794125,0.005867121,90.28642 +0.01826995,0.01817013,0.999668,16.08562,0.08031803,0.9965769,-0.01958184,-2.939984,-0.9966018,0.0806491,0.01674802,90.29611 +0.02942269,0.01608883,0.9994376,16.91051,0.0771242,0.9968532,-0.01831771,-2.960754,-0.9965872,0.07761976,0.02808926,90.32149 +0.04042898,0.01579351,0.9990576,17.74494,0.07510559,0.9969983,-0.01880027,-2.980526,-0.9963556,0.07579487,0.03912144,90.35587 +0.05153679,0.01733169,0.9985207,18.59667,0.07264508,0.9971355,-0.02105709,-3.005455,-0.9960254,0.07362282,0.0501301,90.40215 +0.06239743,0.02161166,0.9978174,19.45362,0.06886126,0.9972898,-0.0259064,-3.03401,-0.9956729,0.07032744,0.06074011,90.45965 +0.07183922,0.02638636,0.9970672,20.31322,0.06496402,0.9974036,-0.03107596,-3.064211,-0.9952983,0.06700595,0.06993852,90.52235 +0.0788494,0.02761002,0.9965041,21.17745,0.05843742,0.9977694,-0.032269,-3.096855,-0.9951722,0.06077751,0.07706006,90.59661 +0.08232797,0.02532889,0.9962834,22.04573,0.05228415,0.9981905,-0.02969789,-3.13109,-0.9952328,0.05453478,0.0808547,90.67111 +0.08322809,0.02389315,0.9962441,22.9146,0.04730782,0.9984906,-0.02789923,-3.169221,-0.9954069,0.04945212,0.08197213,90.7494 +0.08118946,0.02210037,0.9964537,23.77962,0.04346288,0.9987246,-0.02569203,-3.210301,-0.9957506,0.04539465,0.08012536,90.82667 +0.0773212,0.02206442,0.9967621,24.64428,0.03914086,0.9989172,-0.02514838,-3.253324,-0.9962376,0.04095862,0.07637385,90.89534 +0.07116311,0.02247313,0.9972115,25.49897,0.03594641,0.999039,-0.02507953,-3.29482,-0.9968167,0.0376309,0.07028689,90.95836 +0.06298164,0.02367836,0.9977338,26.35325,0.03297296,0.9991234,-0.02579275,-3.334796,-0.9974698,0.0345227,0.06214568,91.01462 +0.05312298,0.02413278,0.9982964,27.20785,0.0302961,0.9992088,-0.02576701,-3.371323,-0.9981283,0.03161329,0.05234982,91.06401 +0.04210354,0.02288973,0.998851,28.06008,0.02863046,0.9992993,-0.02410684,-3.405154,-0.9987029,0.02961254,0.04141869,91.09881 +0.03056435,0.02036693,0.9993253,28.91571,0.02791101,0.9993851,-0.02122182,-3.438232,-0.999143,0.0285408,0.0299771,91.12023 +0.01863177,0.01828016,0.9996593,29.76478,0.02766332,0.9994406,-0.01879176,-3.470581,-0.9994436,0.02800401,0.01811566,91.13015 +0.006673685,0.01899539,0.9997973,30.60909,0.02894733,0.9993969,-0.01918101,-3.500672,-0.9995586,0.02906946,0.006119795,91.12702 +-0.005312876,0.02093624,0.9997667,31.44794,0.03060848,0.9993157,-0.02076415,-3.527684,-0.9995173,0.03049101,-0.005950066,91.11123 +-0.01745769,0.02181656,0.9996096,32.28769,0.03242736,0.9992483,-0.02124235,-3.554151,-0.9993216,0.03204384,-0.01815202,91.08555 +-0.02949662,0.02077817,0.9993489,33.12438,0.03387243,0.9992305,-0.01977594,-3.582327,-0.9989908,0.03326705,-0.03017773,91.0447 +-0.04096088,0.01973606,0.9989658,33.96442,0.03498441,0.9992202,-0.01830662,-3.611519,-0.9985481,0.03419837,-0.04161939,90.99774 +-0.05118554,0.02004373,0.998488,34.80342,0.03749714,0.9991322,-0.01813445,-3.63864,-0.9979849,0.03651222,-0.0518927,90.9378 +-0.05899193,0.02063119,0.9980453,35.64157,0.03994307,0.9990345,-0.01829072,-3.664619,-0.997459,0.03878598,-0.05975904,90.87112 +-0.06473322,0.02188563,0.9976626,36.48132,0.04172831,0.9989444,-0.01920622,-3.695783,-0.9970297,0.04038748,-0.06557813,90.80032 +-0.06864488,0.02476225,0.9973338,37.32512,0.04248099,0.9988577,-0.0218762,-3.725465,-0.9967363,0.04086603,-0.06961839,90.72894 +-0.07088885,0.02748791,0.9971054,38.17446,0.04356661,0.9987516,-0.02443595,-3.753578,-0.9965323,0.04170825,-0.0719979,90.65326 +-0.07180171,0.02948397,0.9969831,39.03116,0.04452366,0.9986613,-0.02632706,-3.784327,-0.9964246,0.042499,-0.07301832,90.5801 +-0.07113755,0.02980377,0.9970212,39.89617,0.04524815,0.9986209,-0.02662314,-3.816446,-0.9964396,0.04321945,-0.072388,90.50847 +-0.06922094,0.02925645,0.9971723,40.77155,0.04530176,0.9986309,-0.02615453,-3.849569,-0.9965722,0.04336321,-0.07045153,90.4329 +-0.06685229,0.02726587,0.9973903,41.65672,0.04523435,0.9986815,-0.02426925,-3.883158,-0.9967369,0.04349383,-0.0679975,90.36243 +-0.06434591,0.02479427,0.9976196,42.54796,0.04558345,0.9987208,-0.02188154,-3.919092,-0.996886,0.04406694,-0.06539381,90.28996 +-0.06200928,0.02307069,0.9978089,43.44429,0.0458985,0.998741,-0.02023987,-3.955976,-0.9970196,0.04454286,-0.06299012,90.22418 +-0.06043167,0.02117074,0.9979478,44.34296,0.0437982,0.9988684,-0.01853804,-3.993405,-0.9972109,0.04258803,-0.06129052,90.16105 +-0.06011656,0.02100958,0.9979703,45.24463,0.04149121,0.998967,-0.01853119,-4.029977,-0.9973286,0.04029295,-0.06092617,90.09781 +-0.0615409,0.02124792,0.9978784,46.14374,0.03761506,0.9991125,-0.01895442,-4.066554,-0.9973955,0.03636877,-0.06228551,90.03424 +-0.06556336,0.02205229,0.9976047,47.04273,0.03296996,0.9992577,-0.01992203,-4.102566,-0.9973035,0.03158482,-0.06624175,89.97108 +-0.07184265,0.02161185,0.9971818,47.94302,0.02704114,0.9994399,-0.0197126,-4.1378,-0.9970493,0.02554872,-0.07238681,89.90361 +-0.08156536,0.02211756,0.9964226,48.83973,0.02205123,0.999549,-0.02038189,-4.172906,-0.996424,0.02030988,-0.08201629,89.82867 +-0.09481393,0.02440724,0.9951958,49.73213,0.01863746,0.9995677,-0.02273885,-4.209393,-0.9953205,0.01639196,-0.09522782,89.73697 +-0.1110383,0.02817634,0.9934167,50.61799,0.01411047,0.9995419,-0.0267729,-4.24625,-0.9937159,0.01104475,-0.111385,89.62973 +-0.129245,0.02965948,0.9911691,51.50038,0.01391428,0.9995084,-0.02809466,-4.280657,-0.991515,0.0101603,-0.1295941,89.50388 +-0.1487487,0.02680706,0.9885116,52.37928,0.0158122,0.9995691,-0.02472756,-4.315191,-0.9887486,0.01195234,-0.1491084,89.35421 +-0.1696414,0.02120907,0.9852776,53.25215,0.01853868,0.9996601,-0.01832676,-4.349397,-0.9853314,0.01515676,-0.1699769,89.18563 +-0.1911515,0.01998029,0.9813572,54.11543,0.02029024,0.9996596,-0.01640074,-4.38318,-0.9813508,0.01677694,-0.1914918,88.99543 +-0.2120472,0.02060376,0.9770422,54.9685,0.02407821,0.9995843,-0.01585345,-4.412621,-0.9769627,0.02016374,-0.2124551,88.78682 +-0.2322954,0.0235245,0.9723608,55.81415,0.03052651,0.9993913,-0.01688574,-4.435513,-0.9721661,0.0257603,-0.2328721,88.55772 +-0.2522846,0.02228046,0.9673966,56.65441,0.03448592,0.9993068,-0.01402192,-4.45828,-0.9670383,0.02982404,-0.2528781,88.31973 +-0.2713065,0.02242172,0.9622318,57.48695,0.0383992,0.9991848,-0.01245593,-4.481396,-0.9617267,0.03356955,-0.2719463,88.06451 +-0.2876833,0.02376735,0.9574307,58.3133,0.04324194,0.9989948,-0.01180606,-4.506276,-0.9567489,0.03800474,-0.2884219,87.79163 +-0.3023285,0.02585022,0.9528532,59.13858,0.04509189,0.9989009,-0.01279237,-4.526147,-0.9521366,0.03909845,-0.3031618,87.51373 +-0.315435,0.02463118,0.9486275,59.96701,0.04794971,0.9987998,-0.009989808,-4.542452,-0.947735,0.04233527,-0.3162374,87.22223 +-0.3256915,0.01558251,0.9453477,60.80547,0.05057203,0.9987199,0.0009608141,-4.550111,-0.9441226,0.04812107,-0.3260626,86.92074 +-0.3335314,0.005416807,0.9427234,61.65008,0.04912119,0.998725,0.01164027,-4.554028,-0.9414583,0.05019008,-0.3333722,86.61605 +-0.3386003,0.0005953644,0.9409301,62.4892,0.04878757,0.9986658,0.01692465,-4.559425,-0.9396646,0.05163637,-0.3381776,86.30672 +-0.3400358,0.007198371,0.940385,63.32202,0.05061897,0.9986611,0.01065895,-4.563838,-0.9390492,0.05122573,-0.3399449,85.9941 +-0.3403208,0.019877,0.9400993,64.1523,0.05337924,0.9985727,-0.001789782,-4.568649,-0.938793,0.04957268,-0.3408961,85.68272 +-0.3396877,0.03040815,0.9400466,64.98797,0.05467388,0.9984255,-0.01254006,-4.579089,-0.9389478,0.04713629,-0.3408153,85.37187 +-0.3391444,0.03207577,0.9401874,65.83372,0.05471004,0.9983995,-0.01432676,-4.600068,-0.9391421,0.04657884,-0.3403564,85.05504 +-0.3398005,0.02843763,0.9400675,66.68849,0.05454409,0.9984563,-0.01048822,-4.622012,-0.9389145,0.04771122,-0.340827,84.73494 +-0.3419605,0.02322833,0.9394272,67.54801,0.05417162,0.9985192,-0.004970464,-4.642618,-0.9381516,0.04919058,-0.3427124,84.41083 +-0.3464151,0.01908796,0.9378871,68.40622,0.05502992,0.9984847,4.427112E-06,-4.658949,-0.9364658,0.05161338,-0.3469406,84.08078 +-0.3523198,0.01658184,0.9357328,69.26862,0.05529304,0.9984653,0.003125292,-4.671587,-0.9342448,0.0528406,-0.352696,83.74444 +-0.3607043,0.01712735,0.932523,70.13521,0.05471058,0.9984982,0.002823198,-4.683307,-0.9310742,0.0520372,-0.3610996,83.40015 +-0.3714754,0.02047584,0.928217,70.99861,0.05373385,0.9985551,-0.000523001,-4.693642,-0.9268865,0.04968238,-0.3720389,83.04535 +-0.385064,0.02328394,0.9225961,71.85907,0.05551004,0.998456,-0.002030228,-4.703205,-0.9212189,0.05043157,-0.385762,82.67153 +-0.3993903,0.02482154,0.9164449,72.72096,0.05597997,0.9984284,-0.002645742,-4.717954,-0.9150702,0.05024587,-0.4001521,82.28193 +-0.4133231,0.02894187,0.9101244,73.57852,0.0553521,0.998445,-0.006612919,-4.736698,-0.9089005,0.04764401,-0.4142823,81.87928 +-0.4254372,0.03877125,0.9041571,74.43372,0.05882827,0.9981536,-0.0151212,-4.755133,-0.9030739,0.04675686,-0.4269325,81.45699 +-0.4357161,0.04483909,0.8989666,75.29464,0.06148719,0.997908,-0.0199722,-4.777015,-0.8979815,0.04657271,-0.4375615,81.02618 +-0.444025,0.04137235,0.8950588,76.17016,0.06031586,0.9980477,-0.01621106,-4.804992,-0.893982,0.04678811,-0.4456535,80.58697 +-0.4489518,0.03625468,0.8928202,77.05191,0.06128903,0.9980728,-0.009709677,-4.831255,-0.8914515,0.05036089,-0.4503086,80.13589 +-0.4513088,0.0343381,0.891707,77.93475,0.06228078,0.9980347,-0.006911194,-4.854281,-0.8901918,0.05241711,-0.4525604,79.6788 +-0.4525031,0.03558518,0.8910526,78.81983,0.0643649,0.9979007,-0.00716587,-4.873979,-0.8894369,0.05410992,-0.4538436,79.21929 +-0.4529457,0.03550914,0.8908307,79.7072,0.06531479,0.9978431,-0.006565232,-4.893911,-0.8891423,0.05521072,-0.454288,78.75908 +-0.4538885,0.03489319,0.8903751,80.59512,0.06430896,0.99791,-0.006324504,-4.918047,-0.8887348,0.05438846,-0.4551837,78.30009 +-0.4547656,0.03315342,0.8899939,81.48042,0.06295391,0.9980038,-0.005008994,-4.940816,-0.8883834,0.05375067,-0.4559449,77.84269 +-0.4558152,0.03184394,0.8895046,82.36123,0.0599815,0.998187,-0.004997996,-4.964461,-0.888051,0.05107564,-0.4568988,77.38417 +-0.457973,0.02982886,0.8884655,83.23781,0.05622121,0.998408,-0.004539948,-4.987705,-0.8871865,0.04787142,-0.4589209,76.92792 +-0.4622218,0.02821657,0.8863153,84.10869,0.05251293,0.9986105,-0.004405595,-5.01216,-0.8852081,0.04450664,-0.4630613,76.46987 +-0.469088,0.02605465,0.882767,84.97689,0.04833222,0.9988241,-0.003797098,-5.03366,-0.8818279,0.04088491,-0.4697957,76.00608 +-0.477751,0.02122747,0.8782388,85.84043,0.04152061,0.9991364,-0.001562936,-5.054052,-0.8775135,0.03571831,-0.4782197,75.53882 +-0.4878093,0.0180701,0.8727632,86.69522,0.04023335,0.9991887,0.00179975,-5.077013,-0.8720226,0.03599211,-0.4881405,75.05529 +-0.4986973,0.01621601,0.8666246,87.53763,0.04046721,0.9991703,0.004590591,-5.096609,-0.865831,0.03735918,-0.4989397,74.56077 +-0.5102223,0.01496338,0.8599124,88.36918,0.03825995,0.9992537,0.005313174,-5.11217,-0.8591911,0.0356111,-0.510414,74.06297 +-0.5223375,0.01417692,0.8526211,89.18892,0.0381488,0.9992492,0.006755949,-5.126981,-0.8518851,0.03605534,-0.5224861,73.55603 +-0.5347818,0.01350912,0.8448822,89.99811,0.03708245,0.9992841,0.007494019,-5.1424,-0.8441761,0.03533796,-0.5348998,73.03782 +-0.5478056,0.01498096,0.8364716,90.79262,0.03712312,0.9992901,0.006414949,-5.157865,-0.8357816,0.03456657,-0.5479728,72.50815 +-0.5606699,0.01645,0.827876,91.57304,0.03584719,0.9993475,0.004419939,-5.173387,-0.8272631,0.03215515,-0.5608937,71.96849 +-0.5733897,0.0176366,0.819093,92.344,0.03550062,0.9993641,0.003333316,-5.186461,-0.8185133,0.03098959,-0.5736511,71.41911 +-0.5857014,0.01643024,0.8103604,93.10435,0.03523788,0.9993654,0.005206389,-5.202598,-0.8097606,0.03160477,-0.5859086,70.85573 +-0.5979125,0.01516468,0.801418,93.85038,0.03553763,0.9993394,0.007603677,-5.217836,-0.8007732,0.03302682,-0.5980564,70.28594 +-0.6095362,0.01558934,0.792605,94.58632,0.03714912,0.9992699,0.008914616,-5.232682,-0.7918874,0.03487835,-0.6096703,69.70255 +-0.6198271,0.01708865,0.7845523,95.30854,0.03919053,0.9991894,0.009198296,-5.243985,-0.7837592,0.03644837,-0.6199944,69.11483 +-0.6280129,0.01934875,0.7779624,96.02056,0.04249902,0.9990517,0.009459996,-5.253594,-0.7770416,0.03900363,-0.6282396,68.52145 +-0.6341159,0.02218777,0.7729197,96.72259,0.04717956,0.998836,0.01003384,-5.262948,-0.7717974,0.04282862,-0.6344245,67.92319 +-0.6382177,0.02404221,0.7694805,97.41618,0.05062572,0.9986594,0.0107868,-5.274783,-0.7681896,0.04583982,-0.6385792,67.33056 +-0.6406394,0.02584892,0.7674067,98.10389,0.05347434,0.9985085,0.01100771,-5.288861,-0.7659776,0.04808853,-0.6410661,66.74088 +-0.6414842,0.02722138,0.7666532,98.78369,0.05542143,0.9984033,0.01092287,-5.300088,-0.7651317,0.04949585,-0.6419685,66.16141 +-0.6407053,0.02956684,0.7672175,99.45129,0.05908683,0.9981936,0.0108754,-5.311916,-0.76551,0.05230036,-0.6412948,65.59079 +-0.6384371,0.03053163,0.7690683,100.1077,0.06147774,0.9980432,0.01141352,-5.322308,-0.7672148,0.05456738,-0.6390647,65.03401 +-0.6348862,0.03097431,0.7719846,100.7513,0.0629235,0.9979497,0.01170806,-5.332047,-0.7700391,0.05600924,-0.6355334,64.49082 +-0.6301973,0.03218236,0.7757678,101.3805,0.0653956,0.9977904,0.01173142,-5.343843,-0.7736761,0.0581249,-0.6309094,63.96662 +-0.6246871,0.03291404,0.7801812,101.9969,0.0670939,0.9976788,0.01163197,-5.356329,-0.7779874,0.05961173,-0.6254454,63.45978 +-0.6195979,0.03275934,0.7842355,102.6011,0.0675466,0.9976476,0.0116922,-5.366131,-0.7820076,0.06021689,-0.6203531,62.97546 +-0.6147912,0.03165442,0.7880545,103.1925,0.06636922,0.9977265,0.01170064,-5.375732,-0.7858924,0.059496,-0.6154943,62.51471 +-0.6104187,0.03060823,0.7914873,103.7649,0.06594865,0.9977475,0.01227689,-5.381607,-0.7893287,0.05969155,-0.6110623,62.07033 +-0.6070105,0.02997522,0.7941283,104.3198,0.06577074,0.997755,0.01261206,-5.387971,-0.7919674,0.05988604,-0.6076192,61.6421 +-0.6046072,0.03148064,0.7959015,104.8666,0.0654604,0.9978024,0.01026051,-5.395081,-0.7938293,0.05830359,-0.6053392,61.22103 +-0.604209,0.03377057,0.79611,105.4086,0.06501546,0.9978596,0.007014881,-5.405058,-0.7941691,0.0559979,-0.6051113,60.80105 +-0.6056417,0.0387658,0.7947927,105.9435,0.06615622,0.9978077,0.00174402,-5.41745,-0.7929827,0.05363672,-0.6068785,60.38059 +-0.6080864,0.04473539,0.7926095,106.472,0.06933534,0.9975885,-0.003110808,-5.42823,-0.7908373,0.05306419,-0.6097217,59.95676 +-0.6114291,0.04641389,0.7899368,107.0032,0.07145501,0.9974383,-0.003298161,-5.431921,-0.7880663,0.05442834,-0.6131793,59.53031 +-0.6155426,0.0429811,0.7869307,107.5343,0.07223474,0.9973856,0.002026629,-5.43395,-0.7847862,0.0580912,-0.617038,59.10283 +-0.6197251,0.03451389,0.7840597,108.0659,0.07043828,0.9974467,0.01176772,-5.438101,-0.7816516,0.06252055,-0.6205738,58.67499 +-0.6244219,0.02769098,0.7805963,108.5895,0.06885058,0.9974326,0.01969253,-5.44414,-0.7780468,0.06604094,-0.6247253,58.24739 +-0.6295767,0.02597734,0.776504,109.1088,0.06580914,0.9976321,0.01998194,-5.44724,-0.7741462,0.06368121,-0.6297954,57.82387 +-0.6335674,0.02932711,0.7731315,109.621,0.06433075,0.9978178,0.01486778,-5.443059,-0.7710083,0.05915585,-0.6340715,57.40047 +-0.6364415,0.0332185,0.7706094,110.1347,0.06322953,0.9979566,0.009202148,-5.438721,-0.768729,0.05458189,-0.6372413,56.97052 +-0.6387524,0.03424343,0.76865,110.6568,0.06092352,0.9981234,0.006161273,-5.438021,-0.7669965,0.05076438,-0.6396399,56.53321 +-0.6404581,0.03276151,0.7672941,111.184,0.05830311,0.9982806,0.006041361,-5.441619,-0.7657768,0.04860486,-0.641267,56.09051 +-0.64195,0.03244187,0.7660599,111.7175,0.05696712,0.9983611,0.005458226,-5.449228,-0.7646273,0.04714412,-0.642746,55.63752 +-0.6427054,0.03282606,0.7654099,112.255,0.05569933,0.9984398,0.003950048,-5.453527,-0.764086,0.04517153,-0.6435309,55.18251 +-0.6422485,0.03355497,0.7657617,112.7974,0.0572517,0.9983506,0.004270491,-5.457903,-0.7643553,0.04658386,-0.6431102,54.71893 +-0.6411919,0.0296379,0.766808,113.3534,0.05816615,0.9982563,0.01005393,-5.460142,-0.7651729,0.05104876,-0.6417977,54.24711 +-0.6381135,0.02273562,0.7696066,113.9193,0.05729589,0.9981946,0.0180179,-5.461816,-0.7678075,0.05559275,-0.6382641,53.77514 +-0.6327678,0.0208252,0.7740616,114.495,0.05919078,0.9980143,0.02153597,-5.458762,-0.772076,0.05944456,-0.6327439,53.30169 +-0.6262972,0.01863611,0.7793616,115.0778,0.05845864,0.9980222,0.02311279,-5.455029,-0.7773895,0.06003588,-0.6261479,52.83118 +-0.6191735,0.02178062,0.7849521,115.6689,0.05728985,0.9982043,0.01749258,-5.448047,-0.7831615,0.05580072,-0.6193095,52.3668 +-0.6125545,0.01836904,0.7902149,116.2817,0.05288585,0.9984421,0.01778634,-5.43495,-0.7886571,0.05268628,-0.6125716,51.90196 +-0.606078,0.01450264,0.7952731,116.9042,0.04672667,0.9987562,0.01739704,-5.427523,-0.7940315,0.04770441,-0.6060017,51.43942 +-0.6007378,0.009456598,0.7993903,117.5404,0.04005683,0.9990301,0.0182842,-5.429666,-0.798442,0.04300504,-0.6005338,50.97277 +-0.5971841,0.0097662,0.8020448,118.1849,0.03129052,0.9994484,0.01112828,-5.428109,-0.8014936,0.03174202,-0.5971602,50.51552 +-0.5950455,0.009997867,0.8036298,118.8368,0.02145527,0.9997638,0.00344854,-5.424749,-0.8034055,0.01929413,-0.5951195,50.05485 +-0.5929657,0.007668099,0.8051912,119.5011,0.01153652,0.9999329,-0.001026877,-5.427672,-0.8051451,0.008680197,-0.5930143,49.58858 +-0.591173,0.007255966,0.8065122,120.1732,0.01151324,0.9999335,-0.0005569393,-5.435032,-0.8064626,0.008956314,-0.5912172,49.10054 +-0.5891582,0.005830784,0.8079967,120.8536,0.0128947,0.9999144,0.002186549,-5.44247,-0.8079148,0.01170709,-0.5891829,48.60209 +-0.5880861,0.002111739,0.8087956,121.5468,0.01094391,0.9999258,0.005346679,-5.448708,-0.8087243,0.01199569,-0.5880655,48.10172 +-0.5873617,0.0006297722,0.8093244,122.2422,0.009644464,0.9999341,0.006221301,-5.453884,-0.8092671,0.01145965,-0.587329,47.59932 +-0.5872525,0.004395187,0.8093919,122.938,0.01364042,0.999897,0.004467093,-5.457842,-0.8092889,0.01366375,-0.5872519,47.0837 +-0.5887474,0.008776894,0.8082695,123.643,0.01626387,0.9998672,0.0009892398,-5.464293,-0.8081534,0.01372799,-0.588812,46.56055 +-0.5907008,0.006821827,0.8068619,124.3646,0.01476423,0.9998882,0.002355008,-5.474036,-0.8067556,0.01330379,-0.5907354,46.02841 +-0.5918372,0.005311517,0.8060401,125.0927,0.01606406,0.9998574,0.005206365,-5.480393,-0.8058974,0.01602959,-0.5918381,45.4853 +-0.5930751,0.003112271,0.8051412,125.8296,0.01808709,0.9997917,0.009458428,-5.486769,-0.804944,0.02017221,-0.5930078,44.9311 +-0.5943485,0.002346666,0.8042042,126.5762,0.01555241,0.9998422,0.008576488,-5.488875,-0.8040572,0.01760473,-0.5942912,44.37813 +-0.5962034,0.0003975874,0.8028334,127.33,0.01082895,0.9999129,0.007546639,-5.493653,-0.8027604,0.01319317,-0.5961557,43.82078 +-0.5974831,0.000426087,0.8018814,128.0916,0.01043333,0.9999193,0.007242565,-5.497699,-0.8018136,0.0126936,-0.5974393,43.25051 +-0.5990944,-0.001270129,0.8006775,128.8568,0.01105093,0.9998904,0.009854817,-5.501943,-0.8006022,0.01475219,-0.5990146,42.67077 +-0.6005203,-0.007926374,0.7995703,129.6376,0.007978514,0.9998417,0.01590401,-5.504398,-0.7995697,0.01593006,-0.6003619,42.08783 +-0.6018413,-0.01437867,0.7984863,130.4148,0.006820256,0.9997089,0.02314277,-5.50481,-0.7985866,0.01937415,-0.6015679,41.50532 +-0.6020677,-0.02036483,0.7981853,131.1944,0.004375011,0.9995755,0.02880313,-5.500901,-0.7984331,0.0208335,-0.601723,40.92278 +-0.6026707,-0.01924382,0.797758,131.9671,0.001043889,0.9996893,0.02490349,-5.49677,-0.7979893,0.01584137,-0.6024633,40.34476 +-0.6025785,-0.01705859,0.7978773,132.7406,-0.002998241,0.9998128,0.0191116,-5.491393,-0.798054,0.009124008,-0.6025168,39.76902 +-0.6017541,-0.0163298,0.7985145,133.518,-0.008097235,0.9998643,0.01434543,-5.486478,-0.7986403,0.002166657,-0.6018046,39.194 +-0.6000987,-0.01774791,0.7997291,134.2979,-0.00829637,0.9998381,0.01596339,-5.490212,-0.7998829,0.002944759,-0.6001488,38.60985 +-0.5979382,-0.01681949,0.8013658,135.0764,-0.005643891,0.9998434,0.01677405,-5.495403,-0.8015223,0.005507022,-0.5979394,38.02133 +-0.5959977,-0.0119451,0.8028973,135.848,0.004140507,0.9998303,0.0179485,-5.497469,-0.8029755,0.01402166,-0.595847,37.43039 +-0.591764,-0.007285497,0.8060784,136.6219,0.008914666,0.9998388,0.01558123,-5.500053,-0.806062,0.01640633,-0.5916036,36.84905 +-0.5879279,-0.001979461,0.8089109,137.3969,0.01512022,0.9997954,0.01343615,-5.505042,-0.808772,0.02013039,-0.5877776,36.27032 +-0.5847057,0.004205512,0.8112346,138.166,0.02525042,0.9995964,0.0130175,-5.511037,-0.8108524,0.02809542,-0.5845758,35.69148 +-0.5837786,0.003292868,0.8119063,138.9499,0.02325411,0.9996493,0.01266591,-5.516939,-0.8115798,0.02627423,-0.5836504,35.13111 +-0.5848985,-0.0001504781,0.8111065,139.7334,0.02041388,0.9996805,0.01490615,-5.518711,-0.8108495,0.02527641,-0.5847085,34.57697 +-0.5859899,-0.006414322,0.810293,140.5139,0.01785298,0.9996237,0.02082403,-5.515813,-0.8101217,0.02666881,-0.5856548,34.02156 +-0.5873445,-0.01306549,0.8092316,141.2935,0.01129582,0.9996399,0.02433829,-5.517039,-0.8092582,0.02343588,-0.5869854,33.46473 +-0.5891969,-0.01254556,0.8078921,142.0627,0.01041821,0.9996783,0.02312177,-5.515589,-0.8079223,0.02204006,-0.5888766,32.90818 +-0.590873,-0.01448276,0.8066347,142.8327,0.007467858,0.9996978,0.02341944,-5.512398,-0.80673,0.01986174,-0.5905862,32.34819 +-0.5923399,-0.01802965,0.8054865,143.6039,-0.0005490664,0.9997584,0.02197437,-5.509807,-0.805688,0.01257402,-0.5922066,31.79564 +-0.5945166,-0.01430203,0.8039562,144.3637,0.002312001,0.9998072,0.01949583,-5.515093,-0.80408,0.01344934,-0.5943689,31.23472 +-0.5956829,-0.009967938,0.8031579,145.1188,0.001589264,0.9999064,0.01358848,-5.521195,-0.8032181,0.009370852,-0.5956112,30.67549 +-0.5980831,-0.006236022,0.8014099,145.8718,-0.001778438,0.9999776,0.006453903,-5.53085,-0.8014321,0.00243471,-0.5980807,30.12158 +-0.5993217,-0.003351106,0.8005013,146.6232,0.003280699,0.9999725,0.006642339,-5.542701,-0.8005016,0.006607098,-0.5992942,29.5517 +-0.5994469,-0.008527358,0.8003691,147.3785,0.00538656,0.9998776,0.0146873,-5.549202,-0.8003964,0.01311549,-0.5993276,28.98277 +-0.5998303,-0.01274602,0.8000257,148.1333,0.003900797,0.9998146,0.01885373,-5.558442,-0.8001177,0.01442977,-0.5996694,28.4182 +-0.5995356,-0.009377041,0.8002932,148.8767,0.008878261,0.9997919,0.01836567,-5.56361,-0.8002988,0.01811608,-0.5993276,27.85319 +-0.5989863,-0.006029893,0.8007366,149.6167,0.007508305,0.9998854,0.0131461,-5.566991,-0.8007241,0.01388651,-0.5988723,27.299 +-0.5987928,-0.003264638,0.8008973,150.3476,0.006496807,0.999939,0.008933322,-5.568938,-0.8008776,0.01055248,-0.598735,26.75099 +-0.5978793,-0.003403618,0.8015789,151.0728,0.003822013,0.9999675,0.007096748,-5.577653,-0.801577,0.00730664,-0.5978468,26.20853 +-0.5980526,-0.01036033,0.8013899,151.7983,-0.0007254328,0.999923,0.01238558,-5.586083,-0.8014565,0.006825871,-0.598014,25.67277 +-0.5985356,-0.01248056,0.800999,152.5061,0.002474445,0.999845,0.01742781,-5.591898,-0.8010924,0.01241319,-0.5984119,25.13714 +-0.5994424,-0.01445651,0.8002874,153.2005,0.004174724,0.9997668,0.02118694,-5.599003,-0.800407,0.01604133,-0.5992422,24.61232 +-0.6014541,-0.006338801,0.7988822,153.8709,0.005899078,0.999906,0.01237507,-5.607533,-0.7988855,0.0121557,-0.6013602,24.10306 +-0.6037688,0.005012884,0.7971438,154.5212,0.00793236,0.9999685,-0.0002802797,-5.616276,-0.79712,0.006154003,-0.6037895,23.60351 +-0.6069117,0.009582125,0.7947115,155.1614,0.01295244,0.9999137,-0.002164707,-5.627315,-0.7946637,0.008979666,-0.6069834,23.09991 +-0.6110028,0.003254121,0.7916218,155.7951,0.01530337,0.9998532,0.0077016,-5.641768,-0.7914805,0.01682018,-0.6109629,22.59936 +-0.6155539,-0.005603978,0.7880749,156.4098,0.01801482,0.9996134,0.02117933,-5.661844,-0.7878888,0.02723403,-0.6152149,22.1043 +-0.619171,-0.01354336,0.7851394,157.0048,0.01505868,0.9994626,0.02911581,-5.675327,-0.7851117,0.02985083,-0.6186343,21.63112 +-0.6232684,-0.0077706,0.7819694,157.5671,0.01680231,0.9995867,0.02332538,-5.68306,-0.7818274,0.02767686,-0.6228802,21.17512 +-0.6257394,0.0005539719,0.780032,158.1026,0.0194767,0.999699,0.01491416,-5.694254,-0.779789,0.02452482,-0.6255618,20.72955 +-0.6278879,0.01022796,0.7782366,158.6182,0.02218869,0.9997424,0.004762919,-5.705713,-0.7779874,0.02025863,-0.6279531,20.30073 +-0.6293214,0.01393136,0.7770203,159.1201,0.02631197,0.999648,0.003387581,-5.720431,-0.7766996,0.0225768,-0.6294664,19.88174 +-0.6265234,0.01604992,0.7792374,159.6057,0.03296437,0.999439,0.005918632,-5.737221,-0.7787052,0.02939522,-0.6267009,19.47009 +-0.6207529,0.0191433,0.7837725,160.0792,0.04204697,0.999076,0.008899472,-5.755764,-0.7828779,0.03847963,-0.6209842,19.06944 +-0.6088696,0.02106548,0.7929906,160.5555,0.04829897,0.9987772,0.01055251,-5.769695,-0.7917986,0.04472572,-0.6091425,18.69011 +-0.5911877,0.02650757,0.8060983,161.0344,0.05676595,0.9983487,0.008802305,-5.781742,-0.8045339,0.05096274,-0.5917161,18.32864 +-0.5676195,0.03275383,0.8226393,161.5121,0.06590899,0.9978091,0.005748746,-5.794799,-0.8206486,0.05748241,-0.5685346,17.98182 +-0.5375079,0.03614787,0.8424836,162.0108,0.07232769,0.9973753,0.003351643,-5.806721,-0.8401511,0.06273641,-0.5387116,17.66252 +-0.5028576,0.03473612,0.863671,162.5275,0.07546731,0.9971409,0.003835366,-5.819248,-0.8610684,0.06710756,-0.5040413,17.36993 +-0.4658409,0.03012666,0.8843555,163.0425,0.07073727,0.9974896,0.003280664,-5.828843,-0.8820365,0.06408515,-0.4668025,17.11344 +-0.4266461,0.02851108,0.9039692,163.5796,0.06671331,0.9977722,1.703684E-05,-5.84466,-0.9019547,0.06031403,-0.4275977,16.89122 +-0.3873896,0.03038864,0.9214152,164.102,0.06510685,0.9978629,-0.005537126,-5.863518,-0.9196143,0.0578454,-0.3885401,16.69004 +-0.3474703,0.03535677,0.9370242,164.6402,0.06675743,0.9976859,-0.01289053,-5.887082,-0.9353116,0.05807424,-0.3490266,16.51101 +-0.3065741,0.04045207,0.9509869,165.1817,0.06635851,0.9975739,-0.02104145,-5.910052,-0.9495308,0.05665529,-0.3085146,16.3598 +-0.2657549,0.04126525,0.9631571,165.7257,0.0644396,0.9976094,-0.02496111,-5.932357,-0.9618845,0.05543191,-0.2677787,16.23124 +-0.223493,0.03892703,0.9739279,166.302,0.06197244,0.997748,-0.02565792,-5.954339,-0.9727334,0.05462231,-0.2254021,16.12414 +-0.182375,0.03437639,0.9826279,166.8844,0.0598063,0.9979259,-0.02381158,-5.978445,-0.9814084,0.05442469,-0.1840526,16.03917 +-0.1450693,0.02702584,0.9890524,167.4654,0.05321185,0.9983933,-0.01947624,-6.003373,-0.9879895,0.04980388,-0.1462743,15.98023 +-0.1119875,0.01966634,0.993515,168.0665,0.04742017,0.9987708,-0.01442526,-6.028742,-0.9925775,0.04549719,-0.1127824,15.94135 +-0.08204748,0.01775142,0.9964703,168.6674,0.04313046,0.9989679,-0.01424464,-6.043175,-0.9956947,0.04180947,-0.08272841,15.92211 +-0.05689049,0.0157999,0.9982554,169.2753,0.0378204,0.9991912,-0.01365934,-6.055693,-0.9976638,0.03697732,-0.05744202,15.91281 +-0.03924801,0.01137962,0.9991647,169.8988,0.02843079,0.999543,-0.01026715,-6.074099,-0.9988249,0.02800406,-0.0395536,15.92521 +-0.02594042,0.009355596,0.9996197,170.5258,0.01604419,0.9998313,-0.008941231,-6.093598,-0.9995347,0.01580614,-0.02608614,15.9418 +-0.01793801,0.01152393,0.9997727,171.1572,0.009455914,0.9998908,-0.01135564,-6.110564,-0.9997943,0.009250062,-0.01804502,15.95774 +-0.01367089,0.01412014,0.9998069,171.794,0.002975861,0.9998964,-0.01408072,-6.127817,-0.9999021,0.002782786,-0.01371149,15.96664 +-0.01235513,0.01547868,0.9998039,172.4402,0.0007608754,0.99988,-0.01547047,-6.147256,-0.9999233,0.0005695831,-0.01236542,15.97113 +-0.01364737,0.01461402,0.9998001,173.0987,0.0007518024,0.999893,-0.01460513,-6.165077,-0.9999065,0.0005523269,-0.0136569,15.97268 +-0.01750263,0.01291904,0.9997634,173.7678,0.00047894,0.9999165,-0.01291264,-6.181189,-0.9998467,0.0002528181,-0.01750736,15.96818 +-0.02219561,0.008715634,0.9997157,174.444,2.655289E-05,0.999962,-0.008717198,-6.197603,-0.9997536,-0.0001669415,-0.02219499,15.95901 +-0.02809249,0.008390768,0.9995701,175.1205,-0.000588511,0.9999644,-0.008410624,-6.220298,-0.9996051,-0.0008245365,-0.02808654,15.94518 +-0.0337954,0.01624204,0.9992968,175.7972,-0.001309588,0.9998663,-0.01629559,-6.244754,-0.9994279,-0.001859386,-0.0337696,15.92832 +-0.04035256,0.02582447,0.9988518,176.477,-0.00153847,0.9996631,-0.02590761,-6.267177,-0.9991843,-0.002582145,-0.04029923,15.90335 +-0.04724558,0.03243961,0.9983564,177.1672,-0.0002663024,0.9994721,-0.03248847,-6.287121,-0.9988832,-0.001800804,-0.047212,15.87215 +-0.05327461,0.02996182,0.9981303,177.8705,0.0002150486,0.9995501,-0.02999297,-6.309818,-0.9985798,-0.00138322,-0.05325708,15.83326 +-0.0607074,0.02283224,0.9978945,178.5853,-0.001663789,0.9997346,-0.02297556,-6.342144,-0.9981542,-0.003055075,-0.06065329,15.79677 +-0.06810493,0.0152656,0.9975614,179.3053,-0.006842321,0.9998522,-0.0157678,-6.377428,-0.9976547,-0.007899502,-0.06799041,15.75628 +-0.07440709,0.01435313,0.9971247,180.0295,-0.01031632,0.9998318,-0.01516192,-6.404246,-0.9971745,-0.01141481,-0.07424649,15.70949 +-0.0796381,0.0179742,0.9966618,180.7506,-0.01249555,0.9997408,-0.01902819,-6.426161,-0.9967455,-0.01396921,-0.07939286,15.65927 +-0.08314285,0.01972672,0.9963424,181.477,-0.01101138,0.9997248,-0.02071258,-6.448627,-0.9964768,-0.01269321,-0.08290275,15.60401 +-0.08512564,0.02288512,0.9961074,182.2109,-0.002261324,0.9997292,-0.02316159,-6.474966,-0.9963676,-0.004224169,-0.08505082,15.53203 +-0.08627319,0.02204272,0.9960277,182.9538,0.001488543,0.9997569,-0.02199633,-6.499988,-0.9962704,-0.0004150668,-0.08628502,15.46437 +-0.08681398,0.0196444,0.9960309,183.7037,-0.003503155,0.9997933,-0.02002395,-6.527412,-0.9962183,-0.005227611,-0.08672722,15.41202 +-0.08522385,0.01596745,0.9962339,184.4609,-0.009026224,0.9998182,-0.01679706,-6.552659,-0.9963209,-0.01042374,-0.08506422,15.36267 +-0.08276166,0.01488265,0.9964583,185.219,-0.00619795,0.9998614,-0.01544826,-6.574898,-0.9965501,-0.007454524,-0.08265794,15.30234 +-0.081185,0.01556693,0.9965775,185.9807,-0.001459851,0.9998751,-0.01573737,-6.598723,-0.9966979,-0.002732496,-0.08115213,15.23813 +-0.08100125,0.01426118,0.996612,186.7415,-0.003763366,0.9998861,-0.01461391,-6.62411,-0.9967069,-0.004934362,-0.08093835,15.18121 +-0.08097337,0.01207882,0.9966431,187.5112,-0.007506419,0.9998908,-0.01272806,-6.64292,-0.996688,-0.008511856,-0.08087385,15.13049 +-0.07958189,0.008273964,0.996794,188.2805,-0.01042908,0.9999039,-0.009132419,-6.660596,-0.9967737,-0.01112242,-0.07948794,15.07299 +-0.07805432,0.005314473,0.996935,189.0477,-0.01125534,0.9999173,-0.006211606,-6.678413,-0.9968855,-0.01170568,-0.07798804,15.01701 +-0.077333,0.007428141,0.9969777,189.8139,-0.01021435,0.9999138,-0.008242325,-6.696543,-0.996953,-0.01082089,-0.07725046,14.95921 +-0.07623648,0.0121235,0.9970161,190.5775,-0.005266648,0.9999072,-0.01256137,-6.715856,-0.9970758,-0.00620857,-0.07616555,14.8968 +-0.07637619,0.01621743,0.9969472,191.3435,-0.005613517,0.9998449,-0.01669463,-6.731426,-0.9970632,-0.006871454,-0.0762733,14.8327 +-0.0777819,0.0189957,0.9967894,192.1137,-0.01021884,0.9997507,-0.01984954,-6.744713,-0.996918,-0.01172997,-0.07756839,14.77624 +-0.07935577,0.01536882,0.9967279,192.8943,-0.01118727,0.9998044,-0.01630696,-6.763373,-0.9967835,-0.01244472,-0.07916831,14.71461 +-0.08018055,0.01214262,0.9967064,193.6769,-0.008005828,0.9998857,-0.01282539,-6.782223,-0.9967482,-0.009007809,-0.08007417,14.64361 +-0.08146299,0.0138016,0.9965808,194.461,-0.01005176,0.9998419,-0.01466842,-6.800695,-0.9966256,-0.01121233,-0.08131137,14.57804 +-0.08427518,0.01955703,0.9962506,195.2442,-0.01375677,0.9996892,-0.02078826,-6.821679,-0.9963475,-0.01545713,-0.08397994,14.51022 +-0.08680915,0.02338049,0.9959506,196.0363,-0.01361029,0.9996034,-0.02465255,-6.843719,-0.9961319,-0.01569525,-0.0864565,14.43769 +-0.09004595,0.02556518,0.9956095,196.8309,-0.014239,0.9995352,-0.02695382,-6.870881,-0.9958358,-0.01660357,-0.08964007,14.36297 +-0.09329235,0.02640049,0.9952887,197.6335,-0.01245398,0.9995392,-0.02768061,-6.899957,-0.9955608,-0.01497769,-0.09292057,14.28382 +-0.09461977,0.02241014,0.9952612,198.4476,-0.01305399,0.9996327,-0.02374962,-6.926056,-0.9954279,-0.01523931,-0.09429247,14.2027 +-0.09637709,0.02395552,0.9950566,199.2633,-0.007150833,0.9996678,-0.02475914,-6.95111,-0.9953192,-0.009501699,-0.09617377,14.11607 +-0.09653529,0.02113942,0.9951051,200.09,-0.003384296,0.9997617,-0.02156666,-6.975899,-0.9953238,-0.005449676,-0.09644074,14.03013 +-0.09662727,0.01657852,0.9951826,200.9222,-0.00335445,0.9998501,-0.01698198,-7.003243,-0.9953149,-0.004979215,-0.09655717,13.95074 +-0.0970331,0.01640242,0.995146,201.7552,0.0004259526,0.9998648,-0.01643867,-7.033032,-0.995281,-0.001171213,-0.09702695,13.8698 +-0.09647752,0.01548902,0.9952147,202.5921,0.00222968,0.9998797,-0.01534548,-7.057848,-0.9953326,0.0007385126,-0.09650044,13.7853 +-0.09616667,0.01696433,0.9952207,203.4268,0.005089313,0.99985,-0.01655148,-7.080672,-0.9953522,0.003473285,-0.09623857,13.70436 +-0.09489924,0.01833236,0.9953181,204.2626,0.01170632,0.9997818,-0.01729844,-7.105891,-0.995418,0.0100099,-0.09509313,13.61782 +-0.09292034,0.01850693,0.9955016,205.1049,0.01433271,0.9997485,-0.01724807,-7.133726,-0.9955703,0.01266553,-0.09316222,13.53411 +-0.09073915,0.01826601,0.9957072,205.9506,0.009394459,0.999803,-0.01748503,-7.160891,-0.9958303,0.007767548,-0.09089286,13.45887 +-0.08754861,0.01638025,0.9960256,206.796,0.005632082,0.9998569,-0.01594822,-7.183536,-0.9961443,0.00421345,-0.08762834,13.3891 +-0.08477974,0.0139836,0.9963016,207.6407,0.008420279,0.9998758,-0.01331725,-7.212206,-0.9963641,0.0072601,-0.08488695,13.31007 +-0.08234073,0.01993588,0.9964048,208.4765,0.009781573,0.9997679,-0.01919484,-7.243658,-0.9965562,0.008165884,-0.08251661,13.23858 +-0.08126266,0.03269127,0.9961565,209.3057,0.008148951,0.9994503,-0.03213461,-7.272696,-0.9966594,0.005506282,-0.08148438,13.17348 +-0.07958072,0.03306522,0.9962799,210.1489,0.005615453,0.9994487,-0.03272185,-7.30156,-0.9968126,0.002990531,-0.07972252,13.11109 +-0.07899809,0.02151984,0.9966425,211.0003,0.001925666,0.9997684,-0.02143471,-7.337463,-0.9968729,0.0002258957,-0.07902123,13.04697 +-0.07987383,0.00889202,0.9967653,211.8545,0.0009467885,0.9999604,-0.00884466,-7.375526,-0.9968045,0.0002372657,-0.07987908,12.97973 +-0.0811428,0.008917724,0.9966626,212.6973,0.001023606,0.9999602,-0.008863899,-7.402686,-0.9967019,0.0003009449,-0.08114869,12.91574 +-0.08279248,0.0155664,0.9964453,213.5357,-0.001949911,0.9998735,-0.01578197,-7.424758,-0.9965649,-0.003249611,-0.08275165,12.85506 +-0.08414544,0.02117115,0.9962286,214.376,-0.009954508,0.9997065,-0.02208587,-7.447524,-0.9964037,-0.01177539,-0.08390999,12.80054 +-0.08541943,0.02164057,0.9961101,215.2247,-0.01272836,0.9996588,-0.02280917,-7.474428,-0.9962637,-0.01462719,-0.08511483,12.73783 +-0.08580104,0.01676606,0.9961712,216.0829,-0.009461199,0.9997996,-0.01764203,-7.501955,-0.9962673,-0.01093868,-0.08562521,12.66143 +-0.0861526,0.01028798,0.9962289,216.9447,-0.007685555,0.99991,-0.01099063,-7.527571,-0.9962523,-0.008603445,-0.08606577,12.58792 +-0.08674103,0.007002636,0.9962063,217.808,-0.005114517,0.999959,-0.007474348,-7.551199,-0.9962177,-0.005743449,-0.08670164,12.51318 +-0.0863315,0.01099684,0.9962058,218.6649,-0.0009504835,0.9999377,-0.01112041,-7.577625,-0.996266,-0.001906922,-0.08631566,12.43313 +-0.08567454,0.01744821,0.9961704,219.5233,0.002014139,0.9998476,-0.0173394,-7.604253,-0.9963211,0.000520877,-0.08569661,12.35733 +-0.08464285,0.02219382,0.9961642,220.3895,0.005542556,0.9997469,-0.02180271,-7.631751,-0.9963959,0.003675848,-0.08474443,12.2827 +-0.08303465,0.02009378,0.9963441,221.2664,0.008270822,0.9997761,-0.01947372,-7.667763,-0.9965123,0.006623586,-0.08318225,12.20917 +-0.08240877,0.01478314,0.996489,222.1519,0.01084376,0.9998441,-0.01393615,-7.701483,-0.9965396,0.009657217,-0.08255621,12.13701 +-0.08200145,0.01273098,0.9965509,223.0375,0.01199619,0.9998586,-0.01178613,-7.731282,-0.99656,0.01098833,-0.08214256,12.06855 +-0.08191883,0.01286426,0.996556,223.9226,0.004300428,0.9999119,-0.01255409,-7.757379,-0.9966297,0.003257197,-0.08196693,12.00953 +-0.0820399,0.01364394,0.9965357,224.8091,0.0002885113,0.9999065,-0.01366635,-7.78435,-0.996629,-0.000833677,-0.08203616,11.94495 +-0.08237865,0.01245217,0.9965233,225.6999,-0.0003003953,0.9999216,-0.01251947,-7.812128,-0.996601,-0.001330691,-0.08236844,11.87296 +-0.08397316,0.01294523,0.996384,226.5911,-0.004592499,0.9998999,-0.01337796,-7.846308,-0.9964574,-0.005699284,-0.0839053,11.80622 +-0.0859581,0.01318869,0.9962115,227.4843,-0.007654405,0.9998741,-0.01389764,-7.873427,-0.9962693,-0.008820022,-0.08584631,11.73678 +-0.08790897,0.01154202,0.9960617,228.3808,-0.004411111,0.9999185,-0.01197603,-7.899172,-0.9961187,-0.005446541,-0.08785088,11.65757 +-0.08977953,0.00722988,0.9959355,229.2805,-0.001207309,0.9999721,-0.007368024,-7.929607,-0.9959609,-0.001863903,-0.08976828,11.57203 +-0.09091573,0.01203973,0.9957858,230.1748,-0.001097474,0.9999251,-0.01218998,-7.962599,-0.9958579,-0.002201114,-0.0908957,11.49123 +-0.09237283,0.01924013,0.9955386,231.0638,0.005431961,0.9998081,-0.01881863,-7.987456,-0.9957096,0.003669393,-0.09245961,11.40077 +-0.09294313,0.02204973,0.9954273,231.9589,0.008431966,0.9997363,-0.02135789,-8.014843,-0.9956357,0.006408335,-0.09310454,11.31023 +-0.09467232,0.01504487,0.9953948,232.8634,0.002149084,0.9998865,-0.01490836,-8.047081,-0.9955061,0.0007277748,-0.0946939,11.23237 +-0.09673557,0.004960678,0.9952978,233.7688,-0.00532883,0.9999706,-0.005501897,-8.077315,-0.9952958,-0.005836004,-0.09670629,11.15712 +-0.09808272,0.001244037,0.9951775,234.6716,-0.008816435,0.9999589,-0.002118949,-8.101966,-0.9951392,-0.008981752,-0.09806771,11.076 +-0.100482,0.004490813,0.9949288,235.5637,-0.01037558,0.9999307,-0.005561269,-8.124246,-0.9948847,-0.01088177,-0.1004285,10.99112 +-0.1007824,0.01044428,0.9948537,236.4527,-0.01509232,0.9998138,-0.01202526,-8.151328,-0.994794,-0.01622658,-0.100606,10.90857 +-0.1006063,0.01590387,0.9947992,237.3404,-0.01255094,0.9997724,-0.01725269,-8.183767,-0.9948471,-0.01422139,-0.1003837,10.816 +-0.1000779,0.02197418,0.9947369,238.2244,-0.007048972,0.9997153,-0.02279334,-8.217037,-0.9949546,-0.009292985,-0.09989455,10.71992 +-0.0994636,0.01936369,0.9948528,239.1154,-0.003587847,0.9997971,-0.01981864,-8.251307,-0.9950347,-0.005540615,-0.09937393,10.62832 +-0.09764598,0.01441209,0.9951169,240.0045,-0.002841663,0.999887,-0.01476002,-8.29203,-0.9952171,-0.004269046,-0.09759399,10.54401 +-0.09480073,0.01355055,0.9954041,240.8856,-0.004325245,0.9998923,-0.01402358,-8.332096,-0.9954868,-0.005634814,-0.0947319,10.46736 +-0.09288538,0.02030032,0.9954699,241.7562,-0.00448615,0.9997734,-0.02080668,-8.367849,-0.9956667,-0.006398466,-0.09277326,10.39098 +-0.09023279,0.02560115,0.9955916,242.6249,-0.003696055,0.999654,-0.0260406,-8.408269,-0.9959138,-0.00602948,-0.09010694,10.31952 +-0.08840812,0.02071551,0.9958689,243.5008,-0.003874415,0.999769,-0.0211406,-8.447662,-0.9960768,-0.005727412,-0.08830743,10.24877 +-0.08667361,0.01130192,0.9961727,244.3762,-0.002589973,0.9999297,-0.01156989,-8.492051,-0.9962334,-0.003582867,-0.08663824,10.17767 +-0.08457565,0.01231196,0.996341,245.2395,-0.001043194,0.999922,-0.01244477,-8.531326,-0.9964165,-0.002091905,-0.0845562,10.10603 +-0.08283668,0.02100874,0.9963417,246.0908,-0.003383793,0.9997661,-0.02136229,-8.565326,-0.9965573,-0.005140998,-0.0827462,10.04291 +-0.08105971,0.02764788,0.9963257,246.9424,-0.002692794,0.9996054,-0.02795799,-8.603928,-0.9967056,-0.004949169,-0.08095327,9.977031 +-0.07926717,0.02404408,0.9965634,247.7997,-4.799469E-05,0.999709,-0.02412379,-8.641954,-0.9968534,-0.001960058,-0.07924294,9.910329 +-0.07837681,0.01542385,0.9968045,248.6556,-0.003242125,0.9998711,-0.01572623,-8.680654,-0.9969185,-0.00446434,-0.07831669,9.850928 +-0.07663806,0.01042669,0.9970045,249.5031,-0.005292416,0.999927,-0.01086408,-8.716152,-0.9970449,-0.006109167,-0.07657727,9.79641 +-0.07410993,0.0121569,0.997176,250.3337,-0.00183142,0.9999223,-0.0123265,-8.745963,-0.9972484,-0.002739767,-0.0740819,9.735576 +-0.07168237,0.01527945,0.9973105,251.1539,0.00374112,0.9998797,-0.01504993,-8.776984,-0.9974205,0.00265224,-0.0717309,9.672126 +-0.06969719,0.0171624,0.9974206,251.9624,0.004065964,0.9998486,-0.01692007,-8.813807,-0.9975599,0.002876191,-0.06975641,9.616742 +-0.06828961,0.01703939,0.99752,252.7631,0.002461954,0.9998539,-0.01691072,-8.849214,-0.9976625,0.001301018,-0.06832158,9.56537 +-0.06695463,0.01364649,0.9976627,253.5514,0.002856412,0.999905,-0.01348547,-8.883502,-0.9977519,0.001946818,-0.06698724,9.508367 +-0.06664667,0.006915517,0.9977527,254.3298,0.003260745,0.9999721,-0.006713099,-8.91595,-0.9977713,0.002806007,-0.06666735,9.452561 +-0.0670362,0.002066923,0.9977484,255.0932,0.0004985503,0.9999978,-0.002038092,-8.949611,-0.9977504,0.0003607984,-0.06703708,9.401473 +-0.06727331,0.005398346,0.99772,255.8336,0.001585951,0.9999847,-0.005303669,-8.976456,-0.9977333,0.001225536,-0.06728083,9.34923 +-0.06759994,0.01274632,0.9976311,256.5572,0.001795611,0.9999183,-0.01265388,-9.002864,-0.9977108,0.0009359526,-0.0676173,9.297183 +-0.06901831,0.01759124,0.9974603,257.2723,0.001591304,0.9998452,-0.01752319,-9.027674,-0.9976141,0.0003778377,-0.06903561,9.248384 +-0.07057872,0.01949577,0.9973157,257.9792,0.00186868,0.9998098,-0.01941228,-9.05498,-0.9975044,0.0004935665,-0.07060172,9.197519 +-0.07362924,0.01812946,0.9971209,258.6815,0.00387048,0.9998324,-0.01789296,-9.085217,-0.9972781,0.002541888,-0.07368706,9.140396 +-0.07835569,0.01715726,0.9967778,259.3643,0.005094722,0.9998457,-0.01680958,-9.115183,-0.9969124,0.003761176,-0.07843101,9.079415 +-0.08809153,0.01653635,0.9959751,259.9257,0.008092081,0.9998411,-0.01588482,-9.134049,-0.9960795,0.006660188,-0.08821133,8.975124 +-0.09783723,0.01596976,0.9950743,260.4879,0.01109054,0.9998266,-0.0149556,-9.152938,-0.9951406,0.009572697,-0.09799737,8.870715 +-0.1075635,0.01545921,0.994078,261.0492,0.01408139,0.9998025,-0.01402458,-9.171786,-0.9940984,0.01248947,-0.1077599,8.76649 +-0.1172804,0.01500414,0.9929855,261.6101,0.01706801,0.9997686,-0.01309075,-9.190618,-0.9929521,0.01541299,-0.1175093,8.662332 +-0.1269908,0.01460445,0.9917964,262.171,0.02005159,0.999725,-0.01215378,-9.209445,-0.9917012,0.01834367,-0.1272487,8.558198 +-0.1366933,0.01426028,0.9905108,262.7319,0.02303201,0.9996718,-0.01121369,-9.228266,-0.9903456,0.02128062,-0.1369768,8.454091 +-0.1463855,0.01397178,0.989129,263.2926,0.02600884,0.9996089,-0.01027066,-9.247078,-0.9888856,0.02422261,-0.1466916,8.350028 +-0.1560623,0.01373911,0.9876517,263.8529,0.02898077,0.9995364,-0.009325096,-9.265872,-0.9873219,0.02716761,-0.1563881,8.246051 +-0.1657313,0.01356215,0.9860777,264.4134,0.03195048,0.9994543,-0.008376182,-9.284669,-0.9856532,0.03011745,-0.1660742,8.142069 +-0.1753698,0.01344122,0.9844109,264.9728,0.03491129,0.9993628,-0.007426045,-9.303418,-0.9838834,0.03306474,-0.1757273,8.038313 +-0.1850214,0.01337597,0.9826435,265.5337,0.03787687,0.9992614,-0.006470367,-9.322224,-0.9820042,0.0360223,-0.1853914,7.934305 +-0.1946288,0.01336673,0.9807859,266.0928,0.04082991,0.9991509,-0.005514671,-9.340956,-0.9800268,0.03897208,-0.1950093,7.83065 +-0.2042373,0.01341347,0.9788295,266.6529,0.04378468,0.9990306,-0.004554425,-9.359723,-0.9779417,0.04192755,-0.2046266,7.726849 +-0.2138163,0.01351607,0.9767804,267.2122,0.04673207,0.998901,-0.003592564,-9.378456,-0.9757554,0.04487881,-0.2142129,7.62322 +-0.2233809,0.01367469,0.9746353,267.7717,0.04967698,0.9987619,-0.002627529,-9.397193,-0.9734645,0.04782999,-0.2237836,7.51959 +-0.2328638,0.01389894,0.9724101,268.3296,0.05261463,0.9986135,-0.001673813,-9.41585,-0.971085,0.05077322,-0.2332722,7.415961 +-0.2356841,0.01510242,0.9717124,268.7197,0.05511818,0.9984775,-0.002149772,-9.429499,-0.9702654,0.05305233,-0.2361576,7.309828 +-0.236144,0.01508151,0.971601,269.0908,0.05698212,0.9983738,-0.001647804,-9.443289,-0.9700459,0.05497475,-0.2366194,7.211929 +-0.233491,0.01487578,0.9722452,269.4389,0.05939409,0.9982341,-0.001009554,-9.454256,-0.9705433,0.05750989,-0.2339622,7.121329 +-0.2286292,0.01538044,0.9733921,269.7686,0.0605921,0.9981614,-0.001540025,-9.463785,-0.9716261,0.05862776,-0.2291407,7.040605 +-0.2211366,0.01648228,0.9751036,270.0809,0.05992423,0.9981975,-0.003282872,-9.473729,-0.9734,0.05770635,-0.2217257,6.972944 +-0.2102571,0.01774915,0.977485,270.3777,0.0582977,0.9982836,-0.005586984,-9.484841,-0.9759064,0.05581041,-0.2109309,6.915676 +-0.1967017,0.02028628,0.9802535,270.6548,0.05577289,0.9983985,-0.00947018,-9.49823,-0.9788758,0.05280875,-0.1975181,6.869143 +-0.1806319,0.023514,0.9832697,270.9235,0.05556675,0.9983614,-0.013667,-9.511336,-0.9819798,0.05216839,-0.1816425,6.829242 +-0.1610958,0.02495492,0.9866233,271.1802,0.05531383,0.9983372,-0.01621958,-9.529927,-0.9853875,0.05196099,-0.1622083,6.803106 +-0.1389799,0.02353185,0.9900156,271.4334,0.05634238,0.9982862,-0.01581902,-9.544996,-0.9886911,0.05358129,-0.1400675,6.780906 +-0.1136676,0.02240003,0.9932663,271.689,0.05573097,0.9983154,-0.01613616,-9.559927,-0.9919545,0.05352152,-0.1147245,6.77571 +-0.08372953,0.02194371,0.9962469,271.9474,0.05303783,0.9984385,-0.01753443,-9.573521,-0.995076,0.05137061,-0.08476263,6.781844 +-0.05082579,0.02073706,0.9984922,272.1883,0.05065284,0.9985512,-0.01815994,-9.586484,-0.9974221,0.04965346,-0.05180254,6.79891 +-0.01621006,0.01693418,0.9997252,272.4487,0.04776928,0.9987279,-0.01614273,-9.599096,-0.9987268,0.04749447,-0.01699837,6.826436 +0.01994941,0.01227483,0.9997257,272.7085,0.04600685,0.9988541,-0.0131822,-9.612324,-0.9987419,0.04625719,0.01936182,6.863175 +0.05867973,0.009173971,0.9982347,272.9398,0.04097646,0.9990929,-0.0115906,-9.62631,-0.9974355,0.04158425,0.05825058,6.915555 +0.1003292,0.007991341,0.9949222,273.1906,0.03543775,0.9993045,-0.01160013,-9.643554,-0.994323,0.03642162,0.09997622,6.984555 +0.1460155,0.00832976,0.9892473,273.4483,0.02704353,0.9995572,-0.01240827,-9.662857,-0.9889125,0.02856453,0.1457256,7.056585 +0.1938408,0.009289321,0.9809891,273.6741,0.02296611,0.9996381,-0.01400396,-9.67712,-0.9807641,0.02524403,0.1935573,7.139266 +0.2438794,0.01129196,0.9697399,273.937,0.01940377,0.9996752,-0.01652038,-9.69374,-0.9696114,0.02284559,0.2435811,7.237626 +0.2953493,0.01334585,0.9552962,274.1601,0.01621396,0.9996884,-0.01897891,-9.711016,-0.9552517,0.02109454,0.2950409,7.353067 +0.3500849,0.01375018,0.9366171,274.4355,0.01903687,0.9995813,-0.02179006,-9.72674,-0.9365245,0.02545862,0.3496766,7.486302 +0.4058729,0.0142873,0.9138179,274.7081,0.02853173,0.9991923,-0.0282945,-9.734573,-0.913484,0.03755677,0.4051375,7.63491 +0.4609076,0.01267508,0.8873576,274.9354,0.03407831,0.9989077,-0.03196928,-9.744474,-0.8867935,0.04497453,0.4599722,7.808509 +0.5149155,0.01247167,0.8571503,275.2061,0.02881971,0.9990771,-0.03184958,-9.760267,-0.8567563,0.04110265,0.5140808,8.021776 +0.5677527,0.01598069,0.8230441,275.4581,0.02133431,0.9991901,-0.03411769,-9.77804,-0.8229227,0.03692947,0.5669518,8.264455 +0.6056124,0.01724098,0.795573,275.651,0.01671439,0.9992691,-0.03437877,-9.79447,-0.7955842,0.03411771,0.6048815,8.694219 +0.6421416,0.01871794,0.7663576,275.8414,0.01208548,0.9993304,-0.0345348,-9.810775,-0.7664908,0.03143802,0.6414853,9.123003 +0.6773074,0.0204043,0.7354172,276.03,0.007446072,0.999374,-0.03458555,-9.827008,-0.7356624,0.02890101,0.6767314,9.551308 +0.7109247,0.022284,0.7029151,276.2154,0.002815373,0.9993996,-0.0345307,-9.843049,-0.7032625,0.02652769,0.7104351,9.977662 +0.7430629,0.02435422,0.6687783,276.3997,-0.001821832,0.9994075,-0.03437023,-9.859061,-0.6692191,0.02432083,0.742667,10.40388 +0.7735546,0.02659749,0.6331714,276.5818,-0.006447541,0.9993975,-0.03410443,-9.874937,-0.6336969,0.02229923,0.7732599,10.8286 +0.8023024,0.0289981,0.596213,276.761,-0.01105189,0.9993697,-0.03373436,-9.890639,-0.5968154,0.02047588,0.8021172,11.25127 +0.8293099,0.0315481,0.5578978,276.9388,-0.01564075,0.9993243,-0.03326014,-9.906249,-0.5585701,0.01885702,0.8292429,11.6728 +0.8545088,0.03423353,0.5183077,277.115,-0.02020886,0.9992614,-0.03268252,-9.921765,-0.5190437,0.01745308,0.8545694,12.09308 +0.8778116,0.03703669,0.4775721,277.2893,-0.02474623,0.9991814,-0.03200322,-9.937146,-0.4783664,0.01627468,0.8780095,12.5115 +0.8991665,0.03994221,0.4357802,277.4617,-0.02924813,0.9990844,-0.03122378,-9.952392,-0.4366283,0.01532962,0.8995113,12.92799 +0.9185229,0.0429338,0.3930299,277.6324,-0.03370924,0.9989708,-0.03034608,-9.967501,-0.3939282,0.01462482,0.9190248,13.34241 +0.9358408,0.04599565,0.3494087,277.8014,-0.03812565,0.9988412,-0.02937196,-9.982477,-0.3503548,0.01416605,0.9365099,13.75473 +0.9510964,0.04911451,0.3049648,277.9693,-0.04249747,0.9986956,-0.02830248,-9.997356,-0.3059571,0.01395815,0.9519429,14.16535 +0.9642109,0.05226371,0.2599346,278.135,-0.04680684,0.9985351,-0.0271433,-10.01204,-0.2609724,0.01400515,0.9652446,14.57286 +0.9750681,0.05530532,0.2149039,278.2922,-0.05093218,0.9983678,-0.02583812,-10.02657,-0.2159821,0.0142484,0.9762933,14.98313 +0.9806319,0.05394348,0.1882849,278.402,-0.0506411,0.9984677,-0.02230955,-10.04318,-0.1891999,0.0123425,0.981861,15.60076 +0.9848867,0.05335417,0.1647774,278.5111,-0.05010966,0.9984604,-0.02378781,-10.07051,-0.1657929,0.01517136,0.9860438,16.2289 +0.9879243,0.05740584,0.1439105,278.5999,-0.05386267,0.9981444,-0.02840019,-10.10274,-0.1452738,0.02030582,0.989183,16.87504 +0.9900744,0.06355418,0.125354,278.6755,-0.05969482,0.9976268,-0.03431117,-10.13097,-0.1272371,0.02648762,0.9915185,17.54087 +0.9916821,0.06671441,0.1100719,278.7489,-0.06229849,0.9971271,-0.04308508,-10.14776,-0.1126301,0.03586939,0.9929893,18.22626 +0.9930479,0.06842705,0.09577925,278.8166,-0.06516723,0.9971969,-0.03676246,-10.1709,-0.09802631,0.03026521,0.9947235,18.94345 +0.994475,0.06433402,0.0829498,278.8871,-0.06264976,0.9977762,-0.02275279,-10.20698,-0.08422911,0.01743029,0.9962939,19.68959 +0.9955555,0.05974325,0.07280169,278.9518,-0.05878482,0.9981543,-0.01523922,-10.24079,-0.07357776,0.01089186,0.9972299,20.44628 +0.9961723,0.05941056,0.06411927,279.0064,-0.05847166,0.9981539,-0.01642336,-10.26544,-0.06497662,0.01261133,0.997807,21.2127 +0.9962312,0.06437953,0.05812715,279.0503,-0.06325085,0.9977755,-0.02105485,-10.28186,-0.05935335,0.0172989,0.9980871,22.00409 +0.9961865,0.06838144,0.05418897,279.0817,-0.06712758,0.9974403,-0.02463277,-10.29864,-0.05573468,0.02090125,0.9982268,22.82539 +0.9961255,0.07112069,0.05172907,279.1188,-0.06997358,0.9972682,-0.02366081,-10.32374,-0.05327053,0.01994947,0.9983808,23.65989 +0.9959335,0.07420515,0.05108998,279.1531,-0.07311329,0.9970602,-0.02292105,-10.34729,-0.05264064,0.01909248,0.9984309,24.51304 +0.996096,0.07169975,0.05149782,279.1989,-0.07052098,0.9972129,-0.02435563,-10.37467,-0.05310058,0.02062887,0.998376,25.37629 +0.9962631,0.06826151,0.05291774,279.2569,-0.06696379,0.9974186,-0.02592247,-10.40958,-0.05455064,0.02228203,0.9982623,26.25637 +0.9960773,0.06913572,0.05523011,279.31,-0.06764909,0.9973063,-0.02835003,-10.44057,-0.05704134,0.02450255,0.998071,27.14755 +0.9958293,0.06989583,0.05863981,279.367,-0.0684852,0.9973202,-0.02573276,-10.47041,-0.06028128,0.02160947,0.9979474,28.05463 +0.9961028,0.06327448,0.06144551,279.43,-0.06225581,0.9978914,-0.0183557,-10.49615,-0.06247739,0.01445882,0.9979416,28.98177 +0.996,0.06184018,0.06449784,279.492,-0.06106382,0.9980365,-0.01394158,-10.52519,-0.06523334,0.009947323,0.9978204,29.91016 +0.9959468,0.05954303,0.06741393,279.5555,-0.05868528,0.9981692,-0.01463512,-10.54802,-0.06816193,0.0106196,0.9976177,30.84221 +0.9960013,0.05622204,0.0694308,279.6233,-0.0550774,0.9983145,-0.01829339,-10.57151,-0.07034226,0.01439617,0.997419,31.77604 +0.9959256,0.05439153,0.07192941,279.682,-0.05277817,0.9983143,-0.02414478,-10.60332,-0.07312143,0.0202501,0.9971174,32.72243 +0.9957581,0.05439853,0.07420682,279.7476,-0.05223628,0.9981604,-0.03077568,-10.63896,-0.07574446,0.02676884,0.9967678,33.67308 +0.995626,0.0545508,0.07584953,279.815,-0.05227298,0.9981295,-0.03170017,-10.67413,-0.07743692,0.02759663,0.9966152,34.64065 +0.9956407,0.05179737,0.07756711,279.8916,-0.05007458,0.9984572,-0.02399435,-10.7157,-0.07869028,0.02000561,0.9966983,35.6187 +0.9957716,0.0475725,0.07858603,279.9697,-0.04652088,0.9988023,-0.01515993,-10.75207,-0.0792131,0.01143993,0.996792,36.60789 +0.9958436,0.04505057,0.07915852,280.0476,-0.04418864,0.9989436,-0.01260775,-10.78236,-0.07964288,0.009057442,0.9967823,37.59549 +0.9957008,0.04799311,0.07922543,280.1173,-0.04706765,0.9988003,-0.01350885,-10.81541,-0.07977872,0.009721818,0.9967651,38.58797 +0.9958215,0.04619938,0.07877372,280.1964,-0.04452406,0.9987459,-0.02289394,-10.85174,-0.07973261,0.01929095,0.9966295,39.57091 +0.9958053,0.0474364,0.07824163,280.2713,-0.04526279,0.9985446,-0.02932506,-10.88964,-0.07951882,0.02566061,0.996503,40.56118 +0.9956153,0.05105855,0.07837862,280.3408,-0.04903721,0.9984182,-0.02750223,-10.93233,-0.07965886,0.02353817,0.9965442,41.56315 +0.9955594,0.052441,0.07817608,280.4126,-0.05100185,0.9984923,-0.02029477,-10.97462,-0.07912249,0.01621752,0.9967329,42.57375 +0.99556,0.05300544,0.0777868,280.4856,-0.05193443,0.9985266,-0.01572898,-11.01262,-0.07850591,0.01161933,0.9968459,43.59155 +0.9953295,0.05765077,0.0774321,280.5539,-0.05660129,0.9982737,-0.0156825,-11.0483,-0.07820253,0.0112265,0.9968742,44.60279 +0.9951933,0.06026571,0.07719121,280.6232,-0.05928053,0.9981287,-0.0149935,-11.0805,-0.07795036,0.01034549,0.9969035,45.61968 +0.9949059,0.06617181,0.07604954,280.6912,-0.0656665,0.9977998,-0.009128744,-11.10898,-0.07648628,0.004088334,0.9970622,46.63824 +0.994831,0.06731904,0.07602278,280.7575,-0.06737918,0.9977258,-0.001776569,-11.14183,-0.07596948,-0.003354965,0.9971044,47.66917 +0.9948398,0.06926645,0.07413452,280.8247,-0.06919141,0.9975969,-0.003583198,-11.16933,-0.07420456,-0.001564763,0.9972418,48.68662 +0.9950316,0.06843401,0.07231177,280.8942,-0.06784169,0.9976396,-0.01061883,-11.19345,-0.07286777,0.005660316,0.9973255,49.69895 +0.9951638,0.06740927,0.07145007,280.9667,-0.06698358,0.9977192,-0.008340177,-11.21703,-0.0718493,0.003513861,0.9974093,50.71799 +0.9951705,0.0674452,0.07132218,281.0372,-0.06680968,0.9977022,-0.01126163,-11.2477,-0.07191784,0.006442233,0.9973897,51.72891 +0.9950633,0.06890809,0.07142032,281.1096,-0.0675796,0.9974958,-0.02085629,-11.28176,-0.07267864,0.01592677,0.9972282,52.74541 +0.9953629,0.06382479,0.07196587,281.1898,-0.06221197,0.9977637,-0.02443642,-11.31856,-0.07336458,0.01984596,0.9971076,53.76325 +0.995429,0.0612267,0.07329761,281.2698,-0.06018968,0.9980542,-0.01627646,-11.35809,-0.07415154,0.0117903,0.9971772,54.79308 +0.9948658,0.0676707,0.07525108,281.3237,-0.06691675,0.9976803,-0.01249882,-11.39973,-0.07592231,0.007399089,0.9970862,55.82211 +0.9948128,0.06670008,0.07680326,281.3875,-0.06582081,0.9977343,-0.01392618,-11.43904,-0.07755811,0.008798685,0.996949,56.84029 +0.9941975,0.0736622,0.07839249,281.457,-0.07246143,0.9972077,-0.01805738,-11.47506,-0.07950374,0.01227216,0.996759,57.86125 +0.9937271,0.07556547,0.08244051,281.5355,-0.07425104,0.9970605,-0.01889954,-11.51657,-0.08362632,0.01265969,0.9964167,58.89446 +0.9931127,0.08029882,0.08531901,281.6092,-0.07947297,0.9967517,-0.01303791,-11.55132,-0.08608879,0.006167561,0.9962683,59.91592 +0.992705,0.08106097,0.08925248,281.6941,-0.08085186,0.9967083,-0.005961919,-11.57887,-0.08944196,-0.001297802,0.9959911,60.9464 +0.9927726,0.07879411,0.09052159,281.7904,-0.07888362,0.9968804,-0.002594076,-11.60922,-0.0904436,-0.004565343,0.9958911,61.97385 +0.9932073,0.07354668,0.09016811,281.8928,-0.07322593,0.9972917,-0.006864756,-11.64024,-0.09042879,0.0002154823,0.9959028,62.99394 +0.9941004,0.06303642,0.08826649,282,-0.06191416,0.9979627,-0.01539785,-11.66955,-0.08905728,0.00984206,0.9959778,64.00966 +0.9942679,0.06388651,0.08573156,282.0889,-0.06269505,0.9978959,-0.01652158,-11.70038,-0.08660667,0.01105193,0.9961812,65.03015 +0.9945102,0.06296877,0.08357277,282.1681,-0.06240074,0.9980069,-0.009394279,-11.7337,-0.08399774,0.004127704,0.9964573,66.06144 +0.9946978,0.06392828,0.0805583,282.2464,-0.06391395,0.9979516,-0.002759114,-11.76324,-0.08056966,-0.002404314,0.996746,67.0949 +0.9947241,0.06678803,0.07786851,282.3171,-0.06651521,0.9977668,-0.006094968,-11.7941,-0.07810168,0.0008833703,0.996945,68.12345 +0.9946012,0.07178659,0.07493467,282.3862,-0.07066601,0.9973464,-0.01750338,-11.82862,-0.07599233,0.01211355,0.9970348,69.13786 +0.9945628,0.0757141,0.07150038,282.4545,-0.07394973,0.996896,-0.02701315,-11.86542,-0.07332371,0.02157883,0.9970747,70.15552 +0.9948563,0.07387187,0.06931051,282.5238,-0.07222419,0.9970498,-0.02598801,-11.90562,-0.07102581,0.02084844,0.9972565,71.17579 +0.9949103,0.07431902,0.06804647,282.5814,-0.0733047,0.9971597,-0.01728734,-11.94809,-0.06913797,0.01221122,0.9975323,72.20572 +0.9948512,0.07599914,0.06704762,282.6307,-0.07525197,0.9970717,-0.01360373,-11.9836,-0.06788516,0.008488218,0.997657,73.22797 +0.9947044,0.0788058,0.06597631,282.6806,-0.07801502,0.996847,-0.01448169,-12.01441,-0.06690952,0.009257858,0.997716,74.24511 +0.9946239,0.07977263,0.0660282,282.7411,-0.07885337,0.996751,-0.01641741,-12.04708,-0.06712334,0.0111226,0.9976826,75.26042 +0.9944161,0.0815996,0.06691993,282.8004,-0.08040454,0.9965542,-0.02036564,-12.08175,-0.06835117,0.01487125,0.9975504,76.27062 +0.9942241,0.08265824,0.06845509,282.8649,-0.08139591,0.9964599,-0.02103355,-12.12188,-0.06995134,0.0153401,0.9974324,77.28264 +0.994403,0.07925357,0.06986912,282.9456,-0.07822258,0.9967845,-0.017375,-12.15944,-0.07102148,0.01181241,0.9974048,78.30048 +0.9943705,0.07795148,0.0717704,283.018,-0.07715821,0.9969238,-0.0137641,-12.19137,-0.07262255,0.008148942,0.9973261,79.31696 +0.9944183,0.07544151,0.07376304,283.0985,-0.07469245,0.9971236,-0.01286533,-12.23081,-0.07452145,0.007283979,0.9971928,80.32553 +0.9946829,0.07195798,0.07367573,283.1854,-0.07054769,0.9972751,-0.0215719,-12.26467,-0.07502724,0.01625954,0.9970489,81.32284 +0.9947905,0.06981717,0.07428007,283.2718,-0.06794836,0.9973125,-0.0273986,-12.29917,-0.07599333,0.02220865,0.9968609,82.32352 +0.9946765,0.06918316,0.07636991,283.3571,-0.0677081,0.9974682,-0.02174095,-12.33753,-0.07768066,0.01645435,0.9968424,83.33901 +0.9946891,0.06619557,0.0788151,283.4417,-0.06584825,0.9978051,-0.007000552,-12.37684,-0.07910551,0.001773536,0.9968646,84.36387 +0.994702,0.06433678,0.08017993,283.5256,-0.06441513,0.9979219,-0.001611709,-12.40882,-0.080117,-0.00356163,0.996779,85.37498 +0.9945309,0.06583346,0.08108242,283.6047,-0.06502945,0.9978048,-0.01252006,-12.43749,-0.08172866,0.007178838,0.9966287,86.36577 +0.9943989,0.06743004,0.08138816,283.6838,-0.06533238,0.9974658,-0.02817028,-12.4711,-0.08308142,0.02269521,0.9962842,87.3533 +0.9942211,0.06759971,0.08339535,283.7655,-0.06476082,0.9972406,-0.03629236,-12.51326,-0.08561858,0.03068187,0.9958554,88.34335 +0.9939708,0.0669836,0.08680579,283.8524,-0.064651,0.9974743,-0.029413,-12.56371,-0.08855674,0.02362358,0.9957909,89.3453 +0.993627,0.06639506,0.09108927,283.9401,-0.06503064,0.9977232,-0.01786933,-12.60724,-0.09206831,0.01183185,0.9956823,90.35215 +0.9934272,0.06396914,0.09492313,284.0338,-0.06336968,0.9979466,-0.00931939,-12.64165,-0.09532436,0.003242887,0.9954409,91.35817 +0.9932876,0.06100488,0.09827658,284.1298,-0.06049992,0.9981352,-0.008112864,-12.6703,-0.09858823,0.002112681,0.995126,92.36134 +0.9930084,0.05973996,0.1018111,284.2313,-0.05868737,0.9981877,-0.01330545,-12.69838,-0.1024214,0.007237394,0.9947147,93.35526 +0.9925856,0.06026566,0.1055554,284.3305,-0.05848636,0.9980903,-0.0198745,-12.72971,-0.1065515,0.01355359,0.9942147,94.34479 +0.9918536,0.06320656,0.1105959,284.4303,-0.0609456,0.9978594,-0.02370932,-12.76707,-0.1118578,0.01677583,0.9935826,95.34096 +0.9912924,0.06040523,0.1170075,284.5419,-0.05825942,0.9980661,-0.02167642,-12.80761,-0.1180906,0.01467088,0.9928944,96.3451 +0.9906894,0.05754886,0.12338,284.6672,-0.05559815,0.9982686,-0.01919869,-12.84708,-0.1242713,0.01216023,0.9921737,97.34234 +0.9897377,0.05956727,0.1298885,284.793,-0.05772536,0.9981719,-0.01790318,-12.88414,-0.1307174,0.0102216,0.9913669,98.33546 +0.9894464,0.05059501,0.1357797,284.938,-0.04899896,0.9986851,-0.01507333,-12.91595,-0.1363638,0.008261186,0.9906243,99.32058 +0.988952,0.04736796,0.1404646,285.0835,-0.04610523,0.9988617,-0.01223217,-12.94839,-0.1408841,0.005620871,0.9900101,100.3058 +0.9883429,0.0412466,0.1465508,285.2372,-0.03980413,0.999126,-0.01276298,-12.98065,-0.1469491,0.006780873,0.9891208,101.2855 +0.9878306,0.03673413,0.151134,285.395,-0.03473504,0.9992709,-0.015847,-13.01542,-0.1516059,0.01040451,0.9883862,102.2595 +0.9869511,0.0435911,0.1550078,285.5419,-0.04181653,0.9990173,-0.01469211,-13.05481,-0.1554959,0.008018506,0.9878039,103.2332 +0.9860711,0.04984959,0.1586786,285.6886,-0.04774076,0.9987138,-0.01707659,-13.09108,-0.1593258,0.009263293,0.9871825,104.2036 +0.9856634,0.05050868,0.1609864,285.8441,-0.04763957,0.9986302,-0.02163492,-13.12913,-0.1618586,0.01365542,0.9867194,105.1632 +0.9852472,0.05281612,0.1627838,285.9984,-0.0497414,0.9984993,-0.02290952,-13.16767,-0.1637495,0.01447444,0.9863957,106.1292 +0.9851163,0.05214476,0.1637889,286.1578,-0.04970552,0.9985839,-0.0189586,-13.20807,-0.1645456,0.01053522,0.9863132,107.0955 +0.98543,0.04841488,0.1630454,286.3219,-0.04626508,0.9987852,-0.01695889,-13.25071,-0.1636684,0.009168485,0.9864728,108.0537 +0.9860995,0.04148697,0.1608937,286.4851,-0.03911793,0.9990749,-0.01786539,-13.28753,-0.161486,0.01132322,0.98681,109.0066 +0.986777,0.03568008,0.158108,286.6417,-0.03341209,0.9992974,-0.01698039,-13.32242,-0.1586028,0.01147313,0.9872758,109.9571 +0.9874257,0.03224614,0.1547601,286.7938,-0.03053742,0.9994437,-0.01340637,-13.35867,-0.1551063,0.008511816,0.9878611,110.9122 +0.9879797,0.03322151,0.150972,286.9362,-0.03176099,0.9994225,-0.01207583,-13.397,-0.1512859,0.007135652,0.9884642,111.8603 +0.9886135,0.03280421,0.1468581,287.077,-0.03103349,0.9994156,-0.01433295,-13.4318,-0.1472425,0.009612226,0.9890537,112.8028 +0.9891959,0.03213855,0.1430338,287.2169,-0.02939177,0.9993415,-0.02127591,-13.47199,-0.1436233,0.01684202,0.989489,113.7365 +0.9896387,0.02983259,0.1404473,287.3536,-0.0264581,0.999316,-0.02583341,-13.51254,-0.1411219,0.02184976,0.989751,114.6651 +0.9900155,0.02662197,0.1384223,287.491,-0.02352154,0.9994355,-0.02398648,-13.56,-0.1389827,0.02049107,0.9900827,115.5929 +0.9903541,0.02069235,0.1370058,287.6309,-0.01870616,0.9997006,-0.015769,-13.60737,-0.1372911,0.01305404,0.9904447,116.5185 +0.9906344,0.01071487,0.1361202,287.7744,-0.009927284,0.9999298,-0.006463472,-13.6507,-0.1361799,0.005051633,0.9906712,117.4361 +0.9908081,0.0002470638,0.1352748,287.9206,-8.28313E-05,0.9999992,-0.001219694,-13.68599,-0.135275,0.001197277,0.9908073,118.3379 +0.9907837,-0.004037341,0.1353934,288.0587,0.004310644,0.9999892,-0.001725471,-13.72267,-0.135385,0.002293201,0.9907904,119.2193 +0.9906465,-0.002862894,0.1364239,288.19,0.003934601,0.9999635,-0.00758671,-13.76306,-0.1363972,0.008052519,0.9906214,120.0844 +0.990425,-0.0007741897,0.1380499,288.3171,0.002963701,0.999873,-0.01565544,-13.79791,-0.1380203,0.01591467,0.9903015,120.9359 +0.990081,-0.000961444,0.1404945,288.4488,0.003500617,0.9998349,-0.0178271,-13.83618,-0.1404542,0.01814208,0.9899209,121.7809 +0.9895547,-0.0002364167,0.1441582,288.5773,0.001842821,0.9999377,-0.01100992,-13.87237,-0.1441466,0.01116057,0.9894933,122.6291 +0.9888756,-0.0003926532,0.148745,288.7102,0.001267845,0.9999824,-0.005789067,-13.90798,-0.1487401,0.005913251,0.9888586,123.4632 +0.9880462,-0.001311528,0.1541527,288.8464,0.002111554,0.9999851,-0.005026212,-13.93832,-0.1541438,0.005291631,0.9880342,124.2806 +0.9869974,-0.002272381,0.1607206,288.9881,0.003269075,0.999977,-0.005937258,-13.96658,-0.1607034,0.006385465,0.986982,125.082 +0.9857593,-0.003540913,0.1681256,289.1349,0.005032636,0.9999516,-0.00844741,-13.9957,-0.1680875,0.009173226,0.9857293,125.8653 +0.9842807,-0.005509228,0.1765252,289.2853,0.007593788,0.9999091,-0.01113546,-14.02651,-0.1764478,0.01230092,0.9842331,126.6301 +0.9825554,-0.005585028,0.1858863,289.438,0.008165972,0.9998805,-0.01312177,-14.05307,-0.1857908,0.01441081,0.9824836,127.3821 +0.9806437,-0.002801404,0.195781,289.5906,0.005260063,0.9999137,-0.01203939,-14.0782,-0.1957304,0.01283617,0.9805737,128.1183 +0.9786013,8.549226E-06,0.2057657,289.7435,0.00186146,0.9999587,-0.008894468,-14.10606,-0.2057572,0.009087161,0.9785608,128.8354 +0.9766453,0.00215563,0.2148475,289.9024,-0.0001112036,0.9999546,-0.009527347,-14.13734,-0.2148582,0.009280944,0.9766011,129.5255 +0.9745758,0.006160374,0.2239735,290.0588,-0.002457503,0.9998557,-0.01680763,-14.16961,-0.2240447,0.01582989,0.9744502,130.1828 +0.9723461,0.01204043,0.2332341,290.213,-0.006386251,0.9996675,-0.02498255,-14.19655,-0.2334573,0.02280219,0.9720996,130.8211 +0.9703111,0.01602358,0.2413289,290.3682,-0.009625385,0.9995708,-0.02766798,-14.21879,-0.2416687,0.02452366,0.9700488,131.4393 +0.9686044,0.01804972,0.2479511,290.5237,-0.01265319,0.9996475,-0.02334099,-14.24517,-0.248285,0.01947081,0.9684913,132.039 +0.9670919,0.01559798,0.253949,290.6836,-0.01304927,0.9998462,-0.01171787,-14.27296,-0.2540927,0.008018402,0.9671466,132.626 +0.9660759,0.01108302,0.2580206,290.8457,-0.01030489,0.9999373,-0.00436797,-14.30013,-0.2580529,0.001560916,0.9661295,133.1895 +0.9655003,0.003299459,0.2603811,291.0103,-0.002137197,0.9999864,-0.004746696,-14.32375,-0.2603932,0.004026449,0.9654942,133.7319 +0.9653899,-0.0003073954,0.260811,291.1662,0.002050802,0.9999773,-0.006412443,-14.34499,-0.2608031,0.006725378,0.9653685,134.262 +0.9656345,0.003007119,0.2598865,291.3078,-0.0001177049,0.999938,-0.01113283,-14.36365,-0.2599039,0.01071965,0.9655749,134.7893 +0.9664966,0.008454522,0.2565403,291.4387,-0.003519176,0.9997999,-0.0196911,-14.38105,-0.2566554,0.01812856,0.9663329,135.3099 +0.9681031,0.004215975,0.2505169,291.5808,0.001619359,0.9997322,-0.02308249,-14.39899,-0.2505471,0.0227519,0.967837,135.8325 +0.9704463,-0.005752899,0.241249,291.7267,0.01114746,0.9997173,-0.02100209,-14.41589,-0.2410599,0.02307071,0.9702359,136.3603 +0.973437,-0.01201617,0.2286396,291.8558,0.01628553,0.9997263,-0.01679525,-14.43191,-0.2283752,0.02007263,0.9733662,136.8918 +0.9770671,-0.01037141,0.212679,291.9617,0.01328393,0.9998365,-0.01226999,-14.45144,-0.2125169,0.01481381,0.977045,137.4264 +0.9812611,-0.00527812,0.1926109,292.0425,0.006666048,0.9999563,-0.00655852,-14.47554,-0.1925679,0.007719572,0.9812532,137.9609 +0.9860165,-0.006979756,0.1665017,292.1239,0.008059672,0.9999506,-0.005811091,-14.49602,-0.1664529,0.00707178,0.986024,138.49 +0.9906926,-0.00933976,0.1357975,292.194,0.01061907,0.9999058,-0.008699345,-14.51375,-0.1357035,0.01006042,0.9906984,139.0201 +0.9947455,-0.009549321,0.101933,292.2216,0.01100309,0.9998455,-0.01370925,-14.53151,-0.1017864,0.01475879,0.9946967,139.5377 +0.9979445,-0.007546865,0.06363957,292.2395,0.008781174,0.9997783,-0.01913796,-14.54607,-0.06348102,0.01965744,0.9977894,140.0685 +0.9997695,-0.006334002,0.02051851,292.2379,0.006785853,0.9997343,-0.02202735,-14.56311,-0.02037354,0.0221615,0.9995467,140.6044 +0.9996353,-0.007292827,-0.02600221,292.1809,0.006706055,0.9997225,-0.0225825,-14.57853,0.02615968,0.02239989,0.9994067,141.1316 +0.9971942,-0.006751992,-0.07455281,292.1276,0.005331233,0.9998007,-0.01923968,-14.59807,0.07466785,0.01878824,0.9970314,141.6769 +0.9913688,-0.004471404,-0.1310267,291.7623,0.002263437,0.999853,-0.01699536,-14.60962,0.1310834,0.0165521,0.9912331,142.1582 +0.9823566,-0.002081704,-0.1870062,291.396,-0.0006730589,0.9998922,-0.01466616,-14.62098,0.1870166,0.01453326,0.9822492,142.6347 +0.9701549,0.0004076394,-0.2424857,291.0295,-0.003482536,0.9999189,-0.01225224,-14.63229,0.242461,0.01273103,0.9700776,143.1088 +0.9548354,0.002972094,-0.2971206,290.6625,-0.006152089,0.9999333,-0.009768209,-14.64343,0.2970717,0.01115494,0.9547899,143.5781 +0.9364252,0.005598286,-0.3508228,290.2955,-0.008682181,0.9999362,-0.007218125,-14.65447,0.35076,0.009805141,0.936414,144.0439 +0.9149882,0.008267732,-0.4033961,289.9287,-0.01106815,0.9999281,-0.004611072,-14.66542,0.4033289,0.008683926,0.9150138,144.5058 +0.8905694,0.0109654,-0.4547153,289.5624,-0.01330886,0.9999095,-0.001952993,-14.67631,0.4546527,0.00779102,0.8906346,144.9646 +0.8632689,0.01367117,-0.5045592,289.1967,-0.01539921,0.9998811,0.000744979,-14.6871,0.5045094,0.007126699,0.8633767,145.4195 +0.833168,0.01636851,-0.5527778,288.8319,-0.0173376,0.9998436,0.003474853,-14.69782,0.5527482,0.006688705,0.8333214,145.8707 +0.8003688,0.01904004,-0.5992056,288.4681,-0.01912204,0.9997977,0.00622742,-14.70847,0.5992029,0.006473803,0.800571,146.3181 +0.7649758,0.02166938,-0.6436944,288.1056,-0.02075155,0.9997442,0.008994019,-14.71907,0.6437246,0.006477454,0.7652297,146.7619 +0.7270108,0.02424637,-0.6861979,287.7451,-0.02222885,0.9996836,0.01177224,-14.72969,0.6862661,0.006694839,0.7273197,147.2039 +0.6867285,0.02674618,-0.7264218,287.386,-0.02354827,0.9996169,0.01454344,-14.74024,0.7265325,0.00711859,0.6870952,147.6415 +0.6441627,0.02915974,-0.7643326,287.0291,-0.02471347,0.9995448,0.01730526,-14.75079,0.7644892,0.007741906,0.64459,148.0766 +0.5995009,0.03146954,-0.7997552,286.6742,-0.02572364,0.9994681,0.02004545,-14.76132,0.7999606,0.008555353,0.5999915,148.5084 +0.5508725,0.03437757,-0.8338811,286.3994,-0.02594433,0.9993738,0.02406102,-14.78195,0.8341861,0.008379938,0.5514194,148.9762 +0.5171396,0.03114512,-0.8553342,285.8334,-0.03136806,0.999356,0.01742407,-14.78774,0.855326,0.0178195,0.5177835,149.3063 +0.4904783,0.03433853,-0.8707767,285.2184,-0.03778333,0.9991217,0.0181177,-14.7978,0.8706339,0.0240145,0.4913449,149.6157 +0.4704194,0.04280493,-0.8814042,284.5719,-0.04429686,0.998709,0.02485985,-14.80978,0.8813304,0.02734889,0.4717082,149.9194 +0.4564363,0.04948111,-0.8883792,283.9252,-0.04686304,0.9984035,0.03153173,-14.81823,0.8885211,0.02723993,0.4580264,150.2242 +0.4467789,0.05588476,-0.8928973,283.2644,-0.05078708,0.9980219,0.037052,-14.82401,0.8932017,0.0287936,0.4487333,150.5268 +0.4419837,0.05896435,-0.895083,282.5986,-0.05395537,0.997778,0.03908681,-14.81969,0.8953989,0.0310188,0.4441831,150.8355 +0.4407294,0.05917028,-0.8956878,281.9257,-0.05792303,0.9976201,0.03740264,-14.83474,0.8957692,0.0353965,0.4431078,151.1481 +0.4413447,0.059101,-0.8953893,281.2436,-0.06068408,0.9975101,0.03592991,-14.83739,0.8952834,0.0384784,0.4438322,151.4721 +0.44206,0.05878923,-0.8950569,280.5534,-0.06118167,0.9975022,0.03530102,-14.84138,0.8948965,0.0391559,0.4445526,151.8054 +0.4439269,0.05818474,-0.8941719,279.8546,-0.06232227,0.9974779,0.03396603,-14.83845,0.893893,0.04064839,0.4464335,152.1458 +0.4463569,0.05917448,-0.8928964,279.1422,-0.06674274,0.9972334,0.03272462,-14.85001,0.8923625,0.04498749,0.4490714,152.4892 +0.4495609,0.05830193,-0.891345,278.4185,-0.0699087,0.9971034,0.02996016,-14.8643,0.8905098,0.04884385,0.4523345,152.847 +0.4525449,0.05659976,-0.8899436,277.6855,-0.07527132,0.9968465,0.02512253,-14.88056,0.8885591,0.05561815,0.4553781,153.2088 +0.4561196,0.05547102,-0.888188,276.943,-0.07760443,0.9967326,0.02239715,-14.89145,0.8865283,0.05871154,0.4589341,153.5835 +0.4597848,0.05605008,-0.8862598,276.187,-0.07742938,0.9967355,0.02286718,-14.90243,0.8846483,0.05810855,0.4626238,153.9729 +0.4636368,0.05779616,-0.8841383,275.4205,-0.07851917,0.9966243,0.0239744,-14.91059,0.8825393,0.05830638,0.4666098,154.3706 +0.4671865,0.06054826,-0.8820832,274.6402,-0.07951878,0.9964867,0.02628488,-14.91202,0.8805756,0.05786223,0.4703599,154.7801 +0.4696492,0.06065301,-0.8807672,273.8488,-0.07924542,0.9965063,0.02636743,-14.91226,0.8792893,0.05741332,0.4728149,155.2017 +0.4713839,0.05851986,-0.8799845,273.0503,-0.07647763,0.9967498,0.02531788,-14.91816,0.8786059,0.05536468,0.4743272,155.6341 +0.4724708,0.05597397,-0.8795671,272.2412,-0.07425683,0.9969609,0.02355668,-14.93058,0.8782125,0.05418402,0.4751914,156.0747 +0.4732616,0.05450978,-0.8792339,271.4235,-0.07325586,0.9970619,0.02238364,-14.93703,0.8778707,0.05381571,0.4758643,156.516 +0.4741148,0.05726319,-0.878599,270.5906,-0.07356616,0.9969699,0.02527988,-14.9386,0.8773843,0.05264958,0.4768907,156.9672 +0.4751844,0.06203666,-0.8776966,269.7527,-0.07372503,0.9968108,0.03054112,-14.93865,0.8767921,0.05019554,0.4782426,157.422 +0.4773532,0.06723742,-0.8761354,268.9095,-0.07485073,0.9965556,0.03569723,-14.9374,0.8755177,0.04853918,0.4807417,157.8809 +0.48034,0.06321322,-0.8748015,268.0716,-0.07387211,0.9967712,0.03146474,-14.93337,0.8739659,0.04950965,0.4834587,158.3428 +0.4834496,0.0550452,-0.8736399,267.2366,-0.07309837,0.9970738,0.02237164,-14.92951,0.8723148,0.05304609,0.4860585,158.8076 +0.4870183,0.05042459,-0.8719349,266.3931,-0.07411799,0.9971168,0.01626542,-14.92764,0.8702411,0.0567045,0.4893515,159.2788 +0.4909782,0.0523389,-0.8695983,265.5431,-0.07578582,0.9969755,0.01721649,-14.93282,0.8678692,0.05745029,0.4934597,159.7433 +0.4965062,0.05730163,-0.8661398,264.6843,-0.07574002,0.9968729,0.02253339,-14.9519,0.8647225,0.05441347,0.4992935,160.2224 +0.5037342,0.06233516,-0.8616068,263.8317,-0.07602554,0.9967221,0.02766248,-14.96713,0.8605068,0.05156958,0.506822,160.7122 +0.5123371,0.06398076,-0.8563978,262.9761,-0.07693836,0.9966304,0.02842931,-14.97995,0.855331,0.05132445,0.5155333,161.2125 +0.521172,0.06170989,-0.8512178,262.1606,-0.0766941,0.9967336,0.025302,-14.97143,0.8499987,0.05209668,0.5242023,161.7093 +0.5301848,0.0570259,-0.8459623,261.3431,-0.07311648,0.997094,0.02138978,-14.95873,0.8447236,0.05051324,0.5328136,162.2294 +0.5393351,0.05538765,-0.8402678,260.5331,-0.06992809,0.997334,0.02085683,-14.95181,0.8391828,0.0475095,0.5417703,162.7589 +0.5480156,0.0561991,-0.8345781,259.7251,-0.06912712,0.9973703,0.02176978,-14.94049,0.8336068,0.04576179,0.5504593,163.2934 +0.5568587,0.05632836,-0.8286951,258.9277,-0.06583899,0.997552,0.0235641,-14.92164,0.8279937,0.04143857,0.5592041,163.8391 +0.5649508,0.05472345,-0.823308,258.1366,-0.05928442,0.9979116,0.02564824,-14.90177,0.8229921,0.03431934,0.5670151,164.3956 +0.5713244,0.05475531,-0.8188958,257.3499,-0.05660736,0.9980248,0.02723913,-14.88677,0.8187698,0.03079316,0.5732954,164.9528 +0.5760926,0.05210708,-0.8157219,256.5679,-0.05185624,0.9982855,0.02714621,-14.87254,0.8157378,0.02666154,0.5778069,165.5157 +0.5790174,0.04523714,-0.8140593,255.7909,-0.04260851,0.9987741,0.02519549,-14.85131,0.8142011,0.02009722,0.580235,166.0834 +0.5795838,0.04026331,-0.8139174,255.011,-0.03576372,0.9990731,0.02395566,-14.82236,0.8141275,0.0152244,0.5804865,166.6515 +0.5781798,0.03952305,-0.8149516,254.2206,-0.03188346,0.9991576,0.02583637,-14.78342,0.8152862,0.01104542,0.5789528,167.2198 +0.5748458,0.04141828,-0.8172129,253.4213,-0.03005098,0.999113,0.02949888,-14.76026,0.8177098,0.007600747,0.5755805,167.7851 +0.5700229,0.03909996,-0.820698,252.6195,-0.02584412,0.999226,0.0296552,-14.72574,0.8212222,0.004306082,0.5705922,168.3511 +0.5646348,0.03657263,-0.8245302,251.8124,-0.01877016,0.9993283,0.03147221,-14.69464,0.8251274,-0.002293737,0.564942,168.9153 +0.5582592,0.03740193,-0.8288232,250.9912,-0.02063112,0.9993002,0.03119876,-14.65667,0.8294101,-0.0003174385,0.5586401,169.4706 +0.5515355,0.03922665,-0.8332286,250.1643,-0.02350498,0.9992278,0.031483,-14.61797,0.8338202,0.002221032,0.5520316,170.0182 +0.5440084,0.03333297,-0.8384175,249.3429,-0.0195033,0.999443,0.02708012,-14.5761,0.8388531,0.001620096,0.5443554,170.5648 +0.5373406,0.02653666,-0.8429478,248.5162,-0.01534386,0.999647,0.02168867,-14.54374,0.8432257,0.00127987,0.5375581,171.1031 +0.5311679,0.02269359,-0.8469626,247.6823,-0.01096373,0.9997416,0.01991132,-14.51402,0.8471956,-0.001290385,0.5312795,171.6359 +0.5239739,0.02746361,-0.8512915,246.8297,-0.008776716,0.999601,0.02684614,-14.48307,0.8516891,-0.006595125,0.5240058,172.1711 +0.5170137,0.02948047,-0.8554693,245.9791,-0.001669024,0.9994395,0.03343317,-14.45692,0.8559754,-0.0158576,0.5167731,172.7018 +0.5102721,0.02883266,-0.8595296,245.1263,0.004521727,0.9993341,0.03620675,-14.42624,0.8600011,-0.02236185,0.5098019,173.224 +0.5037941,0.02790445,-0.863373,244.2688,0.00758643,0.9992966,0.03672437,-14.40057,0.8637904,-0.02505143,0.503228,173.7284 +0.4969454,0.02661618,-0.8673736,243.4072,0.00942959,0.9993049,0.03606713,-14.37066,0.8677305,-0.02610236,0.4963489,174.2274 +0.4900908,0.02130586,-0.871411,242.5488,0.01474679,0.9993555,0.03272783,-14.34179,0.8715466,-0.02889011,0.4894607,174.722 +0.4835622,0.01711863,-0.8751426,241.6878,0.01784857,0.999408,0.02941165,-14.312,0.875128,-0.02984239,0.4829703,175.2111 +0.4782141,0.01551209,-0.8781063,240.8196,0.0197068,0.9994027,0.0283871,-14.28753,0.8780221,-0.03087976,0.4776228,175.6926 +0.4735843,0.0169176,-0.8805861,239.9455,0.02170171,0.9992878,0.03086938,-14.26868,0.8804811,-0.03372947,0.4728798,176.1723 +0.4694526,0.02047347,-0.8827203,239.0666,0.02280313,0.9991165,0.0353004,-14.25265,0.8826631,-0.03670064,0.468571,176.6464 +0.4655699,0.02519035,-0.8846526,238.1863,0.01837166,0.9991043,0.03811789,-14.23471,0.8848204,-0.03399907,0.4646901,177.1088 +0.463274,0.02635855,-0.8858231,237.3097,0.0189166,0.9990357,0.03962045,-14.21372,0.8860132,-0.03511187,0.4623286,177.5718 +0.4618357,0.02831917,-0.8865133,236.4351,0.01678627,0.9990321,0.04065846,-14.18734,0.8868066,-0.03365876,0.4609133,178.0277 +0.4619413,0.02846357,-0.8864537,235.5599,0.01206805,0.9991906,0.0383723,-14.16623,0.8868284,-0.0284235,0.4612238,178.4782 +0.4618181,0.02279829,-0.8866817,234.6932,0.01193017,0.9994195,0.0319107,-14.14473,0.8868944,-0.02531519,0.461278,178.9321 +0.4616016,0.01798896,-0.886905,233.8286,0.01215064,0.9995723,0.02659814,-14.11729,0.8870041,-0.0230542,0.4611856,179.3917 +0.4623985,0.01726948,-0.886504,232.9645,0.01537525,0.9995038,0.02749046,-14.10097,0.8865389,-0.02634176,0.4619035,179.8511 +0.4642328,0.01804312,-0.8855294,232.1043,0.01152037,0.9995849,0.02640654,-14.0914,0.8856382,-0.0224604,0.4638322,180.3056 +0.467227,0.01580026,-0.8839962,231.2512,0.006991158,0.999743,0.02156419,-14.08309,0.8841097,-0.01625552,0.4669965,180.7526 +0.4700517,0.01428596,-0.8825233,230.4022,0.002619721,0.999842,0.0175804,-14.06977,0.882635,-0.01057565,0.46994,181.2038 +0.4727249,0.01920178,-0.8810009,229.5516,0.0002912329,0.9997591,0.02194644,-14.06684,0.88121,-0.0106312,0.4726054,181.6586 +0.4750387,0.02438918,-0.8796269,228.7077,-0.002073831,0.9996441,0.0265969,-14.06108,0.8799625,-0.01081035,0.4749201,182.1131 +0.4769137,0.02715035,-0.8785307,227.8697,-0.002067665,0.9995547,0.02976807,-14.04863,0.8789476,-0.01238029,0.4767575,182.5698 +0.477426,0.02860964,-0.8782061,227.0432,0.0005237217,0.9994603,0.0328445,-14.03677,0.8786718,-0.01614075,0.4771533,183.0248 +0.4767459,0.02907285,-0.8785603,226.2149,0.003056755,0.999392,0.03473008,-14.01951,0.8790358,-0.01924296,0.4763671,183.4845 +0.4755753,0.02885,-0.8792018,225.3932,0.003142599,0.9993999,0.03449405,-14.00774,0.8796694,-0.01916749,0.4751992,183.9336 +0.4746047,0.02601797,-0.8798145,224.5795,0.003465267,0.9995,0.03142663,-13.99773,0.8801922,-0.01796401,0.4742773,184.3764 +0.4736067,0.02283874,-0.8804403,223.7699,0.001192063,0.9996462,0.0265722,-13.98644,0.8807356,-0.01363431,0.4734119,184.8117 +0.4729552,0.02428356,-0.8807518,222.9616,-0.002894965,0.9996575,0.02600739,-13.96843,0.8810817,-0.009750578,0.4728635,185.2426 +0.4718172,0.0279327,-0.8812538,222.1602,-0.00404262,0.999556,0.02951809,-13.95469,0.8816871,-0.01036456,0.4717207,185.6726 +0.4703146,0.03021966,-0.8819813,221.3673,0.0002475656,0.9994089,0.03437514,-13.9405,0.8824988,-0.01638547,0.4700291,186.1071 +0.4689199,0.03163535,-0.882674,220.5869,0.00472521,0.9992542,0.03832389,-13.92145,0.883228,-0.02214165,0.4684206,186.5343 +0.4675889,0.03394553,-0.8832941,219.8147,0.001874438,0.999222,0.03939299,-13.91172,0.883944,-0.02007539,0.4671614,186.9443 +0.4661391,0.03000521,-0.8842025,219.0592,0.0003993942,0.9994175,0.03412556,-13.90122,0.8847113,-0.01626039,0.4658555,187.344 +0.4643023,0.01892128,-0.8854747,218.3174,0.003894118,0.9997185,0.0234044,-13.8901,0.8856682,-0.01431485,0.4640979,187.7425 +0.4628199,0.01399383,-0.8863419,217.5792,0.008092561,0.999767,0.0200103,-13.87919,0.8864154,-0.01643393,0.4625988,188.1373 +0.4615098,0.0194489,-0.8869219,216.8437,0.006723798,0.9996542,0.02541969,-13.87317,0.8871096,-0.01769491,0.4612194,188.5212 +0.4593783,0.03048412,-0.8877175,216.115,0.004768152,0.9993119,0.03678369,-13.87181,0.8882279,-0.02113039,0.4589168,188.8953 +0.4572805,0.03358023,-0.8886883,215.4108,0.005641299,0.9991572,0.04065722,-13.86559,0.8893046,-0.0236051,0.4567057,189.2597 +0.4551794,0.02889664,-0.8899308,214.7303,0.005942319,0.9993524,0.035489,-13.85704,0.8903799,-0.02144211,0.4547129,189.6122 +0.4536993,0.02497728,-0.8908048,214.0622,0.003270306,0.9995537,0.02969211,-13.84865,0.8911488,-0.01638449,0.4534151,189.9494 +0.4529661,0.02483148,-0.8911819,213.4036,-0.0006709007,0.9996212,0.02751198,-13.84168,0.8915275,-0.01186409,0.4528111,190.2779 +0.451711,0.02848173,-0.8917096,212.7616,-0.001972238,0.9995197,0.03092618,-13.84026,0.8921621,-0.01221103,0.4515502,190.6006 +0.4505465,0.03068438,-0.8922255,212.138,-0.002918172,0.9994544,0.03289849,-13.83654,0.8927482,-0.01221862,0.4503902,190.9167 +0.4493466,0.03472918,-0.8926822,211.5271,-0.007228593,0.9993527,0.03524049,-13.82577,0.8933282,-0.009382352,0.4493068,191.2219 +0.4482361,0.03995274,-0.893022,210.9341,-0.009529233,0.9991575,0.0399181,-13.81376,0.8938644,-0.009382912,0.4482392,191.5188 +0.4472812,0.04107426,-0.8934498,210.3623,-0.00669812,0.9990707,0.04257672,-13.8046,0.8943683,-0.01305933,0.4471407,191.8132 +0.4462374,0.03961846,-0.8940373,209.8176,-0.003841588,0.9990951,0.04235658,-13.80196,0.8949063,-0.01546656,0.4459858,192.0958 +0.4467443,0.03482255,-0.8939838,209.2935,-0.003653884,0.9993049,0.0370991,-13.79138,0.8946542,-0.01330729,0.446561,192.3649 +0.4479671,0.03186131,-0.8934822,208.7857,-0.00565731,0.9994458,0.03280353,-13.77793,0.8940321,-0.00964019,0.447899,192.6184 +0.449181,0.03192326,-0.8928703,208.2977,-0.006820188,0.9994548,0.03230297,-13.7658,0.8934147,-0.00842033,0.4491538,192.8681 +0.4520879,0.03286185,-0.8913679,207.8314,-0.005236514,0.9994016,0.03418883,-13.76168,0.891958,-0.01078869,0.4519894,193.1133 +0.4561874,0.03594035,-0.8891577,207.3857,-0.005469835,0.9992784,0.03758518,-13.76465,0.8898669,-0.01228233,0.4560548,193.3514 +0.4608607,0.0363736,-0.8867268,206.9682,-0.006138347,0.9992665,0.0377997,-13.75997,0.8874512,-0.01197735,0.4607459,193.5784 +0.4658034,0.03343784,-0.8842563,206.5767,-0.003899857,0.9993536,0.03573587,-13.74741,0.8848796,-0.01319741,0.4656327,193.7983 +0.4720979,0.03154808,-0.8809815,206.2106,-0.003115087,0.9994129,0.03411983,-13.73804,0.8815406,-0.01336356,0.471919,194.0068 +0.4794562,0.03261563,-0.8769595,205.8653,-0.002715488,0.9993594,0.03568328,-13.7288,0.8775616,-0.01472719,0.4792376,194.2062 +0.4863374,0.03745268,-0.8729681,205.5399,-0.00421551,0.9991699,0.04051859,-13.71886,0.8737609,-0.01602569,0.4860915,194.3956 +0.4943822,0.04274491,-0.868193,205.2423,-0.005585015,0.9989258,0.04600114,-13.71347,0.8692267,-0.01789327,0.4940898,194.5792 +0.50306,0.04525584,-0.8630658,204.9738,-0.007000773,0.9988087,0.0482931,-13.714,0.8642231,-0.01825219,0.5027775,194.7476 +0.5118968,0.04561105,-0.8578353,204.7297,-0.008865431,0.9988168,0.04781676,-13.71641,0.8590012,-0.01687216,0.5116954,194.9016 +0.5214969,0.04521142,-0.8520545,204.5148,-0.01018415,0.9988539,0.04676766,-13.71967,0.8531923,-0.01571173,0.5213596,195.041 +0.5301027,0.04374822,-0.8468042,204.3163,-0.009813257,0.9989178,0.0454637,-13.72167,0.8478766,-0.01579051,0.5299583,195.1811 +0.5384578,0.04059801,-0.841674,204.1223,-0.008135773,0.9990426,0.04298384,-13.72114,0.8426132,-0.01629731,0.5382725,195.3215 +0.5465675,0.03745693,-0.836577,203.9306,-0.008478972,0.9991955,0.0391984,-13.72379,0.8373721,-0.01433125,0.5464453,195.4618 +0.554553,0.0357784,-0.8313789,203.7374,-0.01105057,0.9993038,0.03563402,-13.7246,0.832075,-0.01057373,0.5545623,195.6023 +0.5627119,0.03474146,-0.8259228,203.5463,-0.0134884,0.9993693,0.03284747,-13.72293,0.826543,-0.007343276,0.5628256,195.7452 +0.570582,0.0331993,-0.8205693,203.3726,-0.01504083,0.9994374,0.02997747,-13.71801,0.8211028,-0.004762561,0.5707603,195.9171 +0.5780036,0.02966194,-0.815495,203.2102,-0.01387787,0.9995519,0.02652033,-13.713,0.8159162,-0.004011508,0.5781562,196.1058 +0.5854633,0.02690854,-0.8102523,203.0429,-0.01249317,0.9996298,0.02417061,-13.71067,0.8106027,-0.00402838,0.5855826,196.2824 +0.5930122,0.02631166,-0.8047635,202.8747,-0.01369308,0.9996509,0.02259337,-13.70872,0.8050771,-0.002378443,0.5931654,196.4607 +0.6001986,0.02823031,-0.7993527,202.6968,-0.01842488,0.9995997,0.0214679,-13.70926,0.7996388,0.001842978,0.6004784,196.6206 +0.6069455,0.03200059,-0.794099,202.5157,-0.02422065,0.9994697,0.0217643,-13.70786,0.7943743,0.006023855,0.6073986,196.7881 +0.6133084,0.03538476,-0.7890506,202.3296,-0.02866225,0.999335,0.02253649,-13.70617,0.7893233,0.008794153,0.6139147,196.9507 +0.619593,0.03070439,-0.7843226,202.1442,-0.02736788,0.9994721,0.01750714,-13.7043,0.784446,0.01061795,0.6201061,197.133 +0.6259051,0.02123132,-0.7796102,201.9454,-0.02370683,0.9996854,0.008191808,-13.70466,0.7795388,0.0133548,0.6262115,197.3255 +0.6316219,0.01322153,-0.7751639,201.7259,-0.02057036,0.9997883,0.0002916079,-13.70537,0.7750037,0.01576122,0.6317601,197.534 +0.6368497,0.01128826,-0.7709054,201.4737,-0.02150214,0.9997639,-0.003123649,-13.70834,0.7706881,0.01856541,0.636942,197.7594 +0.6407806,0.01435373,-0.7675899,201.1953,-0.02453023,0.9996975,-0.001783642,-13.71162,0.7673321,0.01997208,0.6409388,198 +0.642759,0.01719987,-0.7658753,200.8951,-0.02609284,0.9996593,0.0005517974,-13.71056,0.7656239,0.01962919,0.6429888,198.2595 +0.6424791,0.01954642,-0.766054,200.5732,-0.03036962,0.9995387,3.336422E-05,-13.70783,0.7657012,0.02324333,0.6427763,198.5288 +0.6404168,0.02053883,-0.7677529,200.2274,-0.03506204,0.999382,-0.002511452,-13.70269,0.7672268,0.02852736,0.6407411,198.8085 +0.636791,0.02126632,-0.7707432,199.8632,-0.03585757,0.9993548,-0.002051485,-13.70194,0.7702022,0.02894334,0.6371426,199.1068 +0.632943,0.02147065,-0.7739006,199.482,-0.03134706,0.9995063,0.002092193,-13.70715,0.7735635,0.02293527,0.6333036,199.4285 +0.6279217,0.02389882,-0.7779096,199.0768,-0.02939477,0.9995435,0.006980623,-13.71403,0.7777212,0.01848319,0.6283375,199.76 +0.6216983,0.02770993,-0.7827665,198.6508,-0.02831573,0.9995158,0.0128936,-13.71219,0.7827448,0.01414868,0.6221819,200.1013 +0.6138574,0.03550098,-0.7886183,198.1987,-0.03056402,0.9993081,0.02119463,-13.70683,0.788825,0.01109287,0.6145177,200.4467 +0.6051325,0.04101799,-0.7950675,197.7307,-0.03257744,0.9991112,0.02674977,-13.69801,0.795458,0.009714111,0.6059308,200.7917 +0.5959167,0.04341784,-0.8018717,197.2529,-0.030914,0.9990375,0.03111955,-13.69486,0.802451,0.006244404,0.5966852,201.1456 +0.5868903,0.04366091,-0.8084884,196.7611,-0.02548603,0.9990464,0.03545107,-13.68933,0.8092652,-0.000200723,0.5874433,201.5088 +0.5774318,0.03850325,-0.8155305,196.253,-0.01971235,0.9992536,0.03322007,-13.67872,0.8162009,-0.003106295,0.5777598,201.872 +0.5677095,0.03186389,-0.8226121,195.7323,-0.01557899,0.9994875,0.02796363,-13.66522,0.8230816,-0.003059751,0.5679149,202.2339 +0.5571491,0.02847505,-0.8299241,195.1849,-0.01378596,0.9995913,0.02504155,-13.653,0.830298,-0.002510572,0.557314,202.599 +0.5464707,0.02428149,-0.8371262,194.6237,-0.009969527,0.9996974,0.02248896,-13.64311,0.8374189,-0.003943797,0.5465473,202.9671 +0.5424802,0.02376208,-0.8397325,193.9554,-0.008732601,0.9997054,0.02264747,-13.63086,0.8400232,-0.004952749,0.5425278,203.3509 +0.5384823,0.02325272,-0.842316,193.2881,-0.007496301,0.9997118,0.02280546,-13.61862,0.8426034,-0.005966077,0.5385014,203.7342 +0.5344663,0.02275211,-0.8448835,192.6198,-0.006257289,0.9997167,0.02296335,-13.60636,0.8451666,-0.006986455,0.5344573,204.1179 +0.5304375,0.02226094,-0.8474317,191.9516,-0.005017165,0.9997201,0.02312095,-13.5941,0.8477092,-0.008012507,0.5304007,204.5017 +0.5263973,0.02177941,-0.8499598,191.2835,-0.00377633,0.9997219,0.02327817,-13.58184,0.8502303,-0.009043832,0.5263331,204.8853 +0.522344,0.02130736,-0.8524687,190.6153,-0.002534266,0.9997221,0.0234351,-13.56958,0.8527311,-0.0100808,0.5222528,205.269 +0.5182782,0.02084488,-0.8549581,189.9472,-0.001291088,0.9997208,0.02359171,-13.55732,0.8552111,-0.01112324,0.5181603,205.6527 +0.5142,0.02039202,-0.8574279,189.279,-4.684839E-05,0.999718,0.02374799,-13.54506,0.8576703,-0.01217104,0.5140559,206.0363 +0.5101148,0.01994937,-0.859875,188.6116,0.001196902,0.9997135,0.02390373,-13.53281,0.8601054,-0.01322283,0.5099448,206.4195 +0.5060127,0.01951589,-0.8623053,187.9435,0.00244321,0.9997075,0.02405932,-13.52055,0.8625226,-0.01428111,0.5058169,206.8031 +0.5018984,0.01909215,-0.8647159,187.2753,0.003690598,0.9996999,0.02421458,-13.50828,0.8649187,-0.01534457,0.5016773,207.1867 +0.4977778,0.01867872,-0.8671035,186.608,0.004937409,0.9996908,0.02436927,-13.49603,0.8672905,-0.01641172,0.4975316,207.5698 +0.49364,0.01827458,-0.8694744,185.9399,0.00618688,0.9996801,0.02452381,-13.48376,0.8696443,-0.01748526,0.493369,207.9534 +0.4894959,0.01788079,-0.8718223,185.2726,0.007435807,0.9996678,0.0246778,-13.4715,0.8719739,-0.01856238,0.4892003,208.3365 +0.4853355,0.01749644,-0.874153,184.6045,0.008687236,0.9996539,0.0248316,-13.45923,0.8742848,-0.01964562,0.4850155,208.7201 +0.4806292,0.01721423,-0.876755,183.9471,0.01022455,0.9996293,0.02523176,-13.44679,0.8768643,-0.02109154,0.480275,209.1004 +0.4806903,0.01613907,-0.8767419,183.2041,0.009232008,0.999682,0.02346378,-13.43628,0.8768418,-0.01937289,0.4803885,209.5084 +0.4808418,0.01803466,-0.8766219,182.452,0.009369177,0.9996257,0.02570436,-13.42599,0.8767573,-0.02057295,0.4804928,209.9238 +0.4811037,0.02114766,-0.8764086,181.6952,0.01179488,0.9994623,0.03059172,-13.41608,0.8765843,-0.02505492,0.4805955,210.3474 +0.4812579,0.02621332,-0.8761871,180.9299,0.0130414,0.999228,0.03705757,-13.40242,0.876482,-0.02926094,0.4805444,210.7743 +0.4816574,0.02942202,-0.8758656,180.1626,0.01402504,0.9990495,0.04127269,-13.38372,0.8762473,-0.03216334,0.4807869,211.2023 +0.4822548,0.03144,-0.8754667,179.3913,0.0101567,0.9990879,0.0414744,-13.36666,0.8759721,-0.02889307,0.4814956,211.6252 +0.4833627,0.03187627,-0.8748397,178.6193,0.00506207,0.9992183,0.03920509,-13.34759,0.8754055,-0.02337877,0.4828234,212.0481 +0.4837351,0.0275381,-0.8747812,177.855,0.00553244,0.9993887,0.03452007,-13.32749,0.875197,-0.02153823,0.483287,212.4736 +0.4838463,0.02740406,-0.8747239,177.0882,0.005515287,0.9993943,0.03436058,-13.31082,0.8751356,-0.02144958,0.4834021,212.9016 +0.4835302,0.02769406,-0.8748895,176.3215,0.005234356,0.99939,0.03452794,-13.29865,0.875312,-0.02127478,0.4830902,213.3302 +0.4834054,0.03025502,-0.8748737,175.5541,0.006716409,0.9992449,0.03826717,-13.28287,0.8753708,-0.02437455,0.4828372,213.7612 +0.48284,0.02843214,-0.8752469,174.7915,0.007572747,0.9992998,0.03663956,-13.26848,0.8756758,-0.02431906,0.4822866,214.1877 +0.4827552,0.03095142,-0.8752083,174.0245,0.004922988,0.9992635,0.03805406,-13.25105,0.8757415,-0.02267943,0.4822473,214.6087 +0.4825788,0.03241064,-0.8752527,173.2609,0.005750594,0.9991763,0.04017019,-13.23227,0.8758337,-0.02441849,0.4819948,215.0319 +0.4827705,0.03128451,-0.875188,172.5056,0.00997785,0.9991003,0.04121787,-13.21744,0.87569,-0.02863126,0.482024,215.4573 +0.4824646,0.02676319,-0.8755065,171.7547,0.01122813,0.999262,0.03673373,-13.20315,0.8758435,-0.02755301,0.481808,215.8789 +0.481699,0.01890539,-0.8761328,171.0068,0.01172856,0.9995386,0.02801665,-13.18941,0.8762582,-0.02377135,0.481255,216.2961 +0.4799277,0.01319982,-0.8772088,170.2629,0.01425661,0.9996374,0.02284198,-13.17494,0.8771922,-0.02346852,0.4795655,216.7166 +0.4787752,0.01428541,-0.8778213,169.5153,0.01878649,0.9994719,0.02651152,-13.16501,0.8777365,-0.02918423,0.478254,217.1385 +0.4785343,0.02162892,-0.8778025,168.7701,0.0165383,0.9992972,0.0336384,-13.16245,0.8779131,-0.03061448,0.4778402,217.5493 +0.4782023,0.02460729,-0.877905,168.0394,0.01224111,0.9993235,0.03467844,-13.15694,0.8781644,-0.02732984,0.4775776,217.9446 +0.4774434,0.02113787,-0.8784082,167.3275,0.01284774,0.9994357,0.03103342,-13.14619,0.8785685,-0.02610226,0.4769024,218.336 +0.4764047,0.01744598,-0.8790531,166.6357,0.01827781,0.9993905,0.02973994,-13.13813,0.8790361,-0.0302354,0.4757954,218.7259 +0.4751464,0.01941584,-0.8796926,165.9531,0.01820225,0.9993257,0.03188782,-13.13633,0.8797185,-0.03116376,0.4744725,219.1019 +0.4739792,0.02258406,-0.8802464,165.2855,0.01795794,0.9992152,0.03530606,-13.13005,0.8803529,-0.03254173,0.4732016,219.4665 +0.4714124,0.02039358,-0.8816771,164.6404,0.02144318,0.999172,0.03457647,-13.12098,0.8816521,-0.03520572,0.4705847,219.8238 +0.4679308,0.02019021,-0.8835345,164.0103,0.02243096,0.9991456,0.03471183,-13.11339,0.8834804,-0.03606125,0.4670781,220.1651 +0.4638685,0.02300696,-0.8856053,163.3948,0.01929897,0.999163,0.03606561,-13.10511,0.8856938,-0.03382095,0.4630362,220.4854 +0.4581512,0.02338595,-0.8885666,162.8,0.01781532,0.9992114,0.03548369,-13.09912,0.8886957,-0.03208698,0.4573733,220.7923 +0.4510456,0.02793047,-0.8920638,162.2184,0.01493453,0.999134,0.03883404,-13.0952,0.8923759,-0.03083846,0.4502378,221.0836 +0.4430413,0.03016908,-0.8959935,161.6533,0.01310484,0.9991089,0.04012103,-13.08579,0.8964054,-0.02951711,0.4422511,221.3597 +0.4336028,0.02863614,-0.900649,161.1099,0.01093821,0.999254,0.03703732,-13.0779,0.9010377,-0.02591097,0.4329661,221.6184 +0.4224025,0.02276732,-0.9061224,160.5745,0.01262654,0.9994397,0.03099807,-13.07197,0.9063204,-0.02453484,0.4218783,221.8666 +0.4072767,0.01488145,-0.9131836,160.045,0.01779051,0.9995482,0.02422338,-13.06665,0.9131315,-0.02611161,0.4068279,222.1078 +0.3877417,0.01048569,-0.9217084,159.5105,0.02082597,0.9995804,0.0201326,-13.05916,0.9215327,-0.02700172,0.3873606,222.3344 +0.362898,0.01152449,-0.9317576,158.9597,0.02048369,0.9995832,0.02034132,-13.0475,0.9316037,-0.02646765,0.3625106,222.5303 +0.3341617,0.01780759,-0.9423475,158.4078,0.01731034,0.9995369,0.02502665,-13.04181,0.9423567,-0.02467529,0.3336987,222.7128 +0.3033347,0.024376,-0.9525723,157.8455,0.01333916,0.9994661,0.02982369,-13.03627,0.9527907,-0.02175307,0.3028476,222.8609 +0.2701181,0.02950691,-0.962375,157.2935,0.01067282,0.9993771,0.03363705,-13.03293,0.962768,-0.01935722,0.2696349,223.0029 +0.2350725,0.03438459,-0.9713695,156.7418,0.006247567,0.9993,0.0368852,-13.02324,0.9719577,-0.01473939,0.2346931,223.1194 +0.1995452,0.0376431,-0.9791653,156.1813,0.003874573,0.9992237,0.03920384,-13.01146,0.9798809,-0.01161678,0.1992444,223.2029 +0.1643203,0.03965515,-0.9856096,155.633,0.001418464,0.999181,0.04043768,-12.99975,0.986406,-0.008042776,0.1641294,223.2797 +0.1295844,0.0415662,-0.9906968,155.0797,-6.588615E-05,0.9991213,0.04191105,-12.98801,0.9915683,-0.005365741,0.1294733,223.3373 +0.09428605,0.04334018,-0.9946013,154.5111,-0.002901006,0.9990596,0.04325945,-12.9757,0.9955409,-0.001193413,0.09432311,223.364 +0.05949781,0.0423045,-0.9973316,153.9455,-0.007614139,0.9990917,0.04192493,-12.96146,0.9981994,0.005099384,0.05976588,223.3808 +0.02711923,0.04203405,-0.9987481,153.3752,-0.01414789,0.9990316,0.04166182,-12.94579,0.999532,0.01300035,0.02768766,223.3752 +-0.0008222368,0.04416913,-0.9990238,152.7869,-0.01960906,0.9988312,0.04417676,-12.92888,0.9998073,0.01962624,4.483795E-05,223.3501 +-0.02354346,0.04834786,-0.9985531,152.1943,-0.01988611,0.9986096,0.04881948,-12.90903,0.999525,0.02100672,-0.02254927,223.327 +-0.04207706,0.05533234,-0.997581,151.5876,-0.02064122,0.998204,0.05623753,-12.88729,0.9989011,0.0229576,-0.04085936,223.2912 +-0.05590427,0.06205942,-0.9965056,150.969,-0.02323179,0.9977153,0.06343808,-12.86332,0.9981658,0.02669706,-0.0543348,223.245 +-0.06760167,0.06335309,-0.995699,150.3433,-0.02894645,0.9974373,0.06542899,-12.83453,0.9972924,0.03324506,-0.06559458,223.1852 +-0.07737906,0.06446982,-0.9949152,149.7033,-0.03350369,0.9971753,0.06722203,-12.80137,0.9964386,0.03853491,-0.07500051,223.1223 +-0.08222978,0.06470021,-0.994511,149.0484,-0.03312732,0.9971616,0.06761175,-12.76027,0.9960626,0.03850518,-0.07985303,223.0606 +-0.08474431,0.06409654,-0.994339,148.3828,-0.02982754,0.9973184,0.06683071,-12.71981,0.9959561,0.03532221,-0.08260521,223.0039 +-0.0868399,0.06418345,-0.9941526,147.6974,-0.02443302,0.997485,0.06653285,-12.68253,0.9959226,0.03006785,-0.0850533,222.9478 +-0.08789487,0.06205017,-0.9941953,146.9992,-0.02222927,0.9976873,0.06423337,-12.64604,0.9958817,0.02774602,-0.08631226,222.8867 +-0.08824412,0.057335,-0.9944474,146.2857,-0.01887895,0.9980665,0.05921892,-12.60905,0.9959199,0.02399985,-0.08699107,222.8246 +-0.0881659,0.05293815,-0.9946981,145.5594,-0.01384197,0.9984252,0.05436342,-12.57239,0.9960096,0.01856159,-0.08729429,222.768 +-0.0879691,0.05058553,-0.994838,144.8114,-0.01098608,0.9985997,0.05174827,-12.53739,0.9960626,0.01548162,-0.08729018,222.7091 +-0.08706422,0.05263787,-0.9948111,144.0411,-0.01053615,0.9984985,0.0537551,-12.50132,0.9961469,0.01516162,-0.08637889,222.6439 +-0.08567685,0.05812097,-0.9946263,143.2551,-0.01049497,0.998189,0.0592332,-12.46716,0.9962677,0.01551349,-0.0849117,222.5763 +-0.08373952,0.06198262,-0.9945581,142.4524,-0.009759367,0.9979647,0.06301665,-12.42912,0.9964398,0.01498324,-0.08296417,222.5073 +-0.08115212,0.05786517,-0.9950206,141.6445,-0.0121495,0.9981817,0.0590399,-12.39462,0.9966276,0.01688022,-0.08030152,222.4364 +-0.07746158,0.04914391,-0.9957834,140.8253,-0.01369209,0.9986378,0.05034989,-12.36158,0.9969013,0.01753454,-0.07668317,222.3675 +-0.07342452,0.04264017,-0.9963888,139.9857,-0.01080014,0.998993,0.04354749,-12.3258,0.9972423,0.01395859,-0.07289005,222.3071 +-0.06883671,0.04252423,-0.9967213,139.1198,-0.004787056,0.9990655,0.04295487,-12.2907,0.9976164,0.007728236,-0.06856882,222.2589 +-0.06404852,0.04637848,-0.9968685,138.2383,1.526372E-05,0.9989195,0.04647293,-12.26035,0.9979467,0.002961311,-0.06398002,222.2126 +-0.05971975,0.04959708,-0.9969823,137.3423,0.0007468267,0.9987668,0.04964113,-12.22648,0.9982149,0.002219988,-0.05968314,222.1626 +-0.05528257,0.05117367,-0.9971585,136.4334,0.002871919,0.9986897,0.05109304,-12.18824,0.9984666,-3.919844E-05,-0.0553571,222.1176 +-0.05123481,0.05504755,-0.9971684,135.5059,0.004787304,0.9984818,0.05487409,-12.14967,0.9986751,-0.001962279,-0.05142055,222.0757 +-0.04778735,0.05725098,-0.9972155,134.5688,0.00307898,0.9983597,0.05716914,-12.10689,0.9988527,-0.0003384395,-0.04788524,222.0299 +-0.04498307,0.05438516,-0.9975063,133.6194,0.008721299,0.9985003,0.05404608,-12.0638,0.9989496,-0.006268385,-0.04538992,221.9953 +-0.04329342,0.0490787,-0.9978562,132.6636,0.01847181,0.9986612,0.04831688,-12.02327,0.9988916,-0.0163404,-0.04414203,221.9695 +-0.04287589,0.0405653,-0.9982566,131.6955,0.01911436,0.9990258,0.03977559,-11.98937,0.9988975,-0.01737561,-0.04360949,221.9321 +-0.04281996,0.03536587,-0.9984567,130.7148,0.02188384,0.9991667,0.03445251,-11.95903,0.9988431,-0.0203748,-0.04355822,221.8936 +-0.04313959,0.0328378,-0.9985293,129.7209,0.02569699,0.9991655,0.03174854,-11.93204,0.9987385,-0.02428956,-0.04394742,221.858 +-0.04221006,0.03988414,-0.9983124,128.711,0.01902553,0.9990538,0.03910934,-11.90519,0.9989276,-0.01734261,-0.04292894,221.803 +-0.04245654,0.05257085,-0.9977143,127.6893,0.01468555,0.9985396,0.05198943,-11.87237,0.9989903,-0.01244468,-0.04316657,221.7498 +-0.04420843,0.06335304,-0.9970116,126.6632,0.01248776,0.9979443,0.0628586,-11.8311,0.9989442,-0.009671552,-0.04490869,221.6993 +-0.04653768,0.06326343,-0.9969112,125.6473,0.0128992,0.9979473,0.06272703,-11.78857,0.9988332,-0.009940182,-0.0472582,221.6501 +-0.04920968,0.05379329,-0.9973388,124.6384,0.01758151,0.9984405,0.05298524,-11.74693,0.9986337,-0.01492732,-0.0500787,221.6057 +-0.0520937,0.04310859,-0.9977114,123.6299,0.01937767,0.9989234,0.0421492,-11.71079,0.9984541,-0.0171376,-0.05287295,221.5549 +-0.05475037,0.04017399,-0.9976916,122.6154,0.02046168,0.9990256,0.03910484,-11.68307,0.9982904,-0.01827343,-0.05551905,221.5 +-0.05755884,0.04181232,-0.9974662,121.5907,0.02072184,0.9989573,0.04067908,-11.65845,0.998127,-0.01832789,-0.05836525,221.4404 +-0.06016527,0.04621742,-0.9971179,120.5664,0.01831304,0.9988105,0.04519089,-11.63185,0.9980204,-0.01554133,-0.06094008,221.3745 +-0.06172986,0.05304327,-0.9966824,119.5438,0.01870497,0.9984729,0.05198007,-11.598,0.9979176,-0.01543418,-0.06262776,221.3095 +-0.06299868,0.06183956,-0.9960959,118.5197,0.01829928,0.9979822,0.06079933,-11.56188,0.9978458,-0.01439755,-0.06400317,221.243 +-0.06395264,0.06360303,-0.9959241,117.5108,0.01491932,0.9979163,0.06277224,-11.5245,0.9978414,-0.01084405,-0.0647683,221.1714 +-0.06511373,0.05612872,-0.9962981,116.5115,0.01335764,0.9983764,0.05537282,-11.48765,0.9977884,-0.009702649,-0.06575776,221.1021 +-0.0660084,0.04829726,-0.9966495,115.5151,0.01510763,0.9987618,0.04739904,-11.45162,0.9977046,-0.01192827,-0.06665632,221.0361 +-0.06649185,0.04678938,-0.9966893,114.5179,0.0145172,0.9988395,0.04592185,-11.42118,0.9976813,-0.0114157,-0.06709393,220.9683 +-0.06611564,0.05211902,-0.9964499,113.5168,0.01660082,0.9985541,0.05112761,-11.39041,0.9976738,-0.01316154,-0.06688526,220.9036 +-0.06598619,0.05817925,-0.996123,112.5172,0.01694346,0.9982201,0.05717936,-11.35672,0.9976766,-0.01310471,-0.0668545,220.8376 +-0.06538092,0.06172327,-0.9959496,111.5259,0.017061,0.9980083,0.06073087,-11.32203,0.9977145,-0.01302125,-0.06630376,220.7722 +-0.06438417,0.0584791,-0.9962103,110.5452,0.01706225,0.9982001,0.05749319,-11.28441,0.9977793,-0.01329593,-0.06526606,220.708 +-0.06288162,0.05270532,-0.9966284,109.5712,0.01755352,0.9985085,0.05169724,-11.24841,0.9978666,-0.01424352,-0.06371299,220.6447 +-0.06185701,0.04867887,-0.9968973,108.6011,0.01910165,0.9986847,0.04758091,-11.22102,0.9979022,-0.01609916,-0.06270549,220.5858 +-0.06101832,0.04141712,-0.997277,107.6362,0.0227587,0.9989367,0.04009357,-11.19656,0.9978771,-0.02025028,-0.06189603,220.5323 +-0.0601669,0.03843186,-0.9974482,106.6654,0.02383373,0.9990289,0.0370551,-11.17178,0.9979037,-0.02154341,-0.06102445,220.4773 +-0.05900779,0.04480234,-0.9972517,105.6915,0.02679659,0.9987034,0.043282,-11.14687,0.9978978,-0.02416896,-0.06013183,220.4242 +-0.05762142,0.0528411,-0.9969391,104.7172,0.03046797,0.9982262,0.05114833,-11.12293,0.9978734,-0.02742746,-0.05912917,220.3737 +-0.05614698,0.0531331,-0.9970077,103.7569,0.03048556,0.9982086,0.0514803,-11.09404,0.9979569,-0.02750386,-0.05766618,220.3198 +-0.05524411,0.05181467,-0.9971276,102.7971,0.02953877,0.9983003,0.05023907,-11.06032,0.9980358,-0.0266785,-0.05668075,220.2638 +-0.05532778,0.05055064,-0.9971878,101.8406,0.03512902,0.9981978,0.04865276,-11.03196,0.99785,-0.03233837,-0.05700386,220.2186 +-0.05566757,0.04917557,-0.9972377,100.8893,0.03771766,0.998177,0.04711643,-11.00563,0.9977366,-0.0349906,-0.05742088,220.1704 +-0.05618866,0.04662701,-0.9973308,99.9429,0.03818941,0.9982783,0.04451976,-10.97856,0.9976895,-0.03558596,-0.05787257,220.117 +-0.05738883,0.05079896,-0.9970587,98.9947,0.04214831,0.9979375,0.04841776,-10.95124,0.9974618,-0.03924569,-0.05941155,220.0673 +-0.05917256,0.05669648,-0.9966364,98.04753,0.0454457,0.9975036,0.05404761,-10.92193,0.9972127,-0.04209469,-0.06160145,220.0164 +-0.06122286,0.05897351,-0.9963804,97.1087,0.04446148,0.9974232,0.05630329,-10.89294,0.9971333,-0.04085348,-0.06368715,219.9567 +-0.06392814,0.05713134,-0.9963179,96.18155,0.04447136,0.9975313,0.05434745,-10.86098,0.9969631,-0.04083327,-0.06631102,219.8951 +-0.06716587,0.05803618,-0.9960525,95.25961,0.04472701,0.9974784,0.05510324,-10.82502,0.9967388,-0.04084938,-0.06959229,219.8316 +-0.07007779,0.06227517,-0.9955958,94.34372,0.04210452,0.9973446,0.05942093,-10.7924,0.9966525,-0.03775498,-0.07251377,219.7619 +-0.07311413,0.06435516,-0.9952451,93.43862,0.03880986,0.9973436,0.06163977,-10.76804,0.9965681,-0.03411857,-0.07541753,219.6865 +-0.07611799,0.06110345,-0.9952248,92.56677,0.03729284,0.9975966,0.05839681,-10.74452,0.9964011,-0.0326697,-0.07821377,219.6263 +-0.0788636,0.05909924,-0.9951321,91.70406,0.03778002,0.9977012,0.05625779,-10.71985,0.9961692,-0.0331594,-0.08091508,219.5641 +-0.08095666,0.05858101,-0.9949946,90.85546,0.03758085,0.9977409,0.05568498,-10.69919,0.9960088,-0.03288466,-0.08297529,219.5006 +-0.08221203,0.05828648,-0.994909,90.01191,0.03510154,0.9978383,0.05555757,-10.674,0.9959965,-0.03035533,-0.08408025,219.4376 +-0.08265354,0.056878,-0.9949539,89.18744,0.03360684,0.9979612,0.05425812,-10.65382,0.9960115,-0.02895262,-0.08439651,219.3826 +-0.08290909,0.05828925,-0.994851,88.38861,0.03293543,0.9979029,0.0557233,-10.63021,0.9960127,-0.02814586,-0.084655,219.3263 +-0.08260331,0.06002373,-0.9947733,87.60818,0.03067178,0.9978648,0.05766337,-10.60537,0.9961104,-0.02574827,-0.08426797,219.2647 +-0.0807529,0.06138104,-0.9948424,86.85337,0.02736189,0.9978624,0.05934637,-10.58478,0.9963585,-0.02242836,-0.08225978,219.2079 +-0.07789468,0.06278269,-0.9949828,86.12368,0.02616745,0.9978001,0.06091188,-10.56501,0.9966181,-0.02129145,-0.07936617,219.1578 +-0.07335971,0.06360745,-0.9952751,85.41269,0.02694566,0.9977261,0.06177799,-10.54111,0.9969414,-0.02228632,-0.07490684,219.1163 +-0.06694243,0.06464425,-0.9956605,84.72207,0.02633264,0.9976658,0.063004,-10.51592,0.9974093,-0.02200072,-0.06848842,219.0786 +-0.05900766,0.06581064,-0.9960859,84.04997,0.02404269,0.9976288,0.06448831,-10.49659,0.9979679,-0.02014327,-0.06045,219.0453 +-0.04825011,0.06534961,-0.9966952,83.40009,0.02226408,0.9976799,0.06433637,-10.47658,0.9985871,-0.01908626,-0.04959311,219.0227 +-0.03465075,0.06271342,-0.9974299,82.76889,0.02162952,0.9978425,0.06198796,-10.45391,0.9991654,-0.01942599,-0.03593245,219.0137 +-0.01800414,0.05973792,-0.9980517,82.15288,0.02218497,0.9979916,0.05933413,-10.43065,0.9995917,-0.02107348,-0.01929327,219.0163 +0.002347454,0.05836181,-0.9982928,81.55702,0.02469122,0.9979877,0.05840204,-10.4104,0.9996923,-0.02478615,0.0009017052,219.0383 +0.02461488,0.05794631,-0.9980162,80.97264,0.02475644,0.9979772,0.05855464,-10.39008,0.9993904,-0.02614864,0.02313054,219.0755 +0.04988773,0.05871985,-0.9970272,80.38801,0.02281815,0.9979426,0.05991552,-10.36871,0.9984941,-0.02573935,0.0484452,219.1151 +0.07814719,0.05980696,-0.9951463,79.80795,0.01925929,0.9979221,0.06148619,-10.34835,0.9967557,-0.02397078,0.07683296,219.1809 +0.1088557,0.06069805,-0.9922027,79.2293,0.0161318,0.9978947,0.06281611,-10.33041,0.9939266,-0.0228439,0.1076474,219.2545 +0.1428723,0.05949249,-0.9879515,78.66476,0.01501882,0.9979465,0.06226632,-10.31093,0.9896271,-0.02373399,0.1416854,219.3662 +0.1799362,0.05567118,-0.9821017,78.11041,0.01108945,0.998219,0.05861657,-10.28769,0.9836157,-0.02143821,0.1789984,219.497 +0.2187947,0.05142446,-0.9744149,77.55609,0.003083292,0.9985689,0.05339151,-10.26351,0.975766,-0.01468618,0.218323,219.6228 +0.260082,0.04854472,-0.9643655,77.01629,-0.003204376,0.9987733,0.04941257,-10.24585,0.9655812,-0.009761125,0.2599185,219.796 +0.304058,0.04761603,-0.9514628,76.47664,-0.01056109,0.9988572,0.04661289,-10.23081,0.952595,-0.004124528,0.3042133,219.9684 +0.3515675,0.04739889,-0.9349619,75.96056,-0.02008972,0.9988694,0.04308455,-10.21989,0.9359469,0.003636004,0.3521222,220.1937 +0.401591,0.04747507,-0.9145878,75.46148,-0.03139309,0.9987822,0.03806095,-10.20937,0.9152809,0.0134268,0.4025923,220.4461 +0.4529098,0.04937159,-0.8901883,74.96605,-0.03240722,0.9987173,0.03890269,-10.19899,0.8909671,0.01122912,0.4539289,220.705 +0.5044661,0.05053854,-0.8619512,74.50344,-0.03434884,0.9986699,0.03845172,-10.18687,0.862748,0.01020944,0.505531,221.0328 +0.5567192,0.05307627,-0.8290034,74.06909,-0.03582561,0.9985623,0.03987337,-10.18099,0.8299278,0.007501287,0.5578203,221.4112 +0.60711,0.05575844,-0.7926592,73.63537,-0.03721733,0.9984356,0.04172815,-10.17688,0.7937458,0.004167086,0.6082353,221.7675 +0.6542676,0.0530195,-0.7544024,73.24704,-0.03489686,0.9985934,0.03991643,-10.17531,0.7554576,0.0002102548,0.6551975,222.197 +0.6975065,0.04789029,-0.7149764,72.86285,-0.03329548,0.9988526,0.03442285,-10.17148,0.7158045,-0.0002046786,0.6983006,222.604 +0.7374695,0.04549857,-0.6738461,72.50725,-0.03816655,0.9989414,0.02567906,-10.17451,0.6743011,0.006780861,0.7384253,223.0666 +0.7744188,0.0444307,-0.6311113,72.1689,-0.04684947,0.9988195,0.01282998,-10.17618,0.6309363,0.01963145,0.7755862,223.5403 +0.8082989,0.0371741,-0.5875977,71.84185,-0.0455402,0.9989623,0.0005538454,-10.17024,0.5870086,0.02631164,0.809153,224.0153 +0.8396443,0.02758955,-0.5424355,71.55033,-0.03608562,0.999336,-0.005028881,-10.17198,0.5419366,0.02379659,0.8400823,224.5696 +0.8685969,0.02232557,-0.4950162,71.28157,-0.02902154,0.9995617,-0.005842703,-10.1765,0.4946688,0.01944109,0.868864,225.1433 +0.894254,0.02215078,-0.4470115,71.02342,-0.02724884,0.9996163,-0.00497772,-10.18054,0.4467296,0.01663189,0.8945143,225.6999 +0.9164636,0.02299835,-0.3994566,70.7974,-0.02779695,0.9995942,-0.006223134,-10.18818,0.3991513,0.01680695,0.9167309,226.3027 +0.9354029,0.01930074,-0.3530566,70.60149,-0.02337059,0.9997004,-0.007267829,-10.19211,0.3528105,0.01504949,0.9355737,226.9267 +0.9513064,0.0174432,-0.3077531,70.42167,-0.0208444,0.9997525,-0.007767681,-10.20149,0.3075414,0.01380437,0.9514345,227.54 +0.9646614,0.02045116,-0.2626979,70.26524,-0.02391619,0.9996639,-0.009999105,-10.21211,0.2624051,0.01592848,0.9648263,228.1954 +0.975325,0.02066793,-0.2198048,70.1277,-0.02435327,0.9996044,-0.01406976,-10.22504,0.2194271,0.01907556,0.9754424,228.8409 +0.9831866,0.009612003,-0.182351,70.03455,-0.01409269,0.9996294,-0.02329187,-10.24406,0.1820595,0.02547007,0.9829575,229.5184 +0.9884949,0.002776275,-0.1512287,69.95561,-0.00782138,0.9994321,-0.03277615,-10.26536,0.1510518,0.03358187,0.9879552,230.2032 +0.9923675,0.0006593881,-0.1233145,69.8783,-0.005183574,0.9993249,-0.03637096,-10.2848,0.1232073,0.03673256,0.9917008,230.8964 +0.9949636,0.00112098,-0.1002306,69.81376,-0.00437274,0.9994709,-0.03222898,-10.30134,0.1001415,0.03250494,0.9944421,231.6107 +0.9966757,0.001090469,-0.0814639,69.76113,-0.002871854,0.9997592,-0.0217532,-10.3178,0.08142056,0.02191483,0.9964388,232.3354 +0.997882,0.003943078,-0.06493113,69.71324,-0.004686066,0.9999252,-0.01129438,-10.33202,0.06488174,0.01157473,0.9978258,233.0647 +0.9986297,0.0009290676,-0.05232624,69.67008,-0.001331731,0.9999697,-0.007660902,-10.3491,0.05231754,0.007720087,0.9986006,233.8003 +0.9989955,-0.007581621,-0.04416583,69.63999,0.007267931,0.9999472,-0.00725883,-10.36057,0.04421853,0.006930543,0.9989978,234.535 +0.9991442,-0.01472181,-0.03865457,69.62499,0.01426981,0.9998268,-0.01194338,-10.37864,0.0388237,0.01138156,0.9991812,235.2773 +0.999235,-0.01811761,-0.03465922,69.60369,0.0175487,0.9997074,-0.01664893,-10.40114,0.03495071,0.01602797,0.9992604,236.0228 +0.9992565,-0.01966312,-0.03316507,69.5821,0.01904407,0.9996404,-0.01887979,-10.42199,0.03352438,0.01823415,0.9992715,236.7854 +0.9992235,-0.02170496,-0.03288369,69.56092,0.02109931,0.9996033,-0.01865437,-10.44222,0.03327554,0.01794606,0.999285,237.5595 +0.999266,-0.02181829,-0.03148786,69.54591,0.02136719,0.9996652,-0.01459219,-10.47452,0.03179569,0.01390867,0.9993976,238.3426 +0.9991444,-0.02696035,-0.03136284,69.53554,0.02653211,0.99955,-0.01399132,-10.5086,0.03172594,0.01314723,0.9994101,239.133 +0.999083,-0.02883186,-0.03165456,69.52027,0.02828358,0.9994444,-0.01763417,-10.54974,0.0321454,0.01672269,0.9993432,239.9198 +0.9989859,-0.03182985,-0.03184474,69.4994,0.03099191,0.9991691,-0.02646982,-10.59153,0.03266081,0.02545604,0.9991422,240.7197 +0.9986173,-0.0410362,-0.03285827,69.48807,0.0399536,0.998658,-0.03295314,-10.63608,0.03416644,0.03159477,0.9989166,241.5298 +0.9980863,-0.05133182,-0.03448041,69.48017,0.05006289,0.9980712,-0.03670878,-10.682,0.03629823,0.03491234,0.9987309,242.3555 +0.9975505,-0.05984302,-0.03621931,69.4694,0.0585358,0.997631,-0.03613659,-10.73043,0.03829603,0.03392794,0.9986902,243.1928 +0.9971341,-0.06525994,-0.03827178,69.44972,0.06404611,0.9974296,-0.03212918,-10.77141,0.04027015,0.02958594,0.9987507,244.0434 +0.9969578,-0.06618579,-0.04116713,69.42243,0.06520826,0.9975671,-0.02465281,-10.81483,0.04269864,0.02189337,0.998848,244.9043 +0.9971171,-0.0620416,-0.04368616,69.38453,0.0614172,0.9979919,-0.01549436,-10.85518,0.04455973,0.01276661,0.9989251,245.7764 +0.9973962,-0.05583721,-0.04564079,69.33841,0.05536521,0.9983994,-0.01154223,-10.88638,0.04621222,0.008985264,0.9988912,246.6525 +0.9976785,-0.04975729,-0.04649607,69.28696,0.04942183,0.9987432,-0.008337367,-10.91134,0.04685248,0.00602009,0.9988836,247.5373 +0.9980139,-0.04151412,-0.0473797,69.23258,0.04127318,0.9991295,-0.006052836,-10.93709,0.04758973,0.004085304,0.9988586,248.4328 +0.9980674,-0.03988496,-0.04765209,69.18578,0.03938421,0.9991591,-0.01140197,-10.96374,0.04806679,0.009503193,0.9987989,249.3312 +0.9980885,-0.03886791,-0.04804959,69.14107,0.03788594,0.9990575,-0.02118163,-10.99093,0.04882758,0.01932074,0.9986203,250.2287 +0.9981421,-0.03774956,-0.04782585,69.09464,0.03644718,0.9989483,-0.02781761,-11.02141,0.04882565,0.0260228,0.9984682,251.146 +0.9981996,-0.03659371,-0.04752496,69.04817,0.03529995,0.9989905,-0.02778274,-11.05722,0.04849366,0.02605508,0.9984835,252.078 +0.9983881,-0.03239677,-0.04660225,68.99988,0.03137572,0.9992549,-0.02247727,-11.09595,0.04729572,0.02097885,0.9986605,253.0256 +0.9985908,-0.02825775,-0.04492132,68.95099,0.02752156,0.9994779,-0.01692345,-11.12932,0.04537609,0.01566329,0.9988471,253.9827 +0.9987636,-0.02409933,-0.04348121,68.90225,0.02361257,0.999653,-0.01167392,-11.15833,0.04374745,0.01063278,0.998986,254.9496 +0.998882,-0.02175531,-0.04197082,68.85691,0.02148102,0.9997449,-0.006975412,-11.18181,0.04211186,0.006066037,0.9990944,255.9309 +0.9989956,-0.01902185,-0.04057092,68.81186,0.01877924,0.9998034,-0.006352704,-11.20662,0.04068378,0.005584432,0.9991564,256.9171 +0.9990138,-0.01948392,-0.03989736,68.75672,0.01901111,0.9997449,-0.0121961,-11.2429,0.04012481,0.01142557,0.9991293,257.9108 +0.9989758,-0.02059777,-0.04028771,68.7036,0.01964366,0.9995204,-0.02393674,-11.28031,0.04076144,0.02312082,0.9989013,258.9078 +0.99887,-0.02360822,-0.04124983,68.65058,0.02247757,0.9993646,-0.02766209,-11.3195,0.04187667,0.02670363,0.9987658,259.9201 +0.9988001,-0.02224466,-0.04363108,68.58719,0.02149665,0.999615,-0.01753887,-11.36666,0.04400443,0.0165799,0.9988937,260.9702 +0.9987453,-0.02181438,-0.04507869,68.51787,0.02156364,0.9997492,-0.006041118,-11.41027,0.04519916,0.005061477,0.9989651,262.0295 +0.9987029,-0.02133334,-0.04623277,68.4622,0.02114072,0.9997657,-0.004651451,-11.4422,0.04632117,0.003668024,0.9989198,263.0821 +0.9986435,-0.02185571,-0.04725989,68.40379,0.02141395,0.9997223,-0.009833753,-11.46949,0.04746168,0.008808392,0.9988342,264.1348 +0.9985251,-0.02479338,-0.04830024,68.35062,0.02404452,0.9995824,-0.0160241,-11.49978,0.04867736,0.01483911,0.9987043,265.1864 +0.9985002,-0.02354771,-0.04942683,68.28856,0.02272417,0.9995945,-0.01715826,-11.53683,0.04981082,0.01600934,0.9986303,266.2528 +0.9984717,-0.02300823,-0.05024907,68.22673,0.02225496,0.9996322,-0.01549925,-11.58104,0.05058719,0.01435726,0.9986164,267.3248 +0.9984219,-0.02326774,-0.05111194,68.16394,0.02264712,0.999663,-0.01268812,-11.61771,0.05138993,0.01151055,0.9986123,268.396 +0.9984721,-0.01969636,-0.05162865,68.09665,0.01915398,0.9997562,-0.01097931,-11.65107,0.05183231,0.009973643,0.9986059,269.4641 +0.9984899,-0.01798414,-0.05191008,68.03033,0.01764011,0.9998193,-0.007078199,-11.68154,0.05202799,0.00615181,0.9986266,270.5387 +0.9984746,-0.0176471,-0.05231817,67.96554,0.01742231,0.9998369,-0.004749557,-11.7088,0.05239345,0.003830809,0.9986191,271.6135 +0.9984527,-0.01851719,-0.05243392,67.90383,0.01826667,0.9998193,-0.00525317,-11.73743,0.05252172,0.004287249,0.9986105,272.683 +0.9984002,-0.02122321,-0.05240813,67.84423,0.02077418,0.9997428,-0.009098062,-11.76733,0.05258773,0.007994771,0.9985842,273.7493 +0.9983175,-0.02270571,-0.0533548,67.78534,0.02193455,0.9996469,-0.01499507,-11.79767,0.05367644,0.01379952,0.998463,274.8066 +0.9982677,-0.0224983,-0.05436489,67.72147,0.0217101,0.9996511,-0.01504573,-11.83323,0.05468442,0.01383939,0.9984077,275.8724 +0.9981288,-0.02490332,-0.05584548,67.65981,0.02408004,0.9995919,-0.01536704,-11.87068,0.05620538,0.01399353,0.9983211,276.9334 +0.9980663,-0.02474973,-0.05702002,67.59292,0.02360278,0.999507,-0.02070134,-11.91077,0.05750426,0.01931548,0.9981583,277.9886 +0.9980204,-0.02436777,-0.05797948,67.52546,0.02311648,0.9994872,-0.02215554,-11.95025,0.05848962,0.0207714,0.9980718,279.0433 +0.9979945,-0.0228451,-0.05903584,67.45634,0.02200462,0.9996476,-0.01484803,-11.98777,0.05935424,0.01351919,0.9981454,280.1032 +0.9980009,-0.02060482,-0.05974734,67.38662,0.02029993,0.9997776,-0.005705626,-12.02617,0.05985162,0.004481353,0.9981972,281.1665 +0.9980446,-0.01973479,-0.05930918,67.31745,0.01948727,0.9997988,-0.004748994,-12.05628,0.05939097,0.003583934,0.9982283,282.2191 +0.9981253,-0.01853929,-0.05832924,67.24878,0.01794236,0.9997813,-0.01074103,-12.08302,0.05851562,0.009674331,0.9982396,283.2603 +0.9982273,-0.01630187,-0.05724079,67.17999,0.01546716,0.9997679,-0.01499546,-12.11419,0.05747196,0.01408352,0.9982477,284.3009 +0.9983448,-0.01353025,-0.05589911,67.11165,0.01263846,0.9997876,-0.01627634,-12.14831,0.05610746,0.01554291,0.9983037,285.3435 +0.9984509,-0.01058534,-0.05462534,67.04325,0.00985309,0.9998582,-0.01365697,-12.18235,0.05476216,0.01309758,0.9984135,286.3985 +0.9985405,-0.008388309,-0.05335305,66.97369,0.007803442,0.9999072,-0.0111611,-12.20862,0.05344172,0.01072847,0.9985133,287.4563 +0.9986095,-0.004680366,-0.05250875,66.90135,0.004203729,0.999949,-0.009184071,-12.22821,0.05254905,0.008950567,0.9985782,288.5084 +0.9986289,-0.004086813,-0.0521885,66.82893,0.003523414,0.9999345,-0.01088291,-12.24028,0.05222956,0.01068411,0.9985779,289.5582 +0.9986271,-0.00426692,-0.05220891,66.75997,0.003505086,0.9998862,-0.01467492,-12.25277,0.05226558,0.01447177,0.9985283,290.6051 +0.9986449,-0.002474452,-0.05198401,66.68814,0.001581929,0.9998507,-0.01720332,-12.25402,0.05201882,0.01709777,0.9984997,291.6524 +0.9986721,-0.00118212,-0.0515054,66.6192,0.0003892283,0.9998813,-0.01540165,-12.27164,0.05151749,0.01536115,0.9985539,292.6968 +0.9987046,0.0007320975,-0.05087911,66.54974,-0.001246394,0.9999484,-0.0100772,-12.28572,0.0508691,0.01012756,0.9986539,293.7418 +0.9987328,0.001815456,-0.05029582,66.4788,-0.002168904,0.9999733,-0.006973696,-12.29898,0.05028182,0.007073945,0.99871,294.7822 +0.9987191,0.00537314,-0.0503137,66.40794,-0.005693581,0.9999644,-0.006227691,-12.31508,0.05027844,0.006506178,0.998714,295.8182 +0.9986934,0.0004446866,-0.05110187,66.34676,-0.0007859861,0.9999775,-0.006658906,-12.32714,0.05109775,0.00669037,0.9986712,296.8507 +0.9985957,-0.004913242,-0.05274987,66.28919,0.004500582,0.9999583,-0.00793891,-12.34465,0.05278668,0.007690355,0.9985761,297.8782 +0.9984951,-0.005923744,-0.05452091,66.22145,0.005322213,0.9999234,-0.01117163,-12.36302,0.05458291,0.01086464,0.9984501,298.9033 +0.9983993,-0.002114474,-0.05651912,66.14628,0.001430187,0.9999252,-0.01214489,-12.38715,0.05654057,0.01204461,0.9983276,299.9198 +0.9982359,-0.006522325,-0.05901408,66.07807,0.005870068,0.9999198,-0.01121919,-12.40214,0.05908252,0.01085298,0.998194,300.9445 +0.9980499,-0.004405967,-0.06226632,65.99903,0.004280742,0.9999885,-0.002144377,-12.42256,0.06227505,0.001873649,0.9980572,301.9709 +0.997903,0.002958168,-0.06466081,65.90919,-0.002626042,0.9999829,0.005220821,-12.43954,0.06467515,-0.005040069,0.9978936,302.991 +0.9976877,0.002631545,-0.06791529,65.82627,-0.002309656,0.9999857,0.004817655,-12.45262,0.067927,-0.004649652,0.9976794,304.0085 +0.9974702,0.0005610345,-0.07108405,65.74479,-0.0004871111,0.9999993,0.001057273,-12.46723,0.07108459,-0.001019972,0.9974697,305.0138 +0.9972165,-0.004010025,-0.07445359,65.66126,0.003499052,0.9999694,-0.006992146,-12.47957,0.07447934,0.006712166,0.9971999,306.0167 +0.9969554,-0.007998305,-0.07756322,65.57359,0.006892605,0.9998709,-0.01451272,-12.49164,0.07766928,0.01393392,0.9968817,307.0182 +0.9966494,-0.0106087,-0.08110156,65.48054,0.009503459,0.9998568,-0.01400173,-12.50702,0.08123848,0.01318407,0.9966074,308.0259 +0.9962654,-0.01226778,-0.0854688,65.38313,0.01142528,0.9998812,-0.01033961,-12.53112,0.08558549,0.00932449,0.9962871,309.0388 +0.995838,-0.009738008,-0.09062021,65.27471,0.008466516,0.9998604,-0.01440487,-12.55384,0.09074782,0.01357767,0.9957813,310.0323 +0.9953505,-0.01200291,-0.09556931,65.16792,0.01014763,0.9997509,-0.01987533,-12.57651,0.09578407,0.01881311,0.9952243,311.0292 +0.9949651,-0.00861155,-0.09985154,65.04766,0.006669039,0.9997823,-0.01977151,-12.60101,0.1000001,0.01900604,0.9948058,312.0284 +0.9944586,-0.007137047,-0.1048869,64.92447,0.005748209,0.9998918,-0.01353763,-12.63184,0.1049721,0.0128597,0.994392,313.0306 +0.993846,-0.002837663,-0.1107349,64.79958,0.002083452,0.9999738,-0.006926077,-12.65875,0.1107517,0.006652743,0.9938258,314.0236 +0.993238,-0.003616121,-0.1160402,64.67468,0.002723054,0.9999654,-0.007853797,-12.6826,0.1160646,0.007484705,0.9932134,315.0083 +0.9927316,-0.0001882054,-0.12035,64.54061,-0.000751016,0.9999696,-0.00775868,-12.70404,0.1203478,0.007792671,0.9927012,315.9953 +0.9923847,0.008974862,-0.1228497,64.39547,-0.00992787,0.9999251,-0.007147556,-12.72666,0.1227764,0.008312761,0.9923995,316.9795 +0.9920343,0.02088319,-0.1242252,64.24186,-0.02199521,0.9997293,-0.007586732,-12.75115,0.1240331,0.01025866,0.992225,317.9632 +0.9917372,0.02946745,-0.1248562,64.08999,-0.03057802,0.9995079,-0.006987221,-12.77584,0.1245888,0.01074734,0.9921502,318.9476 +0.9916108,0.03662278,-0.1239635,63.94426,-0.03801263,0.9992379,-0.00886435,-12.79599,0.1235444,0.01350216,0.9922471,319.9305 +0.9917801,0.03769811,-0.1222747,63.80681,-0.03968484,0.9991162,-0.0138527,-12.81874,0.1216444,0.01859128,0.9923996,320.9166 +0.992044,0.03920098,-0.1196332,63.67172,-0.04129916,0.9990326,-0.01510884,-12.84441,0.1189252,0.01992938,0.9927031,321.9037 +0.9924467,0.04162653,-0.1153986,63.54101,-0.04328732,0.9989915,-0.01192217,-12.87181,0.114786,0.01682742,0.9932477,322.8976 +0.9931005,0.04033344,-0.1101123,63.4201,-0.04146227,0.9991082,-0.007980235,-12.89752,0.1096922,0.01249068,0.9938871,323.896 +0.9937005,0.0413558,-0.1041586,63.30426,-0.04209706,0.9991014,-0.004927456,-12.91743,0.1038612,0.009281185,0.9945484,324.8959 +0.9942791,0.04385231,-0.09739689,63.19056,-0.04407888,0.999028,-0.0001746928,-12.93434,0.09729455,0.004466839,0.9952456,325.8993 +0.9945135,0.05151928,-0.0910424,63.07559,-0.05151522,0.9986693,0.002396205,-12.95117,0.0910447,0.002307011,0.9958441,326.9016 +0.9947637,0.05664034,-0.08507174,62.96758,-0.05704031,0.9983693,-0.002276287,-12.96731,0.08480407,0.007116886,0.9963722,327.8981 +0.9950931,0.05870086,-0.07964956,62.86847,-0.05977361,0.9981497,-0.01114953,-12.98511,0.07884769,0.01585576,0.9967605,328.8954 +0.9954553,0.05966727,-0.07422036,62.77719,-0.06099094,0.9980149,-0.01569545,-13.00726,0.07313651,0.02015088,0.9971183,329.8966 +0.9960827,0.05522104,-0.06906473,62.69908,-0.05634147,0.998308,-0.01438002,-13.03326,0.06815378,0.01821489,0.9975085,330.9092 +0.9966194,0.05012351,-0.06509591,62.64298,-0.05080434,0.9986694,-0.008844863,-13.06157,0.06456595,0.01212211,0.9978398,331.9218 +0.9972039,0.04234907,-0.06157053,62.59494,-0.0429138,0.9990477,-0.007878194,-13.08971,0.06117826,0.01049839,0.9980716,332.9321 +0.9974886,0.03826066,-0.05960426,62.55359,-0.03908766,0.9991542,-0.01277072,-13.10731,0.05906522,0.01506844,0.9981403,333.929 +0.9975766,0.04068064,-0.05644512,62.5014,-0.04151227,0.9990449,-0.01363951,-13.12676,0.05583634,0.01594962,0.9983125,334.9352 +0.997724,0.04227708,-0.052531,62.45476,-0.0428766,0.9990269,-0.01033806,-13.14928,0.05204281,0.01256688,0.9985657,335.9506 +0.9977282,0.04609041,-0.04913486,62.40233,-0.04672688,0.998837,-0.01188371,-13.17243,0.04852999,0.01415263,0.9987214,336.9634 +0.997648,0.05019246,-0.04668328,62.35329,-0.0510238,0.9985563,-0.01678945,-13.19165,0.04577317,0.01913191,0.9987686,337.9738 +0.9977851,0.04870591,-0.04530578,62.3105,-0.04934935,0.9986944,-0.01319298,-13.21303,0.04460405,0.01539957,0.998886,338.994 +0.9979131,0.04713268,-0.04413646,62.26657,-0.04735276,0.9988704,-0.003953408,-13.24241,0.04390027,0.00603514,0.9990176,340.0213 +0.9978938,0.04844563,-0.04313914,62.21832,-0.04852655,0.9988215,-0.0008298428,-13.26965,0.0430481,0.002921489,0.9990687,341.0391 +0.997785,0.0509435,-0.04277814,62.1644,-0.05127576,0.998662,-0.006705136,-13.3021,0.04237932,0.008883764,0.999062,342.0414 +0.9977879,0.05084846,-0.04282283,62.1165,-0.0515306,0.9985591,-0.01497841,-13.33378,0.04199949,0.01715196,0.9989703,343.0358 +0.9978637,0.04927457,-0.04289628,62.0718,-0.05007133,0.9985887,-0.01770146,-13.35633,0.04196351,0.01981151,0.9989226,344.0311 +0.9979993,0.0470705,-0.04221227,62.03277,-0.04760057,0.9987986,-0.01164064,-13.38222,0.04161362,0.01362668,0.9990408,345.0315 +0.9981832,0.04340187,-0.04179275,61.9935,-0.04373612,0.9990177,-0.007116624,-13.40929,0.04144282,0.008931546,0.9991009,346.0329 +0.9983687,0.03856272,-0.04210603,61.95688,-0.03899478,0.9991943,-0.009488201,-13.42898,0.04170621,0.01111464,0.999068,347.0203 +0.9984029,0.03756726,-0.0421962,61.91201,-0.03802337,0.9992262,-0.01005893,-13.44291,0.04178566,0.0116473,0.9990587,348.0069 +0.9984251,0.03752178,-0.04170669,61.86637,-0.03777253,0.9992726,-0.005240163,-13.4581,0.04147973,0.006807277,0.9991161,348.9891 +0.9984943,0.03692021,-0.04057141,61.8244,-0.03703212,0.999312,-0.002010048,-13.47425,0.04046929,0.003509467,0.9991746,349.9622 +0.9985995,0.03473026,-0.03991228,61.78986,-0.03496255,0.9993754,-0.005136563,-13.48794,0.03970896,0.006524803,0.9991899,350.9183 +0.9987485,0.03091859,-0.03931436,61.76087,-0.03130925,0.9994659,-0.009360158,-13.49829,0.03900396,0.01057935,0.999183,351.8578 +0.998921,0.02602149,-0.03846837,61.73422,-0.0264585,0.9995905,-0.01089499,-13.51502,0.03816911,0.01190104,0.9992004,352.7875 +0.9990267,0.02405496,-0.036973,61.70791,-0.02442913,0.9996545,-0.009701672,-13.53427,0.03672685,0.01059545,0.9992691,353.7061 +0.9991527,0.02243239,-0.03450609,61.67929,-0.02283415,0.9996755,-0.01129335,-13.55394,0.03424155,0.0120717,0.9993406,354.6117 +0.9991254,0.02789222,-0.03115283,61.64661,-0.0282477,0.9995401,-0.01102969,-13.57062,0.03083086,0.01190004,0.9994537,355.5074 +0.9991359,0.03202827,-0.02649101,61.61399,-0.03228926,0.9994335,-0.009483627,-13.5933,0.02617226,0.01033081,0.999604,356.3914 +0.9990853,0.03715171,-0.02117577,61.5863,-0.03734555,0.9992634,-0.008832942,-13.616,0.02083202,0.009615682,0.9997367,357.2614 +0.9990908,0.03972863,-0.01546714,61.56519,-0.03995136,0.9990983,-0.01436708,-13.64223,0.01488241,0.01497195,0.9997771,358.1135 +0.9991446,0.04021899,-0.00962093,61.55241,-0.04038004,0.9990367,-0.01717618,-13.66753,0.008920854,0.01754998,0.9998061,358.9535 +0.9992508,0.03844574,-0.004456123,61.5506,-0.03851154,0.9991338,-0.0157647,-13.69303,0.003846177,0.0159245,0.9998657,359.7769 +0.9993635,0.03564648,0.001403407,61.55571,-0.0356343,0.9993335,-0.007917158,-13.71943,-0.00168469,0.007862109,0.9999676,360.5893 +0.9994939,0.03114453,0.0064854,61.56597,-0.03114582,0.9995148,9.66389E-05,-13.74183,-0.006479243,-0.0002985825,0.9999789,361.3787 +0.9995767,0.02712678,0.01052295,61.57724,-0.0271572,0.9996273,0.00275865,-13.7606,-0.0104442,-0.003043255,0.9999408,362.1457 +0.9995327,0.02788579,0.01252096,61.58359,-0.0278702,0.9996105,-0.001418467,-13.77996,-0.01255564,0.001068843,0.9999205,362.8822 +0.9995267,0.02759619,0.01360043,61.5902,-0.02750985,0.9996004,-0.006495708,-13.79541,-0.01377425,0.006118487,0.9998864,363.5975 +0.9996216,0.02388269,0.01365376,61.60302,-0.02373683,0.9996605,-0.01074684,-13.80916,-0.01390579,0.01041868,0.999849,364.2908 +0.9997351,0.01848675,0.01371125,61.61817,-0.01829084,0.9997307,-0.01427873,-13.82908,-0.01397153,0.01402415,0.999804,364.972 +0.999738,0.01840285,0.0136153,61.62589,-0.01814442,0.9996573,-0.01886694,-13.85587,-0.01395784,0.01861495,0.9997292,365.65 +0.9997291,0.01957617,0.01259032,61.62869,-0.01927196,0.9995298,-0.02384592,-13.8789,-0.01305122,0.02359681,0.9996363,366.3231 +0.9997378,0.02026922,0.01065955,61.63203,-0.01997851,0.9994437,-0.02670572,-13.8966,-0.01119492,0.02648575,0.9995864,366.9963 +0.9998508,0.01545104,0.007722933,61.6414,-0.01526891,0.9996163,-0.02310967,-13.91728,-0.008077038,0.0229883,0.9997031,367.6727 +0.9999369,0.01069681,0.003443033,61.64988,-0.0106454,0.9998364,-0.01461902,-13.94262,-0.003598846,0.01458145,0.9998872,368.3524 +0.999974,0.006783105,-0.002484346,61.65211,-0.006804285,0.9999397,-0.008618236,-13.96674,0.002425737,0.008634914,0.9999597,369.028 +0.999926,0.004578028,-0.01127121,61.64391,-0.004687291,0.9999421,-0.009686658,-13.98697,0.01122621,0.009738771,0.9998895,369.6924 +0.9997343,0.004889872,-0.02252869,61.62473,-0.00515529,0.9999178,-0.01173836,-14.00683,0.02246944,0.01185138,0.9996772,370.3558 +0.9993596,0.005393021,-0.03537569,61.59376,-0.005902486,0.9998801,-0.01431295,-14.01984,0.03529426,0.01451259,0.9992715,371.0126 +0.9987549,0.002553714,-0.04982189,61.55833,-0.003346129,0.9998691,-0.01582802,-14.03029,0.04977495,0.01597502,0.9986326,371.6756 +0.9978625,0.00138739,-0.06533414,61.51118,-0.002378888,0.9998831,-0.01510046,-14.04429,0.06530555,0.0152236,0.9977491,372.3395 +0.9967445,0.00640773,-0.0803704,61.44542,-0.007537106,0.9998769,-0.01375664,-14.0653,0.08027236,0.01431762,0.9966701,373.0157 +0.9954055,0.01418819,-0.09469278,61.36189,-0.01542687,0.9998046,-0.01236181,-14.08297,0.09449888,0.01376582,0.9954297,373.6982 +0.993874,0.01956935,-0.1087734,61.27159,-0.02091332,0.9997182,-0.01122853,-14.09856,0.108523,0.01343456,0.9940031,374.3882 +0.9924002,0.0196922,-0.1214663,61.18214,-0.02155956,0.9996684,-0.01407829,-14.11114,0.1211487,0.01659006,0.9924957,375.091 +0.9910718,0.02489039,-0.1309853,61.08059,-0.02715475,0.9995106,-0.01552919,-14.12641,0.1305347,0.01894741,0.9912626,375.8055 +0.9899912,0.03335339,-0.1371314,60.96671,-0.03619836,0.999177,-0.01830451,-14.14594,0.136408,0.02308523,0.9903837,376.5284 +0.9892476,0.04035227,-0.1405739,60.84972,-0.04367394,0.9988329,-0.02062371,-14.16784,0.1395777,0.02654137,0.9898553,377.2646 +0.9892228,0.03794109,-0.1414171,60.74507,-0.04128656,0.9989309,-0.02079716,-14.18454,0.1404769,0.02641165,0.9897316,378.022 +0.9893746,0.03981161,-0.1398318,60.63715,-0.04265277,0.9989388,-0.01737944,-14.19874,0.1389915,0.02315898,0.9900227,378.7962 +0.9896605,0.04732865,-0.1353962,60.52298,-0.04899293,0.9987587,-0.008984459,-14.21763,0.1348029,0.01552502,0.9907507,379.59 +0.9902836,0.05104442,-0.1293559,60.41374,-0.05195479,0.9986427,-0.003670775,-14.23869,0.1289929,0.01035577,0.9915914,380.3936 +0.9914581,0.04482361,-0.1224818,60.32454,-0.04608718,0.9989092,-0.007501371,-14.25909,0.122012,0.01308214,0.9924424,381.208 +0.9926911,0.04044807,-0.1137033,60.23753,-0.042603,0.9989544,-0.01658553,-14.27425,0.1129136,0.02130841,0.9933762,382.0327 +0.9936781,0.04279511,-0.1037906,60.1474,-0.04516548,0.9987672,-0.02059523,-14.29007,0.1027812,0.02515278,0.9943859,382.8751 +0.9946449,0.04431672,-0.09336844,60.06557,-0.04628627,0.9987468,-0.01903439,-14.31204,0.09240789,0.02325413,0.9954496,383.7413 +0.9956186,0.04522462,-0.08184403,59.99739,-0.04647432,0.9988292,-0.01342821,-14.34133,0.08114092,0.01717302,0.9965546,384.6238 +0.9965303,0.04595035,-0.06939722,59.93719,-0.04684395,0.9988382,-0.01130358,-14.36719,0.06879719,0.0145152,0.997525,385.5155 +0.9972227,0.04739595,-0.05745106,59.88483,-0.04843075,0.998686,-0.01675456,-14.39286,0.05658147,0.01949042,0.9982077,386.4137 +0.9978573,0.04561471,-0.04690587,59.84353,-0.04683766,0.9985817,-0.02531186,-14.42481,0.04568475,0.02745458,0.9985785,387.3252 +0.998414,0.04120166,-0.03836761,59.81431,-0.04246533,0.9985618,-0.03272474,-14.45856,0.03696411,0.03430212,0.9987276,388.2519 +0.9987456,0.03904881,-0.03134605,59.78983,-0.04008176,0.9986503,-0.03303043,-14.49961,0.03001394,0.03424539,0.9989626,389.1956 +0.998985,0.03739707,-0.02510909,59.76798,-0.03805854,0.9989266,-0.02640403,-14.53983,0.0240947,0.02733284,0.9993359,390.1599 +0.9992481,0.03336376,-0.01975042,59.7661,-0.03377836,0.9992078,-0.02104396,-14.59815,0.01903267,0.02169527,0.9995834,391.1281 +0.9994201,0.0308721,-0.01436767,59.76542,-0.03112204,0.9993622,-0.0175103,-14.64881,0.01381792,0.01794729,0.9997434,392.1067 +0.9995018,0.02980479,-0.01038472,59.76528,-0.02999023,0.9993848,-0.01818282,-14.69849,0.009836393,0.0184852,0.9997807,393.0928 +0.9995547,0.0282647,-0.009568011,59.76442,-0.02845219,0.9993938,-0.02006229,-14.73892,0.008995156,0.02032558,0.9997529,394.0969 +0.9996729,0.02332855,-0.01048252,59.7664,-0.02352398,0.9995441,-0.01892361,-14.77563,0.01003628,0.019164,0.9997659,395.1213 +0.9996774,0.02212832,-0.01247129,59.76162,-0.0223083,0.9996462,-0.01448188,-14.81712,0.01214642,0.01475542,0.9998173,396.1597 +0.9996204,0.02321541,-0.01484071,59.74722,-0.02342056,0.9996304,-0.01380254,-14.85438,0.01451479,0.01414488,0.9997945,397.211 +0.9995224,0.02537689,-0.01763504,59.72862,-0.02566101,0.9995414,-0.01607559,-14.89124,0.017219,0.01652044,0.9997152,398.2688 +0.9995163,0.02270545,-0.02125416,59.71318,-0.02302647,0.9996226,-0.01498263,-14.92621,0.02090595,0.01546479,0.9996618,399.3403 +0.9994952,0.01960538,-0.02500109,59.69604,-0.01987732,0.9997454,-0.01067525,-14.96445,0.02478543,0.01116681,0.9996304,400.4182 +0.9994541,0.01627503,-0.0287515,59.67327,-0.016548,0.99982,-0.009281686,-15.00203,0.02859526,0.009752398,0.9995434,401.4929 +0.999365,0.01277137,-0.03326484,59.64675,-0.01326629,0.9998039,-0.01469996,-15.04264,0.03307058,0.01513193,0.9993384,402.5603 +0.9992601,0.008587248,-0.03749044,59.6159,-0.00938082,0.9997345,-0.02104296,-15.08099,0.03729978,0.02137908,0.9990753,403.6242 +0.9991191,0.008361482,-0.04112422,59.57539,-0.009225307,0.9997398,-0.02086052,-15.12563,0.0409391,0.02122152,0.9989362,404.6946 +0.9990015,0.01094108,-0.04331784,59.52883,-0.01152359,0.9998462,-0.01322054,-15.1669,0.04316653,0.01370651,0.9989738,405.7719 +0.9988715,0.01528517,-0.044968,59.47664,-0.01552287,0.9998673,-0.004941489,-15.20557,0.04488649,0.005633945,0.9989762,406.849 +0.9987741,0.01748304,-0.04631015,59.42514,-0.01774454,0.9998288,-0.005241646,-15.23835,0.04621058,0.006056972,0.9989133,407.9166 +0.9986826,0.01764641,-0.04818558,59.37438,-0.01809721,0.9997963,-0.008935191,-15.26821,0.04801809,0.009795442,0.9987984,408.9796 +0.9985789,0.01888472,-0.04983612,59.32089,-0.01939893,0.9997632,-0.009854464,-15.29901,0.04963822,0.01080723,0.9987087,410.0425 +0.9984863,0.01946309,-0.05144309,59.267,-0.01989426,0.999771,-0.007882592,-15.33264,0.05127789,0.008894082,0.9986448,411.1043 +0.9983562,0.02141033,-0.05316587,59.20589,-0.02205126,0.9996907,-0.0114981,-15.37085,0.05290325,0.01265157,0.9985194,412.1563 +0.9982978,0.02264134,-0.0537481,59.14511,-0.0235874,0.9995766,-0.01703306,-15.4057,0.05333969,0.01827184,0.9984092,413.2056 +0.9982897,0.02255339,-0.05393667,59.08467,-0.02354278,0.9995647,-0.01777896,-15.44272,0.05351221,0.01901837,0.998386,414.2578 +0.998317,0.0226039,-0.05340649,59.0265,-0.02336425,0.9996337,-0.01365569,-15.47985,0.05307826,0.01488051,0.9984794,415.3126 +0.9983455,0.02245892,-0.05293351,58.96893,-0.02284731,0.9997162,-0.006743582,-15.51299,0.05276704,0.007941813,0.9985752,416.3646 +0.9983765,0.02154102,-0.05272957,58.91368,-0.02160224,0.9997664,-0.0005913803,-15.54099,0.05270452,0.001729498,0.9986086,417.4171 +0.998406,0.02077367,-0.05247853,58.85967,-0.02076041,0.9997841,0.0007978581,-15.56518,0.05248378,0.0002928902,0.9986217,418.4607 +0.9984284,0.02053422,-0.05214627,58.80686,-0.02083301,0.9997695,-0.005192683,-15.59016,0.05202762,0.006270885,0.9986259,419.4964 +0.9983888,0.02355659,-0.05162411,58.74881,-0.02413414,0.9996526,-0.01059284,-15.61868,0.05135664,0.01182167,0.9986104,420.5281 +0.9982837,0.02788857,-0.05149634,58.68504,-0.02870058,0.999474,-0.01509662,-15.64958,0.05104823,0.01654868,0.998559,421.557 +0.9983316,0.02711233,-0.05098085,58.62784,-0.02802014,0.9994597,-0.01717721,-15.68558,0.05048759,0.01857704,0.9985518,422.5846 +0.9984142,0.02485602,-0.05051004,58.57414,-0.02590557,0.9994596,-0.0202317,-15.7212,0.04997986,0.02150811,0.9985186,423.6042 +0.9985779,0.02013589,-0.04936321,58.52715,-0.02106721,0.9996083,-0.01841946,-15.75751,0.04897298,0.01943321,0.998611,424.6242 +0.9986212,0.0178989,-0.04935052,58.48082,-0.01845751,0.9997703,-0.0108868,-15.79727,0.04914433,0.01178268,0.9987221,425.6365 +0.9986872,0.0149514,-0.04899479,58.4372,-0.01525002,0.9998673,-0.005726675,-15.83191,0.04890266,0.006466328,0.9987826,426.6354 +0.9987031,0.01444444,-0.04882272,58.39171,-0.01483208,0.9998612,-0.007586856,-15.86596,0.04870635,0.008301158,0.9987786,427.6159 +0.9987065,0.01551986,-0.04842087,58.34298,-0.01611382,0.9997993,-0.01190045,-15.89808,0.04822646,0.0126653,0.9987561,428.5773 +0.9985456,0.02089953,-0.04969854,58.28685,-0.02168221,0.9996484,-0.01526175,-15.93052,0.0493621,0.01631712,0.9986476,429.5233 +0.9983508,0.02305804,-0.05257499,58.23246,-0.02385038,0.9996105,-0.01449318,-15.96625,0.05222033,0.01572321,0.9985117,430.4576 +0.9981267,0.02206719,-0.05706338,58.17911,-0.02282332,0.9996597,-0.01263311,-16.0004,0.05676518,0.01391182,0.9982906,431.3774 +0.9977805,0.02364094,-0.06225105,58.11757,-0.02443097,0.9996299,-0.01196036,-16.03584,0.06194525,0.01345466,0.9979888,432.2805 +0.9974423,0.0220162,-0.06800181,58.05705,-0.02277426,0.9996866,-0.01039252,-16.06875,0.06775169,0.01191463,0.997631,433.1686 +0.9969984,0.01891711,-0.0750762,57.99581,-0.01957555,0.999776,-0.008044067,-16.09857,0.07490721,0.009489579,0.9971453,434.0437 +0.996435,0.0201954,-0.08191151,57.9241,-0.02052128,0.9997845,-0.003138386,-16.12658,0.08183047,0.004808127,0.9966346,434.902 +0.9958084,0.0223347,-0.08869541,57.84065,-0.02241981,0.9997486,3.658255E-05,-16.15406,0.08867393,0.001952106,0.9960587,435.7462 +0.9951076,0.02412563,-0.09580706,57.75392,-0.02430545,0.9997043,-0.0007101303,-16.17716,0.09576159,0.00303529,0.9953996,436.5721 +0.9944583,0.02424696,-0.1022978,57.66319,-0.02469173,0.9996903,-0.003083521,-16.198,0.1021914,0.005592344,0.994749,437.3827 +0.9937696,0.02498655,-0.1086173,57.5691,-0.02583583,0.9996456,-0.006418549,-16.21944,0.1084184,0.009184776,0.9940629,438.1737 +0.9929671,0.02559625,-0.1155906,57.47062,-0.02685547,0.9995956,-0.009349353,-16.24168,0.1153046,0.01238784,0.9932529,438.9478 +0.9919474,0.02618599,-0.123914,57.36825,-0.02787191,0.9995407,-0.01189135,-16.26608,0.1235457,0.01524931,0.9922217,439.7104 +0.9906949,0.02817153,-0.1331541,57.25999,-0.03009753,0.9994691,-0.01247349,-16.29215,0.132732,0.01636503,0.9910168,440.4555 +0.9893724,0.02760883,-0.142759,57.14887,-0.02947052,0.9995057,-0.01094245,-16.31654,0.1423863,0.01503334,0.9896969,441.1817 +0.9879735,0.02909761,-0.1518609,57.03669,-0.03069163,0.9994955,-0.008162632,-16.34238,0.1515468,0.01272532,0.9883681,441.889 +0.9865583,0.03284145,-0.1600758,56.91449,-0.03419738,0.9993987,-0.005722322,-16.36661,0.1597916,0.01111958,0.9870881,442.5732 +0.9851823,0.03732886,-0.1673991,56.78747,-0.03837863,0.9992586,-0.003039189,-16.38826,0.1671615,0.009418703,0.9858845,443.2379 +0.9839853,0.04564169,-0.1723076,56.65685,-0.04642832,0.9989215,-0.0005357432,-16.40569,0.1720973,0.008527117,0.985043,443.8748 +0.9834822,0.05510353,-0.1724136,56.53228,-0.05558776,0.9984517,0.002022146,-16.42058,0.172258,0.00759534,0.9850225,444.4878 +0.9841082,0.06288296,-0.1660627,56.41955,-0.06309557,0.9979994,0.004000303,-16.43657,0.1659821,0.006541091,0.986107,445.0756 +0.985635,0.06761158,-0.1547656,56.32386,-0.06761341,0.9976977,0.005258197,-16.45398,0.1547648,0.005281565,0.9879372,445.6454 +0.9878039,0.07043923,-0.1388589,56.24445,-0.07065205,0.9974952,0.003402288,-16.47357,0.1387507,0.006449872,0.9903063,446.1921 +0.9904503,0.07383614,-0.1164324,56.18642,-0.07443566,0.9972255,-0.0008032975,-16.49151,0.11605,0.009462344,0.9931982,446.7321 +0.9931144,0.07845409,-0.08699861,56.14866,-0.07908678,0.9968603,-0.003844192,-16.50757,0.08642387,0.01069816,0.996201,447.2701 +0.9953993,0.08142196,-0.05050473,56.12959,-0.08174735,0.9966433,-0.004407293,-16.52522,0.04997634,0.008515643,0.998714,447.7627 +0.9969734,0.0762919,0.01495884,56.37354,-0.07619468,0.9970686,-0.00696621,-16.54917,-0.01544645,0.005805341,0.9998638,448.1493 +0.9942326,0.07101582,0.08036447,56.61613,-0.07048836,0.9974684,-0.009384972,-16.5729,-0.08082749,0.003666085,0.9967213,448.5308 +0.9871759,0.06563932,0.1455171,56.85753,-0.06463015,0.9978412,-0.011657,-16.59654,-0.1459681,0.002102719,0.989287,448.909 +0.9758535,0.06023011,0.209958,57.09636,-0.05864639,0.9981839,-0.01376677,-16.61978,-0.2104058,0.001121075,0.9776134,449.2805 +0.9602731,0.05482307,0.2736239,57.33354,-0.05252787,0.9984958,-0.01571325,-16.64298,-0.2740738,0.0007161289,0.9617083,449.6498 +0.940605,0.04950145,0.3358749,57.5666,-0.04632182,0.9987736,-0.01747739,-16.66551,-0.3363281,0.0008809839,0.9417444,450.0094 +0.9168127,0.04428157,0.3968548,57.79766,-0.03999948,0.9990178,-0.01906504,-16.68805,-0.3973092,0.001605078,0.9176833,450.3681 +0.8890326,0.03922545,0.4561605,58.02562,-0.03358655,0.9992262,-0.02046552,-16.71036,-0.4566103,0.002873655,0.8896621,450.7233 +0.8575594,0.03440684,0.5132329,58.24828,-0.02713046,0.999397,-0.0216668,-16.73191,-0.5136689,0.00465632,0.8579758,451.0683 +0.8221816,0.02982081,0.5684437,58.46928,-0.02057385,0.9995311,-0.02267839,-16.75378,-0.5688534,0.006950677,0.8224095,451.4173 +0.7835769,0.02557555,0.6207682,58.68323,-0.01402482,0.9996259,-0.02348132,-16.77453,-0.6211365,0.009693247,0.7836424,451.7521 +0.7415393,0.02166513,0.6705596,58.89364,-0.007428033,0.9996823,-0.02408448,-16.79522,-0.6708683,0.01287864,0.7414646,452.0863 +0.6962757,0.01813334,0.7175454,59.09996,-0.000803108,0.9996999,-0.02448446,-16.81575,-0.717774,0.01647166,0.6960812,452.419 +0.6481558,0.01502733,0.7613595,59.30076,0.005811945,0.9996785,-0.02467895,-16.83577,-0.7614856,0.02042078,0.64786,452.7459 +0.5971621,0.0123646,0.8020253,59.49747,0.01243492,0.9996183,-0.02466947,-16.85573,-0.8020242,0.02470478,0.5967803,453.0732 +0.5436244,0.01016424,0.8392671,59.69389,0.01890164,0.9995248,-0.0243484,-16.87518,-0.8391157,0.0290999,0.5431739,453.3957 +0.4879108,0.007485277,0.8728614,60.05191,0.01988916,0.9996083,-0.01968982,-16.88418,-0.8726668,0.02696735,0.4875708,453.4931 +0.4302463,0.006225964,0.9026901,60.4355,0.01580295,0.999771,-0.01442766,-16.89523,-0.9025732,0.02047261,0.4300493,453.6732 +0.372291,0.009087113,0.9280716,60.82955,0.01366215,0.99979,-0.01526984,-16.90196,-0.9280154,0.01836427,0.3720887,453.8234 +0.3162863,0.01066007,0.9486039,61.2321,0.02216902,0.9995807,-0.01862459,-16.90829,-0.9484047,0.02692031,0.3159173,453.8636 +0.2642663,0.009338816,0.9644046,61.66843,0.03416858,0.9992347,-0.01903898,-16.92438,-0.9638443,0.03798368,0.2637449,453.9541 +0.2172398,0.0104285,0.9760626,62.1176,0.03763461,0.9991099,-0.01905099,-16.93856,-0.9753924,0.04087235,0.216654,454.0315 +0.174738,0.01549462,0.9844931,62.57515,0.03435994,0.9991712,-0.0218242,-16.95047,-0.9840152,0.03764063,0.1740608,454.0722 +0.1364044,0.02170969,0.9904154,63.05511,0.02949719,0.9992275,-0.02596534,-16.95654,-0.990214,0.03275624,0.1356586,454.1371 +0.1029939,0.02472847,0.9943746,63.55579,0.02882486,0.9991969,-0.02783398,-16.9664,-0.9942642,0.03152943,0.1021984,454.1813 +0.07625369,0.02305829,0.9968218,64.07556,0.03398803,0.9990914,-0.02571077,-16.97911,-0.996509,0.03584054,0.0754007,454.1936 +0.05641671,0.02187309,0.9981677,64.61298,0.03884386,0.9989549,-0.02408582,-16.99897,-0.9976514,0.04013152,0.05550812,454.2096 +0.0422678,0.02337194,0.9988329,65.16526,0.04299,0.9987579,-0.02518941,-17.01404,-0.9981809,0.04400451,0.04121054,454.224 +0.03536481,0.02354254,0.9990972,65.72987,0.04611383,0.9986192,-0.02516356,-17.02729,-0.99831,0.04696209,0.03423034,454.2378 +0.03500886,0.02199664,0.9991449,66.31685,0.04599504,0.9986629,-0.02359765,-17.03923,-0.998328,0.04678182,0.03395031,454.2609 +0.03751487,0.02046368,0.9990865,66.91875,0.04534492,0.9987256,-0.02215896,-17.05511,-0.9982667,0.04613478,0.03653913,454.2886 +0.04034753,0.01824313,0.9990192,67.53618,0.04335879,0.9988595,-0.01999136,-17.07471,-0.9982445,0.04412286,0.03951052,454.3204 +0.04291222,0.01756834,0.9989244,68.16862,0.04481491,0.9988051,-0.01949143,-17.09206,-0.9980732,0.04560311,0.04207362,454.3493 +0.04578439,0.01832632,0.9987833,68.81514,0.04539742,0.9987605,-0.02040693,-17.10837,-0.9979192,0.04627649,0.04489568,454.38 +0.04812445,0.01853747,0.9986693,69.48192,0.0456411,0.9987426,-0.02073821,-17.12311,-0.997798,0.04657837,0.04721786,454.4136 +0.05097248,0.01747688,0.9985472,70.16492,0.04581171,0.9987534,-0.01981903,-17.14124,-0.9976487,0.04675537,0.05010829,454.4484 +0.05359697,0.0181591,0.9983976,70.86176,0.04587746,0.998734,-0.02062806,-17.16519,-0.9975082,0.04690953,0.05269602,454.4814 +0.0559724,0.02192663,0.9981916,71.56844,0.04451789,0.9987097,-0.0244343,-17.19316,-0.9974393,0.04580502,0.05492405,454.5179 +0.05802165,0.02803357,0.9979217,72.2875,0.04490768,0.9985205,-0.03066145,-17.22009,-0.9973047,0.04659336,0.05667688,454.5529 +0.06049124,0.03278686,0.9976301,73.02494,0.04386901,0.9984073,-0.0354724,-17.24416,-0.9972042,0.0459108,0.05895657,454.5937 +0.06228593,0.03129396,0.9975676,73.78323,0.04188244,0.9985459,-0.0339397,-17.27336,-0.9971791,0.04389452,0.06088469,454.6373 +0.06357711,0.0261848,0.9976334,74.5618,0.03834275,0.9988535,-0.02866034,-17.30779,-0.99724,0.04007414,0.06250022,454.689 +0.0648943,0.01916324,0.9977082,75.35567,0.03650126,0.9991009,-0.02156416,-17.34186,-0.9972243,0.03781699,0.06413647,454.7396 +0.06675369,0.01892068,0.9975901,76.15867,0.03695043,0.9990875,-0.02142163,-17.36898,-0.997085,0.03829134,0.06599364,454.7898 +0.06800454,0.01896079,0.9975049,76.97382,0.03633122,0.9991092,-0.02146815,-17.39434,-0.9970232,0.03770049,0.06725509,454.8427 +0.0693703,0.01756557,0.9974363,77.80419,0.03627932,0.9991391,-0.02011874,-17.42263,-0.996931,0.03758195,0.06867331,454.8927 +0.07031176,0.01427567,0.9974229,78.64677,0.03884485,0.9991,-0.01703799,-17.45272,-0.9967684,0.03994271,0.06969393,454.9431 +0.0708709,0.01211018,0.997412,79.49974,0.04053485,0.9990653,-0.01501045,-17.48584,-0.9966615,0.04149374,0.07031378,454.9938 +0.07121039,0.0145803,0.9973548,80.35896,0.04141585,0.9989876,-0.01756124,-17.51605,-0.9966011,0.04255683,0.07053444,455.0524 +0.0713407,0.0247354,0.9971453,81.22587,0.03803515,0.9988979,-0.02750011,-17.54771,-0.9967265,0.03988843,0.07032125,455.1156 +0.07056942,0.03447901,0.9969108,82.10395,0.03660642,0.9986397,-0.03713011,-17.5802,-0.9968349,0.03911358,0.06921127,455.178 +0.06893404,0.03824959,0.9968877,83.00244,0.03476391,0.9985657,-0.04071788,-17.61761,-0.9970153,0.03746255,0.06750546,455.2393 +0.06632842,0.0340026,0.9972183,83.91678,0.0317462,0.9988413,-0.0361695,-17.66024,-0.9972927,0.03405694,0.06517211,455.3023 +0.06321145,0.02561646,0.9976714,84.83881,0.02753333,0.9992452,-0.02740137,-17.70174,-0.9976202,0.02920129,0.06245843,455.363 +0.0598463,0.01935479,0.99802,85.77095,0.02865769,0.9993666,-0.02109938,-17.73483,-0.9977961,0.02986366,0.05925372,455.4132 +0.05719878,0.01799107,0.9982007,86.70202,0.03169981,0.9993007,-0.01982736,-17.76588,-0.9978594,0.03277686,0.05658847,455.4567 +0.05487749,0.01766998,0.9983368,87.63539,0.03325057,0.9992565,-0.01951401,-17.79453,-0.9979393,0.03426614,0.05424915,455.4985 +0.05329551,0.02091217,0.9983598,88.57696,0.03837362,0.9989993,-0.02297407,-17.82493,-0.9978412,0.03953509,0.0524397,455.5371 +0.05307553,0.02078145,0.9983743,89.52635,0.04013475,0.9989312,-0.0229267,-17.85708,-0.9977836,0.04128633,0.05218474,455.5783 +0.05332045,0.0207735,0.9983614,90.48321,0.03682276,0.9990627,-0.02275472,-17.89014,-0.9978983,0.0379757,0.05250553,455.6301 +0.05359366,0.02014235,0.9983597,91.44549,0.03395478,0.9991816,-0.02198169,-17.92387,-0.9979853,0.03507715,0.05286587,455.6821 +0.05404909,0.01976583,0.9983427,92.41235,0.03297041,0.9992236,-0.02156826,-17.95632,-0.9979938,0.0340815,0.05335543,455.7337 +0.05433614,0.01889604,0.9983439,93.38406,0.0308252,0.9993126,-0.02059208,-17.98828,-0.9980467,0.03189304,0.05371631,455.7873 +0.05445703,0.01845589,0.9983456,94.36001,0.02753679,0.9994211,-0.01997784,-18.0248,-0.9981363,0.02857916,0.05391729,455.8435 +0.05511189,0.02308575,0.9982133,95.32857,0.02671805,0.9993406,-0.02458695,-18.06266,-0.9981226,0.02802533,0.05445874,455.8954 +0.05586691,0.02560801,0.9981098,96.30089,0.026528,0.99928,-0.02712288,-18.09746,-0.9980857,0.02799312,0.05514736,455.9483 +0.05639448,0.02617408,0.9980654,97.27373,0.02589288,0.9992817,-0.02766903,-18.13883,-0.9980727,0.02740316,0.05567625,456.0023 +0.05690987,0.02497994,0.9980668,98.24989,0.02711704,0.9992794,-0.02655651,-18.18222,-0.9980109,0.02857594,0.05619147,456.0543 +0.05761101,0.02265503,0.9980821,99.22807,0.0282672,0.9993046,-0.02431442,-18.2203,-0.9979388,0.02961376,0.05693055,456.1087 +0.05708315,0.01743407,0.9982172,100.2052,0.03026114,0.9993579,-0.01918448,-18.25342,-0.9979107,0.03130229,0.05651892,456.1674 +0.05702203,0.01155349,0.9983061,101.1841,0.03156234,0.9994123,-0.0133691,-18.28197,-0.9978739,0.0322712,0.05662386,456.2242 +0.05588595,0.01035456,0.9983835,102.159,0.03124252,0.9994384,-0.01211435,-18.30903,-0.9979482,0.03186903,0.05553106,456.2817 +0.05478572,0.01070967,0.9984407,103.1275,0.02945142,0.9994901,-0.01233697,-18.33657,-0.9980637,0.03008137,0.05444237,456.3403 +0.0530345,0.01430796,0.9984902,104.0931,0.03011042,0.9994198,-0.0159206,-18.3709,-0.9981386,0.03090929,0.05257291,456.3931 +0.05128927,0.01802538,0.9985212,105.0596,0.03273219,0.9992696,-0.0197202,-18.40604,-0.9981472,0.03369521,0.0506618,456.4396 +0.04974969,0.01943737,0.9985726,106.0275,0.03355566,0.9992136,-0.02112162,-18.44084,-0.9981978,0.03455854,0.04905834,456.4855 +0.04901332,0.01636413,0.9986641,106.9989,0.03735889,0.999136,-0.0182054,-18.47595,-0.9980992,0.03820128,0.04835963,456.5256 +0.04928242,0.01478644,0.9986755,107.9675,0.04096478,0.9990191,-0.01681306,-18.51114,-0.9979444,0.0417391,0.04862836,456.5652 +0.0498341,0.01488866,0.9986466,108.9376,0.04131862,0.9990021,-0.01695583,-18.54235,-0.9979024,0.04210766,0.04916919,456.6076 +0.05040173,0.01956617,0.9985374,109.8972,0.03974748,0.9989767,-0.02158106,-18.57694,-0.9979377,0.04077706,0.04957244,456.6545 +0.0512732,0.02603087,0.9983454,110.8532,0.0367714,0.9989332,-0.02793472,-18.61336,-0.9980074,0.03814285,0.05026131,456.7044 +0.05166154,0.03415921,0.9980803,111.8011,0.03135161,0.9988667,-0.03580893,-18.65851,-0.9981724,0.03314136,0.05053205,456.761 +0.05228197,0.03663078,0.9979603,112.7497,0.02940459,0.9988372,-0.03820345,-18.70618,-0.9981993,0.03134196,0.05114407,456.8166 +0.05272315,0.0311722,0.9981225,113.7043,0.03232296,0.9989357,-0.03290497,-18.74798,-0.9980859,0.03399711,0.05165945,456.8631 +0.052779,0.01970723,0.9984118,114.6632,0.02906394,0.9993514,-0.02126219,-18.79289,-0.9981831,0.03013997,0.05217199,456.9174 +0.05295409,0.01128703,0.9985332,115.6102,0.03125706,0.9994274,-0.01295476,-18.83295,-0.9981076,0.03189721,0.05257097,456.9655 +0.05436259,0.01139746,0.9984562,116.5475,0.03613459,0.9992574,-0.01337402,-18.86475,-0.9978672,0.03680584,0.05391038,457.0069 +0.05472439,0.01486753,0.9983908,117.4761,0.04006794,0.9990511,-0.0170736,-18.89868,-0.9976972,0.0409378,0.05407675,457.0505 +0.05533853,0.02019913,0.9982633,118.4001,0.03940135,0.9989724,-0.02239769,-18.93406,-0.9976899,0.04057236,0.05448578,457.0993 +0.05593605,0.02525757,0.9981149,119.3194,0.03713231,0.9989357,-0.02735931,-18.97438,-0.9977436,0.03859267,0.05493864,457.1524 +0.05707011,0.02920474,0.997943,120.2357,0.03529744,0.9988881,-0.03125099,-19.01741,-0.997746,0.03700832,0.0559758,457.2047 +0.05779613,0.03319939,0.9977763,121.1459,0.03395211,0.9988034,-0.03520025,-19.06799,-0.9977509,0.03591104,0.05659977,457.2589 +0.05893482,0.0331028,0.9977129,122.0572,0.0294261,0.9989581,-0.03488232,-19.11893,-0.997828,0.03141457,0.05789933,457.3218 +0.06156396,0.03122859,0.9976145,122.9661,0.02731975,0.9990832,-0.03296051,-19.1663,-0.9977291,0.02928375,0.06065435,457.3856 +0.06612858,0.02403198,0.9975217,123.8765,0.02675784,0.9993077,-0.02584886,-19.21379,-0.9974522,0.02840087,0.06543975,457.4481 +0.07197113,0.01847829,0.9972356,124.7831,0.02855969,0.9993802,-0.02057921,-19.25654,-0.9969977,0.02996183,0.07139878,457.5153 +0.07953218,0.01486883,0.9967214,125.6834,0.02872214,0.9994394,-0.01720123,-19.29294,-0.9964184,0.02999602,0.07906053,457.5899 +0.08823503,0.01125662,0.9960361,126.5838,0.03267794,0.9993652,-0.01418906,-19.32717,-0.9955635,0.03380037,0.08781117,457.6665 +0.09848879,0.009712496,0.9950908,127.4757,0.0349923,0.9993002,-0.01321694,-19.36016,-0.9945227,0.03612222,0.09808,457.7576 +0.10903,0.01153683,0.9939715,128.3608,0.03079457,0.9994135,-0.0149779,-19.39938,-0.9935613,0.03224196,0.1086108,457.8608 +0.1197771,0.01629454,0.9926671,129.2416,0.02797439,0.9994129,-0.01978072,-19.43961,-0.9924066,0.03013852,0.119251,457.9727 +0.1305472,0.02090905,0.9912216,130.1192,0.02673978,0.9993396,-0.02460202,-19.4808,-0.9910814,0.02971676,0.1299019,458.0905 +0.1403262,0.02062925,0.9898904,130.9968,0.02347724,0.9994325,-0.02415623,-19.52025,-0.9898269,0.02662964,0.1397622,458.2201 +0.1499819,0.01526744,0.9885709,131.8773,0.02041917,0.9996196,-0.018536,-19.5577,-0.9884778,0.02296586,0.1496131,458.3602 +0.1598231,0.01059456,0.9870888,132.7527,0.01998022,0.9997028,-0.01396502,-19.59831,-0.9869434,0.02195417,0.159564,458.5038 +0.1689358,0.01161787,0.9855586,133.6179,0.01876439,0.9997114,-0.01500114,-19.63306,-0.9854484,0.02102763,0.1686691,458.6527 +0.1766944,0.02000768,0.9840624,134.4723,0.01741317,0.9995733,-0.02344969,-19.66674,-0.9841117,0.02127907,0.1762706,458.8109 +0.1839622,0.02359862,0.98265,135.3263,0.01799304,0.9994634,-0.02737089,-19.70045,-0.9827686,0.02271606,0.1834388,458.9774 +0.1913139,0.02147594,0.981294,136.1843,0.01766404,0.9995233,-0.0253187,-19.73513,-0.9813699,0.02217743,0.1908434,459.149 +0.1983963,0.01294421,0.9800364,137.0478,0.01749522,0.9997067,-0.01674571,-19.76674,-0.9799657,0.02046824,0.1981117,459.3281 +0.2065171,0.004947218,0.9784305,137.9053,0.02043983,0.9997472,-0.009369236,-19.79615,-0.9782294,0.02193385,0.2063638,459.5119 +0.2156262,0.005567123,0.9764601,138.7477,0.01835738,0.9997839,-0.00975386,-19.82313,-0.9763034,0.02002843,0.2154774,459.7078 +0.2255337,0.01479107,0.9741231,139.5789,0.01721012,0.9996682,-0.01916353,-19.85194,-0.9740833,0.02108679,0.2252043,459.9093 +0.2354444,0.02050072,0.9716716,140.4094,0.01891349,0.9994915,-0.02567058,-19.87662,-0.9717037,0.02442169,0.2349369,460.1134 +0.2450595,0.01608453,0.9693746,141.2456,0.02126084,0.9995327,-0.02195972,-19.90477,-0.9692748,0.02599115,0.2446031,460.3268 +0.2552755,0.007482232,0.9668394,142.0823,0.02327587,0.9996327,-0.01388157,-19.93899,-0.9665881,0.02604765,0.2550075,460.5481 +0.267378,0.001474146,0.9635906,142.9125,0.02679017,0.9996009,-0.008962998,-19.96853,-0.9632192,0.02821125,0.2672317,460.7721 +0.280004,0.003185173,0.9599936,143.73,0.032542,0.9993883,-0.0128075,-19.99516,-0.9594471,0.03482625,0.2797291,461.0046 +0.2908361,0.009019465,0.9567304,144.5361,0.03638179,0.9991281,-0.02047885,-20.02408,-0.9560809,0.04076354,0.2902543,461.2404 +0.3009031,0.01623041,0.9535166,145.3404,0.04117997,0.9987014,-0.02999477,-20.05968,-0.9527652,0.04829129,0.2998439,461.4895 +0.3099385,0.01837043,0.9505791,146.1388,0.04112312,0.9986186,-0.03270711,-20.09772,-0.9498668,0.04922796,0.3087549,461.7549 +0.317952,0.01928961,0.9479106,146.9351,0.03561348,0.9988444,-0.03227171,-20.14307,-0.9474377,0.04401924,0.3168976,462.0357 +0.325145,0.01375072,0.9455642,147.7346,0.03539163,0.9990168,-0.02669794,-20.18213,-0.9450016,0.04214575,0.3243386,462.3184 +0.3324508,0.006753672,0.9430964,148.5333,0.0377932,0.9990757,-0.02047703,-20.21462,-0.942363,0.04245023,0.3318883,462.6011 +0.3390362,-0.0001675636,0.9407734,149.3239,0.03768832,0.9991996,-0.01340416,-20.25038,-0.9400181,0.04000066,0.3387711,462.8908 +0.3456806,0.000269468,0.9383522,150.1033,0.03238581,0.9994007,-0.01221765,-20.28734,-0.9377931,0.03461269,0.3454647,463.1915 +0.352738,0.007383346,0.935693,150.8754,0.03112033,0.9993231,-0.0196172,-20.32187,-0.9352045,0.03603879,0.3522694,463.4939 +0.3604424,0.01234908,0.9326998,151.6452,0.03488812,0.9990342,-0.0267099,-20.35285,-0.9321288,0.04216751,0.3596634,463.7897 +0.3676796,0.01458651,0.9298381,152.4141,0.0325826,0.999061,-0.02855634,-20.3888,-0.9293815,0.04079612,0.3668591,464.1031 +0.3740929,0.01164206,0.9273182,153.1854,0.0279906,0.9993239,-0.02383786,-20.42917,-0.9269687,0.03487376,0.3735141,464.4271 +0.3815945,0.007028949,0.9243031,153.9537,0.03020924,0.999342,-0.02007134,-20.46787,-0.923836,0.0355816,0.381131,464.746 +0.3889158,0.008280901,0.9212361,154.717,0.03353602,0.9991696,-0.02313925,-20.50819,-0.9206627,0.0398938,0.3883151,465.065 +0.3956064,0.01480446,0.9183009,155.4665,0.03322363,0.9989849,-0.03041805,-20.54749,-0.917819,0.04254285,0.3947129,465.3898 +0.403016,0.02090326,0.9149542,156.2163,0.03267122,0.9987733,-0.03720913,-20.59044,-0.9146095,0.04488853,0.4018387,465.7191 +0.4109411,0.0213882,0.911411,156.9634,0.03163627,0.9987881,-0.037703,-20.63939,-0.9111128,0.04432735,0.4097664,466.0557 +0.4198345,0.01909277,0.9073999,157.7066,0.02865832,0.9990013,-0.03427977,-20.69316,-0.9071481,0.04039637,0.418868,466.4014 +0.4285453,0.01898644,0.9033208,158.442,0.02645953,0.9990866,-0.03355199,-20.74416,-0.9031327,0.03827998,0.4276515,466.755 +0.4375805,0.02038194,0.8989482,159.1689,0.02289312,0.9991664,-0.03379787,-20.79448,-0.8988877,0.03536901,0.4367491,467.1119 +0.4457623,0.02174886,0.8948872,159.8906,0.02184526,0.9991427,-0.03516422,-20.84541,-0.8948847,0.03522391,0.444905,467.475 +0.4541728,0.02060763,0.8906753,160.6108,0.02104242,0.9992054,-0.03384865,-20.89615,-0.890665,0.03411509,0.4533783,467.8441 +0.4623871,0.01980487,0.886457,161.3226,0.01899733,0.9992997,-0.03223521,-20.94383,-0.8864746,0.03174546,0.4616871,468.2182 +0.4705003,0.02016147,0.8821695,162.0266,0.01687966,0.9993503,-0.03184225,-20.99503,-0.8822383,0.02987251,0.4698543,468.5968 +0.4791301,0.02389928,0.8774185,162.7215,0.01538565,0.999247,-0.03561928,-21.04596,-0.877609,0.03056591,0.4784016,468.9764 +0.4877785,0.02936067,0.8724736,163.4071,0.01567817,0.9989784,-0.04238313,-21.0963,-0.8728266,0.03435235,0.4868198,469.3527 +0.495982,0.03284609,0.8677114,164.0863,0.01528273,0.9987993,-0.04654384,-21.14614,-0.8681983,0.03634589,0.4948844,469.7397 +0.5038631,0.02999627,0.8632625,164.7652,0.01391212,0.9989854,-0.04283244,-21.19682,-0.8636714,0.03359149,0.5029346,470.1362 +0.5118681,0.02131669,0.8587996,165.4415,0.01382847,0.9993581,-0.03304771,-21.25193,-0.8589527,0.02879195,0.5112447,470.5344 +0.5197791,0.01161916,0.8542217,166.113,0.01529449,0.9996207,-0.02290332,-21.30275,-0.8541638,0.02496954,0.5194042,470.938 +0.5277919,0.005177362,0.849358,166.7724,0.01735689,0.9997068,-0.01687942,-21.34913,-0.8491963,0.02365103,0.5275473,471.3359 +0.5358729,0.005677966,0.8442796,167.4102,0.01917432,0.9996376,-0.01889292,-21.39105,-0.8440809,0.02631269,0.5355698,471.7323 +0.5438114,0.01019405,0.8391456,168.03,0.0181867,0.9995482,-0.0239286,-21.43463,-0.8390103,0.02827393,0.5433803,472.13 +0.5511932,0.01374534,0.8342645,168.6322,0.01669008,0.9994826,-0.02749451,-21.47504,-0.8342107,0.02907872,0.5506786,472.526 +0.5583232,0.01592191,0.8294707,169.2195,0.0154935,0.9994413,-0.02961333,-21.51842,-0.8294788,0.02938521,0.5577646,472.9195 +0.5646148,0.016736,0.8251849,169.7904,0.01314177,0.9994853,-0.02926305,-21.56296,-0.8252499,0.02736673,0.5641042,473.3119 +0.5697092,0.01557953,0.8216987,170.3465,0.01124439,0.9995789,-0.02674825,-21.60638,-0.8217694,0.02447821,0.5692941,473.6974 +0.5736358,0.01675102,0.8189392,170.884,0.008766874,0.999608,-0.02658739,-21.64716,-0.8190635,0.02243101,0.5732641,474.0754 +0.5759752,0.01687474,0.8172931,171.4059,0.0083087,0.9996144,-0.02649459,-21.6871,-0.817425,0.02205086,0.5756128,474.4416 +0.5772236,0.01793641,0.8163892,171.9093,0.006407015,0.9996285,-0.0264923,-21.72668,-0.816561,0.02052259,0.5768942,474.797 +0.5768077,0.02160017,0.8165943,172.3931,0.004761851,0.9995444,-0.02980305,-21.76205,-0.816866,0.02107913,0.5764421,475.137 +0.5746037,0.02618728,0.8180127,172.8551,0.0018184,0.9994446,-0.03327284,-21.79969,-0.8184297,0.02060616,0.574237,475.4692 +0.5703315,0.0286197,0.8209159,173.3028,0.0005058412,0.9993804,-0.03519297,-21.83433,-0.8214144,0.02048691,0.5699636,475.7833 +0.5635956,0.02934948,0.8255293,173.7348,-0.0008752135,0.9993892,-0.0349331,-21.86508,-0.8260503,0.01896562,0.5632771,476.0821 +0.5544213,0.02576928,0.8318371,174.1541,-0.001430915,0.9995485,-0.03001107,-21.89308,-0.8322349,0.01544848,0.5542078,476.3598 +0.5436271,0.01943252,0.8391019,174.5565,-0.001113972,0.9997477,-0.02243117,-21.92165,-0.8393261,0.01125945,0.5435116,476.6254 +0.531606,0.01239284,0.8469011,174.9418,0.000868266,0.9998844,-0.01517649,-21.94546,-0.8469913,0.008803242,0.5315338,476.861 +0.5191476,0.006870781,0.854657,175.3061,0.005119078,0.9999247,-0.01114813,-21.96707,-0.8546692,0.01016257,0.5190734,477.0785 +0.5056287,0.004176889,0.8627411,175.6567,0.01033369,0.9998872,-0.01089716,-21.98635,-0.8626893,0.01442521,0.5055285,477.2739 +0.4906938,0.004687981,0.8713195,175.9911,0.01304385,0.9998339,-0.01272523,-22.001,-0.8712344,0.01760955,0.4905511,477.4531 +0.4739069,0.007968889,0.8805389,176.3286,0.01857892,0.999646,-0.01904601,-22.01675,-0.8803789,0.02538549,0.473591,477.6235 +0.4549659,0.01521192,0.890379,176.6674,0.01863215,0.9994726,-0.02659642,-22.02875,-0.8903139,0.02869013,0.4544424,477.7914 +0.432796,0.0214367,0.901237,177.0022,0.01666267,0.9993562,-0.03177238,-22.04077,-0.9013378,0.02876796,0.4321602,477.938 +0.4063014,0.02451254,0.9134103,177.3495,0.01273318,0.9993911,-0.03248391,-22.05233,-0.9136504,0.02482887,0.4057419,478.0911 +0.3765741,0.02365574,0.9260845,177.6937,0.01393197,0.9994162,-0.03119409,-22.05946,-0.9262817,0.02464905,0.3760246,478.1992 +0.3439993,0.02166224,0.93872,178.0557,0.01633373,0.9994445,-0.02904914,-22.07282,-0.9388278,0.02532568,0.3434544,478.3098 +0.3085024,0.01955943,0.9510225,178.4188,0.02115188,0.9994003,-0.02741587,-22.08797,-0.9509883,0.02857377,0.3079037,478.4065 +0.271965,0.01541738,0.9621836,178.7692,0.02785668,0.9993265,-0.02388633,-22.1004,-0.9619038,0.03329948,0.2713524,478.4452 +0.2340961,0.01336752,0.9721216,179.1282,0.03228266,0.9992472,-0.02151449,-22.1169,-0.9716773,0.03641911,0.2334883,478.5046 +0.1940697,0.015265,0.980869,179.4833,0.03287434,0.9992161,-0.02205488,-22.13229,-0.9804367,0.0365256,0.1934158,478.5578 +0.1515676,0.0192428,0.9882596,179.8202,0.02821096,0.999319,-0.02378482,-22.14906,-0.9880442,0.03148475,0.1509215,478.5605 +0.1066424,0.02383907,0.9940116,180.1822,0.02461864,0.9993427,-0.02660814,-22.16095,-0.9939926,0.02730877,0.1059854,478.5915 +0.0596051,0.0265872,0.9978679,180.532,0.02566309,0.999274,-0.02815759,-22.17144,-0.9978921,0.0272867,0.05887951,478.5456 +0.01050348,0.02966864,0.9995046,180.9112,0.02717231,0.9991821,-0.02994462,-22.17927,-0.9995755,0.02747336,0.009688726,478.5316 +-0.03980096,0.03215287,0.9986902,181.2983,0.0325224,0.9989942,-0.03086655,-22.18668,-0.9986782,0.03125128,-0.04080661,478.4953 +-0.09136758,0.03134533,0.9953238,181.6648,0.03456582,0.999002,-0.02828815,-22.19406,-0.9952171,0.03181955,-0.09235986,478.3846 +-0.1438537,0.02637938,0.9892473,182.0686,0.03295505,0.9992179,-0.02185303,-22.21025,-0.98905,0.02945705,-0.1446105,478.3237 +-0.196473,0.02045321,0.9802959,182.475,0.02888558,0.9994692,-0.01506394,-22.22634,-0.9800836,0.02535675,-0.1969595,478.2446 +-0.2495938,0.01604798,0.9682177,182.8536,0.02663778,0.9995981,-0.009701243,-22.23521,-0.9679841,0.02336979,-0.249921,478.0686 +-0.3036409,0.01288727,0.9526994,183.2622,0.02550979,0.99966,-0.005392135,-22.24815,-0.952445,0.02266588,-0.3038664,477.9351 +-0.3584045,0.01042843,0.9335082,183.6426,0.0249434,0.9996876,-0.001591157,-22.25515,-0.9332331,0.02271458,-0.3585526,477.6936 +-0.4133503,0.008705867,0.9105305,184.0625,0.02324248,0.9997293,0.0009925735,-22.26385,-0.9102754,0.02157326,-0.4134407,477.5051 +-0.4674428,0.007849424,0.8839885,184.4883,0.0204031,0.99979,0.001911223,-22.26981,-0.8837878,0.01892949,-0.4675047,477.2852 +-0.5199215,0.007071037,0.8541848,184.888,0.01748907,0.9998442,0.002368341,-22.2739,-0.8540349,0.01617024,-0.5199642,476.9508 +-0.5682859,0.00610823,0.8228086,185.324,0.01616386,0.9998623,0.003741208,-22.27835,-0.8226724,0.01542583,-0.5683063,476.6519 +-0.6109399,0.006343671,0.7916516,185.7594,0.01717988,0.9998386,0.005246274,-22.28346,-0.7914905,0.01680563,-0.6109503,476.3068 +-0.6470006,0.007169332,0.7624558,186.1803,0.0196294,0.999781,0.007256109,-22.28821,-0.7622367,0.01966125,-0.6469996,475.8804 +-0.6760413,0.007683644,0.7368237,186.6263,0.02128898,0.9997319,0.009107522,-22.29195,-0.7365561,0.02184328,-0.6760236,475.461 +-0.6991496,0.008143192,0.7149291,187.0774,0.022525,0.9996896,0.01064116,-22.29447,-0.7146205,0.02354354,-0.699116,475.0069 +-0.7176211,0.008450355,0.6963826,187.5314,0.02321978,0.9996608,0.01179741,-22.29466,-0.6960466,0.02463592,-0.7175738,474.5112 +-0.7323401,0.009152447,0.6808776,188.0019,0.02386389,0.9996404,0.01223027,-22.29351,-0.6805208,0.0252051,-0.7322951,474.0017 +-0.7444937,0.01015969,0.6675522,188.4787,0.02459606,0.9996228,0.0122174,-22.29192,-0.6671762,0.02551493,-0.7444627,473.4645 +-0.7545365,0.01127238,0.6561613,188.961,0.02554549,0.9995992,0.01220299,-22.28926,-0.6557607,0.02596956,-0.754522,472.901 +-0.7632839,0.01157748,0.6459595,189.4552,0.02518526,0.9996126,0.01184361,-22.28709,-0.6455722,0.02530869,-0.7632797,472.3165 +-0.770938,0.0118466,0.6368001,189.9537,0.0258715,0.9995843,0.01272556,-22.28748,-0.6363846,0.02628558,-0.7709239,471.7035 +-0.7769622,0.01193999,0.629434,190.4622,0.02724266,0.9995212,0.01466752,-22.27854,-0.6289575,0.02854356,-0.7769154,471.0648 +-0.7809281,0.01148275,0.6245155,190.9835,0.02896247,0.9994213,0.01784021,-22.26966,-0.6239491,0.03201942,-0.7808086,470.4042 +-0.7831914,0.011512,0.6216742,191.5189,0.03075173,0.9993222,0.02023614,-22.25842,-0.6210198,0.03496632,-0.7830145,469.7252 +-0.7847382,0.01333012,0.619684,192.064,0.03160829,0.9993285,0.01853049,-22.25096,-0.6190209,0.03412873,-0.7846326,469.0332 +-0.7858391,0.01566965,0.6182326,192.6283,0.03086225,0.999427,0.01389781,-22.23395,-0.6176605,0.03000149,-0.7858723,468.3284 +-0.7868479,0.01661999,0.6169233,193.2083,0.02940628,0.9995115,0.01057892,-22.21621,-0.6164461,0.02646541,-0.7869522,467.6084 +-0.7879402,0.01569703,0.6155517,193.8022,0.0294131,0.9994933,0.01216256,-22.19934,-0.6150488,0.02768865,-0.7880026,466.8666 +-0.7893553,0.01194066,0.6138206,194.4076,0.02925014,0.9994069,0.01817333,-22.18783,-0.6132395,0.03229954,-0.7892363,466.1013 +-0.7907708,0.01065037,0.6120198,195.0174,0.02966847,0.9993403,0.02094313,-22.17589,-0.611393,0.0347189,-0.7905651,465.3209 +-0.7918078,0.01085745,0.6106739,195.6296,0.03044271,0.9993008,0.02170537,-22.15831,-0.6100112,0.03577704,-0.7915846,464.5323 +-0.7921312,0.01262709,0.6102203,196.2448,0.03208603,0.999265,0.02097362,-22.13909,-0.6095069,0.0361934,-0.7919541,463.7361 +-0.7922391,0.01468929,0.6100341,196.8637,0.03399396,0.9992201,0.02008659,-22.12306,-0.6092632,0.03665085,-0.7921205,462.9331 +-0.7919029,0.0174693,0.6103971,197.4824,0.03627109,0.9991714,0.01846068,-22.11544,-0.6095688,0.03675883,-0.7918804,462.1293 +-0.7911274,0.01888848,0.6113598,198.1054,0.03688706,0.9991771,0.01686309,-22.10943,-0.6105382,0.03589211,-0.7911731,461.3258 +-0.7898463,0.02023706,0.612971,198.7291,0.03841487,0.9991254,0.01651379,-22.10434,-0.6121006,0.03659055,-0.7899328,460.5227 +-0.7884338,0.02105112,0.6147594,199.3558,0.03978259,0.9990669,0.01681057,-22.09633,-0.6138318,0.03771074,-0.7885355,459.7229 +-0.7872558,0.02067332,0.61628,199.9852,0.0406667,0.9990026,0.01843704,-22.09257,-0.6152842,0.03957673,-0.7873112,458.9205 +-0.7861491,0.02024244,0.6177054,200.6142,0.04137084,0.9989453,0.01991653,-22.08882,-0.6166507,0.04121234,-0.7861573,458.1214 +-0.7850313,0.02027045,0.6191245,201.2443,0.04206962,0.9989015,0.02063845,-22.08544,-0.618026,0.04224815,-0.7850216,457.3244 +-0.7838649,0.02091971,0.6205789,201.8766,0.04262356,0.9988876,0.0201661,-22.0792,-0.6194667,0.04225877,-0.7838846,456.5279 +-0.782798,0.02113753,0.6219169,202.5105,0.04338556,0.9988447,0.02066034,-22.06915,-0.6207617,0.04315508,-0.7828106,455.7302 +-0.7816798,0.02116361,0.6233208,203.1447,0.04366163,0.998829,0.02084092,-22.06266,-0.6221498,0.04350612,-0.7816884,454.9361 +-0.7806489,0.02267534,0.6245584,203.7796,0.0442339,0.99884,0.0190248,-22.05633,-0.6234025,0.04247833,-0.7807463,454.1445 +-0.7795383,0.02573364,0.6258257,204.411,0.04560877,0.9988354,0.01573937,-22.05293,-0.6246918,0.04081258,-0.7798041,453.3583 +-0.7787489,0.02544459,0.6268196,205.0471,0.04465558,0.9988908,0.0149311,-22.04931,-0.6257444,0.03961856,-0.7790214,452.5724 +-0.7782935,0.02365748,0.6274548,205.6848,0.04389513,0.9988951,0.01678519,-22.05096,-0.6263645,0.04060601,-0.778472,451.7853 +-0.7781083,0.0203853,0.6277993,206.3218,0.04226262,0.9989074,0.01994564,-22.05089,-0.6267067,0.0420523,-0.7781197,451.001 +-0.7780237,0.01895226,0.6279491,206.9558,0.04148207,0.9989133,0.02124752,-22.05042,-0.6268639,0.04257969,-0.7779643,450.2194 +-0.7780656,0.01908808,0.627893,207.5876,0.04116912,0.9989388,0.02064753,-22.04869,-0.6268326,0.04191493,-0.7780257,449.4396 +-0.77803,0.01849977,0.6279548,208.222,0.03934366,0.999039,0.01931435,-22.04485,-0.626994,0.03973318,-0.7780101,448.6651 +-0.7782215,0.01823176,0.6277253,208.8544,0.03840544,0.9990892,0.01859535,-22.04239,-0.6268145,0.03857936,-0.7782128,447.8904 +-0.7785812,0.01788197,0.6272891,209.4864,0.0382042,0.9990905,0.01893761,-22.03971,-0.6263799,0.03870954,-0.7785562,447.1139 +-0.7792701,0.01752041,0.6264433,210.1144,0.03798319,0.9990918,0.01930687,-22.0395,-0.625536,0.03883957,-0.7792279,446.3379 +-0.7796828,0.0182989,0.6259073,210.7393,0.03925895,0.9990349,0.01969664,-22.039,-0.6249427,0.03992958,-0.7796487,445.5612 +-0.7801946,0.01908503,0.6252458,211.3594,0.04033886,0.998989,0.01984247,-22.03932,-0.6242349,0.04070267,-0.7801756,444.7863 +-0.7805389,0.02023304,0.6247797,211.9765,0.04090725,0.9989869,0.01875405,-22.04187,-0.6237672,0.04019628,-0.7805758,444.0145 +-0.7806757,0.02107192,0.624581,212.5952,0.04081055,0.999017,0.01730534,-22.04275,-0.6236024,0.03899934,-0.7807682,443.243 +-0.7804618,0.01939587,0.6249026,213.2149,0.04025924,0.9990033,0.01927381,-22.04257,-0.6239059,0.04020056,-0.7804648,442.4742 +-0.7800145,0.0154046,0.6255718,213.8362,0.03900491,0.9989499,0.02403555,-22.04023,-0.6245446,0.04314844,-0.7797962,441.7066 +-0.7794661,0.01225732,0.6263245,214.4556,0.03783862,0.9989042,0.02754168,-22.03412,-0.6253006,0.04516705,-0.7790757,440.9403 +-0.7785806,0.01188408,0.6274321,215.0695,0.03760977,0.9989071,0.02774984,-22.02541,-0.6264166,0.04520306,-0.7781766,440.1808 +-0.7769155,0.01290895,0.6294726,215.6837,0.03863907,0.9988828,0.02720493,-22.01679,-0.6284181,0.04545816,-0.7765463,439.4205 +-0.7749132,0.01310981,0.6319317,216.3021,0.03682207,0.9990232,0.0244281,-22.00978,-0.6309942,0.04219868,-0.774639,438.6654 +-0.7731737,0.0147383,0.6340232,216.9249,0.03677864,0.9990894,0.02162601,-22.003,-0.633127,0.04003916,-0.7730116,437.9084 +-0.7715675,0.01539341,0.6359612,217.5556,0.03722159,0.9990869,0.0209755,-21.99467,-0.6350576,0.03985549,-0.7714359,437.1433 +-0.7704077,0.01415664,0.6373945,218.1929,0.03678418,0.999075,0.02227076,-21.98857,-0.6364896,0.04060359,-0.7702157,436.3755 +-0.7691183,0.01690981,0.6388828,218.8302,0.03820884,0.9990784,0.01955427,-21.98274,-0.6379633,0.0394505,-0.7690555,435.6032 +-0.7679626,0.02189733,0.6401203,219.4713,0.04126639,0.9990305,0.01533296,-21.97423,-0.639164,0.03819058,-0.7681216,434.8241 +-0.7672652,0.02273704,0.6409269,220.1217,0.04262868,0.9989693,0.01559286,-21.96754,-0.6399117,0.03928572,-0.7674435,434.0346 +-0.7671918,0.01906414,0.6411345,220.7841,0.03985243,0.9990437,0.01798147,-21.96566,-0.6401786,0.03934599,-0.7672178,433.242 +-0.767527,0.0183219,0.6407547,221.4505,0.03682684,0.9992008,0.01554156,-21.96227,-0.6399578,0.03552553,-0.7675883,432.4471 +-0.7681766,0.01640919,0.6400277,222.1189,0.03301799,0.9993566,0.01400721,-21.95656,-0.639386,0.03189243,-0.7682241,431.6474 +-0.7689608,0.01226341,0.6391784,222.7897,0.02899708,0.999456,0.01570903,-21.95169,-0.638638,0.03061393,-0.768898,430.8401 +-0.7695775,0.009357356,0.6384849,223.4667,0.02790353,0.9994303,0.0189854,-21.94718,-0.6379435,0.03242671,-0.7694001,430.0171 +-0.7690962,0.009368566,0.6390644,224.1398,0.02795555,0.9994287,0.01899227,-21.94408,-0.6385213,0.03247227,-0.7689187,429.213 +-0.7686141,0.009379703,0.639644,224.8136,0.02800759,0.9994271,0.01899914,-21.94098,-0.6390993,0.03251789,-0.7684364,428.4082 +-0.768132,0.009390746,0.6402228,225.4867,0.02805955,0.9994255,0.01900601,-21.93788,-0.6396765,0.03256348,-0.7679541,427.6041 +-0.767649,0.009401715,0.6408016,226.1604,0.02811153,0.9994239,0.01901288,-21.93478,-0.6402537,0.03260912,-0.767471,426.7994 +-0.767166,0.00941259,0.6413796,226.8335,0.02816344,0.9994223,0.01901974,-21.93169,-0.64083,0.03265475,-0.7669879,425.9954 +-0.7666827,0.009423379,0.6419571,227.5065,0.02821531,0.9994208,0.0190266,-21.92859,-0.6414059,0.03270037,-0.7665044,425.1915 +-0.7661988,0.00943409,0.6425345,228.1797,0.02826717,0.9994192,0.01903346,-21.92549,-0.6419817,0.03274604,-0.7660203,424.3873 +-0.7657144,0.009444715,0.6431114,228.8529,0.02831901,0.9994176,0.01904032,-21.9224,-0.642557,0.03279172,-0.7655359,423.5832 +-0.7652296,0.009455258,0.643688,229.5261,0.02837081,0.999416,0.01904718,-21.9193,-0.643132,0.03283741,-0.7650509,422.7791 +-0.7647446,0.009465712,0.644264,230.1991,0.02842257,0.9994144,0.01905404,-21.9162,-0.6437063,0.0328831,-0.7645657,421.9752 +-0.7642595,0.009476076,0.6448393,230.8716,0.02847427,0.9994128,0.01906089,-21.91311,-0.64428,0.03292878,-0.7640804,421.1719 +-0.763773,0.009486376,0.6454153,231.5454,0.02852604,0.9994111,0.01906775,-21.91001,-0.6448543,0.03297456,-0.7635938,420.3671 +-0.7632865,0.009496583,0.6459905,232.2186,0.02857774,0.9994095,0.01907461,-21.90691,-0.6454279,0.03302032,-0.7631071,419.563 +-0.7627997,0.009506704,0.6465651,232.8916,0.02862939,0.9994079,0.01908146,-21.90382,-0.6460008,0.03306609,-0.7626201,418.7591 +-0.7623125,0.009516739,0.6471392,233.5644,0.02868101,0.9994063,0.01908831,-21.90072,-0.6465733,0.03311185,-0.7621328,417.9553 +-0.7618248,0.009526693,0.6477131,234.2375,0.02873261,0.9994047,0.01909516,-21.89762,-0.6471456,0.03315765,-0.761645,417.1513 +-0.7613487,0.009525737,0.6482727,234.9109,0.02878533,0.9994027,0.01912099,-21.89458,-0.6477033,0.03321847,-0.7611681,416.3472 +-0.7617155,0.008773513,0.6478523,235.5593,0.02887119,0.9993747,0.02041144,-21.89533,-0.647268,0.03425197,-0.7614925,415.5883 +-0.7623643,0.008314187,0.6470947,236.1975,0.02985455,0.9993047,0.0223331,-21.89377,-0.6464591,0.03634467,-0.7620824,414.8396 +-0.7633639,0.006171204,0.6459393,236.823,0.02896891,0.9992754,0.0246882,-21.8926,-0.6453188,0.03755823,-0.7629895,414.1078 +-0.7646142,0.005854825,0.6444617,237.4368,0.02950878,0.999228,0.02593252,-21.89242,-0.6438124,0.03884564,-0.7641967,413.389 +-0.7688284,0.005989919,0.6394271,238.0064,0.02971273,0.9992107,0.02636547,-21.88979,-0.6387644,0.03926964,-0.7683995,412.722 +-0.7840339,0.006991709,0.6206787,238.3941,0.0298933,0.9992016,0.02650522,-21.88674,-0.6199978,0.03933511,-0.7836169,412.1137 +-0.7987423,0.007976195,0.6016205,238.7802,0.03004554,0.9991934,0.02664283,-21.88372,-0.6009227,0.03935676,-0.7983376,411.5074 +-0.8129861,0.008944538,0.5822145,239.166,0.03017003,0.999186,0.02677802,-21.8807,-0.581501,0.03933558,-0.8125941,410.9017 +-0.8267811,0.009896909,0.5624368,239.5523,0.03026696,0.9991795,0.02691032,-21.8777,-0.561709,0.03927219,-0.8264022,410.2957 +-0.8400787,0.01082907,0.5423565,239.9378,0.03033615,0.9991739,0.0270387,-21.87471,-0.5416157,0.03916763,-0.8397132,409.6908 +-0.8529149,0.01174266,0.5219179,240.3239,0.03037785,0.9991693,0.0271629,-21.87172,-0.5211654,0.03902237,-0.8525631,409.0855 +-0.865241,0.01263338,0.501197,240.7094,0.03039204,0.9991656,0.02728185,-21.86876,-0.5004342,0.03883777,-0.864903,408.4813 +-0.8770774,0.01350185,0.4801593,241.0951,0.03037885,0.9991629,0.02739517,-21.8658,-0.4793875,0.03861436,-0.8767534,407.877 +-0.8883894,0.01434471,0.4588666,241.4802,0.0303385,0.9991612,0.02750192,-21.86286,-0.4580872,0.03835373,-0.8880794,407.2739 +-0.8992007,0.0151629,0.4372737,241.8657,0.03027099,0.9991605,0.02760173,-21.85993,-0.4364881,0.03805619,-0.8989048,406.6707 +-0.9094889,0.015954,0.415422,242.2512,0.03017654,0.9991608,0.02769379,-21.85702,-0.4146315,0.03772318,-0.9092071,406.068 +-0.9192474,0.01671672,0.3933254,242.6366,0.03005534,0.9991622,0.02777744,-21.85412,-0.3925315,0.03735586,-0.9189796,405.4659 +-0.9284824,0.01745083,0.3709664,243.0226,0.02990737,0.9991645,0.02785213,-21.85123,-0.3701704,0.03695483,-0.9282285,404.8637 +-0.9371662,0.01815346,0.3484107,243.408,0.02973321,0.9991679,0.02791704,-21.84837,-0.347614,0.03652227,-0.9369261,404.2625 +-0.9437329,0.01894385,0.3301657,243.7764,0.02957872,0.9991918,0.02721621,-21.83716,-0.3293833,0.03545071,-0.9435305,403.5786 +-0.962458,0.01874921,0.2707826,243.966,0.02881721,0.9990314,0.03325284,-21.83791,-0.2698968,0.03980765,-0.962066,403.1131 +-0.9785276,0.01868605,0.2052671,244.011,0.02730254,0.9988573,0.0392249,-21.82444,-0.2042996,0.04398695,-0.9779196,402.484 +-0.9907267,0.02005063,0.1343826,244.0732,0.02607779,0.9987242,0.04324149,-21.81585,-0.1333442,0.04634489,-0.9899855,401.9623 +-0.997894,0.02253704,0.06082496,244.0922,0.02528558,0.9986758,0.0448026,-21.80229,-0.05973469,0.04624623,-0.9971424,401.4518 +-0.9996072,0.02459279,-0.013442,244.0014,0.02395159,0.9986568,0.04594403,-21.78347,0.01455383,0.04560402,-0.9988535,400.8392 +-0.9957923,0.02713017,-0.08753093,243.9346,0.0230138,0.9985966,0.04769901,-21.76626,0.08870217,0.04548388,-0.9950191,400.3437 +-0.986574,0.03011763,-0.1605141,243.7613,0.02241888,0.9985195,0.04956059,-21.74572,0.1617691,0.04529663,-0.9857884,399.7517 +-0.9719507,0.03403303,-0.2327093,243.6168,0.02264365,0.9984193,0.05144072,-21.72614,0.2340921,0.04472845,-0.9711849,399.284 +-0.9523131,0.03745213,-0.3028156,243.4311,0.02217485,0.998309,0.05373372,-21.70597,0.3043159,0.04445643,-0.9515331,398.8247 +-0.9280494,0.04045793,-0.3702534,243.1502,0.02097283,0.9981821,0.05650337,-21.68255,0.3718663,0.04467265,-0.9272107,398.2754 +-0.90014,0.04540055,-0.4332283,242.8901,0.02155957,0.9979782,0.05978865,-21.65624,0.4350668,0.04447794,-0.8992989,397.8287 +-0.870119,0.04810416,-0.4904885,242.5958,0.0190082,0.9977602,0.06413408,-21.63038,0.492475,0.04648097,-0.8690844,397.385 +-0.8384504,0.05116408,-0.5425709,242.2267,0.01657849,0.997517,0.06844601,-21.60289,0.5447257,0.04839358,-0.8372167,396.8846 +-0.8059198,0.05234654,-0.589706,241.8736,0.01285376,0.9973956,0.07096948,-21.57671,0.5918852,0.04961576,-0.8044937,396.462 +-0.773253,0.05348638,-0.6318379,241.4917,0.009717709,0.9973187,0.07253242,-21.54843,0.6340232,0.04994589,-0.7716994,396.0469 +-0.7414349,0.05328828,-0.6689056,241.061,0.005215145,0.9972693,0.07366672,-21.51127,0.6710045,0.05113063,-0.7396881,395.5998 +-0.7115578,0.05440858,-0.7005179,240.6315,0.001994156,0.9971497,0.07542214,-21.47767,0.7026247,0.05227026,-0.7096381,395.2015 +-0.6841264,0.05448534,-0.7273256,240.1694,-0.002852164,0.9969984,0.07736982,-21.43638,0.7293579,0.05500518,-0.6819175,394.7841 +-0.6574669,0.05390279,-0.751553,239.7059,-0.008858379,0.996816,0.07924293,-21.39698,0.7534314,0.05875714,-0.6548959,394.4034 +-0.6289266,0.05317776,-0.7756439,239.2216,-0.01497334,0.9966445,0.08047052,-21.35682,0.7773204,0.06222403,-0.62602,394.0338 +-0.5990019,0.0502546,-0.7991691,238.7074,-0.02293355,0.9965426,0.07985556,-21.30633,0.8004191,0.06616141,-0.5957783,393.6438 +-0.5695232,0.04854605,-0.8205405,238.1753,-0.028291,0.9965052,0.07859306,-21.26486,0.8214882,0.06797447,-0.5661594,393.2945 +-0.5394629,0.04724548,-0.8406829,237.6186,-0.03238569,0.9965215,0.07678523,-21.22262,0.8413863,0.06864887,-0.5360562,392.9576 +-0.5101162,0.04562127,-0.8588948,237.0248,-0.03637106,0.9965549,0.07453483,-21.17762,0.8593361,0.06926033,-0.5066995,392.6107 +-0.4819779,0.04562383,-0.8749948,236.4194,-0.03655882,0.9967265,0.07210904,-21.13375,0.8754203,0.06674373,-0.4787321,392.3006 +-0.4543489,0.0458005,-0.8896457,235.7934,-0.03648832,0.9968825,0.06995613,-21.08324,0.8900762,0.06424616,-0.4512613,391.9948 +-0.4272227,0.046645,-0.9029425,235.1318,-0.03568391,0.9970204,0.06838864,-21.03338,0.903442,0.06143768,-0.4242852,391.6856 +-0.4008766,0.04698284,-0.9149266,234.4594,-0.03555414,0.9971339,0.06678242,-20.98293,0.9154419,0.05930093,-0.3980571,391.4036 +-0.3750441,0.04689882,-0.9258199,233.7646,-0.03308713,0.9974058,0.06392852,-20.93953,0.9264163,0.05460873,-0.3725194,391.1388 +-0.3507353,0.0477446,-0.9352568,233.0421,-0.02879977,0.9976772,0.0617315,-20.89645,0.9360316,0.04858659,-0.3485456,390.8764 +-0.3270873,0.05054483,-0.9436415,232.2968,-0.02169049,0.9979039,0.06096974,-20.8493,0.9447451,0.04041047,-0.3253053,390.642 +-0.305292,0.05451203,-0.9506973,231.5269,-0.01673961,0.9978987,0.06259402,-20.80401,0.9521116,0.03502376,-0.3037379,390.4075 +-0.2848599,0.05982027,-0.9567008,230.7368,-0.01221489,0.9977433,0.06602359,-20.75455,0.9584913,0.03049347,-0.2834863,390.1934 +-0.2666036,0.06239913,-0.9617842,229.9283,-0.01110844,0.9976368,0.06780442,-20.70711,0.9637422,0.02876082,-0.2652804,389.9829 +-0.2501618,0.06072214,-0.9662981,229.1019,-0.01245417,0.997747,0.06592262,-20.6562,0.9681239,0.02852576,-0.248842,389.7734 +-0.2333728,0.05670424,-0.9707326,228.2594,-0.0143814,0.9979878,0.06175375,-20.60504,0.9722809,0.02837214,-0.2320877,389.5766 +-0.21605,0.05385987,-0.9748957,227.3933,-0.01747153,0.9981042,0.059014,-20.55167,0.9762259,0.02978289,-0.2146993,389.3896 +-0.1981696,0.05435629,-0.9786594,226.5029,-0.02127404,0.9979874,0.05973761,-20.49991,0.9799368,0.03265822,-0.1966144,389.2043 +-0.1815398,0.0573934,-0.9817074,225.5961,-0.01989006,0.9978769,0.06201685,-20.4467,0.9831824,0.03078474,-0.1800128,389.0437 +-0.1673434,0.05757708,-0.984216,224.6689,-0.01643411,0.9979916,0.06117721,-20.39489,0.9857616,0.02641232,-0.1660611,388.8983 +-0.1554729,0.05440413,-0.9863409,223.7259,-0.0114438,0.9983161,0.0568685,-20.3426,0.9877738,0.020129,-0.1545885,388.759 +-0.1457322,0.0484326,-0.9881379,222.7697,-0.008901339,0.9986963,0.05026291,-20.2934,0.989284,0.01612067,-0.145111,388.6276 +-0.1365262,0.04327345,-0.9896909,221.7934,-0.00668481,0.9989825,0.04460189,-20.24699,0.9906139,0.01270523,-0.136098,388.499 +-0.1289428,0.04162493,-0.9907781,220.7978,-0.005257496,0.9990759,0.04265778,-20.20494,0.9916381,0.01070943,-0.1286048,388.3725 +-0.1246541,0.04407802,-0.9912207,219.7826,-0.006753954,0.9989519,0.04527119,-20.16738,0.9921772,0.0123379,-0.1242257,388.2427 +-0.1214762,0.04616605,-0.9915202,218.753,-0.006618718,0.9988579,0.0473186,-20.1331,0.9925722,0.01231068,-0.1210319,388.1101 +-0.1177511,0.04829812,-0.991868,217.7107,-0.004636305,0.9987789,0.04918505,-20.09684,0.9930323,0.0103902,-0.1173833,387.9834 +-0.1144637,0.04700551,-0.9923148,216.6597,-0.00579385,0.9988313,0.04798253,-20.06123,0.9934105,0.01124158,-0.1140575,387.8539 +-0.1121329,0.04716083,-0.9925735,215.5989,-0.006739565,0.998814,0.04821873,-20.02189,0.9936703,0.01209642,-0.1116821,387.7291 +-0.1097634,0.0478814,-0.9928038,214.5306,-0.006247789,0.998786,0.04886067,-19.98678,0.9939381,0.01156594,-0.109331,387.6072 +-0.1066943,0.04700951,-0.99318,213.4542,-0.006191507,0.9988309,0.04794212,-19.95111,0.9942726,0.01126444,-0.1062785,387.4877 +-0.1039731,0.04479021,-0.9935711,212.3743,-0.009951874,0.9988886,0.04607135,-19.91528,0.9945303,0.01467808,-0.1034118,387.3646 +-0.1013287,0.04531846,-0.9938203,211.2818,-0.01196318,0.9988342,0.04676685,-19.87803,0.994781,0.01662808,-0.1006684,387.2455 +-0.09938475,0.04649215,-0.9939624,210.187,-0.0144187,0.9987357,0.04815713,-19.84214,0.9949446,0.01911773,-0.09858874,387.1296 +-0.09671908,0.04593819,-0.9942511,209.0903,-0.01499536,0.9987537,0.04760496,-19.81323,0.9951987,0.01951346,-0.09590967,387.0168 +-0.09385587,0.04602579,-0.9945214,207.9899,-0.01732265,0.9987041,0.04785417,-19.77988,0.995435,0.02171914,-0.09293694,386.9066 +-0.09059291,0.04693806,-0.9947813,206.8819,-0.01946938,0.9986143,0.04889197,-19.75329,0.9956976,0.02379704,-0.08955351,386.8005 +-0.08808,0.04692122,-0.9950077,205.7692,-0.02153996,0.9985667,0.04899581,-19.72236,0.9958804,0.02574798,-0.08694306,386.6974 +-0.08602468,0.0464546,-0.9952094,204.658,-0.02132346,0.9985977,0.04845594,-19.69466,0.9960647,0.02538971,-0.08491347,386.5955 +-0.0839093,0.04737762,-0.9953465,203.5387,-0.02239941,0.998527,0.04941733,-19.66376,0.9962216,0.02644175,-0.08272446,386.4958 +-0.08159186,0.05001025,-0.9954104,202.4126,-0.02421315,0.9983461,0.05214246,-19.63786,0.9963716,0.02835642,-0.080246,386.399 +-0.07896098,0.05071788,-0.9955867,201.2868,-0.02581232,0.998266,0.05290159,-19.60862,0.9965434,0.02987557,-0.07751492,386.3043 +-0.07642447,0.04888486,-0.9958763,200.1657,-0.02802534,0.9982974,0.05115441,-19.58025,0.9966814,0.03181922,-0.07492433,386.2092 +-0.07355643,0.04781437,-0.9961442,199.0457,-0.02922204,0.9983178,0.0500765,-19.55141,0.9968628,0.03279281,-0.07203546,386.1196 +-0.07107075,0.04901074,-0.9962665,197.9189,-0.0311219,0.9981969,0.05132586,-19.5267,0.9969856,0.03465347,-0.06941729,386.0312 +-0.06842371,0.04884265,-0.9964601,196.7958,-0.03151278,0.9981966,0.05109166,-19.49713,0.9971585,0.0348971,-0.06676114,385.9469 +-0.06650492,0.04895539,-0.9965844,195.6736,-0.03068299,0.9982229,0.05108346,-19.46311,0.9973142,0.03397549,-0.06488464,385.8666 +-0.06472774,0.05026023,-0.9966365,194.5558,-0.03037359,0.9981688,0.05231017,-19.43016,0.9974406,0.03365734,-0.06308263,385.7863 +-0.06312435,0.05215828,-0.9966418,193.4288,-0.03193109,0.9980165,0.05425266,-19.39507,0.9974947,0.03524852,-0.06133367,385.7071 +-0.06151419,0.05337228,-0.9966782,192.3184,-0.03349436,0.9978964,0.05550477,-19.35928,0.997544,0.03679743,-0.05959712,385.628 +-0.05989451,0.05392218,-0.9967473,191.2072,-0.03433238,0.9978378,0.05604421,-19.32368,0.9976141,0.03757744,-0.05791372,385.5546 +-0.05837261,0.05546424,-0.9967529,190.0993,-0.03351825,0.9977835,0.05748452,-19.28759,0.997732,0.03676494,-0.05638416,385.4845 +-0.05698067,0.05768414,-0.9967075,188.9927,-0.03279731,0.9976825,0.05961556,-19.24749,0.9978364,0.03608626,-0.05495673,385.4196 +-0.05558996,0.05915353,-0.9966999,187.8862,-0.03220621,0.9976178,0.06100429,-19.20522,0.9979341,0.03549115,-0.05355242,385.3584 +-0.05417844,0.05992754,-0.9967314,186.7861,-0.03130691,0.9976047,0.06168178,-19.16163,0.9980403,0.0345464,-0.05217252,385.3007 +-0.05276412,0.05899035,-0.9968632,185.6895,-0.03127502,0.9976664,0.06069329,-19.11846,0.9981171,0.03437935,-0.05079606,385.2438 +-0.05156647,0.05689089,-0.9970478,184.5992,-0.03185524,0.9977743,0.05857988,-19.077,0.9981613,0.03478196,-0.04963943,385.1879 +-0.05093102,0.05525206,-0.9971727,183.5108,-0.03186297,0.9978702,0.05691814,-19.03621,0.9981937,0.03467178,-0.04906205,385.1337 +-0.05075078,0.05567486,-0.9971583,182.4291,-0.03073477,0.9978849,0.0572797,-18.9979,0.9982383,0.03355442,-0.04893229,385.0824 +-0.05098593,0.05749524,-0.997043,181.3482,-0.02959924,0.9978159,0.05905344,-18.96222,0.9982606,0.03252261,-0.04917275,385.0289 +-0.05092165,0.05921157,-0.9969459,180.2696,-0.02769353,0.9977733,0.06067525,-18.92984,0.9983186,0.03069864,-0.04916848,384.9816 +-0.05099047,0.0606059,-0.9968585,179.1914,-0.02701619,0.997708,0.06203946,-18.89174,0.9983336,0.03009474,-0.04923625,384.9305 +-0.05126766,0.06004812,-0.9968781,178.1253,-0.02805629,0.9977101,0.06154113,-18.85706,0.9982907,0.03112377,-0.04946553,384.8803 +-0.05155861,0.05939409,-0.9969023,177.0607,-0.02832901,0.9977412,0.06090922,-18.82032,0.998268,0.03138165,-0.04975957,384.8295 +-0.05150853,0.05949,-0.9968991,175.9984,-0.02917855,0.9977084,0.06104592,-18.78602,0.9982462,0.03223245,-0.04965466,384.7805 +-0.05154152,0.05952774,-0.9968952,174.9402,-0.02982764,0.9976848,0.06111705,-18.75404,0.9982253,0.03288509,-0.04964662,384.7322 +-0.05157104,0.05914562,-0.9969164,173.8847,-0.0300651,0.9977003,0.06074742,-18.72164,0.9982166,0.0331052,-0.04967422,384.6868 +-0.05120311,0.05916123,-0.9969344,172.8338,-0.02833324,0.9977559,0.0606652,-18.68858,0.9982862,0.03135263,-0.04941198,384.6469 +-0.05092903,0.0591637,-0.9969483,171.787,-0.02765266,0.9977774,0.06062555,-18.6521,0.9983193,0.03065587,-0.0491798,384.6052 +-0.05045208,0.05883948,-0.9969918,170.7419,-0.02740491,0.9978056,0.06027433,-18.61307,0.9983504,0.03036344,-0.04872887,384.5638 +-0.04926507,0.058703,-0.9970592,169.7031,-0.0279625,0.9977989,0.06012821,-18.57224,0.9983942,0.03084249,-0.04751515,384.5188 +-0.04735143,0.05769289,-0.9972108,168.6675,-0.02786846,0.9978657,0.05905409,-18.5301,0.9984894,0.03058703,-0.04564255,384.4792 +-0.0452591,0.05478157,-0.9974721,167.64,-0.02858023,0.9980155,0.05610822,-18.4897,0.9985663,0.03104739,-0.04360362,384.4394 +-0.04273371,0.05154657,-0.9977559,166.6211,-0.02799579,0.9982142,0.05276931,-18.45106,0.9986941,0.03018799,-0.04121431,384.4059 +-0.03989209,0.05026464,-0.9979389,165.61,-0.02573075,0.998351,0.05131398,-18.41574,0.9988726,0.02772474,-0.03853296,384.3758 +-0.03713536,0.0511382,-0.998001,164.6091,-0.02632601,0.9982931,0.05213276,-18.38192,0.9989634,0.02820935,-0.0357257,384.3467 +-0.03404238,0.0533085,-0.9979977,163.6235,-0.02451048,0.9982315,0.05415707,-18.34801,0.9991197,0.02630504,-0.03267556,384.3215 +-0.03104022,0.05529644,-0.9979874,162.6519,-0.02259026,0.9981746,0.05600944,-18.31433,0.9992628,0.02428334,-0.0297344,384.3009 +-0.02779158,0.05599001,-0.9980445,161.6951,-0.02225938,0.9981479,0.05661565,-18.27983,0.9993658,0.02378929,-0.02649381,384.2813 +-0.02411791,0.05616816,-0.99813,160.7589,-0.0226903,0.9981324,0.05671657,-18.24832,0.9994515,0.02401575,-0.0227984,384.2636 +-0.02023153,0.05555837,-0.9982505,159.8416,-0.02178849,0.9981932,0.05599678,-18.21543,0.9995578,0.02288327,-0.01898444,384.2531 +-0.01620778,0.05510218,-0.9983492,158.9475,-0.02072563,0.9982473,0.05543303,-18.18488,0.9996538,0.02158986,-0.01503734,384.2468 +-0.0132093,0.0559608,-0.9983456,158.0686,-0.01969749,0.9982244,0.05621463,-18.15401,0.9997187,0.02040746,-0.01208356,384.2443 +-0.01190783,0.05467642,-0.9984331,157.2128,-0.01709496,0.9983468,0.05487559,-18.1274,0.9997829,0.01772163,-0.01095345,384.2441 +-0.01285885,0.05380137,-0.9984689,156.3762,-0.01342962,0.998452,0.05397343,-18.10128,0.9998271,0.0141031,-0.01211642,384.2466 +-0.01540844,0.05268107,-0.9984925,155.559,-0.01031626,0.9985495,0.05284328,-18.07348,0.999828,0.01111495,-0.01484262,384.2444 +-0.01884824,0.05378475,-0.9983747,154.7575,-0.008261533,0.9985095,0.05394799,-18.04594,0.9997882,0.009264933,-0.0183758,384.2381 +-0.02441083,0.05708137,-0.9980711,153.975,-0.005194494,0.9983478,0.05722425,-18.02334,0.9996885,0.00658137,-0.02407399,384.2242 +-0.03245852,0.05889311,-0.9977365,153.2125,-0.001836339,0.9982572,0.0589836,-18.00096,0.9994714,0.003746707,-0.0322938,384.2049 +-0.04280651,0.05867571,-0.9973589,152.4655,0.0005813138,0.9982752,0.05870467,-17.97357,0.9990832,0.001933168,-0.04276678,384.1773 +-0.05515255,0.05778085,-0.9968047,151.7415,0.002488184,0.998329,0.05773155,-17.94639,0.9984748,0.0007038134,-0.05520415,384.1407 +-0.06890922,0.05806518,-0.9959317,151.0337,0.004117129,0.9983128,0.05791914,-17.91524,0.9976144,-0.0001092116,-0.06903201,384.0914 +-0.08442122,0.06017112,-0.9946118,150.3495,0.00557517,0.9981879,0.05991427,-17.88945,0.9964145,-0.0004870887,-0.08460371,384.0333 +-0.1024382,0.06266042,-0.9927639,149.6934,0.007471485,0.9980343,0.06222214,-17.8643,0.9947113,-0.001043492,-0.102705,383.9659 +-0.1250101,0.06579974,-0.9899712,149.0594,0.01078838,0.9978295,0.06495975,-17.83684,0.9920968,-0.002559555,-0.1254486,383.8727 +-0.1549964,0.06839529,-0.9855447,148.4628,0.01623012,0.9976422,0.06668235,-17.81261,0.9877817,-0.005659976,-0.155741,383.7747 +-0.1918771,0.06989948,-0.9789266,147.9058,0.02018107,0.9975305,0.06727224,-17.77489,0.9812114,-0.006847782,-0.1928138,383.6702 +-0.2344818,0.07101521,-0.9695232,147.3608,0.02332577,0.997452,0.06741954,-17.73562,0.9718406,-0.006806216,-0.2355408,383.5007 +-0.283599,0.07147498,-0.9562756,146.8618,0.02565189,0.997427,0.06694329,-17.70318,0.9585998,-0.005545223,-0.2847027,383.3429 +-0.3376645,0.07181509,-0.938523,146.3906,0.02690063,0.9974142,0.06664303,-17.6722,0.940882,-0.002743869,-0.3387232,383.1653 +-0.3947221,0.0720766,-0.9159692,145.917,0.02922589,0.9973988,0.06588979,-17.64045,0.9183356,-0.0007618544,-0.3958018,382.8831 +-0.4543529,0.07272694,-0.8878481,145.493,0.03239254,0.9973515,0.06512003,-17.61047,0.8902326,0.0008278251,-0.4555053,382.6521 +-0.5146912,0.07616227,-0.8539861,145.0927,0.03957449,0.9970954,0.06507414,-17.58068,0.8564618,-0.0003029698,-0.5162103,382.4018 +-0.5751265,0.0794162,-0.8142006,144.6826,0.04349595,0.9968375,0.0665062,-17.55212,0.8169073,0.002835054,-0.5767619,382.019 +-0.6335659,0.08065268,-0.7694735,144.3429,0.0465134,0.9967234,0.06617398,-17.52752,0.7722893,0.006134748,-0.6352413,381.7103 +-0.6889992,0.07708669,-0.720651,144.0396,0.04520039,0.9969623,0.06342808,-17.50561,0.7233513,0.01112819,-0.6903905,381.3743 +-0.7404085,0.07664909,-0.6677726,143.7264,0.0475498,0.9969607,0.06171243,-17.47868,0.6704732,0.01393996,-0.7418027,380.9254 +-0.7866919,0.07879388,-0.6122969,143.476,0.05060039,0.9967141,0.06325049,-17.45602,0.6152687,0.01877618,-0.7880938,380.5488 +-0.8285889,0.08043202,-0.5540497,143.2571,0.052447,0.9964259,0.06621708,-17.4299,0.5573955,0.02580849,-0.8298458,380.1573 +-0.8661553,0.07840396,-0.4935868,143.0401,0.0504183,0.9962874,0.06978068,-17.40436,0.4972253,0.03555509,-0.8668925,379.6496 +-0.8980742,0.07334259,-0.4336862,142.8859,0.04702899,0.9963591,0.07111143,-17.38212,0.4373226,0.04346751,-0.8982535,379.215 +-0.9239148,0.07083455,-0.3759841,142.7496,0.04740688,0.9963339,0.07121305,-17.3626,0.37965,0.04797055,-0.9238856,378.76 +-0.9445736,0.06802167,-0.3211757,142.611,0.04831214,0.9964494,0.06895231,-17.34196,0.3247256,0.04961384,-0.944506,378.2309 +-0.9603305,0.06551412,-0.2710594,142.5129,0.04990083,0.9966947,0.06410516,-17.31765,0.2743633,0.04803604,-0.9604255,377.7433 +-0.972399,0.05945456,-0.2256222,142.4206,0.04759487,0.9972017,0.05764946,-17.2946,0.2284183,0.04531981,-0.9725076,377.205 +-0.9809463,0.05283752,-0.1869563,142.3618,0.04390286,0.9977016,0.05161496,-17.267,0.1892538,0.04242358,-0.9810113,376.6825 +-0.9870624,0.04211654,-0.1547067,142.3178,0.03490681,0.9981872,0.04902831,-17.24139,0.1564912,0.04299368,-0.9867431,376.1366 +-0.9911395,0.03683883,-0.1276147,142.2723,0.0306195,0.9982616,0.05035952,-17.21971,0.1292481,0.0460058,-0.9905444,375.5632 +-0.9939612,0.03711148,-0.1032664,142.2304,0.03178696,0.9981023,0.05273794,-17.1996,0.1050276,0.04913693,-0.9932546,374.9889 +-0.9957754,0.04117005,-0.08207624,142.1881,0.03664793,0.9977656,0.05586228,-17.17908,0.0841927,0.05261835,-0.9950592,374.3958 +-0.9970623,0.04171859,-0.06423749,142.1618,0.03809798,0.9976702,0.05659243,-17.1552,0.06644878,0.05397885,-0.9963286,373.7981 +-0.9980184,0.03879603,-0.04953997,142.1497,0.03612157,0.9978989,0.05378563,-17.12791,0.05152255,0.05188958,-0.9973228,373.1902 +-0.9987804,0.03322492,-0.03652141,142.1472,0.03137437,0.99825,0.05012624,-17.10188,0.03812293,0.04891926,-0.9980749,372.5674 +-0.9990586,0.0341451,-0.02675855,142.1404,0.03283013,0.9983014,0.04813004,-17.07609,0.0283565,0.04720624,-0.9984825,371.9316 +-0.9990684,0.03897403,-0.01853204,142.1298,0.03806706,0.998169,0.04700448,-17.04792,0.02033006,0.04625523,-0.9987227,371.2791 +-0.9990835,0.04094679,-0.01247046,142.1229,0.04033068,0.9981181,0.04619161,-17.02105,0.01433839,0.04564633,-0.9988547,370.6074 +-0.9991302,0.04057308,-0.009629084,142.121,0.0400897,0.9981378,0.04597569,-16.99702,0.01147653,0.04554967,-0.9988961,369.9188 +-0.9991373,0.04078289,-0.007843902,142.119,0.0403923,0.99818,0.04477732,-16.97384,0.009655775,0.04442185,-0.9989661,369.215 +-0.999082,0.04267866,-0.003706634,142.1176,0.04248378,0.9981987,0.04236097,-16.94631,0.005507866,0.0421646,-0.9990954,368.4949 +-0.9988172,0.04860028,0.001523622,142.1154,0.04862216,0.9980005,0.040384,-16.91758,0.0004420983,0.04041031,-0.999183,367.7599 +-0.998541,0.05350619,0.007281598,142.1175,0.05374853,0.9978073,0.03862152,-16.89146,-0.005199141,0.03895654,-0.9992273,367.0083 +-0.9984622,0.0537516,0.01356482,142.1308,0.05425099,0.9977412,0.03961442,-16.86816,-0.01140484,0.0402894,-0.9991229,366.2338 +-0.9984225,0.05272142,0.01931344,142.1534,0.05344621,0.9978025,0.0391605,-16.84168,-0.0172064,0.04013095,-0.9990462,365.4425 +-0.998255,0.05320278,0.02562047,142.1767,0.05414936,0.9978175,0.03778964,-16.81628,-0.02355404,0.03911102,-0.9989572,364.6373 +-0.9981093,0.05319974,0.03078548,142.2044,0.05430455,0.9978666,0.0362384,-16.79386,-0.02879192,0.03784167,-0.9988688,363.8146 +-0.9980922,0.0516026,0.03390026,142.2381,0.05274427,0.9980395,0.03369294,-16.77059,-0.03209515,0.0354167,-0.9988571,362.9724 +-0.997837,0.05422086,0.03716736,142.2697,0.05543515,0.9979346,0.03245748,-16.74593,-0.03533072,0.03444765,-0.9987818,362.1104 +-0.9974582,0.05880986,0.04023093,142.2994,0.0602036,0.9975947,0.03435555,-16.72269,-0.03811372,0.03669026,-0.9985995,361.231 +-0.9972308,0.06042218,0.04335908,142.3344,0.06205338,0.997375,0.03731518,-16.69936,-0.04099059,0.03990242,-0.9983624,360.3289 +-0.9969343,0.06319583,0.04613443,142.373,0.06506856,0.9970672,0.04028572,-16.67234,-0.04345324,0.04316411,-0.9981225,359.4117 +-0.9966817,0.06480243,0.04925711,142.4153,0.06686146,0.9969042,0.04136986,-16.64202,-0.04642375,0.04452598,-0.9979289,358.4747 +-0.9967881,0.06103555,0.05184904,142.4687,0.06326189,0.9970941,0.0424402,-16.61061,-0.04910801,0.04558394,-0.9977527,357.5224 +-0.9971242,0.05360876,0.05356707,142.5268,0.05605,0.9974061,0.04516013,-16.57859,-0.05100714,0.04803268,-0.9975425,356.5497 +-0.9974289,0.04669012,0.05436671,142.5825,0.04942104,0.9975247,0.05001972,-16.54547,-0.05189671,0.05257796,-0.9972674,355.5581 +-0.9976584,0.04168216,0.05422592,142.6387,0.04444365,0.9977216,0.05075741,-16.51359,-0.05198669,0.05304854,-0.9972377,354.5569 +-0.99765,0.04283267,0.05347861,142.6925,0.04529635,0.9979259,0.04573903,-16.47653,-0.05140856,0.04805392,-0.9975209,353.5517 +-0.9975958,0.04449517,0.05313015,142.7397,0.04664238,0.9981152,0.03988178,-16.44204,-0.05125546,0.04226401,-0.9977908,352.539 +-0.9974678,0.04739939,0.05302275,142.7868,0.0495857,0.99794,0.04070655,-16.41262,-0.05098406,0.04323264,-0.9977632,351.5111 +-0.997331,0.05022115,0.05299839,142.8356,0.05252472,0.9976932,0.04300545,-16.38823,-0.05071635,0.04567438,-0.9976681,350.477 +-0.997477,0.04772385,0.05255534,142.8919,0.04999038,0.9978369,0.0426908,-16.36326,-0.05040428,0.04521035,-0.997705,349.4391 +-0.997657,0.04429927,0.05213603,142.9521,0.04648533,0.9980569,0.04149164,-16.33502,-0.05019667,0.04381797,-0.9977776,348.3931 +-0.9977335,0.04252542,0.05214934,143.0093,0.0448131,0.9980473,0.0435124,-16.30742,-0.05019712,0.04575074,-0.9976908,347.339 +-0.9977048,0.04257431,0.05265581,143.0645,0.04485152,0.9980743,0.04284871,-16.28715,-0.05073015,0.04511205,-0.997693,346.2813 +-0.997524,0.04574041,0.05342095,143.1174,0.04789071,0.9980637,0.03968996,-16.26189,-0.05150207,0.04215005,-0.9977829,345.2218 +-0.9972545,0.04971784,0.05487889,143.1694,0.05176598,0.9979901,0.03655199,-16.23887,-0.0529513,0.03929249,-0.9978237,344.1583 +-0.9970676,0.05108126,0.05698185,143.2271,0.05322496,0.9979057,0.03675875,-16.21907,-0.05498483,0.03968381,-0.9976982,343.091 +-0.9969347,0.05142548,0.05896431,143.2881,0.05353115,0.9979629,0.03470435,-16.19864,-0.0570595,0.03775439,-0.9976566,342.0248 +-0.997012,0.0483622,0.06023454,143.3564,0.05075966,0.9979519,0.03892832,-16.17981,-0.05822851,0.04186948,-0.9974248,340.9554 +-0.997126,0.04566944,0.06044861,143.4252,0.04831286,0.9979056,0.04301507,-16.15917,-0.05835752,0.04581188,-0.997244,339.8874 +-0.997231,0.04395958,0.05998365,143.492,0.04643104,0.9981022,0.0404494,-16.13849,-0.05809167,0.04312249,-0.9973794,338.8256 +-0.9972362,0.04443067,0.05954854,143.5561,0.04665539,0.9982438,0.03650448,-16.11398,-0.05782204,0.03918184,-0.9975577,337.7636 +-0.9970566,0.04845509,0.05941607,143.6162,0.05064013,0.9980738,0.03583712,-16.0925,-0.05756512,0.03874047,-0.9975898,336.7005 +-0.9968703,0.05273534,0.05889473,143.6725,0.05527321,0.9975737,0.04232658,-16.06795,-0.05651972,0.04544941,-0.9973664,335.6274 +-0.9967986,0.05471163,0.05830304,143.7328,0.05754573,0.9971838,0.04809244,-16.04233,-0.05550763,0.05129356,-0.9971398,334.5637 +-0.9969431,0.0529014,0.0574984,143.7959,0.05575877,0.9972273,0.04928093,-16.01395,-0.05473195,0.05233631,-0.9971285,333.5022 +-0.9970425,0.0520023,0.05658698,143.8639,0.05460826,0.9974696,0.04552332,-15.97726,-0.05407647,0.04847879,-0.9973592,332.4462 +-0.9973379,0.04715442,0.05562136,143.9354,0.04951173,0.9978987,0.04179274,-15.94901,-0.05353377,0.04443538,-0.9975768,331.3938 +-0.9975702,0.04350404,0.05441656,144.0057,0.04578119,0.9980964,0.04132395,-15.91901,-0.05251521,0.04371479,-0.9976628,330.3375 +-0.9976186,0.04304609,0.05389049,144.0831,0.04550321,0.9979397,0.0452294,-15.88074,-0.05183251,0.04757387,-0.9975219,329.2695 +-0.997814,0.03908743,0.05328666,144.158,0.04180323,0.9978315,0.05084122,-15.84408,-0.05118385,0.05295763,-0.9972841,328.1979 +-0.9977903,0.03988104,0.0531429,144.23,0.04242563,0.9979628,0.04764649,-15.80246,-0.05113445,0.04979581,-0.9974495,327.1306 +-0.9976903,0.04237357,0.05309071,144.2916,0.04468789,0.9980669,0.04319016,-15.76302,-0.05115796,0.04546291,-0.9976552,326.0621 +-0.9976149,0.04480946,0.05250397,144.3566,0.04703222,0.9980145,0.04189287,-15.72423,-0.05052253,0.04426232,-0.9977416,324.9835 +-0.9976297,0.04491572,0.0521302,144.4191,0.04735148,0.9977968,0.04646966,-15.68803,-0.04992812,0.04882795,-0.9975585,323.8962 +-0.9975218,0.04719475,0.05218198,144.4784,0.04986573,0.9974464,0.05112689,-15.65682,-0.0496358,0.05360227,-0.9973279,322.7954 +-0.9973867,0.04981123,0.05233206,144.534,0.05254256,0.9972544,0.05218169,-15.62183,-0.04958914,0.05479498,-0.9972654,321.6953 +-0.9971543,0.0546315,0.0519499,144.5874,0.05719458,0.9971499,0.04920144,-15.58365,-0.04911389,0.05203267,-0.9974369,320.5911 +-0.997009,0.05738071,0.05177416,144.6423,0.05998186,0.9969379,0.05016843,-15.54565,-0.04873692,0.05312387,-0.9973978,319.4803 +-0.9968944,0.0595053,0.05158218,144.698,0.06218152,0.9967129,0.05193062,-15.50891,-0.04832248,0.05497679,-0.9973176,318.3644 +-0.9967067,0.06231814,0.05188711,144.7546,0.06498904,0.9965569,0.05148528,-15.47325,-0.04849998,0.05468781,-0.9973249,317.239 +-0.9965441,0.06440538,0.05245918,144.8153,0.06694527,0.9965922,0.04818965,-15.43975,-0.04917674,0.05153499,-0.9974596,316.11 +-0.9964063,0.0662415,0.05278917,144.8789,0.06859479,0.99667,0.04408752,-15.40967,-0.04969295,0.04755013,-0.9976319,314.9702 +-0.9963675,0.06643559,0.05327514,144.9419,0.06871397,0.996746,0.04213863,-15.3796,-0.05030228,0.0456463,-0.9976903,313.833 +-0.996585,0.06286795,0.05353543,145.0151,0.06520405,0.9969421,0.04306775,-15.34786,-0.05066414,0.04641139,-0.9976367,312.6776 +-0.9965389,0.06326003,0.05392991,145.0817,0.06583811,0.9967016,0.0474477,-15.31182,-0.05075048,0.05083412,-0.9974167,311.522 +-0.996659,0.06113092,0.05416605,145.1573,0.06390524,0.9966489,0.0510587,-15.27397,-0.05086327,0.05434959,-0.9972256,310.363 +-0.996695,0.06124587,0.05336671,145.2305,0.06417251,0.9964237,0.05496986,-15.23033,-0.04980918,0.05821285,-0.9970608,309.1981 +-0.9966958,0.06184154,0.05266048,145.3018,0.06483352,0.9962587,0.0571416,-15.18478,-0.04892973,0.06036695,-0.9969762,308.0316 +-0.996695,0.06291087,0.05139523,145.3685,0.06568698,0.996365,0.05423993,-15.13971,-0.04779612,0.05743665,-0.9972043,306.8611 +-0.9967151,0.06329589,0.05052448,145.4357,0.06579484,0.996608,0.04943135,-15.09515,-0.04722429,0.05259322,-0.9974987,305.689 +-0.9967026,0.06377482,0.05016708,145.5038,0.06623722,0.9965973,0.04905557,-15.05207,-0.04686786,0.05221673,-0.9975353,304.508 +-0.9967273,0.06342318,0.05012342,145.5698,0.06596921,0.9965232,0.05088695,-15.01009,-0.04672174,0.054027,-0.9974458,303.3221 +-0.9967002,0.06355282,0.0504962,145.6403,0.06596026,0.9966895,0.04753154,-14.96594,-0.04730826,0.05070543,-0.9975925,302.1453 +-0.9966055,0.06452388,0.05112911,145.7082,0.06674149,0.9968467,0.04292092,-14.9258,-0.04819846,0.04618765,-0.9977693,300.964 +-0.9965299,0.06497408,0.05202599,145.7796,0.0671683,0.9968741,0.04159884,-14.89101,-0.04916051,0.04494897,-0.9977789,299.7815 +-0.9965134,0.06469795,0.05268155,145.8491,0.0670116,0.9968077,0.04340272,-14.86174,-0.0497053,0.04678166,-0.9976677,298.5951 +-0.996505,0.06359091,0.05416726,145.9238,0.06619345,0.9966665,0.04768835,-14.82799,-0.05095414,0.05110719,-0.9973924,297.4085 +-0.9963939,0.06367854,0.05607412,145.9958,0.06659343,0.9964375,0.05174562,-14.79097,-0.05257926,0.05529318,-0.9970847,296.2243 +-0.9965093,0.06059881,0.05742039,146.0741,0.06355253,0.9966698,0.05109085,-14.75456,-0.05413312,0.05456171,-0.9970419,295.0464 +-0.9965307,0.05993589,0.05774336,146.1525,0.06243156,0.9971469,0.04243021,-14.72282,-0.05503552,0.04588801,-0.9974293,293.8828 +-0.9966361,0.05745562,0.05844116,146.2328,0.05941308,0.9977099,0.03232588,-14.69749,-0.05645002,0.0356893,-0.9977673,292.7237 +-0.9965802,0.05709499,0.05973391,146.3117,0.05905055,0.9977579,0.03149999,-14.67747,-0.05780149,0.03491958,-0.9977171,291.5585 +-0.9965327,0.05605135,0.06148974,146.3904,0.05843432,0.9975805,0.03766413,-14.65898,-0.05922984,0.04112664,-0.9973968,290.3897 +-0.9963804,0.05779174,0.06234031,146.4665,0.06061266,0.9971756,0.04434907,-14.63779,-0.05960122,0.04796715,-0.9970691,289.2215 +-0.9964076,0.05653257,0.06305658,146.5443,0.05969151,0.9969942,0.04939071,-14.60953,-0.06007486,0.05297721,-0.996787,288.0564 +-0.9964254,0.0563206,0.06296503,146.6136,0.05971175,0.9967895,0.05333923,-14.57882,-0.05975878,0.05690831,-0.9965893,286.9004 +-0.9964636,0.05607361,0.06257888,146.6852,0.05933524,0.9969067,0.05153861,-14.54396,-0.05949535,0.05506948,-0.9967084,285.746 +-0.9965433,0.05594378,0.06141504,146.7555,0.05912777,0.9969308,0.05131128,-14.50797,-0.05835599,0.05476524,-0.9967925,284.5909 +-0.9966006,0.05601757,0.06041022,146.8261,0.05918875,0.996889,0.05204781,-14.47298,-0.05730669,0.05544647,-0.9968157,283.4352 +-0.9966144,0.05673054,0.05951021,146.8938,0.05960882,0.9970786,0.04775946,-14.44182,-0.05662693,0.05114509,-0.9970845,282.2812 +-0.9966628,0.05672039,0.05870337,146.9629,0.05896488,0.9975654,0.03723443,-14.41352,-0.0564485,0.0405716,-0.9975808,281.1377 +-0.9967791,0.05539459,0.05799035,147.0331,0.05744906,0.9977563,0.03438005,-14.39034,-0.05595576,0.0376008,-0.9977249,279.9936 +-0.9969809,0.05229671,0.0573961,147.1037,0.05461178,0.9977247,0.0395351,-14.37177,-0.05519794,0.04255023,-0.9975683,278.8341 +-0.9973702,0.04506059,0.05676611,147.1766,0.04779857,0.9977109,0.0478351,-14.34568,-0.05448068,0.05042263,-0.9972408,277.6828 +-0.9975915,0.0411728,0.05582136,147.2446,0.04436242,0.997379,0.05715866,-14.3102,-0.05332167,0.05949736,-0.9968033,276.5326 +-0.9976208,0.04213649,0.05456501,147.3092,0.04562743,0.9968807,0.06439672,-14.2694,-0.05168135,0.06673316,-0.9964314,275.38 +-0.9977436,0.0406597,0.05342881,147.372,0.04410506,0.9969119,0.06497217,-14.22198,-0.05062206,0.06718203,-0.9964557,274.2471 +-0.9974675,0.0473456,0.05307655,147.4227,0.05028764,0.9971894,0.05553765,-14.18018,-0.0502979,0.05806609,-0.9970448,273.1258 +-0.9969635,0.05736389,0.05266196,147.469,0.05946498,0.997459,0.03923646,-14.14342,-0.05027738,0.04224885,-0.9978412,272.0149 +-0.9968236,0.06019001,0.05215326,147.5236,0.06182995,0.9976229,0.03042193,-14.1179,-0.05019819,0.03354992,-0.9981756,270.8995 +-0.9967923,0.06014206,0.05280181,147.5825,0.06216242,0.9973616,0.03749165,-14.09516,-0.05040767,0.04065367,-0.9979009,269.7702 +-0.9967756,0.05966704,0.05364978,147.6441,0.06239278,0.9967654,0.05065335,-14.06534,-0.05045391,0.05383738,-0.9972742,268.6405 +-0.9968689,0.05786643,0.05388797,147.7037,0.06105618,0.9963559,0.0595576,-14.03193,-0.05024521,0.0626613,-0.9967692,267.5141 +-0.9969959,0.0552962,0.05423675,147.765,0.05883162,0.9960895,0.06591288,-13.98885,-0.05037992,0.06890569,-0.9963502,266.3995 +-0.9971343,0.05231074,0.05465125,147.828,0.05614437,0.9958848,0.07114174,-13.94239,-0.05070487,0.07400622,-0.9959679,265.2874 +-0.9972286,0.05007767,0.05502217,147.8896,0.05384264,0.9961473,0.06922075,-13.89624,-0.05134377,0.07199144,-0.9960828,264.1887 +-0.9973673,0.04751679,0.05477942,147.9547,0.05086547,0.9968132,0.06144961,-13.84906,-0.05168496,0.0640742,-0.9966058,263.099 +-0.9974256,0.04740774,0.05380381,148.0187,0.05028617,0.9973025,0.05346888,-13.81143,-0.05112383,0.0560368,-0.9971189,262.0113 +-0.9975043,0.04732967,0.05239422,148.0798,0.04982668,0.9976313,0.04742417,-13.78074,-0.05002554,0.04991643,-0.9974997,260.9277 +-0.9976065,0.04793021,0.0498397,148.1364,0.04999601,0.9979049,0.04106251,-13.7558,-0.04776714,0.04345601,-0.9979127,259.8532 +-0.997656,0.04914232,0.04761955,148.1916,0.05107985,0.9978786,0.0403622,-13.73319,-0.04553504,0.04269998,-0.9980497,258.7765 +-0.9976881,0.05009925,0.04591878,148.2468,0.05213497,0.9976586,0.04426243,-13.71001,-0.04359375,0.04655407,-0.997964,257.7052 +-0.9978047,0.04841334,0.04518941,148.3039,0.05087741,0.9971853,0.05507121,-13.68168,-0.04239604,0.05724942,-0.9974593,256.6315 +-0.9979353,0.04506105,0.04576769,148.3625,0.04796803,0.9967616,0.06454014,-13.64657,-0.04271123,0.06660226,-0.996865,255.568 +-0.9981243,0.04017393,0.04619457,148.4218,0.04328883,0.9967091,0.068534,-13.60622,-0.04328927,0.07040515,-0.9965787,254.5244 +-0.9983077,0.0348128,0.04658112,148.4836,0.03783439,0.9971258,0.06564055,-13.56624,-0.0441621,0.06729182,-0.9967554,253.4954 +-0.9983906,0.03141424,0.04721714,148.5427,0.03415315,0.9977109,0.05836546,-13.52881,-0.04527554,0.05988412,-0.997178,252.4848 +-0.9981956,0.03467337,0.0490247,148.5947,0.03706388,0.9981244,0.04872347,-13.49617,-0.04724333,0.05045259,-0.9976084,251.4866 +-0.9979971,0.03751763,0.05093455,148.6459,0.0397301,0.9982786,0.04314277,-13.4719,-0.04922826,0.04507999,-0.9977696,250.5055 +-0.9977628,0.04127888,0.05258835,148.6994,0.04351657,0.9981635,0.04214108,-13.44786,-0.05075223,0.04433526,-0.9977267,249.5301 +-0.9974933,0.04487808,0.05470897,148.7515,0.047509,0.9977275,0.0477765,-13.42428,-0.05244053,0.0502559,-0.9973586,248.5643 +-0.99727,0.04747619,0.05655723,148.8029,0.0504607,0.9973418,0.05256514,-13.4025,-0.05391129,0.05527554,-0.9970146,247.6198 +-0.9970904,0.04954793,0.05793025,148.8551,0.05254347,0.9972958,0.05138284,-13.37998,-0.05522768,0.05427718,-0.9969974,246.697 +-0.9969469,0.0509287,0.05918758,148.9068,0.05359514,0.9975764,0.04437125,-13.36041,-0.05678436,0.04740794,-0.9972602,245.8032 +-0.9968231,0.05244402,0.05994584,148.9571,0.05489394,0.9976915,0.03997899,-13.34445,-0.0577108,0.04314264,-0.9974007,244.924 +-0.9969347,0.04854659,0.06135549,149.012,0.05099347,0.9979386,0.03896359,-13.33483,-0.05933746,0.04197288,-0.9973551,244.0596 +-0.9970477,0.04495153,0.06225256,149.0714,0.04751575,0.9980555,0.04034113,-13.32729,-0.06031811,0.04318,-0.9972448,243.2071 +-0.9969315,0.04573057,0.06353319,149.1209,0.0483059,0.998047,0.03960767,-13.31957,-0.06159782,0.04255515,-0.9971934,242.3762 +-0.997006,0.04157336,0.06519764,149.1775,0.04432682,0.9981601,0.04137,-13.31184,-0.06335779,0.04413614,-0.9970144,241.5653 +-0.9972074,0.03378707,0.06660237,149.2399,0.03691189,0.9982473,0.04625879,-13.30447,-0.06492268,0.04858801,-0.9967066,240.7729 +-0.9971622,0.03019194,0.06896484,149.3001,0.03396521,0.9979516,0.05421186,-13.29094,-0.06718681,0.05640041,-0.996145,239.9943 +-0.9969595,0.03018927,0.07183691,149.3541,0.03441195,0.9977065,0.05828862,-13.28784,-0.06991245,0.06058343,-0.9957117,239.2408 +-0.9967759,0.03103597,0.07399152,149.4097,0.03523568,0.9978006,0.05614626,-13.27893,-0.07208623,0.05857237,-0.995677,238.5162 +-0.9966709,0.02945147,0.07602528,149.468,0.03343137,0.9981071,0.05161882,-13.26869,-0.07436112,0.0539886,-0.9957688,237.8162 +-0.9963671,0.03231927,0.07879159,149.5185,0.03609914,0.9982409,0.04702997,-13.25514,-0.07713301,0.04970342,-0.9957811,237.1411 +-0.9961645,0.03074368,0.08192249,149.573,0.03483525,0.9981915,0.04899201,-13.24552,-0.08026813,0.05165788,-0.9954338,236.4857 +-0.9961317,0.0293124,0.08284031,149.6281,0.03394049,0.9979081,0.05502273,-13.23521,-0.08105416,0.05762151,-0.9950426,235.8495 +-0.996339,0.02211007,0.08258276,149.6884,0.02701933,0.9979035,0.0588099,-13.2247,-0.08110932,0.06082592,-0.9948474,235.2411 +-0.9964933,0.0192928,0.0814179,149.7405,0.02425824,0.9978767,0.06044521,-13.21549,-0.08007886,0.06220829,-0.9948454,234.6539 +-0.9967606,0.01634816,0.07874742,149.7885,0.02123709,0.9978717,0.06165165,-13.20388,-0.07757193,0.06312429,-0.9949863,234.0855 +-0.9972882,0.0140275,0.07224609,149.8316,0.01844377,0.9979778,0.06082828,-13.19032,-0.07124672,0.06199581,-0.9955302,233.5367 +-0.9978438,0.01292017,0.06434999,149.8633,0.01664537,0.9981955,0.05769399,-13.17359,-0.06348845,0.05864071,-0.9962582,233.0076 +-0.9984998,0.009958047,0.05384363,149.8883,0.01290405,0.9984224,0.05464604,-13.16085,-0.05321451,0.05525885,-0.9970529,232.499 +-0.9992366,0.00690463,0.03845262,149.9063,0.008985502,0.9984892,0.05420786,-13.14923,-0.03802024,0.05451198,-0.997789,232.0049 +-0.9997985,0.005562976,0.01928935,149.9055,0.006624025,0.998443,0.0553863,-13.13525,-0.0189512,0.05550291,-0.9982786,231.5225 +-0.9999392,0.01007575,-0.004485011,149.8867,0.009803723,0.9983247,0.05702365,-13.11875,0.005052053,0.0569762,-0.9983627,231.0661 +-0.9993515,0.01740077,-0.03152462,149.8387,0.01558353,0.9982524,0.05700155,-13.10236,0.03246139,0.05647331,-0.9978762,230.6199 +-0.9977709,0.01963561,-0.06377968,149.7894,0.01611074,0.9983388,0.05531831,-13.0907,0.06475993,0.05416745,-0.9964296,230.1941 +-0.9946854,0.02007931,-0.1009847,149.7286,0.01455531,0.9983723,0.05514383,-13.08007,0.1019275,0.05338089,-0.9933585,229.7738 +-0.99,0.02174227,-0.1393823,149.6351,0.01404675,0.9983342,0.0559597,-13.06755,0.1403668,0.05344222,-0.9886562,229.3441 +-0.9834666,0.02869085,-0.1788028,149.5269,0.01887004,0.9982307,0.05638643,-13.05107,0.1801042,0.05208015,-0.9822678,228.9406 +-0.9750411,0.03623072,-0.2190484,149.399,0.02472488,0.9981778,0.05504233,-13.03179,0.2206435,0.04825258,-0.9741602,228.5521 +-0.9644037,0.04384489,-0.2607741,149.2413,0.03073348,0.9980598,0.05414784,-13.01446,0.2626422,0.04420588,-0.9638801,228.1502 +-0.9517688,0.04767227,-0.3030901,149.0871,0.03259296,0.9979751,0.05462003,-12.99856,0.3050802,0.04210703,-0.9513953,227.7785 +-0.9374479,0.04945788,-0.3445945,148.9062,0.03165101,0.9978658,0.057114,-12.98572,0.3466838,0.04263463,-0.9370126,227.3924 +-0.9207692,0.05488001,-0.3862284,148.7161,0.03392309,0.9975688,0.06087402,-12.96801,0.3886301,0.04294886,-0.9203923,227.0396 +-0.9008653,0.0583038,-0.4301656,148.5087,0.03388085,0.99736,0.06422601,-12.94524,0.4327746,0.0432846,-0.9004624,226.6979 +-0.8767842,0.06107394,-0.4769901,148.257,0.03415315,0.9973061,0.06491641,-12.91843,0.4796698,0.04062696,-0.876508,226.3348 +-0.849595,0.06333934,-0.5236188,148.0007,0.03293597,0.9971967,0.06718545,-12.89513,0.5264064,0.03983453,-0.8492994,226.0082 +-0.8197696,0.06595349,-0.5688831,147.6976,0.0310364,0.997003,0.07086367,-12.86996,0.5718518,0.0404358,-0.8193597,225.6569 +-0.7865526,0.06632064,-0.6139516,147.3972,0.02656416,0.9969296,0.07365876,-12.84664,0.6169516,0.04162737,-0.7858993,225.3474 +-0.750051,0.06608918,-0.6580698,147.0757,0.02207522,0.9969421,0.074961,-12.82167,0.6610115,0.04169753,-0.7492163,225.0489 +-0.7107173,0.06257634,-0.700689,146.7034,0.01572215,0.9972,0.07310965,-12.78496,0.703302,0.04094395,-0.7097111,224.7302 +-0.6689842,0.05852419,-0.740969,146.3321,0.01113545,0.9975726,0.06873797,-12.75356,0.7431932,0.03773359,-0.668012,224.4633 +-0.6261037,0.05459629,-0.7778261,145.9376,0.007592093,0.9979252,0.06393407,-12.72341,0.7797028,0.03412403,-0.6252192,224.2173 +-0.5821793,0.05106777,-0.8114551,145.5026,0.004619519,0.9982172,0.05950711,-12.69426,0.8130473,0.03089528,-0.5813773,223.949 +-0.5373361,0.04927613,-0.8419275,145.0547,0.001473802,0.998345,0.0574903,-12.66419,0.8433669,0.02965078,-0.5365194,223.7319 +-0.4915254,0.04898643,-0.8694844,144.5644,-0.003349706,0.9983029,0.05813764,-12.63282,0.8708568,0.03148864,-0.4905271,223.4964 +-0.4471052,0.05054651,-0.8930521,144.068,-0.007381395,0.9981596,0.06019107,-12.59687,0.8944509,0.03350371,-0.4459092,223.3106 +-0.402971,0.05246913,-0.9137075,143.5569,-0.007943351,0.9981171,0.06081956,-12.5613,0.9151782,0.03176642,-0.4017954,223.1391 +-0.3592103,0.05477865,-0.9316476,143.0108,-0.007139969,0.9980853,0.06143797,-12.52742,0.9332293,0.02872109,-0.3581314,222.9639 +-0.316611,0.0568535,-0.9468502,142.4641,-0.009976834,0.9979473,0.06325773,-12.49939,0.948503,0.02947466,-0.3153938,222.8219 +-0.2777328,0.05453138,-0.9591094,141.9041,-0.0155069,0.998003,0.06123313,-12.47404,0.9605332,0.03187926,-0.2763325,222.6903 +-0.2424652,0.04970396,-0.9688861,141.3166,-0.01741847,0.9983027,0.05557205,-12.44205,0.9700037,0.03035081,-0.2411879,222.5687 +-0.211162,0.04587248,-0.9763741,140.7198,-0.01538501,0.9986184,0.05024492,-12.40933,0.97733,0.02563134,-0.2101645,222.4776 +-0.1836433,0.04418026,-0.9819997,140.0954,-0.01037802,0.9988466,0.046879,-12.37499,0.9829381,0.01880022,-0.1829729,222.406 +-0.1594463,0.04454988,-0.9862009,139.4545,-0.005869738,0.9989208,0.04607349,-12.3457,0.9871891,0.01313499,-0.1590127,222.3345 +-0.1382061,0.04656475,-0.9893083,138.7945,-0.003038923,0.9988695,0.04743932,-12.32055,0.9903988,0.009562837,-0.1379083,222.2751 +-0.1203148,0.04608385,-0.9916656,138.1225,-0.003988552,0.9988914,0.04690357,-12.29664,0.9927277,0.009598506,-0.1199976,222.2177 +-0.1047,0.04649176,-0.9934166,137.425,-0.002628048,0.9988902,0.04702492,-12.26601,0.9945003,0.007534257,-0.1044616,222.1663 +-0.0924802,0.04787058,-0.9945632,136.7075,-0.001143274,0.9988379,0.04818265,-12.23195,0.9957138,0.005593003,-0.09231799,222.1224 +-0.08272282,0.05072124,-0.995281,135.9678,0.003499814,0.9987126,0.05060524,-12.19954,0.9965664,0.0007029147,-0.08279383,222.0857 +-0.07470635,0.05286556,-0.9958033,135.2055,0.004858752,0.9986012,0.0526496,-12.16821,0.9971937,-0.0009050969,-0.07485871,222.0501 +-0.06727714,0.05321467,-0.9963142,134.423,0.001869365,0.9985816,0.05320955,-12.12915,0.9977325,0.001717317,-0.06728119,222.0102 +-0.05945123,0.04838748,-0.9970578,133.621,0.003524091,0.9988284,0.04826329,-12.09125,0.9982249,-0.0006444052,-0.0595521,221.9811 +-0.05207225,0.04468869,-0.997643,132.804,0.007857689,0.9989856,0.04433871,-12.0594,0.9986124,-0.005530345,-0.05237058,221.9615 +-0.04513715,0.03604204,-0.9983304,131.9851,0.0102219,0.9993133,0.03561537,-12.0348,0.9989285,-0.008597255,-0.04547457,221.945 +-0.04072102,0.03145417,-0.9986754,131.1365,0.01319421,0.9994341,0.03094008,-12.01166,0.9990834,-0.01191682,-0.04111298,221.9299 +-0.03855449,0.02910157,-0.9988327,130.2674,0.01791719,0.9994352,0.02842754,-11.99076,0.9990958,-0.01680026,-0.03905413,221.9193 +-0.03706989,0.03242543,-0.9987865,129.3683,0.01781927,0.9993359,0.03178192,-11.97037,0.9991537,-0.01661948,-0.03762307,221.9012 +-0.03606513,0.03897002,-0.9985894,128.4498,0.01412935,0.9991594,0.03848197,-11.94131,0.9992495,-0.01272155,-0.03658543,221.8912 +-0.03630774,0.0487244,-0.9981522,127.5087,0.01211792,0.9987587,0.04831323,-11.90638,0.9992671,-0.01034137,-0.03685311,221.8825 +-0.03661016,0.05594379,-0.9977625,126.5508,0.009138398,0.9984088,0.05564472,-11.86811,0.9992878,-0.007080782,-0.03706314,221.8697 +-0.0358419,0.05446718,-0.9978721,125.5892,0.008253717,0.9984957,0.05420477,-11.82662,0.9993233,-0.006293346,-0.03623753,221.8603 +-0.03589512,0.04634322,-0.9982805,124.6227,0.01158736,0.9988763,0.04595424,-11.78537,0.9992883,-0.009917897,-0.03639178,221.8543 +-0.03623061,0.03829041,-0.9986097,123.649,0.01375279,0.9991901,0.03781371,-11.7492,0.9992488,-0.01236365,-0.03672787,221.85 +-0.03610271,0.03831781,-0.9986132,122.6588,0.01621798,0.9991555,0.0377523,-11.72068,0.9992164,-0.01483252,-0.03669365,221.8393 +-0.03629137,0.0419676,-0.9984597,121.664,0.01771186,0.9989879,0.04134603,-11.69407,0.9991842,-0.01618407,-0.03699796,221.8301 +-0.0375145,0.04446748,-0.9983062,120.6638,0.01790666,0.9988789,0.0438201,-11.66561,0.9991356,-0.01623243,-0.03826871,221.8134 +-0.03983994,0.04942416,-0.997983,119.6649,0.01912022,0.9986307,0.04869296,-11.63207,0.9990231,-0.01714172,-0.04073039,221.7898 +-0.04323786,0.05591337,-0.997499,118.6572,0.02023234,0.9982769,0.05507999,-11.59609,0.9988599,-0.01780019,-0.04429462,221.7633 +-0.04665301,0.06040301,-0.9970833,117.6466,0.01803364,0.9980583,0.05961831,-11.55591,0.9987483,-0.01519966,-0.04765171,221.737 +-0.04985033,0.05433326,-0.9972777,116.6441,0.01684269,0.9984229,0.05355376,-11.51182,0.9986146,-0.01412716,-0.05068683,221.7117 +-0.05346584,0.04695488,-0.9974651,115.6416,0.01797375,0.9987773,0.04605323,-11.47092,0.9984079,-0.0154659,-0.05424441,221.6855 +-0.05657767,0.04495196,-0.9973858,114.6347,0.01792275,0.9988706,0.04400221,-11.43302,0.9982373,-0.01538634,-0.05731943,221.657 +-0.05869745,0.0495222,-0.9970467,113.6256,0.02036836,0.9986202,0.04840126,-11.39936,0.998068,-0.01746716,-0.05962514,221.6252 +-0.06169446,0.05597506,-0.9965243,112.6273,0.02069333,0.9982833,0.05479276,-11.36389,0.9978805,-0.01724099,-0.06274686,221.5703 +-0.06381096,0.06135437,-0.9960742,111.6312,0.02072597,0.9979745,0.06014367,-11.32789,0.9977467,-0.01680678,-0.06495333,221.5138 +-0.06549687,0.05867096,-0.9961265,110.6494,0.02068259,0.9981353,0.05742938,-11.29377,0.9976384,-0.01684102,-0.0665882,221.4332 +-0.06624125,0.05314676,-0.9963873,109.6735,0.02032809,0.9984451,0.0519051,-11.25746,0.9975965,-0.01681638,-0.06721861,221.3561 +-0.06641993,0.04790412,-0.9966412,108.702,0.02110403,0.9986908,0.0465962,-11.2272,0.9975685,-0.01793822,-0.06734395,221.2816 +-0.06706818,0.04245263,-0.9968449,107.7265,0.02305029,0.9988937,0.04098906,-11.19992,0.9974821,-0.02022849,-0.06797252,221.2255 +-0.06665191,0.03689317,-0.997094,106.7549,0.02371972,0.9990923,0.03538155,-11.17585,0.9974943,-0.02129253,-0.0674665,221.1665 +-0.06548397,0.04189993,-0.9969736,105.7727,0.02486684,0.9988762,0.04034658,-11.15208,0.9975437,-0.02214952,-0.06645229,221.0968 +-0.06413882,0.05077347,-0.9966485,104.7898,0.0283787,0.9983937,0.04903609,-11.12958,0.9975374,-0.02513846,-0.06547668,221.0386 +-0.06286616,0.05366026,-0.9965784,103.8159,0.02846943,0.9982436,0.05195402,-11.10241,0.9976158,-0.02510586,-0.06428341,220.9772 +-0.06175459,0.05133524,-0.9967703,102.8522,0.02699044,0.9983971,0.04974684,-11.06971,0.9977263,-0.02383116,-0.06304116,220.9184 +-0.06093267,0.0492143,-0.9969279,101.8926,0.03137947,0.9983845,0.04736829,-11.03993,0.9976485,-0.02839678,-0.06237855,220.868 +-0.06021927,0.04869397,-0.9969968,100.9339,0.03282208,0.9983659,0.04677838,-11.01296,0.9976454,-0.02990654,-0.0617191,220.8228 +-0.05915271,0.04584366,-0.9971958,99.978,0.03274869,0.9984963,0.04396084,-10.99036,0.9977116,-0.03005644,-0.06056508,220.7665 +-0.05945406,0.04798764,-0.997077,99.02616,0.03579355,0.998304,0.04591239,-10.96553,0.9975891,-0.03295924,-0.06107087,220.7126 +-0.0611289,0.05383857,-0.9966768,98.07546,0.0401805,0.9978675,0.05143852,-10.93903,0.9973208,-0.03690258,-0.0631618,220.656 +-0.06244998,0.05690491,-0.9964246,97.13185,0.03968096,0.9977254,0.05449225,-10.91126,0.9972589,-0.03613603,-0.06456596,220.5975 +-0.06350814,0.05567975,-0.9964269,96.19303,0.03842615,0.9978384,0.05330952,-10.88028,0.9972412,-0.03490324,-0.06551042,220.5371 +-0.06494787,0.05593778,-0.9963196,95.25302,0.03884521,0.9978126,0.05348937,-10.84018,0.9971323,-0.03522821,-0.06697871,220.4875 +-0.06622716,0.0592442,-0.9960442,94.32411,0.03672451,0.9977041,0.05690112,-10.8047,0.9971285,-0.03281082,-0.06825082,220.4308 +-0.06674543,0.06183084,-0.9958524,93.39619,0.03334513,0.9976588,0.0597081,-10.7682,0.9972126,-0.02922157,-0.06865092,220.3858 +-0.06750451,0.05996356,-0.9959154,92.48691,0.03300029,0.9977803,0.05783905,-10.72514,0.997173,-0.02896109,-0.06933348,220.3445 +-0.06793116,0.05870488,-0.9959614,91.59378,0.0338335,0.9978287,0.05650729,-10.68222,0.9971161,-0.02985825,-0.06976985,220.307 +-0.06833944,0.05883447,-0.9959258,90.72871,0.03456358,0.9978,0.05657348,-10.65304,0.9970632,-0.03055655,-0.07022261,220.2621 +-0.06829056,0.05814399,-0.9959697,89.87537,0.03346288,0.997872,0.05596061,-10.61699,0.9971041,-0.02950642,-0.0700909,220.2232 +-0.06855852,0.05647786,-0.9960472,89.04787,0.03282492,0.9979834,0.0543283,-10.58572,0.9971069,-0.02897049,-0.07027414,220.1828 +-0.06870573,0.05779632,-0.9959614,88.23404,0.0334534,0.9978925,0.05560064,-10.54672,0.9970759,-0.0294982,-0.07049441,220.1492 +-0.06846254,0.05995237,-0.9958507,87.4584,0.03192488,0.9978132,0.05787576,-10.51504,0.9971427,-0.02783008,-0.07022679,220.1031 +-0.06744632,0.06135034,-0.9958349,86.71465,0.02933271,0.9977981,0.05948464,-10.49284,0.9972916,-0.02519851,-0.06909738,220.0422 +-0.06492948,0.06306564,-0.995895,85.99798,0.02794382,0.9977245,0.06135964,-10.47093,0.9974985,-0.02384506,-0.06654403,219.9855 +-0.06156876,0.06469863,-0.9960037,85.3089,0.02934038,0.9975829,0.06298752,-10.44371,0.9976715,-0.02534505,-0.06331822,219.9404 +-0.05823449,0.06610262,-0.9961121,84.6474,0.03164305,0.9974262,0.06433993,-10.41398,0.9978013,-0.02777321,-0.06017629,219.8978 +-0.05499043,0.0680299,-0.9961667,84.01524,0.03089082,0.9973146,0.06640307,-10.38697,0.9980089,-0.02712086,-0.05694426,219.8549 +-0.05041776,0.06941376,-0.9963131,83.40474,0.03050758,0.9972233,0.06793337,-10.36095,0.9982621,-0.02697004,-0.05239541,219.8149 +-0.04503729,0.06815415,-0.9966578,82.84858,0.02998448,0.9973128,0.06684401,-10.33884,0.9985352,-0.02687378,-0.04695983,219.7832 +-0.03869471,0.06491006,-0.9971406,82.32202,0.03079573,0.9974914,0.06373785,-10.32295,0.9987764,-0.02824135,-0.04059659,219.7409 +-0.03209372,0.06164133,-0.9975823,81.81146,0.03176251,0.9976552,0.060624,-10.30473,0.99898,-0.02974006,-0.03397635,219.7194 +-0.02411864,0.05974658,-0.9979222,81.32495,0.03417471,0.9976784,0.05890603,-10.29169,0.9991248,-0.03268295,-0.02610447,219.6989 +-0.01421121,0.058935,-0.9981607,80.85138,0.03318099,0.9977394,0.05843773,-10.27763,0.9993483,-0.03228948,-0.0161346,219.6917 +-0.001046417,0.05914053,-0.9982491,80.38656,0.02877318,0.9978381,0.05908602,-10.26264,0.9995854,-0.02866097,-0.002745817,219.6824 +0.01551655,0.05867051,-0.9981568,79.92744,0.02418978,0.9979628,0.05903515,-10.24653,0.9995869,-0.0250612,0.01406571,219.6926 +0.03550602,0.05814641,-0.9976765,79.46687,0.02071333,0.9980486,0.05890527,-10.23212,0.9991547,-0.02275668,0.03423232,219.7241 +0.05907426,0.0582745,-0.9965512,79.00373,0.01932157,0.9980409,0.05950698,-10.22017,0.9980665,-0.02277025,0.05783256,219.7672 +0.0859365,0.05759922,-0.9946343,78.55048,0.01963346,0.9980356,0.05949254,-10.20609,0.9961071,-0.02464068,0.08463681,219.8365 +0.1164246,0.05545705,-0.9916501,78.09421,0.01886574,0.9981363,0.05803472,-10.1865,0.9930203,-0.02546487,0.1151614,219.9124 +0.1493213,0.0523383,-0.9874026,77.6607,0.01476016,0.9983689,0.05515171,-10.16712,0.9886785,-0.02280954,0.1483052,220.0076 +0.1854913,0.04948339,-0.9813992,77.22736,0.009956843,0.9985853,0.05223185,-10.15005,0.9825954,-0.01946018,0.1847362,220.1243 +0.2240496,0.04793261,-0.9733983,76.7946,0.004147398,0.9987338,0.05013482,-10.13374,0.9745689,-0.01526975,0.2235671,220.2395 +0.2644706,0.04812106,-0.9631925,76.37158,-0.001905912,0.9987784,0.04937563,-10.10898,0.9643918,-0.01122264,0.2642393,220.4064 +0.3061285,0.04751261,-0.9508038,75.95662,-0.006699001,0.9988365,0.04775599,-10.08059,0.9519666,-0.008250028,0.3060906,220.5847 +0.3483314,0.04449797,-0.9363147,75.54616,-0.01153394,0.9990004,0.04318619,-10.04834,0.9373004,-0.004243702,0.3484965,220.7577 +0.3913789,0.0415332,-0.9192919,75.15586,-0.02067059,0.9991257,0.03633978,-10.02786,0.9199974,0.004779691,0.3918952,220.9603 +0.434757,0.03950985,-0.8996807,74.7603,-0.02470077,0.9991844,0.03194333,-10.00949,0.9002089,0.008335219,0.4353783,221.1608 +0.4774632,0.03946535,-0.877765,74.38263,-0.02319707,0.9992087,0.03230749,-9.995573,0.8783455,0.004935942,0.4780008,221.4286 +0.5197986,0.03902316,-0.8533971,74.01104,-0.01999615,0.9992382,0.0335125,-9.980201,0.8540547,-0.0003550922,0.5201829,221.7265 +0.5620499,0.04208123,-0.8260321,73.63107,-0.02238675,0.999113,0.03566622,-9.966355,0.8268003,-0.001554018,0.5624934,222.0171 +0.6033987,0.04536908,-0.7961481,73.26896,-0.02543553,0.9989672,0.03764936,-9.954741,0.7970339,-0.002467118,0.6039294,222.3547 +0.643511,0.04692637,-0.7639971,72.92347,-0.02854171,0.9988959,0.03731383,-9.946438,0.7649046,-0.002206064,0.6441398,222.7156 +0.6827244,0.04587456,-0.7292346,72.5837,-0.03003232,0.9989456,0.03472463,-9.937283,0.7300586,-0.001806744,0.6833821,223.0651 +0.7210547,0.04593251,-0.691354,72.27192,-0.03497063,0.9989411,0.02989517,-9.928401,0.691995,0.002621034,0.7218974,223.4606 +0.7567754,0.04322997,-0.652244,71.97887,-0.03815822,0.9990308,0.02194099,-9.92386,0.6525603,0.008284075,0.7576914,223.8724 +0.7896629,0.04034566,-0.612213,71.67867,-0.04249753,0.9990357,0.01102238,-9.947983,0.6120674,0.01731357,0.790616,224.3103 +0.8201374,0.03244719,-0.5712459,71.4238,-0.0362487,0.9993316,0.004720557,-9.968415,0.5710173,0.01683542,0.8207653,224.7909 +0.8478472,0.02088119,-0.5298294,71.18452,-0.02438723,0.9997025,0.0003743572,-9.986514,0.5296796,0.01260367,0.8481041,225.2667 +0.8715615,0.01366634,-0.4900958,70.97076,-0.01648976,0.999863,-0.001443339,-10.0079,0.4900088,0.009339521,0.8716674,225.7708 +0.8920682,0.01198793,-0.4517418,70.76482,-0.01431331,0.999896,-0.001730556,-10.02861,0.4516741,0.008009695,0.892147,226.2719 +0.9103437,0.01660495,-0.41352,70.55968,-0.01844856,0.9998297,-0.000465267,-10.0523,0.4134418,0.008052399,0.9104949,226.7553 +0.9265644,0.01767904,-0.3757206,70.38559,-0.01937735,0.9998119,-0.000741623,-10.06762,0.3756368,0.007967632,0.9267327,227.2363 +0.9406956,0.01345836,-0.3389849,70.23832,-0.01471168,0.9998911,-0.001127822,-10.08151,0.3389328,0.006047975,0.9407911,227.7263 +0.9528103,0.007904519,-0.3034636,70.10643,-0.008817365,0.9999598,-0.001638004,-10.09299,0.3034384,0.004236457,0.9528416,228.2124 +0.9629685,0.008069543,-0.2694932,69.98684,-0.009370857,0.9999498,-0.003542581,-10.10913,0.269451,0.005936777,0.9629957,228.7114 +0.9713949,0.01111735,-0.2372098,69.87704,-0.01326168,0.9998843,-0.007445982,-10.12546,0.2370995,0.01037879,0.9714299,229.2182 +0.9784121,0.01110201,-0.2063652,69.78035,-0.01381453,0.999836,-0.01170794,-10.14303,0.2062014,0.01430603,0.9784049,229.7174 +0.9842544,0.003356104,-0.1767258,69.71657,-0.006735039,0.9998057,-0.01852327,-10.16163,0.1766293,0.01942186,0.9840857,230.2351 +0.9887985,-0.001167468,-0.149252,69.66177,-0.00254427,0.9996923,-0.02467558,-10.17931,0.1492348,0.02477891,0.9884912,230.754 +0.9923891,-0.00347434,-0.1230927,69.61108,5.885996E-05,0.9996151,-0.02774,-10.19653,0.1231417,0.02752162,0.9920073,231.2736 +0.9948317,-0.00218111,-0.1015143,69.56616,-0.0005163834,0.9996476,-0.02653869,-10.21306,0.1015364,0.02645395,0.99448,231.8059 +0.9963444,-0.005409735,-0.08525599,69.5331,0.003546329,0.9997518,-0.02199292,-10.23146,0.0853538,0.02161018,0.9961163,232.3432 +0.997257,-0.008615302,-0.07351458,69.50733,0.007499222,0.9998526,-0.01544431,-10.25272,0.0736368,0.01485064,0.9971745,232.885 +0.9978924,-0.009297228,-0.0642214,69.4779,0.008639287,0.9999074,-0.01051502,-10.27264,0.06431321,0.009938032,0.9978802,233.4257 +0.9983555,-0.005058996,-0.05710374,69.44334,0.004646736,0.9999622,-0.007349975,-10.29147,0.05713876,0.007072542,0.9983411,233.9672 +0.998616,-0.004579204,-0.05239411,69.41371,0.004221307,0.999967,-0.006939495,-10.30773,0.05242415,0.006708719,0.9986023,234.5089 +0.9985515,-0.01236418,-0.05236574,69.39643,0.01204967,0.9999074,-0.006317477,-10.32016,0.052439,0.005677335,0.9986079,235.0558 +0.9980869,-0.02190051,-0.05781807,69.37884,0.02153395,0.9997439,-0.006955394,-10.33077,0.05795559,0.005697036,0.9983029,235.6015 +0.997394,-0.02630845,-0.06718002,69.34388,0.02575166,0.9996266,-0.009140766,-10.34321,0.06739541,0.007386948,0.9976989,236.1449 +0.996507,-0.02567566,-0.07946466,69.29189,0.02498963,0.9996414,-0.009615738,-10.35888,0.07968306,0.007596357,0.9967912,236.6903 +0.9951864,-0.025692,-0.09457311,69.2301,0.02488813,0.9996435,-0.009669886,-10.376,0.09478783,0.00726959,0.9954709,237.232 +0.9930185,-0.0296712,-0.1141664,69.16533,0.02845359,0.9995197,-0.01228038,-10.38807,0.1144759,0.008946202,0.9933857,237.7751 +0.9899066,-0.0329832,-0.1378297,69.08473,0.03159505,0.9994257,-0.0122478,-10.40131,0.1381545,0.007769443,0.9903802,238.3166 +0.9859246,-0.03060503,-0.1643658,68.97871,0.02960095,0.9995252,-0.008555294,-10.41441,0.1645495,0.003569493,0.9863623,238.8478 +0.9804132,-0.02602558,-0.1952247,68.85058,0.02553614,0.9996613,-0.005023915,-10.43138,0.1952893,-5.977129E-05,0.9807456,239.3772 +0.9726654,-0.02355779,-0.2310131,68.70774,0.02300207,0.9997224,-0.005098975,-10.44887,0.2310691,-0.0003541835,0.9729372,239.8978 +0.9627454,-0.02024061,-0.2696509,68.5751,0.01874801,0.9997913,-0.00810985,-10.43437,0.2697588,0.002752303,0.9629239,240.3834 +0.9500989,-0.01569965,-0.3115535,68.41101,0.01312643,0.9998602,-0.01035473,-10.4137,0.3116725,0.00574843,0.9501721,240.8273 +0.9341982,-0.009426163,-0.35663,68.21683,0.005620304,0.9999157,-0.01170651,-10.40046,0.3567103,0.008931828,0.9341723,241.2894 +0.9148738,-0.0002358564,-0.40374,67.9883,-0.00612664,0.9998766,-0.01446706,-10.33695,0.4036935,0.0157091,0.9147593,241.6933 +0.8910773,0.01303515,-0.4536644,67.72742,-0.02192814,0.9996566,-0.01434758,-10.29006,0.4533215,0.02273282,0.8910571,242.1112 +0.8633013,0.02784557,-0.5039202,67.4447,-0.03781492,0.9992389,-0.00956755,-10.23506,0.5032703,0.02731538,0.8636972,242.4966 +0.8315041,0.03888847,-0.5541558,67.12537,-0.0503025,0.9987194,-0.005392089,-10.19844,0.5532365,0.03235896,0.8323955,242.8311 +0.7953479,0.04648677,-0.6043681,66.8031,-0.05825956,0.9983014,0.0001178046,-10.15367,0.603347,0.03511652,0.7967052,243.1829 +0.7551591,0.05233082,-0.6534495,66.45921,-0.05900849,0.9981884,0.0117457,-10.11992,0.6528804,0.02968919,0.7568789,243.5343 +0.7115133,0.06001076,-0.7001054,66.08635,-0.06017287,0.9978901,0.02438254,-10.06972,0.7000915,0.02477885,0.713623,243.8073 +0.6651519,0.06745535,-0.743655,65.70179,-0.0606676,0.9975007,0.03621787,-10.03379,0.7442395,0.02102538,0.6675818,244.1078 +0.6162921,0.07725151,-0.7837195,65.2972,-0.06728305,0.9967034,0.04533616,-10.00031,0.7846381,0.02479071,0.6194581,244.371 +0.5641729,0.08038141,-0.8217347,64.86954,-0.06944687,0.9963427,0.04978177,-9.961987,0.8227308,0.02898137,0.5676918,244.5565 +0.5079535,0.07498334,-0.8581147,64.44825,-0.0640429,0.9967342,0.0491865,-9.923632,0.8590004,0.02997169,0.5110968,244.7822 +0.4498124,0.0647763,-0.890771,64.01807,-0.05152832,0.9975873,0.04652368,-9.878884,0.8916354,0.02497301,0.4520648,244.9865 +0.3893689,0.0582864,-0.9192359,63.55563,-0.04022251,0.9981197,0.04625083,-9.838486,0.9202032,0.01896534,0.3909812,245.1196 +0.3299005,0.05855375,-0.9421981,63.0808,-0.03619953,0.9981251,0.04935452,-9.79865,0.9433214,0.01782505,0.3314015,245.26 +0.2739344,0.06269287,-0.9597029,62.58919,-0.03776665,0.9978046,0.0544019,-9.766015,0.9610065,0.02134221,0.2757007,245.3658 +0.2218518,0.06583776,-0.9728552,62.08369,-0.04015829,0.9974883,0.05834703,-9.729817,0.974253,0.0261238,0.2239385,245.4135 +0.1736747,0.06603133,-0.9825869,61.58075,-0.04052634,0.9973836,0.05986257,-9.695895,0.9839688,0.02942404,0.1758963,245.4747 +0.1293567,0.06456627,-0.9894939,61.06677,-0.03758707,0.99748,0.06017362,-9.663459,0.9908854,0.02940831,0.1314576,245.5028 +0.08984261,0.06148548,-0.9940563,60.54783,-0.03581451,0.9976465,0.05847064,-9.629494,0.9953118,0.03034849,0.09183323,245.5341 +0.05669005,0.05799537,-0.996706,60.02194,-0.03639654,0.9977679,0.05598703,-9.593959,0.9977281,0.03310274,0.05867434,245.5451 +0.02974916,0.05587639,-0.9979944,59.48324,-0.04025348,0.9976933,0.05465963,-9.559976,0.9987465,0.03854667,0.03192976,245.5337 +0.009653026,0.05357301,-0.9985173,58.93583,-0.04471565,0.997588,0.05309087,-9.528225,0.9989531,0.04413686,0.01202529,245.5202 +-0.00536724,0.05187898,-0.998639,58.37363,-0.04758073,0.997509,0.05207601,-9.496583,0.9988529,0.04779547,-0.002885431,245.5013 +-0.01809084,0.05039661,-0.9985655,57.78293,-0.04670382,0.9975961,0.05119382,-9.482206,0.9987449,0.04756295,-0.01569364,245.462 +-0.02801364,0.04863872,-0.9984235,57.17641,-0.04459136,0.9977604,0.04985756,-9.44036,0.9986124,0.04591776,-0.02578203,245.4243 +-0.03400568,0.04762773,-0.9982862,56.55779,-0.0449958,0.997778,0.04913624,-9.402285,0.9984082,0.04658959,-0.03178707,245.3877 +-0.03713073,0.04791171,-0.9981612,55.91578,-0.04708729,0.9976566,0.0496391,-9.362804,0.9982004,0.04884384,-0.03478768,245.3431 +-0.03834668,0.04829377,-0.9980968,55.25735,-0.0481721,0.9975808,0.05011958,-9.324777,0.9981026,0.05000233,-0.0359275,245.3053 +-0.03727636,0.04911273,-0.9980974,54.5757,-0.04738024,0.9975814,0.05085688,-9.285086,0.9981811,0.04918585,-0.03485923,245.2676 +-0.03624355,0.04934787,-0.9981239,53.87733,-0.04586641,0.9976454,0.05098971,-9.249048,0.9982898,0.0476284,-0.0338948,245.2345 +-0.03524759,0.04817378,-0.9982169,53.15948,-0.04360048,0.9978123,0.04969382,-9.212307,0.998427,0.04527432,-0.03307008,245.2012 +-0.03403347,0.04584723,-0.9983686,52.42232,-0.04039077,0.998068,0.04721032,-9.177032,0.9986041,0.04193161,-0.03211592,245.1719 +-0.03339829,0.04455994,-0.9984483,51.66476,-0.03926779,0.9981757,0.0458613,-9.13911,0.9986704,0.04073855,-0.03158759,245.1412 +-0.03258092,0.04383273,-0.9985075,50.88664,-0.03935916,0.9982066,0.04510381,-9.096874,0.9986938,0.04076993,-0.03079727,245.1114 +-0.03209051,0.04426563,-0.9985043,50.08793,-0.03958302,0.9981787,0.04552334,-9.057067,0.9987008,0.04098468,-0.0302799,245.0819 +-0.03183461,0.04547036,-0.9984583,49.26867,-0.03771969,0.9981983,0.04666117,-9.011002,0.9987811,0.03914698,-0.03006212,245.0587 +-0.03111467,0.04540085,-0.9984842,48.43438,-0.03459195,0.9983205,0.04647136,-8.96818,0.998917,0.03598545,-0.02949191,245.0351 +-0.03106506,0.04496827,-0.9985053,47.58164,-0.03206073,0.9984285,0.04596228,-8.929268,0.999003,0.03344062,-0.02957452,245.0089 +-0.03142502,0.0446154,-0.9985099,46.70967,-0.03024418,0.9985033,0.04556696,-8.895416,0.9990484,0.03163105,-0.03002863,244.9855 +-0.03335764,0.04572485,-0.998397,45.82014,-0.02847243,0.998504,0.04668105,-8.854608,0.9990378,0.02998396,-0.03200584,244.9638 +-0.0360658,0.04821853,-0.9981855,44.91897,-0.02625094,0.9984449,0.04917955,-8.803572,0.9990045,0.027977,-0.03474393,244.9376 +-0.03918556,0.050474,-0.9979564,44.00529,-0.02489504,0.9983641,0.05147215,-8.749626,0.9989217,0.02686113,-0.0378649,244.9075 +-0.04252786,0.04847058,-0.9979189,43.06932,-0.0252522,0.9984512,0.04957261,-8.707462,0.9987761,0.02730787,-0.041238,244.8643 +-0.04569615,0.04364905,-0.9980013,42.12282,-0.02667135,0.9986354,0.04489801,-8.671159,0.9985992,0.02866971,-0.04446962,244.8172 +-0.04823405,0.0400324,-0.9980335,41.15791,-0.02577903,0.9988138,0.04130958,-8.634776,0.9985033,0.02772086,-0.04714484,244.7715 +-0.05031424,0.0417168,-0.9978618,40.17207,-0.02456477,0.9987733,0.04299352,-8.596479,0.9984313,0.02667543,-0.04922775,244.7238 +-0.051814,0.04792876,-0.997506,39.16472,-0.02317078,0.998521,0.04918112,-8.559737,0.9983879,0.02566126,-0.05062683,244.6724 +-0.05301383,0.05443536,-0.997109,38.13727,-0.02124554,0.9982256,0.0556259,-8.510418,0.9983677,0.02413306,-0.05176325,244.6196 +-0.05383321,0.05468975,-0.9970512,37.10263,-0.01771139,0.9982896,0.05571398,-8.453846,0.9983928,0.02065843,-0.0527725,244.5684 +-0.05486648,0.05308811,-0.9970814,36.05706,-0.01767946,0.9983774,0.05412997,-8.397715,0.9983371,0.02059779,-0.05383888,244.5107 +-0.05549851,0.05048846,-0.9971815,34.99762,-0.01916674,0.9984828,0.05162109,-8.342557,0.9982747,0.02197761,-0.0544466,244.4496 +-0.05561886,0.04957916,-0.9972204,33.92407,-0.01986287,0.9985138,0.05075131,-8.289686,0.9982544,0.02263039,-0.05455141,244.386 +-0.05506402,0.04947618,-0.9972563,32.8401,-0.02337642,0.9984339,0.05082536,-8.240889,0.9982091,0.02611093,-0.0538212,244.3182 +-0.05440495,0.04825164,-0.9973525,31.75524,-0.02817307,0.9983599,0.04983721,-8.195062,0.9981214,0.03080987,-0.05295632,244.2483 +-0.0540423,0.04645458,-0.9974575,30.67633,-0.02935109,0.9984117,0.04808927,-8.137184,0.9981071,0.03187532,-0.05259296,244.1839 +-0.053738,0.04654161,-0.9974699,29.59544,-0.03168074,0.9983309,0.04828857,-8.086906,0.9980523,0.03419551,-0.05217383,244.119 +-0.05263858,0.0469378,-0.9975099,28.51574,-0.03093755,0.9983386,0.04860938,-8.039974,0.9981342,0.03341924,-0.05109898,244.0567 +-0.05237234,0.04706576,-0.9975179,27.43854,-0.02944567,0.9983816,0.0486525,-7.979478,0.9981934,0.03192062,-0.0509017,243.9942 +-0.05184967,0.04868328,-0.9974676,26.35603,-0.02659324,0.9983895,0.05011064,-7.92951,0.9983007,0.02912411,-0.05047152,243.9385 +-0.05165916,0.05048031,-0.9973882,25.27991,-0.01851375,0.9985016,0.05149558,-7.868716,0.9984931,0.02112561,-0.05064717,243.891 +-0.05210805,0.05053857,-0.9973619,24.20857,-0.01590955,0.9985498,0.05142999,-7.813927,0.9985147,0.0185475,-0.05122844,243.8374 +-0.05312255,0.05067353,-0.9973015,23.13628,-0.01281644,0.9985948,0.05142194,-7.757005,0.9985057,0.01551353,-0.05239844,243.7781 +-0.05589718,0.05203177,-0.9970799,22.06412,-0.009577439,0.9985673,0.05264632,-7.706868,0.9983906,0.01249226,-0.05531876,243.7192 +-0.05861,0.05649529,-0.9966811,20.99349,-0.01219935,0.9982822,0.05730344,-7.661799,0.9982064,0.01551742,-0.05782011,243.6491 +-0.0615142,0.05972326,-0.9963178,19.92389,-0.01433032,0.9980524,0.06071203,-7.612126,0.9980033,0.01801221,-0.06053854,243.5746 +-0.06476204,0.05896297,-0.9961573,18.86942,-0.01796815,0.9980221,0.0602415,-7.563301,0.9977389,0.02180047,-0.06357448,243.4972 +-0.06683295,0.05462737,-0.9962677,17.81625,-0.02068253,0.9982097,0.05612132,-7.51355,0.9975498,0.02435609,-0.06558347,243.4157 +-0.06849796,0.05110422,-0.9963415,16.77167,-0.02388933,0.9983168,0.05284792,-7.476025,0.9973652,0.02742191,-0.06716182,243.3337 +-0.06990284,0.04958745,-0.9963206,15.72923,-0.02562939,0.9983448,0.05148638,-7.44571,0.9972245,0.02913413,-0.06851624,243.2513 +-0.07124759,0.05147227,-0.9961297,14.6927,-0.02839208,0.9981583,0.05360783,-7.414939,0.9970545,0.03210162,-0.06965496,243.1677 +-0.07211286,0.05602313,-0.9958219,13.66194,-0.03153355,0.9977941,0.05841761,-7.392199,0.9968978,0.03561446,-0.07018717,243.0825 +-0.07330661,0.05849357,-0.9955926,12.64677,-0.03617651,0.9974656,0.06126733,-7.360999,0.9966531,0.04050837,-0.07100472,242.9949 +-0.07416172,0.05798562,-0.995559,11.64596,-0.0399973,0.9973319,0.06106838,-7.332324,0.9964438,0.0443486,-0.07164457,242.9079 +-0.07529792,0.05614125,-0.9955794,10.66639,-0.04620619,0.9971449,0.05972422,-7.304335,0.9960899,0.05049904,-0.07248886,242.8183 +-0.07577645,0.05569604,-0.9955681,9.697182,-0.04966858,0.9969885,0.05955598,-7.281012,0.995887,0.05396139,-0.0727819,242.7289 +-0.07635941,0.05575849,-0.9955201,8.749169,-0.05306604,0.9967928,0.05990011,-7.25431,0.9956672,0.05740224,-0.07315562,242.6424 +-0.07629943,0.05520812,-0.9955554,7.814747,-0.05685215,0.9966007,0.05962325,-7.229902,0.9954628,0.06114867,-0.07290136,242.5552 +-0.07604025,0.05507384,-0.9955826,6.904974,-0.05880584,0.9964877,0.05961537,-7.200265,0.9953691,0.06307923,-0.07253451,242.4742 +-0.076025,0.05726563,-0.9954601,6.011306,-0.0595325,0.9963077,0.061861,-7.16886,0.9953271,0.0639652,-0.07233512,242.3982 +-0.07604756,0.05943514,-0.9953312,5.139292,-0.06003481,0.9961379,0.06407023,-7.146637,0.9952952,0.0646269,-0.07218568,242.3252 +-0.07603218,0.05905873,-0.9953548,4.29605,-0.0612809,0.9960805,0.06378287,-7.127999,0.9952204,0.06584578,-0.072115,242.2525 +-0.07609936,0.05835176,-0.9953914,3.474518,-0.06098669,0.9961447,0.06305847,-7.110408,0.9952334,0.06550432,-0.07224728,242.1822 +-0.07642529,0.05749494,-0.9954163,2.679849,-0.05872327,0.9963435,0.06205712,-7.089124,0.9953445,0.06319683,-0.07276955,242.1187 +-0.07708117,0.05719954,-0.9953827,1.910133,-0.05838389,0.9963808,0.06177808,-7.073093,0.9953139,0.06287623,-0.07346266,242.055 +-0.07690222,0.05824151,-0.9953361,1.168286,-0.05871979,0.996295,0.06283447,-7.052345,0.995308,0.06327804,-0.07319736,241.9926 +-0.07665759,0.05793452,-0.9953729,0.4503087,-0.06032739,0.9962119,0.06262942,-7.034578,0.9952307,0.06484926,-0.07287216,241.9307 +-0.07539126,0.05839331,-0.9954428,-0.2363936,-0.06095442,0.9961471,0.0630511,-7.012744,0.9952892,0.06543014,-0.07154145,241.8736 +-0.07317061,0.06042684,-0.9954872,-0.8987715,-0.06091835,0.9960281,0.06493733,-6.989223,0.9954571,0.06539494,-0.06919888,241.821 +-0.06993858,0.06236856,-0.9955997,-1.528966,-0.06029823,0.9959543,0.0666266,-6.964258,0.9957272,0.06469267,-0.06589491,241.776 +-0.06590134,0.06434813,-0.9957492,-2.127276,-0.06165501,0.995749,0.06842863,-6.941613,0.9959195,0.06590245,-0.06165381,241.7339 +-0.06055624,0.06632598,-0.9959588,-2.696521,-0.06201134,0.9956125,0.07007335,-6.920228,0.9962366,0.0660041,-0.05617759,241.6984 +-0.05450908,0.06754004,-0.9962265,-3.228819,-0.06212506,0.9955474,0.07089322,-6.897384,0.9965787,0.06575494,-0.05007044,241.6707 +-0.047348,0.06735504,-0.996605,-3.728376,-0.06101841,0.9956656,0.07019051,-6.87652,0.997013,0.06413463,-0.04303287,241.6522 +-0.03964776,0.06573368,-0.9970492,-4.190561,-0.05955881,0.9959042,0.06802656,-6.860267,0.9974371,0.06208016,-0.03557035,241.6401 +-0.03100336,0.06415434,-0.9974583,-4.6183,-0.05863363,0.9961027,0.06588964,-6.851022,0.997798,0.0605274,-0.02712093,241.6335 +-0.02180203,0.06241744,-0.997812,-5.015158,-0.05842099,0.9962642,0.06359712,-6.841571,0.9980539,0.0596797,-0.01807409,241.6281 +-0.01280422,0.06166828,-0.9980146,-5.386511,-0.05856867,0.9963365,0.06231601,-6.836364,0.9982012,0.05925029,-0.009145484,241.6295 +-0.003636745,0.06104361,-0.9981285,-5.727634,-0.05871357,0.9964001,0.06115184,-6.828673,0.9982682,0.05882608,-3.956605E-05,241.6356 +0.005746315,0.05629747,-0.9983975,-6.042761,-0.05687366,0.9968163,0.05588098,-6.828111,0.9983648,0.05646141,0.008929861,241.6461 +0.0155066,0.05049864,-0.9986038,-6.326933,-0.05481396,0.9972649,0.04957978,-6.826022,0.9983761,0.05396861,0.01823221,241.66 +0.02627633,0.04791057,-0.998506,-6.59491,-0.05325786,0.9974993,0.04646077,-6.821755,0.998235,0.05195747,0.02876223,241.6763 +0.03687121,0.0477729,-0.9981775,-6.84525,-0.05271683,0.9975588,0.04579602,-6.815427,0.9979285,0.05093219,0.03929963,241.6926 +0.04790024,0.04620213,-0.997783,-7.082572,-0.05266346,0.9976571,0.04366811,-6.808075,0.9974628,0.05045499,0.05022118,241.7122 +0.06031997,0.04299534,-0.9972527,-7.316062,-0.05234086,0.9978337,0.0398545,-6.80387,0.9968058,0.04979303,0.0624397,241.7365 +0.07412221,0.0395194,-0.9964658,-7.544603,-0.05244539,0.9979862,0.03567855,-6.799465,0.9958691,0.04961546,0.07604555,241.7619 +0.09102819,0.03663909,-0.9951741,-7.774661,-0.05254989,0.9981073,0.03194037,-6.788214,0.9944608,0.04938881,0.09278128,241.7928 +0.109331,0.034878,-0.9933933,-7.99983,-0.05333978,0.9981501,0.02917454,-6.777323,0.9925732,0.04979769,0.1109892,241.8246 +0.1288159,0.03374409,-0.9910943,-8.225849,-0.05425893,0.9981636,0.02693257,-6.762931,0.990183,0.05030637,0.1304103,241.8676 +0.1485469,0.03491618,-0.9882888,-8.447696,-0.05256606,0.9982424,0.02736679,-6.75517,0.9875072,0.04788519,0.1501212,241.9199 +0.1695803,0.03891885,-0.9847476,-8.668434,-0.05107783,0.998224,0.03065552,-6.750084,0.9841918,0.0451002,0.171267,241.9718 +0.1917481,0.04259726,-0.9805194,-8.884203,-0.05093003,0.9981434,0.03340316,-6.748442,0.9801218,0.04353289,0.1935616,242.0306 +0.2146542,0.04198544,-0.9757873,-9.09555,-0.05280999,0.998113,0.0313289,-6.745895,0.9752613,0.04480643,0.2164664,242.0944 +0.2406255,0.03622996,-0.9699417,-9.310473,-0.05296199,0.9983044,0.02415045,-6.737931,0.969172,0.04555882,0.2421363,242.1598 +0.2696716,0.02958224,-0.9624979,-9.536865,-0.05065029,0.9985801,0.01650008,-6.731274,0.9616193,0.04430118,0.2707871,242.2532 +0.3002726,0.02658295,-0.953483,-9.780101,-0.04672566,0.9988214,0.01313206,-6.722858,0.9527082,0.04060892,0.3011608,242.3536 +0.3337358,0.02790531,-0.9422536,-10.04028,-0.04112212,0.9990412,0.01502211,-6.717894,0.9417692,0.03373405,0.3345633,242.4896 +0.3703076,0.03162716,-0.9283707,-10.31625,-0.03917736,0.9990627,0.01840843,-6.710409,0.9280826,0.02955433,0.3711995,242.6457 +0.4095053,0.03740001,-0.9115408,-10.60398,-0.0402144,0.9989282,0.02291934,-6.704932,0.911421,0.02727148,0.4105704,242.7928 +0.4502066,0.0427266,-0.8919016,-10.90063,-0.04398185,0.9987032,0.02564215,-6.698509,0.8918406,0.02768322,0.4515019,242.9867 +0.4929454,0.04646108,-0.8688189,-11.20097,-0.04773396,0.9985134,0.02631364,-6.692741,0.8687498,0.02850097,0.4944304,243.2065 +0.5365411,0.04893406,-0.8424542,-11.49833,-0.04869405,0.9984492,0.02698285,-6.68981,0.8424681,0.02654509,0.5380918,243.4202 +0.5807729,0.04947012,-0.8125611,-11.78884,-0.04622707,0.9985453,0.02775266,-6.688949,0.812752,0.02144433,0.5822149,243.6969 +0.6259864,0.04765468,-0.7783766,-12.0709,-0.04353656,0.99871,0.02613121,-6.68842,0.7786178,0.01753006,0.6272535,243.9575 +0.6712781,0.0452716,-0.7398218,-12.34428,-0.04350012,0.9988188,0.02165042,-6.68759,0.739928,0.01764888,0.6724544,244.2799 +0.7164322,0.0403688,-0.6964878,-12.60568,-0.04319203,0.9989759,0.01347232,-6.684619,0.6963184,0.02043072,0.7174421,244.6299 +0.7602273,0.03804431,-0.6485423,-12.85639,-0.04500172,0.9989698,0.0058494,-6.678676,0.6480966,0.02473864,0.7611561,244.9548 +0.8015137,0.03803052,-0.5967659,-13.09955,-0.04685767,0.9989013,0.0007233622,-6.675619,0.5961377,0.02738327,0.802415,245.3621 +0.8393979,0.03816677,-0.5421757,-13.32519,-0.04576826,0.9989519,-0.0005367554,-6.676025,0.5415869,0.02526499,0.840265,245.8054 +0.8731126,0.0348972,-0.4862681,-13.5269,-0.04098398,0.999158,-0.001883348,-6.678304,0.4857929,0.02157358,0.8738076,246.2275 +0.9023501,0.03145702,-0.4298544,-13.71079,-0.03584909,0.9993549,-0.002120943,-6.687935,0.4295104,0.01732372,0.9028957,246.7307 +0.9270626,0.02885887,-0.3737943,-13.87609,-0.03287392,0.9994499,-0.00436919,-6.693307,0.3734625,0.01633859,0.9275013,247.2121 +0.9474538,0.02683863,-0.3187649,-14.02343,-0.03171953,0.9994455,-0.01012986,-6.696054,0.3183163,0.01970865,0.9477796,247.7627 +0.9639915,0.02646981,-0.2646127,-14.15234,-0.03171795,0.9993754,-0.01557958,-6.699757,0.264035,0.02341155,0.9642289,248.3368 +0.9767452,0.02793246,-0.2125761,-14.26228,-0.03226,0.9993363,-0.0169157,-6.70909,0.2119625,0.02338004,0.976998,248.9039 +0.9859099,0.02853811,-0.164825,-14.34841,-0.03178168,0.999349,-0.01707476,-6.72104,0.1642304,0.02207259,0.986175,249.5347 +0.9919632,0.02603261,-0.12382,-14.41196,-0.02840814,0.9994439,-0.01745832,-6.733875,0.1232967,0.0208355,0.9921511,250.1838 +0.9956664,0.02164276,-0.09044382,-14.45672,-0.02336493,0.9995645,-0.01802593,-6.746994,0.09001429,0.02006102,0.9957384,250.8432 +0.9977697,0.01629015,-0.06473238,-14.4879,-0.0175227,0.9996749,-0.01851865,-6.761766,0.06440967,0.01961163,0.9977308,251.5375 +0.9988656,0.01161639,-0.04618058,-14.51196,-0.01248109,0.9997513,-0.01848032,-6.778409,0.04595442,0.01903574,0.9987621,252.255 +0.9993719,0.009193214,-0.03422423,-14.53435,-0.009833383,0.9997789,-0.01858401,-6.795428,0.03404582,0.01890888,0.9992413,252.9941 +0.999602,0.006369113,-0.02748533,-14.55485,-0.006904773,0.9997872,-0.01943824,-6.812917,0.02735567,0.01962028,0.9994331,253.7579 +0.9996962,0.004640308,-0.02420992,-14.57516,-0.005141249,0.9997731,-0.02067049,-6.830954,0.0241085,0.02078868,0.9994931,254.5436 +0.9997404,0.004984347,-0.02223427,-14.59703,-0.005464946,0.9997516,-0.02160708,-6.852364,0.02212104,0.02172297,0.9995192,255.3502 +0.9997598,0.005217851,-0.02128954,-14.61988,-0.005669558,0.9997589,-0.02121241,-6.874307,0.02117372,0.02132801,0.9995482,256.1819 +0.9997607,0.005746823,-0.02111051,-14.64407,-0.006153143,0.9997961,-0.01923301,-6.896091,0.02099568,0.0193583,0.9995921,257.0344 +0.999756,0.005119028,-0.02149187,-14.66709,-0.005466354,0.9998549,-0.01613329,-6.918247,0.02140616,0.01624683,0.9996388,257.9107 +0.9997295,0.00526634,-0.0226554,-14.68821,-0.005583176,0.9998872,-0.01394454,-6.939815,0.02257941,0.01406725,0.999646,258.8052 +0.9996915,0.00482716,-0.02436772,-14.71049,-0.005177702,0.9998837,-0.01434295,-6.961615,0.02429565,0.01446469,0.9996001,259.7176 +0.9996289,0.00319887,-0.02705216,-14.73599,-0.003683048,0.9998336,-0.01786711,-6.981897,0.0269905,0.01796011,0.9994743,260.6496 +0.999548,0.003214811,-0.02989201,-14.76705,-0.003957523,0.9996841,-0.02482061,-7.0036,0.02980277,0.02492768,0.9992449,261.6006 +0.9994506,0.005468723,-0.03269174,-14.80748,-0.006466633,0.9995139,-0.03049744,-7.028589,0.03250907,0.03069209,0.999,262.576 +0.9993656,0.007749248,-0.03476113,-14.84952,-0.0088321,0.999477,-0.03110661,-7.051911,0.03450189,0.03139389,0.9989114,263.5759 +0.9993246,0.009737354,-0.03543353,-14.89391,-0.01051452,0.9997068,-0.02181314,-7.076486,0.03521073,0.02217097,0.9991339,264.6079 +0.9993294,0.009145133,-0.03545603,-14.92906,-0.009575942,0.9998821,-0.01199979,-7.097279,0.03534211,0.01233127,0.9992991,265.6633 +0.9993448,0.006130598,-0.03567262,-14.95748,-0.006461052,0.9999372,-0.009155627,-7.110536,0.03561425,0.00938011,0.9993215,266.7298 +0.9993301,0.001536956,-0.0365673,-14.98774,-0.002109735,0.9998756,-0.01563027,-7.122595,0.03653872,0.01569695,0.9992089,267.808 +0.9992988,-0.0007298224,-0.03743566,-15.02077,-0.0002002828,0.9996915,-0.02483567,-7.139046,0.03744224,0.02482575,0.9989903,268.902 +0.999274,-0.001800961,-0.03805628,-15.05963,0.0006541212,0.9995459,-0.03012636,-7.166925,0.03809325,0.03007959,0.9988213,270.0156 +0.9992704,0.0002910066,-0.03819373,-15.09872,-0.001408505,0.9995715,-0.02923505,-7.199147,0.03816885,0.02926751,0.9988426,271.1538 +0.9992604,0.002282914,-0.03838555,-15.14305,-0.003126073,0.9997548,-0.02191989,-7.232552,0.0383261,0.02202367,0.9990225,272.3074 +0.9992436,0.003586591,-0.03872303,-15.18271,-0.00405284,0.9999201,-0.01196885,-7.258803,0.03867701,0.01211673,0.9991782,273.4772 +0.9992329,0.004283696,-0.038928,-15.22418,-0.004557529,0.9999655,-0.006948327,-7.27739,0.03889689,0.007120412,0.9992178,274.654 +0.9992252,0.001699934,-0.0393204,-15.26136,-0.002057426,0.9999569,-0.009053075,-7.291634,0.03930332,0.009126959,0.9991856,275.8308 +0.9991962,0.0006158513,-0.04008437,-15.30315,-0.00120927,0.99989,-0.01478168,-7.309677,0.04007085,0.01481827,0.9990869,277.0214 +0.9991599,-0.00139838,-0.04095968,-15.34577,0.0006082578,0.9998136,-0.01929635,-7.332706,0.04097902,0.01925523,0.9989744,278.2198 +0.9991244,-0.001298084,-0.04181945,-15.39143,0.0003996136,0.9997691,-0.02148571,-7.362888,0.04183768,0.02145018,0.9988941,279.4335 +0.9991085,-0.001922202,-0.04217392,-15.44023,0.001134563,0.9998246,-0.01869199,-7.394637,0.04220245,0.01862747,0.9989354,280.6585 +0.9990839,-0.0006789334,-0.04278968,-15.49104,0.0001026017,0.9999093,-0.0134697,-7.427375,0.04279494,0.01345297,0.9989932,281.892 +0.9990444,-0.002237245,-0.04365161,-15.54177,0.001596797,0.9998906,-0.01470117,-7.458635,0.04367972,0.01461741,0.9989386,283.1286 +0.9989948,-0.004078626,-0.04464107,-15.59058,0.003196042,0.9997983,-0.01982423,-7.489167,0.04471292,0.01966162,0.9988063,284.3516 +0.9989629,-0.004269014,-0.04533257,-15.64569,0.003199965,0.9997157,-0.02362879,-7.52495,0.04542055,0.02345922,0.9986924,285.5828 +0.9989272,-0.003277193,-0.04619311,-15.70314,0.002165064,0.9997071,-0.02410516,-7.562304,0.04625858,0.02397929,0.9986416,286.824 +0.9989164,-0.0009774785,-0.04653221,-15.76449,-4.126441E-05,0.9997604,-0.02188729,-7.603002,0.04654246,0.02186549,0.9986769,288.0682 +0.9989168,0.001379015,-0.04651225,-15.82556,-0.002171361,0.9998533,-0.01698899,-7.641323,0.046482,0.01707158,0.9987732,289.317 +0.9989508,0.004482574,-0.04557787,-15.88743,-0.005088715,0.9999,-0.01319168,-7.676762,0.04551418,0.01340977,0.9988736,290.5656 +0.9989977,0.006374307,-0.0443072,-15.9468,-0.006905083,0.9999061,-0.01183673,-7.707939,0.04422759,0.01213081,0.9989478,291.8117 +0.9990621,0.004737662,-0.04304069,-15.99936,-0.005294473,0.9999036,-0.01283208,-7.737233,0.04297574,0.01304792,0.9989909,293.0549 +0.9991072,0.004950869,-0.04195602,-16.05246,-0.00566216,0.9998419,-0.0168514,-7.771299,0.04186596,0.01707392,0.9989773,294.3017 +0.9991529,0.003503029,-0.04100335,-16.09994,-0.004418586,0.9997424,-0.02225954,-7.813183,0.04091481,0.02242186,0.998911,295.5434 +0.9991944,0.002802028,-0.04003524,-16.14863,-0.003789777,0.9996897,-0.02461742,-7.859032,0.03995384,0.02474931,0.9988949,296.7911 +0.9992451,0.003698055,-0.03867407,-16.20391,-0.004578231,0.9997319,-0.02269506,-7.906555,0.03857977,0.02285499,0.9989941,298.0516 +0.999263,0.006222291,-0.03788016,-16.26006,-0.006928839,0.9998039,-0.01854958,-7.954308,0.03775731,0.01879837,0.9991101,299.3084 +0.9992693,0.007474533,-0.03748358,-16.31526,-0.008097118,0.9998313,-0.01648529,-8.000839,0.03735404,0.01677675,0.9991612,300.5703 +0.9992661,0.01032653,-0.03688813,-16.37063,-0.01077598,0.9998698,-0.01200604,-8.040366,0.03675934,0.01239473,0.9992472,301.8367 +0.9992609,0.01267851,-0.03628932,-16.42531,-0.01291475,0.9998968,-0.006282763,-8.077115,0.03620592,0.006746787,0.9993215,303.1058 +0.9992157,0.01616603,-0.03614776,-16.48034,-0.01646654,0.9998321,-0.008031087,-8.109685,0.03601186,0.008620016,0.9993141,304.3697 +0.9991776,0.01846009,-0.03610175,-16.53456,-0.01896678,0.9997256,-0.01374324,-8.143183,0.03583815,0.01441667,0.9992536,305.6299 +0.9991014,0.02162902,-0.03645036,-16.59014,-0.02230836,0.999583,-0.01833499,-8.181496,0.03603859,0.01913166,0.9991672,306.8933 +0.9990033,0.02518494,-0.03685513,-16.6472,-0.02595932,0.9994489,-0.02068563,-8.224857,0.03631385,0.02162174,0.9991064,308.1607 +0.9989452,0.02687656,-0.03723194,-16.70249,-0.02768399,0.9993888,-0.02134333,-8.2705,0.03663555,0.02235154,0.9990786,309.4306 +0.9988879,0.02806302,-0.03788818,-16.75668,-0.02876432,0.9994224,-0.01809318,-8.315321,0.03735855,0.01916289,0.9991181,310.7059 +0.9988979,0.02702485,-0.03837644,-16.8065,-0.02760774,0.9995101,-0.01474065,-8.353996,0.03795927,0.01578389,0.9991546,311.9815 +0.9988762,0.02692065,-0.03900939,-16.85845,-0.02742692,0.9995456,-0.01250151,-8.38898,0.03865511,0.01355737,0.9991606,313.2591 +0.9988727,0.02605478,-0.03968143,-16.91025,-0.02656054,0.9995719,-0.01227201,-8.4224,0.0393447,0.01331214,0.999137,314.5361 +0.9988555,0.02576317,-0.04029844,-16.96357,-0.026261,0.9995846,-0.01187303,-8.45494,0.03997581,0.01291771,0.9991171,315.8158 +0.9988468,0.02512435,-0.04091252,-17.01429,-0.02564222,0.9995969,-0.01218257,-8.486316,0.04058995,0.01321761,0.9990884,317.0938 +0.9987829,0.02582171,-0.04202477,-17.06216,-0.02633761,0.9995838,-0.01176888,-8.514715,0.04170338,0.01286139,0.9990472,318.3716 +0.9987549,0.02592811,-0.04261934,-17.10889,-0.02639327,0.9995976,-0.010388,-8.543556,0.04233285,0.01149993,0.9990373,319.6428 +0.9986996,0.02736063,-0.04301924,-17.16005,-0.02792064,0.9995323,-0.01247098,-8.574624,0.0426579,0.01365588,0.9989964,320.9152 +0.9986733,0.02780499,-0.04334394,-17.2117,-0.02858599,0.999438,-0.01750411,-8.606204,0.04283288,0.01871991,0.9989068,322.1833 +0.9987074,0.02640602,-0.04343098,-17.25927,-0.02716736,0.9994857,-0.01703376,-8.641599,0.04295885,0.01819165,0.9989112,323.4533 +0.9986852,0.02797993,-0.04295511,-17.31098,-0.02855539,0.9995097,-0.01284181,-8.680667,0.04257474,0.01405152,0.9989944,324.7256 +0.9987442,0.02687988,-0.04228066,-17.3596,-0.02732422,0.9995769,-0.009966724,-8.713173,0.04199486,0.01110949,0.999056,325.9958 +0.9987754,0.0267344,-0.0416305,-17.40874,-0.02708483,0.9996021,-0.007876277,-8.741762,0.04140337,0.008994185,0.999102,327.2626 +0.9988053,0.02634388,-0.04115954,-17.45799,-0.02665306,0.9996203,-0.006980972,-8.765928,0.04096001,0.008069659,0.9991281,328.526 +0.9989021,0.02230485,-0.04119556,-17.50324,-0.02268257,0.9997046,-0.008724361,-8.787718,0.0409888,0.009649203,0.999113,329.7779 +0.998932,0.02091916,-0.04119963,-17.55187,-0.02140578,0.9997058,-0.01140579,-8.813935,0.04094891,0.01227552,0.9990858,331.0352 +0.9989785,0.01778614,-0.04154197,-17.60106,-0.0183445,0.9997459,-0.01309853,-8.842975,0.04129844,0.01384722,0.9990508,332.2864 +0.9989957,0.01553856,-0.0420265,-17.65295,-0.01604867,0.9998012,-0.01182766,-8.873266,0.04183436,0.01249025,0.9990464,333.5377 +0.9990152,0.01270819,-0.04251085,-17.70297,-0.01307735,0.999879,-0.008417044,-8.901412,0.04239874,0.008964683,0.9990605,334.7888 +0.9990082,0.009601887,-0.04347953,-17.75287,-0.009809778,0.9999414,-0.004570498,-8.927774,0.04343309,0.00499249,0.9990438,336.0357 +0.9989947,0.005909934,-0.04443872,-17.80347,-0.006160016,0.9999659,-0.005492732,-8.954791,0.04440474,0.005760953,0.998997,337.2744 +0.9989658,0.001783329,-0.04543492,-17.85329,-0.002245747,0.9999462,-0.01012857,-8.980762,0.04541441,0.01022013,0.9989159,338.5021 +0.998926,0.0001870776,-0.04633489,-17.90716,-0.0007272722,0.9999319,-0.0116419,-9.010778,0.04632956,0.01166309,0.9988581,339.7189 +0.9988863,-0.000696159,-0.0471781,-17.96551,0.0002065847,0.9999461,-0.01038124,-9.049782,0.04718278,0.01035993,0.9988325,340.9283 +0.9988532,-0.002368995,-0.04782057,-18.02521,0.002023185,0.9999714,-0.007278509,-9.09124,0.04783644,0.007173412,0.9988294,342.124 +0.9988193,-0.00432383,-0.04838751,-18.08204,0.004187423,0.9999869,-0.002920062,-9.126276,0.0483995,0.002713995,0.9988243,343.3053 +0.9987825,-0.006821854,-0.04885761,-18.13902,0.006822398,0.9999767,-0.0001556329,-9.160291,0.04885753,-0.0001778819,0.9988057,344.4674 +0.9987249,-0.009125774,-0.04965266,-18.19339,0.009116231,0.9999583,-0.0004186719,-9.189315,0.04965441,-3.450643E-05,0.9987664,345.6059 +0.9986706,-0.01065035,-0.05043561,-18.24881,0.01053229,0.9999411,-0.002606057,-9.217085,0.0504604,0.00207139,0.9987239,346.7269 +0.9986685,-0.008934298,-0.05080797,-18.30956,0.008672809,0.999948,-0.00536477,-9.247842,0.05085326,0.004916979,0.998694,347.8273 +0.9986677,-0.008115319,-0.05096063,-18.37352,0.007726522,0.9999395,-0.007821753,-9.282638,0.05102103,0.007417583,0.99867,348.9181 +0.9986986,-0.007058272,-0.0505111,-18.43736,0.006639518,0.9999422,-0.008453354,-9.318634,0.05056784,0.008106983,0.9986877,349.9985 +0.9987875,-0.004679533,-0.04900636,-18.49843,0.0043421,0.9999661,-0.006989703,-9.351679,0.0490374,0.006768437,0.998774,351.0693 +0.9988872,-0.002128075,-0.04711688,-18.55833,0.001799141,0.9999737,-0.007022549,-9.386278,0.04713058,0.006929963,0.9988646,352.1217 +0.998987,-0.002039985,-0.04495479,-18.60997,0.001639083,0.9999586,-0.008952944,-9.417944,0.04497119,0.008870189,0.9989488,353.1589 +0.9990973,-0.003177081,-0.04236243,-18.65783,0.002787789,0.9999534,-0.00924547,-9.450543,0.04238982,0.009119026,0.9990595,354.1778 +0.9992052,-0.002837672,-0.03976083,-18.70101,0.002496541,0.9999597,-0.008626605,-9.481883,0.03978371,0.008520483,0.9991719,355.1795 +0.9993008,-0.004434889,-0.03712585,-18.74328,0.004164894,0.9999643,-0.007346608,-9.516554,0.0371571,0.007186846,0.9992835,356.1601 +0.9993655,-0.006686793,-0.03498642,-18.77927,0.006464418,0.9999582,-0.006465315,-9.54796,0.03502819,0.006235045,0.9993668,357.1136 +0.999437,-0.007334277,-0.03274083,-18.81665,0.007153917,0.9999586,-0.005622488,-9.582259,0.03278071,0.005385097,0.999448,358.0516 +0.9994761,-0.01082458,-0.03050418,-18.84448,0.01072182,0.9999363,-0.003530325,-9.609743,0.03054045,0.003201415,0.9995283,358.9666 +0.9995367,-0.01078223,-0.02846398,-18.87016,0.01075442,0.9999415,-0.001130082,-9.638001,0.0284745,0.0008234456,0.9995941,359.864 +0.999611,-0.009734673,-0.02613566,-18.8963,0.009749024,0.9999524,0.0004216831,-9.667497,0.02613031,-0.0006763156,0.9996583,360.7366 +0.9996683,-0.009513488,-0.02393479,-18.92083,0.009472247,0.9999534,-0.001835878,-9.687682,0.02395114,0.001608554,0.9997118,361.5833 +0.9997257,-0.008421073,-0.02185546,-18.9462,0.008262609,0.999939,-0.007330763,-9.710378,0.02191586,0.007148168,0.9997342,362.4023 +0.9997788,-0.007229136,-0.01975527,-18.96398,0.007019251,0.9999184,-0.01067303,-9.726873,0.01983082,0.010532,0.9997478,363.1942 +0.9998067,-0.007492429,-0.01817881,-18.9779,0.007309196,0.999922,-0.01012514,-9.742016,0.01825325,0.009990312,0.9997834,363.9639 +0.9998368,-0.006850526,-0.01671846,-18.99092,0.006750619,0.999959,-0.006025047,-9.761579,0.01675905,0.005911203,0.999842,364.7112 +0.9998612,-0.006009856,-0.01553951,-19.00849,0.005959839,0.9999769,-0.003263046,-9.780495,0.01555876,0.00316998,0.9998739,365.4351 +0.9998428,-0.004502548,-0.01715475,-19.02819,0.004393412,0.9999699,-0.006394287,-9.797774,0.01718302,0.006317913,0.9998323,366.1228 +0.9997683,-0.003712583,-0.02120419,-19.05061,0.003496189,0.9999415,-0.01023324,-9.812531,0.02124094,0.01015673,0.9997227,366.7844 +0.9996376,-0.002477342,-0.02680715,-19.07614,0.00215607,0.9999256,-0.01200684,-9.827076,0.0268349,0.01194469,0.9995685,367.4159 +0.9994454,0.001690099,-0.03325942,-19.10983,-0.002091983,0.9999252,-0.01205223,-9.842937,0.03323656,0.01211512,0.999374,368.0192 +0.9991562,0.005810746,-0.04066003,-19.15077,-0.006284502,0.9999137,-0.01153352,-9.86144,0.0405895,0.01177931,0.9991064,368.5996 +0.9986087,0.008992317,-0.05196116,-19.19069,-0.009557324,0.9998977,-0.01063539,-9.882918,0.05186021,0.0111172,0.9985924,369.1635 +0.9976379,0.008356618,-0.06818277,-19.23495,-0.008992568,0.9999188,-0.009025541,-9.901782,0.06810181,0.009617358,0.997632,369.7126 +0.996039,0.007496261,-0.08860142,-19.28863,-0.008311506,0.9999264,-0.008835902,-9.91789,0.08852866,0.009537313,0.9960279,370.2326 +0.9932427,0.005537908,-0.1159239,-19.36721,-0.006555477,0.9999432,-0.008398487,-9.928671,0.1158708,0.009101672,0.9932225,370.756 +0.9882676,0.005134749,-0.152646,-19.46935,-0.006181513,0.9999605,-0.006383676,-9.943388,0.1526072,0.007252363,0.9882603,371.2737 +0.9804091,0.006271988,-0.1968722,-19.60077,-0.006879654,0.9999734,-0.002402848,-9.956625,0.1968518,0.003710187,0.9804262,371.7463 +0.9688504,0.006779711,-0.247554,-19.76475,-0.006612999,0.999977,0.001504919,-9.968575,0.2475585,0.000179034,0.9688729,372.2444 +0.9524673,0.008244002,-0.3045293,-19.95933,-0.006586317,0.9999574,0.006470312,-9.981102,0.3045696,-0.004157032,0.9524809,372.738 +0.9307297,0.01125478,-0.3655348,-20.18106,-0.006862256,0.9998878,0.01331369,-9.992426,0.3656436,-0.009883048,0.9307024,373.134 +0.9036257,0.01648839,-0.4280056,-20.44152,-0.008546339,0.9997539,0.02047086,-10.0033,0.4282377,-0.01484011,0.9035442,373.5895 +0.8712325,0.02415245,-0.490276,-20.73791,-0.01141443,0.9995155,0.02895538,-10.00991,0.4907378,-0.01963065,0.8710861,374.0328 +0.8344593,0.03098478,-0.5501978,-21.04624,-0.01309412,0.999251,0.03641429,-10.00255,0.550914,-0.02318188,0.83424,374.3529 +0.7930888,0.03516297,-0.6080903,-21.38775,-0.01309678,0.9990859,0.04069121,-10.0013,0.6089653,-0.02430771,0.7928243,374.742 +0.745796,0.03655331,-0.6651709,-21.74853,-0.008777127,0.998946,0.04505434,-9.994063,0.6661166,-0.02776305,0.7453306,375.1143 +0.6921079,0.03988696,-0.7206912,-22.12778,-0.006476277,0.9987749,0.0490582,-9.981493,0.721765,-0.02928616,0.6915182,375.3538 +0.6333004,0.04475311,-0.7726111,-22.5484,-0.007678504,0.9986408,0.0515518,-9.966478,0.773868,-0.02671527,0.6327832,375.6649 +0.5698802,0.05186144,-0.8200896,-22.99937,-0.009380917,0.9983519,0.05661574,-9.945359,0.8216742,-0.02457099,0.5694275,375.9545 +0.5023263,0.05353638,-0.8630192,-23.45704,-0.0007507291,0.998108,0.0614795,-9.928836,0.8646778,-0.03023487,0.5014161,376.0984 +0.4298491,0.04836477,-0.9016045,-23.93364,0.01304251,0.9981275,0.05976071,-9.900892,0.9028065,-0.03744726,0.4284133,376.3352 +0.3533671,0.0441904,-0.9344404,-24.43257,0.01943524,0.9983213,0.054561,-9.880766,0.9352828,-0.03744113,0.351915,376.5292 +0.2749623,0.03865119,-0.9606778,-24.93449,0.02288038,0.9986456,0.04672752,-9.871383,0.9611827,-0.03482897,0.2737055,376.5567 +0.1981369,0.03442426,-0.9795697,-25.46406,0.02372,0.998922,0.03990218,-9.842394,0.9798873,-0.03114148,0.1971067,376.6584 +0.1254741,0.03099903,-0.9916125,-25.99906,0.02706225,0.9990328,0.03465534,-9.809242,0.9917277,-0.03118361,0.1245138,376.6402 +0.05992767,0.03384527,-0.9976288,-26.55504,0.02617261,0.9990281,0.03546495,-9.7888,0.9978595,-0.02823587,0.0589836,376.6704 +0.002395075,0.04341615,-0.9990542,-27.1276,0.02273981,0.9987963,0.04345947,-9.784421,0.9997385,-0.02282238,0.001404915,376.6575 +-0.04631821,0.05225251,-0.9975592,-27.70081,0.01911529,0.9984944,0.05141396,-9.774028,0.9987438,-0.01668722,-0.0472473,376.5814 +-0.08516605,0.05332009,-0.9949391,-28.27676,0.01746117,0.9984936,0.05201593,-9.75649,0.9962137,-0.0129428,-0.08596878,376.5217 +-0.1163316,0.04865421,-0.992018,-28.85234,0.01333859,0.9987859,0.04742197,-9.73498,0.9931208,-0.007715442,-0.1168394,376.4301 +-0.1384491,0.0494599,-0.9891338,-29.44552,0.008502567,0.9987747,0.04875189,-9.710442,0.990333,-0.001660516,-0.1387,376.3345 +-0.1532994,0.054903,-0.9866534,-30.05628,0.005510044,0.9984873,0.0547054,-9.68216,0.9881644,0.002949803,-0.15337,376.2316 +-0.1619166,0.05884593,-0.9850483,-30.68482,0.003704923,0.9982496,0.05902558,-9.658371,0.9867974,0.005907698,-0.1618512,376.1156 +-0.1648855,0.06130619,-0.9844056,-31.32405,0.002954225,0.9980926,0.06166377,-9.63633,0.9863083,0.007259307,-0.1647521,376.0024 +-0.1663391,0.05661815,-0.9844418,-31.97597,0.0004457854,0.9983544,0.057343,-9.61539,0.9860685,0.009099536,-0.1660906,375.8815 +-0.1667556,0.04476692,-0.9849815,-32.63815,0.002434315,0.9989844,0.04499124,-9.585791,0.9859952,0.005104792,-0.1666953,375.7672 +-0.1662938,0.03659263,-0.9853971,-33.32757,0.006029546,0.9993302,0.03609252,-9.55724,0.9860578,6.046876E-05,-0.166403,375.6519 +-0.1655692,0.03579635,-0.9855483,-34.04157,0.01000601,0.9993505,0.03461669,-9.532153,0.9861474,-0.004129945,-0.1658198,375.5388 +-0.1639134,0.04047041,-0.9856443,-34.78313,0.01204863,0.9991657,0.03902191,-9.517546,0.9864011,-0.005479444,-0.1642642,375.4202 +-0.1614159,0.04175041,-0.986003,-35.53523,0.009953988,0.9991228,0.04067641,-9.501098,0.9868362,-0.003248836,-0.1616899,375.2992 +-0.1573019,0.04215369,-0.9866505,-36.3124,0.006592892,0.9991111,0.04163496,-9.4849,0.9875285,4.43855E-05,-0.15744,375.1747 +-0.1512948,0.04156512,-0.9876144,-37.10423,0.001139337,0.9991222,0.04187491,-9.46332,0.988488,0.005210236,-0.1512094,375.0527 +-0.1439978,0.03933052,-0.9887961,-37.91338,-0.006284182,0.9991534,0.04065766,-9.443687,0.989558,0.01206839,-0.1436287,374.9285 +-0.134455,0.03850764,-0.9901712,-38.74067,-0.01124128,0.999121,0.04038215,-9.419001,0.9908559,0.01656038,-0.133904,374.8155 +-0.1243623,0.04017431,-0.9914233,-39.58997,-0.01474944,0.9989947,0.04233127,-9.392673,0.9921272,0.01988735,-0.1236447,374.7074 +-0.114305,0.04438223,-0.9924538,-40.46144,-0.0207667,0.9986765,0.0470523,-9.365232,0.9932286,0.0259883,-0.113232,374.6064 +-0.1039431,0.04738717,-0.9934537,-41.34951,-0.02500227,0.9984241,0.0502402,-9.336701,0.9942689,0.03006072,-0.1025945,374.5135 +-0.09413498,0.04658164,-0.9944691,-42.25178,-0.02815245,0.9983807,0.04942974,-9.304159,0.9951613,0.03264981,-0.09267115,374.4289 +-0.08551499,0.04463443,-0.9953366,-43.16492,-0.02618221,0.9985504,0.04702801,-9.2727,0.9959928,0.03008171,-0.0842224,374.3425 +-0.07881271,0.0458615,-0.995834,-44.09779,-0.02351866,0.9985776,0.04784919,-9.243094,0.9966119,0.02719181,-0.077622,374.2608 +-0.07371098,0.05013192,-0.9960188,-45.05263,-0.02201048,0.9984107,0.05188121,-9.211296,0.9970367,0.02574707,-0.07249039,374.1861 +-0.07087634,0.05575143,-0.9959259,-46.02671,-0.02075931,0.9981381,0.05735264,-9.169842,0.997269,0.02473968,-0.06958701,374.1085 +-0.06932194,0.05798799,-0.9959076,-47.01136,-0.01807909,0.9980721,0.05937246,-9.119413,0.9974305,0.02212092,-0.06813992,374.0407 +-0.06812015,0.05895223,-0.9959339,-48.00886,-0.01855929,0.998005,0.06034426,-9.069119,0.9975044,0.02259449,-0.06689013,373.9643 +-0.06702925,0.05567479,-0.9961965,-49.0185,-0.02280157,0.9980957,0.05731515,-9.024967,0.9974904,0.02655664,-0.06563213,373.8935 +-0.06595859,0.05147459,-0.9964938,-50.04015,-0.02505128,0.9982683,0.05322442,-8.976937,0.9975078,0.02847405,-0.06455486,373.8239 +-0.06491924,0.04969127,-0.9966526,-51.07674,-0.02596933,0.998337,0.05146684,-8.927044,0.9975525,0.02922359,-0.06352082,373.7545 +-0.06365759,0.04959609,-0.9967387,-52.12515,-0.02711673,0.9983096,0.0514061,-8.879053,0.9976033,0.03030068,-0.06220509,373.6856 +-0.06187467,0.0503909,-0.9968111,-53.18568,-0.02654302,0.9982884,0.05211318,-8.832441,0.9977309,0.02968286,-0.06043123,373.6189 +-0.06006279,0.05094295,-0.9968938,-54.25307,-0.02792894,0.9982201,0.05269345,-8.787266,0.9978038,0.0310071,-0.0585331,373.5542 +-0.05804583,0.05221823,-0.9969473,-55.32812,-0.03023166,0.9980811,0.05403782,-8.742463,0.997856,0.03327604,-0.0563558,373.4872 +-0.05637068,0.05335576,-0.9969832,-56.40627,-0.03248306,0.9979443,0.05524384,-8.699294,0.9978813,0.03549919,-0.05452164,373.4219 +-0.05441224,0.05496525,-0.9970046,-57.48887,-0.03185978,0.9978798,0.05675228,-8.659593,0.9980101,0.03485236,-0.05254569,373.3621 +-0.05359951,0.05482379,-0.9970564,-58.57119,-0.03295094,0.9978508,0.05663885,-8.617614,0.9980187,0.03588976,-0.05167782,373.3061 +-0.05339265,0.05325299,-0.9971526,-59.65341,-0.03265754,0.9979497,0.05504422,-8.582225,0.9980394,0.0355035,-0.05154406,373.2513 +-0.05383101,0.05208171,-0.9971909,-60.74575,-0.03237519,0.9980228,0.05387287,-8.543403,0.998025,0.03518428,-0.05203841,373.1973 +-0.05403428,0.05578783,-0.9969795,-61.8444,-0.03317536,0.9977866,0.05763104,-8.506757,0.9979878,0.0361892,-0.0520639,373.1406 +-0.05456934,0.0596299,-0.9967279,-62.94306,-0.03433121,0.997513,0.06155646,-8.464322,0.9979196,0.03757797,-0.05238646,373.0836 +-0.05554685,0.05964143,-0.9966732,-64.03405,-0.03751243,0.9973849,0.06177468,-8.415622,0.9977511,0.04081902,-0.0531643,373.0231 +-0.05660991,0.05786711,-0.996718,-65.12951,-0.03759763,0.9974872,0.06004719,-8.36828,0.9976882,0.04087349,-0.05429199,372.9642 +-0.05873926,0.05417954,-0.9968021,-66.21957,-0.03431856,0.9978263,0.05625753,-8.326584,0.9976832,0.03751334,-0.05675221,372.9093 +-0.06054844,0.05216995,-0.996801,-67.30807,-0.03498392,0.9979087,0.05435295,-8.286409,0.997552,0.03816299,-0.0585967,372.8504 +-0.06202998,0.05493595,-0.9965613,-68.39938,-0.03163116,0.9978743,0.05697719,-8.242833,0.9975729,0.03505668,-0.06016043,372.7923 +-0.06357276,0.05811517,-0.9962837,-69.48687,-0.031021,0.9977055,0.06017756,-8.202328,0.9974949,0.03473137,-0.0616241,372.732 +-0.06483178,0.05531143,-0.9963622,-70.56942,-0.03037231,0.9978907,0.05737257,-8.15745,0.9974338,0.03398138,-0.06301509,372.6691 +-0.06562999,0.05369456,-0.9963983,-71.6481,-0.03263113,0.9979016,0.0559249,-8.116213,0.9973103,0.03618395,-0.06374016,372.6024 +-0.06539097,0.05450925,-0.9963698,-72.72612,-0.0355289,0.9977466,0.05691631,-8.074099,0.997227,0.03912173,-0.06330695,372.5329 +-0.0643491,0.05715196,-0.9962896,-73.80592,-0.03687505,0.9975407,0.05960545,-8.03482,0.9972459,0.04057378,-0.06208336,372.4658 +-0.06225249,0.05918735,-0.9963039,-74.87976,-0.03805554,0.9973734,0.06162874,-7.995837,0.9973346,0.04175142,-0.05983657,372.4034 +-0.05968061,0.06052164,-0.9963812,-75.94999,-0.03748806,0.9973203,0.06282413,-7.958251,0.9975133,0.04110178,-0.05725184,372.3488 +-0.05671638,0.06026185,-0.99657,-77.01652,-0.03741548,0.9973472,0.06243823,-7.920701,0.997689,0.04082841,-0.0543112,372.2963 +-0.05314911,0.0578694,-0.9969084,-78.07571,-0.03776699,0.9974887,0.0599166,-7.882137,0.9978721,0.04083474,-0.05083008,372.2465 +-0.04943831,0.0550469,-0.9972591,-79.13236,-0.03943482,0.9975939,0.05702034,-7.845224,0.9979983,0.04214572,-0.04714859,372.1988 +-0.04562206,0.05419976,-0.9974874,-80.18446,-0.04058358,0.9976021,0.05606218,-7.809958,0.998134,0.04303927,-0.04331304,372.1566 +-0.0418327,0.05525439,-0.9975956,-81.23435,-0.04186522,0.9974957,0.05700443,-7.776644,0.9982471,0.0441492,-0.0394147,372.1199 +-0.03794368,0.05833302,-0.9975759,-82.27667,-0.04353651,0.9972503,0.05996994,-7.73871,0.998331,0.04570644,-0.03529973,372.0859 +-0.03398374,0.06255745,-0.9974626,-83.30662,-0.04381491,0.9969863,0.06402036,-7.696401,0.9984615,0.04587939,-0.03114037,372.0596 +-0.03022301,0.06508779,-0.9974218,-84.32209,-0.04339181,0.9968514,0.0663654,-7.655593,0.9986008,0.04528569,-0.02730357,372.0376 +-0.02640584,0.06424633,-0.9975847,-85.31838,-0.04202997,0.9969788,0.06531984,-7.620704,0.9987673,0.04365327,-0.02362579,372.0213 +-0.02322094,0.06016838,-0.9979181,-86.29444,-0.04020708,0.9973234,0.06106813,-7.583183,0.9989215,0.04154143,-0.02073959,372.0098 +-0.02051127,0.05752796,-0.9981332,-87.25699,-0.03742752,0.9975992,0.05826631,-7.547871,0.9990888,0.03855277,-0.01830889,372.0031 +-0.01844195,0.05699337,-0.9982042,-88.20287,-0.03422533,0.9977529,0.05759993,-7.51582,0.9992439,0.03522613,-0.01644989,371.9992 +-0.01701258,0.05498548,-0.9983422,-89.12695,-0.03121572,0.9979708,0.05549698,-7.488594,0.9993678,0.03210811,-0.01526165,371.9958 +-0.01583773,0.05432731,-0.9983976,-90.03634,-0.02951956,0.9980621,0.05477734,-7.460881,0.9994387,0.0303398,-0.01420332,371.9926 +-0.01503872,0.05386374,-0.9984351,-90.92745,-0.02696547,0.9981629,0.05425523,-7.429303,0.9995232,0.0277392,-0.01355863,371.9896 +-0.01505443,0.05465576,-0.9983918,-91.81075,-0.02474442,0.9981787,0.05501722,-7.399767,0.9995804,0.02553287,-0.01367459,371.9848 +-0.01702228,0.05289773,-0.9984549,-92.68534,-0.0219567,0.9983389,0.05326593,-7.37024,0.999614,0.02282948,-0.01583255,371.9798 +-0.02079009,0.05255801,-0.9984015,-93.55555,-0.01668562,0.9984599,0.05290855,-7.337048,0.9996446,0.01775892,-0.01988111,371.9749 +-0.02765363,0.05371561,-0.9981733,-94.43191,-0.01223304,0.9984622,0.05407007,-7.30202,0.9995427,0.01370594,-0.02695399,371.962 +-0.03690922,0.05663107,-0.9977127,-95.3032,-0.008694464,0.998337,0.05698816,-7.26387,0.9992808,0.01077797,-0.03635546,371.9349 +-0.04859588,0.05783229,-0.9971429,-96.16931,-0.008287038,0.9982646,0.05830123,-7.227268,0.9987841,0.01109656,-0.04803229,371.8941 +-0.06158758,0.05438623,-0.9966189,-97.02694,-0.008399058,0.9984507,0.05500524,-7.193924,0.9980663,0.0117583,-0.06103537,371.8401 +-0.07517879,0.04825661,-0.9960018,-97.87921,-0.008973064,0.9987551,0.04906731,-7.164166,0.9971297,0.01262601,-0.07465219,371.7755 +-0.08837727,0.04257101,-0.995177,-98.7352,-0.009653659,0.9990028,0.04359198,-7.136757,0.9960403,0.01345964,-0.08787817,371.6994 +-0.09995662,0.03890591,-0.9942309,-99.59406,-0.01347775,0.9990906,0.04045109,-7.113516,0.9949005,0.01744336,-0.09934134,371.6064 +-0.1088161,0.0377883,-0.9933434,-100.4613,-0.01728307,0.9990542,0.03989884,-7.090644,0.9939116,0.02150966,-0.1080601,371.5074 +-0.1146553,0.03829921,-0.9926668,-101.3359,-0.01879305,0.9989941,0.04071398,-7.068917,0.9932275,0.02332331,-0.1138202,371.4057 +-0.118419,0.03736302,-0.9922606,-102.2157,-0.02276589,0.998927,0.04033099,-7.049875,0.9927027,0.02736564,-0.1174413,371.3023 +-0.1194453,0.0357336,-0.9921976,-103.1086,-0.02681924,0.9988713,0.03920258,-7.033049,0.9924784,0.03129255,-0.1183521,371.1939 +-0.1189183,0.03432154,-0.9923107,-104.0061,-0.02865962,0.9988673,0.03798289,-7.012862,0.9924903,0.03295611,-0.1178,371.0901 +-0.1171599,0.03446873,-0.9925147,-104.9115,-0.02775901,0.9988933,0.03796703,-6.992092,0.992725,0.03199944,-0.1160735,370.9916 +-0.1152103,0.03837956,-0.9925994,-105.8331,-0.0274073,0.9987501,0.04179854,-6.96882,0.9929629,0.03202009,-0.1140144,370.8926 +-0.1132829,0.04605843,-0.9924947,-106.7711,-0.02817441,0.9983743,0.04954711,-6.945607,0.9931632,0.03357579,-0.1118011,370.7922 +-0.109981,0.06133441,-0.9920395,-107.7376,-0.03324483,0.9973087,0.06534585,-6.905404,0.9933775,0.04016698,-0.107646,370.6851 +-0.1053865,0.06712731,-0.9921631,-108.7068,-0.03592485,0.9968108,0.07125767,-6.862932,0.9937822,0.0431529,-0.1026389,370.5839 +-0.09967221,0.05957683,-0.9932352,-109.6822,-0.04677153,0.9968219,0.06448556,-6.817108,0.9939204,0.05288254,-0.09656894,370.4778 +-0.09222868,0.04645047,-0.9946539,-110.6676,-0.04708702,0.9975903,0.05095373,-6.764922,0.9946238,0.05153468,-0.08981922,370.3877 +-0.08636797,0.04673647,-0.9951665,-111.6894,-0.04736361,0.9975769,0.05096025,-6.71712,0.9951368,0.051536,-0.08394508,370.3035 +-0.08045849,0.05976597,-0.9949646,-112.7391,-0.04643138,0.9968924,0.06363649,-6.673776,0.9956759,0.05131767,-0.07743344,370.2285 +-0.07531554,0.06328173,-0.9951498,-113.797,-0.04315445,0.9968424,0.06665542,-6.626579,0.9962255,0.04796533,-0.07234683,370.1647 +-0.07069485,0.05461681,-0.9960017,-114.8615,-0.04172866,0.9974638,0.05765884,-6.574629,0.9966247,0.04563799,-0.06823646,370.1005 +-0.06720583,0.04339841,-0.9967949,-115.9316,-0.04322569,0.9979889,0.04636476,-6.529774,0.9968023,0.04620312,-0.06519474,370.0364 +-0.06342834,0.04409115,-0.997012,-117.0364,-0.05068512,0.997592,0.04734132,-6.486705,0.9966984,0.05353645,-0.06104084,369.9626 +-0.05843135,0.05178533,-0.9969474,-118.1603,-0.05784793,0.9967999,0.05516816,-6.449135,0.9966139,0.06089489,-0.05524869,369.8888 +-0.05278949,0.04772985,-0.9974644,-119.2866,-0.06169575,0.996793,0.0509629,-6.410174,0.996698,0.06422961,-0.04967546,369.8286 +-0.04698395,0.03478802,-0.9982897,-120.4129,-0.05820321,0.9976,0.0375033,-6.37032,0.9971985,0.05986571,-0.04484641,369.784 +-0.04200463,0.02656194,-0.9987643,-121.5557,-0.05052751,0.9983109,0.02867491,-6.338214,0.9978389,0.05166955,-0.04059156,369.7516 +-0.03646022,0.03328978,-0.9987805,-122.7295,-0.04548352,0.998354,0.03493593,-6.313111,0.9982995,0.04670182,-0.03488606,369.7246 +-0.03034585,0.0459176,-0.9984842,-123.9184,-0.04307963,0.9979559,0.04720258,-6.285628,0.9986106,0.04444673,-0.02830571,369.7019 +-0.02384468,0.05285803,-0.9983173,-125.1055,-0.04311646,0.9976177,0.05385082,-6.247853,0.9987854,0.04432796,-0.02150883,369.6828 +-0.01840532,0.05286477,-0.9984321,-126.2887,-0.04444159,0.997571,0.05363843,-6.203627,0.9988424,0.04535914,-0.01601121,369.6712 +-0.01432789,0.05015907,-0.9986385,-127.4712,-0.04361592,0.997759,0.05074068,-6.159301,0.9989456,0.04428354,-0.01210805,369.6638 +-0.01135737,0.04839508,-0.9987637,-128.6559,-0.0405292,0.997985,0.04881823,-6.123569,0.9991138,0.04103354,-0.009373068,369.6624 +-0.009321924,0.04825774,-0.9987914,-129.8397,-0.03851285,0.9980764,0.04858265,-6.090263,0.9992146,0.03891919,-0.007445449,369.6623 +-0.008535431,0.05203173,-0.998609,-131.028,-0.03890263,0.997872,0.05232585,-6.051368,0.9992065,0.03929514,-0.006493097,369.6587 +-0.008684802,0.05678546,-0.9983487,-132.2129,-0.03897947,0.9976082,0.05708245,-6.009283,0.9992022,0.03941085,-0.006450564,369.6554 +-0.009643575,0.05484784,-0.9984482,-133.3856,-0.03743381,0.9977749,0.05517242,-5.970026,0.9992525,0.03790777,-0.007568954,369.6509 +-0.01215267,0.04639066,-0.9988495,-134.5543,-0.0304241,0.9984435,0.04674197,-5.935395,0.9994632,0.03095713,-0.01072236,369.6476 +-0.01775549,0.03981287,-0.9990494,-135.7203,-0.02117827,0.9989677,0.04018601,-5.903866,0.999618,0.02187166,-0.01689399,369.646 +-0.02493832,0.04322669,-0.998754,-136.8944,-0.01504131,0.9989354,0.04361013,-5.87957,0.9995758,0.01611014,-0.02426158,369.634 +-0.03210374,0.05034875,-0.9982156,-138.0705,-0.01236244,0.998634,0.05076745,-5.850333,0.999408,0.0139702,-0.03143745,369.6092 +-0.03890683,0.05190424,-0.9978939,-139.2367,-0.01528731,0.9985022,0.05253192,-5.819392,0.9991259,0.01729896,-0.03805507,369.5683 +-0.04448749,0.04905582,-0.9978048,-140.3981,-0.02165725,0.9985115,0.05005617,-5.78416,0.9987751,0.02383658,-0.04335886,369.5144 +-0.04891853,0.04504741,-0.9977864,-141.5541,-0.02718655,0.9985522,0.04641487,-5.75159,0.9984327,0.02939692,-0.04762302,369.4563 +-0.05190322,0.0432241,-0.9977163,-142.7082,-0.02907478,0.9985739,0.0447738,-5.719895,0.9982287,0.03133229,-0.05057247,369.3988 +-0.05405641,0.04415548,-0.9975612,-143.8621,-0.02904287,0.9985296,0.04577215,-5.687958,0.9981154,0.03144632,-0.05269452,369.3405 +-0.05591326,0.05120575,-0.9971217,-145.0234,-0.02512261,0.9982956,0.05267479,-5.651594,0.9981195,0.02799552,-0.05453153,369.2862 +-0.05808629,0.05905198,-0.9965635,-146.1786,-0.0233124,0.9978965,0.06048977,-5.60957,0.9980393,0.02674591,-0.05658746,369.229 +-0.05937891,0.06223616,-0.9962936,-147.3276,-0.02372751,0.9976846,0.06373722,-5.563013,0.9979534,0.02742421,-0.05776471,369.1664 +-0.05957293,0.05518337,-0.9966975,-148.466,-0.02708928,0.9980137,0.05687539,-5.517301,0.9978563,0.03038805,-0.05795971,369.0988 +-0.06005162,0.04671951,-0.9971014,-149.5961,-0.02889519,0.9984041,0.0485208,-5.476203,0.9977769,0.03172518,-0.05860581,369.0322 +-0.06020996,0.04658009,-0.9970983,-150.737,-0.02559081,0.9985102,0.04819136,-5.432874,0.9978576,0.02841815,-0.05892824,368.9711 +-0.06210155,0.0520549,-0.9967115,-151.8818,-0.02377177,0.9982785,0.05361788,-5.397091,0.9977867,0.02702335,-0.0607572,368.9079 +-0.0651492,0.05663029,-0.9962674,-153.0299,-0.01931849,0.9981297,0.05799945,-5.35511,0.9976885,0.023025,-0.06393334,368.8467 +-0.06837115,0.06209834,-0.9957255,-154.1586,-0.01561092,0.9978722,0.06330415,-5.306809,0.9975378,0.01987237,-0.06725625,368.7819 +-0.0711167,0.06356489,-0.9954406,-155.2876,-0.01599787,0.9977663,0.06485634,-5.257637,0.9973397,0.0205373,-0.06994094,368.7083 +-0.07360294,0.06239097,-0.9953341,-156.4144,-0.01863141,0.9977809,0.06392212,-5.210757,0.9971135,0.02324933,-0.07227717,368.6264 +-0.07644561,0.05995589,-0.9952695,-157.5339,-0.02290902,0.9978213,0.06186924,-5.161697,0.9968105,0.02753028,-0.07490552,368.5391 +-0.07877595,0.05565447,-0.9953376,-158.6476,-0.02648469,0.9979711,0.05789786,-5.117612,0.9965404,0.03092217,-0.07714212,368.4516 +-0.08051738,0.05164412,-0.9954144,-159.7555,-0.02508831,0.9982354,0.05381984,-5.077488,0.9964374,0.0293067,-0.07907963,368.368 +-0.08188354,0.05021787,-0.995376,-160.8631,-0.02498632,0.9983124,0.0524215,-5.037247,0.9963286,0.02916324,-0.08049059,368.2819 +-0.08319574,0.05594175,-0.9949618,-161.9767,-0.02253351,0.9980622,0.05800027,-4.99885,0.9962784,0.02724536,-0.08177396,368.1974 +-0.08463348,0.06315798,-0.9944085,-163.0853,-0.02078044,0.9976602,0.06513313,-4.955172,0.9961954,0.02617669,-0.083123,368.1135 +-0.08520815,0.06549488,-0.9942083,-164.1857,-0.02171261,0.9974781,0.06757117,-4.907712,0.9961265,0.02734447,-0.0835712,368.025 +-0.08475432,0.06346121,-0.9943789,-165.2717,-0.02185307,0.9976112,0.06553012,-4.855746,0.9961622,0.02728419,-0.08316504,367.9349 +-0.08492544,0.05926061,-0.9946235,-166.3509,-0.02369381,0.9978274,0.0614746,-4.813324,0.9961055,0.02878717,-0.08333681,367.84 +-0.08508518,0.05853494,-0.9946528,-167.4328,-0.02599631,0.9978026,0.06094411,-4.77256,0.9960344,0.03104274,-0.08337651,367.7486 +-0.08529565,0.06185633,-0.9944338,-168.513,-0.02657477,0.9975747,0.06433112,-4.733896,0.9960012,0.03191402,-0.08344496,367.6578 +-0.08536531,0.06018363,-0.9945304,-169.5842,-0.02262731,0.9977994,0.06232368,-4.69586,0.9960927,0.02782383,-0.08381566,367.5744 +-0.08553232,0.05154417,-0.9950012,-170.6424,-0.0212415,0.9983396,0.05354308,-4.656983,0.9961089,0.02571498,-0.08429542,367.4884 +-0.08517997,0.04735391,-0.9952397,-171.7,-0.01783732,0.9986374,0.04904223,-4.623281,0.9962059,0.02192983,-0.08421923,367.4038 +-0.08642691,0.05169398,-0.9949162,-172.7727,-0.01514604,0.9984693,0.05319432,-4.592437,0.996143,0.01966646,-0.08551165,367.3198 +-0.08622111,0.05622673,-0.9946882,-173.8414,-0.01774023,0.9981612,0.05796082,-4.561133,0.996118,0.02264344,-0.08506508,367.2271 +-0.0848516,0.05286237,-0.9949904,-174.8995,-0.02044776,0.998289,0.05478139,-4.524537,0.9961837,0.02499361,-0.08362549,367.1366 +-0.08296979,0.04564258,-0.9955063,-175.9578,-0.02731173,0.9984712,0.04805481,-4.488888,0.9961777,0.03117609,-0.08159636,367.0444 +-0.07999965,0.04331256,-0.9958535,-177.0237,-0.03433893,0.9983428,0.04617938,-4.457455,0.9962032,0.03789088,-0.07837976,366.9514 +-0.07662911,0.04742926,-0.995931,-178.0974,-0.03877316,0.9979706,0.0505097,-4.42987,0.9963054,0.0424859,-0.07463461,366.8671 +-0.07239201,0.04989966,-0.9961272,-179.172,-0.03915381,0.9978356,0.05283069,-4.398167,0.9966074,0.04282669,-0.07028155,366.7944 +-0.06784459,0.04952118,-0.9964662,-180.2445,-0.03677597,0.9979645,0.05209955,-4.366461,0.9970178,0.04018068,-0.0658853,366.7304 +-0.06303127,0.04821053,-0.9968465,-181.3162,-0.03398891,0.9981494,0.0504227,-4.334028,0.9974326,0.03705992,-0.061276,366.6741 +-0.05849042,0.04595638,-0.9972296,-182.3951,-0.02904653,0.9984385,0.04771576,-4.306321,0.9978653,0.03175698,-0.05706421,366.6251 +-0.05432208,0.04438197,-0.9975367,-183.4739,-0.02470184,0.9986462,0.04577651,-4.28127,0.9982178,0.02712766,-0.05315222,366.5809 +-0.0501602,0.04189294,-0.9978622,-184.5558,-0.01875533,0.9989042,0.04287948,-4.255802,0.998565,0.02086608,-0.04931951,366.5426 +-0.04674467,0.04789234,-0.9977581,-185.6479,-0.01438859,0.9987141,0.04861234,-4.229744,0.9988032,0.0166287,-0.04599545,366.5044 +-0.04405224,0.05520512,-0.9975028,-186.7448,-0.008977824,0.9984099,0.05565181,-4.199807,0.9989888,0.01140699,-0.04348656,366.472 +-0.04257189,0.055931,-0.9975266,-187.8389,-0.007496379,0.9983858,0.05629911,-4.167223,0.9990652,0.009874601,-0.04208388,366.4364 +-0.04107822,0.0539318,-0.9976994,-188.9337,-0.004658798,0.9985209,0.05416804,-4.128173,0.999145,0.00687321,-0.0407662,366.4037 +-0.03996354,0.05439839,-0.9977193,-190.0335,-0.006260723,0.9984837,0.05469085,-4.091253,0.9991815,0.008432088,-0.03956237,366.3673 +-0.03753677,0.05631364,-0.9977073,-191.1399,-0.01094968,0.9983277,0.05676063,-4.0544,0.9992352,0.01305518,-0.03685738,366.3277 +-0.03445172,0.0574369,-0.9977545,-192.2453,-0.01843511,0.9981408,0.05809569,-4.016796,0.9992363,0.02039521,-0.03332881,366.2856 +-0.03037586,0.0546148,-0.9980454,-193.3554,-0.02451624,0.998165,0.05536752,-3.977329,0.9992378,0.02615015,-0.02898116,366.2505 +-0.02585525,0.05262524,-0.9982796,-194.4689,-0.02922115,0.9981469,0.05337507,-3.936207,0.9992385,0.0305509,-0.02426956,366.2225 +-0.02121044,0.0536409,-0.998335,-195.5898,-0.03288489,0.9979819,0.05432061,-3.897886,0.999234,0.0339823,-0.01940366,366.2012 +-0.01661007,0.05503956,-0.998346,-196.7169,-0.03422839,0.9978672,0.05558265,-3.857159,0.999276,0.03509501,-0.01469073,366.1886 +-0.01250041,0.05650884,-0.9983239,-197.8457,-0.03429457,0.9977902,0.05690806,-3.817161,0.9993335,0.03494846,-0.01053484,366.1803 +-0.00885814,0.05899163,-0.9982192,-198.9817,-0.03352108,0.9976797,0.05925722,-3.773023,0.9993987,0.03398629,-0.006860125,366.1789 +-0.006320329,0.06070198,-0.998136,-200.1212,-0.03193904,0.9976343,0.06087372,-3.728399,0.9994698,0.03226424,-0.004366616,366.1826 +-0.004490271,0.06055906,-0.9981545,-201.2603,-0.03094353,0.9976781,0.06066937,-3.68271,0.999511,0.03115885,-0.002605935,366.1882 +-0.002918892,0.05964847,-0.9982152,-202.3996,-0.03120966,0.9977277,0.05971061,-3.636529,0.9995086,0.03132825,-0.001050652,366.1935 +-0.001508157,0.06082858,-0.9981471,-203.5405,-0.03098703,0.997666,0.0608461,-3.588992,0.9995186,0.03102138,0.0003802586,366.1998 +0.0001856796,0.06342025,-0.9979869,-204.6831,-0.03174951,0.9974841,0.0633824,-3.542463,0.9994958,0.03167382,0.002198773,366.2058 +0.001663446,0.06441683,-0.9979217,-205.8219,-0.0315164,0.9974307,0.06433262,-3.494788,0.9995018,0.03134389,0.003689358,366.2146 +0.003112342,0.0616017,-0.998096,-206.9527,-0.03024221,0.99765,0.06147988,-3.446857,0.9995377,0.02999329,0.004967998,366.2275 +0.004506414,0.0607279,-0.9981442,-208.0863,-0.02780278,0.997776,0.06057999,-3.399686,0.9996032,0.02747818,0.006184795,366.2469 +0.006157302,0.06132945,-0.9980986,-209.2212,-0.0280217,0.9977361,0.06113432,-3.356508,0.9995883,0.027592,0.007861916,366.2656 +0.008562885,0.05960993,-0.998185,-210.3479,-0.02745326,0.9978593,0.05935499,-3.315181,0.9995864,0.02689518,0.01018104,366.2855 +0.01137625,0.05687092,-0.9983168,-211.4708,-0.02572292,0.9980676,0.05656362,-3.272869,0.9996043,0.02503614,0.01281715,366.3102 +0.01474288,0.05695463,-0.9982679,-212.5902,-0.02623911,0.9980546,0.05655496,-3.231126,0.9995469,0.02535988,0.01620864,366.3364 +0.01859512,0.06046512,-0.9979971,-213.7062,-0.02621831,0.9978559,0.05996806,-3.189682,0.9994832,0.02505069,0.02014054,366.3659 +0.02285028,0.06370088,-0.9977074,-214.818,-0.02626007,0.9976619,0.06309655,-3.148845,0.9993939,0.02475809,0.02446964,366.4016 +0.02825814,0.06555037,-0.9974491,-215.92,-0.02842379,0.9974967,0.06474826,-3.105251,0.9991964,0.02652162,0.03005059,366.4402 +0.03434996,0.06638344,-0.9972028,-217.0095,-0.03148528,0.9973682,0.06530991,-3.061988,0.9989137,0.02915382,0.03634966,366.4819 +0.04118335,0.06524676,-0.997019,-218.0811,-0.03311736,0.9974064,0.06390416,-3.019584,0.9986026,0.03038685,0.04323733,366.5299 +0.04818115,0.06316292,-0.9968395,-219.1371,-0.03404919,0.9975224,0.06156047,-2.980028,0.9982581,0.03097552,0.05021241,366.588 +0.05499586,0.06242479,-0.9965333,-220.1797,-0.03376172,0.9975893,0.06062774,-2.943384,0.9979156,0.03031041,0.05697085,366.6545 +0.06007425,0.06301128,-0.9962032,-221.2023,-0.03197259,0.997615,0.06117254,-2.91297,0.9976817,0.0281763,0.06194559,366.7266 +0.06368795,0.06233262,-0.9960214,-222.2038,-0.02969695,0.9977239,0.06054029,-2.881915,0.9975279,0.02572311,0.06539407,366.8008 +0.06661529,0.06101816,-0.9959113,-223.1912,-0.02746777,0.9978622,0.05930042,-2.852383,0.9974005,0.02340515,0.0681489,366.879 +0.06926483,0.06225865,-0.9956537,-224.1684,-0.02685242,0.9978054,0.06052516,-2.823349,0.9972368,0.02254345,0.07078461,366.9567 +0.07178189,0.06795909,-0.9951025,-225.1444,-0.02371384,0.9975104,0.06641294,-2.791621,0.9971384,0.01883046,0.07321475,367.0358 +0.07353503,0.07058814,-0.9947914,-226.097,-0.02105086,0.9973796,0.06921572,-2.764003,0.9970704,0.01585144,0.07482827,367.1149 +0.07570471,0.06301778,-0.995137,-227.02,-0.01995351,0.9978968,0.0616746,-2.740336,0.9969306,0.01518742,0.07680291,367.1918 +0.07834039,0.05165111,-0.9955878,-227.9211,-0.01833775,0.9985626,0.0503625,-2.712085,0.996758,0.01431142,0.07917495,367.2699 +0.0810782,0.05167088,-0.9953675,-228.8148,-0.0225274,0.9984952,0.04999827,-2.682417,0.9964531,0.01836928,0.0821202,367.3454 +0.08611021,0.06547295,-0.994132,-229.7008,-0.0274087,0.9976163,0.06332833,-2.658722,0.9959085,0.02179465,0.08769947,367.4212 +0.09178659,0.07491474,-0.9929567,-230.5604,-0.03174086,0.9968794,0.07227666,-2.634793,0.9952727,0.02488327,0.09387801,367.5004 +0.09987472,0.07271169,-0.9923397,-231.3863,-0.0350429,0.9969646,0.06952367,-2.607196,0.9943827,0.0278308,0.1021196,367.584 +0.1089074,0.06874587,-0.9916719,-232.1842,-0.03681593,0.9972003,0.06508592,-2.575303,0.9933699,0.02942099,0.1111334,367.6735 +0.1190556,0.07029089,-0.9903964,-232.9631,-0.03628217,0.9971327,0.06640751,-2.546665,0.9922244,0.02802755,0.1212645,367.7771 +0.13138,0.06850277,-0.9889624,-233.7126,-0.03532543,0.9972995,0.06438741,-2.531497,0.9907024,0.02647631,0.1334451,367.8877 +0.1464772,0.06072771,-0.9873483,-234.4282,-0.03507039,0.9978052,0.05616804,-2.513757,0.9885921,0.02639935,0.1482855,368.0027 +0.1634767,0.0508302,-0.9852369,-235.1142,-0.0330547,0.9983933,0.04602433,-2.492462,0.9859932,0.0250428,0.1648942,368.1319 +0.1796045,0.04702238,-0.9826145,-235.782,-0.02843512,0.9986877,0.04259413,-2.474425,0.9833278,0.02029067,0.1807059,368.2758 +0.1953272,0.05204971,-0.979356,-236.4378,-0.02233086,0.9985678,0.048617,-2.464522,0.9804838,0.01237365,0.1962097,368.4236 +0.2103093,0.05727133,-0.975956,-237.0696,-0.0167003,0.9983474,0.05498657,-2.458118,0.9774922,0.004734573,0.2109182,368.5806 +0.2239254,0.05807196,-0.9728747,-237.6754,-0.01020972,0.9983082,0.05724016,-2.445853,0.9745528,-0.002884739,0.2241394,368.7379 +0.2339965,0.05635645,-0.9706027,-238.259,-0.003327268,0.9983591,0.05716594,-2.431902,0.9722317,-0.01014717,0.2338,368.9 +0.2406247,0.05414283,-0.969107,-238.8228,0.001681016,0.9984182,0.05619781,-2.416555,0.9706167,-0.01515165,0.240153,369.0562 +0.2446073,0.05115017,-0.9682722,-239.3694,0.005455487,0.9985192,0.05412619,-2.403606,0.9696068,-0.01852205,0.2439661,369.2062 +0.2463259,0.04909766,-0.9679427,-239.9009,0.009162731,0.9985534,0.05298212,-2.389863,0.9691437,-0.02191986,0.2455197,369.3516 +0.2461955,0.04952976,-0.9679538,-240.4195,0.01132456,0.9984782,0.05397205,-2.380637,0.969154,-0.02424932,0.2452599,369.4881 +0.2435384,0.050386,-0.9685816,-240.9239,0.01364796,0.9983727,0.05536737,-2.371763,0.9697952,-0.02670323,0.2424544,369.6176 +0.2380427,0.05168521,-0.9698785,-241.4113,0.0142158,0.9982907,0.05668838,-2.367036,0.9711506,-0.02728185,0.2369011,369.7379 +0.2310051,0.0535392,-0.9714784,-241.8871,0.01285419,0.9982297,0.05807007,-2.36207,0.9728676,-0.02590204,0.229908,369.8465 +0.2226901,0.05455254,-0.9733618,-242.3437,0.01211123,0.9982013,0.05871555,-2.356458,0.974814,-0.02486397,0.2216288,369.9466 +0.2128052,0.05333092,-0.9756381,-242.7831,0.01205361,0.99829,0.05719827,-2.350995,0.9770202,-0.02393204,0.2117985,370.0378 +0.2013852,0.05191661,-0.9781353,-243.2045,0.01396549,0.9983407,0.05586438,-2.347796,0.9794125,-0.02491039,0.200326,370.1234 +0.1865563,0.05046099,-0.9811475,-243.6178,0.01630886,0.9983834,0.05444843,-2.342781,0.9823089,-0.02615908,0.1854317,370.2007 +0.1671225,0.04921918,-0.9847069,-244.0346,0.01799519,0.9984345,0.05295945,-2.340872,0.9857719,-0.02657069,0.1659751,370.2666 +0.1453678,0.04995386,-0.9881158,-244.4445,0.01732525,0.9984429,0.05302478,-2.337193,0.9892259,-0.02482744,0.1442759,370.3053 +0.1236647,0.05166661,-0.9909781,-244.8594,0.01468176,0.998439,0.05388776,-2.330489,0.9922154,-0.0212133,0.1227131,370.3406 +0.1027641,0.05339013,-0.9932719,-245.2712,0.01245735,0.9984111,0.05495522,-2.321873,0.9946277,-0.01802095,0.1019357,370.3665 +0.08170161,0.05474606,-0.9951521,-245.6744,0.01097083,0.9983803,0.05582436,-2.312545,0.9965964,-0.01547858,0.08096866,370.3787 +0.06147264,0.05562292,-0.9965577,-246.0773,0.00967903,0.9983658,0.05632089,-2.300412,0.9980618,-0.0131079,0.06083379,370.3917 +0.04220919,0.0561667,-0.9975288,-246.4779,0.008723527,0.9983598,0.05658262,-2.287939,0.9990707,-0.01109027,0.04164998,370.3958 +0.02401014,0.0562621,-0.9981273,-246.8718,0.0069075,0.9983819,0.05644263,-2.274355,0.9996878,-0.008249753,0.02358266,370.3881 +0.008416935,0.05596005,-0.9983976,-247.2677,0.003919355,0.9984234,0.05599455,-2.260463,0.9999569,-0.004384371,0.008184335,370.3798 +-0.003697319,0.05441628,-0.9985115,-247.6659,0.0002329361,0.9985183,0.0544158,-2.246632,0.9999931,-3.139156E-05,-0.003704517,370.3647 +-0.0114074,0.05310846,-0.9985236,-248.075,-0.005003955,0.998573,0.05316826,-2.232871,0.9999224,0.005603083,-0.01112537,370.3492 +-0.01590803,0.0534883,-0.9984418,-248.4935,-0.009471421,0.9985152,0.05364315,-2.218646,0.9998286,0.01031002,-0.0153778,370.3337 +-0.01816045,0.05442387,-0.9983528,-248.9198,-0.01199602,0.9984337,0.0546465,-2.203372,0.9997631,0.01296867,-0.01747914,370.3217 +-0.01727253,0.05529708,-0.9983206,-249.3488,-0.01421731,0.9983549,0.05554498,-2.190826,0.9997497,0.01515284,-0.01645794,370.3141 +-0.01414199,0.05595012,-0.9983334,-249.7837,-0.01757884,0.998265,0.05619531,-2.17685,0.9997454,0.01834426,-0.01313391,370.3066 +-0.01076087,0.05608611,-0.998368,-250.2166,-0.01959791,0.9982221,0.05628916,-2.162385,0.99975,0.02017165,-0.009642567,370.3022 +-0.006530376,0.05609491,-0.9984041,-250.6487,-0.02328184,0.9981462,0.05623271,-2.146243,0.9997076,0.0236119,-0.005212279,370.3001 +0.0009865837,0.055397,-0.9984639,-251.0788,-0.02763669,0.9980845,0.05534866,-2.130858,0.9996175,0.02753964,0.002515682,370.2992 +0.01110841,0.05474909,-0.9984384,-251.5068,-0.03163698,0.9980193,0.05437414,-2.116129,0.9994377,0.03098356,0.0128185,370.3093 +0.02208667,0.05493479,-0.9982457,-251.9337,-0.03387002,0.9979571,0.05416954,-2.100918,0.9991821,0.03261418,0.02390219,370.3279 +0.0330467,0.05613024,-0.9978764,-252.3582,-0.03512301,0.9978702,0.05496673,-2.084785,0.9988364,0.03323196,0.03494778,370.3499 +0.04383583,0.0579222,-0.9973583,-252.7844,-0.03576233,0.997769,0.05637424,-2.069893,0.9983984,0.03319664,0.04580946,370.3789 +0.0542606,0.0600023,-0.9967224,-253.2091,-0.03917454,0.9975523,0.05791965,-2.051974,0.997758,0.03590339,0.05647835,370.4072 +0.06588207,0.06424131,-0.9957573,-253.6321,-0.04243392,0.997203,0.06152704,-2.035602,0.9969247,0.03820036,0.0684238,370.4365 +0.07733276,0.06659113,-0.994779,-254.0521,-0.04505242,0.9969811,0.06323624,-2.021253,0.9959869,0.03992696,0.08009938,370.4768 +0.08870868,0.06515045,-0.9939247,-254.4577,-0.0430427,0.9971772,0.06152206,-2.00952,0.9951271,0.03732366,0.09126251,370.5258 +0.09945177,0.06318996,-0.993034,-254.8506,-0.04008601,0.9974258,0.05945485,-1.999342,0.9942346,0.03389388,0.1017288,370.5818 +0.1100545,0.06195611,-0.9919927,-255.2299,-0.0399693,0.9975238,0.05786727,-1.984999,0.9931215,0.0332807,0.1122583,370.6363 +0.1194436,0.06092159,-0.9909701,-255.594,-0.04267485,0.9975082,0.05617985,-1.969759,0.9919234,0.03557917,0.1217458,370.6831 +0.1274114,0.05910522,-0.9900874,-255.9406,-0.04561685,0.9975158,0.05367839,-1.953994,0.9908004,0.03832542,0.129791,370.7299 +0.1338134,0.05494402,-0.9894823,-256.2695,-0.04556106,0.9977472,0.04924148,-1.939161,0.9899586,0.03849269,0.1360152,370.7808 +0.1387611,0.05043869,-0.9890406,-256.5925,-0.04128269,0.9981286,0.04511026,-1.927459,0.989465,0.03457071,0.1405836,370.8359 +0.1416815,0.04937109,-0.9886804,-256.9157,-0.03462072,0.9983916,0.04489477,-1.922542,0.9893067,0.02786807,0.1431629,370.8956 +0.1412481,0.05016055,-0.9887027,-257.24,-0.03073248,0.9984563,0.04626489,-1.917165,0.9894971,0.02385046,0.1425716,370.9487 +0.1371847,0.05034357,-0.9892654,-257.5605,-0.02927258,0.9984775,0.04675306,-1.908885,0.9901128,0.02254455,0.1384495,370.9918 +0.127825,0.05040243,-0.9905152,-257.8803,-0.02921482,0.9984658,0.04703686,-1.899168,0.9913663,0.02292524,0.1291014,371.0266 +0.1133356,0.05098619,-0.9922477,-258.1997,-0.03062387,0.9983872,0.04780378,-1.887866,0.9930847,0.02496859,0.1147142,371.0539 +0.09502575,0.05086282,-0.9941746,-258.5102,-0.03135418,0.9983513,0.0480796,-1.877972,0.9949809,0.02660273,0.09646383,371.0637 +0.07268064,0.05026437,-0.9960879,-258.8253,-0.03119128,0.9983552,0.04810289,-1.868641,0.9968674,0.02757311,0.0741289,371.0747 +0.04642534,0.04758791,-0.9977876,-259.1424,-0.02876505,0.998514,0.04628418,-1.859898,0.9985075,0.02655265,0.04772522,371.0795 +0.01720013,0.04398321,-0.9988842,-259.4536,-0.02737923,0.9986781,0.04350269,-1.853707,0.9994771,0.02660043,0.01838162,371.0566 +-0.01478251,0.04243455,-0.9989899,-259.7902,-0.0268159,0.9987228,0.04282002,-1.842268,0.999531,0.0274218,-0.01362571,371.0367 +-0.05021,0.04358302,-0.9977873,-260.1398,-0.02860378,0.9985748,0.04505681,-1.829167,0.998329,0.03080279,-0.0488918,371.0027 +-0.08924312,0.04594769,-0.9949495,-260.4827,-0.02995016,0.9983598,0.0487916,-1.812082,0.9955594,0.03415321,-0.08772059,370.9267 +-0.1308267,0.04831922,-0.9902271,-260.8538,-0.02786169,0.9982379,0.05239116,-1.795956,0.9910136,0.03444356,-0.1292499,370.865 +-0.1746393,0.04827663,-0.9834483,-261.2046,-0.02424811,0.9982835,0.05331083,-1.779009,0.9843338,0.03315693,-0.1731689,370.7495 +-0.2209639,0.0486702,-0.9740669,-261.5907,-0.02088376,0.9982889,0.0546179,-1.760933,0.9750583,0.03241076,-0.2195693,370.6494 +-0.268976,0.04992273,-0.9618522,-261.9813,-0.01894212,0.9981884,0.05710573,-1.738415,0.9629606,0.03357959,-0.267543,370.5256 +-0.3176642,0.05004455,-0.9468817,-262.3441,-0.0186219,0.9980844,0.05899808,-1.718545,0.9480204,0.03637432,-0.3161237,370.3254 +-0.3670115,0.04782171,-0.9289864,-262.7437,-0.01517111,0.9982371,0.05738016,-1.694404,0.9300927,0.03515293,-0.3656389,370.1514 +-0.4175198,0.04499654,-0.9075531,-263.1499,-0.01073874,0.9984591,0.05444403,-1.669085,0.9086043,0.03247743,-0.4163932,369.9554 +-0.468187,0.04309669,-0.8825779,-263.5125,-0.004369219,0.9986848,0.05108403,-1.645564,0.8836186,0.02777305,-0.4673829,369.6714 +-0.5195031,0.04482979,-0.8532918,-263.9245,0.0009539145,0.9986525,0.05188592,-1.624289,0.854468,0.02614093,-0.5188458,369.4151 +-0.5698641,0.04830062,-0.8203182,-264.3343,0.007736774,0.9985421,0.05341989,-1.601451,0.8217025,0.02409546,-0.569407,369.1288 +-0.6193761,0.05069385,-0.7834561,-264.6835,0.0114801,0.9983912,0.05552552,-1.575511,0.7850104,0.02539703,-0.6189616,368.7352 +-0.6674588,0.05175911,-0.7428458,-265.0731,0.01454303,0.9982972,0.05649102,-1.549228,0.7445047,0.02690219,-0.6670749,368.3762 +-0.7131119,0.05423873,-0.698949,-265.4532,0.01980517,0.9981633,0.05725143,-1.522155,0.7007705,0.02698387,-0.7128762,367.9838 +-0.7556659,0.05798002,-0.652386,-265.771,0.02607369,0.9979474,0.05848996,-1.493142,0.6544381,0.02718876,-0.7556265,367.4886 +-0.7950333,0.06167903,-0.6034217,-266.1217,0.03260469,0.9977239,0.05902479,-1.467416,0.6056889,0.02725229,-0.7952347,367.028 +-0.8302375,0.06313617,-0.5538226,-266.451,0.03655601,0.9975928,0.05892506,-1.440143,0.5562098,0.02867625,-0.830547,366.5252 +-0.8603971,0.06415426,-0.5055702,-266.7197,0.03958947,0.9974609,0.05919795,-1.408592,0.5080843,0.03091848,-0.8607522,365.9366 +-0.8857291,0.06380009,-0.4597974,-267.0078,0.04074403,0.9973722,0.05990526,-1.374613,0.462411,0.03432583,-0.8860009,365.3626 +-0.9063143,0.06373376,-0.4177708,-267.2518,0.04214039,0.9972649,0.06071993,-1.337219,0.420498,0.03742632,-0.9065211,364.7238 +-0.923149,0.06437941,-0.3790136,-267.5078,0.0443781,0.9971333,0.06128347,-1.302547,0.3818724,0.03975387,-0.9233596,364.0868 +-0.9375857,0.06604607,-0.341425,-267.7469,0.04751933,0.9969222,0.06235444,-1.267399,0.3444924,0.04223834,-0.9378384,363.4232 +-0.9496827,0.0667266,-0.3060235,-267.9495,0.05005857,0.9968197,0.0620039,-1.230084,0.3091876,0.04356492,-0.9500026,362.7093 +-0.9592289,0.06799661,-0.2743292,-268.1502,0.05374968,0.9968022,0.05912945,-1.193968,0.2774726,0.04197356,-0.9598162,361.9941 +-0.966769,0.07137306,-0.2454867,-268.3408,0.06002537,0.9967669,0.05341085,-1.159372,0.2485051,0.03690051,-0.9679274,361.2561 +-0.9729963,0.07289191,-0.2190091,-268.5065,0.06438942,0.9968768,0.04572232,-1.125956,0.2216579,0.03038578,-0.9746509,360.4852 +-0.9782656,0.07602974,-0.1929142,-268.6673,0.06914302,0.9967141,0.04219329,-1.0927,0.1954882,0.02793757,-0.980308,359.6972 +-0.9825147,0.08193307,-0.167188,-268.8089,0.07584885,0.9962145,0.04246907,-1.060003,0.1700347,0.02904546,-0.9850099,358.8705 +-0.9859987,0.08655684,-0.1425292,-268.9415,0.08074269,0.9956686,0.0460941,-1.030964,0.1459016,0.03394053,-0.9887167,358.0268 +-0.9889214,0.08703438,-0.1202482,-269.0509,0.08176028,0.9954893,0.04812819,-0.9979144,0.1238946,0.03776347,-0.9915765,357.1544 +-0.9909708,0.08675687,-0.1022258,-269.1431,0.08235772,0.9955172,0.04650373,-0.9680067,0.1058021,0.03766475,-0.9936736,356.2606 +-0.9927362,0.08048477,-0.08942668,-269.2204,0.07682653,0.9960894,0.04362871,-0.933467,0.09258841,0.03644145,-0.9950373,355.3486 +-0.9938855,0.07320323,-0.08266216,-269.2894,0.0701336,0.9967572,0.03945102,-0.8989503,0.08528203,0.03341239,-0.9957964,354.4158 +-0.9945306,0.06854885,-0.07880321,-269.364,0.06574371,0.9971256,0.03765963,-0.8668551,0.08115822,0.03227284,-0.9961785,353.4604 +-0.9944866,0.0705651,-0.07757015,-269.4488,0.06767538,0.9969341,0.03927423,-0.8347736,0.08010371,0.0338081,-0.996213,352.4773 +-0.9944779,0.07146719,-0.07685189,-269.5388,0.068502,0.9968264,0.04055417,-0.8074597,0.07950628,0.03506571,-0.9962174,351.4737 +-0.9943552,0.07402769,-0.0760116,-269.6285,0.07105876,0.996627,0.0410513,-0.7752988,0.07879414,0.03541827,-0.9962615,350.451 +-0.99443,0.07408382,-0.07497139,-269.718,0.07113855,0.9966141,0.04122504,-0.7432067,0.07777166,0.03566206,-0.9963331,349.4071 +-0.9944729,0.07439307,-0.07409115,-269.8046,0.07134851,0.9965272,0.04292801,-0.7077913,0.07702739,0.03740444,-0.996327,348.3477 +-0.9946195,0.07334779,-0.073159,-269.8981,0.07024942,0.9965557,0.04406477,-0.6752965,0.07613907,0.0386883,-0.9963463,347.2685 +-0.99481,0.07197182,-0.07192516,-269.9857,0.06884473,0.9966096,0.04505235,-0.6396188,0.0749238,0.03986686,-0.996392,346.1777 +-0.9949922,0.07050336,-0.07085136,-270.0781,0.06723888,0.9966078,0.04745234,-0.6044389,0.07395656,0.04245074,-0.9963575,345.0728 +-0.9951524,0.06973214,-0.06934791,-270.167,0.06643746,0.9965997,0.04873468,-0.5681308,0.07251048,0.04389113,-0.9964014,343.9592 +-0.9952719,0.06957271,-0.06777569,-270.2576,0.06643108,0.996657,0.04755634,-0.532915,0.07085774,0.04282907,-0.9965665,342.841 +-0.9953864,0.07009043,-0.06552363,-270.3457,0.06722255,0.9967228,0.0449966,-0.4961709,0.06846273,0.04038433,-0.9968359,341.7088 +-0.995489,0.07098331,-0.06295385,-270.4321,0.06827999,0.9966909,0.04410301,-0.4597162,0.06587611,0.03960557,-0.9970414,340.5645 +-0.9955646,0.07226277,-0.06024401,-270.5181,0.06958564,0.9965417,0.04541347,-0.4241724,0.06331737,0.04101992,-0.99715,339.4089 +-0.995703,0.0728375,-0.05718665,-270.599,0.07015935,0.9964031,0.04752267,-0.3861946,0.06044238,0.04330628,-0.9972318,338.2395 +-0.9957444,0.07481285,-0.05381704,-270.6795,0.07200627,0.9960306,0.05232679,-0.3431655,0.05751814,0.04822894,-0.9971788,337.0523 +-0.9958315,0.07642866,-0.04978319,-270.7545,0.07342864,0.9955218,0.05953562,-0.2933356,0.05411048,0.05563192,-0.996984,335.8508 +-0.9957975,0.07920045,-0.04598588,-270.8291,0.0762405,0.9951034,0.06290113,-0.2407114,0.0507425,0.05913079,-0.9969597,334.652 +-0.9958967,0.07958413,-0.04308389,-270.895,0.07709926,0.9954181,0.05655495,-0.1900227,0.04738736,0.05300114,-0.9974694,333.4617 +-0.9961736,0.0768193,-0.04167822,-270.9555,0.07503229,0.9962591,0.04287041,-0.1489872,0.04481558,0.03957915,-0.9982109,332.2792 +-0.9965104,0.07288765,-0.04067542,-271.0093,0.07163075,0.9969321,0.03154914,-0.1216968,0.04285018,0.02852544,-0.9986741,331.0954 +-0.9968862,0.06917515,-0.03785272,-271.0556,0.0680424,0.997218,0.03043887,-0.1013445,0.03985303,0.02776849,-0.9988196,329.9025 +-0.9971641,0.06728411,-0.03371384,-271.1007,0.06585148,0.9969476,0.04194163,-0.08058966,0.03643294,0.03960258,-0.998551,328.6998 +-0.99737,0.06644121,-0.02896235,-271.1435,0.06486956,0.9965279,0.05219162,-0.05039465,0.03232946,0.05017557,-0.998217,327.5012 +-0.9975588,0.0657496,-0.02352587,-271.176,0.06438128,0.9964178,0.05483248,-0.007761684,0.02704681,0.05318399,-0.9982183,326.3136 +-0.9976775,0.06568276,-0.01804203,-271.2052,0.06463639,0.9964746,0.05348349,0.03310083,0.02149137,0.05219309,-0.9984057,325.1264 +-0.9977647,0.06566128,-0.01242496,-271.2318,0.06493222,0.9965321,0.05203395,0.07494099,0.01579849,0.05111085,-0.998568,323.9446 +-0.9978074,0.06582104,-0.006926571,-271.2514,0.06537422,0.9965053,0.05199474,0.1141247,0.01032471,0.05142791,-0.9986233,322.7624 +-0.9977604,0.06687953,-0.001192345,-271.2677,0.06672501,0.9963904,0.05247749,0.1520703,0.004697711,0.05228039,-0.9986213,321.5873 +-0.9977015,0.06760803,0.004569895,-271.2761,0.0677557,0.9962751,0.05333896,0.189384,-0.0009467305,0.05352599,-0.998566,320.4122 +-0.997534,0.06942821,0.01028616,-271.2806,0.06988797,0.9960649,0.05450084,0.2268718,-0.006461783,0.0550853,-0.9984607,319.2394 +-0.9973632,0.07074716,0.01617177,-271.2752,0.07153995,0.9959052,0.05527077,0.2664564,-0.0121953,0.05628195,-0.9983404,318.0705 +-0.9971886,0.07163696,0.02198084,-271.2636,0.07273679,0.9958795,0.05416084,0.306182,-0.01801035,0.05560738,-0.9982902,316.9064 +-0.9968139,0.07481418,0.027658,-271.249,0.07620951,0.9956604,0.05340825,0.3486559,-0.02354228,0.05534588,-0.9981896,315.7431 +-0.9966498,0.07479605,0.03308823,-271.2229,0.07648599,0.9956527,0.05315615,0.3873162,-0.02896852,0.05550884,-0.9980378,314.582 +-0.9964208,0.07538519,0.03824592,-271.1913,0.07736894,0.995574,0.0533511,0.4270762,-0.03405476,0.05611918,-0.9978431,313.4226 +-0.9961519,0.07641674,0.04291773,-271.1557,0.07860147,0.9955615,0.05175995,0.4663933,-0.03877191,0.05493416,-0.9977369,312.2684 +-0.9956684,0.08010701,0.04719433,-271.12,0.08241492,0.9953843,0.04917223,0.5038023,-0.04303745,0.05284874,-0.9976746,311.1206 +-0.9952743,0.0821614,0.05175631,-271.0773,0.08467576,0.9952314,0.04841867,0.5413927,-0.04753136,0.05257235,-0.9974852,309.9715 +-0.9948169,0.08415365,0.05707466,-271.0307,0.0870482,0.9949345,0.0502786,0.573395,-0.05255442,0.05498623,-0.997103,308.826 +-0.9942616,0.08693081,0.06234615,-270.978,0.09003529,0.9947403,0.04884083,0.6073762,-0.05777245,0.05417391,-0.9968588,307.6838 +-0.9937422,0.08920411,0.06722477,-270.9193,0.09247303,0.9945964,0.04718865,0.6429063,-0.06265209,0.05310982,-0.9966213,306.5447 +-0.9932863,0.0904052,0.07217516,-270.8532,0.0940704,0.9943538,0.04910363,0.6769844,-0.06732842,0.0555635,-0.9961824,305.4053 +-0.9928274,0.09204529,0.076299,-270.7827,0.09606595,0.9940773,0.05080987,0.7114903,-0.07117029,0.05777515,-0.9957895,304.2656 +-0.9924589,0.09280009,0.08008425,-270.7055,0.09694584,0.9940567,0.04952537,0.7448834,-0.07501232,0.05691572,-0.995557,303.1267 +-0.9922473,0.09207426,0.08347329,-270.6208,0.09621119,0.9942524,0.04696378,0.7792041,-0.07866935,0.05463074,-0.9954027,301.9899 +-0.9921506,0.09031474,0.08649043,-270.5286,0.09447878,0.9944948,0.04531846,0.815204,-0.08192136,0.05313423,-0.9952213,300.8531 +-0.9918996,0.09023486,0.08940374,-270.4364,0.09448154,0.9945335,0.04445669,0.84837,-0.08490347,0.05254357,-0.9950027,299.7147 +-0.9916109,0.09047224,0.09231853,-270.3399,0.09489726,0.9944823,0.04471575,0.8804023,-0.0877636,0.05310139,-0.9947249,298.574 +-0.9912128,0.09157351,0.09545401,-270.242,0.09612952,0.9943839,0.04426804,0.9113774,-0.09086415,0.05305499,-0.994449,297.4294 +-0.9907236,0.09327854,0.09882264,-270.1397,0.09785017,0.9942949,0.04246059,0.9429296,-0.09429819,0.05173651,-0.9941987,296.2861 +-0.9903366,0.09374056,0.1022063,-270.0337,0.09837716,0.9942919,0.04129893,0.9728408,-0.09775149,0.0509546,-0.9939055,295.14 +-0.9900694,0.09285004,0.1055531,-269.9196,0.09767389,0.9943537,0.04147806,1.000896,-0.1011058,0.05137592,-0.9935482,293.9899 +-0.9899517,0.09021888,0.1088867,-269.7986,0.09526031,0.9945655,0.04201159,1.02621,-0.1045047,0.05196202,-0.993166,292.8398 +-0.989736,0.08858977,0.1121362,-269.6736,0.09376428,0.9947193,0.04173416,1.050612,-0.1078468,0.05182016,-0.992816,291.6854 +-0.9893787,0.08841039,0.1153841,-269.5452,0.09372535,0.994734,0.04147042,1.077503,-0.11111,0.05184435,-0.9924548,290.5285 +-0.9889695,0.08855236,0.1187342,-269.4146,0.09401464,0.9947175,0.04120975,1.10375,-0.1144578,0.05191793,-0.9920705,289.3691 +-0.9884294,0.0897199,0.1223021,-269.2797,0.09530973,0.9946179,0.04063617,1.127733,-0.117998,0.05182256,-0.9916606,288.2058 +-0.9879254,0.09033917,0.1258663,-269.1417,0.09621368,0.9944997,0.04139025,1.153037,-0.1214348,0.05300053,-0.9911833,287.0421 +-0.9875118,0.08970845,0.12951,-268.9946,0.09589498,0.9944906,0.04233795,1.179905,-0.1249984,0.05422858,-0.9906738,285.8684 +-0.987112,0.08908498,0.1329432,-268.8454,0.09551043,0.9945099,0.04275186,1.208103,-0.1284048,0.05489833,-0.9902011,284.6997 +-0.9867916,0.08733446,0.1364374,-268.6883,0.09396902,0.994648,0.04295568,1.237435,-0.1319557,0.05520918,-0.9897169,283.5282 +-0.986477,0.08557021,0.1397893,-268.5263,0.092266,0.9948427,0.0421303,1.268181,-0.1354632,0.05445836,-0.9892845,282.3529 +-0.9861652,0.08360779,0.1431362,-268.3591,0.090247,0.9950945,0.04052626,1.298323,-0.1390457,0.05288319,-0.9888729,281.1767 +-0.9858601,0.0812992,0.1465279,-268.1889,0.08783796,0.9953823,0.03871024,1.326161,-0.1427042,0.05103358,-0.9884488,279.9981 +-0.9855767,0.07823214,0.1500613,-268.0123,0.0848383,0.9956649,0.03812866,1.352951,-0.1464279,0.05030966,-0.9879411,278.8149 +-0.9851662,0.07565081,0.1540276,-267.8302,0.08263458,0.9957989,0.03944602,1.379858,-0.1503964,0.05158889,-0.9872788,277.6277 +-0.9846767,0.07400812,0.1579074,-267.6449,0.0814272,0.9958342,0.04103436,1.407297,-0.1542127,0.05326352,-0.9866009,276.4384 +-0.9840784,0.07289897,0.1620975,-267.4543,0.08047588,0.9959265,0.0406702,1.433716,-0.1584724,0.05306759,-0.9859362,275.2487 +-0.9833353,0.07280203,0.1665884,-267.2599,0.0803842,0.9959918,0.03922465,1.461436,-0.163065,0.05196205,-0.985246,274.0573 +-0.9824828,0.07347186,0.1712588,-267.0607,0.08113623,0.9959714,0.03818228,1.488638,-0.1677635,0.05140872,-0.9844859,272.8648 +-0.9816678,0.07315541,0.1760021,-266.8545,0.08119869,0.9959373,0.03893088,1.516615,-0.172439,0.05250832,-0.9836196,271.6678 +-0.9808425,0.07149137,0.1812099,-266.639,0.08031775,0.9958909,0.04183787,1.544442,-0.1774742,0.05559073,-0.982554,270.4691 +-0.9799766,0.0697142,0.1865095,-266.4156,0.07928227,0.9958658,0.04433429,1.57224,-0.1826477,0.05823346,-0.9814523,269.2676 +-0.9790573,0.06796493,0.1919053,-266.184,0.07789732,0.9959594,0.04468667,1.602224,-0.1880928,0.0586997,-0.9803955,268.0655 +-0.9780783,0.06652413,0.1973259,-265.9467,0.07653354,0.9961162,0.0435321,1.632894,-0.1936636,0.05767984,-0.9793709,266.863 +-0.9769548,0.06680383,0.202723,-265.7037,0.07659329,0.9962262,0.04082633,1.663368,-0.1992306,0.05541269,-0.9783846,265.6599 +-0.9757858,0.06695569,0.2082284,-265.4539,0.07653733,0.996331,0.03829442,1.691837,-0.2049004,0.05330439,-0.9773302,264.4563 +-0.9745438,0.06813991,0.2135919,-265.1976,0.0777518,0.9962889,0.03691846,1.718526,-0.2102836,0.05258579,-0.9762251,263.2442 +-0.9733848,0.06775471,0.2189324,-264.9337,0.07784777,0.9962484,0.0377984,1.742552,-0.21555,0.05383577,-0.9750076,262.0285 +-0.972363,0.06562223,0.2240627,-264.6581,0.07647466,0.996265,0.04009582,1.765839,-0.2205946,0.05612279,-0.9737495,260.8069 +-0.9713754,0.06313514,0.2290065,-264.3736,0.07477152,0.9962951,0.04248774,1.789773,-0.2254755,0.05839469,-0.9724972,259.5807 +-0.9703849,0.06076794,0.2337956,-264.0836,0.07315597,0.9963193,0.04467645,1.816046,-0.2302202,0.06045689,-0.9712587,258.3561 +-0.9691849,0.06140342,0.238559,-263.7905,0.07405334,0.9962644,0.04442217,1.840519,-0.2349402,0.06071938,-0.9701114,257.1331 +-0.9677367,0.06396998,0.2437081,-263.4962,0.07639038,0.9961993,0.04184884,1.865352,-0.2401048,0.0591156,-0.9689453,255.9137 +-0.9659869,0.06891989,0.2492377,-263.1986,0.08144594,0.9958632,0.04028647,1.889323,-0.2454302,0.05921559,-0.967604,254.6945 +-0.9642653,0.07135705,0.2551483,-262.8927,0.08425482,0.9956423,0.03996848,1.911243,-0.2511844,0.06003768,-0.9660755,253.4794 +-0.9625236,0.07331616,0.2610999,-262.579,0.08662878,0.995444,0.03983185,1.931685,-0.25699,0.06095785,-0.9644896,252.2687 +-0.9608804,0.07433512,0.2668019,-262.2495,0.08825249,0.9952728,0.04054076,1.955211,-0.262527,0.06250074,-0.9628983,251.0443 +-0.9595227,0.0736849,0.2718212,-261.9183,0.0885872,0.995142,0.04294909,1.97913,-0.267336,0.06529049,-0.9613888,249.8414 +-0.9582683,0.07384542,0.2761681,-261.5786,0.08933864,0.9950321,0.04392907,2.003568,-0.2715521,0.0667683,-0.9601049,248.6317 +-0.9570234,0.07528777,0.2800681,-261.2373,0.09044385,0.9950335,0.04157206,2.029564,-0.2755472,0.06511585,-0.9590795,247.4298 +-0.9558055,0.07878243,0.2832475,-260.8939,0.09287582,0.9950027,0.03665509,2.053603,-0.2789443,0.06134197,-0.958346,246.2342 +-0.9547763,0.07959008,0.2864745,-260.5436,0.09275947,0.9951523,0.03267401,2.07395,-0.2824852,0.05776958,-0.9575305,245.038 +-0.9536873,0.08091347,0.289713,-260.1893,0.09395803,0.9950815,0.03137955,2.089988,-0.285749,0.05714713,-0.956599,243.8398 +-0.9525884,0.08069721,0.2933657,-259.8303,0.09502108,0.9948639,0.03488212,2.10921,-0.289044,0.06110421,-0.9553637,242.64 +-0.9516789,0.07867988,0.2968448,-259.4631,0.09437786,0.994776,0.03890429,2.130851,-0.2922331,0.06503996,-0.9541329,241.4419 +-0.9505083,0.07923331,0.3004265,-259.094,0.09526147,0.9946857,0.03905968,2.15664,-0.2957351,0.06574561,-0.9530048,240.2461 +-0.94945,0.07873549,0.3038841,-258.7192,0.09474295,0.9947658,0.03827217,2.1819,-0.2992801,0.06512837,-0.9519399,239.0521 +-0.9483478,0.07882646,0.3072832,-258.3388,0.09486225,0.9947809,0.03757878,2.205692,-0.3027172,0.06478731,-0.9508758,237.858 +-0.946917,0.08023515,0.3113045,-257.9556,0.09603721,0.9947358,0.03574144,2.227076,-0.306798,0.06374098,-0.9496378,236.6662 +-0.9451569,0.08200113,0.3161555,-257.5677,0.09734037,0.9947037,0.03300614,2.247151,-0.3117745,0.06197066,-0.948133,235.4799 +-0.9432132,0.08480714,0.3211802,-257.1743,0.09964282,0.9945707,0.03000716,2.265334,-0.3168916,0.06030644,-0.9465425,234.2908 +-0.9414341,0.08577984,0.3261039,-256.7727,0.1009218,0.9944489,0.02976834,2.283581,-0.3217401,0.06093592,-0.9448651,233.1048 +-0.9397855,0.08502834,0.3310189,-256.3644,0.1013855,0.9943184,0.03243124,2.301503,-0.3263806,0.06403892,-0.9430666,231.9181 +-0.938028,0.08430929,0.3361481,-255.9481,0.1025515,0.9940447,0.03685563,2.322949,-0.3310389,0.06904408,-0.9410877,230.7297 +-0.936141,0.08590669,0.3409693,-255.5313,0.1054889,0.9936445,0.03927555,2.344781,-0.3354282,0.07273592,-0.9392536,229.5485 +-0.9343416,0.08779132,0.3453963,-255.1075,0.1078813,0.9933852,0.03933843,2.368457,-0.339658,0.07401731,-0.937632,228.3692 +-0.9327473,0.09027214,0.3490465,-254.6801,0.1099127,0.9932584,0.03683526,2.391368,-0.3433681,0.07272262,-0.9363811,227.1953 +-0.931323,0.09223402,0.3523216,-254.2484,0.1107796,0.9933037,0.03279723,2.41121,-0.3469373,0.06957485,-0.9353041,226.0281 +-0.9302222,0.09176865,0.3553381,-253.8095,0.1090956,0.9936084,0.02898925,2.427567,-0.3504066,0.06573224,-0.9342882,224.8654 +-0.9294076,0.08965632,0.357999,-253.3654,0.1067946,0.9938769,0.02834753,2.442942,-0.3532654,0.06457877,-0.9332915,223.7046 +-0.9287395,0.08595073,0.3606321,-252.9191,0.1038557,0.9941237,0.03052761,2.45545,-0.355889,0.06580589,-0.9322084,222.544 +-0.92807,0.08336548,0.3629549,-252.4734,0.1021337,0.99423,0.032794,2.469371,-0.3581268,0.06750505,-0.9312294,221.3861 +-0.927291,0.08248526,0.3651405,-252.024,0.1014024,0.9943012,0.03290331,2.489475,-0.3603456,0.06753706,-0.9303707,220.2307 +-0.9262944,0.08303124,0.3675386,-251.5748,0.1015451,0.9943388,0.03128769,2.508039,-0.36286,0.06630333,-0.9294818,219.0796 +-0.9254431,0.08261416,0.3697703,-251.1219,0.1005807,0.9944903,0.02953906,2.5287,-0.3652926,0.06452845,-0.9286535,217.9313 +-0.9245149,0.08284573,0.3720334,-250.6671,0.1010007,0.9944476,0.02954288,2.547424,-0.3675202,0.06488847,-0.927749,216.7823 +-0.9236877,0.08175644,0.3743221,-250.2088,0.1011529,0.9943421,0.03243138,2.567341,-0.3695528,0.06782022,-0.9267314,215.6338 +-0.9227484,0.08028408,0.3769482,-249.7482,0.1011494,0.994225,0.03585359,2.587206,-0.3718928,0.0712119,-0.9255401,214.4875 +-0.9214783,0.08130169,0.3798262,-249.2875,0.1020807,0.9941654,0.03485234,2.60773,-0.3747765,0.0708886,-0.924401,213.3483 +-0.9200681,0.0825747,0.3829572,-248.8235,0.1020702,0.9942993,0.03083245,2.629771,-0.378228,0.06745643,-0.9232514,212.2123 +-0.91868,0.08241647,0.386309,-248.3518,0.1013135,0.9944383,0.02877627,2.649914,-0.3817888,0.06557447,-0.9219204,211.0781 +-0.9173459,0.0818736,0.389581,-247.8753,0.1008063,0.9945016,0.02836578,2.667369,-0.3851165,0.06529342,-0.9205552,209.9454 +-0.9158936,0.08158913,0.3930423,-247.3962,0.1012288,0.9944268,0.0294634,2.684589,-0.3884479,0.06677252,-0.9190482,208.8161 +-0.9143172,0.08071936,0.3968734,-246.9117,0.1016429,0.9943082,0.03193436,2.703437,-0.3920368,0.06953748,-0.9173176,207.6871 +-0.9128017,0.07876962,0.400735,-246.4218,0.1005465,0.9943658,0.03357125,2.72104,-0.3958328,0.07093638,-0.9155787,206.5587 +-0.9113985,0.07527294,0.4045822,-245.9246,0.09692621,0.9947352,0.03327319,2.741308,-0.3999476,0.06953974,-0.9138961,205.4384 +-0.9100804,0.07191683,0.4081443,-245.4226,0.09292987,0.9951627,0.03186295,2.759334,-0.4038785,0.06692663,-0.9123612,204.3216 +-0.9088628,0.06855324,0.4114231,-244.9168,0.08842681,0.9956475,0.02944161,2.775884,-0.407614,0.0631392,-0.9109688,203.2077 +-0.9074337,0.06622012,0.4149446,-244.4075,0.08501849,0.9960142,0.02697335,2.791578,-0.4115045,0.05975448,-0.9094467,202.096 +-0.905728,0.06502386,0.4188421,-243.8972,0.08375218,0.9961351,0.02646373,2.80489,-0.4155025,0.05904786,-0.9076733,200.9836 +-0.9036124,0.0643487,0.4234901,-243.384,0.08444567,0.9960106,0.02884164,2.819677,-0.4199447,0.06182356,-0.9054414,199.8708 +-0.9012472,0.06237515,0.4287925,-242.8649,0.08442637,0.9958969,0.03257936,2.835793,-0.4250009,0.06556343,-0.9028153,198.7617 +-0.8988452,0.06122669,0.4339685,-242.3403,0.08486241,0.9957679,0.03528039,2.855221,-0.4299717,0.06853921,-0.900237,197.6577 +-0.8965096,0.06039429,0.4388886,-241.8132,0.08414526,0.995844,0.03484647,2.873074,-0.4349601,0.06817058,-0.8978655,196.5651 +-0.8942391,0.0605977,0.4434686,-241.279,0.08395396,0.9959163,0.0332034,2.895059,-0.4396455,0.06692271,-0.8956747,195.4744 +-0.8920227,0.06034029,0.4479449,-240.7392,0.08351968,0.9959872,0.03215412,2.915425,-0.4442072,0.0660944,-0.8934827,194.3889 +-0.889888,0.05973277,0.4522515,-240.1964,0.08297114,0.9960475,0.03170432,2.935009,-0.4485702,0.0657371,-0.8913267,193.3099 +-0.8878884,0.05924858,0.4562279,-239.6493,0.0821951,0.9961464,0.0305983,2.952041,-0.4526569,0.06466756,-0.8893367,192.2358 +-0.8860484,0.0581834,0.4599272,-239.0982,0.08078032,0.9962927,0.02958632,2.96859,-0.4565007,0.06336797,-0.8874635,191.1669 +-0.8843,0.05728354,0.4633921,-238.5422,0.07994259,0.9963662,0.02938727,2.983923,-0.4600248,0.06303191,-0.8856659,190.0992 +-0.8825944,0.05557932,0.4668385,-237.9832,0.07920763,0.9963721,0.03112535,2.999715,-0.4634149,0.06444822,-0.8837946,189.0354 +-0.8807099,0.05476385,0.4704797,-237.422,0.0799285,0.9962321,0.0336599,3.017412,-0.4668636,0.06724933,-0.8817686,187.9731 +-0.8785514,0.05724164,0.4742055,-236.8616,0.08366913,0.9958858,0.03479815,3.036881,-0.4702626,0.07024831,-0.8797262,186.9137 +-0.8763699,0.05965879,0.4779297,-236.2989,0.08649903,0.9956604,0.03432562,3.056255,-0.4738078,0.07142238,-0.8777271,185.8588 +-0.8741598,0.06213568,0.481647,-235.734,0.08948327,0.9954081,0.03399232,3.073288,-0.4773232,0.07281406,-0.8757058,184.8044 +-0.8719349,0.06365986,0.4854658,-235.165,0.09210139,0.9951369,0.03492753,3.089834,-0.4808814,0.07516659,-0.8735577,183.752 +-0.8698879,0.06355452,0.4891379,-234.5884,0.09306812,0.9950003,0.03623119,3.10816,-0.4843897,0.07704021,-0.8714536,182.7027 +-0.8677588,0.06630878,0.4925423,-234.0133,0.09519785,0.994885,0.03378215,3.130057,-0.4877828,0.07620371,-0.8696326,181.6605 +-0.8657147,0.06826472,0.4958609,-233.4293,0.09530833,0.9950131,0.02941454,3.152357,-0.4913801,0.07272425,-0.8679036,180.6245 +-0.8637456,0.06785668,0.4993387,-232.8356,0.09460738,0.9951089,0.02842139,3.172514,-0.4949678,0.07178996,-0.8659405,179.5899 +-0.8618721,0.06482011,0.5029662,-232.2316,0.09304495,0.9951734,0.03118614,3.189631,-0.498517,0.07367691,-0.8637432,178.5543 +-0.8601191,0.0625176,0.5062478,-231.6222,0.0906385,0.995399,0.0310716,3.205925,-0.501976,0.07261079,-0.8618281,177.5215 +-0.8582762,0.06278684,0.5093328,-231.0093,0.08974146,0.9955573,0.02849815,3.225534,-0.5052807,0.07016754,-0.8600976,176.491 +-0.856248,0.06138408,0.5129049,-230.3786,0.08736738,0.995819,0.02667298,3.203285,-0.5091231,0.06764983,-0.8580309,175.4065 +-0.8541709,0.06123299,0.5163746,-229.7438,0.08659697,0.995926,0.02514659,3.175702,-0.5127311,0.06619594,-0.8559935,174.321 +-0.851619,0.06591762,0.52,-229.1117,0.09009027,0.9957053,0.02132316,3.160404,-0.5163612,0.06500614,-0.8539,173.2459 +-0.849278,0.06809899,0.5235355,-228.4694,0.09197708,0.9955661,0.01970649,3.139283,-0.5198722,0.06488955,-0.8517759,172.1658 +-0.8464209,0.07071033,0.5277991,-227.8261,0.09616752,0.9951457,0.02090018,3.129332,-0.5237591,0.06844746,-0.8491121,171.0979 +-0.8434195,0.07282976,0.5322965,-227.1763,0.09944667,0.9948114,0.02146052,3.108331,-0.5279716,0.07103531,-0.8462859,170.0218 +-0.8407242,0.07480081,0.5362721,-226.5195,0.101153,0.994673,0.01983954,3.097749,-0.5319313,0.07092509,-0.8438119,168.9598 +-0.8382015,0.07562036,0.5400925,-225.8528,0.1010647,0.9947246,0.0175731,3.085601,-0.5359144,0.06931406,-0.8414221,167.8978 +-0.8357196,0.07602738,0.5438682,-225.1771,0.1015375,0.9946868,0.01697726,3.078616,-0.5396878,0.06941121,-0.8389989,166.8381 +-0.8333601,0.07288267,0.5479043,-224.4955,0.09993827,0.994799,0.01967668,3.072032,-0.5436205,0.07115435,-0.8363096,165.7801 +-0.8307063,0.07190805,0.5520474,-223.8105,0.1008283,0.9946569,0.02216267,3.063938,-0.5475041,0.07407265,-0.8335181,164.7226 +-0.8282706,0.06960754,0.555988,-223.1211,0.09831946,0.9949137,0.02190984,3.059714,-0.551635,0.0728117,-0.8309014,163.6783 +-0.8261623,0.06750987,0.5593731,-222.423,0.09496395,0.995277,0.02013791,3.053051,-0.5553716,0.06975744,-0.8286713,162.6405 +-0.8241182,0.06459648,0.5627225,-221.7245,0.09092672,0.9956788,0.01886719,3.046865,-0.5590721,0.0667153,-0.8264305,161.6121 +-0.8220745,0.0610607,0.5660964,-221.0218,0.08758762,0.9959607,0.01976602,3.035788,-0.5626028,0.06583217,-0.824102,160.5835 +-0.819851,0.05983971,0.5694415,-220.3177,0.08750914,0.9959352,0.02133312,3.028763,-0.5658503,0.0673213,-0.8217549,159.5595 +-0.8174532,0.05927868,0.5729367,-219.6138,0.08706186,0.9959779,0.02116939,3.021908,-0.5693774,0.0671859,-0.8193262,158.543 +-0.8150366,0.05951567,0.5763448,-218.9067,0.08790939,0.9958969,0.02147652,3.020274,-0.5727018,0.06817025,-0.8169243,157.5282 +-0.8127269,0.05723559,0.5798268,-218.1954,0.08672652,0.9959608,0.02324928,3.019379,-0.5761541,0.06918166,-0.8144079,156.5189 +-0.8108704,0.05367352,0.5827593,-217.4781,0.08353454,0.9962041,0.0244799,3.015846,-0.5792332,0.06853054,-0.812276,155.515 +-0.8089763,0.05144277,0.5855861,-216.7583,0.08042198,0.9964824,0.0235621,3.01411,-0.5823141,0.06615516,-0.8102676,154.5161 +-0.8068861,0.05000831,0.5885866,-216.0368,0.07907978,0.9965856,0.02373617,3.013882,-0.5853899,0.06569767,-0.8080856,153.5195 +-0.8046609,0.04975703,0.5916462,-215.3166,0.07965003,0.9965212,0.02452025,3.013904,-0.5883679,0.0668551,-0.8058247,152.5275 +-0.8021477,0.05345632,0.5947282,-214.5989,0.0813903,0.9964774,0.02020923,3.012023,-0.5915529,0.06461588,-0.8036728,151.5422 +-0.7997644,0.05640521,0.5976582,-213.878,0.08119804,0.996591,0.01460093,3.007333,-0.5947972,0.06020597,-0.801618,150.5654 +-0.7977655,0.0557653,0.6003837,-213.1507,0.07977474,0.9967225,0.01342307,2.996851,-0.5976673,0.0586039,-0.7995994,149.5868 +-0.7960572,0.05200307,0.6029831,-212.4194,0.07808361,0.9967998,0.01711881,2.988454,-0.6001632,0.06071064,-0.7975702,148.6134 +-0.7939176,0.05090883,0.6058905,-211.6906,0.07905791,0.9966724,0.01984851,2.984573,-0.6028638,0.0636585,-0.7953004,147.642 +-0.7910368,0.05255806,0.6095067,-210.9622,0.08142709,0.9964836,0.01975132,2.984366,-0.6063253,0.06525437,-0.7925348,146.6808 +-0.7875371,0.05436968,0.6138643,-210.2282,0.08291594,0.9963917,0.0181243,2.984566,-0.6106639,0.06517268,-0.7892034,145.723 +-0.7841139,0.05464721,0.6182064,-209.4877,0.08290003,0.9964117,0.01706861,2.985244,-0.6150553,0.06463305,-0.7858304,144.7707 +-0.7813731,0.0512065,0.6219599,-208.7378,0.08065493,0.9965556,0.01928011,2.986185,-0.6188303,0.06522908,-0.7828117,143.8226 +-0.7787636,0.04893977,0.6254057,-207.9845,0.08104193,0.9964467,0.02293968,2.989498,-0.6220607,0.06854866,-0.7799624,142.8752 +-0.7757137,0.04866184,0.6292061,-207.2294,0.08211089,0.9963299,0.02417535,2.995588,-0.6257204,0.0704178,-0.7768624,141.9322 +-0.7719634,0.05370565,0.6333942,-206.4719,0.08799033,0.9958603,0.02280092,3.001932,-0.6295476,0.07333403,-0.7734933,140.9849 +-0.7683124,0.0560453,0.6376167,-205.7093,0.09028619,0.9956886,0.02127346,3.009769,-0.6336754,0.07391263,-0.77006,140.0446 +-0.7649822,0.05527095,0.6416755,-204.937,0.09089979,0.9956033,0.02261075,3.018818,-0.6376045,0.07562497,-0.7666429,139.1073 +-0.76182,0.05170873,0.6457217,-204.1551,0.08887733,0.9957257,0.02512041,3.027309,-0.6416628,0.07652724,-0.7631595,138.1762 +-0.7588285,0.04967617,0.6493933,-203.3677,0.0867713,0.9959092,0.02521055,3.036529,-0.6454844,0.07547917,-0.7600347,137.2526 +-0.7556051,0.05234984,0.6529323,-202.5764,0.08876994,0.9957891,0.02288997,3.043947,-0.6489845,0.07525652,-0.7570703,136.3244 +-0.7523744,0.05627872,0.6563273,-201.7844,0.09132072,0.9956343,0.01931099,3.049451,-0.6523751,0.07446536,-0.7542291,135.4006 +-0.7493392,0.05979232,0.6594814,-200.9869,0.09374761,0.9954631,0.01626686,3.051527,-0.6555167,0.07401418,-0.7515448,134.476 +-0.7465929,0.06126261,0.6624545,-200.1807,0.09550469,0.9953069,0.01559054,3.054536,-0.6583903,0.07490728,-0.7489399,133.5527 +-0.7439385,0.05974274,0.6655722,-199.3662,0.09549048,0.9952783,0.01739612,3.057003,-0.6613903,0.07649744,-0.7461306,132.6301 +-0.7415943,0.05890962,0.6682572,-198.5452,0.09467978,0.995357,0.01732546,3.05923,-0.6641338,0.07611889,-0.7437285,131.7097 +-0.7390999,0.05876594,0.6710275,-197.7204,0.09580996,0.9952301,0.01837106,3.061735,-0.6667471,0.07786915,-0.7412048,130.7861 +-0.7361423,0.05904089,0.6742468,-196.8909,0.09726284,0.9950762,0.01905694,3.062822,-0.6698018,0.07960776,-0.7382601,129.8617 +-0.7330754,0.06231758,0.6772866,-196.0593,0.1005345,0.9947834,0.01728498,3.066053,-0.6726762,0.08076187,-0.7355162,128.9395 +-0.7296919,0.06621103,0.680563,-195.2214,0.1017888,0.9947292,0.01236097,3.068689,-0.6761574,0.0782934,-0.7325853,128.0235 +-0.7263658,0.06618099,0.6841147,-194.3702,0.1001355,0.9949228,0.01007138,3.068446,-0.6799747,0.07581963,-0.7293049,127.1085 +-0.7236389,0.0634342,0.6872575,-193.5102,0.09674164,0.9952593,0.009999915,3.060761,-0.6833651,0.07372273,-0.726345,126.1968 +-0.7211213,0.06124459,0.6900965,-192.6453,0.0928663,0.9956407,0.008680295,3.050508,-0.6865566,0.07034624,-0.7236653,125.2869 +-0.7184836,0.05993655,0.6929567,-191.775,0.09083111,0.9958338,0.008043515,3.043875,-0.6895875,0.06872114,-0.7209343,124.3786 +-0.7153793,0.05926678,0.6962184,-190.8973,0.09036589,0.9958759,0.008077233,3.035985,-0.6928683,0.06869266,-0.7177846,123.4689 +-0.7121966,0.05794794,0.6995843,-190.0135,0.09067339,0.9958322,0.009821313,3.03043,-0.6960994,0.07042837,-0.7144826,122.5625 +-0.7091331,0.05638011,0.7028169,-189.124,0.09094869,0.9957846,0.01188397,3.026804,-0.6991842,0.07234758,-0.7112715,121.6581 +-0.7060753,0.05580201,0.7059348,-188.2274,0.08966506,0.9959117,0.01095906,3.024492,-0.7024371,0.07103559,-0.708192,120.762 +-0.7027405,0.05698175,0.7091607,-187.3269,0.09068833,0.9958306,0.009851343,3.024239,-0.7056425,0.07123552,-0.704978,119.8668 +-0.6994991,0.05496928,0.7125163,-186.4133,0.0894172,0.9959341,0.01094916,3.024331,-0.7090173,0.07137013,-0.7015701,118.9721 +-0.6960434,0.05358803,0.7159972,-185.497,0.08969192,0.9958891,0.01265609,3.022723,-0.7123756,0.07302833,-0.6979884,118.0768 +-0.6925442,0.05452249,0.7193121,-184.5755,0.0888956,0.9959898,0.01009335,3.022062,-0.7158772,0.07093376,-0.6946137,117.19 +-0.6891017,0.05763781,0.7223689,-183.6521,0.09042922,0.9958796,0.006803434,3.016858,-0.7190003,0.0700115,-0.6914744,116.2999 +-0.6857774,0.05873478,0.7254376,-182.724,0.09103188,0.9958332,0.005427828,3.010089,-0.722096,0.06976022,-0.6882665,115.4147 +-0.6823644,0.05750766,0.7287467,-181.782,0.09204558,0.9957257,0.007611366,3.002371,-0.7251941,0.07227162,-0.684741,114.5233 +-0.6793755,0.05408794,0.7317947,-180.8339,0.09100181,0.9957912,0.01088294,2.995427,-0.7281261,0.07398823,-0.6814382,113.6362 +-0.6765217,0.05046263,0.7346918,-179.8749,0.08828483,0.9960119,0.01288323,2.993221,-0.7311116,0.0735779,-0.6782787,112.7562 +-0.6740094,0.04852953,0.7371271,-178.9094,0.0834617,0.9964534,0.01071258,2.992135,-0.7339929,0.06874224,-0.6756692,111.8817 +-0.6717848,0.047732,0.739207,-177.941,0.07917641,0.9968317,0.007587487,2.989644,-0.7365028,0.0636249,-0.6734356,111.0123 +-0.6697021,0.0460581,0.7412003,-176.9635,0.07596797,0.9970879,0.006680943,2.987661,-0.738734,0.06078171,-0.6712507,110.1297 +-0.6677155,0.04510918,0.7430486,-175.9837,0.074987,0.997161,0.006848612,2.98199,-0.7406301,0.0602919,-0.6692024,109.2458 +-0.6658292,0.04452294,0.7447746,-175.0048,0.07519483,0.9971398,0.007614798,2.975721,-0.7423053,0.06107334,-0.6672726,108.364 +-0.6640916,0.0453608,0.7462739,-174.0227,0.07744914,0.9969616,0.008321798,2.968,-0.7436289,0.0633247,-0.6655869,107.4788 +-0.6626707,0.04511867,0.7475506,-173.0399,0.07867535,0.9968543,0.009576734,2.959505,-0.7447669,0.06516002,-0.6641358,106.5961 +-0.6614379,0.04451722,0.7486776,-172.0539,0.07828727,0.9968818,0.009889102,2.949002,-0.7459028,0.06515294,-0.6628605,105.7095 +-0.6602839,0.04162432,0.7498618,-171.0627,0.07484664,0.9971392,0.01055497,2.940399,-0.7472772,0.0630939,-0.6615103,104.8302 +-0.6595502,0.04039126,0.7505745,-170.0685,0.07234822,0.9973302,0.009904209,2.931385,-0.7481706,0.06083504,-0.6607116,103.9537 +-0.6590549,0.03916434,0.7510744,-169.0696,0.06796986,0.9976583,0.007620064,2.924665,-0.7490171,0.05607245,-0.6601735,103.0802 +-0.6580988,0.03763959,0.7519902,-168.0722,0.06581368,0.9978026,0.007653042,2.915289,-0.7500497,0.05452768,-0.6591298,102.2054 +-0.6573243,0.03385862,0.7528469,-167.0695,0.06493919,0.9978192,0.01182354,2.905433,-0.7508046,0.05666115,-0.6580895,101.3233 +-0.6573715,0.02877692,0.7530171,-166.0661,0.06193829,0.9979528,0.01593383,2.90067,-0.7510169,0.05711502,-0.657808,100.4465 +-0.6579111,0.02461645,0.7526932,-165.0635,0.05799796,0.9981535,0.01805052,2.900265,-0.7508589,0.0555303,-0.658124,99.57208 +-0.6589268,0.02333599,0.7518451,-164.066,0.05588518,0.998275,0.01799373,2.900205,-0.7501282,0.05387354,-0.6590942,98.69821 +-0.6602107,0.02788328,0.7505627,-163.0751,0.06056898,0.9980325,0.01620098,2.902397,-0.7486342,0.05615686,-0.6606006,97.81222 +-0.6611281,0.03475154,0.7494679,-162.0942,0.06764014,0.9976196,0.01340946,2.901447,-0.7472178,0.05955947,-0.6619049,96.92524 +-0.6612025,0.04268276,0.7489923,-161.1164,0.07918623,0.9967737,0.01310168,2.897791,-0.7460166,0.06797273,-0.6624491,96.02783 +-0.6614368,0.04759661,0.7484891,-160.1382,0.08795128,0.9960209,0.01438496,2.895574,-0.7448261,0.0753453,-0.662991,95.1341 +-0.6624632,0.05022449,0.7474089,-159.1581,0.09250161,0.9955982,0.0150861,2.894591,-0.7433612,0.07913049,-0.664193,94.24562 +-0.663803,0.05362105,0.7459828,-158.1795,0.09534047,0.995356,0.01329153,2.891041,-0.7418057,0.07994529,-0.6658325,93.3592 +-0.665135,0.05135706,0.744955,-157.2001,0.09256238,0.9956083,0.01400748,2.885337,-0.740964,0.07827165,-0.6669676,92.47581 +-0.6665791,0.04797982,0.7438886,-156.222,0.09054735,0.9957485,0.01691265,2.88071,-0.7399145,0.07863074,-0.6680895,91.59122 +-0.6676058,0.04677791,0.743044,-155.2468,0.08684505,0.996104,0.01531884,2.87595,-0.7394325,0.07475663,-0.6690672,90.71005 +-0.6687665,0.04700381,0.7419852,-154.2756,0.08391207,0.9963946,0.01251133,2.870667,-0.738722,0.07062865,-0.6702995,89.83086 +-0.6693417,0.0460685,0.7415251,-153.3026,0.08144161,0.9966106,0.01159758,2.863059,-0.7384774,0.06815372,-0.6708249,88.94902 +-0.6694547,0.04905554,0.7412314,-152.3396,0.0836675,0.9964473,0.00961957,2.852516,-0.7381261,0.06845683,-0.6711806,88.06282 +-0.6695601,0.05190292,0.7409423,-151.3751,0.08424368,0.9964251,0.006328182,2.841158,-0.737965,0.06665679,-0.6715389,87.17952 +-0.6695792,0.05575472,0.7406451,-150.4157,0.08743748,0.9961617,0.004058076,2.824285,-0.737576,0.06747733,-0.6718842,86.29097 +-0.6689637,0.05795881,0.7410319,-149.4554,0.09135831,0.9958075,0.004587611,2.807029,-0.7376592,0.07076836,-0.6714541,85.4004 +-0.668249,0.05912149,0.7415848,-148.4922,0.09564737,0.9953918,0.006832918,2.790432,-0.7377634,0.07549671,-0.6708243,84.50752 +-0.667256,0.05929246,0.7424648,-147.5257,0.09942128,0.9949962,0.00989091,2.776141,-0.7381632,0.08041655,-0.6698121,83.6167 +-0.6662122,0.06154282,0.7432186,-146.5607,0.102626,0.9946734,0.009627982,2.762336,-0.7386672,0.08268785,-0.6689794,82.73094 +-0.6651234,0.06343023,0.7440347,-145.5908,0.1037354,0.9945732,0.007944313,2.748604,-0.739493,0.08246669,-0.6680938,81.84727 +-0.6644378,0.06216628,0.7447536,-144.6168,0.1019214,0.9947611,0.007895012,2.733728,-0.740361,0.08115208,-0.6672929,80.96788 +-0.6640393,0.05886572,0.7453769,-143.64,0.09950497,0.9949861,0.01006823,2.7234,-0.7410469,0.08085438,-0.6665673,80.0894 +-0.6638925,0.05477046,0.7458197,-142.6605,0.09705759,0.9951897,0.0133126,2.715466,-0.7415029,0.08122558,-0.6660148,79.21159 +-0.663713,0.05298577,0.7461083,-141.68,0.09530672,0.9953481,0.01409582,2.712409,-0.7418906,0.0804647,-0.6656754,78.33403 +-0.6634247,0.05343482,0.7463327,-140.7016,0.09431897,0.9954627,0.01256967,2.709629,-0.7422746,0.07873234,-0.6654544,77.45701 +-0.6632224,0.05547718,0.7463634,-139.7247,0.09487093,0.9954361,0.01031204,2.707677,-0.742385,0.07764735,-0.6654587,76.58021 +-0.6633414,0.05674721,0.7461622,-138.7449,0.09504863,0.9954338,0.00879378,2.704225,-0.742256,0.07675495,-0.6657061,75.70191 +-0.6633471,0.05754899,0.7460957,-137.7656,0.09572588,0.9953728,0.008332393,2.700066,-0.7421639,0.07694792,-0.6657865,74.82429 +-0.6632971,0.05674598,0.7462016,-136.7842,0.09657945,0.9952734,0.01016226,2.694608,-0.7420979,0.07880833,-0.6656424,73.94395 +-0.6633301,0.05512297,0.7462939,-135.803,0.09543349,0.9953716,0.01130393,2.688508,-0.7422167,0.07871965,-0.6655205,73.06679 +-0.6635306,0.05518775,0.7461109,-134.8206,0.09485516,0.9954333,0.01072704,2.681476,-0.7421116,0.07789017,-0.6657353,72.18649 +-0.6638822,0.05630525,0.7457146,-133.8412,0.09363724,0.9955727,0.008190952,2.672027,-0.7419518,0.07526446,-0.6662152,71.3081 +-0.6640214,0.05748984,0.7455002,-132.8638,0.09138687,0.9958048,0.00460653,2.660835,-0.7421078,0.07118775,-0.6664895,70.43461 +-0.6640526,0.05953934,0.7453115,-131.8869,0.09256415,0.9957024,0.002930296,2.646004,-0.741934,0.07093498,-0.6667099,69.55422 +-0.664014,0.05790765,0.7454745,-130.9107,0.09145885,0.9958003,0.004112123,2.625878,-0.7421056,0.07091073,-0.6665215,68.67674 +-0.6641401,0.05688642,0.7454408,-129.933,0.0920444,0.9957367,0.006018545,2.606906,-0.7419203,0.07261079,-0.6665447,67.79665 +-0.6641263,0.05444551,0.7456353,-128.9577,0.09151295,0.995765,0.008799489,2.589498,-0.7419984,0.07407924,-0.6662961,66.91789 +-0.6643774,0.05181856,0.7455988,-127.9849,0.09015676,0.9958654,0.01112365,2.573845,-0.7419397,0.07461106,-0.6663022,66.04028 +-0.6645529,0.04750411,0.7457297,-127.0093,0.08743156,0.9960655,0.01446325,2.561805,-0.7421086,0.07481189,-0.6660916,65.16549 +-0.6650472,0.04351927,0.7455323,-126.0361,0.08340172,0.9963837,0.01623561,2.552233,-0.7421296,0.0729761,-0.6662717,64.29977 +-0.6652594,0.0425947,0.7453963,-125.0631,0.07964796,0.9967229,0.01412863,2.546862,-0.7423517,0.06876849,-0.6664718,63.43043 +-0.6655621,0.04231182,0.7451422,-124.0943,0.07599076,0.9970449,0.01125928,2.541933,-0.7424638,0.06411766,-0.6668105,62.5652 +-0.6657797,0.04179948,0.7449767,-123.1255,0.07365296,0.9972351,0.00986974,2.53571,-0.7425043,0.0614408,-0.6670174,61.6983 +-0.6658302,0.04103077,0.7449743,-122.1569,0.07118693,0.9974251,0.00868926,2.527256,-0.7426995,0.05881799,-0.6670366,60.83136 +-0.6658005,0.04024219,0.7450438,-121.1858,0.06953849,0.997545,0.008261741,2.518958,-0.7428822,0.05730988,-0.6669644,59.96185 +-0.6658251,0.03906167,0.7450847,-120.2166,0.06941764,0.9975402,0.009736351,2.511323,-0.7428716,0.05820471,-0.6668988,59.09597 +-0.665821,0.03646972,0.7452197,-119.2453,0.06804517,0.9976104,0.01197409,2.503723,-0.7430022,0.05868119,-0.6667115,58.22817 +-0.6656899,0.03525386,0.7453953,-118.2758,0.06840777,0.9975604,0.01391274,2.497254,-0.7430863,0.06025239,-0.6664775,57.35815 +-0.6654022,0.0369447,0.7455703,-117.3104,0.06897261,0.9975448,0.01212565,2.491816,-0.7432918,0.05949235,-0.6663167,56.48854 +-0.6650588,0.04042568,0.745696,-116.3473,0.07110545,0.997425,0.009343898,2.486165,-0.7433981,0.05923728,-0.6662208,55.61772 +-0.6646445,0.04290238,0.745927,-115.3835,0.07334431,0.9972746,0.0079933,2.478395,-0.7435511,0.06002219,-0.6659797,54.74648 +-0.6641626,0.04619433,0.7461596,-114.4194,0.07725433,0.9969865,0.007041784,2.467266,-0.7435858,0.06232093,-0.6657298,53.87296 +-0.6635466,0.04902199,0.7465272,-113.4557,0.07863871,0.9968933,0.004434862,2.454513,-0.7439905,0.06164866,-0.6653401,53.00378 +-0.6630702,0.04790426,0.7470229,-112.4871,0.07988827,0.9967793,0.006989797,2.444569,-0.7442821,0.06431308,-0.6647616,52.13291 +-0.6630652,0.04190315,0.7473879,-111.5106,0.07810416,0.9968551,0.01340237,2.434157,-0.7444758,0.06726074,-0.6642527,51.25861 +-0.6631192,0.03925803,0.7474836,-110.5329,0.07840175,0.9967734,0.01720216,2.42937,-0.7443964,0.07001109,-0.6640575,50.38129 +-0.6625951,0.04077258,0.7478672,-109.5631,0.07952594,0.9967024,0.01611965,2.425984,-0.7447438,0.07015563,-0.6636526,49.5078 +-0.6620984,0.04535741,0.7480431,-108.5957,0.0825209,0.9965094,0.01261673,2.426625,-0.7448597,0.07008269,-0.6635302,48.63615 +-0.6614901,0.04810985,0.7484092,-107.6265,0.08425973,0.9963893,0.01042322,2.423411,-0.7452054,0.0699556,-0.6631553,47.76271 +-0.6611049,0.04801154,0.7487558,-106.6494,0.0859314,0.9962289,0.01199212,2.414926,-0.7453564,0.07226967,-0.6627374,46.88231 +-0.6606446,0.04656725,0.7492531,-105.6724,0.08760845,0.9960368,0.0153424,2.407864,-0.7455692,0.07577676,-0.662106,46.00298 +-0.6599742,0.04632686,0.7498586,-104.6945,0.09100072,0.9956775,0.01857883,2.403883,-0.7457566,0.0804992,-0.6613372,45.12013 +-0.6590679,0.04857629,0.7505131,-103.7172,0.09388745,0.9954197,0.01802019,2.402926,-0.7462001,0.08234027,-0.6606098,44.24062 +-0.6584608,0.05425314,0.7506571,-102.7447,0.09514126,0.9953972,0.01151438,2.392828,-0.7465772,0.07900021,-0.6605916,43.36614 +-0.6583063,0.05893129,0.7504399,-101.7638,0.09486939,0.9954769,0.005048177,2.366051,-0.7467481,0.074517,-0.6609194,42.48734 +-0.6585748,0.05772037,0.7502983,-100.7767,0.09143462,0.9958044,0.003649646,2.330149,-0.7469397,0.07100679,-0.6610893,41.60772 +-0.6590381,0.05354826,0.7502009,-99.78508,0.08953269,0.9959551,0.007563009,2.302773,-0.7467615,0.07215181,-0.6611667,40.7259 +-0.6593094,0.05120638,0.750126,-98.79828,0.0899589,0.9958838,0.01108499,2.278169,-0.7464707,0.07478893,-0.661202,39.84163 +-0.659139,0.05480665,0.7500214,-97.81596,0.09352658,0.995572,0.009443788,2.258319,-0.7461826,0.07637169,-0.6613462,38.95596 +-0.6586548,0.06005571,0.7500449,-96.83488,0.09725792,0.9952428,0.005718807,2.234755,-0.7461332,0.0767145,-0.6613622,38.06946 +-0.6585659,0.06260155,0.7499147,-95.85214,0.09908793,0.9950708,0.003951032,2.210109,-0.7459709,0.0769095,-0.6615227,37.18355 +-0.6588869,0.06147082,0.7497262,-94.86317,0.09739947,0.9952373,0.00399768,2.180517,-0.7459097,0.07565694,-0.661736,36.29944 +-0.6593839,0.06007848,0.7494021,-93.8733,0.09585255,0.9953852,0.004540144,2.156117,-0.7456709,0.07482578,-0.6620996,35.4168 +-0.65989,0.05593187,0.7492775,-92.88044,0.09402347,0.9955337,0.008492277,2.134559,-0.745456,0.07605363,-0.6622017,34.5324 +-0.6600457,0.05197821,0.7494251,-91.88444,0.09356421,0.9955236,0.01335838,2.116878,-0.745376,0.0789365,-0.6619543,33.64306 +-0.6598949,0.05187404,0.7495651,-90.8929,0.09341779,0.9955375,0.01334555,2.105305,-0.7455278,0.07882936,-0.6617961,32.75735 +-0.6597431,0.05447615,0.7495141,-89.90287,0.09288709,0.9956323,0.009397275,2.093797,-0.7457285,0.07581996,-0.6619216,31.87427 +-0.6596977,0.05585996,0.7494523,-88.90972,0.0920468,0.9957314,0.006807015,2.081807,-0.7458729,0.07347524,-0.6620234,30.98976 +-0.6595887,0.0544637,0.7496509,-87.91257,0.09102984,0.995818,0.007745344,2.064689,-0.746094,0.07334933,-0.6617881,30.10303 +-0.6593262,0.05305403,0.7499829,-86.91401,0.09045859,0.9958588,0.009076704,2.048978,-0.7463955,0.07382689,-0.6613949,29.21299 +-0.6587683,0.05302189,0.7504753,-85.91748,0.09150735,0.9957544,0.009974143,2.035368,-0.7467602,0.07524464,-0.6608232,28.32268 +-0.6583478,0.05122249,0.750969,-84.92133,0.09112825,0.9957672,0.01196906,2.024104,-0.7471772,0.07631427,-0.6602289,27.43622 +-0.6582001,0.05117761,0.7511015,-83.92618,0.09106262,0.9957734,0.01195059,2.012668,-0.7473153,0.07626314,-0.6600785,26.55097 +-0.6579552,0.05121636,0.7513134,-82.92905,0.09010141,0.9958716,0.01101771,1.999986,-0.7476474,0.07494354,-0.6598536,25.66841 +-0.6578773,0.04960454,0.7514898,-81.93302,0.08717611,0.9961369,0.01056336,1.986745,-0.7480627,0.07246133,-0.6596601,24.79002 +-0.6576945,0.04789214,0.7517608,-80.937,0.08503151,0.9963184,0.01091959,1.973457,-0.7484701,0.07110509,-0.6593455,23.91304 +-0.6578618,0.04506581,0.7517892,-79.94198,0.08283025,0.9964821,0.01274767,1.961549,-0.74857,0.07065707,-0.6592803,23.03803 +-0.6578705,0.04277067,0.7519157,-78.94882,0.0811344,0.9966006,0.01429766,1.949545,-0.7487481,0.07041222,-0.6591042,22.16558 +-0.6576423,0.04072858,0.7522286,-77.95447,0.08046116,0.9966231,0.01638279,1.940907,-0.749021,0.07129918,-0.6586986,21.29083 +-0.6575517,0.03873549,0.7524131,-76.96272,0.07834052,0.9967792,0.01714775,1.932076,-0.7493254,0.07021995,-0.6584683,20.42165 +-0.6574607,0.03766221,0.752547,-75.97455,0.076553,0.9969208,0.01698809,1.924658,-0.7495899,0.06877872,-0.6583193,19.55456 +-0.6577693,0.03649444,0.7523349,-74.98848,0.07570151,0.9969712,0.01782479,1.917272,-0.7494056,0.06867747,-0.6585397,18.68698 +-0.658208,0.03568736,0.7519898,-74.00752,0.07542237,0.9969762,0.01870257,1.91221,-0.7490485,0.06902702,-0.6589093,17.82351 +-0.6587614,0.03685501,0.7514488,-73.02762,0.07717065,0.9968413,0.01876167,1.907459,-0.7483837,0.07034923,-0.6595246,16.95634 +-0.6593925,0.03966827,0.7507517,-72.05728,0.08006036,0.9966336,0.01765758,1.90321,-0.7475239,0.0717487,-0.6603485,16.09136 +-0.6602288,0.04350804,0.7498034,-71.0924,0.08413647,0.9963213,0.01627269,1.898035,-0.7463371,0.07382949,-0.6614606,15.2269 +-0.6609705,0.04653753,0.7489675,-70.12924,0.08697651,0.9960994,0.01486431,1.891136,-0.7453544,0.07496744,-0.6624399,14.36346 +-0.6617324,0.04893717,0.7481413,-69.1699,0.09005831,0.9958307,0.01451778,1.883908,-0.7443115,0.07698321,-0.6633806,13.50034 +-0.6625974,0.04975717,0.7473212,-68.20924,0.09321432,0.9955115,0.01636472,1.876891,-0.7431526,0.08050424,-0.6642614,12.6345 +-0.66358,0.04724596,0.746612,-67.25104,0.09329125,0.9954395,0.01992428,1.871594,-0.7422656,0.0828737,-0.6649613,11.77228 +-0.6646051,0.04805603,0.7456479,-66.29969,0.09520942,0.9952416,0.02071928,1.869532,-0.741104,0.08476282,-0.666018,10.91092 +-0.6656147,0.05143551,0.744521,-65.35425,0.09718017,0.9951016,0.01813379,1.869446,-0.7399412,0.08442277,-0.6673527,10.05197 +-0.6665572,0.05529758,0.7434001,-64.41531,0.0972752,0.99517,0.01319475,1.862267,-0.7390798,0.08110943,-0.6687168,9.197751 +-0.6667926,0.06041853,0.7427902,-63.47903,0.0996696,0.9949839,0.008540062,1.854003,-0.7385483,0.07972803,-0.6694698,8.339491 +-0.6672422,0.06093008,0.7423445,-62.54235,0.1015837,0.9947801,0.009657136,1.842782,-0.7378811,0.08185377,-0.6699487,7.481356 +-0.6673759,0.05965611,0.7423279,-61.60909,0.1030592,0.9945938,0.01272434,1.830242,-0.7375556,0.08499565,-0.669916,6.622286 +-0.6677271,0.05937361,0.7420346,-60.6798,0.1035762,0.9945281,0.01362722,1.820919,-0.7371652,0.08595635,-0.670223,5.771217 +-0.6679546,0.05880627,0.741875,-59.75404,0.103265,0.9945533,0.0141403,1.812094,-0.7370027,0.08605477,-0.670389,4.927046 +-0.6682192,0.05736298,0.7417498,-58.83165,0.1030929,0.9945436,0.01596047,1.802791,-0.7367869,0.08713423,-0.6704868,4.084132 +-0.6684938,0.05498557,0.7416823,-57.9151,0.1016931,0.9946544,0.01791808,1.793339,-0.7367323,0.08740205,-0.6705119,3.248503 +-0.6686946,0.0539904,0.7415744,-57.00918,0.1007021,0.9947468,0.01838277,1.785299,-0.7366862,0.08697058,-0.6706187,2.422195 +-0.6686476,0.05362004,0.7416436,-56.11243,0.09891814,0.9949461,0.01724861,1.778582,-0.7369705,0.08489523,-0.6705723,1.607982 +-0.6682577,0.05207806,0.7421048,-55.22177,0.09666462,0.9951682,0.01720839,1.773091,-0.7376229,0.0832349,-0.6700629,0.8039621 +-0.6673516,0.04961403,0.7430884,-54.34009,0.09436045,0.9953701,0.01828485,1.766582,-0.7387408,0.08232056,-0.6689434,0.009828463 +-0.666395,0.04659512,0.7441416,-53.46936,0.09337165,0.9954037,0.0212882,1.761258,-0.7397293,0.08366806,-0.6676826,-0.7739216 +-0.6653764,0.04374578,0.7452253,-52.61116,0.09103034,0.9955863,0.02283434,1.754369,-0.7409371,0.08303152,-0.6664217,-1.543085 +-0.6641664,0.04330859,0.7463293,-51.76699,0.08801003,0.995908,0.0205297,1.747753,-0.7423862,0.07931959,-0.6652601,-2.296378 +-0.662994,0.0449382,0.7472747,-50.92493,0.08605109,0.996155,0.01644099,1.742437,-0.7436626,0.07520407,-0.6643118,-3.045921 +-0.6618748,0.04627638,0.7481847,-50.08888,0.08483725,0.9963043,0.01342753,1.736205,-0.7447982,0.07236126,-0.6633546,-3.789114 +-0.6607855,0.04697223,0.7491036,-49.2609,0.08571325,0.9962332,0.0131394,1.727906,-0.7456646,0.07289041,-0.6623225,-4.526641 +-0.6591799,0.0479563,0.7504546,-48.44466,0.08831194,0.9959955,0.01392379,1.718201,-0.7467817,0.07545237,-0.6607753,-5.256118 +-0.6575982,0.05020516,0.7516941,-47.64379,0.0920694,0.9956535,0.01404523,1.707765,-0.7477217,0.07844413,-0.6593623,-5.973092 +-0.6559614,0.05319967,0.7529173,-46.8538,0.09672841,0.995213,0.0139525,1.699326,-0.7485708,0.08198078,-0.6579672,-6.677798 +-0.6544574,0.05431363,0.7541456,-46.07808,0.09906497,0.994978,0.01431144,1.689784,-0.749581,0.08407562,-0.6565513,-7.365478 +-0.6530861,0.05373542,0.7553749,-45.31586,0.09887297,0.9949914,0.01470298,1.680201,-0.7508014,0.08428845,-0.655128,-8.034447 +-0.6510931,0.05477019,0.7570192,-44.56973,0.09858095,0.9950467,0.01279553,1.669438,-0.7525686,0.08295873,-0.6532673,-8.685225 +-0.6485598,0.0538564,0.7592561,-43.83554,0.09763225,0.9951401,0.01280948,1.65733,-0.7548762,0.08243557,-0.6506659,-9.31846 +-0.6455813,0.05021203,0.7620392,-43.11323,0.095336,0.9953293,0.01518243,1.645713,-0.7577175,0.08245123,-0.6473529,-9.933193 +-0.6419473,0.04605628,0.7653643,-42.40432,0.09258995,0.995546,0.01775195,1.633933,-0.7611378,0.08226084,-0.6433524,-10.52834 +-0.637728,0.04403903,0.7690017,-41.71061,0.09056568,0.9957263,0.01808246,1.622183,-0.7649189,0.08117684,-0.6389909,-11.10464 +-0.6328777,0.04342123,0.7730333,-41.03145,0.08896073,0.9958919,0.01689242,1.611745,-0.769124,0.07946042,-0.6341405,-11.66237 +-0.6264748,0.04419869,0.7781875,-40.3642,0.08819958,0.9959982,0.01443481,1.603231,-0.7744354,0.07767884,-0.627866,-12.19871 +-0.6177586,0.04519401,0.7850681,-39.70295,0.0873234,0.9961151,0.01137013,1.594994,-0.7815043,0.0755788,-0.6193051,-12.717 +-0.6070334,0.04513872,0.7933934,-39.04694,0.08606579,0.9962472,0.009170065,1.585238,-0.790002,0.07385054,-0.6086402,-13.21425 +-0.5938738,0.04366894,0.8033722,-38.3898,0.08491802,0.9963507,0.008614927,1.572992,-0.8000642,0.07333694,-0.5954148,-13.69543 +-0.5786913,0.04197681,0.8144657,-37.73783,0.08470277,0.9963671,0.00883085,1.559272,-0.8111361,0.07409782,-0.5801445,-14.1554 +-0.5607749,0.04018023,0.8269928,-37.08227,0.08268049,0.9965467,0.007646567,1.543363,-0.8238297,0.07266416,-0.5621605,-14.59178 +-0.5406775,0.03831314,0.8403571,-36.42322,0.07999977,0.9967766,0.006026508,1.527558,-0.8374174,0.07048675,-0.5419997,-15.00371 +-0.5175644,0.03673824,0.8548552,-35.76383,0.07796871,0.9969462,0.004360704,1.510807,-0.8520845,0.06890889,-0.5188483,-15.39289 +-0.49105,0.03479733,0.8704361,-35.09735,0.07614864,0.9970916,0.003098053,1.49215,-0.8677968,0.06780381,-0.4922716,-15.76067 +-0.4622563,0.03188771,0.8861728,-34.4232,0.07348991,0.9972929,0.002448487,1.471252,-0.8836958,0.06625658,-0.4633484,-16.0979 +-0.4296838,0.02973698,0.9024897,-33.74281,0.06929032,0.9975965,0.0001190177,1.450101,-0.900317,0.06258492,-0.4307115,-16.40328 +-0.3931403,0.02833074,0.919042,-33.05817,0.06384982,0.9979535,-0.003450153,1.428664,-0.9172589,0.05732426,-0.3941446,-16.67333 +-0.3533077,0.02621879,0.9351397,-32.36465,0.06102623,0.998124,-0.004928224,1.409914,-0.9335145,0.05532686,-0.3542449,-16.91383 +-0.3110711,0.023032,0.9501076,-31.66962,0.05570635,0.9984294,-0.005964802,1.390296,-0.9487526,0.05107153,-0.3118655,-17.11651 +-0.2682452,0.02007882,0.9631414,-30.97187,0.04865687,0.9987891,-0.00727052,1.365556,-0.9621211,0.04491316,-0.2688973,-17.28046 +-0.2234724,0.01781507,0.9745475,-30.26512,0.04310684,0.9990353,-0.008377942,1.342013,-0.9737565,0.04013741,-0.2240248,-17.41174 +-0.1752445,0.01805725,0.9843594,-29.56612,0.04275914,0.9990279,-0.01071397,1.321679,-0.9835959,0.04021278,-0.1758463,-17.51437 +-0.1248458,0.01637157,0.9920411,-28.86576,0.04147107,0.9990761,-0.01126865,1.300262,-0.991309,0.03973415,-0.1254094,-17.58161 +-0.07305942,0.01211709,0.997254,-28.16342,0.03842559,0.9992179,-0.009325876,1.27782,-0.996587,0.03763872,-0.07346789,-17.60491 +-0.01996364,0.009462323,0.999756,-27.4708,0.03703314,0.999276,-0.008718289,1.254902,-0.9991146,0.03685005,-0.0202996,-17.59454 +0.03381026,0.01136931,0.9993636,-26.78497,0.03532384,0.9992969,-0.01256363,1.229833,-0.9988038,0.03572613,0.03338488,-17.54516 +0.08742328,0.01297186,0.9960868,-26.11056,0.03242623,0.9993483,-0.01586028,1.20442,-0.9956433,0.03368589,0.08694567,-17.45996 +0.1404157,0.01550981,0.9899712,-25.44513,0.03019355,0.9993452,-0.01993927,1.181948,-0.9896321,0.03269052,0.1398555,-17.33697 +0.1933578,0.01868756,0.9809504,-24.78637,0.02763709,0.9993181,-0.0244851,1.158174,-0.9807389,0.03184499,0.1927095,-17.18003 +0.2451349,0.01766463,0.9693281,-24.14336,0.02418742,0.9994113,-0.02432966,1.134314,-0.9691872,0.02940958,0.2445633,-16.98722 +0.2956025,0.01784049,0.9551445,-23.51365,0.0188587,0.9995218,-0.02450586,1.108388,-0.9551248,0.02525677,0.2951247,-16.75975 +0.3439077,0.01630411,0.9388619,-22.89539,0.01360936,0.9996577,-0.02234503,1.080425,-0.9389048,0.02046193,0.343568,-16.50174 +0.3890033,0.01512743,0.9211122,-22.29698,0.008711438,0.99976,-0.02009808,1.047658,-0.9211951,0.01584243,0.3887781,-16.2178 +0.4304391,0.01590783,0.9024795,-21.71314,0.005088844,0.999786,-0.02005017,1.016386,-0.9026052,0.01322295,0.430266,-15.91129 +0.4666966,0.02095507,0.8841692,-21.15192,-0.001863376,0.9997403,-0.02271059,0.9883421,-0.8844155,0.008951411,0.4666144,-15.57956 +0.4956295,0.02840417,0.8680695,-20.60465,-0.009714982,0.9995839,-0.02716064,0.9603896,-0.8684797,0.00502833,0.4956991,-15.2314 +0.5178157,0.03657697,0.8547099,-20.06747,-0.01796443,0.9993301,-0.03188241,0.9316118,-0.8553035,0.001154836,0.5181259,-14.87238 +0.533785,0.04167422,0.8445928,-19.53624,-0.02261891,0.9991311,-0.0350043,0.9003617,-0.8453177,-0.0004190027,0.5342638,-14.51275 +0.5448956,0.04281668,0.83741,-19.00891,-0.02351869,0.9990829,-0.03577959,0.8643181,-0.8381739,-0.0001986525,0.5454029,-14.15364 +0.5519991,0.04184372,0.8327942,-18.47786,-0.02258719,0.9991239,-0.03522955,0.8290798,-0.8335387,0.0006361923,0.5524606,-13.79381 +0.5556621,0.04050173,0.8304212,-17.9524,-0.02166653,0.9991789,-0.0342347,0.791861,-0.8311259,0.00103057,0.5560833,-13.43935 +0.556992,0.038583,0.8296212,-17.42718,-0.01938938,0.9992521,-0.03345433,0.7543507,-0.8302915,0.002547958,0.5573235,-13.09013 +0.5573918,0.03696434,0.8294264,-16.90498,-0.01759024,0.9993099,-0.03271439,0.715345,-0.8300632,0.003644921,0.5576573,-12.74653 +0.557095,0.0351721,0.8297036,-16.38799,-0.01494201,0.9993655,-0.03233163,0.6799779,-0.8303143,0.005614345,0.5572671,-12.40644 +0.5554371,0.03320217,0.8308955,-15.87464,-0.0124606,0.9994227,-0.03160678,0.6482271,-0.8314651,0.007202121,0.5555302,-12.06932 +0.5516085,0.03203289,0.8334879,-15.36708,-0.01066301,0.9994514,-0.03135441,0.616413,-0.834035,0.008407861,0.5516474,-11.73945 +0.5462558,0.03078565,0.8370525,-14.8631,-0.00920679,0.9994846,-0.03075139,0.5853326,-0.8375678,0.009091552,0.5462576,-11.41748 +0.5400629,0.02855788,0.8411401,-14.36358,-0.00618586,0.9995318,-0.02996381,0.5539444,-0.8416019,0.01097916,0.5399866,-11.10693 +0.5343926,0.02635277,0.8448255,-13.87316,-0.001770812,0.9995465,-0.0300589,0.5226973,-0.8452345,0.01456722,0.5341969,-10.80922 +0.5292431,0.02513356,0.848098,-13.39333,0.001918343,0.9995231,-0.0308182,0.4936346,-0.8484681,0.01793725,0.5289424,-10.52211 +0.5235449,0.02440251,0.8516486,-12.92267,0.004403922,0.9994989,-0.03134619,0.4645181,-0.8519867,0.02016173,0.5231751,-10.24262 +0.5177704,0.02466935,0.8551639,-12.46406,0.005605283,0.9994649,-0.03222587,0.436496,-0.8555012,0.02147903,0.517355,-9.97285 +0.512507,0.02446316,0.8583346,-12.01256,0.007010353,0.9994416,-0.03267066,0.4084713,-0.8586544,0.02276116,0.5120492,-9.712136 +0.5074175,0.02386879,0.8613697,-11.57478,0.009189271,0.9994095,-0.03310715,0.3801443,-0.8616513,0.0247145,0.5068985,-9.466819 +0.50193,0.02165062,0.8646372,-11.14413,0.01258254,0.999398,-0.03232934,0.35139,-0.8648167,0.02710639,0.5013554,-9.231205 +0.4966777,0.01906779,0.8677256,-10.72406,0.0157659,0.9993955,-0.03098542,0.3234019,-0.8677918,0.02907023,0.4960768,-9.005949 +0.491768,0.01738682,0.8705527,-10.31791,0.01729472,0.9994083,-0.02973,0.2969119,-0.8705544,0.02967621,0.4911763,-8.788752 +0.4868646,0.01702632,0.8733115,-9.925651,0.01569748,0.999478,-0.02823733,0.2717875,-0.8733363,0.02745654,0.4863432,-8.577088 +0.4817468,0.01615353,0.8761616,-9.550035,0.01545132,0.9995181,-0.02692354,0.2470296,-0.8761742,0.02650817,0.481265,-8.37838 +0.4772789,0.01557987,0.8786138,-9.18823,0.01610619,0.9995197,-0.02647299,0.2217585,-0.8786042,0.02678611,0.4767987,-8.193104 +0.4732671,0.0145292,0.8807992,-8.843007,0.01849921,0.9994795,-0.02642681,0.1979693,-0.8807247,0.02880102,0.472752,-8.02343 +0.4696391,0.01337119,0.8827573,-8.515855,0.020509,0.9994502,-0.02604982,0.1770073,-0.8826202,0.03033847,0.4691067,-7.86436 +0.4658156,0.01301368,0.8847862,-8.211014,0.02203305,0.9994112,-0.02629941,0.1574511,-0.8846074,0.0317452,0.4652546,-7.716742 +0.4626758,0.01353184,0.8864243,-7.926879,0.02237008,0.9993869,-0.02693253,0.1385032,-0.8862453,0.0322904,0.4620894,-7.578078 +0.4599587,0.01564454,0.8878025,-7.666677,0.02180944,0.9993441,-0.02890927,0.1212137,-0.8876724,0.03265954,0.4593157,-7.451274 +0.4581729,0.01780628,0.8886848,-7.426664,0.021499,0.9992848,-0.03110643,0.1043387,-0.888603,0.03335795,0.4574624,-7.335419 +0.4574193,0.02064414,0.8890115,-7.195608,0.02173616,0.9991722,-0.03438605,0.08690973,-0.8889854,0.03505253,0.4565919,-7.225477 +0.4580054,0.02300707,0.8886517,-6.965634,0.02399934,0.9989806,-0.03823259,0.06980096,-0.8886253,0.03883777,0.4569864,-7.116444 +0.4603458,0.02346199,0.8874296,-6.732949,0.0264711,0.9988434,-0.04013921,0.05536442,-0.8873449,0.04196915,0.4591923,-7.004384 +0.4646731,0.02295985,0.8851846,-6.509157,0.02739505,0.9988125,-0.04028802,0.04498962,-0.8850584,0.04297042,0.4634923,-6.882267 +0.471734,0.02221782,0.881461,-6.290034,0.02668982,0.9988646,-0.03946073,0.03483157,-0.8813369,0.04214099,0.4706053,-6.754992 +0.4807006,0.02485092,0.8765326,-6.081177,0.02381464,0.9988596,-0.04137929,0.02288921,-0.8765613,0.04076535,0.4795606,-6.624412 +0.4915793,0.0290704,0.8703475,-5.88026,0.02214281,0.9987022,-0.04586401,0.007391935,-0.8705513,0.04181772,0.4902976,-6.496252 +0.504414,0.03228266,0.8628583,-5.683176,0.02130874,0.9985311,-0.04981545,-0.007289078,-0.8631989,0.04351403,0.5029851,-6.365703 +0.5190654,0.03364022,0.8540723,-5.489078,0.0211258,0.998415,-0.05216488,-0.02195396,-0.8544734,0.04511993,0.5175319,-6.233971 +0.5352225,0.03366225,0.8440402,-5.299828,0.02005515,0.9984176,-0.05253655,-0.03518676,-0.844473,0.04504608,0.5337004,-6.094816 +0.5520796,0.03445039,0.8330794,-5.11593,0.01741012,0.9984519,-0.0528267,-0.05023552,-0.8336096,0.04366855,0.5506251,-5.951422 +0.5699734,0.03563471,0.8208901,-4.934984,0.01585904,0.9983959,-0.05435171,-0.0643069,-0.8215101,0.04399755,0.5684939,-5.802268 +0.5901721,0.03814704,0.8063757,-4.745745,0.01542878,0.9981674,-0.05851215,-0.07864708,-0.80713,0.04697362,0.5885019,-5.644725 +0.6109687,0.03920078,0.7906836,-4.546901,0.01645771,0.9979284,-0.06219267,-0.09511714,-0.7914836,0.0510106,0.6090579,-5.474396 +0.6319729,0.03879883,0.7740187,-4.339961,0.01612521,0.9978715,-0.06318575,-0.1133601,-0.7748227,0.05241289,0.6300021,-5.285216 +0.6534976,0.03915886,0.755915,-4.130458,0.01165876,0.9980217,-0.06177992,-0.1325004,-0.7568387,0.04918605,0.6517482,-5.073841 +0.6749762,0.03860696,0.7368289,-3.918483,0.005235747,0.9983544,-0.05710612,-0.154595,-0.737821,0.04240311,0.6736632,-4.841217 +0.6967523,0.03488786,0.7164629,-3.697865,0.001686952,0.998734,-0.05027348,-0.1802182,-0.7173098,0.03623679,0.6958113,-4.592905 +0.7183685,0.02960816,0.6950324,-3.473372,0.003035099,0.9989509,-0.04569201,-0.2037006,-0.6956561,0.03493318,0.717525,-4.334121 +0.7397501,0.02685171,0.6723458,-3.251958,0.003400964,0.9990415,-0.043641,-0.2245399,-0.6728731,0.03457005,0.7389497,-4.063126 +0.7612835,0.02602284,0.6478969,-3.037676,0.002050853,0.9990927,-0.04253843,-0.24639,-0.648416,0.03371254,0.7605393,-3.769289 +0.7826368,0.02687946,0.6218981,-2.828032,-0.002246108,0.9991827,-0.04035969,-0.2688144,-0.6224746,0.03019012,0.7820574,-3.456951 +0.8029185,0.02563701,0.5955373,-2.618666,-0.003745083,0.9992719,-0.03796798,-0.2919692,-0.5960771,0.02825485,0.8024298,-3.132043 +0.8230044,0.02367897,0.5675413,-2.408137,-0.003048275,0.9993005,-0.03727247,-0.3142259,-0.5680268,0.02894538,0.8225008,-2.790908 +0.843029,0.02035561,0.5374828,-2.19941,0.0009791069,0.9992239,-0.03937843,-0.3329592,-0.5378672,0.0337234,0.8423548,-2.430206 +0.8624118,0.01751042,0.5059044,-1.995823,0.00310765,0.9991995,-0.039882,-0.3521711,-0.5061978,0.03596687,0.861667,-2.041747 +0.8807641,0.01512267,0.4733139,-1.801721,0.002564983,0.9993229,-0.03670202,-0.37107,-0.4735484,0.03353986,0.8801289,-1.625169 +0.8979859,0.01603472,0.4397322,-1.619301,-0.001605042,0.9994485,-0.03316695,-0.3905415,-0.4400215,0.02907766,0.8975163,-1.177136 +0.9142476,0.01697871,0.4048002,-1.444291,-0.004665837,0.9994965,-0.03138445,-0.4091277,-0.4051292,0.02680442,0.9138664,-0.7098437 +0.9293015,0.01175271,0.3691351,-1.26963,-0.001794683,0.9996254,-0.02730845,-0.42883,-0.3693178,0.0247153,0.9289744,-0.2150744 +0.9430129,0.004554133,0.332725,-1.102382,0.003438296,0.9997196,-0.0234284,-0.4503757,-0.3327384,0.02323728,0.9427328,0.3018002 +0.9556077,-0.004713593,0.2946046,-0.9458897,0.0114851,0.999708,-0.0212591,-0.4699004,-0.2944183,0.02369892,0.9553827,0.8454409 +0.9667553,-0.01039661,0.2554919,-0.8121858,0.01619852,0.9996562,-0.02061502,-0.4869598,-0.2551897,0.02406826,0.9665913,1.417145 +0.9763638,-0.01200627,0.2158003,-0.7128327,0.01705916,0.9996218,-0.02156723,-0.5013363,-0.2154597,0.02473882,0.9761993,2.020444 +0.9840273,-0.01038824,0.1777143,-0.6378581,0.01460092,0.9996421,-0.02241338,-0.5154859,-0.1774179,0.02465016,0.9838268,2.648628 +0.989717,-0.009874901,0.1426985,-0.577413,0.01343789,0.999621,-0.02402653,-0.5276563,-0.1424071,0.02569703,0.9894745,3.304554 +0.9937803,-0.01050678,0.1108618,-0.5289363,0.01338549,0.9995914,-0.0252544,-0.540156,-0.1105512,0.02658126,0.9935149,3.992533 +0.9965121,-0.01271911,0.08247366,-0.4902611,0.01477706,0.9995933,-0.02439053,-0.5581546,-0.08212989,0.02552417,0.9962947,4.697446 +0.998311,-0.0134866,0.05650902,-0.4709248,0.01488386,0.999592,-0.0243788,-0.5760638,-0.05615717,0.02517869,0.9981044,5.426109 +0.9993849,-0.01301802,0.03256524,-0.4637439,0.01384848,0.9995812,-0.02540737,-0.5976769,-0.03222085,0.02584272,0.9991466,6.162183 +0.9998655,-0.01134766,0.01184681,-0.4641394,0.01163743,0.9996275,-0.02468459,-0.612016,-0.01156228,0.02481914,0.999625,6.920857 +0.9999316,-0.009466175,-0.006877324,-0.476637,0.009311525,0.9997106,-0.02218139,-0.6276445,0.007085306,0.02211583,0.9997303,7.694492 +0.9997347,-0.008921921,-0.02123679,-0.4970589,0.008515965,0.9997807,-0.01913006,-0.6434513,0.02140281,0.01894413,0.9995914,8.489033 +0.9995322,-0.003646739,-0.03036747,-0.5313984,0.003104036,0.999835,-0.01789922,-0.6633061,0.03042773,0.01779658,0.9993785,9.295931 +0.9993754,0.001325286,-0.03531589,-0.5707381,-0.002022584,0.9998035,-0.01971621,-0.6757426,0.03528282,0.01977532,0.9991816,10.11987 +0.9993099,0.001808124,-0.03710089,-0.604811,-0.002613311,0.9997618,-0.02166562,-0.6892524,0.03705288,0.02174762,0.9990766,10.95905 +0.9992863,-0.0009650904,-0.03776342,-0.6242421,0.0001504223,0.9997673,-0.02156984,-0.7023164,0.03777545,0.02154876,0.9990538,11.81777 +0.999287,-0.002282807,-0.03768894,-0.6484508,0.001536398,0.9998023,-0.01982155,-0.726471,0.03772674,0.01974951,0.9990929,12.68675 +0.9992798,-0.000391163,-0.03794498,-0.6801171,-0.0003604084,0.9998039,-0.01979801,-0.7525259,0.03794528,0.01979743,0.9990836,13.56921 +0.9992673,0.001237887,-0.03825395,-0.7118728,-0.002115448,0.9997353,-0.02290842,-0.7792859,0.03821547,0.02297256,0.9990054,14.46601 +0.9992507,0.004110066,-0.03848721,-0.7483848,-0.005080577,0.9996707,-0.02515268,-0.8066043,0.03837116,0.02532937,0.9989424,15.3785 +0.9992121,0.007504414,-0.0389742,-0.7865066,-0.008524887,0.9996234,-0.02608344,-0.8385639,0.03876378,0.02639513,0.9988997,16.3073 +0.9991758,0.008870216,-0.03961127,-0.8252649,-0.009823865,0.999665,-0.02394578,-0.8761044,0.03938559,0.02431517,0.9989281,17.24745 +0.9991345,0.006169517,-0.04113703,-0.8608237,-0.007056113,0.9997452,-0.021442,-0.9147192,0.04099426,0.02171371,0.9989234,18.19174 +0.9990317,0.006336356,-0.04353747,-0.9050122,-0.007225485,0.9997679,-0.02029522,-0.9533412,0.04339876,0.02059015,0.9988456,19.14491 +0.9988869,0.005412752,-0.04685889,-0.9508667,-0.006411686,0.9997548,-0.02119391,-0.9934983,0.04673268,0.02147076,0.9986766,20.10698 +0.9986886,0.002725992,-0.05112422,-0.9982187,-0.003872697,0.9997428,-0.02234414,-1.029351,0.05105016,0.02251282,0.9984423,21.0844 +0.9984608,0.0007126212,-0.0554588,-1.050402,-0.001982371,0.9997371,-0.02284373,-1.065831,0.05542794,0.0229185,0.9981996,22.07364 +0.9982289,0.001151927,-0.05948024,-1.1117,-0.002333633,0.9998012,-0.01980156,-1.101589,0.0594456,0.01990529,0.998033,23.08151 +0.998,0.0002866564,-0.06321365,-1.176238,-0.001288799,0.9998741,-0.01581305,-1.135143,0.06320115,0.0158629,0.9978747,24.10049 +0.997753,0.0005367514,-0.06699836,-1.245567,-0.001330644,0.9999294,-0.01180537,-1.165681,0.06698729,0.01186799,0.9976832,25.13554 +0.997544,0.00122129,-0.07003307,-1.318962,-0.001918705,0.9999492,-0.009891966,-1.193623,0.07001743,0.01000204,0.9974956,26.17785 +0.9973693,0.002486522,-0.07244577,-1.395968,-0.003442847,0.9999085,-0.01307868,-1.222323,0.07240661,0.0132937,0.9972865,27.22466 +0.997224,0.003612189,-0.07437345,-1.477065,-0.005032127,0.9998084,-0.01891346,-1.250185,0.07429088,0.01923521,0.997051,28.27774 +0.9971173,0.003081023,-0.07581292,-1.558337,-0.004671273,0.9997726,-0.0208076,-1.281898,0.07573156,0.02110176,0.9969049,29.34201 +0.9970078,0.002486793,-0.07726099,-1.640741,-0.003745539,0.9998625,-0.01615149,-1.315662,0.0772102,0.01639255,0.99688,30.42088 +0.996906,0.002573458,-0.07856076,-1.724717,-0.003335734,0.9999486,-0.009573322,-1.350706,0.07853208,0.00980576,0.9968633,31.50487 +0.9967643,-0.003909575,-0.08028551,-1.80238,0.003293692,0.9999641,-0.007802165,-1.379426,0.08031313,0.007512483,0.9967413,32.5908 +0.9966451,-0.007810978,-0.08147111,-1.87932,0.006886688,0.9999087,-0.01161985,-1.407888,0.08155444,0.0110198,0.9966079,33.66468 +0.9966224,-0.00701897,-0.08182096,-1.966625,0.005653365,0.999841,-0.01690991,-1.447013,0.08192664,0.01639023,0.9965035,34.74244 +0.9966235,-0.004983852,-0.08195652,-2.057718,0.003471261,0.9998212,-0.01858817,-1.487218,0.0820345,0.01824091,0.9964625,35.82986 +0.9967386,-0.001396567,-0.08068617,-2.150183,3.34968E-05,0.9998573,-0.01689237,-1.530548,0.08069825,0.01683457,0.9965963,36.92359 +0.9968688,0.002642999,-0.07902991,-2.238254,-0.003782436,0.999891,-0.01427157,-1.569943,0.07898357,0.0145258,0.99677,38.02774 +0.9969725,0.002749507,-0.07770694,-2.317142,-0.003799372,0.9999034,-0.01336595,-1.606535,0.07766268,0.01362072,0.9968866,39.13464 +0.997091,0.002862993,-0.07616721,-2.397389,-0.003681576,0.9999369,-0.01060893,-1.638706,0.07613203,0.01085848,0.9970386,40.24904 +0.9971702,0.009331554,-0.07459634,-2.481505,-0.009880389,0.9999267,-0.006991735,-1.670571,0.07452563,0.00770899,0.9971892,41.36383 +0.9973164,0.01223167,-0.07218377,-2.563489,-0.01272979,0.9998982,-0.006444779,-1.700399,0.07209759,0.007346367,0.9973705,42.47906 +0.9974598,0.01332772,-0.06997458,-2.638864,-0.01396436,0.9998653,-0.008616922,-1.73002,0.06985031,0.009572183,0.9975115,43.59428 +0.9975934,0.01621102,-0.06741456,-2.715061,-0.01708946,0.9997761,-0.01247404,-1.765263,0.06719725,0.0135961,0.997647,44.71026 +0.9977395,0.01579921,-0.06531783,-2.782512,-0.01668325,0.9997761,-0.01301107,-1.803738,0.06509764,0.01407137,0.9977796,45.83249 +0.9978718,0.01542677,-0.06335591,-2.84828,-0.01615549,0.9998089,-0.01100577,-1.840111,0.06317402,0.01200589,0.9979303,46.96257 +0.9980055,0.01563743,-0.06116015,-2.91476,-0.01630573,0.9998125,-0.01044312,-1.877642,0.06098538,0.01141955,0.9980733,48.09641 +0.9980727,0.01785388,-0.05943303,-2.981824,-0.01879918,0.9997049,-0.01538429,-1.915142,0.05914082,0.01647193,0.9981137,49.22285 +0.998133,0.01645449,-0.05882099,-3.041372,-0.01765671,0.9996445,-0.01997763,-1.951517,0.05847135,0.02097891,0.9980686,50.35697 +0.9981519,0.01447239,-0.05901996,-3.098225,-0.01561081,0.9997,-0.01887344,-1.990571,0.0587291,0.01975991,0.9980783,51.50882 +0.9981622,0.01256095,-0.05928416,-3.1566,-0.01345292,0.9998019,-0.01467064,-2.028592,0.05908813,0.01544122,0.9981333,52.66326 +0.9982264,0.008707782,-0.05889307,-3.212653,-0.009492043,0.9998698,-0.01305006,-2.065955,0.05877176,0.01358592,0.9981789,53.82038 +0.9982826,0.003593007,-0.05847319,-3.268258,-0.004416541,0.9998928,-0.01396081,-2.101998,0.05841675,0.01419508,0.9981913,54.97678 +0.9983025,0.002381471,-0.0581937,-3.326194,-0.003242111,0.9998867,-0.0146993,-2.137882,0.05815209,0.01486302,0.998197,56.13489 +0.9983384,-0.001945878,-0.05759066,-3.380992,0.00131607,0.9999389,-0.01097186,-2.170759,0.05760849,0.01087783,0.9982799,57.30736 +0.998375,-0.003236659,-0.05689506,-3.437582,0.00296042,0.9999834,-0.004938856,-2.199954,0.0569101,0.004762397,0.9983679,58.48395 +0.9984334,-0.00312341,-0.05586689,-3.502086,0.002967275,0.9999914,-0.002877501,-2.229392,0.0558754,0.00270722,0.998434,59.6613 +0.9984207,-0.001978011,-0.0561453,-3.567609,0.001438735,0.9999524,-0.009643816,-2.260389,0.0561617,0.009547806,0.998376,60.83477 +0.9984294,6.590204E-05,-0.05602514,-3.637028,-0.001090354,0.9998327,-0.01825521,-2.29904,0.05601456,0.01828762,0.9982624,62.00855 +0.9984213,0.005675464,-0.05588271,-3.713046,-0.006858339,0.999756,-0.02099811,-2.342808,0.0557499,0.02134822,0.9982165,63.19369 +0.9984266,0.00812975,-0.0554827,-3.7866,-0.009204457,0.9997744,-0.01914214,-2.393096,0.05531456,0.0196227,0.9982761,64.38572 +0.9984088,0.009915797,-0.05551167,-3.857193,-0.01096885,0.999765,-0.01869746,-2.442419,0.05531322,0.01927661,0.9982829,65.57899 +0.9983853,0.01046428,-0.05583406,-3.925554,-0.01130035,0.9998284,-0.01467957,-2.488376,0.05567087,0.01528681,0.9983321,66.77803 +0.9983683,0.009468162,-0.05631298,-3.981904,-0.01017903,0.9998719,-0.01235006,-2.529631,0.05618883,0.01290312,0.9983367,67.97354 +0.9983745,0.008291212,-0.05638792,-4.036864,-0.008972448,0.9998896,-0.0118388,-2.569024,0.05628353,0.01232549,0.9983387,69.16868 +0.9983417,0.01042504,-0.05661523,-4.099562,-0.01087209,0.999912,-0.007593888,-2.604193,0.05653108,0.00819682,0.9983671,70.36636 +0.9983297,0.01142732,-0.05663382,-4.173959,-0.0116579,0.999925,-0.003742596,-2.646996,0.05658681,0.004396576,0.9983879,71.56592 +0.998283,0.01386138,-0.05691296,-4.251058,-0.01434715,0.999864,-0.008135505,-2.69467,0.05679244,0.008938074,0.9983459,72.75276 +0.9982798,0.01274442,-0.05722932,-4.323029,-0.01383145,0.9997306,-0.01863855,-2.744968,0.05697636,0.01939805,0.998187,73.92846 +0.998278,0.01296168,-0.05721025,-4.396788,-0.01429501,0.9996342,-0.02295835,-2.802467,0.05689175,0.02373663,0.9980981,75.10649 +0.9982961,0.01041136,-0.05741566,-4.465029,-0.01172032,0.9996779,-0.02250857,-2.863069,0.05716282,0.02314314,0.9980965,76.28381 +0.9983466,0.003244646,-0.05739036,-4.525824,-0.004717933,0.9996623,-0.02555449,-2.925207,0.05728806,0.025783,0.9980247,77.45626 +0.9983658,0.003691504,-0.05702748,-4.593996,-0.005239377,0.9996212,-0.02701694,-2.98303,0.05690614,0.02727158,0.9980069,78.62373 +0.9984042,-0.002420659,-0.05642121,-4.660434,0.001148301,0.9997445,-0.02257258,-3.039445,0.05646143,0.02247176,0.9981518,79.79891 +0.9984173,-0.003625946,-0.05612367,-4.726372,0.002776288,0.9998804,-0.01520962,-3.092053,0.05617211,0.01502973,0.9983079,80.96885 +0.9984861,-0.0007615133,-0.05500021,-4.800834,0.0002479803,0.9999563,-0.009343151,-3.13883,0.05500492,0.009315366,0.9984426,82.13644 +0.9985195,0.001081753,-0.05438531,-4.871285,-0.001483063,0.9999719,-0.007339205,-3.178547,0.05437584,0.007408995,0.998493,83.29433 +0.998541,0.00534618,-0.05373445,-4.9447,-0.005602005,0.9999737,-0.004611426,-3.214267,0.05370838,0.004905718,0.9985446,84.45032 +0.9985646,0.009151735,-0.05277358,-5.0164,-0.00945002,0.9999407,-0.005405399,-3.250277,0.05272098,0.005896351,0.9985918,85.59727 +0.9985602,0.01291144,-0.05206642,-5.08674,-0.01354239,0.9998388,-0.01178365,-3.288225,0.05190588,0.01247178,0.998574,86.7357 +0.9985257,0.01610948,-0.05183638,-5.155943,-0.01706535,0.9996914,-0.01805064,-3.328985,0.05152959,0.01890863,0.9984924,87.86812 +0.9986073,0.01277185,-0.05118999,-5.214554,-0.01369076,0.9997506,-0.01764069,-3.368601,0.05095192,0.01831695,0.9985331,89.00816 +0.9986455,0.01239836,-0.0505321,-5.275069,-0.01304564,0.9998367,-0.01249957,-3.403659,0.05036888,0.01314186,0.9986442,90.15079 +0.9987584,0.008385163,-0.04910641,-5.329539,-0.008705803,0.9999421,-0.006319235,-3.435396,0.04905058,0.006738899,0.9987735,91.29492 +0.9987935,0.003096949,-0.04901132,-5.369648,-0.003267539,0.9999889,-0.003400893,-3.459248,0.04900023,0.003556936,0.9987924,92.43554 +0.9988405,-0.001632819,-0.04811549,-5.418986,0.001437775,0.9999906,-0.004088007,-3.482548,0.04812171,0.004014088,0.9988334,93.56768 +0.9988745,-0.004879842,-0.04718123,-5.470861,0.004550713,0.9999646,-0.007080753,-3.498163,0.04721411,0.006858075,0.9988612,94.70089 +0.9988872,-0.007351853,-0.04658741,-5.524006,0.006830398,0.9999123,-0.0113424,-3.526732,0.04666671,0.01101157,0.9988498,95.82761 +0.9989093,-0.009331753,-0.04575093,-5.583931,0.008633629,0.9998436,-0.01543319,-3.562758,0.04588779,0.01502136,0.9988336,96.96153 diff --git a/fast_evaluator/linspecer.m b/fast_evaluator/linspecer.m new file mode 100644 index 0000000..434a398 --- /dev/null +++ b/fast_evaluator/linspecer.m @@ -0,0 +1,261 @@ +% function lineStyles = linspecer(N) +% This function creates an Nx3 array of N [R B G] colors +% These can be used to plot lots of lines with distinguishable and nice +% looking colors. +% +% lineStyles = linspecer(N); makes N colors for you to use: lineStyles(ii,:) +% +% colormap(linspecer); set your colormap to have easily distinguishable +% colors and a pleasing aesthetic +% +% lineStyles = linspecer(N,'qualitative'); forces the colors to all be distinguishable (up to 12) +% lineStyles = linspecer(N,'sequential'); forces the colors to vary along a spectrum +% +% % Examples demonstrating the colors. +% +% LINE COLORS +% N=6; +% X = linspace(0,pi*3,1000); +% Y = bsxfun(@(x,n)sin(x+2*n*pi/N), X.', 1:N); +% C = linspecer(N); +% axes('NextPlot','replacechildren', 'ColorOrder',C); +% plot(X,Y,'linewidth',5) +% ylim([-1.1 1.1]); +% +% SIMPLER LINE COLOR EXAMPLE +% N = 6; X = linspace(0,pi*3,1000); +% C = linspecer(N) +% hold off; +% for ii=1:N +% Y = sin(X+2*ii*pi/N); +% plot(X,Y,'color',C(ii,:),'linewidth',3); +% hold on; +% end +% +% COLORMAP EXAMPLE +% A = rand(15); +% figure; imagesc(A); % default colormap +% figure; imagesc(A); colormap(linspecer); % linspecer colormap +% +% See also NDHIST, NHIST, PLOT, COLORMAP, 43700-cubehelix-colormaps +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% by Jonathan Lansey, March 2009-2013 – Lansey at gmail.com % +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% +%% credits and where the function came from +% The colors are largely taken from: +% http://colorbrewer2.org and Cynthia Brewer, Mark Harrower and The Pennsylvania State University +% +% +% She studied this from a phsychometric perspective and crafted the colors +% beautifully. +% +% I made choices from the many there to decide the nicest once for plotting +% lines in Matlab. I also made a small change to one of the colors I +% thought was a bit too bright. In addition some interpolation is going on +% for the sequential line styles. +% +% +%% + +function lineStyles=linspecer(N,varargin) + +if nargin==0 % return a colormap + lineStyles = linspecer(128); + return; +end + +if ischar(N) + lineStyles = linspecer(128,N); + return; +end + +if N<=0 % its empty, nothing else to do here + lineStyles=[]; + return; +end + +% interperet varagin +qualFlag = 0; +colorblindFlag = 0; + +if ~isempty(varargin)>0 % you set a parameter? + switch lower(varargin{1}) + case {'qualitative','qua'} + if N>12 % go home, you just can't get this. + warning('qualitiative is not possible for greater than 12 items, please reconsider'); + else + if N>9 + warning(['Default may be nicer for ' num2str(N) ' for clearer colors use: whitebg(''black''); ']); + end + end + qualFlag = 1; + case {'sequential','seq'} + lineStyles = colorm(N); + return; + case {'white','whitefade'} + lineStyles = whiteFade(N);return; + case 'red' + lineStyles = whiteFade(N,'red');return; + case 'blue' + lineStyles = whiteFade(N,'blue');return; + case 'green' + lineStyles = whiteFade(N,'green');return; + case {'gray','grey'} + lineStyles = whiteFade(N,'gray');return; + case {'colorblind'} + colorblindFlag = 1; + otherwise + warning(['parameter ''' varargin{1} ''' not recognized']); + end +end +% *.95 +% predefine some colormaps + set3 = colorBrew2mat({[141, 211, 199];[ 255, 237, 111];[ 190, 186, 218];[ 251, 128, 114];[ 128, 177, 211];[ 253, 180, 98];[ 179, 222, 105];[ 188, 128, 189];[ 217, 217, 217];[ 204, 235, 197];[ 252, 205, 229];[ 255, 255, 179]}'); +set1JL = brighten(colorBrew2mat({[228, 26, 28];[ 55, 126, 184]; [ 77, 175, 74];[ 255, 127, 0];[ 255, 237, 111]*.85;[ 166, 86, 40];[ 247, 129, 191];[ 153, 153, 153];[ 152, 78, 163]}')); +set1 = brighten(colorBrew2mat({[ 55, 126, 184]*.85;[228, 26, 28];[ 77, 175, 74];[ 255, 127, 0];[ 152, 78, 163]}),.8); + +% colorblindSet = {[215,25,28];[253,174,97];[171,217,233];[44,123,182]}; +colorblindSet = {[215,25,28];[253,174,97];[171,217,233]*.8;[44,123,182]*.8}; + +set3 = dim(set3,.93); + +if colorblindFlag + switch N + % sorry about this line folks. kind of legacy here because I used to + % use individual 1x3 cells instead of nx3 arrays + case 4 + lineStyles = colorBrew2mat(colorblindSet); + otherwise + colorblindFlag = false; + warning('sorry unsupported colorblind set for this number, using regular types'); + end +end +if ~colorblindFlag + switch N + case 1 + lineStyles = { [ 55, 126, 184]/255}; + case {2, 3, 4, 5 } + lineStyles = set1(1:N); + case {6 , 7, 8, 9} + lineStyles = set1JL(1:N)'; + case {10, 11, 12} + if qualFlag % force qualitative graphs + lineStyles = set3(1:N)'; + else % 10 is a good number to start with the sequential ones. + lineStyles = cmap2linspecer(colorm(N)); + end + otherwise % any old case where I need a quick job done. + lineStyles = cmap2linspecer(colorm(N)); + end +end +lineStyles = cell2mat(lineStyles); + +end + +% extra functions +function varIn = colorBrew2mat(varIn) +for ii=1:length(varIn) % just divide by 255 + varIn{ii}=varIn{ii}/255; +end +end + +function varIn = brighten(varIn,varargin) % increase the brightness + +if isempty(varargin), + frac = .9; +else + frac = varargin{1}; +end + +for ii=1:length(varIn) + varIn{ii}=varIn{ii}*frac+(1-frac); +end +end + +function varIn = dim(varIn,f) + for ii=1:length(varIn) + varIn{ii} = f*varIn{ii}; + end +end + +function vOut = cmap2linspecer(vIn) % changes the format from a double array to a cell array with the right format +vOut = cell(size(vIn,1),1); +for ii=1:size(vIn,1) + vOut{ii} = vIn(ii,:); +end +end +%% +% colorm returns a colormap which is really good for creating informative +% heatmap style figures. +% No particular color stands out and it doesn't do too badly for colorblind people either. +% It works by interpolating the data from the +% 'spectral' setting on http://colorbrewer2.org/ set to 11 colors +% It is modified a little to make the brightest yellow a little less bright. +function cmap = colorm(varargin) +n = 100; +if ~isempty(varargin) + n = varargin{1}; +end + +if n==1 + cmap = [0.2005 0.5593 0.7380]; + return; +end +if n==2 + cmap = [0.2005 0.5593 0.7380; + 0.9684 0.4799 0.2723]; + return; +end + +frac=.95; % Slight modification from colorbrewer here to make the yellows in the center just a bit darker +cmapp = [158, 1, 66; 213, 62, 79; 244, 109, 67; 253, 174, 97; 254, 224, 139; 255*frac, 255*frac, 191*frac; 230, 245, 152; 171, 221, 164; 102, 194, 165; 50, 136, 189; 94, 79, 162]; +x = linspace(1,n,size(cmapp,1)); +xi = 1:n; +cmap = zeros(n,3); +for ii=1:3 + cmap(:,ii) = pchip(x,cmapp(:,ii),xi); +end +cmap = flipud(cmap/255); +end + +function cmap = whiteFade(varargin) +n = 100; +if nargin>0 + n = varargin{1}; +end + +thisColor = 'blue'; + +if nargin>1 + thisColor = varargin{2}; +end +switch thisColor + case {'gray','grey'} + cmapp = [255,255,255;240,240,240;217,217,217;189,189,189;150,150,150;115,115,115;82,82,82;37,37,37;0,0,0]; + case 'green' + cmapp = [247,252,245;229,245,224;199,233,192;161,217,155;116,196,118;65,171,93;35,139,69;0,109,44;0,68,27]; + case 'blue' + cmapp = [247,251,255;222,235,247;198,219,239;158,202,225;107,174,214;66,146,198;33,113,181;8,81,156;8,48,107]; + case 'red' + cmapp = [255,245,240;254,224,210;252,187,161;252,146,114;251,106,74;239,59,44;203,24,29;165,15,21;103,0,13]; + otherwise + warning(['sorry your color argument ' thisColor ' was not recognized']); +end + +cmap = interpomap(n,cmapp); +end + +% Eat a approximate colormap, then interpolate the rest of it up. +function cmap = interpomap(n,cmapp) + x = linspace(1,n,size(cmapp,1)); + xi = 1:n; + cmap = zeros(n,3); + for ii=1:3 + cmap(:,ii) = pchip(x,cmapp(:,ii),xi); + end + cmap = (cmap/255); % flipud?? +end + + + diff --git a/fast_evaluator/main.m b/fast_evaluator/main.m new file mode 100644 index 0000000..0b3649a --- /dev/null +++ b/fast_evaluator/main.m @@ -0,0 +1,154 @@ +clear; clc; + +addpath(genpath('src')); +addpath(genpath('data')); + +%% data preparation +global data_path; +% your directory should contain files like this +% - 00 +% l- 00.csv (gt pose) +% l- velodyne +% l- <0xxxx.bin> +data_path = '/media/gskim/Data/KITTI odo/data_odometry_velodyne/dataset/sequences/00/'; + + +down_shape = [40, 120]; +skip_data_frame = 1; +[data_scancontexts, data_ringkeys, data_poses] = loadData(down_shape, skip_data_frame); + +figure(101); clf; +plot(data_poses(:,1), data_poses(:,2)); +axis equal; + +%% main - global recognizer +revisit_criteria = 5; % in meter (recommend test for 5, 10, 20 meters) +keyframe_gap = 1; % for_fast_eval (if 1, no skip) + +global num_candidates; num_candidates = 50; +% global num_node_enough_apart; num_node_enough_apart = 50; + +% policy (top N) +num_top_n = 25; +top_n = linspace(1, num_top_n, num_top_n); + +% Entropy thresholds +middle_thres = 0.01; +thresholds1 = linspace(0, middle_thres, 50); +thresholds2 = linspace(middle_thres, 1, 50); +thresholds = [thresholds1, thresholds2]; +num_thresholds = length(thresholds); + +% Main variables to store the result for drawing PR curve +num_hits = zeros(num_top_n, num_thresholds); +num_false_alarms = zeros(num_top_n, num_thresholds); +num_correct_rejections = zeros(num_top_n, num_thresholds); +num_misses = zeros(num_top_n, num_thresholds); + +% main +loop_log = []; + +exp_poses = []; +exp_ringkeys = []; +exp_scancontexts = {}; + +num_queries = length(data_poses); +for query_idx = 1:num_queries - 1 + + % save to (online) DB + query_sc = data_scancontexts{query_idx}; + query_rk = data_ringkeys(query_idx, :); + query_pose = data_poses(query_idx,:); + + exp_scancontexts{end+1} = query_sc; + exp_poses = [exp_poses; query_pose]; + exp_ringkeys = [exp_ringkeys; query_rk]; + + if(rem(query_idx, keyframe_gap) ~= 0) + continue; + end + + if( length(exp_scancontexts) < num_candidates ) + continue; + end + + tree = createns(exp_ringkeys(1:end-(num_candidates-1), :), 'NSMethod', 'kdtree'); % Create object to use in k-nearest neighbor search + + % revisitness + [revisitness, how_far_apart] = isRevisitGlobalLoc(query_pose, exp_poses(1:end-(num_candidates-1), :), revisit_criteria); + disp([revisitness, how_far_apart]) + + % find candidates + candidates = knnsearch(tree, query_rk, 'K', num_candidates); + + % find the nearest (top 1) via pairwise comparison + nearest_idx = 0; + min_dist = inf; % initialization + for ith_candidate = 1:length(candidates) + candidate_node_idx = candidates(ith_candidate); + candidate_img = exp_scancontexts{candidate_node_idx}; + + distance_to_query = sc_dist(query_sc, candidate_img); + + if( distance_to_query < min_dist) + nearest_idx = candidate_node_idx; + min_dist = distance_to_query; + end + end + + % prcurve analysis + for topk = 1:num_top_n + for thres_idx = 1:num_thresholds + threshold = thresholds(thres_idx); + + reject = 0; + if( min_dist > threshold) + reject = 1; + end + + if(reject == 1) + if(revisitness == 0) + % TN: Correct Rejection + num_correct_rejections(topk, thres_idx) = num_correct_rejections(topk, thres_idx) + 1; + else + % FN: MISS + num_misses(topk, thres_idx) = num_misses(topk, thres_idx) + 1; + end + else + % if under the theshold, it is considered seen. + % and then check the correctness + if( dist_btn_pose(query_pose, exp_poses(nearest_idx, :)) < revisit_criteria) + % TP: Hit + num_hits(topk, thres_idx) = num_hits(topk, thres_idx) + 1; + else + % FP: False Alarm + num_false_alarms(topk, thres_idx) = num_false_alarms(topk, thres_idx) + 1; + end + end + + end + end + + if( rem(query_idx, 100) == 0) + disp( strcat(num2str(query_idx/num_queries * 100), ' % processed') ); + end + +end + + +%% save the log +savePath = strcat("pr_result/within ", num2str(revisit_criteria), "m/"); +if((~7==exist(savePath,'dir'))) + mkdir(savePath); +end +save(strcat(savePath, 'nCorrectRejections.mat'), 'num_correct_rejections'); +save(strcat(savePath, 'nMisses.mat'), 'num_misses'); +save(strcat(savePath, 'nHits.mat'), 'num_hits'); +save(strcat(savePath, 'nFalseAlarms.mat'), 'num_false_alarms'); + + + + + + + diff --git a/fast_evaluator/prcurve_drawer.m b/fast_evaluator/prcurve_drawer.m new file mode 100644 index 0000000..ab4d8a2 --- /dev/null +++ b/fast_evaluator/prcurve_drawer.m @@ -0,0 +1,130 @@ +ResultsDir = './pr_result/'; + +%% +title_str = strcat('KITTI 00'); + +%% Params +FigIdx = 2; +figure(FigIdx); clf; + +TopNindexes = [1]; +name = 'top1'; + +nTopNindexes = length(TopNindexes); + +%% Main +SequenceNames = dir(ResultsDir); SequenceNames(1:2, :) = []; SequenceNames = {SequenceNames(:).name}; +nSequences = length(SequenceNames); + +all_Precisions = {}; +all_Recalls = {}; + +for ithTopN = 1:nTopNindexes + + TopNidx = TopNindexes(ithTopN); + + line_width = 4; + + LineColors = colorcube(nSequences); + LineColors = linspecer(nSequences,'qualitative'); +% LineColors = linspecer(nSequences,'sequential'); + LineColors = flipud(LineColors); + + AUCs = zeros(1, nSequences); + for ithSeq = 1:nSequences + + % seq info + ithSeqName = SequenceNames{ithSeq}; + SequenceNames{ithSeq} = string(ithSeqName); + + ithSeqPath = strcat(ResultsDir, ithSeqName, '/'); + ithSeqPRcurveData = dir(ithSeqPath); ithSeqPRcurveData(1:2, :) = []; ithSeqPRcurveData = {ithSeqPRcurveData(:).name}; + + % load + nCorrectRejectionsAll = load(strcat(ithSeqPath, ithSeqPRcurveData{1})); + nCorrectRejectionsAll = nCorrectRejectionsAll.num_correct_rejections; + nCorrectRejectionsForThisTopN = nCorrectRejectionsAll(TopNidx, :); + + nFalseAlarmsAll = load(strcat(ithSeqPath, ithSeqPRcurveData{2})); + nFalseAlarmsAll = nFalseAlarmsAll.num_false_alarms; + nFalseAlarmsForThisTopN = nFalseAlarmsAll(TopNidx, :); + + nHitsAll = load(strcat(ithSeqPath, ithSeqPRcurveData{3})); + nHitsAll = nHitsAll.num_hits; + nHitsForThisTopN = nHitsAll(TopNidx, :); + + nMissesAll = load(strcat(ithSeqPath, ithSeqPRcurveData{4})); + nMissesAll = nMissesAll.num_misses; + nMissesForThisTopN = nMissesAll(TopNidx, :); + + % info + nTopNs = size(nCorrectRejectionsAll, 1); + nThres = size(nCorrectRejectionsAll, 2); + + % main + Precisions = []; + Recalls = []; + Accuracies = []; + for ithThres = 1:nThres + nCorrectRejections = nCorrectRejectionsForThisTopN(ithThres); + nFalseAlarms = nFalseAlarmsForThisTopN(ithThres); + nHits = nHitsForThisTopN(ithThres); + nMisses = nMissesForThisTopN(ithThres); + + nTotalTestPlaces = nCorrectRejections + nFalseAlarms + nHits + nMisses; + + Precision = nHits / (nHits + nFalseAlarms); + Recall = nHits / (nHits + nMisses); + Acc = (nHits + nCorrectRejections)/nTotalTestPlaces; + + Precisions = [Precisions; Precision]; + Recalls = [Recalls; Recall]; + Accuracies = [Accuracies; Acc]; + end + + num_points = length(Precisions); + Precisions(1) = 1; + AUC = 0; + for ith = 1:num_points-1 + small_area = 1/2 * (Precisions(ith) + Precisions(ith+1)) * (Recalls(ith+1)-Recalls(ith)); + AUC = AUC + small_area; + end + AUCs(ithSeq) = AUC; + + all_Precisions{ithSeq} = Precisions; + all_Recalls{ithSeq} = Recalls; + + % draw + figure(FigIdx); + set(gcf, 'Position', [10 10 800 500]); + + fontsize = 10; + p = plot(Recalls, Precisions, 'LineWidth', line_width); % commonly x axis is recall + title(title_str, 'FontSize', fontsize); + xlabel('Recall', 'FontSize', fontsize); ylabel('Precision', 'FontSize', fontsize); + set(gca, 'FontSize', fontsize+5) + xticks([0 0.2 0.4 0.6 0.8 1.0]) + xticklabels({'0','0.2','0.4','0.6','0.8','1'}) + yticks([0 0.2 0.4 0.6 0.8 1.0]) + yticklabels({'0','0.2','0.4','0.6','0.8','1'}) + + p(1).Color = LineColors(ithSeq, :); + p(1).MarkerEdgeColor = LineColors(ithSeq, :); + % axis equal; + xlim([0, 1]); ylim([0,1]); + grid on; grid minor; + hold on; + + end + + lgd = legend(SequenceNames, 'Location', 'best'); + lgd.FontSize = fontsize + 3; + lgd.FontWeight = 'bold'; + + grid minor; + + name = 'prcurve'; + print('-bestfit', name,'-dpdf') + +end + diff --git a/fast_evaluator/src/Ptcloud2ScanContext.m b/fast_evaluator/src/Ptcloud2ScanContext.m new file mode 100644 index 0000000..584659e --- /dev/null +++ b/fast_evaluator/src/Ptcloud2ScanContext.m @@ -0,0 +1,102 @@ +function [ img ] = Ptcloud2ScanContext( ptcloud, num_sector, num_ring, max_range ) + +%% Preprocessing + +% Downsampling for fast search +gridStep = 0.5; % 0.5m cubic grid downsampling is applied in the paper. +ptcloud = pcdownsample(ptcloud, 'gridAverage', gridStep); + +% point cloud information +num_points = ptcloud.Count; +gap = max_range / num_ring; +angle_one_sector = 360/num_sector; + + +%% vacant bins +cell_bins = cell(num_ring, num_sector); +cell_bin_counter = ones(num_ring, num_sector); + +enough_large = 500; % for fast and constant time save, We contain maximum 500 points per each bin. +enough_small = -10000; +for ith_ring = 1:num_ring + for ith_sector = 1:num_sector + bin = enough_small * ones(enough_large, 3); + cell_bins{ith_ring, ith_sector} = bin; + end +end + + +%% Save a point to the corresponding bin +for ith_point =1:num_points + + % Point information + ith_point_xyz = ptcloud.Location(ith_point,:); + ith_point_r = sqrt(ith_point_xyz(1)^2 + ith_point_xyz(2)^2); + ith_point_theta = XY2Theta(ith_point_xyz(1), ith_point_xyz(2)); % degree + + % Find the corresponding ring index + tmp_ring_index = floor(ith_point_r/gap); + if(tmp_ring_index >= num_ring) + ring_index = num_ring; + else + ring_index = tmp_ring_index + 1; + end + + % Find the corresponding sector index + tmp_sector_index = ceil(ith_point_theta/angle_one_sector); + if(tmp_sector_index == 0) + sector_index = 1; + elseif(tmp_sector_index > num_sector || tmp_sector_index < 1) + sector_index = num_sector; + else + sector_index = tmp_sector_index; + end + + % Assign point to the corresponding bin cell + try + corresponding_counter = cell_bin_counter(ring_index, sector_index); % 1D real value. + catch + continue; + end + cell_bins{ring_index, sector_index}(corresponding_counter, :) = ith_point_xyz; + cell_bin_counter(ring_index, sector_index) = cell_bin_counter(ring_index, sector_index) + 1; % increase count 1 + +end + + +%% bin to image format (2D matrix) +img = zeros(num_ring, num_sector); + +min_num_thres = 5; % a bin with few points, we consider it is noise. + +% Find maximum Z value of each bin and Save into img +for ith_ring = 1:num_ring + for ith_sector = 1:num_sector + value_of_the_bin = 0; + points_in_bin_ij = cell_bins{ith_ring, ith_sector}; + + if( IsBinHaveMoreThanMinimumPoints(points_in_bin_ij, min_num_thres, enough_small) ) + value_of_the_bin = max(points_in_bin_ij(:, 3)); + else + value_of_the_bin = 0; + end + + img(ith_ring, ith_sector) = value_of_the_bin; + end +end + + +end % end of the main function + + +function bool = IsBinHaveMoreThanMinimumPoints(mat, minimum_thres, enough_small) + +min_thres_point = mat(minimum_thres, :); + +if( isequal(min_thres_point, [ enough_small, enough_small, enough_small]) ) + bool = 0; +else + bool = 1; +end + +end diff --git a/fast_evaluator/src/XY2Theta.m b/fast_evaluator/src/XY2Theta.m new file mode 100644 index 0000000..cfa7651 --- /dev/null +++ b/fast_evaluator/src/XY2Theta.m @@ -0,0 +1,17 @@ +function [ theta ] = XY2Theta( x, y ) + + if (x >= 0 && y >= 0) + theta = 180/pi * atan(y/x); + end + if (x < 0 && y >= 0) + theta = 180 - ((180/pi) * atan(y/(-x))); + end + if (x < 0 && y < 0) + theta = 180 + ((180/pi) * atan(y/x)); + end + if ( x >= 0 && y < 0) + theta = 360 - ((180/pi) * atan((-y)/x)); + end + +end + diff --git a/fast_evaluator/src/deg2utm.m b/fast_evaluator/src/deg2utm.m new file mode 100644 index 0000000..a6527fa --- /dev/null +++ b/fast_evaluator/src/deg2utm.m @@ -0,0 +1,121 @@ +function [x,y,utmzone] = deg2utm(Lat,Lon) +% ------------------------------------------------------------------------- +% [x,y,utmzone] = deg2utm(Lat,Lon) +% +% Description: Function to convert lat/lon vectors into UTM coordinates (WGS84). +% Some code has been extracted from UTM.m function by Gabriel Ruiz Martinez. +% +% Inputs: +% Lat: Latitude vector. Degrees. +ddd.ddddd WGS84 +% Lon: Longitude vector. Degrees. +ddd.ddddd WGS84 +% +% Outputs: +% x, y , utmzone. See example +% +% Example 1: +% Lat=[40.3154333; 46.283900; 37.577833; 28.645650; 38.855550; 25.061783]; +% Lon=[-3.4857166; 7.8012333; -119.95525; -17.759533; -94.7990166; 121.640266]; +% [x,y,utmzone] = deg2utm(Lat,Lon); +% fprintf('%7.0f ',x) +% 458731 407653 239027 230253 343898 362850 +% fprintf('%7.0f ',y) +% 4462881 5126290 4163083 3171843 4302285 2772478 +% utmzone = +% 30 T +% 32 T +% 11 S +% 28 R +% 15 S +% 51 R +% +% Example 2: If you have Lat/Lon coordinates in Degrees, Minutes and Seconds +% LatDMS=[40 18 55.56; 46 17 2.04]; +% LonDMS=[-3 29 8.58; 7 48 4.44]; +% Lat=dms2deg(mat2dms(LatDMS)); %convert into degrees +% Lon=dms2deg(mat2dms(LonDMS)); %convert into degrees +% [x,y,utmzone] = deg2utm(Lat,Lon) +% +% Author: +% Rafael Palacios +% Universidad Pontificia Comillas +% Madrid, Spain +% Version: Apr/06, Jun/06, Aug/06, Aug/06 +% Aug/06: fixed a problem (found by Rodolphe Dewarrat) related to southern +% hemisphere coordinates. +% Aug/06: corrected m-Lint warnings +%------------------------------------------------------------------------- +% Argument checking +% +error(nargchk(2, 2, nargin)); %2 arguments required +n1=length(Lat); +n2=length(Lon); +if (n1~=n2) + error('Lat and Lon vectors should have the same length'); +end +% Memory pre-allocation +% +x=zeros(n1,1); +y=zeros(n1,1); +utmzone(n1,:)='60 X'; +% Main Loop +% +for i=1:n1 + la=Lat(i); + lo=Lon(i); + sa = 6378137.000000 ; sb = 6356752.314245; + + %e = ( ( ( sa ^ 2 ) - ( sb ^ 2 ) ) ^ 0.5 ) / sa; + e2 = ( ( ( sa ^ 2 ) - ( sb ^ 2 ) ) ^ 0.5 ) / sb; + e2cuadrada = e2 ^ 2; + c = ( sa ^ 2 ) / sb; + %alpha = ( sa - sb ) / sa; %f + %ablandamiento = 1 / alpha; % 1/f + lat = la * ( pi / 180 ); + lon = lo * ( pi / 180 ); + Huso = fix( ( lo / 6 ) + 31); + S = ( ( Huso * 6 ) - 183 ); + deltaS = lon - ( S * ( pi / 180 ) ); + if (la<-72), Letra='C'; + elseif (la<-64), Letra='D'; + elseif (la<-56), Letra='E'; + elseif (la<-48), Letra='F'; + elseif (la<-40), Letra='G'; + elseif (la<-32), Letra='H'; + elseif (la<-24), Letra='J'; + elseif (la<-16), Letra='K'; + elseif (la<-8), Letra='L'; + elseif (la<0), Letra='M'; + elseif (la<8), Letra='N'; + elseif (la<16), Letra='P'; + elseif (la<24), Letra='Q'; + elseif (la<32), Letra='R'; + elseif (la<40), Letra='S'; + elseif (la<48), Letra='T'; + elseif (la<56), Letra='U'; + elseif (la<64), Letra='V'; + elseif (la<72), Letra='W'; + else Letra='X'; + end + a = cos(lat) * sin(deltaS); + epsilon = 0.5 * log( ( 1 + a) / ( 1 - a ) ); + nu = atan( tan(lat) / cos(deltaS) ) - lat; + v = ( c / ( ( 1 + ( e2cuadrada * ( cos(lat) ) ^ 2 ) ) ) ^ 0.5 ) * 0.9996; + ta = ( e2cuadrada / 2 ) * epsilon ^ 2 * ( cos(lat) ) ^ 2; + a1 = sin( 2 * lat ); + a2 = a1 * ( cos(lat) ) ^ 2; + j2 = lat + ( a1 / 2 ); + j4 = ( ( 3 * j2 ) + a2 ) / 4; + j6 = ( ( 5 * j4 ) + ( a2 * ( cos(lat) ) ^ 2) ) / 3; + alfa = ( 3 / 4 ) * e2cuadrada; + beta = ( 5 / 3 ) * alfa ^ 2; + gama = ( 35 / 27 ) * alfa ^ 3; + Bm = 0.9996 * c * ( lat - alfa * j2 + beta * j4 - gama * j6 ); + xx = epsilon * v * ( 1 + ( ta / 3 ) ) + 500000; + yy = nu * v * ( 1 + ta ) + Bm; + if (yy<0) + yy=9999999+yy; + end + x(i)=xx; + y(i)=yy; + utmzone(i,:)=sprintf('%02d %c',Huso,Letra); +end diff --git a/fast_evaluator/src/dist_btn_pose.m b/fast_evaluator/src/dist_btn_pose.m new file mode 100644 index 0000000..39aa646 --- /dev/null +++ b/fast_evaluator/src/dist_btn_pose.m @@ -0,0 +1,3 @@ +function dist = dist_btn_pose(pose1, pose2) + dist = sqrt( (pose1(1) - pose2(1))^2 + (pose1(2) - pose2(2))^2); +end diff --git a/fast_evaluator/src/find_topk_from_candidates.m b/fast_evaluator/src/find_topk_from_candidates.m new file mode 100644 index 0000000..96b899e --- /dev/null +++ b/fast_evaluator/src/find_topk_from_candidates.m @@ -0,0 +1,28 @@ +function [ nearest_idx, min_dist ] = find_topk_from_candidates(query_img, query_idx, candidates, thres) + +global num_node_enough_apart; +global radar_imgs; + +nearest_idx = 0; +min_dist = inf; % initialization +for ith_candidate = 1:length(candidates) + candidate_node_idx = candidates(ith_candidate); + candidate_img = radar_imgs{candidate_node_idx}; + + if( abs(query_idx - candidate_node_idx) < num_node_enough_apart) + continue; + end + + distance_to_query = dist(query_img, candidate_img); + if( distance_to_query > thres) + continue; + end + + if( distance_to_query < min_dist) + nearest_idx = candidate_node_idx; + min_dist = distance_to_query; + end +end + +end + diff --git a/fast_evaluator/src/isRevisitGlobalLoc.m b/fast_evaluator/src/isRevisitGlobalLoc.m new file mode 100644 index 0000000..6ebf0ec --- /dev/null +++ b/fast_evaluator/src/isRevisitGlobalLoc.m @@ -0,0 +1,20 @@ +function [is_revisit, min_dist] = isRevisitGlobalLoc(query_pose, db_poses, thres) + +num_dbs = length(db_poses); + +dists = zeros(1, num_dbs); +for ii=1:num_dbs + dist = norm(query_pose - db_poses(ii, :)); + dists(ii) = dist; +end + +if ( min(dists) < thres ) + is_revisit = 1; +else + is_revisit = 0; +end + +min_dist = min(dists); + +end + diff --git a/fast_evaluator/src/is_revisit.m b/fast_evaluator/src/is_revisit.m new file mode 100644 index 0000000..4cd2ca3 --- /dev/null +++ b/fast_evaluator/src/is_revisit.m @@ -0,0 +1,23 @@ +function [ revisitness ] = is_revisit(query_idx, query_pose, radar_poses, revisit_criteria, num_node_enough_apart) + +num_db = size(radar_poses, 1); + +revisitness = 0; +for ii = 1:num_db + + if( abs(query_idx - ii) < num_node_enough_apart) + continue; + end + + pose = radar_poses(ii, :); + + dist = dist_btn_pose(query_pose, pose); + + if(dist < revisit_criteria) + revisitness = 1; + break; + end +end + +end + diff --git a/fast_evaluator/src/loadData.m b/fast_evaluator/src/loadData.m new file mode 100644 index 0000000..c189be3 --- /dev/null +++ b/fast_evaluator/src/loadData.m @@ -0,0 +1,45 @@ +function [scancontexts, ringkeys, poses] = loadData(down_shape, skip_data_frame) + +%% +global data_path; +data_save_path = fullfile('data/'); + +%% +% newly make +if exist(data_save_path) == 0 + % make + [scancontexts, ringkeys, poses] = makeExperience(data_path, down_shape, skip_data_frame); + + % save + mkdir(data_save_path); + + filename = strcat(data_save_path, 'scancontexts', num2str(down_shape(1)), 'x', num2str(down_shape(2)), '.mat'); + save(filename, 'scancontexts'); + filename = strcat(data_save_path, 'ringkeys', num2str(down_shape(1)), 'x', num2str(down_shape(2)), '.mat'); + save(filename, 'ringkeys'); + filename = strcat(data_save_path, 'poses', num2str(down_shape(1)), 'x', num2str(down_shape(2)), '.mat'); + save(filename, 'poses'); + +% or load +else + filename = strcat(data_save_path, 'scancontexts', num2str(down_shape(1)), 'x', num2str(down_shape(2)), '.mat'); + load(filename); + % fix + for iii = 1:length(scancontexts) + sc = double(scancontexts{iii}); + scancontexts{iii} = sc; + end + + filename = strcat(data_save_path, 'ringkeys', num2str(down_shape(1)), 'x', num2str(down_shape(2)), '.mat'); + load(filename); + filename = strcat(data_save_path, 'poses', num2str(down_shape(1)), 'x', num2str(down_shape(2)), '.mat'); + load(filename); + + disp('- successfully loaded.'); +end + +%% +disp(' '); + +end + diff --git a/fast_evaluator/src/makeExperience.m b/fast_evaluator/src/makeExperience.m new file mode 100644 index 0000000..6731c1d --- /dev/null +++ b/fast_evaluator/src/makeExperience.m @@ -0,0 +1,63 @@ +function [scancontexts, ringkeys, xy_poses] = makeExperience(data_dir, shape, skip_data_frame) + +%% +num_rings = shape(1); +num_sectors = shape(2); + +%% +lidar_data_dir = strcat(data_dir, 'velodyne/'); +data_names = osdir(lidar_data_dir); + +%% gps to xyz +gtpose = csvread(strcat(data_dir, '00.csv')); +% gtpose_time = gtpose(:, 1); +gtpose_xy = gtpose(:, [4,12]); + + +%% +num_data = length(data_names); +num_data_save = floor(num_data/skip_data_frame) + 1; +save_counter = 1; + +scancontexts = cell(1, num_data_save); +ringkeys = zeros(num_data_save, num_rings); +xy_poses = zeros(num_data_save, 2); + +for data_idx = 1:num_data + + if(rem(data_idx, skip_data_frame) ~=0) + continue; + end + + file_name = data_names{data_idx}; + data_time = str2double(file_name(1:end-4)); + data_path = strcat(lidar_data_dir, file_name); + + % get + ptcloud = readBin(data_path); + sc = Ptcloud2ScanContext(ptcloud, shape(2), shape(1), 80); % up to 80 meter + + rk = ringkey(sc); + +% [nearest_time_gap, nearest_idx] = min(abs(repmat(data_time, length(gtpose_time), 1) - gtpose_time)); + xy_pose = gtpose_xy(data_idx, :); + + % save + scancontexts{save_counter} = sc; + ringkeys(save_counter, :) = rk; + xy_poses(save_counter, :) = xy_pose; + save_counter = save_counter + 1; + + % log + if(rem(data_idx, 100) == 0) + message = strcat(num2str(data_idx), " / ", num2str(num_data), " processed (skip: ", num2str(skip_data_frame), ")"); + disp(message); + end +end + +scancontexts = scancontexts(1:save_counter-1); +ringkeys = ringkeys(1:save_counter-1, :); +xy_poses = xy_poses(1:save_counter-1, :); + + +end diff --git a/fast_evaluator/src/osdir.m b/fast_evaluator/src/osdir.m new file mode 100644 index 0000000..2698637 --- /dev/null +++ b/fast_evaluator/src/osdir.m @@ -0,0 +1,4 @@ +function [files] = osdir(path) + files = dir(path); files(1:2) = []; files = {files(:).name}; +end + diff --git a/fast_evaluator/src/readBin.m b/fast_evaluator/src/readBin.m new file mode 100644 index 0000000..011e3de --- /dev/null +++ b/fast_evaluator/src/readBin.m @@ -0,0 +1,10 @@ +function ptcloud = readBin(bin_path) + +%% Read +fid = fopen(bin_path, 'rb'); raw_data = fread(fid, [4 inf], 'single'); fclose(fid); +points = raw_data(1:3,:)'; +points(:, 3) = points(:, 3) + 1.9; % z in car coord. + +ptcloud = pointCloud(points); + +end % end of function diff --git a/fast_evaluator/src/resize_polar_img.m b/fast_evaluator/src/resize_polar_img.m new file mode 100644 index 0000000..f1250fc --- /dev/null +++ b/fast_evaluator/src/resize_polar_img.m @@ -0,0 +1,29 @@ +function [down_img] = resize_polar_img(varargin) + +%% +% arg 1: target image +% arg 2: size of the downsized image; the number of [r, theta] for 200m, 360 deg +% arg 3: interpolation type + +%% +if nargin == 1 + img = varargin{1}; + rescale_pixel = [40, 60]; + interpolation_method = 'box'; +end + +if nargin == 2 + img = varargin{1}; + rescale_pixel = varargin{2}; + interpolation_method = 'box'; +end + +if nargin == 3 + img = varargin{1}; + rescale_pixel = varargin{2}; + interpolation_method = varargin{3}; +end + +down_img = imresize(img, rescale_pixel, 'method', interpolation_method); + +end \ No newline at end of file diff --git a/fast_evaluator/src/ringkey.m b/fast_evaluator/src/ringkey.m new file mode 100644 index 0000000..80b24ee --- /dev/null +++ b/fast_evaluator/src/ringkey.m @@ -0,0 +1,13 @@ +function [ ring_key ] = ringkey(sc) + +num_rings = size(sc, 1); + +ring_key = zeros(1, num_rings); +for ith=1:num_rings + ith_ring = sc(ith,:); +% ring_key(ith) = mean(ith_ring); + ring_key(ith) = nnz(ith_ring); +end + +end + diff --git a/fast_evaluator/src/sc_dist.m b/fast_evaluator/src/sc_dist.m new file mode 100644 index 0000000..837fc10 --- /dev/null +++ b/fast_evaluator/src/sc_dist.m @@ -0,0 +1,41 @@ +function [dist] = dist(sc1,sc2) + +num_sectors = size(sc1, 2); + +% repeate to move 1 columns +sim_for_each_cols = zeros(1, num_sectors); + +for i = 1:num_sectors + %% Shift + one_step = 1; % const + sc1 = circshift(sc1, one_step, 2); % 2 means columne shift + + %% compare + sum_of_cos_sim = 0; + num_col_engaged = 0; + + for j = 1:num_sectors + col_j_1 = sc1(:,j); + col_j_2 = sc2(:,j); + + if( ~any(col_j_1) || ~any(col_j_2)) + continue; + end + + % calc sim + cos_similarity = dot(col_j_1, col_j_2) / (norm(col_j_1)*norm(col_j_2)); + sum_of_cos_sim = sum_of_cos_sim + cos_similarity; + + num_col_engaged = num_col_engaged +1; + end + + % devided by num_col_engaged: So, even if there are many columns that are excluded from the calculation, we can get high scores if other columns are well fit. + sim_for_each_cols(i) = sum_of_cos_sim/num_col_engaged; + +end + +sim = max(sim_for_each_cols); + +dist = 1 - sim; + +end diff --git a/fast_evaluator_radar/README.md b/fast_evaluator_radar/README.md new file mode 100644 index 0000000..a955927 --- /dev/null +++ b/fast_evaluator_radar/README.md @@ -0,0 +1,23 @@ +# Radar Scan Context + +- Scan Context also works for the radar data (i.e., Navtech radar) + - Radar Scan Context was introduced in the [MulRan dataset paper](https://irap.kaist.ac.kr/publications/gskim-2020-icra.pdf) + - This directory contains the evaluation code, for [radar place recognition](https://sites.google.com/view/mulran-pr/radar-place-recognition), used in the MulRan paper. + - if you use the dataset or our method, please refer the paper: + ``` + @INPROCEEDINGS { gskim-2020-icra, + AUTHOR = { Giseop Kim, Yeong Sang Park, Younghun Cho, Jinyong Jeong, Ayoung Kim }, + TITLE = { MulRan: Multimodal Range Dataset for Urban Place Recognition }, + BOOKTITLE = { Proceedings of the IEEE International Conference on Robotics and Automation (ICRA) }, + YEAR = { 2020 }, + MONTH = { May }, + ADDRESS = { Paris } + } + ``` + +- More information about the radar data, please refer [MulRan](https://sites.google.com/view/mulran-pr/home) or Oxford Radar RobotCar dataset + +## How to use +- 1. write your own MulRan dataset path in the main.m file +- 2. run main.m (then some data and evaluation files will be generated) +- 3. run prcurve_drawer.m diff --git a/fast_evaluator_radar/linspecer.m b/fast_evaluator_radar/linspecer.m new file mode 100755 index 0000000..434a398 --- /dev/null +++ b/fast_evaluator_radar/linspecer.m @@ -0,0 +1,261 @@ +% function lineStyles = linspecer(N) +% This function creates an Nx3 array of N [R B G] colors +% These can be used to plot lots of lines with distinguishable and nice +% looking colors. +% +% lineStyles = linspecer(N); makes N colors for you to use: lineStyles(ii,:) +% +% colormap(linspecer); set your colormap to have easily distinguishable +% colors and a pleasing aesthetic +% +% lineStyles = linspecer(N,'qualitative'); forces the colors to all be distinguishable (up to 12) +% lineStyles = linspecer(N,'sequential'); forces the colors to vary along a spectrum +% +% % Examples demonstrating the colors. +% +% LINE COLORS +% N=6; +% X = linspace(0,pi*3,1000); +% Y = bsxfun(@(x,n)sin(x+2*n*pi/N), X.', 1:N); +% C = linspecer(N); +% axes('NextPlot','replacechildren', 'ColorOrder',C); +% plot(X,Y,'linewidth',5) +% ylim([-1.1 1.1]); +% +% SIMPLER LINE COLOR EXAMPLE +% N = 6; X = linspace(0,pi*3,1000); +% C = linspecer(N) +% hold off; +% for ii=1:N +% Y = sin(X+2*ii*pi/N); +% plot(X,Y,'color',C(ii,:),'linewidth',3); +% hold on; +% end +% +% COLORMAP EXAMPLE +% A = rand(15); +% figure; imagesc(A); % default colormap +% figure; imagesc(A); colormap(linspecer); % linspecer colormap +% +% See also NDHIST, NHIST, PLOT, COLORMAP, 43700-cubehelix-colormaps +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% by Jonathan Lansey, March 2009-2013 – Lansey at gmail.com % +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% +%% credits and where the function came from +% The colors are largely taken from: +% http://colorbrewer2.org and Cynthia Brewer, Mark Harrower and The Pennsylvania State University +% +% +% She studied this from a phsychometric perspective and crafted the colors +% beautifully. +% +% I made choices from the many there to decide the nicest once for plotting +% lines in Matlab. I also made a small change to one of the colors I +% thought was a bit too bright. In addition some interpolation is going on +% for the sequential line styles. +% +% +%% + +function lineStyles=linspecer(N,varargin) + +if nargin==0 % return a colormap + lineStyles = linspecer(128); + return; +end + +if ischar(N) + lineStyles = linspecer(128,N); + return; +end + +if N<=0 % its empty, nothing else to do here + lineStyles=[]; + return; +end + +% interperet varagin +qualFlag = 0; +colorblindFlag = 0; + +if ~isempty(varargin)>0 % you set a parameter? + switch lower(varargin{1}) + case {'qualitative','qua'} + if N>12 % go home, you just can't get this. + warning('qualitiative is not possible for greater than 12 items, please reconsider'); + else + if N>9 + warning(['Default may be nicer for ' num2str(N) ' for clearer colors use: whitebg(''black''); ']); + end + end + qualFlag = 1; + case {'sequential','seq'} + lineStyles = colorm(N); + return; + case {'white','whitefade'} + lineStyles = whiteFade(N);return; + case 'red' + lineStyles = whiteFade(N,'red');return; + case 'blue' + lineStyles = whiteFade(N,'blue');return; + case 'green' + lineStyles = whiteFade(N,'green');return; + case {'gray','grey'} + lineStyles = whiteFade(N,'gray');return; + case {'colorblind'} + colorblindFlag = 1; + otherwise + warning(['parameter ''' varargin{1} ''' not recognized']); + end +end +% *.95 +% predefine some colormaps + set3 = colorBrew2mat({[141, 211, 199];[ 255, 237, 111];[ 190, 186, 218];[ 251, 128, 114];[ 128, 177, 211];[ 253, 180, 98];[ 179, 222, 105];[ 188, 128, 189];[ 217, 217, 217];[ 204, 235, 197];[ 252, 205, 229];[ 255, 255, 179]}'); +set1JL = brighten(colorBrew2mat({[228, 26, 28];[ 55, 126, 184]; [ 77, 175, 74];[ 255, 127, 0];[ 255, 237, 111]*.85;[ 166, 86, 40];[ 247, 129, 191];[ 153, 153, 153];[ 152, 78, 163]}')); +set1 = brighten(colorBrew2mat({[ 55, 126, 184]*.85;[228, 26, 28];[ 77, 175, 74];[ 255, 127, 0];[ 152, 78, 163]}),.8); + +% colorblindSet = {[215,25,28];[253,174,97];[171,217,233];[44,123,182]}; +colorblindSet = {[215,25,28];[253,174,97];[171,217,233]*.8;[44,123,182]*.8}; + +set3 = dim(set3,.93); + +if colorblindFlag + switch N + % sorry about this line folks. kind of legacy here because I used to + % use individual 1x3 cells instead of nx3 arrays + case 4 + lineStyles = colorBrew2mat(colorblindSet); + otherwise + colorblindFlag = false; + warning('sorry unsupported colorblind set for this number, using regular types'); + end +end +if ~colorblindFlag + switch N + case 1 + lineStyles = { [ 55, 126, 184]/255}; + case {2, 3, 4, 5 } + lineStyles = set1(1:N); + case {6 , 7, 8, 9} + lineStyles = set1JL(1:N)'; + case {10, 11, 12} + if qualFlag % force qualitative graphs + lineStyles = set3(1:N)'; + else % 10 is a good number to start with the sequential ones. + lineStyles = cmap2linspecer(colorm(N)); + end + otherwise % any old case where I need a quick job done. + lineStyles = cmap2linspecer(colorm(N)); + end +end +lineStyles = cell2mat(lineStyles); + +end + +% extra functions +function varIn = colorBrew2mat(varIn) +for ii=1:length(varIn) % just divide by 255 + varIn{ii}=varIn{ii}/255; +end +end + +function varIn = brighten(varIn,varargin) % increase the brightness + +if isempty(varargin), + frac = .9; +else + frac = varargin{1}; +end + +for ii=1:length(varIn) + varIn{ii}=varIn{ii}*frac+(1-frac); +end +end + +function varIn = dim(varIn,f) + for ii=1:length(varIn) + varIn{ii} = f*varIn{ii}; + end +end + +function vOut = cmap2linspecer(vIn) % changes the format from a double array to a cell array with the right format +vOut = cell(size(vIn,1),1); +for ii=1:size(vIn,1) + vOut{ii} = vIn(ii,:); +end +end +%% +% colorm returns a colormap which is really good for creating informative +% heatmap style figures. +% No particular color stands out and it doesn't do too badly for colorblind people either. +% It works by interpolating the data from the +% 'spectral' setting on http://colorbrewer2.org/ set to 11 colors +% It is modified a little to make the brightest yellow a little less bright. +function cmap = colorm(varargin) +n = 100; +if ~isempty(varargin) + n = varargin{1}; +end + +if n==1 + cmap = [0.2005 0.5593 0.7380]; + return; +end +if n==2 + cmap = [0.2005 0.5593 0.7380; + 0.9684 0.4799 0.2723]; + return; +end + +frac=.95; % Slight modification from colorbrewer here to make the yellows in the center just a bit darker +cmapp = [158, 1, 66; 213, 62, 79; 244, 109, 67; 253, 174, 97; 254, 224, 139; 255*frac, 255*frac, 191*frac; 230, 245, 152; 171, 221, 164; 102, 194, 165; 50, 136, 189; 94, 79, 162]; +x = linspace(1,n,size(cmapp,1)); +xi = 1:n; +cmap = zeros(n,3); +for ii=1:3 + cmap(:,ii) = pchip(x,cmapp(:,ii),xi); +end +cmap = flipud(cmap/255); +end + +function cmap = whiteFade(varargin) +n = 100; +if nargin>0 + n = varargin{1}; +end + +thisColor = 'blue'; + +if nargin>1 + thisColor = varargin{2}; +end +switch thisColor + case {'gray','grey'} + cmapp = [255,255,255;240,240,240;217,217,217;189,189,189;150,150,150;115,115,115;82,82,82;37,37,37;0,0,0]; + case 'green' + cmapp = [247,252,245;229,245,224;199,233,192;161,217,155;116,196,118;65,171,93;35,139,69;0,109,44;0,68,27]; + case 'blue' + cmapp = [247,251,255;222,235,247;198,219,239;158,202,225;107,174,214;66,146,198;33,113,181;8,81,156;8,48,107]; + case 'red' + cmapp = [255,245,240;254,224,210;252,187,161;252,146,114;251,106,74;239,59,44;203,24,29;165,15,21;103,0,13]; + otherwise + warning(['sorry your color argument ' thisColor ' was not recognized']); +end + +cmap = interpomap(n,cmapp); +end + +% Eat a approximate colormap, then interpolate the rest of it up. +function cmap = interpomap(n,cmapp) + x = linspace(1,n,size(cmapp,1)); + xi = 1:n; + cmap = zeros(n,3); + for ii=1:3 + cmap(:,ii) = pchip(x,cmapp(:,ii),xi); + end + cmap = (cmap/255); % flipud?? +end + + + diff --git a/fast_evaluator_radar/main.m b/fast_evaluator_radar/main.m new file mode 100755 index 0000000..c9da55f --- /dev/null +++ b/fast_evaluator_radar/main.m @@ -0,0 +1,162 @@ +clear; clc; + +addpath(genpath('src')); +addpath(genpath('data')); + +%% data preparation +global data_path; +% data_path = '/your/mulran/sequence/dir/Riverside02/'; +data_path = '/media/user/My Passport/data/MulRan_eval/Riverside_2_20190816/20190816/'; +% ### NOTE: Use this sequence directory structure +% example: +% /your/MulRan/sequence/dir/Riverside02/ +% L sensor_data/ +% L radar/ +% L polar/ +% L {unix_times}.png +% L global_pose.csv + +down_shape = [40, 120]; +[data_scancontexts, data_ringkeys, data_poses] = loadData(down_shape); + + +%% main - global recognizer +revisit_criteria = 5; % in meter (recommend test for 5, 10, 20 meters) +keyframe_gap = 1; % for_fast_eval (if 1, no skip) + +global num_candidates; num_candidates = 5; +% NOTE about num_candidates +% - 50 was used in the MulRan paper +% - But we found, interestingly, using less keys showed similar +% performance - also we can save the computation time of course. +% - That means our ring key has good disriminative power. + +global num_node_enough_apart; num_node_enough_apart = 50; + +% policy (top N) +num_top_n = 25; +top_n = linspace(1, num_top_n, num_top_n); + +% Entropy thresholds +middle_thres = 0.01; +thresholds1 = linspace(0, middle_thres, 50); +thresholds2 = linspace(middle_thres, 1, 50); +thresholds = [thresholds1, thresholds2]; +num_thresholds = length(thresholds); + +% Main variables to store the result for drawing PR curve +num_hits = zeros(num_top_n, num_thresholds); +num_false_alarms = zeros(num_top_n, num_thresholds); +num_correct_rejections = zeros(num_top_n, num_thresholds); +num_misses = zeros(num_top_n, num_thresholds); + +% main +loop_log = []; + +exp_poses = []; +exp_ringkeys = []; +exp_scancontexts = {}; + +num_queries = length(data_poses); +for query_idx = 1:num_queries - 1 + + % save to (online) DB + query_sc = data_scancontexts{query_idx}; + query_rk = data_ringkeys(query_idx, :); + query_pose = data_poses(query_idx,:); + + exp_scancontexts{end+1} = query_sc; + exp_poses = [exp_poses; query_pose]; + exp_ringkeys = [exp_ringkeys; query_rk]; + + if(rem(query_idx, keyframe_gap) ~= 0) + continue; + end + + if( length(exp_scancontexts) < num_node_enough_apart ) + continue; + end + + tree = createns(exp_ringkeys(1:end-(num_node_enough_apart-1), :), 'NSMethod', 'kdtree'); % Create object to use in k-nearest neighbor search + + % revisitness + [revisitness, how_far_apart] = isRevisitGlobalLoc(query_pose, exp_poses(1:end-(num_node_enough_apart-1), :), revisit_criteria); + + % find candidates + candidates = knnsearch(tree, query_rk, 'K', num_candidates); + + % find the nearest (top 1) via pairwise comparison + nearest_idx = 0; + min_dist = inf; % initialization + for ith_candidate = 1:length(candidates) + candidate_node_idx = candidates(ith_candidate); + candidate_img = exp_scancontexts{candidate_node_idx}; + +% if( abs(query_idx - candidate_node_idx) < num_node_enough_apart) +% continue; +% end + + distance_to_query = sc_dist(query_sc, candidate_img); + + if( distance_to_query < min_dist) + nearest_idx = candidate_node_idx; + min_dist = distance_to_query; + end + end + + % prcurve analysis + for topk = 1:num_top_n + for thres_idx = 1:num_thresholds + threshold = thresholds(thres_idx); + + reject = 0; + if( min_dist > threshold) + reject = 1; + end + + if(reject == 1) + if(revisitness == 0) + % TN: Correct Rejection + num_correct_rejections(topk, thres_idx) = num_correct_rejections(topk, thres_idx) + 1; + else + % FN: MISS + num_misses(topk, thres_idx) = num_misses(topk, thres_idx) + 1; + end + else + % if under the theshold, it is considered seen. + % and then check the correctness + if( dist_btn_pose(query_pose, exp_poses(nearest_idx, :)) < revisit_criteria) + % TP: Hit + num_hits(topk, thres_idx) = num_hits(topk, thres_idx) + 1; + else + % FP: False Alarm + num_false_alarms(topk, thres_idx) = num_false_alarms(topk, thres_idx) + 1; + end + end + + end + end + + if( rem(query_idx, 100) == 0) + disp( strcat(num2str(query_idx/num_queries * 100), ' % processed') ); + end + +end + + +%% save the log +savePath = strcat("pr_result/within ", num2str(revisit_criteria), "m/"); +if((~7==exist(savePath,'dir'))) + mkdir(savePath); +end +save(strcat(savePath, 'nCorrectRejections.mat'), 'num_correct_rejections'); +save(strcat(savePath, 'nMisses.mat'), 'num_misses'); +save(strcat(savePath, 'nHits.mat'), 'num_hits'); +save(strcat(savePath, 'nFalseAlarms.mat'), 'num_false_alarms'); + + + + + + + diff --git a/fast_evaluator_radar/prcurve_drawer.m b/fast_evaluator_radar/prcurve_drawer.m new file mode 100755 index 0000000..f5d409d --- /dev/null +++ b/fast_evaluator_radar/prcurve_drawer.m @@ -0,0 +1,132 @@ +rmpath(genpath('../../')); + +ResultsDir = './pr_result/'; + +%% +title_str = strcat('MulRan Sequence (radar polar)'); + +%% Params +FigIdx = 2; +figure(FigIdx); clf; + +TopNindexes = [1]; +name = 'top1'; + +nTopNindexes = length(TopNindexes); + +%% Main +SequenceNames = dir(ResultsDir); SequenceNames(1:2, :) = []; SequenceNames = {SequenceNames(:).name}; +nSequences = length(SequenceNames); + +all_Precisions = {}; +all_Recalls = {}; + +for ithTopN = 1:nTopNindexes + + TopNidx = TopNindexes(ithTopN); + + line_width = 4; + + LineColors = colorcube(nSequences); + LineColors = linspecer(nSequences,'qualitative'); +% LineColors = linspecer(nSequences,'sequential'); + LineColors = flipud(LineColors); + + AUCs = zeros(1, nSequences); + for ithSeq = 1:nSequences + + % seq info + ithSeqName = SequenceNames{ithSeq}; + SequenceNames{ithSeq} = string(ithSeqName); + + ithSeqPath = strcat(ResultsDir, ithSeqName, '/'); + ithSeqPRcurveData = dir(ithSeqPath); ithSeqPRcurveData(1:2, :) = []; ithSeqPRcurveData = {ithSeqPRcurveData(:).name}; + + % load + nCorrectRejectionsAll = load(strcat(ithSeqPath, ithSeqPRcurveData{1})); + nCorrectRejectionsAll = nCorrectRejectionsAll.num_correct_rejections; + nCorrectRejectionsForThisTopN = nCorrectRejectionsAll(TopNidx, :); + + nFalseAlarmsAll = load(strcat(ithSeqPath, ithSeqPRcurveData{2})); + nFalseAlarmsAll = nFalseAlarmsAll.num_false_alarms; + nFalseAlarmsForThisTopN = nFalseAlarmsAll(TopNidx, :); + + nHitsAll = load(strcat(ithSeqPath, ithSeqPRcurveData{3})); + nHitsAll = nHitsAll.num_hits; + nHitsForThisTopN = nHitsAll(TopNidx, :); + + nMissesAll = load(strcat(ithSeqPath, ithSeqPRcurveData{4})); + nMissesAll = nMissesAll.num_misses; + nMissesForThisTopN = nMissesAll(TopNidx, :); + + % info + nTopNs = size(nCorrectRejectionsAll, 1); + nThres = size(nCorrectRejectionsAll, 2); + + % main + Precisions = []; + Recalls = []; + Accuracies = []; + for ithThres = 1:nThres + nCorrectRejections = nCorrectRejectionsForThisTopN(ithThres); + nFalseAlarms = nFalseAlarmsForThisTopN(ithThres); + nHits = nHitsForThisTopN(ithThres); + nMisses = nMissesForThisTopN(ithThres); + + nTotalTestPlaces = nCorrectRejections + nFalseAlarms + nHits + nMisses; + + Precision = nHits / (nHits + nFalseAlarms); + Recall = nHits / (nHits + nMisses); + Acc = (nHits + nCorrectRejections)/nTotalTestPlaces; + + Precisions = [Precisions; Precision]; + Recalls = [Recalls; Recall]; + Accuracies = [Accuracies; Acc]; + end + + num_points = length(Precisions); + Precisions(1) = 1; + AUC = 0; + for ith = 1:num_points-1 + small_area = 1/2 * (Precisions(ith) + Precisions(ith+1)) * (Recalls(ith+1)-Recalls(ith)); + AUC = AUC + small_area; + end + AUCs(ithSeq) = AUC; + + all_Precisions{ithSeq} = Precisions; + all_Recalls{ithSeq} = Recalls; + + % draw + figure(FigIdx); + set(gcf, 'Position', [10 10 800 500]); + + fontsize = 10; + p = plot(Recalls, Precisions, 'LineWidth', line_width); % commonly x axis is recall + title(title_str, 'FontSize', fontsize); + xlabel('Recall', 'FontSize', fontsize); ylabel('Precision', 'FontSize', fontsize); + set(gca, 'FontSize', fontsize+5) + xticks([0 0.2 0.4 0.6 0.8 1.0]) + xticklabels({'0','0.2','0.4','0.6','0.8','1'}) + yticks([0 0.2 0.4 0.6 0.8 1.0]) + yticklabels({'0','0.2','0.4','0.6','0.8','1'}) + + p(1).Color = LineColors(ithSeq, :); + p(1).MarkerEdgeColor = LineColors(ithSeq, :); + % axis equal; + xlim([0, 1]); ylim([0,1]); + grid on; grid minor; + hold on; + + end + + lgd = legend(SequenceNames, 'Location', 'best'); + lgd.FontSize = fontsize + 3; + lgd.FontWeight = 'bold'; + + grid minor; + + name = 'prcurve'; + print('-bestfit', name,'-dpdf') + +end + diff --git a/fast_evaluator_radar/src/deg2utm.m b/fast_evaluator_radar/src/deg2utm.m new file mode 100755 index 0000000..a6527fa --- /dev/null +++ b/fast_evaluator_radar/src/deg2utm.m @@ -0,0 +1,121 @@ +function [x,y,utmzone] = deg2utm(Lat,Lon) +% ------------------------------------------------------------------------- +% [x,y,utmzone] = deg2utm(Lat,Lon) +% +% Description: Function to convert lat/lon vectors into UTM coordinates (WGS84). +% Some code has been extracted from UTM.m function by Gabriel Ruiz Martinez. +% +% Inputs: +% Lat: Latitude vector. Degrees. +ddd.ddddd WGS84 +% Lon: Longitude vector. Degrees. +ddd.ddddd WGS84 +% +% Outputs: +% x, y , utmzone. See example +% +% Example 1: +% Lat=[40.3154333; 46.283900; 37.577833; 28.645650; 38.855550; 25.061783]; +% Lon=[-3.4857166; 7.8012333; -119.95525; -17.759533; -94.7990166; 121.640266]; +% [x,y,utmzone] = deg2utm(Lat,Lon); +% fprintf('%7.0f ',x) +% 458731 407653 239027 230253 343898 362850 +% fprintf('%7.0f ',y) +% 4462881 5126290 4163083 3171843 4302285 2772478 +% utmzone = +% 30 T +% 32 T +% 11 S +% 28 R +% 15 S +% 51 R +% +% Example 2: If you have Lat/Lon coordinates in Degrees, Minutes and Seconds +% LatDMS=[40 18 55.56; 46 17 2.04]; +% LonDMS=[-3 29 8.58; 7 48 4.44]; +% Lat=dms2deg(mat2dms(LatDMS)); %convert into degrees +% Lon=dms2deg(mat2dms(LonDMS)); %convert into degrees +% [x,y,utmzone] = deg2utm(Lat,Lon) +% +% Author: +% Rafael Palacios +% Universidad Pontificia Comillas +% Madrid, Spain +% Version: Apr/06, Jun/06, Aug/06, Aug/06 +% Aug/06: fixed a problem (found by Rodolphe Dewarrat) related to southern +% hemisphere coordinates. +% Aug/06: corrected m-Lint warnings +%------------------------------------------------------------------------- +% Argument checking +% +error(nargchk(2, 2, nargin)); %2 arguments required +n1=length(Lat); +n2=length(Lon); +if (n1~=n2) + error('Lat and Lon vectors should have the same length'); +end +% Memory pre-allocation +% +x=zeros(n1,1); +y=zeros(n1,1); +utmzone(n1,:)='60 X'; +% Main Loop +% +for i=1:n1 + la=Lat(i); + lo=Lon(i); + sa = 6378137.000000 ; sb = 6356752.314245; + + %e = ( ( ( sa ^ 2 ) - ( sb ^ 2 ) ) ^ 0.5 ) / sa; + e2 = ( ( ( sa ^ 2 ) - ( sb ^ 2 ) ) ^ 0.5 ) / sb; + e2cuadrada = e2 ^ 2; + c = ( sa ^ 2 ) / sb; + %alpha = ( sa - sb ) / sa; %f + %ablandamiento = 1 / alpha; % 1/f + lat = la * ( pi / 180 ); + lon = lo * ( pi / 180 ); + Huso = fix( ( lo / 6 ) + 31); + S = ( ( Huso * 6 ) - 183 ); + deltaS = lon - ( S * ( pi / 180 ) ); + if (la<-72), Letra='C'; + elseif (la<-64), Letra='D'; + elseif (la<-56), Letra='E'; + elseif (la<-48), Letra='F'; + elseif (la<-40), Letra='G'; + elseif (la<-32), Letra='H'; + elseif (la<-24), Letra='J'; + elseif (la<-16), Letra='K'; + elseif (la<-8), Letra='L'; + elseif (la<0), Letra='M'; + elseif (la<8), Letra='N'; + elseif (la<16), Letra='P'; + elseif (la<24), Letra='Q'; + elseif (la<32), Letra='R'; + elseif (la<40), Letra='S'; + elseif (la<48), Letra='T'; + elseif (la<56), Letra='U'; + elseif (la<64), Letra='V'; + elseif (la<72), Letra='W'; + else Letra='X'; + end + a = cos(lat) * sin(deltaS); + epsilon = 0.5 * log( ( 1 + a) / ( 1 - a ) ); + nu = atan( tan(lat) / cos(deltaS) ) - lat; + v = ( c / ( ( 1 + ( e2cuadrada * ( cos(lat) ) ^ 2 ) ) ) ^ 0.5 ) * 0.9996; + ta = ( e2cuadrada / 2 ) * epsilon ^ 2 * ( cos(lat) ) ^ 2; + a1 = sin( 2 * lat ); + a2 = a1 * ( cos(lat) ) ^ 2; + j2 = lat + ( a1 / 2 ); + j4 = ( ( 3 * j2 ) + a2 ) / 4; + j6 = ( ( 5 * j4 ) + ( a2 * ( cos(lat) ) ^ 2) ) / 3; + alfa = ( 3 / 4 ) * e2cuadrada; + beta = ( 5 / 3 ) * alfa ^ 2; + gama = ( 35 / 27 ) * alfa ^ 3; + Bm = 0.9996 * c * ( lat - alfa * j2 + beta * j4 - gama * j6 ); + xx = epsilon * v * ( 1 + ( ta / 3 ) ) + 500000; + yy = nu * v * ( 1 + ta ) + Bm; + if (yy<0) + yy=9999999+yy; + end + x(i)=xx; + y(i)=yy; + utmzone(i,:)=sprintf('%02d %c',Huso,Letra); +end diff --git a/fast_evaluator_radar/src/dist_btn_pose.m b/fast_evaluator_radar/src/dist_btn_pose.m new file mode 100755 index 0000000..39aa646 --- /dev/null +++ b/fast_evaluator_radar/src/dist_btn_pose.m @@ -0,0 +1,3 @@ +function dist = dist_btn_pose(pose1, pose2) + dist = sqrt( (pose1(1) - pose2(1))^2 + (pose1(2) - pose2(2))^2); +end diff --git a/fast_evaluator_radar/src/find_topk_from_candidates.m b/fast_evaluator_radar/src/find_topk_from_candidates.m new file mode 100755 index 0000000..96b899e --- /dev/null +++ b/fast_evaluator_radar/src/find_topk_from_candidates.m @@ -0,0 +1,28 @@ +function [ nearest_idx, min_dist ] = find_topk_from_candidates(query_img, query_idx, candidates, thres) + +global num_node_enough_apart; +global radar_imgs; + +nearest_idx = 0; +min_dist = inf; % initialization +for ith_candidate = 1:length(candidates) + candidate_node_idx = candidates(ith_candidate); + candidate_img = radar_imgs{candidate_node_idx}; + + if( abs(query_idx - candidate_node_idx) < num_node_enough_apart) + continue; + end + + distance_to_query = dist(query_img, candidate_img); + if( distance_to_query > thres) + continue; + end + + if( distance_to_query < min_dist) + nearest_idx = candidate_node_idx; + min_dist = distance_to_query; + end +end + +end + diff --git a/fast_evaluator_radar/src/isRevisitGlobalLoc.m b/fast_evaluator_radar/src/isRevisitGlobalLoc.m new file mode 100755 index 0000000..ef532c2 --- /dev/null +++ b/fast_evaluator_radar/src/isRevisitGlobalLoc.m @@ -0,0 +1,20 @@ +function [is_revisit, min_dist] = isRevisitGlobalLoc(query_pose, db_poses, thres) + +num_dbs = size(db_poses, 1); + +dists = zeros(1, num_dbs); +for ii=1:num_dbs + dist = norm(query_pose - db_poses(ii, :)); + dists(ii) = dist; +end + +if ( min(dists) < thres ) + is_revisit = 1; +else + is_revisit = 0; +end + +min_dist = min(dists); + +end + diff --git a/fast_evaluator_radar/src/is_revisit.m b/fast_evaluator_radar/src/is_revisit.m new file mode 100755 index 0000000..4cd2ca3 --- /dev/null +++ b/fast_evaluator_radar/src/is_revisit.m @@ -0,0 +1,23 @@ +function [ revisitness ] = is_revisit(query_idx, query_pose, radar_poses, revisit_criteria, num_node_enough_apart) + +num_db = size(radar_poses, 1); + +revisitness = 0; +for ii = 1:num_db + + if( abs(query_idx - ii) < num_node_enough_apart) + continue; + end + + pose = radar_poses(ii, :); + + dist = dist_btn_pose(query_pose, pose); + + if(dist < revisit_criteria) + revisitness = 1; + break; + end +end + +end + diff --git a/fast_evaluator_radar/src/loadData.m b/fast_evaluator_radar/src/loadData.m new file mode 100755 index 0000000..ba366b7 --- /dev/null +++ b/fast_evaluator_radar/src/loadData.m @@ -0,0 +1,47 @@ +function [scancontexts, ringkeys, poses] = loadData(down_shape) + +%% +global data_path; + +data_save_path = fullfile('.', 'data/'); + +%% +% newly make +is_already_made_data_exist = exist(data_save_path); +if is_already_made_data_exist == 0 + % make + [scancontexts, ringkeys, poses] = makeExperience(data_path, down_shape); + + % save + mkdir(data_save_path); + + filename = strcat(data_save_path, 'scancontexts', num2str(down_shape(1)), 'x', num2str(down_shape(2)), '.mat'); + save(filename, 'scancontexts'); + filename = strcat(data_save_path, 'ringkeys', num2str(down_shape(1)), 'x', num2str(down_shape(2)), '.mat'); + save(filename, 'ringkeys'); + filename = strcat(data_save_path, 'poses', num2str(down_shape(1)), 'x', num2str(down_shape(2)), '.mat'); + save(filename, 'poses'); + +% or load +else + filename = strcat(data_save_path, 'scancontexts', num2str(down_shape(1)), 'x', num2str(down_shape(2)), '.mat'); + load(filename); + % fix + for iii = 1:length(scancontexts) + sc = double(scancontexts{iii}); + scancontexts{iii} = sc; + end + + filename = strcat(data_save_path, 'ringkeys', num2str(down_shape(1)), 'x', num2str(down_shape(2)), '.mat'); + load(filename); + filename = strcat(data_save_path, 'poses', num2str(down_shape(1)), 'x', num2str(down_shape(2)), '.mat'); + load(filename); + + disp('- successfully loaded.'); +end + +%% +disp(' '); + +end + diff --git a/fast_evaluator_radar/src/makeExperience.m b/fast_evaluator_radar/src/makeExperience.m new file mode 100755 index 0000000..d52515f --- /dev/null +++ b/fast_evaluator_radar/src/makeExperience.m @@ -0,0 +1,54 @@ +function [scancontexts, ringkeys, xy_poses] = makeExperience(data_dir, shape) + +%% +num_rings = shape(1); +num_sectors = shape(2); + +%% +radar_data_dir = fullfile(data_dir, 'sensor_data/radar/polar/'); +data_names = osdir(radar_data_dir); + +%% gps to xyz +gtpose = csvread(strcat(data_dir, 'global_pose.csv')); +gtpose_time = gtpose(:, 1); +gtpose_xy = gtpose(:, [5,9]); + +% figure(1); hold on; +% plot(traj_x, traj_y); + +%% +num_data = length(data_names); + +scancontexts = cell(1, num_data); +ringkeys = zeros(num_data, num_rings); +xy_poses = zeros(num_data, 2); + +for data_idx = 1:num_data + file_name = data_names{data_idx}; + data_time = str2double(file_name(1:end-4)); + data_path = strcat(radar_data_dir, file_name); + + % get + sc = imread(data_path); + sc = imresize(sc, shape); + sc = double(sc); + + rk = ringkey(sc); + + [nearest_time_gap, nearest_idx] = min(abs(repmat(data_time, length(gtpose_time), 1) - gtpose_time)); + xy_pose = gtpose_xy(nearest_idx, :); + + % save + scancontexts{data_idx} = sc; + ringkeys(data_idx, :) = rk; + xy_poses(data_idx, :) = xy_pose; + + % log + if(rem(data_idx, 100) == 0) + message = strcat(num2str(data_idx), " / ", num2str(num_data), " processed."); + disp(message); + end +end + +end + diff --git a/fast_evaluator_radar/src/osdir.m b/fast_evaluator_radar/src/osdir.m new file mode 100755 index 0000000..2698637 --- /dev/null +++ b/fast_evaluator_radar/src/osdir.m @@ -0,0 +1,4 @@ +function [files] = osdir(path) + files = dir(path); files(1:2) = []; files = {files(:).name}; +end + diff --git a/fast_evaluator_radar/src/resize_polar_img.m b/fast_evaluator_radar/src/resize_polar_img.m new file mode 100755 index 0000000..f1250fc --- /dev/null +++ b/fast_evaluator_radar/src/resize_polar_img.m @@ -0,0 +1,29 @@ +function [down_img] = resize_polar_img(varargin) + +%% +% arg 1: target image +% arg 2: size of the downsized image; the number of [r, theta] for 200m, 360 deg +% arg 3: interpolation type + +%% +if nargin == 1 + img = varargin{1}; + rescale_pixel = [40, 60]; + interpolation_method = 'box'; +end + +if nargin == 2 + img = varargin{1}; + rescale_pixel = varargin{2}; + interpolation_method = 'box'; +end + +if nargin == 3 + img = varargin{1}; + rescale_pixel = varargin{2}; + interpolation_method = varargin{3}; +end + +down_img = imresize(img, rescale_pixel, 'method', interpolation_method); + +end \ No newline at end of file diff --git a/fast_evaluator_radar/src/ringkey.m b/fast_evaluator_radar/src/ringkey.m new file mode 100755 index 0000000..cdfb761 --- /dev/null +++ b/fast_evaluator_radar/src/ringkey.m @@ -0,0 +1,12 @@ +function [ ring_key ] = ringkey(sc) + +num_rings = size(sc, 1); + +ring_key = zeros(1, num_rings); +for ith=1:num_rings + ith_ring = sc(ith,:); + ring_key(ith) = mean(ith_ring); +end + +end + diff --git a/fast_evaluator_radar/src/sc_dist.m b/fast_evaluator_radar/src/sc_dist.m new file mode 100755 index 0000000..837fc10 --- /dev/null +++ b/fast_evaluator_radar/src/sc_dist.m @@ -0,0 +1,41 @@ +function [dist] = dist(sc1,sc2) + +num_sectors = size(sc1, 2); + +% repeate to move 1 columns +sim_for_each_cols = zeros(1, num_sectors); + +for i = 1:num_sectors + %% Shift + one_step = 1; % const + sc1 = circshift(sc1, one_step, 2); % 2 means columne shift + + %% compare + sum_of_cos_sim = 0; + num_col_engaged = 0; + + for j = 1:num_sectors + col_j_1 = sc1(:,j); + col_j_2 = sc2(:,j); + + if( ~any(col_j_1) || ~any(col_j_2)) + continue; + end + + % calc sim + cos_similarity = dot(col_j_1, col_j_2) / (norm(col_j_1)*norm(col_j_2)); + sum_of_cos_sim = sum_of_cos_sim + cos_similarity; + + num_col_engaged = num_col_engaged +1; + end + + % devided by num_col_engaged: So, even if there are many columns that are excluded from the calculation, we can get high scores if other columns are well fit. + sim_for_each_cols(i) = sum_of_cos_sim/num_col_engaged; + +end + +sim = max(sim_for_each_cols); + +dist = 1 - sim; + +end diff --git a/matlab/descriptor/Ptcloud2ScanContext.m b/matlab/descriptor/Ptcloud2ScanContext.m new file mode 100755 index 0000000..584659e --- /dev/null +++ b/matlab/descriptor/Ptcloud2ScanContext.m @@ -0,0 +1,102 @@ +function [ img ] = Ptcloud2ScanContext( ptcloud, num_sector, num_ring, max_range ) + +%% Preprocessing + +% Downsampling for fast search +gridStep = 0.5; % 0.5m cubic grid downsampling is applied in the paper. +ptcloud = pcdownsample(ptcloud, 'gridAverage', gridStep); + +% point cloud information +num_points = ptcloud.Count; +gap = max_range / num_ring; +angle_one_sector = 360/num_sector; + + +%% vacant bins +cell_bins = cell(num_ring, num_sector); +cell_bin_counter = ones(num_ring, num_sector); + +enough_large = 500; % for fast and constant time save, We contain maximum 500 points per each bin. +enough_small = -10000; +for ith_ring = 1:num_ring + for ith_sector = 1:num_sector + bin = enough_small * ones(enough_large, 3); + cell_bins{ith_ring, ith_sector} = bin; + end +end + + +%% Save a point to the corresponding bin +for ith_point =1:num_points + + % Point information + ith_point_xyz = ptcloud.Location(ith_point,:); + ith_point_r = sqrt(ith_point_xyz(1)^2 + ith_point_xyz(2)^2); + ith_point_theta = XY2Theta(ith_point_xyz(1), ith_point_xyz(2)); % degree + + % Find the corresponding ring index + tmp_ring_index = floor(ith_point_r/gap); + if(tmp_ring_index >= num_ring) + ring_index = num_ring; + else + ring_index = tmp_ring_index + 1; + end + + % Find the corresponding sector index + tmp_sector_index = ceil(ith_point_theta/angle_one_sector); + if(tmp_sector_index == 0) + sector_index = 1; + elseif(tmp_sector_index > num_sector || tmp_sector_index < 1) + sector_index = num_sector; + else + sector_index = tmp_sector_index; + end + + % Assign point to the corresponding bin cell + try + corresponding_counter = cell_bin_counter(ring_index, sector_index); % 1D real value. + catch + continue; + end + cell_bins{ring_index, sector_index}(corresponding_counter, :) = ith_point_xyz; + cell_bin_counter(ring_index, sector_index) = cell_bin_counter(ring_index, sector_index) + 1; % increase count 1 + +end + + +%% bin to image format (2D matrix) +img = zeros(num_ring, num_sector); + +min_num_thres = 5; % a bin with few points, we consider it is noise. + +% Find maximum Z value of each bin and Save into img +for ith_ring = 1:num_ring + for ith_sector = 1:num_sector + value_of_the_bin = 0; + points_in_bin_ij = cell_bins{ith_ring, ith_sector}; + + if( IsBinHaveMoreThanMinimumPoints(points_in_bin_ij, min_num_thres, enough_small) ) + value_of_the_bin = max(points_in_bin_ij(:, 3)); + else + value_of_the_bin = 0; + end + + img(ith_ring, ith_sector) = value_of_the_bin; + end +end + + +end % end of the main function + + +function bool = IsBinHaveMoreThanMinimumPoints(mat, minimum_thres, enough_small) + +min_thres_point = mat(minimum_thres, :); + +if( isequal(min_thres_point, [ enough_small, enough_small, enough_small]) ) + bool = 0; +else + bool = 1; +end + +end diff --git a/matlab/descriptor/ScanContext2RingKey.m b/matlab/descriptor/ScanContext2RingKey.m new file mode 100755 index 0000000..b00a252 --- /dev/null +++ b/matlab/descriptor/ScanContext2RingKey.m @@ -0,0 +1,14 @@ +function [ ring_key ] = ScanContext2RingKey(sc) + +num_rings = size(sc, 1); +num_sectors = size(sc, 2); + +ring_key = zeros(1, num_rings); +for ith=1:num_rings + ith_ring = sc(ith,:); + num_zeros_ith_shell = nnz(~ith_ring); + ring_key(ith) = (num_sectors - num_zeros_ith_shell)/num_sectors; +end + +end + diff --git a/matlab/descriptor/XY2Theta.m b/matlab/descriptor/XY2Theta.m new file mode 100755 index 0000000..cfa7651 --- /dev/null +++ b/matlab/descriptor/XY2Theta.m @@ -0,0 +1,17 @@ +function [ theta ] = XY2Theta( x, y ) + + if (x >= 0 && y >= 0) + theta = 180/pi * atan(y/x); + end + if (x < 0 && y >= 0) + theta = 180 - ((180/pi) * atan(y/(-x))); + end + if (x < 0 && y < 0) + theta = 180 + ((180/pi) * atan(y/x)); + end + if ( x >= 0 && y < 0) + theta = 360 - ((180/pi) * atan((-y)/x)); + end + +end + diff --git a/matlab/matcher/DistanceBtnScanContexts.m b/matlab/matcher/DistanceBtnScanContexts.m new file mode 100755 index 0000000..58eb73a --- /dev/null +++ b/matlab/matcher/DistanceBtnScanContexts.m @@ -0,0 +1,42 @@ +function [dist] = DistanceBtnScanContexts(sc1,sc2) + +num_sectors = size(sc1, 2); + +% repeate to move 1 columns +sim_for_each_cols = zeros(1, num_sectors); + +for i = 1:num_sectors + %% Shift + one_step = 1; % const + sc1 = circshift(sc1, one_step, 2); % 2 means columne shift + + %% compare + sum_of_cos_sim = 0; + num_col_engaged = 0; + + for j = 1:num_sectors + col_j_1 = sc1(:,j); + col_j_2 = sc2(:,j); + + if( ~any(col_j_1) || ~any(col_j_2)) + continue; + end + + % calc sim + cos_similarity = dot(col_j_1, col_j_2) / (norm(col_j_1)*norm(col_j_2)); + sum_of_cos_sim = sum_of_cos_sim + cos_similarity; + + num_col_engaged = num_col_engaged +1; + end + + % devided by num_col_engaged: So, even if there are many columns that are excluded from the calculation, we can get high scores if other columns are well fit. + sim_for_each_cols(i) = sum_of_cos_sim/num_col_engaged; + +end + +sim = max(sim_for_each_cols); + +dist = 1 - sim; + +end + diff --git a/matlab/reader/KITTIbin2Ptcloud.m b/matlab/reader/KITTIbin2Ptcloud.m new file mode 100755 index 0000000..d745269 --- /dev/null +++ b/matlab/reader/KITTIbin2Ptcloud.m @@ -0,0 +1,10 @@ +function ptcloud = KITTIbin2Ptcloud(bin_path) + +%% Read +fid = fopen(bin_path, 'rb'); raw_data = fread(fid, [4 inf], 'single'); fclose(fid); +points = raw_data(1:3,:)'; +points(:, 3) = points(:, 3) + 1.9; % z in car coord. + +ptcloud = pointCloud(points); + +end % end of function diff --git a/matlab/reader/NCLTbin2Ptcloud.m b/matlab/reader/NCLTbin2Ptcloud.m new file mode 100755 index 0000000..70c1d57 --- /dev/null +++ b/matlab/reader/NCLTbin2Ptcloud.m @@ -0,0 +1,22 @@ +function Ptcloud = NCLTbin2Ptcloud(BinPath) + +%% Hardware Constants +SCALING = 0.005; % 5mm +OFFSET = -100.0; + +%% Reading +fidBin = fopen(BinPath, 'r'); + XYZ_Raw = fread(fidBin, 'uint16'); + XYZ_Scaled = XYZ_Raw*SCALING + OFFSET; +fclose(fidBin); + +XYZ = reshape(XYZ_Scaled, 4, []); +XYZ = transpose(XYZ(1:3, :)); +X = XYZ(:, 1); +Y = XYZ(:, 2); +Z = -1.*XYZ(:, 3); +XYZ = [X, Y, Z]; + +Ptcloud = pointCloud(XYZ); + +end % end of function diff --git a/python/Distance_SC.py b/python/Distance_SC.py new file mode 100644 index 0000000..fe8ee99 --- /dev/null +++ b/python/Distance_SC.py @@ -0,0 +1,72 @@ +import numpy as np +def distance_sc(sc1, sc2): + num_sectors = sc1.shape[1] + # repeate to move 1 columns + sim_for_each_cols = np.zeros(num_sectors) + + for i in range(num_sectors): + # Shift + one_step = 1 # const + sc1 = np.roll(sc1, one_step, axis=1) # columne shift + + #compare + sum_of_cos_sim = 0 + num_col_engaged = 0 + + for j in range(num_sectors): + col_j_1 = sc1[:, j] + col_j_2 = sc2[:, j] + + if (~np.any(col_j_1) or ~np.any(col_j_2)): + continue + + # calc sim + cos_similarity = np.dot(col_j_1, col_j_2) / (np.linalg.norm(col_j_1) * np.linalg.norm(col_j_2)) + sum_of_cos_sim = sum_of_cos_sim + cos_similarity + + num_col_engaged = num_col_engaged + 1 + + # devided by num_col_engaged: So, even if there are many columns that are excluded from the calculation, we + # can get high scores if other columns are well fit. + sim_for_each_cols[i] = sum_of_cos_sim / num_col_engaged + + sim = max(sim_for_each_cols) + + dist = 1 - sim + + return dist + +if __name__ == "__main__": + from python.make_sc_example import * + bin_dir = '../sample_data/KITTI/00/velodyne/' + bin_db = kitti_vlp_database(bin_dir) + SCs = [] + for bin_idx in range(bin_db.num_bins): + bin_file_name = bin_db.bin_files[bin_idx] + bin_path = bin_db.bin_dir + bin_file_name + + sc = ScanContext(bin_dir, bin_file_name) + + # fig_idx = 1 + # # sc.plot_multiple_sc(fig_idx) + # + # print(len(sc.SCs)) + + SCs.append(sc.SCs[0]) + + sc_1a = SCs[0] + sc_1b = SCs[1] + sc_2a = SCs[2] + sc_2b = SCs[3] + + dist_1a_1b = distance_sc(sc_1a, sc_1b) + dist_1b_2a = distance_sc(sc_1b, sc_2a) + dist_1a_2a = distance_sc(sc_1a, sc_2a) + dist_2a_2b = distance_sc(sc_2a, sc_2b) + + print("--------------------") + print(dist_1a_1b) + print(dist_1b_2a) + print(dist_1a_2a) + print(dist_2a_2b) + diff --git a/python/README.md b/python/README.md new file mode 100644 index 0000000..6e9fc2d --- /dev/null +++ b/python/README.md @@ -0,0 +1,6 @@ +# Practical Python classes of Scan Context for LiDAR SLAM +- In the [PyICP SLAM](https://github.com/kissb2/PyICP-SLAM) repository, Scan Context Manager class is implemented. + +## NEWS +- To support the fast python API, python bind of the C++ codes using pybind11 is supported. + - see https://github.com/gisbi-kim/scancontext-pybind diff --git a/python/data/000095.bin b/python/data/000095.bin new file mode 100755 index 0000000..75d4dc4 Binary files /dev/null and b/python/data/000095.bin differ diff --git a/python/make_sc_example.py b/python/make_sc_example.py new file mode 100644 index 0000000..3f7a0c4 --- /dev/null +++ b/python/make_sc_example.py @@ -0,0 +1,172 @@ +import os +import numpy as np +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +from open3d import * + + +class kitti_vlp_database: + def __init__(self, bin_dir): + self.bin_dir = bin_dir + self.bin_files = os.listdir(bin_dir); self.bin_files.sort() + + self.num_bins = len(self.bin_files) + + +class ScanContext: + + # static variables + viz = 0 + + downcell_size = 0.5 + + kitti_lidar_height = 2.0; + + # sector_res = np.array([45, 90, 180, 360, 720]) + # ring_res = np.array([10, 20, 40, 80, 160]) + sector_res = np.array([60]) + ring_res = np.array([20]) + max_length = 80 + + + def __init__(self, bin_dir, bin_file_name): + + self.bin_dir = bin_dir + self.bin_file_name = bin_file_name + self.bin_path = bin_dir + bin_file_name + + self.scancontexts = self.genSCs() + + + def load_velo_scan(self): + scan = np.fromfile(self.bin_path, dtype=np.float32) + scan = scan.reshape((-1, 4)) + ptcloud_xyz = scan[:, :-1] + + return ptcloud_xyz + + + def xy2theta(self, x, y): + if (x >= 0 and y >= 0): + theta = 180/np.pi * np.arctan(y/x); + if (x < 0 and y >= 0): + theta = 180 - ((180/np.pi) * np.arctan(y/(-x))); + if (x < 0 and y < 0): + theta = 180 + ((180/np.pi) * np.arctan(y/x)); + if ( x >= 0 and y < 0): + theta = 360 - ((180/np.pi) * np.arctan((-y)/x)); + + return theta + + + def pt2rs(self, point, gap_ring, gap_sector, num_ring, num_sector): + x = point[0] + y = point[1] + z = point[2] + + if(x == 0.0): + x = 0.001 + if(y == 0.0): + y = 0.001 + + theta = self.xy2theta(x, y) + faraway = np.sqrt(x*x + y*y) + + idx_ring = np.divmod(faraway, gap_ring)[0] + idx_sector = np.divmod(theta, gap_sector)[0] + + if(idx_ring >= num_ring): + idx_ring = num_ring-1 # python starts with 0 and ends with N-1 + + return int(idx_ring), int(idx_sector) + + + def ptcloud2sc(self, ptcloud, num_sector, num_ring, max_length): + + num_points = ptcloud.shape[0] + + gap_ring = max_length/num_ring + gap_sector = 360/num_sector + + enough_large = 1000 + sc_storage = np.zeros([enough_large, num_ring, num_sector]) + sc_counter = np.zeros([num_ring, num_sector]) + + for pt_idx in range(num_points): + + point = ptcloud[pt_idx, :] + point_height = point[2] + ScanContext.kitti_lidar_height + + idx_ring, idx_sector = self.pt2rs(point, gap_ring, gap_sector, num_ring, num_sector) + + if sc_counter[idx_ring, idx_sector] >= enough_large: + continue + sc_storage[int(sc_counter[idx_ring, idx_sector]), idx_ring, idx_sector] = point_height + sc_counter[idx_ring, idx_sector] = sc_counter[idx_ring, idx_sector] + 1 + + sc = np.amax(sc_storage, axis=0) + + return sc + + + def genSCs(self): + ptcloud_xyz = self.load_velo_scan() + print("The number of original points: " + str(ptcloud_xyz.shape) ) + + pcd = PointCloud() + pcd.points = Vector3dVector(ptcloud_xyz) + downpcd = voxel_down_sample(pcd, voxel_size = ScanContext.downcell_size) + ptcloud_xyz_downed = np.asarray(downpcd.points) + print("The number of downsampled points: " + str(ptcloud_xyz_downed.shape) ) + # draw_geometries([downpcd]) + + if(ScanContext.viz): + draw_geometries([downpcd]) + + self.SCs = [] + for res in range(len(ScanContext.sector_res)): + num_sector = ScanContext.sector_res[res] + num_ring = ScanContext.ring_res[res] + + sc = self.ptcloud2sc(ptcloud_xyz_downed, num_sector, num_ring, ScanContext.max_length) + self.SCs.append(sc) + + + def plot_multiple_sc(self, fig_idx=1): + + num_res = len(ScanContext.sector_res) + + fig, axes = plt.subplots(nrows=num_res) + + axes[0].set_title('Scan Contexts with multiple resolutions', fontsize=14) + for ax, res in zip(axes, range(num_res)): + ax.imshow(self.SCs[res]) + + plt.show() + + +if __name__ == "__main__": + + bin_dir = './data/' + bin_db = kitti_vlp_database(bin_dir) + + for bin_idx in range(bin_db.num_bins): + + bin_file_name = bin_db.bin_files[bin_idx] + bin_path = bin_db.bin_dir + bin_file_name + + sc = ScanContext(bin_dir, bin_file_name) + + fig_idx = 1 + # sc.plot_multiple_sc(fig_idx) + + print(len(sc.SCs)) + print(sc.SCs[0].shape) + + + + + + + + diff --git a/python/result/ptcloud_filtered.png b/python/result/ptcloud_filtered.png new file mode 100644 index 0000000..8a751e5 Binary files /dev/null and b/python/result/ptcloud_filtered.png differ diff --git a/python/result/sc_multires.png b/python/result/sc_multires.png new file mode 100644 index 0000000..c13aa90 Binary files /dev/null and b/python/result/sc_multires.png differ diff --git a/sample_data/KITTI/00/velodyne/000094.bin b/sample_data/KITTI/00/velodyne/000094.bin new file mode 100755 index 0000000..afe106c Binary files /dev/null and b/sample_data/KITTI/00/velodyne/000094.bin differ diff --git a/sample_data/KITTI/00/velodyne/000095.bin b/sample_data/KITTI/00/velodyne/000095.bin new file mode 100755 index 0000000..75d4dc4 Binary files /dev/null and b/sample_data/KITTI/00/velodyne/000095.bin differ diff --git a/sample_data/KITTI/00/velodyne/000198.bin b/sample_data/KITTI/00/velodyne/000198.bin new file mode 100755 index 0000000..63ef1e2 Binary files /dev/null and b/sample_data/KITTI/00/velodyne/000198.bin differ diff --git a/sample_data/KITTI/00/velodyne/000199.bin b/sample_data/KITTI/00/velodyne/000199.bin new file mode 100755 index 0000000..aebeff8 Binary files /dev/null and b/sample_data/KITTI/00/velodyne/000199.bin differ diff --git a/sample_data/NCLT/2012-01-15/velodyne_sync/1326652795280148.bin b/sample_data/NCLT/2012-01-15/velodyne_sync/1326652795280148.bin new file mode 100755 index 0000000..816c356 Binary files /dev/null and b/sample_data/NCLT/2012-01-15/velodyne_sync/1326652795280148.bin differ diff --git a/tro2022/batch_processor/SingleTestInfo.m b/tro2022/batch_processor/SingleTestInfo.m new file mode 100644 index 0000000..04788e4 --- /dev/null +++ b/tro2022/batch_processor/SingleTestInfo.m @@ -0,0 +1,238 @@ +classdef SingleTestInfo + %SINGLETESTINFO Summary of this class goes here + % Detailed explanation goes here + + properties + % fixed + MulRanBaseDir_ + KITTIBaseDir_ + OxfordBaseDir_ + NaverLabsBaseDir_ + + % user input + dataset_ + sequence_ + dataset_dir_ + method_ + res_ + roi_ + num_candidates_ + + % dataset-dependant + scan_dir_ + scan_names_ + scan2_dir_ + scan2_names_ + scan2_times_ + + gtpose_path_ + gtpose_ + gtpose_time_ + gtpose_xyz_ + gtpose_xy_ + gtpose_rpy_ + gtpose_6d_ + gtpose_se3_ + gtlidarpose_xyz_ + gtlidarpose_se3_ + + % save + SaveBaseDir_ + save_path_ + save_path_core_ + + end + + methods + function obj = SingleTestInfo(dataset, seq, method, res, roi, num_candidates) + %% default + obj.MulRanBaseDir_ = '/media/user/My Passport/data/MulRan_release/'; + obj.KITTIBaseDir_ = '/media/user/GS1TB/KITTI/dataset/sequences/'; + obj.OxfordBaseDir_ = '/media/user/GS1TB/OxfordRadar/sequences/'; + obj.NaverLabsBaseDir_ = '/media/user/NAVER_LABS_DATAS/outdoor_dataset/ForTRO/'; + + obj.SaveBaseDir_ = 'data'; + + %% basic + obj.dataset_ = dataset; + obj.sequence_ = seq; + obj.method_ = method; + obj.res_ = res; + obj.roi_ = roi; + obj.num_candidates_ = num_candidates; + + %% dataset-dependant + % MulRan + if( strcmp( obj.dataset_, 'MulRan') ) + obj.dataset_dir_ = fullfile(obj.MulRanBaseDir_, obj.sequence_); + obj.scan_dir_ = fullfile( obj.dataset_dir_, 'sensor_data/Ouster/' ); + obj.scan_names_ = osdir(obj.scan_dir_); + + obj.gtpose_path_ = fullfile( obj.dataset_dir_, 'global_pose.csv' ); + obj.gtpose_ = csvread( obj.gtpose_path_ ); + obj.gtpose_time_ = obj.gtpose_(:, 1); + obj.gtpose_xyz_ = obj.gtpose_(:, [5, 9, 13]); + obj.gtpose_xy_ = obj.gtpose_xyz_(:, 1:2); + + % KITTI + elseif( strcmp( obj.dataset_, 'KITTI') ) + obj.dataset_dir_ = fullfile(obj.KITTIBaseDir_, obj.sequence_); + obj.scan_dir_ = fullfile( obj.dataset_dir_, 'velodyne/' ); + obj.scan_names_ = osdir(obj.scan_dir_); + + obj.gtpose_path_ = fullfile( obj.dataset_dir_, 'poses.txt' ); + obj.gtpose_ = dlmread( obj.gtpose_path_ ); + obj.gtpose_xyz_ = obj.gtpose_(:, [4, 8, 12]); % WARNING: in camera coord + obj.gtpose_xy_ = obj.gtpose_xyz_(:, [1, 3]); % WARNING: in camera coord so use x,z in cam coord as x,y in lidar coord. + + obj.gtpose_time_ = dlmread( fullfile( obj.dataset_dir_, 'times.txt' ) ); +% obj.gtpose_time_ = linspace(1, length(obj.gtpose_), length(obj.gtpose_)); % KITTI has no time + + % Oxford + elseif( strcmp( obj.dataset_, 'Oxford') ) + obj.dataset_dir_ = fullfile(obj.OxfordBaseDir_, obj.sequence_); + obj.scan_dir_ = fullfile( obj.dataset_dir_, 'velodyne_left/' ); + obj.scan_names_ = osdir(obj.scan_dir_); + obj.scan2_dir_ = fullfile( obj.dataset_dir_, 'velodyne_right/' ); + obj.scan2_names_ = osdir(obj.scan2_dir_); + + for ii_scan2_bin_name = obj.scan2_names_ + ii_scan2_bin_time = str2double(ii_scan2_bin_name{1}(1:end-4)); + obj.scan2_times_ = [obj.scan2_times_; ii_scan2_bin_time]; + end + + % parse pose + % oxford pose reader: ref https://github.com/ori-mrg/robotcar-dataset-sdk/blob/master/matlab/InterpolatePoses.m + obj.gtpose_path_ = fullfile( obj.dataset_dir_, 'gps/ins.csv' ); + + ins_file_id = fopen(obj.gtpose_path_); + headers = textscan(ins_file_id, '%s', 15, 'Delimiter',','); + format_str = '%u64 %s %f %f %f %f %f %f %s %f %f %f %f %f %f'; + ins_data = textscan(ins_file_id, format_str,'Delimiter',','); + fclose(ins_file_id); + + obj.gtpose_ = ins_data; + + obj.gtpose_time_ = double(obj.gtpose_{1}); + northings = obj.gtpose_{6}; % y + eastings = obj.gtpose_{7}; % x + downs = obj.gtpose_{8}; % z + rolls = obj.gtpose_{13}; + pitches = obj.gtpose_{14}; + yaws = obj.gtpose_{15}; + obj.gtpose_xyz_ = [eastings, northings, downs]; + obj.gtpose_xy_ = obj.gtpose_xyz_(:, [1, 2]); + obj.gtpose_rpy_ = [rolls, pitches, yaws]; + + obj.gtpose_6d_ = [obj.gtpose_xyz_, obj.gtpose_rpy_]; + obj.gtpose_se3_ = {}; + for ii = 1:length(obj.gtpose_6d_) + pose_6d = obj.gtpose_6d_(ii, :); + pose_xyz = pose_6d(1:3); + pose_euler = pose_6d(4:6); + pose_so3 = RotmatFromEuler(flip(pose_euler)); + pose_se3 = [pose_so3, pose_xyz'; 0,0,0, 1]; + obj.gtpose_se3_{end+1} = pose_se3; + end + + base2lidar_se3 = [RotmatFromEuler(flip([pi, pi, 0])), [1.13, 0, -1.24]'; 0,0,0,1]; + lidar2base_se3 = inv(base2lidar_se3); + + % ref: makeLiDARGTposes.m (head2tail is required) + obj.gtlidarpose_se3_ = {}; + + obj.gtlidarpose_se3_{end+1} = eye(4); + obj.gtlidarpose_xyz_(1, :) = [0,0,0]; + + lidarpose_prev = eye(4); + obj.gtlidarpose_xyz_ = zeros(length(obj.gtpose_6d_), 3); + for ii = 2:length(obj.gtpose_6d_) + basepose_prev = obj.gtpose_se3_{ii-1}; + basepose_curr = obj.gtpose_se3_{ii}; + + rel_base2base = inv(basepose_prev) * basepose_curr; + + rel_lidar2lidar = lidar2base_se3 * rel_base2base * base2lidar_se3; + lidarpose_curr = lidarpose_prev * rel_lidar2lidar; + + obj.gtlidarpose_se3_{end+1} = lidarpose_curr; + obj.gtlidarpose_xyz_(ii, :) = lidarpose_curr(1:3, 4); + + lidarpose_prev = lidarpose_curr; + end + + % NaverLabs + elseif( strcmp( obj.dataset_, 'NaverLabs') ) + obj.dataset_dir_ = fullfile(obj.NaverLabsBaseDir_, obj.sequence_); + obj.scan_dir_ = fullfile( obj.dataset_dir_, 'lidars/' ); + obj.scan_names_ = osdir(obj.scan_dir_); + + obj.gtpose_path_ = fullfile( obj.dataset_dir_, 'poses.txt' ); + obj.gtpose_ = dlmread( obj.gtpose_path_ ); +% obj.gtpose_time_ = linspace(1, length(obj.gtpose_), length(obj.gtpose_)); % not consider time in this experiment + obj.gtpose_time_ = dlmread( fullfile(obj.dataset_dir_, 'timestamps.txt') ); + obj.gtpose_xyz_ = obj.gtpose_(:, [4, 8, 12]); + obj.gtpose_xy_ = obj.gtpose_xyz_(:, 1:2); + + obj.gtpose_se3_ = {}; + for ii = 1:length(obj.gtpose_) + pose_se3line = obj.gtpose_(ii, :); + pose_se3 = reshape(pose_se3line, 4, 4)'; + obj.gtpose_se3_{end+1} = pose_se3; + end + + base2lidar_se3 = [0.999853669, 0.006310318, 0.0159003363, 1.09403653; ... + -0.006315622,0.999980016,0.0002834088,-0.01908349; ... + -0.01589823,-0.000383787,0.999873541,0.354; ... + 0,0,0,1]; + lidar2base_se3 = inv(base2lidar_se3); + + % ref: makeLiDARGTposes.m (head2tail is required) + obj.gtlidarpose_se3_ = {}; + + obj.gtlidarpose_se3_{end+1} = eye(4); + obj.gtlidarpose_xyz_(1, :) = [0,0,0]; + + lidarpose_prev = eye(4); + obj.gtlidarpose_xyz_ = zeros(length(obj.gtpose_6d_), 3); + for ii = 2:length(obj.gtpose_time_) + basepose_prev = obj.gtpose_se3_{ii-1}; + basepose_curr = obj.gtpose_se3_{ii}; + + rel_base2base = inv(basepose_prev) * basepose_curr; + + rel_lidar2lidar = lidar2base_se3 * rel_base2base * base2lidar_se3; + lidarpose_curr = lidarpose_prev * rel_lidar2lidar; + + obj.gtlidarpose_se3_{end+1} = lidarpose_curr; + obj.gtlidarpose_xyz_(ii, :) = lidarpose_curr(1:3, 4); + + lidarpose_prev = lidarpose_curr; + end + + end + + %% save + obj.save_path_core_ = fullfile( obj.SaveBaseDir_, ... + strcat( ... + obj.dataset_, '-', obj.sequence_, '-', obj.method_, '-', ... + num2str(obj.res_(1)), 'x', num2str(obj.res_(2)), '-', num2str(obj.num_candidates_) ... + ) ... + ); + + [curr_time, ~] = clock; + curr_time_int = round(curr_time); + curr_time_str = regexprep(num2str(curr_time_int), ' +', ''); + obj.save_path_ = fullfile( obj.save_path_core_, curr_time_str ); + + end + + end +end + + +%% helper funcs +function [files] = osdir(path) + files = dir(path); files(1:2) = []; files = {files(:).name}; +end + diff --git a/tro2022/batch_processor/loadData.m b/tro2022/batch_processor/loadData.m new file mode 100644 index 0000000..91abbff --- /dev/null +++ b/tro2022/batch_processor/loadData.m @@ -0,0 +1,45 @@ +function [scancontexts, ringkeys, poses] = loadData(down_shape, skip_data_frame, data_dir) + +%% +global data_path; +data_save_path = fullfile(data_dir); + +%% +% newly make +if exist(data_save_path) == 0 + % make + [scancontexts, ringkeys, poses] = makeExperience(data_path, down_shape, skip_data_frame); + + % save + mkdir(data_save_path); + + filename = strcat(data_save_path, 'scancontexts', num2str(down_shape(1)), 'x', num2str(down_shape(2)), '.mat'); + save(filename, 'scancontexts'); + filename = strcat(data_save_path, 'ringkeys', num2str(down_shape(1)), 'x', num2str(down_shape(2)), '.mat'); + save(filename, 'ringkeys'); + filename = strcat(data_save_path, 'poses', num2str(down_shape(1)), 'x', num2str(down_shape(2)), '.mat'); + save(filename, 'poses'); + +% or load +else + filename = strcat(data_save_path, 'scancontexts', num2str(down_shape(1)), 'x', num2str(down_shape(2)), '.mat'); + load(filename); + % fix + for iii = 1:length(scancontexts) + sc = double(scancontexts{iii}); + scancontexts{iii} = sc; + end + + filename = strcat(data_save_path, 'ringkeys', num2str(down_shape(1)), 'x', num2str(down_shape(2)), '.mat'); + load(filename); + filename = strcat(data_save_path, 'poses', num2str(down_shape(1)), 'x', num2str(down_shape(2)), '.mat'); + load(filename); + + disp('- successfully loaded.'); +end + +%% +disp(' '); + +end + diff --git a/tro2022/batch_processor/loadDataV2.m b/tro2022/batch_processor/loadDataV2.m new file mode 100644 index 0000000..61fb16c --- /dev/null +++ b/tro2022/batch_processor/loadDataV2.m @@ -0,0 +1,43 @@ +function [descs, invkeys, poses] = loadDataV2(testinfo, SKIP_FRAMES) + +%% Newely make + if ~ exist(testinfo.save_path_core_, 'dir') + + % make + [descs, invkeys, poses] = makeExperience(testinfo, SKIP_FRAMES); + + % save + mkdir(testinfo.save_path_); + + descs_filename = fullfile(testinfo.save_path_, 'descs.mat'); + invkeys_filename = fullfile(testinfo.save_path_, 'invkeys.mat'); + poses_filename = fullfile(testinfo.save_path_, 'poses.mat'); + + save(descs_filename, 'descs'); + save(invkeys_filename, 'invkeys'); + save(poses_filename, 'poses'); + + disp('- successfully made.'); + + %% Or load existing ones + else + saved_time = osdir(testinfo.save_path_core_); saved_time = saved_time{1}; + descs_filename = fullfile(testinfo.save_path_core_, saved_time, 'descs.mat'); + invkeys_filename = fullfile(testinfo.save_path_core_, saved_time, 'invkeys.mat'); + poses_filename = fullfile(testinfo.save_path_core_, saved_time, 'poses.mat'); + + load(descs_filename); + % fix + for desc_idx = 1:length(descs) + desc = double(descs{desc_idx}); + descs{desc_idx} = desc; + end + + load(invkeys_filename); + load(poses_filename); + + disp('- successfully loaded.'); + end + +end + diff --git a/tro2022/batch_processor/loadDataV22_aug.m b/tro2022/batch_processor/loadDataV22_aug.m new file mode 100644 index 0000000..24f1061 --- /dev/null +++ b/tro2022/batch_processor/loadDataV22_aug.m @@ -0,0 +1,43 @@ +function [descs_list, invkeys_list, poses] = loadDataV22_aug(testinfo, SKIP_FRAMES) + +%% Newely make + if ~ exist(testinfo.save_path_core_, 'dir') + + % make + [descs_list, invkeys_list, poses] = makeAugmentedExperience(testinfo, SKIP_FRAMES); + + % save + mkdir(testinfo.save_path_); + + descs_filename = fullfile(testinfo.save_path_, 'descs_list.mat'); + invkeys_filename = fullfile(testinfo.save_path_, 'invkeys_list.mat'); + poses_filename = fullfile(testinfo.save_path_, 'poses.mat'); + + save(descs_filename, 'descs_list'); + save(invkeys_filename, 'invkeys_list'); + save(poses_filename, 'poses'); + + disp('- successfully made.'); + + %% Or load existing ones + else + saved_time = osdir(testinfo.save_path_core_); saved_time = saved_time{1}; + descs_filename = fullfile(testinfo.save_path_core_, saved_time, 'descs_list.mat'); + invkeys_filename = fullfile(testinfo.save_path_core_, saved_time, 'invkeys_list.mat'); + poses_filename = fullfile(testinfo.save_path_core_, saved_time, 'poses.mat'); + + load(descs_filename); +% % fix +% for desc_idx = 1:length(descs_list) +% desc = double(descs_list{desc_idx}); +% descs_list{desc_idx} = desc; +% end + + load(invkeys_filename); + load(poses_filename); + + disp('- successfully loaded.'); + end + +end + diff --git a/tro2022/batch_processor/loadDataV2EquiDist.m b/tro2022/batch_processor/loadDataV2EquiDist.m new file mode 100644 index 0000000..75abec9 --- /dev/null +++ b/tro2022/batch_processor/loadDataV2EquiDist.m @@ -0,0 +1,43 @@ +function [descs, invkeys, poses] = loadDataV2EquiDist(testinfo, SKIP_FRAMES) + +%% Newely make + if ~ exist(testinfo.save_path_core_, 'dir') + + % make + [descs, invkeys, poses] = makeExperienceEquiDist(testinfo, SKIP_FRAMES); + + % save + mkdir(testinfo.save_path_); + + descs_filename = fullfile(testinfo.save_path_, 'descs.mat'); + invkeys_filename = fullfile(testinfo.save_path_, 'invkeys.mat'); + poses_filename = fullfile(testinfo.save_path_, 'poses.mat'); + + save(descs_filename, 'descs'); + save(invkeys_filename, 'invkeys'); + save(poses_filename, 'poses'); + + disp('- successfully made.'); + + %% Or load existing ones + else + saved_time = osdir(testinfo.save_path_core_); saved_time = saved_time{1}; + descs_filename = fullfile(testinfo.save_path_core_, saved_time, 'descs.mat'); + invkeys_filename = fullfile(testinfo.save_path_core_, saved_time, 'invkeys.mat'); + poses_filename = fullfile(testinfo.save_path_core_, saved_time, 'poses.mat'); + + load(descs_filename); + % fix + for desc_idx = 1:length(descs) + desc = double(descs{desc_idx}); + descs{desc_idx} = desc; + end + + load(invkeys_filename); + load(poses_filename); + + disp('- successfully loaded.'); + end + +end + diff --git a/tro2022/batch_processor/loadDataV2EquiDistXYZT.m b/tro2022/batch_processor/loadDataV2EquiDistXYZT.m new file mode 100644 index 0000000..9c7a91b --- /dev/null +++ b/tro2022/batch_processor/loadDataV2EquiDistXYZT.m @@ -0,0 +1,43 @@ +function [descs, invkeys, poses] = loadDataV2EquiDistXYZT(testinfo, SKIP_FRAMES) + +%% Newely make + if ~ exist(testinfo.save_path_core_, 'dir') + + % make + [descs, invkeys, poses] = makeExperienceEquiDistXYZT(testinfo, SKIP_FRAMES); + + % save + mkdir(testinfo.save_path_); + + descs_filename = fullfile(testinfo.save_path_, 'descs.mat'); + invkeys_filename = fullfile(testinfo.save_path_, 'invkeys.mat'); + poses_filename = fullfile(testinfo.save_path_, 'poses.mat'); + + save(descs_filename, 'descs'); + save(invkeys_filename, 'invkeys'); + save(poses_filename, 'poses'); + + disp('- successfully made.'); + + %% Or load existing ones + else + saved_time = osdir(testinfo.save_path_core_); saved_time = saved_time{1}; + descs_filename = fullfile(testinfo.save_path_core_, saved_time, 'descs.mat'); + invkeys_filename = fullfile(testinfo.save_path_core_, saved_time, 'invkeys.mat'); + poses_filename = fullfile(testinfo.save_path_core_, saved_time, 'poses.mat'); + + load(descs_filename); + % fix + for desc_idx = 1:length(descs) + desc = double(descs{desc_idx}); + descs{desc_idx} = desc; + end + + load(invkeys_filename); + load(poses_filename); + + disp('- successfully loaded.'); + end + +end + diff --git a/tro2022/batch_processor/loadData_augIros18ver.m b/tro2022/batch_processor/loadData_augIros18ver.m new file mode 100644 index 0000000..3097ad1 --- /dev/null +++ b/tro2022/batch_processor/loadData_augIros18ver.m @@ -0,0 +1,49 @@ +function [descs_list, invkeys_list, poses] = loadData_augIros18ver(testinfo, SKIP_FRAMES) + +%% Newely make + if ~ exist(testinfo.save_path_core_, 'dir') + + % make +% [descs_list, invkeys_list, poses] = makeAugmentedExperienceTreeMerge(testinfo, SKIP_FRAMES); + + % for Oxford etc (equi dist samplling) + [descs_list, invkeys_list, poses] = makeAugmentedExperienceEquiDistTreeMerge(testinfo, SKIP_FRAMES); + + % for KITTI (using all frames) + % [descs_list, invkeys_list, poses] = makeAugmentedExperienceAllFramesTreeMerge(testinfo, SKIP_FRAMES); + + % save + mkdir(testinfo.save_path_); + + descs_filename = fullfile(testinfo.save_path_, 'descs_list.mat'); + invkeys_filename = fullfile(testinfo.save_path_, 'invkeys_list.mat'); + poses_filename = fullfile(testinfo.save_path_, 'poses.mat'); + + save(descs_filename, 'descs_list'); + save(invkeys_filename, 'invkeys_list'); + save(poses_filename, 'poses'); + + disp('- successfully made.'); + + %% Or load existing ones + else + saved_time = osdir(testinfo.save_path_core_); saved_time = saved_time{1}; + descs_filename = fullfile(testinfo.save_path_core_, saved_time, 'descs_list.mat'); + invkeys_filename = fullfile(testinfo.save_path_core_, saved_time, 'invkeys_list.mat'); + poses_filename = fullfile(testinfo.save_path_core_, saved_time, 'poses.mat'); + + load(descs_filename); +% % fix +% for desc_idx = 1:length(descs_list) +% desc = double(descs_list{desc_idx}); +% descs_list{desc_idx} = desc; +% end + + load(invkeys_filename); + load(poses_filename); + + disp('- successfully loaded.'); + end + +end + diff --git a/tro2022/batch_processor/loadData_augIros18verForKITTI.m b/tro2022/batch_processor/loadData_augIros18verForKITTI.m new file mode 100644 index 0000000..18633fd --- /dev/null +++ b/tro2022/batch_processor/loadData_augIros18verForKITTI.m @@ -0,0 +1,47 @@ +function [descs_list, invkeys_list, poses] = loadData_augIros18verForKITTI(testinfo, SKIP_FRAMES) + +%% Newely make + if ~ exist(testinfo.save_path_core_, 'dir') + + % make +% [descs_list, invkeys_list, poses] = makeAugmentedExperienceTreeMerge(testinfo, SKIP_FRAMES); + + % +% [descs_list, invkeys_list, poses] = makeAugmentedExperienceEquiDistTreeMerge(testinfo, SKIP_FRAMES); + [descs_list, invkeys_list, poses] = makeAugmentedExperienceAllFramesTreeMerge(testinfo, SKIP_FRAMES); + + % save + mkdir(testinfo.save_path_); + + descs_filename = fullfile(testinfo.save_path_, 'descs_list.mat'); + invkeys_filename = fullfile(testinfo.save_path_, 'invkeys_list.mat'); + poses_filename = fullfile(testinfo.save_path_, 'poses.mat'); + + save(descs_filename, 'descs_list'); + save(invkeys_filename, 'invkeys_list'); + save(poses_filename, 'poses'); + + disp('- successfully made.'); + + %% Or load existing ones + else + saved_time = osdir(testinfo.save_path_core_); saved_time = saved_time{1}; + descs_filename = fullfile(testinfo.save_path_core_, saved_time, 'descs_list.mat'); + invkeys_filename = fullfile(testinfo.save_path_core_, saved_time, 'invkeys_list.mat'); + poses_filename = fullfile(testinfo.save_path_core_, saved_time, 'poses.mat'); + + load(descs_filename); +% % fix +% for desc_idx = 1:length(descs_list) +% desc = double(descs_list{desc_idx}); +% descs_list{desc_idx} = desc; +% end + + load(invkeys_filename); + load(poses_filename); + + disp('- successfully loaded.'); + end + +end + diff --git a/tro2022/batch_processor/makeAugmentedExperience.m b/tro2022/batch_processor/makeAugmentedExperience.m new file mode 100644 index 0000000..f2b01f5 --- /dev/null +++ b/tro2022/batch_processor/makeAugmentedExperience.m @@ -0,0 +1,149 @@ +function [descs_list, invkeys_list, xy_poses] = makeAugmentedExperience(testinfo, SKIP_FRAMES) + +%% +LIDAR_HEIGHT = 1.9; + +%% +num_data = length(testinfo.scan_names_); +num_data_save = floor(num_data/SKIP_FRAMES) + 1; + +save_counter = 1; + +num_invaxis = testinfo.res_(1); +% num_vaxis = testinfo.res_(2); + +%% aug save vars (currently supports only pc, 20200513) +NUM_AUG_SCs = 5; + +descs_list = {}; +invkeys_list = {}; + +for idx_copies = 1:NUM_AUG_SCs + descs_list{end+1} = cell(1, num_data_save); + invkeys_list{end+1} = zeros(num_data_save, num_invaxis); +end + +xy_poses = zeros(num_data_save, 2); + + +%% main parse +for scan_idx = 1:num_data +% for scan_idx = 1:10000 + + %% curr info + if(rem(scan_idx, SKIP_FRAMES) ~=0) + continue; + end + + scan_name = testinfo.scan_names_{scan_idx}; + scan_time = str2double(scan_name(1:end-4)); + scan_path = strcat(testinfo.scan_dir_, scan_name); + + + %% load datat + if( strcmp(testinfo.dataset_, 'MulRan') ) + try + points = readBin(scan_path); + catch + continue; + end + [~, nearest_idx] = min(abs(repmat(scan_time, length(testinfo.gtpose_time_), 1) - testinfo.gtpose_time_)); + xy_pose = testinfo.gtpose_xy_(nearest_idx, :); + + elseif( strcmp(testinfo.dataset_, 'KITTI') ) + points = readBin(scan_path); + xy_pose = testinfo.gtpose_xy_(scan_idx, :); + + elseif( strcmp(testinfo.dataset_, 'Oxford') ) + points = readBinOxford(scan_path); + [~, nearest_idx] = min(abs(repmat(scan_time, length(testinfo.gtpose_time_), 1) - testinfo.gtpose_time_)); + xy_pose = testinfo.gtpose_xy_(nearest_idx, :); + +% figure(1); clf; +% pcshow(points); +% xlim([-80, 80]); ylim([-80, 80]); zlim([-5, 20]); + + elseif( strcmp(testinfo.dataset_, 'NaverLabs') ) + points = readScanNaverlabs(scan_path); + xy_pose = testinfo.gtpose_xy_(scan_idx, :); + + end + + + %% description: ptcloud to descriptor + % different for the method + + % ccpoints + if( strcmp(testinfo.method_, 'pc') ) + points(:, 3) = points(:, 3) + LIDAR_HEIGHT; + ptcloud = pointCloud(points); + + num_sector = testinfo.res_(2); + num_ring = testinfo.res_(1); + max_range = testinfo.roi_(1); + + desc_list = ptcloud2polarcontextAug(ptcloud, num_sector, num_ring, max_range); % up to 80 meter + + % pc + elseif( strcmp(testinfo.method_, 'cc') ) + points(:, 3) = points(:, 3) + LIDAR_HEIGHT; + ptcloud = pointCloud(points); + +% unit_move = round( testinfo.roi_(1)*2 / testinfo.res_(1) ); +% unit_lateral = round( testinfo.roi_(2)*2 / testinfo.res_(2) ); + unit_move = ( testinfo.roi_(1)*2 / testinfo.res_(1) ); + unit_lateral = ( testinfo.roi_(2)*2 / testinfo.res_(2) ); + axis_unit = [unit_move, unit_lateral]; + axis_range = testinfo.roi_; + + desc = ptcloud2cartcontext(ptcloud, axis_unit, axis_range); % up to 80 meter + ikey = sc2invkey(desc); + + % m2dp + elseif( strcmp(testinfo.method_, 'm2dp') ) + ptcloud_down = pcdownsample(pointCloud(points), 'gridAverage', 0.1); + [desc, ~] = M2DP(ptcloud_down.Location); + ikey = 0; % dummy + end + +% figure(1); clf; +% imagesc(desc); +% colormap jet; caxis([0, 4]); axis equal; + + + %% save + for idx_aug = 1:NUM_AUG_SCs + desc = desc_list{idx_aug}; + ikey = sc2invkey(desc); + + descs_list{idx_aug}{save_counter} = desc; + invkeys_list{idx_aug}(save_counter, :) = ikey; % for only cc and pc (not for M2DP and pnvlad) + end + + xy_poses(save_counter, :) = xy_pose; + + save_counter = save_counter + 1; + + % log + if(rem(scan_idx, 100) == 0) + message = strcat(num2str(scan_idx), " / ", num2str(num_data), " processed (skip: ", num2str(SKIP_FRAMES), ")"); + disp(message); + end + +% figure(13); clf; +% xy_poses_show = xy_poses; +% xy_poses_show = xy_poses_show(1:save_counter-1, :); +% scatter(xy_poses_show(:, 1), xy_poses_show(:, 2)); axis equal; + +end + +% take valids +for idx_aug = 1:NUM_AUG_SCs + descs_list{idx_aug} = descs_list{idx_aug}(1:save_counter-1); + invkeys_list{idx_aug} = invkeys_list{idx_aug}(1:save_counter-1, :); +end + +xy_poses = xy_poses(1:save_counter-1, :); + +end + diff --git a/tro2022/batch_processor/makeAugmentedExperienceAllFramesTreeMerge.m b/tro2022/batch_processor/makeAugmentedExperienceAllFramesTreeMerge.m new file mode 100644 index 0000000..5be2fa9 --- /dev/null +++ b/tro2022/batch_processor/makeAugmentedExperienceAllFramesTreeMerge.m @@ -0,0 +1,191 @@ +function [descs_list, invkeys_list, xy_poses] = makeAugmentedExperienceAllFramesTreeMerge(testinfo, SKIP_FRAMES) + +% KITTI used all frames + +%% +LIDAR_HEIGHT = 1.9; + +%% +num_data = length(testinfo.scan_names_); +num_data_save = floor(num_data/SKIP_FRAMES) + 1; + +save_counter = 1; + +num_invaxis = testinfo.res_(1); +% num_vaxis = testinfo.res_(2); + +%% aug save vars (currently supports only pc, 20200513) +NUM_AUG_SCs = 3; % including the original (no-root-shifted) + +descs_list = {}; +invkeys_list = {}; + +for idx_copies = 1:NUM_AUG_SCs + descs_list{end+1} = cell(1, num_data_save); + invkeys_list{end+1} = zeros(num_data_save, num_invaxis); +end + +xy_poses = zeros(num_data_save, 2); + + +ENOUGH_MOVEMENT_GAP = 1; % meter +bfr_xy = [0, 0]; % init +cur_xy = bfr_xy; +accum_movement = 0; + +%% main parse +for scan_idx = 1:num_data +% for scan_idx = 1:10000 + + %% curr info + if(rem(scan_idx, SKIP_FRAMES) ~=0) + continue; + end + + scan_name = testinfo.scan_names_{scan_idx}; + scan_time = str2double(scan_name(1:end-4)); + scan_path = strcat(testinfo.scan_dir_, scan_name); + + + %% load datat + if( strcmp(testinfo.dataset_, 'MulRan') ) + try + points = readBin(scan_path); + catch + continue; + end + [~, nearest_idx] = min(abs(repmat(scan_time, length(testinfo.gtpose_time_), 1) - testinfo.gtpose_time_)); + xy_pose = testinfo.gtpose_xy_(nearest_idx, :); + + elseif( strcmp(testinfo.dataset_, 'KITTI') ) + points = readBin(scan_path); + xy_pose = testinfo.gtpose_xy_(scan_idx, :); + + elseif( strcmp(testinfo.dataset_, 'Oxford') ) +% points = readBinOxford(scan_path); +% [~, nearest_idx] = min(abs(repmat(scan_time, length(testinfo.gtpose_time_), 1) - testinfo.gtpose_time_)); +% xy_pose = testinfo.gtpose_xy_(nearest_idx, :); + + [~, near_scan2_idx] = min( abs(testinfo.scan2_times_ - scan_time) ); + scan2_name = testinfo.scan2_names_{near_scan2_idx}; + scan2_path = strcat(testinfo.scan2_dir_, scan2_name); + + points_left = readBinOxford(scan_path); + points_left(:,2) = points_left(:,2) + 0.47; + + points_right = readBinOxford(scan2_path); + points_right(:,2) = points_right(:,2) - 0.47; + + points = [points_left; points_right]; + + [~, nearest_idx] = min(abs(repmat(scan_time, length(testinfo.gtpose_time_), 1) - testinfo.gtpose_time_)); + xy_pose = testinfo.gtpose_xy_(nearest_idx, :); + + +% figure(1); clf; +% pcshow(points); +% xlim([-80, 80]); ylim([-80, 80]); zlim([-5, 20]); + + elseif( strcmp(testinfo.dataset_, 'NaverLabs') ) + points = readScanNaverlabs(scan_path); + xy_pose = testinfo.gtpose_xy_(scan_idx, :); + + end + + + %% check enough movement +% ENOUGH_MOVEMENT_GAP = 1; +% cur_xy = xy_pose; +% accum_movement = accum_movement + norm(cur_xy - bfr_xy); +% bfr_xy = cur_xy; % reset +% if( accum_movement < ENOUGH_MOVEMENT_GAP) +% continue; +% else +% accum_movement = 0; +% end + + %% description: ptcloud to descriptor + % different for the method + + % pc + if( strcmp(testinfo.method_, 'pc') ) + points(:, 3) = points(:, 3) + LIDAR_HEIGHT; + ptcloud = pointCloud(points); + + num_sector = testinfo.res_(2); + num_ring = testinfo.res_(1); + max_range = testinfo.roi_(1); + + desc_list = ptcloud2polarcontextAug(ptcloud, num_sector, num_ring, max_range, NUM_AUG_SCs); % up to 80 meter + + % cc + elseif( strcmp(testinfo.method_, 'cc') ) + points(:, 3) = points(:, 3) + LIDAR_HEIGHT; + ptcloud = pointCloud(points); + +% unit_move = round( testinfo.roi_(1)*2 / testinfo.res_(1) ); +% unit_lateral = round( testinfo.roi_(2)*2 / testinfo.res_(2) ); + unit_move = ( testinfo.roi_(1)*2 / testinfo.res_(1) ); + unit_lateral = ( testinfo.roi_(2)*2 / testinfo.res_(2) ); + axis_unit = [unit_move, unit_lateral]; + axis_range = testinfo.roi_; + + desc = ptcloud2cartcontext(ptcloud, axis_unit, axis_range); % up to 80 meter + ikey = sc2invkey(desc); + + % m2dp + elseif( strcmp(testinfo.method_, 'm2dp') ) + ptcloud_down = pcdownsample(pointCloud(points), 'gridAverage', 0.1); + [desc, ~] = M2DP(ptcloud_down.Location); + ikey = 0; % dummy + end + +% figure(1); clf; +% imagesc(desc); +% colormap jet; caxis([0, 4]); axis equal; + + + %% 20200810 debugger:to see the point cloud +% if(save_counter == 1056 || save_counter == 2714) +% disp(1); +% end + + + %% save + for idx_aug = 1:NUM_AUG_SCs + desc = desc_list{idx_aug}; + + ikey = sc2invkey(desc); +% ikey = sc2invkeyL0(desc); + + descs_list{idx_aug}{save_counter} = desc; + invkeys_list{idx_aug}(save_counter, :) = ikey; % for only cc and pc (not for M2DP and pnvlad) + end + + xy_poses(save_counter, :) = xy_pose; + + save_counter = save_counter + 1; + + % log + if(rem(scan_idx, 100) == 0) + message = strcat(num2str(scan_idx), " / ", num2str(num_data), " processed (skip: ", num2str(SKIP_FRAMES), ")"); + disp(message); + end + +% figure(13); clf; +% xy_poses_show = xy_poses; +% xy_poses_show = xy_poses_show(1:save_counter-1, :); +% scatter(xy_poses_show(:, 1), xy_poses_show(:, 2)); axis equal; + +end + +% take valids +for idx_aug = 1:NUM_AUG_SCs + descs_list{idx_aug} = descs_list{idx_aug}(1:save_counter-1); + invkeys_list{idx_aug} = invkeys_list{idx_aug}(1:save_counter-1, :); +end + +xy_poses = xy_poses(1:save_counter-1, :); + +end + diff --git a/tro2022/batch_processor/makeAugmentedExperienceEquiDistTreeMerge.m b/tro2022/batch_processor/makeAugmentedExperienceEquiDistTreeMerge.m new file mode 100644 index 0000000..e451dca --- /dev/null +++ b/tro2022/batch_processor/makeAugmentedExperienceEquiDistTreeMerge.m @@ -0,0 +1,189 @@ +function [descs_list, invkeys_list, xy_poses] = makeAugmentedExperienceEquiDistTreeMerge(testinfo, SKIP_FRAMES) + +%% +LIDAR_HEIGHT = 1.9; + +%% +num_data = length(testinfo.scan_names_); +num_data_save = floor(num_data/SKIP_FRAMES) + 1; + +save_counter = 1; + +num_invaxis = testinfo.res_(1); +% num_vaxis = testinfo.res_(2); + +%% aug save vars (currently supports only pc, 20200513) +NUM_AUG_SCs = 3; % including the original (no-root-shifted) + +descs_list = {}; +invkeys_list = {}; + +for idx_copies = 1:NUM_AUG_SCs + descs_list{end+1} = cell(1, num_data_save); + invkeys_list{end+1} = zeros(num_data_save, num_invaxis); +end + +xy_poses = zeros(num_data_save, 2); + + +ENOUGH_MOVEMENT_GAP = 1; % meter +bfr_xy = [0, 0]; % init +cur_xy = bfr_xy; +accum_movement = 0; + +%% main parse +for scan_idx = 1:num_data +% for scan_idx = 1:10000 + + %% curr info + if(rem(scan_idx, SKIP_FRAMES) ~=0) + continue; + end + + scan_name = testinfo.scan_names_{scan_idx}; + scan_time = str2double(scan_name(1:end-4)); + scan_path = strcat(testinfo.scan_dir_, scan_name); + + + %% load datat + if( strcmp(testinfo.dataset_, 'MulRan') ) + try + points = readBin(scan_path); + catch + continue; + end + [~, nearest_idx] = min(abs(repmat(scan_time, length(testinfo.gtpose_time_), 1) - testinfo.gtpose_time_)); + xy_pose = testinfo.gtpose_xy_(nearest_idx, :); + + elseif( strcmp(testinfo.dataset_, 'KITTI') ) + points = readBin(scan_path); + xy_pose = testinfo.gtpose_xy_(scan_idx, :); + + elseif( strcmp(testinfo.dataset_, 'Oxford') ) +% points = readBinOxford(scan_path); +% [~, nearest_idx] = min(abs(repmat(scan_time, length(testinfo.gtpose_time_), 1) - testinfo.gtpose_time_)); +% xy_pose = testinfo.gtpose_xy_(nearest_idx, :); + + [~, near_scan2_idx] = min( abs(testinfo.scan2_times_ - scan_time) ); + scan2_name = testinfo.scan2_names_{near_scan2_idx}; + scan2_path = strcat(testinfo.scan2_dir_, scan2_name); + + points_left = readBinOxford(scan_path); + points_left(:,2) = points_left(:,2) + 0.47; + + points_right = readBinOxford(scan2_path); + points_right(:,2) = points_right(:,2) - 0.47; + + points = [points_left; points_right]; + + [~, nearest_idx] = min(abs(repmat(scan_time, length(testinfo.gtpose_time_), 1) - testinfo.gtpose_time_)); + xy_pose = testinfo.gtpose_xy_(nearest_idx, :); + + +% figure(1); clf; +% pcshow(points); +% xlim([-80, 80]); ylim([-80, 80]); zlim([-5, 20]); + + elseif( strcmp(testinfo.dataset_, 'NaverLabs') ) + points = readScanNaverlabs(scan_path); + xy_pose = testinfo.gtpose_xy_(scan_idx, :); + + end + + + %% check enough movement +% ENOUGH_MOVEMENT_GAP = 1; + cur_xy = xy_pose; + accum_movement = accum_movement + norm(cur_xy - bfr_xy); + bfr_xy = cur_xy; % reset + if( accum_movement < ENOUGH_MOVEMENT_GAP) + continue; + else + accum_movement = 0; + end + + %% description: ptcloud to descriptor + % different for the method + + % pc + if( strcmp(testinfo.method_, 'pc') ) + points(:, 3) = points(:, 3) + LIDAR_HEIGHT; + ptcloud = pointCloud(points); + + num_sector = testinfo.res_(2); + num_ring = testinfo.res_(1); + max_range = testinfo.roi_(1); + + desc_list = ptcloud2polarcontextAug(ptcloud, num_sector, num_ring, max_range, NUM_AUG_SCs); % up to 80 meter + + % cc + elseif( strcmp(testinfo.method_, 'cc') ) + points(:, 3) = points(:, 3) + LIDAR_HEIGHT; + ptcloud = pointCloud(points); + +% unit_move = round( testinfo.roi_(1)*2 / testinfo.res_(1) ); +% unit_lateral = round( testinfo.roi_(2)*2 / testinfo.res_(2) ); + unit_move = ( testinfo.roi_(1)*2 / testinfo.res_(1) ); + unit_lateral = ( testinfo.roi_(2)*2 / testinfo.res_(2) ); + axis_unit = [unit_move, unit_lateral]; + axis_range = testinfo.roi_; + + desc = ptcloud2cartcontext(ptcloud, axis_unit, axis_range); % up to 80 meter + ikey = sc2invkey(desc); + + % m2dp + elseif( strcmp(testinfo.method_, 'm2dp') ) + ptcloud_down = pcdownsample(pointCloud(points), 'gridAverage', 0.1); + [desc, ~] = M2DP(ptcloud_down.Location); + ikey = 0; % dummy + end + +% figure(1); clf; +% imagesc(desc); +% colormap jet; caxis([0, 4]); axis equal; + + + %% 20200810 debugger:to see the point cloud +% if(save_counter == 1056 || save_counter == 2714) +% disp(1); +% end + + + %% save + for idx_aug = 1:NUM_AUG_SCs + desc = desc_list{idx_aug}; + + ikey = sc2invkey(desc); +% ikey = sc2invkeyL0(desc); + + descs_list{idx_aug}{save_counter} = desc; + invkeys_list{idx_aug}(save_counter, :) = ikey; % for only cc and pc (not for M2DP and pnvlad) + end + + xy_poses(save_counter, :) = xy_pose; + + save_counter = save_counter + 1; + + % log + if(rem(scan_idx, 100) == 0) + message = strcat(num2str(scan_idx), " / ", num2str(num_data), " processed (skip: ", num2str(SKIP_FRAMES), ")"); + disp(message); + end + +% figure(13); clf; +% xy_poses_show = xy_poses; +% xy_poses_show = xy_poses_show(1:save_counter-1, :); +% scatter(xy_poses_show(:, 1), xy_poses_show(:, 2)); axis equal; + +end + +% take valids +for idx_aug = 1:NUM_AUG_SCs + descs_list{idx_aug} = descs_list{idx_aug}(1:save_counter-1); + invkeys_list{idx_aug} = invkeys_list{idx_aug}(1:save_counter-1, :); +end + +xy_poses = xy_poses(1:save_counter-1, :); + +end + diff --git a/tro2022/batch_processor/makeAugmentedExperienceTreeMerge.m b/tro2022/batch_processor/makeAugmentedExperienceTreeMerge.m new file mode 100644 index 0000000..8b937be --- /dev/null +++ b/tro2022/batch_processor/makeAugmentedExperienceTreeMerge.m @@ -0,0 +1,151 @@ +function [descs_list, invkeys_list, xy_poses] = makeAugmentedExperienceTreeMerge(testinfo, SKIP_FRAMES) + +%% +LIDAR_HEIGHT = 1.9; + +%% +num_data = length(testinfo.scan_names_); +num_data_save = floor(num_data/SKIP_FRAMES) + 1; + +save_counter = 1; + +num_invaxis = testinfo.res_(1); +% num_vaxis = testinfo.res_(2); + +%% aug save vars (currently supports only pc, 20200513) +NUM_AUG_SCs = 3; % including the original (no-root-shifted) + +descs_list = {}; +invkeys_list = {}; + +for idx_copies = 1:NUM_AUG_SCs + descs_list{end+1} = cell(1, num_data_save); + invkeys_list{end+1} = zeros(num_data_save, num_invaxis); +end + +xy_poses = zeros(num_data_save, 2); + + +%% main parse +for scan_idx = 1:num_data +% for scan_idx = 1:10000 + + %% curr info + if(rem(scan_idx, SKIP_FRAMES) ~=0) + continue; + end + + scan_name = testinfo.scan_names_{scan_idx}; + scan_time = str2double(scan_name(1:end-4)); + scan_path = strcat(testinfo.scan_dir_, scan_name); + + + %% load datat + if( strcmp(testinfo.dataset_, 'MulRan') ) + try + points = readBin(scan_path); + catch + continue; + end + [~, nearest_idx] = min(abs(repmat(scan_time, length(testinfo.gtpose_time_), 1) - testinfo.gtpose_time_)); + xy_pose = testinfo.gtpose_xy_(nearest_idx, :); + + elseif( strcmp(testinfo.dataset_, 'KITTI') ) + points = readBin(scan_path); + xy_pose = testinfo.gtpose_xy_(scan_idx, :); + + elseif( strcmp(testinfo.dataset_, 'Oxford') ) + points = readBinOxford(scan_path); + [~, nearest_idx] = min(abs(repmat(scan_time, length(testinfo.gtpose_time_), 1) - testinfo.gtpose_time_)); + xy_pose = testinfo.gtpose_xy_(nearest_idx, :); + +% figure(1); clf; +% pcshow(points); +% xlim([-80, 80]); ylim([-80, 80]); zlim([-5, 20]); + + elseif( strcmp(testinfo.dataset_, 'NaverLabs') ) + points = readScanNaverlabs(scan_path); + xy_pose = testinfo.gtpose_xy_(scan_idx, :); + + end + + + %% description: ptcloud to descriptor + % different for the method + + % pc + if( strcmp(testinfo.method_, 'pc') ) + points(:, 3) = points(:, 3) + LIDAR_HEIGHT; + ptcloud = pointCloud(points); + + num_sector = testinfo.res_(2); + num_ring = testinfo.res_(1); + max_range = testinfo.roi_(1); + + desc_list = ptcloud2polarcontextAug(ptcloud, num_sector, num_ring, max_range); % up to 80 meter + + % cc + elseif( strcmp(testinfo.method_, 'cc') ) + points(:, 3) = points(:, 3) + LIDAR_HEIGHT; + ptcloud = pointCloud(points); + +% unit_move = round( testinfo.roi_(1)*2 / testinfo.res_(1) ); +% unit_lateral = round( testinfo.roi_(2)*2 / testinfo.res_(2) ); + unit_move = ( testinfo.roi_(1)*2 / testinfo.res_(1) ); + unit_lateral = ( testinfo.roi_(2)*2 / testinfo.res_(2) ); + axis_unit = [unit_move, unit_lateral]; + axis_range = testinfo.roi_; + + desc = ptcloud2cartcontext(ptcloud, axis_unit, axis_range); % up to 80 meter + ikey = sc2invkeyL0(desc); + + % m2dp + elseif( strcmp(testinfo.method_, 'm2dp') ) + ptcloud_down = pcdownsample(pointCloud(points), 'gridAverage', 0.1); + [desc, ~] = M2DP(ptcloud_down.Location); + ikey = 0; % dummy + end + +% figure(1); clf; +% imagesc(desc); +% colormap jet; caxis([0, 4]); axis equal; + + + %% save + for idx_aug = 1:NUM_AUG_SCs + desc = desc_list{idx_aug}; + +% ikey = sc2invkey(desc); + ikey = sc2invkeyL0(desc); + + descs_list{idx_aug}{save_counter} = desc; + invkeys_list{idx_aug}(save_counter, :) = ikey; % for only cc and pc (not for M2DP and pnvlad) + end + + xy_poses(save_counter, :) = xy_pose; + + save_counter = save_counter + 1; + + % log + if(rem(scan_idx, 100) == 0) + message = strcat(num2str(scan_idx), " / ", num2str(num_data), " processed (skip: ", num2str(SKIP_FRAMES), ")"); + disp(message); + end + +% figure(13); clf; +% xy_poses_show = xy_poses; +% xy_poses_show = xy_poses_show(1:save_counter-1, :); +% scatter(xy_poses_show(:, 1), xy_poses_show(:, 2)); axis equal; + +end + +% take valids +for idx_aug = 1:NUM_AUG_SCs + descs_list{idx_aug} = descs_list{idx_aug}(1:save_counter-1); + invkeys_list{idx_aug} = invkeys_list{idx_aug}(1:save_counter-1, :); +end + +xy_poses = xy_poses(1:save_counter-1, :); + +end + diff --git a/tro2022/batch_processor/makeExperience.m b/tro2022/batch_processor/makeExperience.m new file mode 100644 index 0000000..828d0ff --- /dev/null +++ b/tro2022/batch_processor/makeExperience.m @@ -0,0 +1,140 @@ +function [descs, invkeys, xy_poses] = makeExperience(testinfo, SKIP_FRAMES) + +%% +LIDAR_HEIGHT = 1.9; + +%% +num_data = length(testinfo.scan_names_); +num_data_save = floor(num_data/SKIP_FRAMES) + 1; + +save_counter = 1; + +num_invaxis = testinfo.res_(1); +% num_vaxis = testinfo.res_(2); + +descs = cell(1, num_data_save); +invkeys = zeros(num_data_save, num_invaxis); +xy_poses = zeros(num_data_save, 2); + +for scan_idx = 1:num_data + + %% curr info + if(rem(scan_idx, SKIP_FRAMES) ~=0) + continue; + end + + scan_name = testinfo.scan_names_{scan_idx}; + scan_time = str2double(scan_name(1:end-4)); + scan_path = strcat(testinfo.scan_dir_, scan_name); + + + %% load datat + if( strcmp(testinfo.dataset_, 'MulRan') ) + try + points = readBin(scan_path); + catch + continue; + end + [~, nearest_idx] = min(abs(repmat(scan_time, length(testinfo.gtpose_time_), 1) - testinfo.gtpose_time_)); + xy_pose = testinfo.gtpose_xy_(nearest_idx, :); + + elseif( strcmp(testinfo.dataset_, 'KITTI') ) + points = readBin(scan_path); + xy_pose = testinfo.gtpose_xy_(scan_idx, :); + + elseif( strcmp(testinfo.dataset_, 'Oxford') ) + points = readBinOxford(scan_path); + [~, nearest_idx] = min(abs(repmat(scan_time, length(testinfo.gtpose_time_), 1) - testinfo.gtpose_time_)); + xy_pose = testinfo.gtpose_xy_(nearest_idx, :); + + % debug oxford +% figure(1); clf; +% pcshow(points); colormap jet; caxis([0, 10]); +% xlim([-80, 80]); ylim([-80, 80]); zlim([0, 20]); +% xlabel('x (moving)'); ylabel('y (lateral)'); +% view(0, 90); + + elseif( strcmp(testinfo.dataset_, 'NaverLabs') ) + points = readScanNaverlabs(scan_path); + xy_pose = testinfo.gtpose_xy_(scan_idx, :); + + end + + + %% description: ptcloud to descriptor + % different for the method + + % ccpoints + if( strcmp(testinfo.method_, 'pc') ) + points(:, 3) = points(:, 3) + LIDAR_HEIGHT; + ptcloud = pointCloud(points); + + num_sector = testinfo.res_(2); + num_ring = testinfo.res_(1); + max_range = testinfo.roi_(1); + + desc = ptcloud2polarcontext(ptcloud, num_sector, num_ring, max_range); % up to 80 meter + ikey = sc2invkey(desc); + + % pc + elseif( strcmp(testinfo.method_, 'cc') ) + points(:, 3) = points(:, 3) + LIDAR_HEIGHT; + ptcloud = pointCloud(points); + +% unit_move = round( testinfo.roi_(1)*2 / testinfo.res_(1) ); +% unit_lateral = round( testinfo.roi_(2)*2 / testinfo.res_(2) ); + unit_move = ( testinfo.roi_(1)*2 / testinfo.res_(1) ); + unit_lateral = ( testinfo.roi_(2)*2 / testinfo.res_(2) ); + axis_unit = [unit_move, unit_lateral]; + axis_range = testinfo.roi_; + + desc = ptcloud2cartcontext(ptcloud, axis_unit, axis_range); % up to 80 meter + ikey = sc2invkey(desc); + +% figure(2); clf; +% % subplot(1,2, 2); +% % desc_viz = flip( flip(desc, 2), 1); +% imagesc(desc); +% colormap jet; caxis([0, 10]); + + + % m2dp + elseif( strcmp(testinfo.method_, 'm2dp') ) + ptcloud_down = pcdownsample(pointCloud(points), 'gridAverage', 0.1); + [desc, ~] = M2DP(ptcloud_down.Location); + ikey = 0; % dummy + end + + + %% save + descs{save_counter} = desc; + invkeys(save_counter, :) = ikey; % for only cc and pc (not for M2DP and pnvlad) + xy_poses(save_counter, :) = xy_pose; + + save_counter = save_counter + 1; + + xy = xy_poses(1:save_counter-1, :); + +% if(length(xy) > 3) +% figure(3); clf; +% xyt = [xy, linspace(1, 0.01 * length(xy), length(xy))']; +% pcshow(xyt, 'MarkerSize', 100); +% colormap jet; +% end + + % log + if(rem(scan_idx, 100) == 0) + message = strcat(num2str(scan_idx), " / ", num2str(num_data), " processed (skip: ", num2str(SKIP_FRAMES), ")"); + disp(message); + end + +end + +% take valids +descs = descs(1:save_counter-1); +invkeys = invkeys(1:save_counter-1, :); +xy_poses = xy_poses(1:save_counter-1, :); + + +end + diff --git a/tro2022/batch_processor/makeExperienceEquiDist.m b/tro2022/batch_processor/makeExperienceEquiDist.m new file mode 100644 index 0000000..df81a13 --- /dev/null +++ b/tro2022/batch_processor/makeExperienceEquiDist.m @@ -0,0 +1,209 @@ +function [descs, invkeys, xyt_poses] = makeExperienceEquiDist(testinfo, SKIP_FRAMES) + +%% +LIDAR_HEIGHT = 1.9; + +%% +num_data = length(testinfo.scan_names_); +num_data_save = floor(num_data/SKIP_FRAMES) + 1; + +save_counter = 1; + +num_invaxis = testinfo.res_(1); +% num_vaxis = testinfo.res_(2); + +descs = cell(1, num_data_save); +invkeys = zeros(num_data_save, num_invaxis); +xy_poses = zeros(num_data_save, 2); +xyt_poses = zeros(num_data_save, 3); + +% +ENOUGH_MOVEMENT_GAP = 1; % meter +bfr_xy = [0, 0]; % init +cur_xy = bfr_xy; +accum_movement = 0; + +scan_save_idx = 1; + +scan_idx_list = []; % 20210425 rebutal +for scan_idx = 1:num_data +% for scan_idx = 10000:num_data + + %% curr info + if(rem(scan_idx, SKIP_FRAMES) ~=0) + continue; + end + + scan_name = testinfo.scan_names_{scan_idx}; + scan_time = str2double(scan_name(1:end-4)); + scan_path = strcat(testinfo.scan_dir_, scan_name); + + + %% load datat + if( strcmp(testinfo.dataset_, 'MulRan') ) + try + points = readBin(scan_path); + catch + continue; + end + [~, nearest_idx] = min(abs(repmat(scan_time, length(testinfo.gtpose_time_), 1) - testinfo.gtpose_time_)); + xy_pose = testinfo.gtpose_xy_(nearest_idx, :); + + elseif( strcmp(testinfo.dataset_, 'KITTI') ) + points = readBin(scan_path); + xy_pose = testinfo.gtpose_xy_(scan_idx, :); + + elseif( strcmp(testinfo.dataset_, 'Oxford') ) + [~, near_scan2_idx] = min( abs(testinfo.scan2_times_ - scan_time) ); + scan2_name = testinfo.scan2_names_{near_scan2_idx}; + scan2_path = strcat(testinfo.scan2_dir_, scan2_name); + + points_left = readBinOxford(scan_path); + points_left(:,2) = points_left(:,2) + 0.47; + + points_right = readBinOxford(scan2_path); + points_right(:,2) = points_right(:,2) - 0.47; + +% figure(1); clf; +% pcshow(points_left, 'r'); hold on; +% pcshow(points_right, 'b'); hold on; +% set(gcf,'color','w'); +% set(gca,'color','w'); +% set(gca,'XColor', 'none', 'YColor','none', 'ZColor','none'); +% grid off; +% xlim([-100, 100]); +% ylim([-100, 100]); +% zlim([-2, 15]); + + points = [points_left; points_right]; + % points need to be x shifted 1.5m (20200811 thought) because span + % cpt and lidar gap + + [~, nearest_idx] = min(abs(repmat(scan_time, length(testinfo.gtpose_time_), 1) - testinfo.gtpose_time_)); + xy_pose = testinfo.gtpose_xy_(nearest_idx, :); + + % debug oxford +% figure(1); clf; +% pcshow(points); colormap jet; caxis([0, 10]); +% xlim([-80, 80]); ylim([-80, 80]); zlim([0, 20]); +% xlabel('x (moving)'); ylabel('y (lateral)'); +% view(0, 90); + + elseif( strcmp(testinfo.dataset_, 'NaverLabs') ) + points = readScanNaverlabs(scan_path); + xy_pose = testinfo.gtpose_xy_(scan_idx, :); + + end + + xyt_pose = [xy_pose, scan_time]; + + + %% check enough movement + cur_xy = xy_pose; + accum_movement = accum_movement + norm(cur_xy - bfr_xy); + bfr_xy = cur_xy; % reset + if( accum_movement < ENOUGH_MOVEMENT_GAP) + continue; + else + accum_movement = 0; + end + +% scan_idx_list = [scan_idx_list, scan_idx]; +% continue; + + + %% description: ptcloud to descriptor + % different for the method + + % ccpoints + if( strcmp(testinfo.method_, 'pc') ) + points(:, 3) = points(:, 3) + LIDAR_HEIGHT; + ptcloud = pointCloud(points); + + num_sector = testinfo.res_(2); + num_ring = testinfo.res_(1); + max_range = testinfo.roi_(1); + + desc = ptcloud2polarcontext(ptcloud, num_sector, num_ring, max_range); % up to 80 meter + ikey = sc2invkey(desc); + + % pc + elseif( strcmp(testinfo.method_, 'cc') ) + points(:, 3) = points(:, 3) + LIDAR_HEIGHT; + ptcloud = pointCloud(points); + +% unit_move = round( testinfo.roi_(1)*2 / testinfo.res_(1) ); +% unit_lateral = round( testinfo.roi_(2)*2 / testinfo.res_(2) ); + unit_move = ( testinfo.roi_(1)*2 / testinfo.res_(1) ); + unit_lateral = ( testinfo.roi_(2)*2 / testinfo.res_(2) ); + axis_unit = [unit_move, unit_lateral]; + axis_range = testinfo.roi_; + + desc = ptcloud2cartcontext(ptcloud, axis_unit, axis_range); % up to 80 meter + ikey = sc2invkey(desc); + +% figure(2); clf; +% % subplot(1,2, 2); +% % desc_viz = flip( flip(desc, 2), 1); +% imagesc(desc); +% colormap jet; caxis([0, 10]); + + + % m2dp + elseif( strcmp(testinfo.method_, 'm2dp') ) + ptcloud_down = pcdownsample(pointCloud(points), 'gridAverage', 0.1); + [desc, ~] = M2DP(ptcloud_down.Location); + ikey = 0; % dummy + end + +% %% 20200810 debugger:to see the point cloud +% if(save_counter == 1056 || save_counter == 2714) +% disp(1); +% end + + % 20200810 save ptcloud for debugging to visuzliae figrue +% save(strcat("draw_aug/ptcloud_save_jan15/", num2str(save_counter), ".mat"), 'ptcloud') + +% % save poitns to find the failure case (20200907) +% pts_save_dir = "/home/user/Documents/Dropbox-local/My Papers/20 TRO/20200511 evaluation code main (include ablation)/main/3-2. main_parts (fig maker - recent adaptive num ignore ver)/#. July_selected/fail_case_saver/scan_saver/pangyo/npy"; +% scan_name = strcat(num2str(scan_save_idx), '.npy'); +% writeNPY(points, fullfile(pts_save_dir, scan_name)); +% scan_save_idx = scan_save_idx + 1; + + %% save + descs{save_counter} = desc; + invkeys(save_counter, :) = ikey; % for only cc and pc (not for M2DP and pnvlad) + xy_poses(save_counter, :) = xy_pose; + xyt_poses(save_counter, :) = xyt_pose; + + save_counter = save_counter + 1; + + xy = xy_poses(1:save_counter-1, :); + + +% if(length(xy) > 3) +% figure(3); clf; +% xyt = [xy, linspace(1, 0.01 * length(xy), length(xy))']; +% pcshow(xyt, 'MarkerSize', 100); +% colormap jet; +% end + + % log + if(rem(scan_idx, 100) == 0) + message = strcat(num2str(scan_idx), " / ", num2str(num_data), " processed (skip: ", num2str(SKIP_FRAMES), ")"); + disp(message); + end + +end + +disp(scan_idx_list) + +% take valids +descs = descs(1:save_counter-1); +invkeys = invkeys(1:save_counter-1, :); +xy_poses = xy_poses(1:save_counter-1, :); +xyt_poses = xyt_poses(1:save_counter-1, :); + + +end + diff --git a/tro2022/batch_processor/makeExperienceEquiDistXYZT.m b/tro2022/batch_processor/makeExperienceEquiDistXYZT.m new file mode 100644 index 0000000..2885a85 --- /dev/null +++ b/tro2022/batch_processor/makeExperienceEquiDistXYZT.m @@ -0,0 +1,186 @@ +function [descs, invkeys, xyt_poses] = makeExperienceEquiDistXYZT(testinfo, SKIP_FRAMES) + +%% +LIDAR_HEIGHT = 1.9; + +%% +num_data = length(testinfo.scan_names_); +num_data_save = floor(num_data/SKIP_FRAMES) + 1; + +save_counter = 1; + +num_invaxis = testinfo.res_(1); +% num_vaxis = testinfo.res_(2); + +descs = cell(1, num_data_save); +invkeys = zeros(num_data_save, num_invaxis); +xy_poses = zeros(num_data_save, 2); +xyt_poses = zeros(num_data_save, 3); + +% +ENOUGH_MOVEMENT_GAP = 1; % meter +bfr_xy = [0, 0]; % init +cur_xy = bfr_xy; +accum_movement = 0; + +for scan_idx = 1:num_data +% for scan_idx = 10000:num_data + + %% curr info + if(rem(scan_idx, SKIP_FRAMES) ~=0) + continue; + end + + scan_name = testinfo.scan_names_{scan_idx}; + scan_time = str2double(scan_name(1:end-4)); + scan_path = strcat(testinfo.scan_dir_, scan_name); + + + %% load datat + if( strcmp(testinfo.dataset_, 'MulRan') ) + try + points = readBin(scan_path); + catch + continue; + end + [~, nearest_idx] = min(abs(repmat(scan_time, length(testinfo.gtpose_time_), 1) - testinfo.gtpose_time_)); + xy_pose = testinfo.gtpose_xy_(nearest_idx, :); + + elseif( strcmp(testinfo.dataset_, 'KITTI') ) + points = readBin(scan_path); + xy_pose = testinfo.gtpose_xy_(scan_idx, :); + + elseif( strcmp(testinfo.dataset_, 'Oxford') ) + [~, near_scan2_idx] = min( abs(testinfo.scan2_times_ - scan_time) ); + scan2_name = testinfo.scan2_names_{near_scan2_idx}; + scan2_path = strcat(testinfo.scan2_dir_, scan2_name); + + points_left = readBinOxford(scan_path); + points_left(:,2) = points_left(:,2) + 0.47; + + points_right = readBinOxford(scan2_path); + points_right(:,2) = points_right(:,2) - 0.47; + +% figure(1); clf; +% pcshow(points_left, 'r'); hold on; +% pcshow(points_right, 'b'); hold on; +% set(gcf,'color','w'); +% set(gca,'color','w'); +% set(gca,'XColor', 'none', 'YColor','none', 'ZColor','none'); +% grid off; +% xlim([-100, 100]); +% ylim([-100, 100]); +% zlim([-2, 15]); + + points = [points_left; points_right]; + + [~, nearest_idx] = min(abs(repmat(scan_time, length(testinfo.gtpose_time_), 1) - testinfo.gtpose_time_)); + xy_pose = testinfo.gtpose_xy_(nearest_idx, :); + + % debug oxford +% figure(1); clf; +% pcshow(points); colormap jet; caxis([0, 10]); +% xlim([-80, 80]); ylim([-80, 80]); zlim([0, 20]); +% xlabel('x (moving)'); ylabel('y (lateral)'); +% view(0, 90); + + elseif( strcmp(testinfo.dataset_, 'NaverLabs') ) + points = readScanNaverlabs(scan_path); + xy_pose = testinfo.gtpose_xy_(scan_idx, :); + + end + + xyt_pose = [xy_pose, scan_time]; + + + %% check enough movement + cur_xy = xy_pose; + accum_movement = accum_movement + norm(cur_xy - bfr_xy); + bfr_xy = cur_xy; % reset + if( accum_movement < ENOUGH_MOVEMENT_GAP) + continue; + else + accum_movement = 0; + end + + + + %% description: ptcloud to descriptor + % different for the method + + % ccpoints + if( strcmp(testinfo.method_, 'pc') ) + points(:, 3) = points(:, 3) + LIDAR_HEIGHT; + ptcloud = pointCloud(points); + + num_sector = testinfo.res_(2); + num_ring = testinfo.res_(1); + max_range = testinfo.roi_(1); + + desc = ptcloud2polarcontext(ptcloud, num_sector, num_ring, max_range); % up to 80 meter + ikey = sc2invkey(desc); + + % pc + elseif( strcmp(testinfo.method_, 'cc') ) + points(:, 3) = points(:, 3) + LIDAR_HEIGHT; + ptcloud = pointCloud(points); + +% unit_move = round( testinfo.roi_(1)*2 / testinfo.res_(1) ); +% unit_lateral = round( testinfo.roi_(2)*2 / testinfo.res_(2) ); + unit_move = ( testinfo.roi_(1)*2 / testinfo.res_(1) ); + unit_lateral = ( testinfo.roi_(2)*2 / testinfo.res_(2) ); + axis_unit = [unit_move, unit_lateral]; + axis_range = testinfo.roi_; + + desc = ptcloud2cartcontext(ptcloud, axis_unit, axis_range); % up to 80 meter + ikey = sc2invkey(desc); + +% figure(2); clf; +% % subplot(1,2, 2); +% % desc_viz = flip( flip(desc, 2), 1); +% imagesc(desc); +% colormap jet; caxis([0, 10]); + + + % m2dp + elseif( strcmp(testinfo.method_, 'm2dp') ) + ptcloud_down = pcdownsample(pointCloud(points), 'gridAverage', 0.1); + [desc, ~] = M2DP(ptcloud_down.Location); + ikey = 0; % dummy + end + + + %% save + descs{save_counter} = desc; + invkeys(save_counter, :) = ikey; % for only cc and pc (not for M2DP and pnvlad) + xy_poses(save_counter, :) = xy_pose; + xyt_poses(save_counter, :) = xyt_pose; + + save_counter = save_counter + 1; + + xy = xy_poses(1:save_counter-1, :); + +% if(length(xy) > 3) +% figure(3); clf; +% xyt = [xy, linspace(1, 0.01 * length(xy), length(xy))']; +% pcshow(xyt, 'MarkerSize', 100); +% colormap jet; +% end + + % log + if(rem(scan_idx, 100) == 0) + message = strcat(num2str(scan_idx), " / ", num2str(num_data), " processed (skip: ", num2str(SKIP_FRAMES), ")"); + disp(message); + end + +end + +% take valids +descs = descs(1:save_counter-1); +invkeys = invkeys(1:save_counter-1, :); +xy_poses = xy_poses(1:save_counter-1, :); +xyt_poses = xyt_poses(1:save_counter-1, :); + + +end + diff --git a/tro2022/m2dp/M2DP b/tro2022/m2dp/M2DP new file mode 160000 index 0000000..d80e80b --- /dev/null +++ b/tro2022/m2dp/M2DP @@ -0,0 +1 @@ +Subproject commit d80e80b58bc941bf8fe6bb912a95700a5bf35cb5 diff --git a/tro2022/misc/deg2utm.m b/tro2022/misc/deg2utm.m new file mode 100644 index 0000000..a6527fa --- /dev/null +++ b/tro2022/misc/deg2utm.m @@ -0,0 +1,121 @@ +function [x,y,utmzone] = deg2utm(Lat,Lon) +% ------------------------------------------------------------------------- +% [x,y,utmzone] = deg2utm(Lat,Lon) +% +% Description: Function to convert lat/lon vectors into UTM coordinates (WGS84). +% Some code has been extracted from UTM.m function by Gabriel Ruiz Martinez. +% +% Inputs: +% Lat: Latitude vector. Degrees. +ddd.ddddd WGS84 +% Lon: Longitude vector. Degrees. +ddd.ddddd WGS84 +% +% Outputs: +% x, y , utmzone. See example +% +% Example 1: +% Lat=[40.3154333; 46.283900; 37.577833; 28.645650; 38.855550; 25.061783]; +% Lon=[-3.4857166; 7.8012333; -119.95525; -17.759533; -94.7990166; 121.640266]; +% [x,y,utmzone] = deg2utm(Lat,Lon); +% fprintf('%7.0f ',x) +% 458731 407653 239027 230253 343898 362850 +% fprintf('%7.0f ',y) +% 4462881 5126290 4163083 3171843 4302285 2772478 +% utmzone = +% 30 T +% 32 T +% 11 S +% 28 R +% 15 S +% 51 R +% +% Example 2: If you have Lat/Lon coordinates in Degrees, Minutes and Seconds +% LatDMS=[40 18 55.56; 46 17 2.04]; +% LonDMS=[-3 29 8.58; 7 48 4.44]; +% Lat=dms2deg(mat2dms(LatDMS)); %convert into degrees +% Lon=dms2deg(mat2dms(LonDMS)); %convert into degrees +% [x,y,utmzone] = deg2utm(Lat,Lon) +% +% Author: +% Rafael Palacios +% Universidad Pontificia Comillas +% Madrid, Spain +% Version: Apr/06, Jun/06, Aug/06, Aug/06 +% Aug/06: fixed a problem (found by Rodolphe Dewarrat) related to southern +% hemisphere coordinates. +% Aug/06: corrected m-Lint warnings +%------------------------------------------------------------------------- +% Argument checking +% +error(nargchk(2, 2, nargin)); %2 arguments required +n1=length(Lat); +n2=length(Lon); +if (n1~=n2) + error('Lat and Lon vectors should have the same length'); +end +% Memory pre-allocation +% +x=zeros(n1,1); +y=zeros(n1,1); +utmzone(n1,:)='60 X'; +% Main Loop +% +for i=1:n1 + la=Lat(i); + lo=Lon(i); + sa = 6378137.000000 ; sb = 6356752.314245; + + %e = ( ( ( sa ^ 2 ) - ( sb ^ 2 ) ) ^ 0.5 ) / sa; + e2 = ( ( ( sa ^ 2 ) - ( sb ^ 2 ) ) ^ 0.5 ) / sb; + e2cuadrada = e2 ^ 2; + c = ( sa ^ 2 ) / sb; + %alpha = ( sa - sb ) / sa; %f + %ablandamiento = 1 / alpha; % 1/f + lat = la * ( pi / 180 ); + lon = lo * ( pi / 180 ); + Huso = fix( ( lo / 6 ) + 31); + S = ( ( Huso * 6 ) - 183 ); + deltaS = lon - ( S * ( pi / 180 ) ); + if (la<-72), Letra='C'; + elseif (la<-64), Letra='D'; + elseif (la<-56), Letra='E'; + elseif (la<-48), Letra='F'; + elseif (la<-40), Letra='G'; + elseif (la<-32), Letra='H'; + elseif (la<-24), Letra='J'; + elseif (la<-16), Letra='K'; + elseif (la<-8), Letra='L'; + elseif (la<0), Letra='M'; + elseif (la<8), Letra='N'; + elseif (la<16), Letra='P'; + elseif (la<24), Letra='Q'; + elseif (la<32), Letra='R'; + elseif (la<40), Letra='S'; + elseif (la<48), Letra='T'; + elseif (la<56), Letra='U'; + elseif (la<64), Letra='V'; + elseif (la<72), Letra='W'; + else Letra='X'; + end + a = cos(lat) * sin(deltaS); + epsilon = 0.5 * log( ( 1 + a) / ( 1 - a ) ); + nu = atan( tan(lat) / cos(deltaS) ) - lat; + v = ( c / ( ( 1 + ( e2cuadrada * ( cos(lat) ) ^ 2 ) ) ) ^ 0.5 ) * 0.9996; + ta = ( e2cuadrada / 2 ) * epsilon ^ 2 * ( cos(lat) ) ^ 2; + a1 = sin( 2 * lat ); + a2 = a1 * ( cos(lat) ) ^ 2; + j2 = lat + ( a1 / 2 ); + j4 = ( ( 3 * j2 ) + a2 ) / 4; + j6 = ( ( 5 * j4 ) + ( a2 * ( cos(lat) ) ^ 2) ) / 3; + alfa = ( 3 / 4 ) * e2cuadrada; + beta = ( 5 / 3 ) * alfa ^ 2; + gama = ( 35 / 27 ) * alfa ^ 3; + Bm = 0.9996 * c * ( lat - alfa * j2 + beta * j4 - gama * j6 ); + xx = epsilon * v * ( 1 + ( ta / 3 ) ) + 500000; + yy = nu * v * ( 1 + ta ) + Bm; + if (yy<0) + yy=9999999+yy; + end + x(i)=xx; + y(i)=yy; + utmzone(i,:)=sprintf('%02d %c',Huso,Letra); +end diff --git a/tro2022/misc/dist_btn_pose.m b/tro2022/misc/dist_btn_pose.m new file mode 100644 index 0000000..39aa646 --- /dev/null +++ b/tro2022/misc/dist_btn_pose.m @@ -0,0 +1,3 @@ +function dist = dist_btn_pose(pose1, pose2) + dist = sqrt( (pose1(1) - pose2(1))^2 + (pose1(2) - pose2(2))^2); +end diff --git a/tro2022/misc/find_topk_from_candidates.m b/tro2022/misc/find_topk_from_candidates.m new file mode 100644 index 0000000..96b899e --- /dev/null +++ b/tro2022/misc/find_topk_from_candidates.m @@ -0,0 +1,28 @@ +function [ nearest_idx, min_dist ] = find_topk_from_candidates(query_img, query_idx, candidates, thres) + +global num_node_enough_apart; +global radar_imgs; + +nearest_idx = 0; +min_dist = inf; % initialization +for ith_candidate = 1:length(candidates) + candidate_node_idx = candidates(ith_candidate); + candidate_img = radar_imgs{candidate_node_idx}; + + if( abs(query_idx - candidate_node_idx) < num_node_enough_apart) + continue; + end + + distance_to_query = dist(query_img, candidate_img); + if( distance_to_query > thres) + continue; + end + + if( distance_to_query < min_dist) + nearest_idx = candidate_node_idx; + min_dist = distance_to_query; + end +end + +end + diff --git a/tro2022/misc/getHistMat.m b/tro2022/misc/getHistMat.m new file mode 100644 index 0000000..5c92326 --- /dev/null +++ b/tro2022/misc/getHistMat.m @@ -0,0 +1,36 @@ +function [hist_mat] = getHistMat( data, loop_distribution_viz ) + +% +unit_cell_meter = 0.5; +unit_cell_deg = 10; +upperbound_cell_meter = 8; +upperbound_cell_deg = 180; +num_cells_meter = round(upperbound_cell_meter/unit_cell_meter); +num_cells_deg = round(upperbound_cell_deg/unit_cell_deg); + +if( isempty(data) ) + hist_mat = zeros(num_cells_meter, num_cells_deg); +else + % + hist_mat = hist3(data,'CDataMode','auto','FaceColor','interp', 'EdgeColor', 'none' ... + ,'Edges', {0:unit_cell_meter:upperbound_cell_meter, 0:unit_cell_deg:upperbound_cell_deg} ); % [1m, 15deg] + + % return: remove the zero paddings + hist_mat = hist_mat(1:end-1, 1:end-1); + + % viz + if( loop_distribution_viz ) + figure(round(rand(1)*1000)); clf; +% figure(viz_fig_idx); clf; + hist3(data,'CDataMode','auto','FaceColor','interp', 'EdgeColor', 'none' ... + ,'Edges', {0:unit_cell_meter:upperbound_cell_meter, 0:unit_cell_deg:upperbound_cell_deg}); % [1m, 15deg] + caxis([0, 10]); + % colormap((jet)); + colormap(flipud(bone)); +% colorbar; + end + +end + +end + diff --git a/tro2022/misc/isNonSameDirectionRevisit.m b/tro2022/misc/isNonSameDirectionRevisit.m new file mode 100644 index 0000000..c802400 --- /dev/null +++ b/tro2022/misc/isNonSameDirectionRevisit.m @@ -0,0 +1,18 @@ +function [bool_revisit, relative_angle_deg] = isNonSameDirectionRevisit( submap_traj1, submap_traj2 ) + % return direction 0 (same) or 1 (reverse) + bool_revisit = 0; + + traj1_moving_vec = [submap_traj1(end, 1) - submap_traj1(1, 1), submap_traj1(end, 2) - submap_traj1(1, 2)]; + traj2_moving_vec = [submap_traj2(end, 1) - submap_traj2(1, 1), submap_traj2(end, 2) - submap_traj2(1, 2)]; + + unit_dot = dot(traj1_moving_vec, traj2_moving_vec) ... + / (norm(traj1_moving_vec) * norm(traj2_moving_vec)); + + relative_angle_deg = rad2deg(acos(unit_dot)); + + % disp(unit_dot) + if( unit_dot < 0.2 ) % 0.2 used for not sameness direction + bool_revisit = 1; + end + +end diff --git a/tro2022/misc/isRevisitGlobalLoc.m b/tro2022/misc/isRevisitGlobalLoc.m new file mode 100644 index 0000000..ef532c2 --- /dev/null +++ b/tro2022/misc/isRevisitGlobalLoc.m @@ -0,0 +1,20 @@ +function [is_revisit, min_dist] = isRevisitGlobalLoc(query_pose, db_poses, thres) + +num_dbs = size(db_poses, 1); + +dists = zeros(1, num_dbs); +for ii=1:num_dbs + dist = norm(query_pose - db_poses(ii, :)); + dists(ii) = dist; +end + +if ( min(dists) < thres ) + is_revisit = 1; +else + is_revisit = 0; +end + +min_dist = min(dists); + +end + diff --git a/tro2022/misc/isRevisitGlobalLocV2.m b/tro2022/misc/isRevisitGlobalLocV2.m new file mode 100644 index 0000000..b5e553f --- /dev/null +++ b/tro2022/misc/isRevisitGlobalLocV2.m @@ -0,0 +1,23 @@ +function [is_revisit, min_dist, armin_idx] = isRevisitGlobalLocV2(query_pose, db_poses, thres) + +num_dbs = size(db_poses, 1); + +dists = zeros(1, num_dbs); +for ii=1:num_dbs + dist = norm(query_pose - db_poses(ii, :)); + dists(ii) = dist; +end + +if ( min(dists) < thres ) + is_revisit = 1; +else + is_revisit = 0; +end + +% min_dist = min(dists); +[min_dist, armin_idx] = min(dists); + +% disp(armin_idx); + +end + diff --git a/tro2022/misc/is_revisit.m b/tro2022/misc/is_revisit.m new file mode 100644 index 0000000..4cd2ca3 --- /dev/null +++ b/tro2022/misc/is_revisit.m @@ -0,0 +1,23 @@ +function [ revisitness ] = is_revisit(query_idx, query_pose, radar_poses, revisit_criteria, num_node_enough_apart) + +num_db = size(radar_poses, 1); + +revisitness = 0; +for ii = 1:num_db + + if( abs(query_idx - ii) < num_node_enough_apart) + continue; + end + + pose = radar_poses(ii, :); + + dist = dist_btn_pose(query_pose, pose); + + if(dist < revisit_criteria) + revisitness = 1; + break; + end +end + +end + diff --git a/tro2022/misc/normalizedKLDiv.m b/tro2022/misc/normalizedKLDiv.m new file mode 100644 index 0000000..b6aed1b --- /dev/null +++ b/tro2022/misc/normalizedKLDiv.m @@ -0,0 +1,7 @@ +function [dist] = normalizedKLDiv(P, Q) + +dist_kl = KLDiv(P, Q); +dist = 1 - exp(-dist_kl); + +end + diff --git a/tro2022/misc/osdir.m b/tro2022/misc/osdir.m new file mode 100644 index 0000000..2698637 --- /dev/null +++ b/tro2022/misc/osdir.m @@ -0,0 +1,4 @@ +function [files] = osdir(path) + files = dir(path); files(1:2) = []; files = {files(:).name}; +end + diff --git a/tro2022/reader/npymatlab/constructNPYheader.m b/tro2022/reader/npymatlab/constructNPYheader.m new file mode 100644 index 0000000..ca2840e --- /dev/null +++ b/tro2022/reader/npymatlab/constructNPYheader.m @@ -0,0 +1,88 @@ + + + +function header = constructNPYheader(dataType, shape, varargin) + + if ~isempty(varargin) + fortranOrder = varargin{1}; % must be true/false + littleEndian = varargin{2}; % must be true/false + else + fortranOrder = true; + littleEndian = true; + end + + dtypesMatlab = {'uint8','uint16','uint32','uint64','int8','int16','int32','int64','single','double', 'logical'}; + dtypesNPY = {'u1', 'u2', 'u4', 'u8', 'i1', 'i2', 'i4', 'i8', 'f4', 'f8', 'b1'}; + + magicString = uint8([147 78 85 77 80 89]); %x93NUMPY + + majorVersion = uint8(1); + minorVersion = uint8(0); + + % build the dict specifying data type, array order, endianness, and + % shape + dictString = '{''descr'': '''; + + if littleEndian + dictString = [dictString '<']; + else + dictString = [dictString '>']; + end + + dictString = [dictString dtypesNPY{strcmp(dtypesMatlab,dataType)} ''', ']; + + dictString = [dictString '''fortran_order'': ']; + + if fortranOrder + dictString = [dictString 'True, ']; + else + dictString = [dictString 'False, ']; + end + + dictString = [dictString '''shape'': (']; + +% if length(shape)==1 && shape==1 +% +% else +% for s = 1:length(shape) +% if s==length(shape) && shape(s)==1 +% +% else +% dictString = [dictString num2str(shape(s))]; +% if length(shape)>1 && s+1==length(shape) && shape(s+1)==1 +% dictString = [dictString ',']; +% elseif length(shape)>1 && s %s', tempFilename, inFilename, outFilename)); + + otherwise + fprintf(1, 'I don''t know how to concatenate files for your OS, but you can finish making the NPY youself by concatenating %s with %s.\n', tempFilename, inFilename); +end + diff --git a/tro2022/reader/npymatlab/readNPY.m b/tro2022/reader/npymatlab/readNPY.m new file mode 100644 index 0000000..9095d00 --- /dev/null +++ b/tro2022/reader/npymatlab/readNPY.m @@ -0,0 +1,37 @@ + + +function data = readNPY(filename) +% Function to read NPY files into matlab. +% *** Only reads a subset of all possible NPY files, specifically N-D arrays of certain data types. +% See https://github.com/kwikteam/npy-matlab/blob/master/tests/npy.ipynb for +% more. +% + +[shape, dataType, fortranOrder, littleEndian, totalHeaderLength, ~] = readNPYheader(filename); + +if littleEndian + fid = fopen(filename, 'r', 'l'); +else + fid = fopen(filename, 'r', 'b'); +end + +try + + [~] = fread(fid, totalHeaderLength, 'uint8'); + + % read the data + data = fread(fid, prod(shape), [dataType '=>' dataType]); + + if length(shape)>1 && ~fortranOrder + data = reshape(data, shape(end:-1:1)); + data = permute(data, [length(shape):-1:1]); + elseif length(shape)>1 + data = reshape(data, shape); + end + + fclose(fid); + +catch me + fclose(fid); + rethrow(me); +end diff --git a/tro2022/reader/npymatlab/readNPYheader.m b/tro2022/reader/npymatlab/readNPYheader.m new file mode 100644 index 0000000..165a58c --- /dev/null +++ b/tro2022/reader/npymatlab/readNPYheader.m @@ -0,0 +1,69 @@ + + +function [arrayShape, dataType, fortranOrder, littleEndian, totalHeaderLength, npyVersion] = readNPYheader(filename) +% function [arrayShape, dataType, fortranOrder, littleEndian, ... +% totalHeaderLength, npyVersion] = readNPYheader(filename) +% +% parse the header of a .npy file and return all the info contained +% therein. +% +% Based on spec at http://docs.scipy.org/doc/numpy-dev/neps/npy-format.html + +fid = fopen(filename); + +% verify that the file exists +if (fid == -1) + if ~isempty(dir(filename)) + error('Permission denied: %s', filename); + else + error('File not found: %s', filename); + end +end + +try + + dtypesMatlab = {'uint8','uint16','uint32','uint64','int8','int16','int32','int64','single','double', 'logical'}; + dtypesNPY = {'u1', 'u2', 'u4', 'u8', 'i1', 'i2', 'i4', 'i8', 'f4', 'f8', 'b1'}; + + + magicString = fread(fid, [1 6], 'uint8=>uint8'); + + if ~all(magicString == [147,78,85,77,80,89]) + error('readNPY:NotNUMPYFile', 'Error: This file does not appear to be NUMPY format based on the header.'); + end + + majorVersion = fread(fid, [1 1], 'uint8=>uint8'); + minorVersion = fread(fid, [1 1], 'uint8=>uint8'); + + npyVersion = [majorVersion minorVersion]; + + headerLength = fread(fid, [1 1], 'uint16=>uint16'); + + totalHeaderLength = 10+headerLength; + + arrayFormat = fread(fid, [1 headerLength], 'char=>char'); + + % to interpret the array format info, we make some fairly strict + % assumptions about its format... + + r = regexp(arrayFormat, '''descr''\s*:\s*''(.*?)''', 'tokens'); + dtNPY = r{1}{1}; + + littleEndian = ~strcmp(dtNPY(1), '>'); + + dataType = dtypesMatlab{strcmp(dtNPY(2:3), dtypesNPY)}; + + r = regexp(arrayFormat, '''fortran_order''\s*:\s*(\w+)', 'tokens'); + fortranOrder = strcmp(r{1}{1}, 'True'); + + r = regexp(arrayFormat, '''shape''\s*:\s*\((.*?)\)', 'tokens'); + shapeStr = r{1}{1}; + arrayShape = str2num(shapeStr(shapeStr~='L')); + + + fclose(fid); + +catch me + fclose(fid); + rethrow(me); +end diff --git a/tro2022/reader/npymatlab/writeNPY.m b/tro2022/reader/npymatlab/writeNPY.m new file mode 100644 index 0000000..844dc35 --- /dev/null +++ b/tro2022/reader/npymatlab/writeNPY.m @@ -0,0 +1,25 @@ + + +function writeNPY(var, filename) +% function writeNPY(var, filename) +% +% Only writes little endian, fortran (column-major) ordering; only writes +% with NPY version number 1.0. +% +% Always outputs a shape according to matlab's convention, e.g. (10, 1) +% rather than (10,). + + +shape = size(var); +dataType = class(var); + +header = constructNPYheader(dataType, shape); + +fid = fopen(filename, 'w'); +fwrite(fid, header, 'uint8'); +fwrite(fid, var, dataType); +fclose(fid); + + +end + diff --git a/tro2022/reader/readBin.m b/tro2022/reader/readBin.m new file mode 100644 index 0000000..685cb20 --- /dev/null +++ b/tro2022/reader/readBin.m @@ -0,0 +1,9 @@ +function points = readBin(bin_path) + +%% Read +fid = fopen(bin_path, 'rb'); raw_data = fread(fid, [4 inf], 'single'); fclose(fid); +points = raw_data(1:3,:)'; + +% ptcloud = pointCloud(points); + +end % end of function diff --git a/tro2022/reader/readBinOxford.m b/tro2022/reader/readBinOxford.m new file mode 100644 index 0000000..a35b371 --- /dev/null +++ b/tro2022/reader/readBinOxford.m @@ -0,0 +1,17 @@ +function points = readBinOxford(bin_path) + +%% Read +% fid = fopen(bin_path, 'rb'); raw_data = fread(fid, [4 inf], 'single'); fclose(fid); +% points = raw_data(1:3,:)'; + +velodyne_file = fopen(bin_path); +data = fread(velodyne_file, 'single'); +fclose(velodyne_file); +pointcloud = reshape(data, [numel(data)/4 4]); +points = pointcloud(:, 1:3); +points(:, 3) = -1 * points(:, 3); % because hdl32e lidar was mounted vertically reversed. see https://oxford-robotics-institute.github.io/radar-robotcar-dataset/documentation +% ê·¼ë° ì´ë ‡ê²Œ 바꾸면 엄밀하진 않다. (좌우가 바뀌어버림) + +% ptcloud = pointCloud(points); + +end % end of function diff --git a/tro2022/reader/readScanNaverlabs.m b/tro2022/reader/readScanNaverlabs.m new file mode 100644 index 0000000..9fff31f --- /dev/null +++ b/tro2022/reader/readScanNaverlabs.m @@ -0,0 +1,9 @@ +function [ pc_xyz ] = readScanNaverlabs( global_path ) + +pc_info = double(readNPY(global_path)); +pc_xyz = pc_info(:, 1:3) / 100; % because original cm +% ptcloud = pointCloud(pc_xyz); +% ptcloud.Intensity = pc_info(:, 4); + +end + diff --git a/tro2022/scancontext/alg/alignUsingSectorKey.m b/tro2022/scancontext/alg/alignUsingSectorKey.m new file mode 100644 index 0000000..11593cc --- /dev/null +++ b/tro2022/scancontext/alg/alignUsingSectorKey.m @@ -0,0 +1,15 @@ +function [init_rot] = alignUsingSectorKey(query_sk, candidate_sk) + +min_rmse = inf; +init_rot = 0; +for ii = 1:length(candidate_sk) + shifted_candidate_sk = circshift(candidate_sk, ii, 2); + rmse = norm(query_sk - shifted_candidate_sk); + if( rmse < min_rmse) + min_rmse = rmse; + init_rot = ii; + end +end + +end + diff --git a/tro2022/scancontext/alg/sc_dist.m b/tro2022/scancontext/alg/sc_dist.m new file mode 100644 index 0000000..aad4e73 --- /dev/null +++ b/tro2022/scancontext/alg/sc_dist.m @@ -0,0 +1,41 @@ +function [dist] = sc_dist(sc1, sc2) + +num_sectors = size(sc1, 2); + +% repeate to move 1 columns +sim_for_each_cols = zeros(1, num_sectors); + +for i = 1:num_sectors + %% Shift + one_step = 1; % const + sc1 = circshift(sc1, one_step, 2); % 2 means columne shift + + %% compare + sum_of_cos_sim = 0; + num_col_engaged = 0; + + for j = 1:num_sectors + col_j_1 = sc1(:,j); + col_j_2 = sc2(:,j); + + if( ~any(col_j_1) || ~any(col_j_2)) + continue; + end + + % calc sim + cos_similarity = dot(col_j_1, col_j_2) / (norm(col_j_1)*norm(col_j_2)); + sum_of_cos_sim = sum_of_cos_sim + cos_similarity; + + num_col_engaged = num_col_engaged +1; + end + + % devided by num_col_engaged: So, even if there are many columns that are excluded from the calculation, we can get high scores if other columns are well fit. + sim_for_each_cols(i) = sum_of_cos_sim/num_col_engaged; + +end + +sim = max(sim_for_each_cols); + +dist = 1 - sim; + +end diff --git a/tro2022/scancontext/alg/sc_dist_fast.m b/tro2022/scancontext/alg/sc_dist_fast.m new file mode 100644 index 0000000..03708a6 --- /dev/null +++ b/tro2022/scancontext/alg/sc_dist_fast.m @@ -0,0 +1,51 @@ +function [dist] = sc_dist_fast(sc1, sc2, init_rot, search_ratio) + +num_sectors = size(sc1, 2); + +%% shrinked search space +search_space_ratio = 0.5 * search_ratio; + +yaws_to_search = []; +yaws_to_search = [yaws_to_search, init_rot]; +for ii = 1:ceil(search_space_ratio * num_sectors) + yaws_to_search = [ yaws_to_search, rem(init_rot - ii, num_sectors) ]; + yaws_to_search = [ yaws_to_search, rem(init_rot + ii, num_sectors) ]; +end +yaws_to_search = sort(yaws_to_search); + +%% search +sim_for_each_cols = zeros(1, length(yaws_to_search)); +for ii = 1:length(yaws_to_search) + %% Shift + ii_shift = yaws_to_search(ii); + sc1 = circshift(sc1, ii_shift, 2); % 2 means columne shift + + %% compare + sum_of_cos_sim = 0; + num_col_engaged = 0; + + for j = 1:num_sectors + col_j_1 = sc1(:,j); + col_j_2 = sc2(:,j); + + if( ~any(col_j_1) || ~any(col_j_2)) + continue; + end + + % calc sim + cos_similarity = dot(col_j_1, col_j_2) / (norm(col_j_1)*norm(col_j_2)); + sum_of_cos_sim = sum_of_cos_sim + cos_similarity; + + num_col_engaged = num_col_engaged +1; + end + + % devided by num_col_engaged: So, even if there are many columns that are excluded from the calculation, we can get high scores if other columns are well fit. + sim_for_each_cols(ii) = sum_of_cos_sim/num_col_engaged; + +end + +sim = max(sim_for_each_cols); + +dist = 1 - sim; + +end diff --git a/tro2022/scancontext/alg/sc_dist_fast_with_argalign.m b/tro2022/scancontext/alg/sc_dist_fast_with_argalign.m new file mode 100644 index 0000000..76c5df9 --- /dev/null +++ b/tro2022/scancontext/alg/sc_dist_fast_with_argalign.m @@ -0,0 +1,53 @@ +function [dist, argalign] = sc_dist_fast_with_argalign(sc1_in, sc2, init_rot, search_ratio) + +num_sectors = size(sc1_in, 2); + +%% shrinked search space +search_space_ratio = 0.5 * search_ratio; + +yaws_to_search = []; +yaws_to_search = [yaws_to_search, init_rot]; +for ii = 1:ceil(search_space_ratio * num_sectors) + yaws_to_search = [ yaws_to_search, rem(init_rot - ii, num_sectors) ]; + yaws_to_search = [ yaws_to_search, rem(init_rot + ii, num_sectors) ]; +end +yaws_to_search = sort(yaws_to_search); + +%% search +sim_for_each_cols = zeros(1, length(yaws_to_search)); +for ii = 1:length(yaws_to_search) + %% Shift + ii_shift = yaws_to_search(ii); + sc1 = circshift(sc1_in, ii_shift, 2); % 2 means columne shift + + %% compare + sum_of_cos_sim = 0; + num_col_engaged = 0; + + for j = 1:num_sectors + col_j_1 = sc1(:,j); + col_j_2 = sc2(:,j); + + if( ~any(col_j_1) || ~any(col_j_2)) + continue; + end + + % calc sim + cos_similarity = dot(col_j_1, col_j_2) / (norm(col_j_1)*norm(col_j_2)); + sum_of_cos_sim = sum_of_cos_sim + cos_similarity; + + num_col_engaged = num_col_engaged +1; + end + + % devided by num_col_engaged: So, even if there are many columns that are excluded from the calculation, we can get high scores if other columns are well fit. + sim_for_each_cols(ii) = sum_of_cos_sim/num_col_engaged; + +end + +[sim, argsim] = max(sim_for_each_cols); + +argalign = yaws_to_search(argsim); + +dist = 1 - sim; + +end diff --git a/tro2022/scancontext/alg/sc_dist_with_argalign.m b/tro2022/scancontext/alg/sc_dist_with_argalign.m new file mode 100644 index 0000000..fdbc538 --- /dev/null +++ b/tro2022/scancontext/alg/sc_dist_with_argalign.m @@ -0,0 +1,44 @@ +function [dist, argalign] = sc_dist_with_argaling(sc1, sc2) + +num_sectors = size(sc1, 2); + +% repeate to move 1 columns +sim_for_each_cols = zeros(1, num_sectors); + +for i = 1:num_sectors + %% Shift + one_step = 1; % const + sc1 = circshift(sc1, one_step, 2); % 2 means columne shift + + %% compare + sum_of_cos_sim = 0; + num_col_engaged = 0; + + for j = 1:num_sectors + col_j_1 = sc1(:,j); + col_j_2 = sc2(:,j); + + if( ~any(col_j_1) || ~any(col_j_2)) + continue; + end + + % calc sim + cos_similarity = dot(col_j_1, col_j_2) / (norm(col_j_1)*norm(col_j_2)); + sum_of_cos_sim = sum_of_cos_sim + cos_similarity; + + num_col_engaged = num_col_engaged +1; + end + + % devided by num_col_engaged: So, even if there are many columns that are excluded from the calculation, we can get high scores if other columns are well fit. + sim_for_each_cols(i) = sum_of_cos_sim/num_col_engaged; + +end + +% sim = max(sim_for_each_cols); +[sim, argsim] = max(sim_for_each_cols); + +argalign = argsim; + +dist = 1 - sim; + +end diff --git a/tro2022/scancontext/desc/ptcloud2cartcontext.m b/tro2022/scancontext/desc/ptcloud2cartcontext.m new file mode 100644 index 0000000..58a483b --- /dev/null +++ b/tro2022/scancontext/desc/ptcloud2cartcontext.m @@ -0,0 +1,71 @@ +function [ img ] = ptcloud2cartcontext( ptcloud, axis_unit, axis_range ) +%% NOTE +% date: 2020. 05. 05 + +%% params +% NOTE: author's param +% - axis_unit: [5, 2]; % unit: meter / i.e., 5m x 2m is a bin size . +% - axis_range: [100, -40]; % unit: meter / i.e., the coverage of a cart context + +x_unit = axis_unit(1); % moving direction +y_unit = axis_unit(2); % lateral direction + +x_max = axis_range(1); +y_max = axis_range(2); +x_range = [-x_max, x_max]; +y_range = [-y_max, y_max]; + +num_x = round((2*x_max) / x_unit); +num_y = round((2*y_max) / y_unit); + + +%% +% Downsampling for fast search +gridStep = 0.5; % cubic grid downsampling is applied in the paper. +ptcloud = pcdownsample(ptcloud, 'gridAverage', gridStep); + +% point cloud information +points = ptcloud.Location; +points = points( (points(:, 1) ~= 0) & (points(:, 2) ~= 0), : ); +points = points( (x_range(1) < points(:, 1)) & (points(:, 1) < x_range(2)), : ); +points = points( (y_range(1) < points(:, 2)) & (points(:, 2) < y_range(2)), : ); + +points_x = points(:, 1); +points_y = points(:, 2); + +% points_z = points(:, 3); +% num_points = ptcloud.Count; + +% +points_xindex = ( sign(points_x) .* (floor(abs(points_x) / x_unit) ) ) + (floor(num_x/2) + 1); +points_yindex = ( sign(points_y) .* (floor(abs(points_y) / y_unit) ) ) + (floor(num_y/2) + 1); + +points_5d = [points, points_xindex, points_yindex]; +points_5d = sortrows(points_5d, [4, 5]); + +img = zeros(num_x, num_y); +for ii = 1:length(points_5d) + pt_z = points_5d(ii, 3); + + pixel_u = points_5d(ii, 4); + pixel_v = points_5d(ii, 5); +% disp([pixel_u, pixel_v]); + + if(img(pixel_u, pixel_v) < pt_z) + img(pixel_u, pixel_v) = pt_z; + end +end + +% figure(2); clf; +% imagesc(img); +% colormap jet; +% caxis([2, 14]); +% axis equal; +% +% figure(3); clf; +% imagesc(circshift(img, 10, 2)); +% colormap jet; +% caxis([2, 14]); +% axis equal; + +end \ No newline at end of file diff --git a/tro2022/scancontext/desc/ptcloud2polarcontext.m b/tro2022/scancontext/desc/ptcloud2polarcontext.m new file mode 100644 index 0000000..a5ec07d --- /dev/null +++ b/tro2022/scancontext/desc/ptcloud2polarcontext.m @@ -0,0 +1,104 @@ +function [ img ] = ptcloud2polarcontext( ptcloud, num_sector, num_ring, max_range ) +%% NOTE +% date: 2020. 05. 05 + +%% Preprocessing + +% Downsampling for fast search +gridStep = 0.5; % 0.5m cubic grid downsampling is applied in the paper. +ptcloud = pcdownsample(ptcloud, 'gridAverage', gridStep); + +% point cloud information +num_points = ptcloud.Count; +gap = max_range / num_ring; +angle_one_sector = 360/num_sector; + + +%% vacant bins +cell_bins = cell(num_ring, num_sector); +cell_bin_counter = ones(num_ring, num_sector); + +enough_large = 500; % for fast and constant time save, We contain maximum 500 points per each bin. +enough_small = -10000; +for ith_ring = 1:num_ring + for ith_sector = 1:num_sector + bin = enough_small * ones(enough_large, 3); + cell_bins{ith_ring, ith_sector} = bin; + end +end + + +%% Save a point to the corresponding bin +for ith_point =1:num_points + + % Point information + ith_point_xyz = ptcloud.Location(ith_point,:); + ith_point_r = sqrt(ith_point_xyz(1)^2 + ith_point_xyz(2)^2); + ith_point_theta = xy2theta(ith_point_xyz(1), ith_point_xyz(2)); % degree + + % Find the corresponding ring index + tmp_ring_index = floor(ith_point_r/gap); + if(tmp_ring_index >= num_ring) + ring_index = num_ring; + else + ring_index = tmp_ring_index + 1; + end + + % Find the corresponding sector index + tmp_sector_index = ceil(ith_point_theta/angle_one_sector); + if(tmp_sector_index == 0) + sector_index = 1; + elseif(tmp_sector_index > num_sector || tmp_sector_index < 1) + sector_index = num_sector; + else + sector_index = tmp_sector_index; + end + + % Assign point to the corresponding bin cell + try + corresponding_counter = cell_bin_counter(ring_index, sector_index); % 1D real value. + catch + continue; + end + cell_bins{ring_index, sector_index}(corresponding_counter, :) = ith_point_xyz; + cell_bin_counter(ring_index, sector_index) = cell_bin_counter(ring_index, sector_index) + 1; % increase count 1 + +end + + +%% bin to image format (2D matrix) +img = zeros(num_ring, num_sector); + +min_num_thres = 5; % a bin with few points, we consider it is noise. + +% Find maximum Z value of each bin and Save into img +for ith_ring = 1:num_ring + for ith_sector = 1:num_sector + value_of_the_bin = 0; + points_in_bin_ij = cell_bins{ith_ring, ith_sector}; + + if( IsBinHaveMoreThanMinimumPoints(points_in_bin_ij, min_num_thres, enough_small) ) + value_of_the_bin = max(points_in_bin_ij(:, 3)); + else + value_of_the_bin = 0; + end + + img(ith_ring, ith_sector) = value_of_the_bin; + end +end + + +end % end of the main function + + +function bool = IsBinHaveMoreThanMinimumPoints(mat, minimum_thres, enough_small) + +min_thres_point = mat(minimum_thres, :); + +if( isequal(min_thres_point, [ enough_small, enough_small, enough_small]) ) + bool = 0; +else + bool = 1; +end + +end diff --git a/tro2022/scancontext/desc/ptcloud2polarcontextAug.m b/tro2022/scancontext/desc/ptcloud2polarcontextAug.m new file mode 100644 index 0000000..17612e7 --- /dev/null +++ b/tro2022/scancontext/desc/ptcloud2polarcontextAug.m @@ -0,0 +1,158 @@ +function [ imgs_cell ] = ptcloud2polarcontextAug( ptcloud_in, num_sector, num_ring, max_range, NUM_NEIGHBOR ) +%% NOTE +% date: 2020. 05. 05 +% update: 2020. 07. 20 + +%% Preprocessing +VIZFIG_IDX = 321; +IF_VISFIG = 0; +if(IF_VISFIG) +figure(VIZFIG_IDX); clf; +end + +% Downsampling for fast search +gridStep = 0.5; % 0.5m cubic grid downsampling is applied in the paper. +ptcloud_orig = pcdownsample(ptcloud_in, 'gridAverage', gridStep); + +% point cloud information +gap = max_range / num_ring; +angle_one_sector = 360/num_sector; + +% aug main +imgs_cell = {}; +% lateral_move_aug_list = [-4, -2, 0, 2, 4]; +% lateral_move_aug_list = [-4, -2, 0, 2, 4]; + +SHIFT_LEN = 2.0; % at iros18 used 3m + +% 8 neighbor +% NUM_NEIGHBOR = myself (orig) + num neighbors +% NUM_NEIGHBOR = 7; + +% order: orig, east (-y), west (+y), south (-x), north (+x), es (-y, -x), ws (+y, -x), en (-y, +x), wn (+y, +x) +% 8 aug ver +% move_aug_list = [0, 0 , 0, -SHIFT_LEN, SHIFT_LEN, -SHIFT_LEN, SHIFT_LEN , -SHIFT_LEN, SHIFT_LEN; ... +% 0, -SHIFT_LEN, SHIFT_LEN, 0 , 0 , -SHIFT_LEN, -SHIFT_LEN, SHIFT_LEN , SHIFT_LEN]; + +% 5 aug ver +move_aug_list = [0, 0 , 0, -SHIFT_LEN, SHIFT_LEN , -SHIFT_LEN, SHIFT_LEN; ... + 0, -SHIFT_LEN, SHIFT_LEN, -SHIFT_LEN, -SHIFT_LEN, SHIFT_LEN , SHIFT_LEN]; + +% for move_idx = 1:size(move_aug_list, 2) +for move_idx = 1:NUM_NEIGHBOR + + xyz_orig = ptcloud_orig.Location; + + x_move_ith_neighbor = move_aug_list(1, move_idx); + y_move_ith_neighbor = move_aug_list(2, move_idx); + + T = [ 1 0 0 x_move_ith_neighbor; 0 1 0 y_move_ith_neighbor; 0 0 1 0; 0 0 0 1]; + ptcloud_moved = [xyz_orig, ones(length(xyz_orig), 1) ]; + ptcloud_ith_neighbor = inv(T) * transpose(ptcloud_moved); + + ptcloud_ith_neighbor = transpose(ptcloud_ith_neighbor); + xyz_aug = ptcloud_ith_neighbor(:, 1:3); + + ptcloud_root_shifted = pointCloud(xyz_aug); + num_points = ptcloud_root_shifted.Count; + + % debug + if (IF_VISFIG) + rand_color = repmat(rand(1,3), ptcloud_root_shifted.Count, 1); + figure(VIZFIG_IDX); + pcshow(ptcloud_root_shifted.Location, rand_color); hold on; + end + + %% vacant bins + cell_bins = cell(num_ring, num_sector); + cell_bin_counter = ones(num_ring, num_sector); + + enough_large = 500; % for fast and constant time save, We contain maximum 500 points per each bin. + enough_small = -10000; + for ith_ring = 1:num_ring + for ith_sector = 1:num_sector + bin = enough_small * ones(enough_large, 3); + cell_bins{ith_ring, ith_sector} = bin; + end + end + + + %% Save a point to the corresponding bin + for ith_point =1:num_points + + % Point information + ith_point_xyz = ptcloud_root_shifted.Location(ith_point,:); + ith_point_r = sqrt(ith_point_xyz(1)^2 + ith_point_xyz(2)^2); + ith_point_theta = xy2theta(ith_point_xyz(1), ith_point_xyz(2)); % degree + + % Find the corresponding ring index + tmp_ring_index = floor(ith_point_r/gap); + if(tmp_ring_index >= num_ring) + ring_index = num_ring; + else + ring_index = tmp_ring_index + 1; + end + + % Find the corresponding sector index + tmp_sector_index = ceil(ith_point_theta/angle_one_sector); + if(tmp_sector_index == 0) + sector_index = 1; + elseif(tmp_sector_index > num_sector || tmp_sector_index < 1) + sector_index = num_sector; + else + sector_index = tmp_sector_index; + end + + % Assign point to the corresponding bin cell + try + corresponding_counter = cell_bin_counter(ring_index, sector_index); % 1D real value. + catch + continue; + end + cell_bins{ring_index, sector_index}(corresponding_counter, :) = ith_point_xyz; + cell_bin_counter(ring_index, sector_index) = cell_bin_counter(ring_index, sector_index) + 1; % increase count 1 + + end + + + %% bin to image format (2D matrix) + img = zeros(num_ring, num_sector); + + min_num_thres = 5; % a bin with few points, we consider it is noise. + + % Find maximum Z value of each bin and Save into img + for ith_ring = 1:num_ring + for ith_sector = 1:num_sector + value_of_the_bin = 0; + points_in_bin_ij = cell_bins{ith_ring, ith_sector}; + + if( IsBinHaveMoreThanMinimumPoints(points_in_bin_ij, min_num_thres, enough_small) ) + value_of_the_bin = max(points_in_bin_ij(:, 3)); + else + value_of_the_bin = 0; + end + + img(ith_ring, ith_sector) = value_of_the_bin; + end + end + + + imgs_cell{end+1} = img; + +end + + +end % end of the main function + + +function bool = IsBinHaveMoreThanMinimumPoints(mat, minimum_thres, enough_small) + +min_thres_point = mat(minimum_thres, :); + +if( isequal(min_thres_point, [ enough_small, enough_small, enough_small]) ) + bool = 0; +else + bool = 1; +end + +end diff --git a/tro2022/scancontext/desc/sc2invkey.m b/tro2022/scancontext/desc/sc2invkey.m new file mode 100644 index 0000000..c64c116 --- /dev/null +++ b/tro2022/scancontext/desc/sc2invkey.m @@ -0,0 +1,12 @@ +function [ ring_key ] = sc2invkey(sc) + +num_rings = size(sc, 1); + +ring_key = zeros(1, num_rings); +for ith=1:num_rings + ith_ring = sc(ith,:); + ring_key(ith) = mean(ith_ring); +end + +end + diff --git a/tro2022/scancontext/desc/sc2invkeyL0.m b/tro2022/scancontext/desc/sc2invkeyL0.m new file mode 100644 index 0000000..13e1193 --- /dev/null +++ b/tro2022/scancontext/desc/sc2invkeyL0.m @@ -0,0 +1,13 @@ +function [ ring_key ] = sc2invkeyL0(sc) + +num_rings = size(sc, 1); + +ring_key = zeros(1, num_rings); +for ith=1:num_rings + ith_ring = sc(ith,:); +% ring_key(ith) = mean(ith_ring); + ring_key(ith) = nnz(ith_ring); +end + +end + diff --git a/tro2022/scancontext/desc/sc2vkey.m b/tro2022/scancontext/desc/sc2vkey.m new file mode 100644 index 0000000..b551187 --- /dev/null +++ b/tro2022/scancontext/desc/sc2vkey.m @@ -0,0 +1,12 @@ +function [ sector_key ] = sc2vkey(sc) + +num_sectors = size(sc, 2); + +sector_key = zeros(1, num_sectors); +for ith = 1:num_sectors + ith_sector = sc(:, ith); + sector_key(ith) = mean(ith_sector); +end + +end + diff --git a/tro2022/scancontext/desc/xy2theta.m b/tro2022/scancontext/desc/xy2theta.m new file mode 100644 index 0000000..cbc4b39 --- /dev/null +++ b/tro2022/scancontext/desc/xy2theta.m @@ -0,0 +1,17 @@ +function [ theta ] = xy2theta( x, y ) + + if (x >= 0 && y >= 0) + theta = 180/pi * atan(y/x); + end + if (x < 0 && y >= 0) + theta = 180 - ((180/pi) * atan(y/(-x))); + end + if (x < 0 && y < 0) + theta = 180 + ((180/pi) * atan(y/x)); + end + if ( x >= 0 && y < 0) + theta = 360 - ((180/pi) * atan((-y)/x)); + end + +end + diff --git a/tro2022/transform/RotationsLib/AbsPhaseFromPhase.m b/tro2022/transform/RotationsLib/AbsPhaseFromPhase.m new file mode 100644 index 0000000..692a195 --- /dev/null +++ b/tro2022/transform/RotationsLib/AbsPhaseFromPhase.m @@ -0,0 +1,20 @@ +% AbsPhaseFromPhase.m - Philipp Allgeuer - 16/01/18 +% Converts a tilt phase rotation to the corresponding absolute tilt phase representation. +% +% function [AbsPhase] = AbsPhaseFromPhase(Phase) +% +% Phase ==> Input tilt phase rotation +% AbsPhase ==> Equivalent absolute tilt phase rotation + +% Main function +function [AbsPhase] = AbsPhaseFromPhase(Phase) + + % Precalculate trigonometric values + cpsi = cos(Phase(3)); + spsi = sin(Phase(3)); + + % Construct the output absolute tilt phase + AbsPhase = [cpsi*Phase(1)-spsi*Phase(2) spsi*Phase(1)+cpsi*Phase(2) Phase(3)]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/AbsPhaseFromQuat.m b/tro2022/transform/RotationsLib/AbsPhaseFromQuat.m new file mode 100644 index 0000000..30c6c7b --- /dev/null +++ b/tro2022/transform/RotationsLib/AbsPhaseFromQuat.m @@ -0,0 +1,17 @@ +% AbsPhaseFromQuat.m - Philipp Allgeuer - 16/01/18 +% Converts a quaternion rotation to the corresponding absolute tilt phase representation. +% +% function [AbsPhase, Tilt] = AbsPhaseFromQuat(Quat) +% +% Quat ==> Input quaternion rotation +% AbsPhase ==> Equivalent absolute tilt phase rotation + +% Main function +function [AbsPhase, Tilt] = AbsPhaseFromQuat(Quat) + + % Construct the output tilt phase + Tilt = TiltFromQuat(Quat); + AbsPhase = AbsPhaseFromTilt(Tilt); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/AbsPhaseFromTilt.m b/tro2022/transform/RotationsLib/AbsPhaseFromTilt.m new file mode 100644 index 0000000..ce6c22b --- /dev/null +++ b/tro2022/transform/RotationsLib/AbsPhaseFromTilt.m @@ -0,0 +1,17 @@ +% AbsPhaseFromTilt.m - Philipp Allgeuer - 16/01/18 +% Converts a tilt angles rotation to the corresponding absolute tilt phase representation. +% +% function [AbsPhase] = AbsPhaseFromTilt(Tilt) +% +% Tilt ==> Input tilt angles rotation +% AbsPhase ==> Equivalent absolute tilt phase rotation + +% Main function +function [AbsPhase] = AbsPhaseFromTilt(Tilt) + + % Construct the output absolute tilt phase + psigam = Tilt(1) + Tilt(2); + AbsPhase = [Tilt(3)*cos(psigam) Tilt(3)*sin(psigam) Tilt(1)]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/AngVelFromEulerVel.m b/tro2022/transform/RotationsLib/AngVelFromEulerVel.m new file mode 100644 index 0000000..a71ae3b --- /dev/null +++ b/tro2022/transform/RotationsLib/AngVelFromEulerVel.m @@ -0,0 +1,30 @@ +% AngVelFromEulerVel.m - Philipp Allgeuer - 05/11/14 +% Convert a ZYX Euler angles velocity to an angular velocity vector +% +% function [AngVel] = AngVelFromEulerVel(Euler, EulerVel) +% +% The angular velocity vector is expressed in global coordinates. +% +% Euler ==> ZYX Euler angles rotation [phi theta psi] +% EulerVel ==> ZYX Euler angles velocity [dphi dtheta dpsi] +% AngVel ==> Angular velocity vector [wx;wy;wz] + +% Main function +function [AngVel] = AngVelFromEulerVel(Euler, EulerVel) + + % Precompute sin and cos values + cphi = cos(Euler(1)); + sphi = sin(Euler(1)); + cth = cos(Euler(2)); + sth = sin(Euler(2)); + + % Store Euler angle velocities in conveniently named variables + dphi = EulerVel(1); + dth = EulerVel(2); + dpsi = EulerVel(3); + + % Calculate the angular velocity vector in unrotated global coordinates + AngVel = [dpsi*cth*cphi-dth*sphi ; dpsi*cth*sphi+dth*cphi ; dphi-dpsi*sth]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/AngVelFromQuatTime.m b/tro2022/transform/RotationsLib/AngVelFromQuatTime.m new file mode 100644 index 0000000..1a13e03 --- /dev/null +++ b/tro2022/transform/RotationsLib/AngVelFromQuatTime.m @@ -0,0 +1,45 @@ +% AngVelFromQuatTime.m - Philipp Allgeuer - 06/04/17 +% Convert a quaternion rotation plus time into the corresponding angular velocity vector +% +% function [AngVel] = AngVelFromQuatTime(dt, QuatB, QuatA) +% +% QuatA and QuatB are rotations relative to global coordinates. +% If QuatA is not provided, the quaternion rotation QuatB is converted to an angular velocity. +% If QuatA is provided, the global quaternion rotation from QuatA to QuatB is converted to an +% angular velocity, that is, QuatB*inv(QuatA) is converted instead of just QuatB. +% The angular velocity vector is always expressed in global coordinates. +% +% dt ==> The time of the rotation +% QuatB ==> Input quaternion rotation B (assumed to have unit norm) +% QuatA ==> Input quaternion rotation A (assumed to have unit norm) + +% Main function +function [AngVel] = AngVelFromQuatTime(dt, QuatB, QuatA) + + % Input arguments + if nargin < 3 + Quat = QuatB; + else + Quat = QuatMult(QuatB, QuatInv(QuatA)); + end + if Quat(1) < 0 + Quat = -Quat; + end + + % Calculate the angular velocity direction + vec = Quat(2:4); + vecnorm = sqrt(vec(1)*vec(1) + vec(2)*vec(2) + vec(3)*vec(3)); + if vecnorm <= 0 + AngVel = [0 0 0]; + return + end + vec = vec / vecnorm; + + % Calculate the angular velocity magnitude + theta = 2*atan2(vecnorm, Quat(1)); + + % Construct the output angular velocity + AngVel = theta*vec/dt; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/AngVelFromTiltVel.m b/tro2022/transform/RotationsLib/AngVelFromTiltVel.m new file mode 100644 index 0000000..62d5e49 --- /dev/null +++ b/tro2022/transform/RotationsLib/AngVelFromTiltVel.m @@ -0,0 +1,31 @@ +% AngVelFromTiltVel.m - Philipp Allgeuer - 02/05/17 +% Convert a tilt angles velocity to an angular velocity vector +% +% function [AngVel] = AngVelFromTiltVel(Tilt, TiltVel) +% +% The angular velocity vector is expressed in global coordinates. +% +% Tilt ==> Tilt angles rotation +% TiltVel ==> Tilt angles velocity +% AngVel ==> Angular velocity column vector + +% Main function +function [AngVel] = AngVelFromTiltVel(Tilt, TiltVel) + + % Precompute sin and cos values + beta = Tilt(1) + Tilt(2); + cbeta = cos(beta); + sbeta = sin(beta); + calpha = cos(Tilt(3)); + salpha = sin(Tilt(3)); + + % Store tilt angles velocities in conveniently named variables + dpsi = TiltVel(1); + dgamma = TiltVel(2); + dalpha = TiltVel(3); + + % Calculate the angular velocity vector in unrotated global coordinates + AngVel = [cbeta*dalpha - salpha*sbeta*dgamma; sbeta*dalpha + salpha*cbeta*dgamma; dpsi + (1 - calpha)*dgamma]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/ComposeEuler.m b/tro2022/transform/RotationsLib/ComposeEuler.m new file mode 100644 index 0000000..c646a61 --- /dev/null +++ b/tro2022/transform/RotationsLib/ComposeEuler.m @@ -0,0 +1,40 @@ +% ComposeEuler.m - Philipp Allgeuer - 05/11/14 +% Composes multiple ZYX Euler angles rotations and returns the equivalent +% total rotation. +% +% function [Eout, Rout] = ComposeEuler(varargin) +% +% The rotations are applied right to left. That is, ComposeEuler(E3,E2,E1) +% returns the rotation given by applying E1 then E2 then E3. +% +% Eout ==> ZYX Euler angles representation of the composed rotation +% Rout ==> Rotation matrix representation of the composed rotation + +% Main function +function [Eout, Rout] = ComposeEuler(varargin) + + % Compose the required rotations + Num = length(varargin); + if Num <= 0 + Eout = [0 0 0]; + Rout = eye(3); + elseif Num == 1 + Eout = varargin{1}; + if nargout >= 2 + Rout = RotmatFromEuler(Eout); + end + else + Rout = RotmatFromEuler(varargin{1}); + for k = 2:Num + E = varargin{k}; + if numel(E) ~= 3 + warning('Ignoring an invalid rotation parameter!'); + continue; + end + Rout = Rout * RotmatFromEuler(E); + end + Eout = EulerFromRotmat(Rout); + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/ComposeFused.m b/tro2022/transform/RotationsLib/ComposeFused.m new file mode 100644 index 0000000..86b053f --- /dev/null +++ b/tro2022/transform/RotationsLib/ComposeFused.m @@ -0,0 +1,40 @@ +% ComposeFused.m - Philipp Allgeuer - 05/11/14 +% Composes multiple fused angles rotations and returns the equivalent +% total rotation. +% +% function [Fout, Rout] = ComposeFused(varargin) +% +% The rotations are applied right to left. That is, ComposeFused(F3,F2,F1) +% returns the rotation given by applying F1 then F2 then F3. +% +% Fout ==> Fused angles representation of the composed rotation +% Rout ==> Rotation matrix representation of the composed rotation + +% Main function +function [Fout, Rout] = ComposeFused(varargin) + + % Compose the required rotations + Num = length(varargin); + if Num <= 0 + Fout = [0 0 0 1]; + Rout = eye(3); + elseif Num == 1 + Fout = varargin{1}; + if nargout >= 2 + Rout = RotmatFromFused(Fout); + end + else + Rout = RotmatFromFused(varargin{1}); + for k = 2:Num + F = varargin{k}; + if numel(F) ~= 4 + warning('Ignoring an invalid rotation parameter!'); + continue; + end + Rout = Rout * RotmatFromFused(F); + end + Fout = FusedFromRotmat(Rout); + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/ComposeQuat.m b/tro2022/transform/RotationsLib/ComposeQuat.m new file mode 100644 index 0000000..f595095 --- /dev/null +++ b/tro2022/transform/RotationsLib/ComposeQuat.m @@ -0,0 +1,32 @@ +% ComposeQuat.m - Philipp Allgeuer - 05/11/14 +% Composes multiple quaternion rotations and returns the equivalent +% total rotation. +% +% function [Qout] = ComposeQuat(varargin) +% +% The rotations are applied right to left. That is, ComposeQuat(Q3,Q2,Q1) +% returns the rotation given by applying Q1 then Q2 then Q3. +% +% Qout ==> Quaternion representation of the composed rotation + +% Main function +function [Qout] = ComposeQuat(varargin) + + % Compose the required rotations + Num = length(varargin); + if Num <= 0 + Qout = [1 0 0 0]; + else + Qout = varargin{1}; + for k = 2:Num + Q = varargin{k}; + if numel(Q) ~= 4 + warning('Ignoring an invalid rotation parameter!'); + continue; + end + Qout = [Qout(1)*Q(1)-dot(Qout(2:4),Q(2:4)) Qout(1)*Q(2:4)+Q(1)*Qout(2:4)+cross(Qout(2:4),Q(2:4))]; + end + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/ComposeRotmat.m b/tro2022/transform/RotationsLib/ComposeRotmat.m new file mode 100644 index 0000000..9b89ac6 --- /dev/null +++ b/tro2022/transform/RotationsLib/ComposeRotmat.m @@ -0,0 +1,32 @@ +% ComposeRotmat.m - Philipp Allgeuer - 05/11/14 +% Composes multiple rotation matrices and returns the equivalent +% total rotation. +% +% function [Rout] = ComposeRotmat(varargin) +% +% The rotations are applied right to left. That is, ComposeRotmat(R3,R2,R1) +% returns the rotation given by applying R1 then R2 then R3. +% +% Rout ==> Rotation matrix representation of the composed rotation + +% Main function +function [Rout] = ComposeRotmat(varargin) + + % Compose the required rotations + Num = length(varargin); + if Num <= 0 + Rout = eye(3); + else + Rout = varargin{1}; + for k = 2:Num + R = varargin{k}; + if size(R,1) ~= 3 || size(R,2) ~= 3 + warning('Ignoring an invalid rotation parameter!'); + continue; + end + Rout = Rout * R; + end + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/ComposeTilt.m b/tro2022/transform/RotationsLib/ComposeTilt.m new file mode 100644 index 0000000..ba06015 --- /dev/null +++ b/tro2022/transform/RotationsLib/ComposeTilt.m @@ -0,0 +1,40 @@ +% ComposeTilt.m - Philipp Allgeuer - 05/11/14 +% Composes multiple tilt angles rotations and returns the equivalent +% total rotation. +% +% function [Tout, Rout] = ComposeTilt(varargin) +% +% The rotations are applied right to left. That is, ComposeTilt(T3,T2,T1) +% returns the rotation given by applying T1 then T2 then T3. +% +% Tout ==> Tilt angles representation of the composed rotation +% Rout ==> Rotation matrix representation of the composed rotation + +% Main function +function [Tout, Rout] = ComposeTilt(varargin) + + % Compose the required rotations + Num = length(varargin); + if Num <= 0 + Tout = [0 0 0]; + Rout = eye(3); + elseif Num == 1 + Tout = varargin{1}; + if nargout >= 2 + Rout = RotmatFromTilt(Tout); + end + else + Rout = RotmatFromTilt(varargin{1}); + for k = 2:Num + T = varargin{k}; + if numel(T) ~= 3 + warning('Ignoring an invalid rotation parameter!'); + continue; + end + Rout = Rout * RotmatFromTilt(T); + end + Tout = TiltFromRotmat(Rout); + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/EYawOfEuler.m b/tro2022/transform/RotationsLib/EYawOfEuler.m new file mode 100644 index 0000000..be51ea0 --- /dev/null +++ b/tro2022/transform/RotationsLib/EYawOfEuler.m @@ -0,0 +1,16 @@ +% EYawOfEuler.m - Philipp Allgeuer - 05/11/14 +% Calculates the ZYX yaw of a ZYX Euler angles rotation. +% +% function [EYaw] = EYawOfEuler(Euler) +% +% Euler ==> Input ZYX Euler angles rotation +% EYaw ==> ZYX yaw of the input rotation + +% Main function +function [EYaw] = EYawOfEuler(Euler) + + % Retrieve the ZYX yaw of the rotation + EYaw = Euler(1); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/EYawOfFused.m b/tro2022/transform/RotationsLib/EYawOfFused.m new file mode 100644 index 0000000..f864884 --- /dev/null +++ b/tro2022/transform/RotationsLib/EYawOfFused.m @@ -0,0 +1,43 @@ +% EYawOfFused.m - Philipp Allgeuer - 05/11/14 +% Calculates the ZYX yaw of a fused angles rotation. +% +% function [EYaw] = EYawOfFused(Fused) +% +% Fused ==> Input fused angles rotation +% EYaw ==> ZYX yaw of the input rotation + +% Main function +function [EYaw] = EYawOfFused(Fused) + + % Precalculate the sin values + sth = sin(Fused(2)); + sphi = sin(Fused(3)); + + % Calculate the cos of the tilt angle alpha + crit = sth*sth + sphi*sphi; + if crit >= 1.0 + calpha = 0.0; + else + if Fused(4) >= 0 + calpha = sqrt(1-crit); + else + calpha = -sqrt(1-crit); + end + end + + % Calculate the tilt axis angle gamma + gamma = atan2(sth,sphi); + + % Precalculate trigonometric terms involved in the rotation matrix expression + psigam = Fused(1) + gamma; + cpsigam = cos(psigam); + spsigam = sin(psigam); + cgam = cos(gamma); + sgam = sin(gamma); + A = sgam*calpha; + + % Calculate the ZYX yaw + EYaw = atan2(cgam*spsigam-A*cpsigam, cgam*cpsigam+A*spsigam); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/EYawOfQuat.m b/tro2022/transform/RotationsLib/EYawOfQuat.m new file mode 100644 index 0000000..ff62013 --- /dev/null +++ b/tro2022/transform/RotationsLib/EYawOfQuat.m @@ -0,0 +1,16 @@ +% EYawOfQuat.m - Philipp Allgeuer - 05/11/14 +% Calculates the ZYX yaw of a quaternion rotation. +% +% function [EYaw] = EYawOfQuat(Quat) +% +% Quat ==> Input quaternion rotation (assumed to have unit norm) +% EYaw ==> ZYX yaw of the input rotation + +% Main function +function [EYaw] = EYawOfQuat(Quat) + + % Calculate the ZYX yaw of the rotation + EYaw = atan2(2.0*(Quat(1)*Quat(4)+Quat(2)*Quat(3)), 1.0-2.0*(Quat(3)*Quat(3)+Quat(4)*Quat(4))); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/EYawOfRotmat.m b/tro2022/transform/RotationsLib/EYawOfRotmat.m new file mode 100644 index 0000000..1c6b755 --- /dev/null +++ b/tro2022/transform/RotationsLib/EYawOfRotmat.m @@ -0,0 +1,16 @@ +% EYawOfRotmat.m - Philipp Allgeuer - 05/11/14 +% Calculates the ZYX yaw of a rotation matrix. +% +% function [EYaw] = EYawOfRotmat(Rotmat) +% +% Rotmat ==> Input rotation matrix +% EYaw ==> ZYX yaw of the input rotation + +% Main function +function [EYaw] = EYawOfRotmat(Rotmat) + + % Calculate the ZYX yaw of the rotation + EYaw = atan2(Rotmat(2,1),Rotmat(1,1)); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/EYawOfTilt.m b/tro2022/transform/RotationsLib/EYawOfTilt.m new file mode 100644 index 0000000..4f1f5e3 --- /dev/null +++ b/tro2022/transform/RotationsLib/EYawOfTilt.m @@ -0,0 +1,25 @@ +% EYawOfTilt.m - Philipp Allgeuer - 05/11/14 +% Calculates the ZYX yaw of a tilt angles rotation. +% +% function [EYaw] = EYawOfTilt(Tilt) +% +% Tilt ==> Input tilt angles rotation +% EYaw ==> ZYX yaw of the input rotation + +% Main function +function [EYaw] = EYawOfTilt(Tilt) + + % Precalculate trigonometric terms involved in the rotation matrix expression + psigam = Tilt(1) + Tilt(2); + cpsigam = cos(psigam); + spsigam = sin(psigam); + cgam = cos(Tilt(2)); + sgam = sin(Tilt(2)); + calpha = cos(Tilt(3)); + A = sgam*calpha; + + % Calculate the ZYX yaw + EYaw = atan2(cgam*spsigam-A*cpsigam, cgam*cpsigam+A*spsigam); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/EnsureEuler.m b/tro2022/transform/RotationsLib/EnsureEuler.m new file mode 100644 index 0000000..3c9a656 --- /dev/null +++ b/tro2022/transform/RotationsLib/EnsureEuler.m @@ -0,0 +1,63 @@ +% EnsureEuler.m - Philipp Allgeuer - 05/11/14 +% Checks whether a ZYX Euler angles rotation is valid to within a certain +% tolerance and fixes it if not. +% +% function [Eout, WasBad] = EnsureEuler(Ein, Tol, Unique) +% +% A set of ZYX Euler angles (psi, theta, phi) is valid if psi is in (-pi,pi], +% theta is in [-pi/2,pi/2], and phi is in (-pi,pi]. Uniqueness is achieved by +% shifting the yaw to zero in situations of gimbal lock. +% +% Ein ==> Input ZYX Euler angles rotation +% Tol ==> Tolerance bound for the L_inf norm of the input/output difference +% Unique ==> Boolean flag whether to make the output rotation unique +% Eout ==> Output ZYX Euler angles rotation (fixed) +% WasBad ==> Boolean flag whether the input and output differ more than Tol + +% Main function +function [Eout, WasBad] = EnsureEuler(Ein, Tol, Unique) + + % Default arguments + if nargin < 2 + Tol = 1e-10; + else + Tol = abs(Tol); + end + if nargin < 3 + Unique = false; + end + + % Get the required value of pi + PI = GetPI(Ein); + + % Make a copy of the input + Eout = Ein; + + % Wrap theta to (-pi,pi] + Eout(2) = wrap(Eout(2)); + + % Collapse theta into the [-pi/2,pi/2] interval + if abs(Eout(2)) > PI/2 + Eout = [PI+Eout(1) PI-mod(Eout(2),2*PI) PI+Eout(3)]; + end + + % Make positive and negative gimbal lock representations unique + if Unique + if abs(sin(Eout(2)) - 1) <= Tol + Eout(3) = Eout(3) - Eout(1); + Eout(1) = 0; + elseif abs(sin(Eout(2)) + 1) <= Tol + Eout(3) = Eout(3) + Eout(1); + Eout(1) = 0; + end + end + + % Wrap psi and phi to (-pi,pi] + Eout(1) = wrap(Eout(1)); + Eout(3) = wrap(Eout(3)); + + % Work out whether we changed anything + WasBad = any(abs(Eout - Ein) > Tol); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/EnsureFused.m b/tro2022/transform/RotationsLib/EnsureFused.m new file mode 100644 index 0000000..aa911ee --- /dev/null +++ b/tro2022/transform/RotationsLib/EnsureFused.m @@ -0,0 +1,70 @@ +% EnsureFused.m - Philipp Allgeuer - 05/11/14 +% Checks whether a fused angles rotation is valid to within a certain +% tolerance and fixes it if not. +% +% function [Fout, WasBad] = EnsureFused(Fin, Tol, Unique) +% +% A set of fused angles (psi, theta, phi, h) is valid if psi is in (-pi,pi], +% theta is in [-pi/2,pi/2], phi is in [-pi/2,pi/2], h is in {-1,1}, and +% abs(theta) + abs(phi) <= pi/2 (equivalent to sin(theta)^2 + sin(phi)^2 <= 1). +% Uniqueness is achieved by setting h = 1 when equality holds in the above +% inequality, and setting psi = 0 when theta = phi = 0 and h = -1. +% +% Fin ==> Input fused angles rotation +% Tol ==> Tolerance bound for the L_inf norm of the input/output difference +% Unique ==> Boolean flag whether to make the output rotation unique +% Fout ==> Output fused angles rotation (fixed) +% WasBad ==> Boolean flag whether the input and output differ more than Tol + +% Main function +function [Fout, WasBad] = EnsureFused(Fin, Tol, Unique) + + % Default arguments + if nargin < 2 + Tol = 1e-10; + else + Tol = abs(Tol); + end + if nargin < 3 + Unique = false; + end + + % Get the required value of pi + PI = GetPI(Fin); + + % Make a copy of the input + Fout = Fin; + + % Wrap the angles to (-pi,pi] + Fout(1:3) = wrap(Fout(1:3)); + + % Enforce h to {-1,1} + if Fout(4) >= 0 + Fout(4) = 1; + else + Fout(4) = -1; + end + + % Check the L1 norm + L1Norm = abs(Fout(2)) + abs(Fout(3)); + if L1Norm > PI/2 + Fout(2:3) = (PI/2)*Fout(2:3)/L1Norm; + end + + % Check whether we need to make the representation unique + if Unique + SineSum = sin(Fout(2))^2 + sin(Fout(3))^2; + if SineSum >= 1 - Tol + Fout(4) = 1; + end + L1Norm = abs(Fout(2)) + abs(Fout(3)); + if L1Norm <= Tol && Fout(4) == -1 + Fout(1) = 0; + end + end + + % Work out whether we changed anything + WasBad = any(abs(Fout - Fin) > Tol); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/EnsureQuat.m b/tro2022/transform/RotationsLib/EnsureQuat.m new file mode 100644 index 0000000..6b7660c --- /dev/null +++ b/tro2022/transform/RotationsLib/EnsureQuat.m @@ -0,0 +1,52 @@ +% EnsureQuat.m - Philipp Allgeuer - 05/11/14 +% Checks whether a quaternion rotation is valid to within a certain +% tolerance and fixes it if not. +% +% function [Qout, WasBad, Norm] = EnsureQuat(Qin, Tol, Unique) +% +% The input quaternion is assumed to be of the format [w x y z]. +% the w parameter is positive. The zero quaternion gets fixed to the identity. +% +% Qin ==> Input quaternion rotation +% Tol ==> Tolerance bound for the L_inf norm of the input/output difference +% Unique ==> Boolean flag whether to make the output rotation unique +% Qout ==> Output quaternion rotation (fixed) +% WasBad ==> Boolean flag whether the input and output differ more than Tol +% Norm ==> The L_2 norm of the input quaternion + +% Main function +function [Qout, WasBad, Norm] = EnsureQuat(Qin, Tol, Unique) + + % Default arguments + if nargin < 2 + Tol = 1e-10; + else + Tol = abs(Tol); + end + if nargin < 3 + Unique = false; + end + + % Make a copy of the input + Qout = Qin; + + % Renormalise the quaternion + Norm = norm(Qout); + if Norm <= 0 + Qout = [1 0 0 0]; % Zero quaternion gets fixed to the identity quaternion + else + Qout = Qout / Norm; % Scale to unit norm + end + + % Make the quaternion unique + if Unique + if Qout(1) < 0 + Qout = -Qout; + end + end + + % Work out whether we changed anything + WasBad = any(abs(Qout - Qin) > Tol); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/EnsureRotmat.m b/tro2022/transform/RotationsLib/EnsureRotmat.m new file mode 100644 index 0000000..fcadb6c --- /dev/null +++ b/tro2022/transform/RotationsLib/EnsureRotmat.m @@ -0,0 +1,43 @@ +% EnsureRotmat.m - Philipp Allgeuer - 05/11/14 +% Checks whether a rotation matrix is valid to within a certain +% tolerance and fixes it if not. +% +% function [Rout, WasBad] = EnsureRotmat(Rin, Tol) +% +% A 3x3 rotation matrix is valid if R'*R = I and det(R) = 1. Rotation matrices +% that are valid are always automatically unique. +% +% Rin ==> Input rotation matrix +% Tol ==> Tolerance bound for the L_inf norm of the input/output difference +% Rout ==> Output rotation matrix (fixed) +% WasBad ==> Boolean flag whether the input and output differ more than Tol + +% Main function +function [Rout, WasBad] = EnsureRotmat(Rin, Tol) + + % Default arguments + if nargin < 2 + Tol = 1e-10; + else + Tol = abs(Tol); + end + + % Make a copy of the input + Rout = Rin; + + % Orthogonalise the rotation matrix + Rout = Rout / sqrtm(Rout' * Rout); % Calculate the 'closest' valid rotation matrix to the given one + if ~isreal(Rout) + Rout = eye(3); + end + + % Filter out invalid left hand coordinate systems + if det(Rout) < 0 + Rout = eye(3); + end + + % Work out whether we changed anything + WasBad = any(any(abs(Rout - Rin) > Tol)); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/EnsureTilt.m b/tro2022/transform/RotationsLib/EnsureTilt.m new file mode 100644 index 0000000..2b8a1a2 --- /dev/null +++ b/tro2022/transform/RotationsLib/EnsureTilt.m @@ -0,0 +1,63 @@ +% EnsureTilt.m - Philipp Allgeuer - 05/11/14 +% Checks whether a tilt angles rotation is valid to within a certain +% tolerance and fixes it if not. +% +% function [Tout, WasBad] = EnsureTilt(Tin, Tol, Unique) +% +% A set of tilt angles (psi, gamma, alpha) is valid if psi is in (-pi,pi], +% gamma is in (-pi,pi] and alpha is in [0,pi]. If alpha is in the range (-pi,0), +% then the equivalence alpha <-> -alpha, gamma <-> gamma+pi applies. Uniqueness +% is achieved by setting psi = 0 when alpha = pi, and setting gamma = 0 when +% alpha = 0, pi. +% +% Tin ==> Input tilt angles rotation +% Tol ==> Tolerance bound for the L_inf norm of the input/output difference +% Unique ==> Boolean flag whether to make the output rotation unique +% Tout ==> Output tilt angles rotation (fixed) +% WasBad ==> Boolean flag whether the input and output differ more than Tol + +% Main function +function [Tout, WasBad] = EnsureTilt(Tin, Tol, Unique) + + % Default arguments + if nargin < 2 + Tol = 1e-10; + else + Tol = abs(Tol); + end + if nargin < 3 + Unique = false; + end + + % Get the required value of pi + PI = GetPI(Tin); + + % Make a copy of the input + Tout = Tin; + + % Wrap the angles to (-pi,pi] + Tout = wrap(Tout); + + % Handle case of negative alpha + if Tout(3) < 0 + Tout(2) = wrap(Tout(2) + PI); + Tout(3) = -Tout(3); + end + + % Check whether we need to make the representation unique + if Unique + Near0 = (abs(cos(Tout(3)) - 1) <= Tol); + Near180 = (abs(cos(Tout(3)) + 1) <= Tol); + if Near0 || Near180 + Tout(2) = 0; + end + if Near180 + Tout(1) = 0; + end + end + + % Work out whether we changed anything + WasBad = any(abs(Tout - Tin) > Tol); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/EulerEqual.m b/tro2022/transform/RotationsLib/EulerEqual.m new file mode 100644 index 0000000..9dc78d1 --- /dev/null +++ b/tro2022/transform/RotationsLib/EulerEqual.m @@ -0,0 +1,67 @@ +% EulerEqual.m - Philipp Allgeuer - 05/11/14 +% Returns whether two ZYX Euler angles rotations represent the same rotation. +% +% function [Equal, Err] = EulerEqual(E, F, Tol) +% +% The following fundamental ZYX Euler angle equivalences hold: +% 1) (psi, th, phi) == (psi+2*pi*k1, th+2*pi*k2, phi+2*pi*k3) for integers k1, k2, k3 +% 2) (psi, th, phi) == (pi+psi, pi-th, pi+phi) +% 3) (psia, th, phia) == (psib, th, phib) IF sin(th) = 1 and psia - phia = psib - phib, +% OR sin(th) = -1 and psia + phia = psib + phib +% sin(th) = 1 resolves to th = pi/2 + 2*pi*k, and sin(th) = -1 resolves +% to th = -pi/2 + 2*pi*k. +% Refer to EnsureEuler for more information on the unique form of a ZYX Euler +% angles rotation. +% +% E ==> First ZYX Euler angles rotation to compare +% F ==> Second ZYX Euler angles rotation to compare +% Tol ==> Allowed tolerance between E and F for which equality is still asserted +% Equal ==> Boolean flag whether E equals F to the given L_inf norm tolerance +% Err ==> The quantified error between E and F (0 if exactly equal) + +% Main function +function [Equal, Err] = EulerEqual(E, F, Tol) + + % Default tolerance + if nargin < 3 + Tol = 1e-10; + else + Tol = abs(Tol); + end + + % Get the required value of pi + PI = GetPI(E); + + % Convert both Euler angles into their unique representations + E = EnsureEuler(E, Tol, true); + F = EnsureEuler(F, Tol, true); + + % Handle angle wrapping issues + if abs(E(1)-F(1)) > PI + if E(1) > F(1) + F(1) = F(1) + 2*PI; + else + E(1) = E(1) + 2*PI; + end + end + if abs(E(3)-F(3)) > PI + if E(3) > F(3) + F(3) = F(3) + 2*PI; + else + E(3) = E(3) + 2*PI; + end + end + + % Construct the vectors to compare + % The pitch suffers from the numerical insensitivity of asin, so the sin thereof is checked + EComp = [E(1) sin(E(2)) E(3)]; + FComp = [F(1) sin(F(2)) F(3)]; + + % Calculate the deviation between the two rotations + Err = max(abs(EComp-FComp)); + + % Check whether the two rotations are within tolerance + Equal = (Err <= Tol); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/EulerFromAxis.m b/tro2022/transform/RotationsLib/EulerFromAxis.m new file mode 100644 index 0000000..f5955d9 --- /dev/null +++ b/tro2022/transform/RotationsLib/EulerFromAxis.m @@ -0,0 +1,73 @@ +% EulerFromAxis.m - Philipp Allgeuer - 05/11/14 +% Constructs a ZYX Euler angles rotation based on an axis-angle specification. +% +% function [Euler, Quat] = EulerFromAxis(Axis, Angle) +% +% Axis ==> The vector corresponding to the axis of rotation (magnitude is unimportant) +% either given as the vector [ux uy uz] or one of {'x', 'y', 'z'} +% Angle ==> The angle of the rotation given by the right hand rule +% Euler ==> Equivalent ZYX Euler angles rotation +% Quat ==> Equivalent quaternion rotation + +% Main function +function [Euler, Quat] = EulerFromAxis(Axis, Angle) + + % Handle case of missing arguments + if nargin < 2 + Euler = [0 0 0]; + Quat = [1 0 0 0]; + return; + end + + % Get the required value of pi + PI = GetPI(Angle); + + % Wrap the rotation angle to (-pi,pi] + Angle = wrap(Angle); + + % Precompute the required sin and cos terms + hcang = cos(Angle/2); + hsang = sin(Angle/2); + + % Handle case of standard axis rotations + if strcmpi(Axis, 'x') + Euler = [0 0 Angle]; + Quat = [hcang hsang 0 0]; + return; + elseif strcmpi(Axis, 'y') + if abs(Angle) <= PI/2 + Euler = [0 Angle 0]; + elseif Angle >= PI/2 + Euler = [PI PI-Angle PI]; + else + Euler = [PI -PI-Angle PI]; + end + Quat = [hcang 0 hsang 0]; + return; + elseif strcmpi(Axis, 'z') + Euler = [Angle 0 0]; + Quat = [hcang 0 0 hsang]; + return; + end + + % Axis error checking + if ~(isnumeric(Axis) && isvector(Axis) && length(Axis) == 3) + error('Axis must be a 3-vector, or one of {''x'',''y'',''z''}.'); + end + + % Normalise the axis vector + AxisNorm = norm(Axis); + if AxisNorm <= 0 + Euler = [0 0 0]; % Return the identity rotation if Axis is the zero vector + Quat = [1 0 0 0]; + return; + else + Axis = Axis/AxisNorm; + end + + % Construct the required rotation + Quat = [hcang hsang*Axis(:)']; + Euler = EulerFromQuat(Quat); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/EulerFromFYawBzG.m b/tro2022/transform/RotationsLib/EulerFromFYawBzG.m new file mode 100644 index 0000000..e15d007 --- /dev/null +++ b/tro2022/transform/RotationsLib/EulerFromFYawBzG.m @@ -0,0 +1,32 @@ +% EulerFromFYawBzG - Philipp Allgeuer - 20/10/16 +% Calculates the ZYX Euler angles (EGB) with a given fused yaw and global z-axis in body-fixed coordinates (BzG). +% +% function [EGB] = EulerFromFYawBzG(FYaw, BzG) +% +% It is assumed that BzG is a unit vector! + +% Main function +function [EGB] = EulerFromFYawBzG(FYaw, BzG) + + % Calculate the Euler pitch + stheta = max(min(-BzG(1),1.0),-1.0); % Note: If BzG is a unit vector then this should only trim at most a few eps... + theta = asin(stheta); + + % Calculate the Euler roll + phi = atan2(BzG(2),BzG(3)); + + % Calculate the ZYX yaw + if stheta == 0 && BzG(2) == 0 + EYaw = FYaw; + else + cphi = cos(phi); + sphi = sin(phi); + EYaw = FYaw + atan2(sphi,stheta*cphi) - atan2(BzG(2),stheta); + end + EYaw = wrap(EYaw); + + % Construct the output Euler angles + EGB = [EYaw theta phi]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/EulerFromFYawGzB.m b/tro2022/transform/RotationsLib/EulerFromFYawGzB.m new file mode 100644 index 0000000..bccf1aa --- /dev/null +++ b/tro2022/transform/RotationsLib/EulerFromFYawGzB.m @@ -0,0 +1,38 @@ +% EulerFromFYawGzB - Philipp Allgeuer - 26/10/16 +% Calculates the ZYX Euler angles (EGB) with a given fused yaw and body-fixed z-axis in global coordinates (GzB). +% +% function [EGB] = EulerFromFYawGzB(FYaw, GzB) +% +% It is assumed that GzB is a unit vector! + +% Main function +function [EGB] = EulerFromFYawGzB(FYaw, GzB) + + % Precalculate trigonometric terms + cfyaw = cos(FYaw); + sfyaw = sin(FYaw); + + % Calculate the Euler pitch + stheta = cfyaw*GzB(1) + sfyaw*GzB(2); + stheta = max(min(stheta,1.0),-1.0); % Note: If GzB is a unit vector then this should only trim at most a few eps... + theta = asin(stheta); + + % Calculate the Euler roll + sfphi = sfyaw*GzB(1) - cfyaw*GzB(2); + phi = atan2(sfphi, GzB(3)); + + % Calculate the ZYX yaw + if stheta == 0 && sfphi == 0 + EYaw = FYaw; + else + cphi = cos(phi); + sphi = sin(phi); + EYaw = FYaw + atan2(sphi,stheta*cphi) - atan2(sfphi,stheta); + end + EYaw = wrap(EYaw); + + % Construct the output Euler angles + EGB = [EYaw theta phi]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/EulerFromFused.m b/tro2022/transform/RotationsLib/EulerFromFused.m new file mode 100644 index 0000000..f9760db --- /dev/null +++ b/tro2022/transform/RotationsLib/EulerFromFused.m @@ -0,0 +1,54 @@ +% EulerFromFused.m - Philipp Allgeuer - 05/11/14 +% Converts a fused angles rotation to the corresponding ZYX Euler angles representation. +% +% function [Euler, Tilt] = EulerFromFused(Fused) +% +% The output ranges are: +% Yaw: psi is in (-pi,pi] +% Pitch: theta is in [-pi/2,pi/2] +% Roll: phi is in (-pi,pi] +% +% Fused ==> Input fused angles rotation +% Euler ==> Equivalent ZYX Euler angles rotation +% Tilt ==> Equivalent tilt angles rotation + +% Main function +function [Euler, Tilt] = EulerFromFused(Fused) + + % Precalculate the sin values + sth = sin(Fused(2)); + sphi = sin(Fused(3)); + + % Calculate the cos of the tilt angle alpha + crit = sth*sth + sphi*sphi; + if crit >= 1.0 + calpha = 0.0; + else + if Fused(4) >= 0 + calpha = sqrt(1-crit); + else + calpha = -sqrt(1-crit); + end + end + + % Calculate the tilt axis angle gamma + gamma = atan2(sth,sphi); + + % Precalculate the required trigonometric terms + cgam = cos(gamma); % Equals sphi/salpha + sgam = sin(gamma); % Equals sth/salpha + psigam = Fused(1) + gamma; + cpsigam = cos(psigam); + spsigam = sin(psigam); + A = sgam*calpha; + + % Calculate the required Euler angles representation + Euler = [atan2(cgam*spsigam-A*cpsigam,cgam*cpsigam+A*spsigam) Fused(2) atan2(sphi,calpha)]; % Note: Fused pitch is equivalent to ZYX Euler pitch! + + % Calculate the required tilt angles representation + if nargout >= 2 + Tilt = [Fused(1) gamma acos(calpha)]; + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/EulerFromQuat.m b/tro2022/transform/RotationsLib/EulerFromQuat.m new file mode 100644 index 0000000..a8ad451 --- /dev/null +++ b/tro2022/transform/RotationsLib/EulerFromQuat.m @@ -0,0 +1,25 @@ +% EulerFromQuat.m - Philipp Allgeuer - 05/11/14 +% Converts a quaternion rotation to the corresponding ZYX Euler angles representation. +% +% function [Euler] = EulerFromQuat(Quat) +% +% The output ranges are: +% Yaw: psi is in (-pi,pi] +% Pitch: theta is in [-pi/2,pi/2] +% Roll: phi is in (-pi,pi] +% +% Quat ==> Input quaternion rotation (assumed to have unit norm) +% Euler ==> Equivalent ZYX Euler angles rotation + +% Main function +function [Euler] = EulerFromQuat(Quat) + + % Extract the sin of the theta angle + sth = max(min(2.0*(Quat(1)*Quat(3)-Quat(4)*Quat(2)),1.0),-1.0); % Note: If Quat is valid then this should only trim at most a few eps... + + % Calculate the required Euler angles + Q3sq = Quat(3)*Quat(3); + Euler = [atan2(2.0*(Quat(1)*Quat(4)+Quat(2)*Quat(3)),1.0-2.0*(Q3sq+Quat(4)*Quat(4))) asin(sth) atan2(2.0*(Quat(1)*Quat(2)+Quat(3)*Quat(4)),1.0-2.0*(Quat(2)*Quat(2)+Q3sq))]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/EulerFromRotmat.m b/tro2022/transform/RotationsLib/EulerFromRotmat.m new file mode 100644 index 0000000..0f21c83 --- /dev/null +++ b/tro2022/transform/RotationsLib/EulerFromRotmat.m @@ -0,0 +1,24 @@ +% EulerFromRotmat.m - Philipp Allgeuer - 05/11/14 +% Converts a rotation matrix to the corresponding ZYX Euler angles representation. +% +% function [Euler] = EulerFromRotmat(Rotmat) +% +% The output ranges are: +% Yaw: psi is in (-pi,pi] +% Pitch: theta is in [-pi/2,pi/2] +% Roll: phi is in (-pi,pi] +% +% Rotmat ==> Input rotation matrix +% Euler ==> Equivalent ZYX Euler angles rotation + +% Main function +function [Euler] = EulerFromRotmat(Rotmat) + + % Extract the sin of the theta angle + sth = max(min(-Rotmat(3,1),1.0),-1.0); % Note: If Rotmat is valid then this should only trim at most a few eps... + + % Calculate the required Euler angles + Euler = [atan2(Rotmat(2,1),Rotmat(1,1)) asin(sth) atan2(Rotmat(3,2),Rotmat(3,3))]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/EulerFromTilt.m b/tro2022/transform/RotationsLib/EulerFromTilt.m new file mode 100644 index 0000000..44f0adb --- /dev/null +++ b/tro2022/transform/RotationsLib/EulerFromTilt.m @@ -0,0 +1,35 @@ +% EulerFromTilt.m - Philipp Allgeuer - 05/11/14 +% Converts a tilt angles rotation to the corresponding ZYX Euler angles representation. +% +% function [Euler] = EulerFromTilt(Tilt) +% +% The output ranges are: +% Yaw: psi is in (-pi,pi] +% Pitch: theta is in [-pi/2,pi/2] +% Roll: phi is in (-pi,pi] +% +% Tilt ==> Input tilt angles rotation +% Euler ==> Equivalent ZYX Euler angles rotation + +% Main function +function [Euler] = EulerFromTilt(Tilt) + + % Precalculate the required trigonometric terms + cgam = cos(Tilt(2)); + sgam = sin(Tilt(2)); + calpha = cos(Tilt(3)); + salpha = sin(Tilt(3)); + psigam = Tilt(1) + Tilt(2); + cpsigam = cos(psigam); + spsigam = sin(psigam); + + % Precalculate additional required terms + sphi = cgam*salpha; + stheta = sgam*salpha; % No need to manually coerce to [-1,1] due to construction + A = sgam*calpha; + + % Calculate the required Euler angles representation + Euler = [atan2(cgam*spsigam-A*cpsigam,cgam*cpsigam+A*spsigam) asin(stheta) atan2(sphi,calpha)]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/EulerFromZVec.m b/tro2022/transform/RotationsLib/EulerFromZVec.m new file mode 100644 index 0000000..44746a6 --- /dev/null +++ b/tro2022/transform/RotationsLib/EulerFromZVec.m @@ -0,0 +1,21 @@ +% EulerFromZVec.m - Philipp Allgeuer - 21/10/16 +% Converts a z-vector to the corresponding zero ZYX yaw Euler angles representation. +% +% function [Euler] = EulerFromZVec(ZVec) +% +% It is assumed that ZVec is a unit vector! +% +% ZVec ==> Input z-vector +% Euler ==> Corresponding zero ZYX yaw Euler angles rotation + +% Main function +function [Euler] = EulerFromZVec(ZVec) + + % Extract the sin of the theta angle + sth = max(min(-ZVec(1),1.0),-1.0); % Note: If ZVec is a unit vector then this should only trim at most a few eps... + + % Calculate the required Euler angles + Euler = [0 asin(sth) atan2(ZVec(2),ZVec(3))]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/EulerIdentity.m b/tro2022/transform/RotationsLib/EulerIdentity.m new file mode 100644 index 0000000..349bcba --- /dev/null +++ b/tro2022/transform/RotationsLib/EulerIdentity.m @@ -0,0 +1,15 @@ +% EulerIdentity.m - Philipp Allgeuer - 05/11/14 +% Returns the identity ZYX Euler angles rotation. +% +% function [EI] = EulerIdentity() +% +% EI ==> Identity rotation in the ZYX Euler angles representation + +% Main function +function [EI] = EulerIdentity() + + % Return the required identity rotation + EI = [0 0 0]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/EulerInv.m b/tro2022/transform/RotationsLib/EulerInv.m new file mode 100644 index 0000000..fe3132c --- /dev/null +++ b/tro2022/transform/RotationsLib/EulerInv.m @@ -0,0 +1,27 @@ +% EulerInv.m - Philipp Allgeuer - 05/11/14 +% Calculates the inverse of a ZYX Euler angles rotation. +% +% function [Einv] = EulerInv(E) +% +% E ==> Input ZYX Euler angles rotation +% Einv ==> Inverse ZYX Euler angles rotation + +% Main function +function [Einv] = EulerInv(E) + + % Precalculate the required sin and cos values + cpsi = cos(E(1)); + cth = cos(E(2)); + cphi = cos(E(3)); + spsi = sin(E(1)); + sth = sin(E(2)); + sphi = sin(E(3)); + + % Extract the sin of the theta angle + newsth = max(min(-(cpsi*sth*cphi+spsi*sphi),1.0),-1.0); % Note: This should only trim at most a few eps... + + % Calculate the required inverse Euler angles representation + Einv = [atan2(cpsi*sth*sphi-spsi*cphi,cpsi*cth) asin(newsth) atan2(spsi*sth*cphi-cpsi*sphi,cth*cphi)]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/EulerNoEYaw.m b/tro2022/transform/RotationsLib/EulerNoEYaw.m new file mode 100644 index 0000000..c964709 --- /dev/null +++ b/tro2022/transform/RotationsLib/EulerNoEYaw.m @@ -0,0 +1,25 @@ +% EulerNoEYaw.m - Philipp Allgeuer - 05/11/14 +% Removes the ZYX yaw component of a ZYX Euler angles rotation. +% +% function [Eout, EYaw, EEYaw] = EulerNoEYaw(Ein) +% +% Ein ==> Input ZYX Euler angles rotation +% Eout ==> Output ZYX Euler angles rotation with no ZYX yaw +% EYaw ==> ZYX yaw of the input rotation +% EEYaw ==> ZYX Euler angles representation of the ZYX yaw component +% of the input rotation + +% Main function +function [Eout, EYaw, EEYaw] = EulerNoEYaw(Ein) + + % Calculate the ZYX yaw of the input + EYaw = Ein(1); + + % Construct the ZYX yaw component of the rotation + EEYaw = [EYaw 0 0]; + + % Remove the ZYX yaw component of the rotation + Eout = [0 Ein(2:3)]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/EulerNoFYaw.m b/tro2022/transform/RotationsLib/EulerNoFYaw.m new file mode 100644 index 0000000..03ac5f4 --- /dev/null +++ b/tro2022/transform/RotationsLib/EulerNoFYaw.m @@ -0,0 +1,25 @@ +% EulerNoFYaw.m - Philipp Allgeuer - 05/11/14 +% Removes the fused yaw component of a ZYX Euler angles rotation. +% +% function [Eout, FYaw, EFYaw] = EulerNoFYaw(Ein) +% +% Ein ==> Input ZYX Euler angles rotation +% Eout ==> Output ZYX Euler angles rotation with zero fused yaw +% FYaw ==> Fused yaw of the input rotation +% EFYaw ==> ZYX Euler angles representation of the fused yaw component +% of the input rotation + +% Main function +function [Eout, FYaw, EFYaw] = EulerNoFYaw(Ein) + + % Calculate the fused yaw of the input + FYaw = FYawOfEuler(Ein); + + % Construct the fused yaw component of the rotation + EFYaw = [FYaw 0 0]; + + % Remove the fused yaw component of the rotation + Eout = [Ein(1)-FYaw Ein(2:3)]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/EulerRotVec.m b/tro2022/transform/RotationsLib/EulerRotVec.m new file mode 100644 index 0000000..f08d537 --- /dev/null +++ b/tro2022/transform/RotationsLib/EulerRotVec.m @@ -0,0 +1,18 @@ +% EulerRotVec.m - Philipp Allgeuer - 05/11/14 +% Rotate a vector by a given ZYX Euler angles rotation. +% +% function [Vout] = EulerRotVec(Euler, Vin) +% +% Euler ==> ZYX Euler angles rotation to apply to Vin +% Vin ==> Input vector to rotate +% Vout ==> Rotated output vector + +% Main function +function [Vout] = EulerRotVec(Euler, Vin) + + % Rotate the vector as required + Vout = zeros(size(Vin)); + Vout(:) = RotmatFromEuler(Euler) * Vin(:); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/FYawOfEuler.m b/tro2022/transform/RotationsLib/FYawOfEuler.m new file mode 100644 index 0000000..07a3153 --- /dev/null +++ b/tro2022/transform/RotationsLib/FYawOfEuler.m @@ -0,0 +1,20 @@ +% FYawOfEuler.m - Philipp Allgeuer - 05/11/14 +% Calculates the fused yaw of a ZYX Euler angles rotation. +% +% function [FYaw, Rotmat] = FYawOfEuler(Euler) +% +% Euler ==> Input ZYX Euler angles rotation +% FYaw ==> Fused yaw of the input rotation +% Rotmat ==> Equivalent rotation matrix + +% Main function +function [FYaw, Rotmat] = FYawOfEuler(Euler) + + % Calculation of the fused yaw in a numerically stable manner requires the complete rotation matrix representation + Rotmat = RotmatFromEuler(Euler); + + % Calculate the fused yaw + FYaw = FYawOfRotmat(Rotmat); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/FYawOfFused.m b/tro2022/transform/RotationsLib/FYawOfFused.m new file mode 100644 index 0000000..01ba669 --- /dev/null +++ b/tro2022/transform/RotationsLib/FYawOfFused.m @@ -0,0 +1,16 @@ +% FYawOfFused.m - Philipp Allgeuer - 05/11/14 +% Calculates the fused yaw of a fused angles rotation. +% +% function [FYaw] = FYawOfFused(Fused) +% +% Fused ==> Input fused angles rotation +% FYaw ==> Fused yaw of the input rotation + +% Main function +function [FYaw] = FYawOfFused(Fused) + + % Retrieve the fused yaw of the rotation + FYaw = Fused(1); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/FYawOfQuat.m b/tro2022/transform/RotationsLib/FYawOfQuat.m new file mode 100644 index 0000000..5060b2a --- /dev/null +++ b/tro2022/transform/RotationsLib/FYawOfQuat.m @@ -0,0 +1,17 @@ +% FYawOfQuat.m - Philipp Allgeuer - 05/11/14 +% Calculates the fused yaw of a quaternion rotation. +% +% function [FYaw] = FYawOfQuat(Quat) +% +% Quat ==> Input quaternion rotation (assumed to have unit norm) +% FYaw ==> Fused yaw of the input rotation + +% Main function +function [FYaw] = FYawOfQuat(Quat) + + % Calculate the fused yaw of the rotation + FYaw = 2.0*atan2(Quat(4),Quat(1)); + FYaw = wrap(FYaw); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/FYawOfRotmat.m b/tro2022/transform/RotationsLib/FYawOfRotmat.m new file mode 100644 index 0000000..09add1d --- /dev/null +++ b/tro2022/transform/RotationsLib/FYawOfRotmat.m @@ -0,0 +1,26 @@ +% FYawOfRotmat.m - Philipp Allgeuer - 05/11/14 +% Calculates the fused yaw of a rotation matrix. +% +% function [FYaw] = FYawOfRotmat(Rotmat) +% +% Rotmat ==> Input rotation matrix +% FYaw ==> Fused yaw of the input rotation + +% Main function +function [FYaw] = FYawOfRotmat(Rotmat) + + % Calculate the fused yaw of the rotation + t = Rotmat(1,1) + Rotmat(2,2) + Rotmat(3,3); + if t >= 0.0 + FYaw = 2*atan2(Rotmat(2,1)-Rotmat(1,2), 1+t); + elseif Rotmat(3,3) >= Rotmat(2,2) && Rotmat(3,3) >= Rotmat(1,1) + FYaw = 2*atan2(1-Rotmat(1,1)-Rotmat(2,2)+Rotmat(3,3), Rotmat(2,1)-Rotmat(1,2)); + elseif Rotmat(2,2) >= Rotmat(1,1) + FYaw = 2*atan2(Rotmat(3,2)+Rotmat(2,3), Rotmat(1,3)-Rotmat(3,1)); + else + FYaw = 2*atan2(Rotmat(1,3)+Rotmat(3,1), Rotmat(3,2)-Rotmat(2,3)); + end + FYaw = wrap(FYaw); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/FYawOfTilt.m b/tro2022/transform/RotationsLib/FYawOfTilt.m new file mode 100644 index 0000000..5912469 --- /dev/null +++ b/tro2022/transform/RotationsLib/FYawOfTilt.m @@ -0,0 +1,16 @@ +% FYawOfTilt.m - Philipp Allgeuer - 05/11/14 +% Calculates the fused yaw of a tilt angles rotation. +% +% function [FYaw] = FYawOfTilt(Tilt) +% +% Tilt ==> Input tilt angles rotation +% FYaw ==> Fused yaw of the input rotation + +% Main function +function [FYaw] = FYawOfTilt(Tilt) + + % Retrieve the fused yaw of the rotation + FYaw = Tilt(1); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/FusedEqual.m b/tro2022/transform/RotationsLib/FusedEqual.m new file mode 100644 index 0000000..b28b9b8 --- /dev/null +++ b/tro2022/transform/RotationsLib/FusedEqual.m @@ -0,0 +1,53 @@ +% FusedEqual.m - Philipp Allgeuer - 05/11/14 +% Returns whether two fused angles rotations represent the same rotation. +% +% function [Equal, Err] = FusedEqual(F, G, Tol) +% +% Refer to EnsureFused for more information on the unique form of a fused +% angles rotation. +% +% F ==> First fused angles rotation to compare +% G ==> Second fused angles rotation to compare +% Tol ==> Allowed tolerance between F and G for which equality is still asserted +% Equal ==> Boolean flag whether F equals G to the given L_inf norm tolerance +% Err ==> The quantified error between F and G (0 if exactly equal) + +% Main function +function [Equal, Err] = FusedEqual(F, G, Tol) + + % Default tolerance + if nargin < 3 + Tol = 1e-10; + else + Tol = abs(Tol); + end + + % Get the required value of pi + PI = GetPI(F); + + % Convert both fused angles into their unique representations + F = EnsureFused(F, Tol, true); + G = EnsureFused(G, Tol, true); + + % Handle angle wrapping issues + if abs(F(1)-G(1)) > PI + if F(1) > G(1) + G(1) = G(1) + 2*PI; + else + F(1) = F(1) + 2*PI; + end + end + + % Construct the vectors to compare + % The fused pitch and roll suffer from the numerical insensitivity of asin, so the sin thereof is checked + FComp = [F(1) sin(F(2)) sin(F(3)) F(4)]; + GComp = [G(1) sin(G(2)) sin(G(3)) G(4)]; + + % Calculate the deviation between the two rotations + Err = max(abs(FComp-GComp)); + + % Check whether the two rotations are within tolerance + Equal = (Err <= Tol); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/FusedFromAxis.m b/tro2022/transform/RotationsLib/FusedFromAxis.m new file mode 100644 index 0000000..1da83c3 --- /dev/null +++ b/tro2022/transform/RotationsLib/FusedFromAxis.m @@ -0,0 +1,79 @@ +% FusedFromAxis.m - Philipp Allgeuer - 05/11/14 +% Constructs a fused angles rotation based on an axis-angle specification. +% +% function [Fused, Quat] = FusedFromAxis(Axis, Angle) +% +% Axis ==> The vector corresponding to the axis of rotation (magnitude is unimportant) +% either given as the vector [ux uy uz] or one of {'x', 'y', 'z'} +% Angle ==> The angle of the rotation given by the right hand rule +% Fused ==> Equivalent fused angles rotation +% Quat ==> Equivalent quaternion rotation + +% Main function +function [Fused, Quat] = FusedFromAxis(Axis, Angle) + + % Handle case of missing arguments + if nargin < 2 + Fused = [0 0 0 1]; + Quat = [1 0 0 0]; + return; + end + + % Get the required value of pi + PI = GetPI(Angle); + + % Wrap the rotation angle to (-pi,pi] + Angle = wrap(Angle); + + % Precompute the required sin and cos terms + hcang = cos(Angle/2); + hsang = sin(Angle/2); + + % Handle case of standard axis rotations + if strcmpi(Axis, 'x') + if abs(Angle) <= PI/2 + Fused = [0 0 Angle 1]; + elseif Angle >= PI/2 + Fused = [0 0 PI-Angle -1]; + else + Fused = [0 0 -PI-Angle -1]; + end + Quat = [hcang hsang 0 0]; + return; + elseif strcmpi(Axis, 'y') + if abs(Angle) <= PI/2 + Fused = [0 Angle 0 1]; + elseif Angle >= PI/2 + Fused = [0 PI-Angle 0 -1]; + else + Fused = [0 -PI-Angle 0 -1]; + end + Quat = [hcang 0 hsang 0]; + return; + elseif strcmpi(Axis, 'z') + Fused = [Angle 0 0 1]; + Quat = [hcang 0 0 hsang]; + return; + end + + % Axis error checking + if ~(isnumeric(Axis) && isvector(Axis) && length(Axis) == 3) + error('Axis must be a 3-vector, or one of {''x'',''y'',''z''}.'); + end + + % Normalise the axis vector + AxisNorm = norm(Axis); + if AxisNorm <= 0 + Fused = [0 0 0 1]; % Return the identity rotation if Axis is the zero vector + Quat = [1 0 0 0]; + return; + else + Axis = Axis/AxisNorm; + end + + % Construct the required rotation + Quat = [hcang hsang*Axis(:)']; + Fused = FusedFromQuat(Quat); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/FusedFromEuler.m b/tro2022/transform/RotationsLib/FusedFromEuler.m new file mode 100644 index 0000000..c9a11b6 --- /dev/null +++ b/tro2022/transform/RotationsLib/FusedFromEuler.m @@ -0,0 +1,52 @@ +% FusedFromEuler.m - Philipp Allgeuer - 05/11/14 +% Converts a ZYX Euler angles rotation to the corresponding fused angles representation. +% +% function [Fused, Rotmat, Tilt] = FusedFromEuler(Euler) +% +% The output ranges are: +% Fused yaw: psi is in (-pi,pi] +% Fused pitch: theta is in [-pi/2,pi/2] +% Fused roll: phi is in [-pi/2,pi/2] +% Hemisphere: h is in {-1,1} +% +% Euler ==> Input ZYX Euler angles rotation +% Fused ==> Equivalent fused angles rotation +% Rotmat ==> Equivalent rotation matrix +% Tilt ==> Equivalent tilt angles rotation + +% Main function +function [Fused, Rotmat, Tilt] = FusedFromEuler(Euler) + + % Calculation of the fused yaw in a numerically stable manner requires the complete rotation matrix representation + Rotmat = RotmatFromEuler(Euler); + + % Calculate the fused yaw + psi = FYawOfRotmat(Rotmat); + + % Calculate the fused pitch + theta = Euler(2); % ZYX Euler pitch is equivalent to fused pitch! + + % Calculate the fused roll + sphi = max(min(Rotmat(3,2),1.0),-1.0); % Note: If Rotmat is valid then this should only trim at most a few eps... + phi = asin(sphi); + + % See which hemisphere we're in + if Rotmat(3,3) >= 0 + h = 1; + else + h = -1; + end + + % Construct the output fused angles + Fused = [psi theta phi h]; + + % Construct the output tilt angles + if nargout >= 3 + gamma = atan2(-Rotmat(3,1),Rotmat(3,2)); + calpha = max(min(Rotmat(3,3),1.0),-1.0); % Note: If Rotmat is valid then this should only trim at most a few eps... + alpha = acos(calpha); + Tilt = [psi gamma alpha]; + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/FusedFromFYawBzG.m b/tro2022/transform/RotationsLib/FusedFromFYawBzG.m new file mode 100644 index 0000000..235c9d0 --- /dev/null +++ b/tro2022/transform/RotationsLib/FusedFromFYawBzG.m @@ -0,0 +1,33 @@ +% FusedFromFYawBzG - Philipp Allgeuer - 20/10/16 +% Calculates the fused angles (FGB) with a given fused yaw and global z-axis in body-fixed coordinates (BzG). +% +% function [FGB] = FusedFromFYawBzG(FYaw, BzG) +% +% It is assumed that BzG is a unit vector! + +% Main function +function [FGB] = FusedFromFYawBzG(FYaw, BzG) + + % Wrap the fused yaw to the desired range + FYaw = wrap(FYaw); + + % Calculate the fused pitch + stheta = max(min(-BzG(1),1.0),-1.0); % Note: If BzG is a unit vector then this should only trim at most a few eps... + theta = asin(stheta); + + % Calculate the fused roll + sphi = max(min(BzG(2),1.0),-1.0); % Note: If BzG is a unit vector then this should only trim at most a few eps... + phi = asin(sphi); + + % See which hemisphere we're in + if BzG(3) >= 0 + h = 1; + else + h = -1; + end + + % Construct the output fused angles + FGB = [FYaw theta phi h]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/FusedFromFYawGzB.m b/tro2022/transform/RotationsLib/FusedFromFYawGzB.m new file mode 100644 index 0000000..957bcad --- /dev/null +++ b/tro2022/transform/RotationsLib/FusedFromFYawGzB.m @@ -0,0 +1,39 @@ +% FusedFromFYawGzB - Philipp Allgeuer - 26/10/16 +% Calculates the fused angles (FGB) with a given fused yaw and body-fixed z-axis in global coordinates (GzB). +% +% function [FGB] = FusedFromFYawGzB(FYaw, GzB) +% +% It is assumed that GzB is a unit vector! + +% Main function +function [FGB] = FusedFromFYawGzB(FYaw, GzB) + + % Wrap the fused yaw to the desired range + FYaw = wrap(FYaw); + + % Precalculate trigonometric terms + cpsi = cos(FYaw); + spsi = sin(FYaw); + + % Calculate the fused pitch + stheta = cpsi*GzB(1) + spsi*GzB(2); + stheta = max(min(stheta,1.0),-1.0); % Note: If GzB is a unit vector then this should only trim at most a few eps... + theta = asin(stheta); + + % Calculate the fused roll + sphi = spsi*GzB(1) - cpsi*GzB(2); + sphi = max(min(sphi,1.0),-1.0); % Note: If GzB is a unit vector then this should only trim at most a few eps... + phi = asin(sphi); + + % See which hemisphere we're in + if GzB(3) >= 0 + h = 1; + else + h = -1; + end + + % Construct the output fused angles + FGB = [FYaw theta phi h]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/FusedFromQuat.m b/tro2022/transform/RotationsLib/FusedFromQuat.m new file mode 100644 index 0000000..20b2b23 --- /dev/null +++ b/tro2022/transform/RotationsLib/FusedFromQuat.m @@ -0,0 +1,53 @@ +% FusedFromQuat.m - Philipp Allgeuer - 05/11/14 +% Converts a quaternion rotation to the corresponding fused angles representation. +% +% function [Fused, Tilt] = FusedFromQuat(Quat) +% +% The output ranges are: +% Fused yaw: psi is in (-pi,pi] +% Fused pitch: theta is in [-pi/2,pi/2] +% Fused roll: phi is in [-pi/2,pi/2] +% Hemisphere: h is in {-1,1} +% +% Quat ==> Input quaternion rotation (assumed to have unit norm) +% Fused ==> Equivalent fused angles rotation +% Tilt ==> Equivalent tilt angles rotation + +% Main function +function [Fused, Tilt] = FusedFromQuat(Quat) + + % Calculate the fused yaw + psi = 2.0*atan2(Quat(4),Quat(1)); + psi = wrap(psi); + + % Calculate the fused pitch + rawstheta = 2.0*(Quat(3)*Quat(1)-Quat(2)*Quat(4)); + stheta = max(min(rawstheta,1.0),-1.0); % Note: If Quat is valid then this should only trim at most a few eps... + theta = asin(stheta); + + % Calculate the fused roll + rawsphi = 2.0*(Quat(3)*Quat(4)+Quat(2)*Quat(1)); + sphi = max(min(rawsphi,1.0),-1.0); % Note: If Quat is valid then this should only trim at most a few eps... + phi = asin(sphi); + + % See which hemisphere we're in + zzG = 2.0*(Quat(1)*Quat(1) + Quat(4)*Quat(4)) - 1.0; + if zzG >= 0 + h = 1; + else + h = -1; + end + + % Construct the output fused angles + Fused = [psi theta phi h]; + + % Construct the output tilt angles + if nargout >= 2 + gamma = atan2(rawstheta,rawsphi); + calpha = max(min(zzG,1.0),-1.0); % Note: If Quat is valid then this should only trim at most a few eps... + alpha = acos(calpha); + Tilt = [psi gamma alpha]; + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/FusedFromRotmat.m b/tro2022/transform/RotationsLib/FusedFromRotmat.m new file mode 100644 index 0000000..45441c2 --- /dev/null +++ b/tro2022/transform/RotationsLib/FusedFromRotmat.m @@ -0,0 +1,49 @@ +% FusedFromRotmat.m - Philipp Allgeuer - 05/11/14 +% Converts a rotation matrix to the corresponding fused angles representation. +% +% function [Fused, Tilt] = FusedFromRotmat(Rotmat) +% +% The output ranges are: +% Fused yaw: psi is in (-pi,pi] +% Fused pitch: theta is in [-pi/2,pi/2] +% Fused roll: phi is in [-pi/2,pi/2] +% Hemisphere: h is in {-1,1} +% +% Rotmat ==> Input rotation matrix +% Fused ==> Equivalent fused angles rotation +% Tilt ==> Equivalent tilt angles rotation + +% Main function +function [Fused, Tilt] = FusedFromRotmat(Rotmat) + + % Calculate the fused yaw + psi = FYawOfRotmat(Rotmat); + + % Calculate the fused pitch + stheta = max(min(-Rotmat(3,1),1.0),-1.0); % Note: If Rotmat is valid then this should only trim at most a few eps... + theta = asin(stheta); + + % Calculate the fused roll + sphi = max(min(Rotmat(3,2),1.0),-1.0); % Note: If Rotmat is valid then this should only trim at most a few eps... + phi = asin(sphi); + + % See which hemisphere we're in + if Rotmat(3,3) >= 0 + h = 1; + else + h = -1; + end + + % Construct the output fused angles + Fused = [psi theta phi h]; + + % Construct the output tilt angles + if nargout >= 2 + gamma = atan2(-Rotmat(3,1),Rotmat(3,2)); + calpha = max(min(Rotmat(3,3),1.0),-1.0); % Note: If Rotmat is valid then this should only trim at most a few eps... + alpha = acos(calpha); + Tilt = [psi gamma alpha]; + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/FusedFromTilt.m b/tro2022/transform/RotationsLib/FusedFromTilt.m new file mode 100644 index 0000000..7bdb6a3 --- /dev/null +++ b/tro2022/transform/RotationsLib/FusedFromTilt.m @@ -0,0 +1,41 @@ +% FusedFromTilt.m - Philipp Allgeuer - 05/11/14 +% Converts a tilt angles rotation to the corresponding fused angles representation. +% +% function [Fused] = FusedFromTilt(Tilt) +% +% The output ranges are: +% Fused yaw: psi is in (-pi,pi] +% Fused pitch: theta is in [-pi/2,pi/2] +% Fused roll: phi is in [-pi/2,pi/2] +% Hemisphere: h is in {-1,1} +% +% Tilt ==> Input tilt angles rotation +% Fused ==> Equivalent fused angles rotation + +% Main function +function [Fused] = FusedFromTilt(Tilt) + + % Get the required value of pi + PI = GetPI(Tilt); + + % Precalculate sin and cos values + cgamma = cos(Tilt(2)); + sgamma = sin(Tilt(2)); + salpha = sin(Tilt(3)); + + % Calculate theta and phi + theta = asin(salpha*sgamma); + phi = asin(salpha*cgamma); + + % See which hemisphere we're in + if abs(Tilt(3)) <= PI/2 + h = 1; + else + h = -1; + end + + % Construct the output fused angles + Fused = [Tilt(1) theta phi h]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/FusedFromZVec.m b/tro2022/transform/RotationsLib/FusedFromZVec.m new file mode 100644 index 0000000..0521093 --- /dev/null +++ b/tro2022/transform/RotationsLib/FusedFromZVec.m @@ -0,0 +1,33 @@ +% FusedFromZVec.m - Philipp Allgeuer - 21/10/16 +% Converts a z-vector to the corresponding zero fused yaw fused angles representation. +% +% function [Fused] = FusedFromZVec(ZVec) +% +% It is assumed that ZVec is a unit vector! +% +% ZVec ==> Input z-vector +% Fused ==> Corresponding zero fused yaw fused angles rotation + +% Main function +function [Fused] = FusedFromZVec(ZVec) + + % Calculate the fused pitch + stheta = max(min(-ZVec(1),1.0),-1.0); % Note: If ZVec is a unit vector then this should only trim at most a few eps... + theta = asin(stheta); + + % Calculate the fused roll + sphi = max(min(ZVec(2),1.0),-1.0); % Note: If ZVec is a unit vector then this should only trim at most a few eps... + phi = asin(sphi); + + % See which hemisphere we're in + if ZVec(3) >= 0 + h = 1; + else + h = -1; + end + + % Construct the output fused angles + Fused = [0 theta phi h]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/FusedIdentity.m b/tro2022/transform/RotationsLib/FusedIdentity.m new file mode 100644 index 0000000..6366ece --- /dev/null +++ b/tro2022/transform/RotationsLib/FusedIdentity.m @@ -0,0 +1,15 @@ +% FusedIdentity.m - Philipp Allgeuer - 05/11/14 +% Returns the identity fused angles rotation. +% +% function [FI] = FusedIdentity() +% +% FI ==> Identity rotation in the fused angles representation + +% Main function +function [FI] = FusedIdentity() + + % Return the required identity rotation + FI = [0 0 0 1]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/FusedInv.m b/tro2022/transform/RotationsLib/FusedInv.m new file mode 100644 index 0000000..4abaf5b --- /dev/null +++ b/tro2022/transform/RotationsLib/FusedInv.m @@ -0,0 +1,38 @@ +% FusedInv.m - Philipp Allgeuer - 05/11/14 +% Calculates the inverse of a fused angles rotation. +% +% function [Finv] = FusedInv(F) +% +% F ==> Input fused angles rotation +% Finv ==> Inverse fused angles rotation + +% Main function +function [Finv] = FusedInv(F) + + % Calculate the tilt axis angle + sth = sin(F(2)); + sph = sin(F(3)); + gamma = atan2(sth,sph); + + % Calculate the sin of the tilt angle + crit = sth*sth + sph*sph; + if crit >= 1.0 + salpha = 1.0; + else + salpha = sqrt(crit); + end + + % Precalculate trigonometric values + psigam = F(1) + gamma; + cpsigam = cos(psigam); + spsigam = sin(psigam); + + % Calculate the inverse fused pitch and roll + thinv = asin(-salpha*spsigam); + phinv = asin(-salpha*cpsigam); + + % Construct the inverse rotation + Finv = [-F(1) thinv phinv F(4)]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/FusedNoEYaw.m b/tro2022/transform/RotationsLib/FusedNoEYaw.m new file mode 100644 index 0000000..3542f3c --- /dev/null +++ b/tro2022/transform/RotationsLib/FusedNoEYaw.m @@ -0,0 +1,25 @@ +% FusedNoEYaw.m - Philipp Allgeuer - 05/11/14 +% Removes the ZYX yaw component of a fused angles rotation. +% +% function [Fout, EYaw, FEYaw] = FusedNoEYaw(Fin) +% +% Fin ==> Input fused angles rotation +% Fout ==> Output fused angles rotation with no ZYX yaw +% EYaw ==> ZYX yaw of the input rotation +% FEYaw ==> Fused angles representation of the ZYX yaw component +% of the input rotation + +% Main function +function [Fout, EYaw, FEYaw] = FusedNoEYaw(Fin) + + % Calculate the ZYX yaw of the input + EYaw = EYawOfFused(Fin); + + % Construct the ZYX yaw component of the rotation + FEYaw = [EYaw 0 0 1]; + + % Remove the ZYX yaw component of the rotation + Fout = [Fin(1)-EYaw Fin(2:4)]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/FusedNoFYaw.m b/tro2022/transform/RotationsLib/FusedNoFYaw.m new file mode 100644 index 0000000..bbad3dd --- /dev/null +++ b/tro2022/transform/RotationsLib/FusedNoFYaw.m @@ -0,0 +1,25 @@ +% FusedNoFYaw.m - Philipp Allgeuer - 05/11/14 +% Removes the fused yaw component of a fused angles rotation. +% +% function [Fout, FYaw, FFYaw] = FusedNoFYaw(Fin) +% +% Fin ==> Input fused angles rotation +% Fout ==> Output fused angles rotation with zero fused yaw +% FYaw ==> Fused yaw of the input rotation +% FFYaw ==> Fused angles representation of the fused yaw component +% of the input rotation + +% Main function +function [Fout, FYaw, FFYaw] = FusedNoFYaw(Fin) + + % Calculate the fused yaw of the input + FYaw = Fin(1); + + % Construct the fused yaw component of the rotation + FFYaw = [FYaw 0 0 1]; + + % Remove the fused yaw component of the rotation + Fout = [0 Fin(2:4)]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/FusedRotVec.m b/tro2022/transform/RotationsLib/FusedRotVec.m new file mode 100644 index 0000000..5e07c32 --- /dev/null +++ b/tro2022/transform/RotationsLib/FusedRotVec.m @@ -0,0 +1,18 @@ +% FusedRotVec.m - Philipp Allgeuer - 05/11/14 +% Rotate a vector by a given fused angles rotation. +% +% function [Vout] = FusedRotVec(Fused, Vin) +% +% Fused ==> Fused angles rotation to apply to Vin +% Vin ==> Input vector to rotate +% Vout ==> Rotated output vector + +% Main function +function [Vout] = FusedRotVec(Fused, Vin) + + % Rotate the vector as required + Vout = zeros(size(Vin)); + Vout(:) = RotmatFromFused(Fused) * Vin(:); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/GetPI.m b/tro2022/transform/RotationsLib/GetPI.m new file mode 100644 index 0000000..13d9446 --- /dev/null +++ b/tro2022/transform/RotationsLib/GetPI.m @@ -0,0 +1,14 @@ +% Return the value of pi in the required type +function PI = GetPI(in) + if nargin < 1 + PI = pi; + return + end + switch class(in(1)) + case 'hpf' + PI = hpf('pi', in(1).NumberOfDigits); + otherwise + PI = pi; + end +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/LoadRotations.m b/tro2022/transform/RotationsLib/LoadRotations.m new file mode 100644 index 0000000..ac3d4d8 --- /dev/null +++ b/tro2022/transform/RotationsLib/LoadRotations.m @@ -0,0 +1,28 @@ +% LoadRotations.m - Philipp Allgeuer - 05/11/14 +% Add the Rotations Library directory to the working path. +% +% If the current directory is the rotations library directory then simply +% execute this script using 'LoadRotations' and change to whatever other +% working directory you wish to have. If not, run the script using: +% >> run path/to/LoadRotations.m +% After this the library functions will be available on the path. + +% Display a header +disp(''); +disp('Loading Rotations Library v1.4.0...'); + +% Add the path of this script to the working path +ScriptPath = [pwd '/LoadRotations.m']; +if exist(ScriptPath,'file') ~= 2 + disp('Error: Could not find the path to the LoadRotations.m script'); + disp('Note: This script is intended for execution using the run command'); + disp(' when LoadRotations.m is not on the current path, or just'); + disp(' normally when this script is in the current working directory.'); + disp('Nothing was changed.'); +else + disp(['Adding path: ' pwd]); + addpath(pwd); + disp('Done.'); +end +disp(''); +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/LoadRotationsTest.m b/tro2022/transform/RotationsLib/LoadRotationsTest.m new file mode 100644 index 0000000..8b1e261 --- /dev/null +++ b/tro2022/transform/RotationsLib/LoadRotationsTest.m @@ -0,0 +1,34 @@ +% LoadRotationsTest.m - Philipp Allgeuer - 05/11/14 +% Add the Rotations Library and corresponding test directory to the working path. +% +% If the current directory is the rotations library directory then simply +% execute this script using 'LoadRotationsTest' and change to whatever other +% working directory you wish to have. If not, run the script using: +% >> run path/to/LoadRotationsTest.m +% After this the library and test functions will be available on the path. + +% Display a header +disp(''); +disp('Loading Rotations Library and Test v1.4.0...'); + +% Add the path of this script and the Test subdirectory thereof to the working path +ScriptPath = [pwd '/LoadRotationsTest.m']; +TestPath = [pwd '/Test']; +if exist(ScriptPath,'file') ~= 2 + disp('Error: Could not find the path to the LoadRotationsTest.m script'); + disp('Note: This script is intended for execution using the run command'); + disp(' when LoadRotationsTest.m is not on the current path, or just'); + disp(' normally when this script is in the current working directory.'); + disp('Nothing was changed.'); +elseif exist(TestPath,'dir') ~= 7 + disp(['Error: Test folder not found at ' TestPath]); + disp('Nothing was changed.'); +else + disp(['Adding path: ' pwd]); + addpath(pwd); + disp(['Adding path: ' TestPath]); + addpath(TestPath); + disp('Done.'); +end +disp(''); +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/PhaseFromAbsPhase.m b/tro2022/transform/RotationsLib/PhaseFromAbsPhase.m new file mode 100644 index 0000000..979a162 --- /dev/null +++ b/tro2022/transform/RotationsLib/PhaseFromAbsPhase.m @@ -0,0 +1,20 @@ +% PhaseFromAbsPhase.m - Philipp Allgeuer - 16/01/18 +% Converts an absolute tilt phase rotation to the corresponding tilt phase representation. +% +% function [Phase] = PhaseFromAbsPhase(AbsPhase) +% +% AbsPhase ==> Input absolute tilt phase rotation +% Phase ==> Equivalent tilt phase rotation + +% Main function +function [Phase] = PhaseFromAbsPhase(AbsPhase) + + % Precalculate trigonometric values + cpsi = cos(AbsPhase(3)); + spsi = sin(AbsPhase(3)); + + % Construct the output tilt phase + Phase = [cpsi*AbsPhase(1)+spsi*AbsPhase(2) cpsi*AbsPhase(2)-spsi*AbsPhase(1) AbsPhase(3)]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/PhaseFromQuat.m b/tro2022/transform/RotationsLib/PhaseFromQuat.m new file mode 100644 index 0000000..0c2a60c --- /dev/null +++ b/tro2022/transform/RotationsLib/PhaseFromQuat.m @@ -0,0 +1,17 @@ +% PhaseFromQuat.m - Philipp Allgeuer - 16/01/18 +% Converts a quaternion rotation to the corresponding tilt phase representation. +% +% function [Phase, Tilt] = PhaseFromQuat(Quat) +% +% Quat ==> Input quaternion rotation +% Phase ==> Equivalent tilt phase rotation + +% Main function +function [Phase, Tilt] = PhaseFromQuat(Quat) + + % Construct the output tilt phase + Tilt = TiltFromQuat(Quat); + Phase = PhaseFromTilt(Tilt); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/PhaseFromTilt.m b/tro2022/transform/RotationsLib/PhaseFromTilt.m new file mode 100644 index 0000000..8a5a63f --- /dev/null +++ b/tro2022/transform/RotationsLib/PhaseFromTilt.m @@ -0,0 +1,16 @@ +% PhaseFromTilt.m - Philipp Allgeuer - 16/01/18 +% Converts a tilt angles rotation to the corresponding tilt phase representation. +% +% function [Phase] = PhaseFromTilt(Tilt) +% +% Tilt ==> Input tilt angles rotation +% Phase ==> Equivalent tilt phase rotation + +% Main function +function [Phase] = PhaseFromTilt(Tilt) + + % Construct the output tilt phase + Phase = [Tilt(3)*cos(Tilt(2)) Tilt(3)*sin(Tilt(2)) Tilt(1)]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/PlotCsys.m b/tro2022/transform/RotationsLib/PlotCsys.m new file mode 100644 index 0000000..76f7c35 --- /dev/null +++ b/tro2022/transform/RotationsLib/PlotCsys.m @@ -0,0 +1,98 @@ +% PlotCsys.m - Philipp Allgeuer - 05/11/14 +% Plot an XYZ coordinate system in the currently active 3D plot. +% +% function PlotCsys(R, label, P, L, varargin) +% +% The coordinate system axes are plotted at point P (3D coordinate), and with +% orientation given by the rotation matrix R. L is a 3-vector that specifies +% the lengths of the individual axes (e.g. the x-axis is drawn from -L(1) to +% L(1)). If L is a scalar then the same length is used for all three axes. +% Axis labels are drawn if the label (string) parameter is non-zero. The string +% value is used as the axis subscripts. Custom arguments such as marker +% specifications can be forwarded to the internal plot3 function calls using +% the variable input arguments. If a quaternion is passed via the R parameter +% instead of a rotation matrix, this is detected and the quaternion is +% internally converted into a rotation matrix for processing. +% +% R ==> Rotation matrix or quaternion to plot the axes of +% label ==> String to use for the axis label subscripts (e.g. use 'G' for x_G) +% P ==> The origin at which to plot the coordinate axes +% L ==> The axis lengths to use for plotting +% varargin ==> Custom line graphics properties to set in all internal calls to plot3 + +% Main function +function PlotCsys(R, label, P, L, varargin) + + % Default argument values + if nargin < 1 + R = eye(3); + end + if nargin < 2 + label = ''; + end + if nargin < 3 + P = [0 0 0]; + end + if nargin < 4 + L = [1 1 1]; + end + + % Convert the input to a rotation matrix if it's a quaternion + if size(R,1) == 1 && size(R,2) == 4 + R = RotmatFromQuat(R); + end + + % Check parameter sizes and values + if isscalar(L) + L = [L L L]; + end + L = abs(L); + L = L(:)'; + P = P(:)'; + if any(size(R) ~= 3) + error('R doesn''t have the right dimensions for a rotation matrix or quaternion!'); + end + if norm(R*R'-eye(3),'fro') > 1024*eps + warning('R is not orthogonal to 1024*eps!'); + end + + % Generate the required plotting data + LXAxis = [-L(1) 0 0;L(1) 0 0]; + LYAxis = [0 -L(2) 0;0 L(2) 0]; + LZAxis = [0 0 -L(3);0 0 L(3)]; + GXAxis = LocalToGlobal(LXAxis, P, R); + GYAxis = LocalToGlobal(LYAxis, P, R); + GZAxis = LocalToGlobal(LZAxis, P, R); + + % Generate the data required for the axes labels + TSCL = L*1.1; + LTLbl = diag(TSCL); + GTLbl = LocalToGlobal(LTLbl, P, R); + + % Plot the required coordinate axes + plot3(GXAxis(:,1),GXAxis(:,2),GXAxis(:,3),'r-',varargin{:}); % x => Red + washeld = ishold; + hold on; + plot3(GYAxis(:,1),GYAxis(:,2),GYAxis(:,3),'-','Color',[0.15 0.70 0.15],varargin{:}); % y => Green + plot3(GZAxis(:,1),GZAxis(:,2),GZAxis(:,3),'b-',varargin{:}); % z => Blue + if any(label ~= 0) || isempty(label) + if isempty(label) + xlbl = 'x'; ylbl = 'y'; zlbl = 'z'; + else + xlbl = ['x_{' label '}']; ylbl = ['y_{' label '}']; zlbl = ['z_{' label '}']; + end + text(GTLbl(1,1),GTLbl(1,2),GTLbl(1,3),xlbl,'Color','r','HorizontalAlignment','center'); % x => Red + text(GTLbl(2,1),GTLbl(2,2),GTLbl(2,3),ylbl,'Color',[0.15 0.70 0.15],'HorizontalAlignment','center'); % y => Green + text(GTLbl(3,1),GTLbl(3,2),GTLbl(3,3),zlbl,'Color','b','HorizontalAlignment','center'); % z => Blue + end + if ~washeld + hold off; + end + +end + +% Function that transforms local into global coordinates +function Global = LocalToGlobal(Local, P, R) % Local needs to be Nx3... + Global = repmat(P,size(Local,1),1) + Local*R'; +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/QuatEqual.m b/tro2022/transform/RotationsLib/QuatEqual.m new file mode 100644 index 0000000..2cbc07e --- /dev/null +++ b/tro2022/transform/RotationsLib/QuatEqual.m @@ -0,0 +1,35 @@ +% QuatEqual.m - Philipp Allgeuer - 05/11/14 +% Returns whether two quaternion rotations represent the same rotation. +% +% function [Equal, Err] = QuatEqual(P, Q, Tol) +% +% Two quaternions P and Q are equivalent iff P == Q or P == -Q. +% Refer to EnsureQuat for more information on the unique form of a +% quaternion rotation. +% +% P ==> First quaternion rotation to compare +% Q ==> Second quaternion rotation to compare +% Tol ==> Allowed tolerance between P and Q for which equality is still asserted +% Equal ==> Boolean flag whether P equals Q to the given L_inf norm tolerance +% Err ==> The quantified error between P and Q (0 if exactly equal) + +% Main function +function [Equal, Err] = QuatEqual(P, Q, Tol) + + % Default tolerance + if nargin < 3 + Tol = 1e-10; + else + Tol = abs(Tol); + end + + % Calculate the deviation between the two rotations + ErrPos = max(abs(P-Q)); + ErrNeg = max(abs(P+Q)); + Err = min(ErrPos,ErrNeg); + + % Check whether the two rotations are within tolerance + Equal = (Err <= Tol); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/QuatFromAbsPhase.m b/tro2022/transform/RotationsLib/QuatFromAbsPhase.m new file mode 100644 index 0000000..bbc54bc --- /dev/null +++ b/tro2022/transform/RotationsLib/QuatFromAbsPhase.m @@ -0,0 +1,17 @@ +% QuatFromAbsPhase.m - Philipp Allgeuer - 16/01/18 +% Converts an absolute tilt phase rotation to the corresponding quaternion representation. +% +% function [Quat, Tilt] = QuatFromAbsPhase(AbsPhase) +% +% AbsPhase ==> Input absolute tilt phase rotation +% Quat ==> Equivalent quaternion rotation + +% Main function +function [Quat, Tilt] = QuatFromAbsPhase(AbsPhase) + + % Construct the output quaternion + Tilt = TiltFromAbsPhase(AbsPhase); + Quat = QuatFromTilt(Tilt); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/QuatFromAxis.m b/tro2022/transform/RotationsLib/QuatFromAxis.m new file mode 100644 index 0000000..e578608 --- /dev/null +++ b/tro2022/transform/RotationsLib/QuatFromAxis.m @@ -0,0 +1,57 @@ +% QuatFromAxis.m - Philipp Allgeuer - 05/11/14 +% Constructs a quaternion rotation based on an axis-angle specification. +% +% function [Quat] = QuatFromAxis(Axis, Angle) +% +% Axis ==> The vector corresponding to the axis of rotation (magnitude is unimportant) +% either given as the vector [ux uy uz] or one of {'x', 'y', 'z'} +% Angle ==> The angle of the rotation given by the right hand rule +% Quat ==> Equivalent quaternion rotation + +% Main function +function [Quat] = QuatFromAxis(Axis, Angle) + + % Handle case of missing arguments + if nargin < 2 + Quat = [1 0 0 0]; + return; + end + + % Wrap the rotation angle to (-pi,pi] + Angle = wrap(Angle); + + % Precompute the required sin and cos terms + hcang = cos(Angle/2); + hsang = sin(Angle/2); + + % Handle case of standard axis rotations + if strcmpi(Axis, 'x') + Quat = [hcang hsang 0 0]; + return; + elseif strcmpi(Axis, 'y') + Quat = [hcang 0 hsang 0]; + return; + elseif strcmpi(Axis, 'z') + Quat = [hcang 0 0 hsang]; + return; + end + + % Axis error checking + if ~(isnumeric(Axis) && isvector(Axis) && length(Axis) == 3) + error('Axis must be a 3-vector, or one of {''x'',''y'',''z''}.'); + end + + % Normalise the axis vector + AxisNorm = norm(Axis); + if AxisNorm <= 0 + Quat = [1 0 0 0]; % Return the identity rotation if Axis is the zero vector + return; + else + Axis = Axis/AxisNorm; + end + + % Construct the required rotation + Quat = [hcang hsang*Axis(:)']; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/QuatFromEuler.m b/tro2022/transform/RotationsLib/QuatFromEuler.m new file mode 100644 index 0000000..adbb168 --- /dev/null +++ b/tro2022/transform/RotationsLib/QuatFromEuler.m @@ -0,0 +1,31 @@ +% QuatFromEuler.m - Philipp Allgeuer - 05/11/14 +% Converts a ZYX Euler angles rotation to the corresponding quaternion representation. +% +% function [Quat] = QuatFromEuler(Euler) +% +% The output quaternion is in the format [w x y z]. +% +% Euler ==> Input ZYX Euler angles rotation +% Quat ==> Equivalent quaternion rotation + +% Main function +function [Quat] = QuatFromEuler(Euler) + + % Halve the Euler angles + hpsi = Euler(1)/2; + hth = Euler(2)/2; + hphi = Euler(3)/2; + + % Precalculate the sin and cos values + cpsi = cos(hpsi); + spsi = sin(hpsi); + cth = cos(hth); + sth = sin(hth); + cphi = cos(hphi); + sphi = sin(hphi); + + % Calculate the required quaternion + Quat = [cphi*cth*cpsi+sphi*sth*spsi sphi*cth*cpsi-cphi*sth*spsi cphi*sth*cpsi+sphi*cth*spsi cphi*cth*spsi-sphi*sth*cpsi]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/QuatFromFYawBzG.m b/tro2022/transform/RotationsLib/QuatFromFYawBzG.m new file mode 100644 index 0000000..cfcbf2d --- /dev/null +++ b/tro2022/transform/RotationsLib/QuatFromFYawBzG.m @@ -0,0 +1,39 @@ +% QuatFromFYawBzG - Philipp Allgeuer - 19/10/16 +% Calculates the quaternion (qGB) with a given fused yaw and global z-axis in body-fixed coordinates (BzG). +% +% function [qGB] = QuatFromFYawBzG(FYaw, BzG) +% +% It is assumed that BzG is a unit vector! + +% Main function +function [qGB] = QuatFromFYawBzG(FYaw, BzG) + + % Precalculate trigonometric terms + chpsi = cos(FYaw/2); + shpsi = sin(FYaw/2); + + % Calculate the w and z components + wsqpluszsq = min(max((1 + BzG(3))/2, 0), 1); % Note: If BzG is a unit vector then this should only trim at most a few eps... + wznorm = sqrt(wsqpluszsq); + w = wznorm * chpsi; + z = wznorm * shpsi; + + % Calculate the x and y components + xsqplusysq = 1 - wsqpluszsq; + xtilde = BzG(1)*z + BzG(2)*w; + ytilde = BzG(2)*z - BzG(1)*w; + xytildenormsq = xtilde*xtilde + ytilde*ytilde; + if xytildenormsq <= 0 + x = sqrt(xsqplusysq); + y = 0; + else + factor = sqrt(xsqplusysq / xytildenormsq); + x = factor * xtilde; + y = factor * ytilde; + end + + % Construct the output quaternion + qGB = [w x y z]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/QuatFromFYawGzB.m b/tro2022/transform/RotationsLib/QuatFromFYawGzB.m new file mode 100644 index 0000000..3aac0ec --- /dev/null +++ b/tro2022/transform/RotationsLib/QuatFromFYawGzB.m @@ -0,0 +1,39 @@ +% QuatFromFYawGzB - Philipp Allgeuer - 26/10/16 +% Calculates the quaternion (qGB) with a given fused yaw and body-fixed z-axis in global coordinates (GzB). +% +% function [qGB] = QuatFromFYawGzB(FYaw, GzB) +% +% It is assumed that GzB is a unit vector! + +% Main function +function [qGB] = QuatFromFYawGzB(FYaw, GzB) + + % Precalculate trigonometric terms + chpsi = cos(FYaw/2); + shpsi = sin(FYaw/2); + + % Calculate the w and z components + wsqpluszsq = min(max((1 + GzB(3))/2, 0), 1); % Note: If GzB is a unit vector then this should only trim at most a few eps... + wznorm = sqrt(wsqpluszsq); + w = wznorm * chpsi; + z = wznorm * shpsi; + + % Calculate the x and y components + xsqplusysq = 1 - wsqpluszsq; + xtilde = GzB(1)*z - GzB(2)*w; + ytilde = GzB(2)*z + GzB(1)*w; + xytildenormsq = xtilde*xtilde + ytilde*ytilde; + if xytildenormsq <= 0 + x = sqrt(xsqplusysq); + y = 0; + else + factor = sqrt(xsqplusysq / xytildenormsq); + x = factor * xtilde; + y = factor * ytilde; + end + + % Construct the output quaternion + qGB = [w x y z]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/QuatFromFYawTilt.m b/tro2022/transform/RotationsLib/QuatFromFYawTilt.m new file mode 100644 index 0000000..ea2bbb9 --- /dev/null +++ b/tro2022/transform/RotationsLib/QuatFromFYawTilt.m @@ -0,0 +1,64 @@ +% QuatFromFYawTilt.m - Philipp Allgeuer - 04/05/17 +% Calculates the frame B with a given fused yaw relative to G and tilt rotation component relative to H. +% +% function [qHB, qGB] = QuatFromFYawTilt(FYawG, TiltH, qGH) +% +% FYawG ==> Desired fused yaw of B relative to G +% TiltH ==> Specification of the desired tilt rotation component of B relative to H +% Can be any quaternion [w x y z] with the required tilt rotation component +% (i.e. any qHC that has the same tilt rotation component as the desired qHB) +% Can be any tilt angles [psi gamma alpha] with the required tilt rotation component +% Can be a direct tilt rotation component specification [gamma alpha] +% qGH ==> Relative quaternion rotation between G and H +% qHB ==> Output quaternion rotation from H to B +% qGB ==> Output quaternion rotation from G to B + +% Main function +function [qHB, qGB] = QuatFromFYawTilt(FYawG, TiltH, qGH) + + % Retrieve the reference quaternion with the required tilt rotation component + N = numel(TiltH); + if N == 2 + qHC = QuatFromTilt([0 TiltH(1) TiltH(2)]); + elseif N == 3 + qHC = QuatFromTilt(TiltH); + elseif N == 4 + qHC = TiltH; + else + error('Invalid TiltH argument!'); + end + + % Precompute trigonometric values + chpsi = cos(FYawG/2); + shpsi = sin(FYawG/2); + + % Construct the base components of the solution + a = qGH(2)*qHC(2) + qGH(3)*qHC(3); + b = qGH(2)*qHC(3) - qGH(3)*qHC(2); + c = qGH(1)*qHC(4) + qGH(4)*qHC(1); + d = qGH(1)*qHC(1) - qGH(4)*qHC(4); + A = d - a; + B = b - c; + C = b + c; + D = d + a; + G = D*chpsi - B*shpsi; + H = A*shpsi - C*chpsi; + F = sqrt(G*G + H*H); + + % Construct the z-rotation qHCB that maps C to the required B + if F <= 0 + qHCB = [1 0 0 0]; + else + chphi = G/F; + shphi = H/F; + qHCB = [chphi 0 0 shphi]; + end + + % Construct the output quaternions + qHB = QuatMult(qHCB, qHC); + if nargout >= 2 + qGB = QuatMult(qGH, qHB); + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/QuatFromFused.m b/tro2022/transform/RotationsLib/QuatFromFused.m new file mode 100644 index 0000000..af43dc4 --- /dev/null +++ b/tro2022/transform/RotationsLib/QuatFromFused.m @@ -0,0 +1,79 @@ +% QuatFromFused.m - Philipp Allgeuer - 05/11/14 +% Converts a fused angles rotation to the corresponding quaternion representation. +% +% function [Quat, Tilt] = QuatFromFused(Fused) +% +% The output quaternion is in the format [w x y z]. +% +% Fused ==> Input fused angles rotation +% Quat ==> Equivalent quaternion rotation + +% Main function +function [Quat, Tilt] = QuatFromFused(Fused) + + % Precalculate the required trigonometric values + hpsi = Fused(1)/2; + chpsi = cos(hpsi); + shpsi = sin(hpsi); + sth = sin(Fused(2)); + sphi = sin(Fused(3)); + + % Calculate the cos of the tilt angle + crit = sth*sth + sphi*sphi; + if crit >= 1.0 + calpha = 0; + else + if Fused(4) >= 0 + calpha = sqrt(1-crit); + else + calpha = -sqrt(1-crit); + end + end + + % Construct the output quaternion using the best conditioned expression + if calpha >= 0 + + % Precalculate terms involved in the quaternion expression + C = 1 + calpha; + + % Calculate the required quaternion + Quat = [C*chpsi sphi*chpsi-sth*shpsi sphi*shpsi+sth*chpsi C*shpsi]; + Quat = Quat/sqrt(C*C+crit); % Note: norm(Quat) = sqrt(C*C+sth*sth+sphi*sphi) = sqrt(2*C) > 1 + + % Return the tilt angles representation + if nargout >= 2 + gamma = atan2(sth,sphi); + alpha = acos(calpha); + Tilt = [Fused(1) gamma alpha]; + end + + else + + % Calculate the sin of the tilt angle + if crit >= 1.0 + salpha = 1.0; + else + salpha = sqrt(crit); + end + + % Precalculate terms involved in the quaternion expression + C = 1 - calpha; + gamma = atan2(sth,sphi); + hgampsi = gamma + hpsi; + chgampsi = cos(hgampsi); + shgampsi = sin(hgampsi); + + % Calculate the required quaternion + Quat = [salpha*chpsi C*chgampsi C*shgampsi salpha*shpsi]; + Quat = Quat/sqrt(C*C+crit); % Note: norm(Quat) = sqrt(C*C+sth*sth+sphi*sphi) = sqrt(2*C) > 1 + + % Return the tilt angles representation + if nargout >= 2 + alpha = acos(calpha); + Tilt = [Fused(1) gamma alpha]; + end + + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/QuatFromPhase.m b/tro2022/transform/RotationsLib/QuatFromPhase.m new file mode 100644 index 0000000..75583d8 --- /dev/null +++ b/tro2022/transform/RotationsLib/QuatFromPhase.m @@ -0,0 +1,17 @@ +% QuatFromPhase.m - Philipp Allgeuer - 16/01/18 +% Converts a tilt phase rotation to the corresponding quaternion representation. +% +% function [Quat, Tilt] = QuatFromPhase(Phase) +% +% Phase ==> Input tilt phase rotation +% Quat ==> Equivalent quaternion rotation + +% Main function +function [Quat, Tilt] = QuatFromPhase(Phase) + + % Construct the output quaternion + Tilt = TiltFromPhase(Phase); + Quat = QuatFromTilt(Tilt); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/QuatFromRotmat.m b/tro2022/transform/RotationsLib/QuatFromRotmat.m new file mode 100644 index 0000000..85f7011 --- /dev/null +++ b/tro2022/transform/RotationsLib/QuatFromRotmat.m @@ -0,0 +1,40 @@ +% QuatFromRotmat.m - Philipp Allgeuer - 05/11/14 +% Converts a rotation matrix to the corresponding quaternion representation. +% +% function [Quat] = QuatFromRotmat(Rotmat) +% +% The output quaternion is in the format [w x y z]. +% Note that Quat and -Quat are both valid and completely equivalent outputs +% here, so we have the freedom to arbitrarily choose the sign of *one* of +% the quaternion parameters during the conversion. At the end of the +% conversion here, we change the sign of the whole quaternion so that the +% w component is non-negative. +% +% Rotmat ==> Input rotation matrix +% Quat ==> Equivalent quaternion rotation + +% Main function +function [Quat] = QuatFromRotmat(Rotmat) + + % Calculate a scaled version of the required quaternion + t = Rotmat(1,1) + Rotmat(2,2) + Rotmat(3,3); + if t >= 0 + qtilde = [1+t Rotmat(3,2)-Rotmat(2,3) Rotmat(1,3)-Rotmat(3,1) Rotmat(2,1)-Rotmat(1,2)]; + elseif Rotmat(3,3) >= Rotmat(2,2) && Rotmat(3,3) >= Rotmat(1,1) + qtilde = [Rotmat(2,1)-Rotmat(1,2) Rotmat(1,3)+Rotmat(3,1) Rotmat(3,2)+Rotmat(2,3) 1-Rotmat(1,1)-Rotmat(2,2)+Rotmat(3,3)]; + elseif Rotmat(2,2) >= Rotmat(1,1) + qtilde = [Rotmat(1,3)-Rotmat(3,1) Rotmat(2,1)+Rotmat(1,2) 1-Rotmat(1,1)+Rotmat(2,2)-Rotmat(3,3) Rotmat(3,2)+Rotmat(2,3)]; + else + qtilde = [Rotmat(3,2)-Rotmat(2,3) 1+Rotmat(1,1)-Rotmat(2,2)-Rotmat(3,3) Rotmat(2,1)+Rotmat(1,2) Rotmat(1,3)+Rotmat(3,1)]; + end + + % Normalise the output quaternion + Quat = qtilde / norm(qtilde); + + % Ensure that the w component is non-negative (not strictly speaking necessary, as Quat is actually entirely equivalent to -Quat as a rotation) + if Quat(1) < 0 + Quat = -Quat; + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/QuatFromTilt.m b/tro2022/transform/RotationsLib/QuatFromTilt.m new file mode 100644 index 0000000..72e37c4 --- /dev/null +++ b/tro2022/transform/RotationsLib/QuatFromTilt.m @@ -0,0 +1,31 @@ +% QuatFromTilt.m - Philipp Allgeuer - 05/11/14 +% Converts a tilt angles rotation to the corresponding quaternion representation. +% +% function [Quat] = QuatFromTilt(Tilt) +% +% The output quaternion is in the format [w x y z]. +% +% Tilt ==> Input tilt angles rotation +% Quat ==> Equivalent quaternion rotation + +% Main function +function [Quat] = QuatFromTilt(Tilt) + + % Evaluate the required intermediate angles + hpsi = Tilt(1)/2; + halpha = Tilt(3)/2; + hgampsi = Tilt(2) + hpsi; + + % Precalculate trigonometric terms involved in the quaternion expression + chpsi = cos(hpsi); + shpsi = sin(hpsi); + chalpha = cos(halpha); + shalpha = sin(halpha); + chgampsi = cos(hgampsi); + shgampsi = sin(hgampsi); + + % Calculate the required quaternion + Quat = [chalpha*chpsi shalpha*chgampsi shalpha*shgampsi chalpha*shpsi]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/QuatFromZVec.m b/tro2022/transform/RotationsLib/QuatFromZVec.m new file mode 100644 index 0000000..0b123d1 --- /dev/null +++ b/tro2022/transform/RotationsLib/QuatFromZVec.m @@ -0,0 +1,36 @@ +% QuatFromZVec.m - Philipp Allgeuer - 21/10/16 +% Converts a z-vector to the corresponding zero fused yaw quaternion representation. +% +% function [Quat] = QuatFromZVec(ZVec) +% +% It is assumed that ZVec is a unit vector! +% +% ZVec ==> Input z-vector +% Quat ==> Corresponding zero fused yaw quaternion rotation + +% Main function +function [Quat] = QuatFromZVec(ZVec) + + % Calculate the w component + wsq = min(max((1 + ZVec(3))/2, 0), 1); % Note: If ZVec is a unit vector then this should only trim at most a few eps... + w = sqrt(wsq); + + % Calculate the x and y components + xsqplusysq = 1 - wsq; + xtilde = ZVec(2); + ytilde = -ZVec(1); + xytildenormsq = xtilde*xtilde + ytilde*ytilde; + if xytildenormsq <= 0 + x = sqrt(xsqplusysq); + y = 0; + else + factor = sqrt(xsqplusysq / xytildenormsq); + x = factor * xtilde; + y = factor * ytilde; + end + + % Construct the output quaternion + Quat = [w x y 0]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/QuatIdentity.m b/tro2022/transform/RotationsLib/QuatIdentity.m new file mode 100644 index 0000000..966dda4 --- /dev/null +++ b/tro2022/transform/RotationsLib/QuatIdentity.m @@ -0,0 +1,15 @@ +% QuatIdentity.m - Philipp Allgeuer - 05/11/14 +% Returns the identity quaternion rotation. +% +% function [QI] = QuatIdentity() +% +% QI ==> Identity rotation in the quaternion representation + +% Main function +function [QI] = QuatIdentity() + + % Return the required identity rotation + QI = [1 0 0 0]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/QuatInv.m b/tro2022/transform/RotationsLib/QuatInv.m new file mode 100644 index 0000000..0b80dde --- /dev/null +++ b/tro2022/transform/RotationsLib/QuatInv.m @@ -0,0 +1,19 @@ +% QuatInv.m - Philipp Allgeuer - 05/11/14 +% Calculates the inverse of a quaternion rotation. +% +% function [Qinv] = QuatInv(Q) +% +% Note that the calculation internally is merely conjugation, so the input +% quaternion must be of unit magnitude for the output to be valid. +% +% Q ==> Input quaternion rotation (assumed to have unit norm) +% Qinv ==> Inverse quaternion rotation (i.e. the conjugation) + +% Main function +function [Qinv] = QuatInv(Q) + + % Calculate the inverse/conjugation of the quaternion + Qinv = [Q(1) -Q(2:4)]; % Assumes the quaternion is valid + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/QuatMult.m b/tro2022/transform/RotationsLib/QuatMult.m new file mode 100644 index 0000000..da95309 --- /dev/null +++ b/tro2022/transform/RotationsLib/QuatMult.m @@ -0,0 +1,30 @@ +% QuatMult.m - Philipp Allgeuer - 03/11/16 +% Multiplies together a series of quaternions. +% +% function [Qout] = QuatMult(varargin) +% +% For example: QuatMult(Q1,Q2,Q3) = Q1*Q2*Q3 +% +% Qout ==> Quaternion representation of the composed rotation + +% Main function +function [Qout] = QuatMult(varargin) + + % Compose the required rotations + Num = length(varargin); + if Num <= 0 + Qout = [1 0 0 0]; + else + Qout = varargin{1}; + for k = 2:Num + Q = varargin{k}; + if numel(Q) ~= 4 + warning('Ignoring an invalid rotation parameter!'); + continue; + end + Qout = [Qout(1)*Q(1)-dot(Qout(2:4),Q(2:4)) Qout(1)*Q(2:4)+Q(1)*Qout(2:4)+cross(Qout(2:4),Q(2:4))]; + end + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/QuatNoEYaw.m b/tro2022/transform/RotationsLib/QuatNoEYaw.m new file mode 100644 index 0000000..32271d1 --- /dev/null +++ b/tro2022/transform/RotationsLib/QuatNoEYaw.m @@ -0,0 +1,27 @@ +% QuatNoEYaw.m - Philipp Allgeuer - 05/11/14 +% Removes the ZYX yaw component of a quaternion rotation. +% +% function [Qout, EYaw, QEYaw] = QuatNoEYaw(Qin) +% +% Qin ==> Input quaternion rotation (assumed to have unit norm) +% Qout ==> Output quaternion rotation with no ZYX yaw +% EYaw ==> ZYX yaw of the input rotation +% QEYaw ==> Quaternion representation of the ZYX yaw component +% of the input rotation + +% Main function +function [Qout, EYaw, QEYaw] = QuatNoEYaw(Qin) + + % Calculate the ZYX yaw of the input + EYaw = atan2(2.0*(Qin(1)*Qin(4)+Qin(2)*Qin(3)),1.0-2.0*(Qin(3)*Qin(3)+Qin(4)*Qin(4))); + + % Construct the ZYX yaw component of the rotation + hcEYaw = cos(EYaw/2); + hsEYaw = sin(EYaw/2); + QEYaw = [hcEYaw 0 0 hsEYaw]; + + % Remove the ZYX yaw component of the rotation + Qout = hcEYaw*Qin + hsEYaw*[Qin(4) Qin(3) -Qin(2) -Qin(1)]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/QuatNoFYaw.m b/tro2022/transform/RotationsLib/QuatNoFYaw.m new file mode 100644 index 0000000..96adde6 --- /dev/null +++ b/tro2022/transform/RotationsLib/QuatNoFYaw.m @@ -0,0 +1,28 @@ +% QuatNoFYaw.m - Philipp Allgeuer - 05/11/14 +% Removes the fused yaw component of a quaternion rotation. +% +% function [Qout, FYaw, QFYaw] = QuatNoFYaw(Qin) +% +% Qin ==> Input quaternion rotation (assumed to have unit norm) +% Qout ==> Output quaternion rotation with zero fused yaw +% FYaw ==> Fused yaw of the input rotation +% QFYaw ==> Quaternion representation of the fused yaw component +% of the input rotation + +% Main function +function [Qout, FYaw, QFYaw] = QuatNoFYaw(Qin) + + % Calculate the fused yaw of the input + FYaw = 2.0*atan2(Qin(4),Qin(1)); + FYaw = wrap(FYaw); + + % Construct the fused yaw component of the rotation + hcFYaw = cos(FYaw/2); + hsFYaw = sin(FYaw/2); + QFYaw = [hcFYaw 0 0 hsFYaw]; + + % Remove the fused yaw component of the rotation + Qout = hcFYaw*Qin + hsFYaw*[Qin(4) Qin(3) -Qin(2) -Qin(1)]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/QuatRotVec.m b/tro2022/transform/RotationsLib/QuatRotVec.m new file mode 100644 index 0000000..970a3da --- /dev/null +++ b/tro2022/transform/RotationsLib/QuatRotVec.m @@ -0,0 +1,19 @@ +% QuatRotVec.m - Philipp Allgeuer - 05/11/14 +% Rotate a vector by a given quaternion rotation. +% +% function [Vout] = QuatRotVec(Quat, Vin) +% +% Quat ==> Quaternion rotation to apply to Vin (assumed to have unit norm) +% Vin ==> Input vector to rotate +% Vout ==> Rotated output vector + +% Main function +function [Vout] = QuatRotVec(Quat, Vin) + + % Rotate the vector as required + Vout = zeros(size(Vin)); + Vin = Vin(:)'; + Vout(:) = (Quat(1)*Quat(1) - Quat(2)*Quat(2) - Quat(3)*Quat(3) - Quat(4)*Quat(4))*Vin + 2*dot(Quat(2:4),Vin)*Quat(2:4) + 2*Quat(1)*cross(Quat(2:4),Vin); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/QuatRotVecPureZ.m b/tro2022/transform/RotationsLib/QuatRotVecPureZ.m new file mode 100644 index 0000000..4822759 --- /dev/null +++ b/tro2022/transform/RotationsLib/QuatRotVecPureZ.m @@ -0,0 +1,25 @@ +% QuatRotVecPureZ.m - Philipp Allgeuer - 20/03/17 +% Rotate a pure z vector by a given quaternion rotation. +% +% function [Vout] = QuatRotVecPureZ(Quat, Vinz) +% +% Quat ==> Quaternion rotation to apply to Vin (assumed to have unit norm) +% Vinz ==> Z component of the input vector to rotate i.e. [0 0 Vinz] +% Vout ==> Rotated output vector (column vector) +% +% This function is an optimisation of QuatRotVec(Quat, [0; 0; Vinz]). + +% Main function +function [Vout] = QuatRotVecPureZ(Quat, Vinz) + + % Data aliases + w = Quat(1); + x = Quat(2); + y = Quat(3); + z = Quat(4); + + % Calculate the output vector + Vout = Vinz*[2*(x*z+y*w); 2*(y*z-x*w); 1-2*(x*x+y*y)]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/QuatSlerp.m b/tro2022/transform/RotationsLib/QuatSlerp.m new file mode 100644 index 0000000..ac8d5fe --- /dev/null +++ b/tro2022/transform/RotationsLib/QuatSlerp.m @@ -0,0 +1,47 @@ +% QuatSlerp.m - Philipp Allgeuer - 03/12/14 +% Calculates spherical linear interpolation (slerp) for quaternions. +% +% function [Qu] = QuatSlerp(Q1, Q2, u) +% +% The output quaternion Qu is either on an interpolated path from Q1 to Q2, +% or Q1 to -Q2, depending on the sign of the dot product of the two quaternions. +% +% Q1 ==> Quaternion to interpolate from (u = 0) +% Q2 ==> Quaternion to interpolate to (u = 1) +% u ==> Dimensionless parameter at which to evaluate the interpolation +% Qu ==> Interpolated output quaternion + +% Main function +function [Qu] = QuatSlerp(Q1, Q2, u) + + % Ensure u is of the right shape for vectorisation + u = u(:); + + % Calculate the dot product of the two quaternions + dprod = Q1(1)*Q2(1) + Q1(2)*Q2(2) + Q1(3)*Q2(3) + Q1(4)*Q2(4); + if dprod < 0 + Q2 = -Q2; + dprod = -dprod; + end + + % If Q1 and Q2 are close enough together then just use linear interpolation with normalisation + if dprod >= 1 - 5e-9 % A dot product within this tolerance of unity produces a negligible amount of error if using linear interpolation instead + + % Perform the required interpolation + Qu = (1-u)*Q1 + u*Q2; + + else + + % Calculate half the angle between the two quaternions + htheta = acos(dprod); + + % Calculate the required interpolation + Qu = sin((1-u)*htheta)*Q1 + sin(u*htheta)*Q2; + + end + + % Normalise the output quaternion + Qu = Qu./repmat(sqrt(sum(Qu.^2,2)),1,4); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/RandAng.m b/tro2022/transform/RotationsLib/RandAng.m new file mode 100644 index 0000000..6165a02 --- /dev/null +++ b/tro2022/transform/RotationsLib/RandAng.m @@ -0,0 +1,47 @@ +% RandAng.m - Philipp Allgeuer - 05/11/14 +% Generates a random angle. +% +% function [Ang] = RandAng(N, Range) +% +% If Range is omitted then random angles in [-pi,pi] are generated. If Range is +% scalar then random angles in [0,Range] are generated. If Range is a vector of +% length 2 then random angles in [Range(1),Range(2)] are generated. If Range(2) +% is less than Range(1) then the two values are swapped. +% +% N ==> Number of random angles to generate (Default: 1) +% Range ==> Allowed range of the generated angles (Default: [-pi,pi]) +% Ang ==> Nx1 column vector of random angle(s) + +% Main function +function [Ang] = RandAng(N, Range) + + % Default arguments + if nargin < 1 + N = 1; + end + if nargin < 2 + Range = [-pi,pi]; + end + if ~isscalar(N) + error('N should be an integer scalar.'); + end + if N < 0 + N = 0; + end + if N ~= floor(N) + warning('N should be an integer scalar.'); + N = floor(N); + end + if isscalar(Range) + Range = [0,Range]; + end + if Range(2) < Range(1) + Range = [Range(2) Range(1)]; + end + + % Generate the required number of random angles + Diff = Range(2) - Range(1); + Ang = Range(1) + Diff*rand(N,1); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/RandEuler.m b/tro2022/transform/RotationsLib/RandEuler.m new file mode 100644 index 0000000..9c54f20 --- /dev/null +++ b/tro2022/transform/RotationsLib/RandEuler.m @@ -0,0 +1,31 @@ +% RandEuler.m - Philipp Allgeuer - 05/11/14 +% Generates random ZYX Euler angles rotations. +% +% function [E] = RandEuler(N) +% +% N ==> Number of random rotations to generate (Default: 1) +% E ==> Nx3 matrix of random ZYX Euler angles rotations + +% Main function +function [E] = RandEuler(N) + + % Default arguments + if nargin < 1 + N = 1; + end + if ~isscalar(N) + error('N should be an integer scalar.'); + end + if N < 0 + N = 0; + end + if N ~= floor(N) + warning('N should be an integer scalar.'); + N = floor(N); + end + + % Generate the required number of random rotations + E = (2*rand(N,3)-1)*(pi*diag([1 0.5 1])); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/RandFused.m b/tro2022/transform/RotationsLib/RandFused.m new file mode 100644 index 0000000..e1eac80 --- /dev/null +++ b/tro2022/transform/RotationsLib/RandFused.m @@ -0,0 +1,33 @@ +% RandFused.m - Philipp Allgeuer - 05/11/14 +% Generates random fused angles rotations. +% +% function [F] = RandFused(N) +% +% N ==> Number of random rotations to generate (Default: 1) +% F ==> Nx4 matrix of random fused angles rotations + +% Main function +function [F] = RandFused(N) + + % Default arguments + if nargin < 1 + N = 1; + end + if ~isscalar(N) + error('N should be an integer scalar.'); + end + if N < 0 + N = 0; + end + if N ~= floor(N) + warning('N should be an integer scalar.'); + N = floor(N); + end + + % Generate the required number of random rotations + yaw = pi*(2*rand(N,1)-1); + lam = 2*rand(N,2)-1; + F = [yaw (pi/4)*[lam(:,1)+lam(:,2) lam(:,1)-lam(:,2)] 2*(rand(N,1)>0.5)-1]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/RandQuat.m b/tro2022/transform/RotationsLib/RandQuat.m new file mode 100644 index 0000000..62ab1f5 --- /dev/null +++ b/tro2022/transform/RotationsLib/RandQuat.m @@ -0,0 +1,32 @@ +% RandQuat.m - Philipp Allgeuer - 05/11/14 +% Generates random quaternion rotations. +% +% function [Q] = RandQuat(N) +% +% N ==> Number of random rotations to generate (Default: 1) +% Q ==> Nx4 matrix of random quaternion rotations + +% Main function +function [Q] = RandQuat(N) + + % Default arguments + if nargin < 1 + N = 1; + end + if ~isscalar(N) + error('N should be an integer scalar.'); + end + if N < 0 + N = 0; + end + if N ~= floor(N) + warning('N should be an integer scalar.'); + N = floor(N); + end + + % Generate the required number of random rotations + Q = 2*rand(N,4)-1; + Q = Q./repmat(sqrt(sum(Q.^2,2)),1,4); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/RandRotmat.m b/tro2022/transform/RotationsLib/RandRotmat.m new file mode 100644 index 0000000..035ce73 --- /dev/null +++ b/tro2022/transform/RotationsLib/RandRotmat.m @@ -0,0 +1,35 @@ +% RandRotmat.m - Philipp Allgeuer - 05/11/14 +% Generates random rotation matrices. +% +% function [R] = RandRotmat(N) +% +% N ==> Number of random rotations to generate (Default: 1) +% R ==> 3x3xN matrix of random rotation matrices + +% Main function +function [R] = RandRotmat(N) + + % Default arguments + if nargin < 1 + N = 1; + end + if ~isscalar(N) + error('N should be an integer scalar.'); + end + if N < 0 + N = 0; + end + if N ~= floor(N) + warning('N should be an integer scalar.'); + N = floor(N); + end + + % Generate the required number of random rotations + Q = RandQuat(N); + R = zeros(3, 3, N); + for k = 1:N + R(:,:,k) = RotmatFromQuat(Q(k,:)); + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/RandTilt.m b/tro2022/transform/RotationsLib/RandTilt.m new file mode 100644 index 0000000..694cc7c --- /dev/null +++ b/tro2022/transform/RotationsLib/RandTilt.m @@ -0,0 +1,31 @@ +% RandTilt.m - Philipp Allgeuer - 05/11/14 +% Generates random tilt angles rotations. +% +% function [T] = RandTilt(N) +% +% N ==> Number of random rotations to generate (Default: 1) +% T ==> Nx3 matrix of random tilt angles rotations + +% Main function +function [T] = RandTilt(N) + + % Default arguments + if nargin < 1 + N = 1; + end + if ~isscalar(N) + error('N should be an integer scalar.'); + end + if N < 0 + N = 0; + end + if N ~= floor(N) + warning('N should be an integer scalar.'); + N = floor(N); + end + + % Generate the required number of random rotations + T = (2*rand(N,3)-1)*(pi*diag([1 1 0.5])) + repmat([0 0 0.5*pi],N,1); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/RandUnitVec.m b/tro2022/transform/RotationsLib/RandUnitVec.m new file mode 100644 index 0000000..6f83d9f --- /dev/null +++ b/tro2022/transform/RotationsLib/RandUnitVec.m @@ -0,0 +1,45 @@ +% RandUnitVec.m - Philipp Allgeuer - 05/11/14 +% Generates random M-vectors of unit norm. +% +% function [V] = RandUnitVec(N, M) +% +% N ==> Number of random unit vectors to generate (Default: 1) +% M ==> Required dimension of each of the generated vectors (Default: 3) +% V ==> MxN matrix of M-vectors expressed as column vectors + +% Main function +function [V] = RandUnitVec(N, M) + + % Default arguments + if nargin < 1 + N = 1; + end + if nargin < 2 + M = 3; + end + if ~isscalar(N) || ~isscalar(M) + error('N and M should both be integer scalars.'); + end + if N < 0 + N = 0; + end + if N ~= floor(N) + warning('N should be an integer scalar.'); + N = floor(N); + end + if M < 0 + M = 0; + end + if M ~= floor(M) + warning('M should be an integer scalar.'); + M = floor(M); + end + + % Generate the required number of random unit vectors + V = 2*rand(M,N) - 1; + Vnorm = sqrt(sum(V.^2,1)); + Vnorm(Vnorm == 0) = 1; + V = V./repmat(Vnorm,M,1); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/RandVec.m b/tro2022/transform/RotationsLib/RandVec.m new file mode 100644 index 0000000..87f81fb --- /dev/null +++ b/tro2022/transform/RotationsLib/RandVec.m @@ -0,0 +1,42 @@ +% RandVec.m - Philipp Allgeuer - 05/11/14 +% Generates random 3-vectors of up to a given norm. +% +% function [V] = RandVec(N, MaxNorm) +% +% N ==> Number of random vectors to generate (Default: 1) +% MaxNorm ==> Maximum norm of the generated vectors (i.e. norm range is [0,MaxNorm]) +% V ==> 3xN matrix of 3-vectors expressed as column vectors + +% Main function +function [V] = RandVec(N, MaxNorm) + + % Default arguments + if nargin < 1 + N = 1; + end + if nargin < 2 + MaxNorm = 1; + end + if ~isscalar(N) || ~isscalar(MaxNorm) + error('N and MaxNorm should each be scalar values.'); + end + if N < 0 + N = 0; + end + if N ~= floor(N) + warning('N should be an integer scalar.'); + N = floor(N); + end + if MaxNorm < 0 + MaxNorm = 0; + end + + % Generate the required number of random vectors + RealNorm = MaxNorm * rand(1,N); + V = 2*rand(3,N) - 1; + Vnorm = sqrt(V(1,:).^2 + V(2,:).^2 + V(3,:).^2); + Vnorm(Vnorm == 0) = 1; + V = V.*repmat(RealNorm./Vnorm,3,1); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/RotmatEqual.m b/tro2022/transform/RotationsLib/RotmatEqual.m new file mode 100644 index 0000000..000de96 --- /dev/null +++ b/tro2022/transform/RotationsLib/RotmatEqual.m @@ -0,0 +1,32 @@ +% RotmatEqual.m - Philipp Allgeuer - 05/11/14 +% Returns whether two rotation matrices represent the same rotation. +% +% function [Equal, Err] = RotmatEqual(Ra, Rb, Tol) +% +% Two rotation matrices are equivalent iff they are equal as matrices. +% Refer to EnsureRotmat for more information on rotation matrices. +% +% Ra ==> First rotation matrix to compare +% Rb ==> Second rotation matrix to compare +% Tol ==> Allowed tolerance between Ra and Rb for which equality is still asserted +% Equal ==> Boolean flag whether Ra equals Rb to the given L_inf norm tolerance +% Err ==> The quantified error between Ra and Rb (0 if exactly equal) + +% Main function +function [Equal, Err] = RotmatEqual(Ra, Rb, Tol) + + % Default tolerance + if nargin < 3 + Tol = 1e-10; + else + Tol = abs(Tol); + end + + % Calculate the deviation between the two rotations + Err = max(abs(Ra(:)-Rb(:))); + + % Check whether the two rotations are within tolerance + Equal = (Err <= Tol); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/RotmatFromAxis.m b/tro2022/transform/RotationsLib/RotmatFromAxis.m new file mode 100644 index 0000000..5a31dee --- /dev/null +++ b/tro2022/transform/RotationsLib/RotmatFromAxis.m @@ -0,0 +1,69 @@ +% RotmatFromAxis.m - Philipp Allgeuer - 05/11/14 +% Constructs a rotation matrix based on an axis-angle specification. +% +% function [Rotmat] = RotmatFromAxis(Axis, Angle) +% +% Axis ==> The vector corresponding to the axis of rotation (magnitude is unimportant) +% either given as the vector [ux uy uz] or one of {'x', 'y', 'z'} +% Angle ==> The angle of the rotation given by the right hand rule +% Rotmat ==> Equivalent rotation matrix + +% Main function +function [Rotmat] = RotmatFromAxis(Axis, Angle) + + % Handle case of missing arguments + if nargin < 2 + Rotmat = eye(3); + return; + end + + % Precompute the required sin and cos terms + cang = cos(Angle); + sang = sin(Angle); + + % Handle case of standard axis rotations + if strcmpi(Axis, 'x') + Rotmat = [1 0 0;0 cang -sang;0 sang cang]; + return; + elseif strcmpi(Axis, 'y') + Rotmat = [cang 0 sang;0 1 0;-sang 0 cang]; + return; + elseif strcmpi(Axis, 'z') + Rotmat = [cang -sang 0;sang cang 0;0 0 1]; + return; + end + + % Axis error checking + if ~(isnumeric(Axis) && isvector(Axis) && length(Axis) == 3) + error('Axis must be a 3-vector, or one of {''x'',''y'',''z''}.'); + end + + % Normalise the axis vector + AxisNorm = norm(Axis); + if AxisNorm <= 0 + Rotmat = eye(3); % Return the identity rotation if Axis is the zero vector + return; + else + Axis = Axis/AxisNorm; + end + + % Precompute the required terms + C = 1 - cang; + xC = Axis(1)*C; + yC = Axis(2)*C; + zC = Axis(3)*C; + xxC = Axis(1)*xC; + yyC = Axis(2)*yC; + zzC = Axis(3)*zC; + xyC = Axis(1)*yC; + yzC = Axis(2)*zC; + zxC = Axis(3)*xC; + xs = Axis(1)*sang; + ys = Axis(2)*sang; + zs = Axis(3)*sang; + + % Construct the required rotation + Rotmat = [xxC+cang xyC-zs zxC+ys;xyC+zs yyC+cang yzC-xs;zxC-ys yzC+xs zzC+cang]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/RotmatFromEuler.m b/tro2022/transform/RotationsLib/RotmatFromEuler.m new file mode 100644 index 0000000..ada8b43 --- /dev/null +++ b/tro2022/transform/RotationsLib/RotmatFromEuler.m @@ -0,0 +1,32 @@ +% RotmatFromEuler.m - Philipp Allgeuer - 05/11/14 +% Converts a ZYX Euler angles rotation to the corresponding rotation matrix representation. +% +% function [Rotmat] = RotmatFromEuler(Euler) +% +% The output of this function is a 3x3 matrix that to machine precision is orthogonal. +% Note however, that it cannot be assumed that every element of Rotmat is strictly and +% irrefutably in the range [-1,1]. It is possible that values such as 1+k*eps come out, +% and so care should be taken when applying (for instance) inverse trigonometric functions +% (e.g. asin, acos) to the rotation matrix elements, as otherwise complex values may result. +% +% Euler ==> Input ZYX Euler angles rotation +% Rotmat ==> Equivalent rotation matrix + +% Main function +function [Rotmat] = RotmatFromEuler(Euler) + + % Precalculate the sin and cos values + cpsi = cos(Euler(1)); + cth = cos(Euler(2)); + cphi = cos(Euler(3)); + spsi = sin(Euler(1)); + sth = sin(Euler(2)); + sphi = sin(Euler(3)); + + % Calculate the required rotation matrix + Rotmat = [cpsi*cth cpsi*sth*sphi-spsi*cphi cpsi*sth*cphi+spsi*sphi; + spsi*cth spsi*sth*sphi+cpsi*cphi spsi*sth*cphi-cpsi*sphi; + -sth cth*sphi cth*cphi]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/RotmatFromFYawBzG.m b/tro2022/transform/RotationsLib/RotmatFromFYawBzG.m new file mode 100644 index 0000000..cd7f32b --- /dev/null +++ b/tro2022/transform/RotationsLib/RotmatFromFYawBzG.m @@ -0,0 +1,19 @@ +% RotmatFromFYawBzG - Philipp Allgeuer - 20/10/16 +% Calculates the rotation matrix (RGB) with a given fused yaw and global z-axis in body-fixed coordinates (BzG). +% +% function [RGB, qGB] = RotmatFromFYawBzG(FYaw, BzG) +% +% It is assumed that BzG is a unit vector! +% BzG by definition will always be the third row of RGB. + +% Main function +function [RGB, qGB] = RotmatFromFYawBzG(FYaw, BzG) + + % Calculate the quaternion representation of the required rotation + qGB = QuatFromFYawBzG(FYaw, BzG); + + % Return the required rotation matrix representation + RGB = RotmatFromQuat(qGB); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/RotmatFromFYawGzB.m b/tro2022/transform/RotationsLib/RotmatFromFYawGzB.m new file mode 100644 index 0000000..7be04a1 --- /dev/null +++ b/tro2022/transform/RotationsLib/RotmatFromFYawGzB.m @@ -0,0 +1,19 @@ +% RotmatFromFYawGzB - Philipp Allgeuer - 26/10/16 +% Calculates the rotation matrix (RGB) with a given fused yaw and body-fixed z-axis in global coordinates (GzB). +% +% function [RGB, qGB] = RotmatFromFYawGzB(FYaw, GzB) +% +% It is assumed that GzB is a unit vector! +% GzB by definition will always be the third column of RGB. + +% Main function +function [RGB, qGB] = RotmatFromFYawGzB(FYaw, GzB) + + % Calculate the quaternion representation of the required rotation + qGB = QuatFromFYawGzB(FYaw, GzB); + + % Return the required rotation matrix representation + RGB = RotmatFromQuat(qGB); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/RotmatFromFused.m b/tro2022/transform/RotationsLib/RotmatFromFused.m new file mode 100644 index 0000000..8ace2a2 --- /dev/null +++ b/tro2022/transform/RotationsLib/RotmatFromFused.m @@ -0,0 +1,64 @@ +% RotmatFromFused.m - Philipp Allgeuer - 05/11/14 +% Converts a fused angles rotation to the corresponding rotation matrix representation. +% +% function [Rotmat, Tilt] = RotmatFromFused(Fused) +% +% The output of this function is a 3x3 matrix that to machine precision is orthogonal. +% Note however, that it cannot be assumed that every element of Rotmat is strictly and +% irrefutably in the range [-1,1]. It is possible that values such as 1+k*eps come out, +% and so care should be taken when applying (for instance) inverse trigonometric functions +% (e.g. asin, acos) to the rotation matrix elements, as otherwise complex values may result. +% +% Fused ==> Input fused angles rotation +% Rotmat ==> Equivalent rotation matrix +% Tilt ==> Equivalent tilt angles rotation + +% Main function +function [Rotmat, Tilt] = RotmatFromFused(Fused) + + % Precalculate the sin values + sth = sin(Fused(2)); + sphi = sin(Fused(3)); + + % Calculate the sin and cos of the tilt angle + crit = sth*sth + sphi*sphi; + if crit >= 1.0 + salpha = 1.0; + calpha = 0.0; + else + salpha = sqrt(crit); + if Fused(4) >= 0 + calpha = sqrt(1-crit); + else + calpha = -sqrt(1-crit); + end + end + + % Calculate the tilt axis angle gamma + gamma = atan2(sth,sphi); + + % Precalculate trigonometric terms involved in the rotation matrix expression + cgam = cos(gamma); % Equals sphi/salpha + sgam = sin(gamma); % Equals sth/salpha + psigam = Fused(1) + gamma; + cpsigam = cos(psigam); + spsigam = sin(psigam); + + % Precalculate additional terms involved in the rotation matrix expression + A = cgam * cpsigam; + B = sgam * cpsigam; + C = cgam * spsigam; + D = sgam * spsigam; + + % Calculate the required rotation matrix + Rotmat = [A+D*calpha B-C*calpha salpha*spsigam; + C-B*calpha D+A*calpha -salpha*cpsigam; + -sth sphi calpha]; + + % Calculate the required tilt angles representation + if nargout >= 2 + Tilt = [Fused(1) gamma acos(calpha)]; + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/RotmatFromQuat.m b/tro2022/transform/RotationsLib/RotmatFromQuat.m new file mode 100644 index 0000000..7270feb --- /dev/null +++ b/tro2022/transform/RotationsLib/RotmatFromQuat.m @@ -0,0 +1,30 @@ +% RotmatFromQuat.m - Philipp Allgeuer - 05/11/14 +% Converts a quaternion rotation to the corresponding rotation matrix representation. +% +% function [Rotmat] = RotmatFromQuat(Quat) +% +% The output of this function is a 3x3 matrix that to machine precision is orthogonal. +% Note however, that it cannot be assumed that every element of Rotmat is strictly and +% irrefutably in the range [-1,1]. It is possible that values such as 1+k*eps come out, +% and so care should be taken when applying (for instance) inverse trigonometric functions +% (e.g. asin, acos) to the rotation matrix elements, as otherwise complex values may result. +% +% Quat ==> Input quaternion rotation (assumed to have unit norm) +% Rotmat ==> Equivalent rotation matrix + +% Main function +function [Rotmat] = RotmatFromQuat(Quat) + + % Data aliases + w = Quat(1); + x = Quat(2); + y = Quat(3); + z = Quat(4); + + % Construct the required rotation matrix (assuming the quaternion is normalised) + Rotmat = [1-2*(y*y+z*z) 2*(x*y-z*w) 2*(x*z+y*w) ; + 2*(x*y+z*w) 1-2*(x*x+z*z) 2*(y*z-x*w) ; + 2*(x*z-y*w) 2*(y*z+x*w) 1-2*(x*x+y*y)]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/RotmatFromTilt.m b/tro2022/transform/RotationsLib/RotmatFromTilt.m new file mode 100644 index 0000000..3b7a1c8 --- /dev/null +++ b/tro2022/transform/RotationsLib/RotmatFromTilt.m @@ -0,0 +1,39 @@ +% RotmatFromTilt.m - Philipp Allgeuer - 05/11/14 +% Converts a tilt angles rotation to the corresponding rotation matrix representation. +% +% function [Rotmat] = RotmatFromTilt(Tilt) +% +% The output of this function is a 3x3 matrix that to machine precision is orthogonal. +% Note however, that it cannot be assumed that every element of Rotmat is strictly and +% irrefutably in the range [-1,1]. It is possible that values such as 1+k*eps come out, +% and so care should be taken when applying (for instance) inverse trigonometric functions +% (e.g. asin, acos) to the rotation matrix elements, as otherwise complex values may result. +% +% Tilt ==> Input tilt angles rotation +% Rotmat ==> Equivalent rotation matrix + +% Main function +function [Rotmat] = RotmatFromTilt(Tilt) + + % Precalculate the required trigonometric terms + cgam = cos(Tilt(2)); + sgam = sin(Tilt(2)); + calpha = cos(Tilt(3)); + salpha = sin(Tilt(3)); + psigam = Tilt(1) + Tilt(2); + cpsigam = cos(psigam); + spsigam = sin(psigam); + + % Precalculate additional terms involved in the rotation matrix expression + A = cgam * cpsigam; + B = sgam * cpsigam; + C = cgam * spsigam; + D = sgam * spsigam; + + % Calculate the required rotation matrix + Rotmat = [ A+D*calpha B-C*calpha salpha*spsigam; + C-B*calpha D+A*calpha -salpha*cpsigam; + -salpha*sgam salpha*cgam calpha ]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/RotmatFromZVec.m b/tro2022/transform/RotationsLib/RotmatFromZVec.m new file mode 100644 index 0000000..417eefa --- /dev/null +++ b/tro2022/transform/RotationsLib/RotmatFromZVec.m @@ -0,0 +1,22 @@ +% RotmatFromZVec.m - Philipp Allgeuer - 21/10/16 +% Converts a z-vector to the corresponding zero fused yaw rotation matrix representation. +% +% function [Rotmat, Quat] = RotmatFromZVec(ZVec) +% +% It is assumed that ZVec is a unit vector! +% +% ZVec ==> Input z-vector +% Rotmat ==> Corresponding zero fused yaw rotation matrix +% Quat ==> Corresponding zero fused yaw quaternion + +% Main function +function [Rotmat, Quat] = RotmatFromZVec(ZVec) + + % Calculate the quaternion representation of the required rotation + Quat = QuatFromZVec(ZVec); + + % Return the required rotation matrix representation + Rotmat = RotmatFromQuat(Quat); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/RotmatIdentity.m b/tro2022/transform/RotationsLib/RotmatIdentity.m new file mode 100644 index 0000000..e319284 --- /dev/null +++ b/tro2022/transform/RotationsLib/RotmatIdentity.m @@ -0,0 +1,15 @@ +% RotmatIdentity.m - Philipp Allgeuer - 05/11/14 +% Returns the identity rotation matrix. +% +% function [RI] = RotmatIdentity() +% +% RI ==> Identity rotation in the rotation matrix representation + +% Main function +function [RI] = RotmatIdentity() + + % Return the required identity rotation + RI = eye(3); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/RotmatInv.m b/tro2022/transform/RotationsLib/RotmatInv.m new file mode 100644 index 0000000..6be58b5 --- /dev/null +++ b/tro2022/transform/RotationsLib/RotmatInv.m @@ -0,0 +1,19 @@ +% RotmatInv.m - Philipp Allgeuer - 05/11/14 +% Calculates the inverse of a rotation matrix. +% +% function [Rinv] = RotmatInv(R) +% +% Note that the calculation internally is merely transposition, so the input +% rotation matrix must be valid itself for the output to be valid. +% +% R ==> Input rotation matrix +% Rinv ==> Inverse rotation matrix (i.e. the transpose) + +% Main function +function [Rinv] = RotmatInv(R) + + % Calculate the inverse/transpose of the rotation matrix + Rinv = R'; % Assumes the rotation matrix is valid + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/RotmatNoEYaw.m b/tro2022/transform/RotationsLib/RotmatNoEYaw.m new file mode 100644 index 0000000..2f03945 --- /dev/null +++ b/tro2022/transform/RotationsLib/RotmatNoEYaw.m @@ -0,0 +1,27 @@ +% RotmatNoEYaw.m - Philipp Allgeuer - 05/11/14 +% Removes the ZYX yaw component of a rotation matrix. +% +% function [Rout, EYaw, REYaw] = RotmatNoEYaw(Rin) +% +% Rin ==> Input rotation matrix +% Rout ==> Output rotation matrix with no ZYX yaw +% EYaw ==> ZYX yaw of the input rotation +% REYaw ==> Rotation matrix representation of the ZYX yaw component +% of the input rotation + +% Main function +function [Rout, EYaw, REYaw] = RotmatNoEYaw(Rin) + + % Calculate the ZYX yaw of the input + EYaw = atan2(Rin(2,1),Rin(1,1)); + + % Construct the ZYX yaw component of the rotation + cEYaw = cos(EYaw); + sEYaw = sin(EYaw); + REYaw = [cEYaw -sEYaw 0;sEYaw cEYaw 0;0 0 1]; + + % Remove the ZYX yaw component of the rotation + Rout = REYaw' * Rin; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/RotmatNoFYaw.m b/tro2022/transform/RotationsLib/RotmatNoFYaw.m new file mode 100644 index 0000000..d3c2731 --- /dev/null +++ b/tro2022/transform/RotationsLib/RotmatNoFYaw.m @@ -0,0 +1,27 @@ +% RotmatNoFYaw.m - Philipp Allgeuer - 05/11/14 +% Removes the fused yaw component of a rotation matrix. +% +% function [Rout, FYaw, RFYaw] = RotmatNoFYaw(Rin) +% +% Rin ==> Input rotation matrix +% Rout ==> Output rotation matrix with zero fused yaw +% FYaw ==> Fused yaw of the input rotation +% RFYaw ==> Rotation matrix representation of the fused yaw component +% of the input rotation + +% Main function +function [Rout, FYaw, RFYaw] = RotmatNoFYaw(Rin) + + % Calculate the fused yaw of the input + FYaw = FYawOfRotmat(Rin); + + % Construct the fused yaw component of the rotation + cFYaw = cos(FYaw); + sFYaw = sin(FYaw); + RFYaw = [cFYaw -sFYaw 0;sFYaw cFYaw 0;0 0 1]; + + % Remove the fused yaw component of the rotation + Rout = RFYaw' * Rin; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/RotmatRotVec.m b/tro2022/transform/RotationsLib/RotmatRotVec.m new file mode 100644 index 0000000..309fcbb --- /dev/null +++ b/tro2022/transform/RotationsLib/RotmatRotVec.m @@ -0,0 +1,18 @@ +% RotmatRotVec.m - Philipp Allgeuer - 05/11/14 +% Rotate a vector by a given rotation matrix. +% +% function [Vout] = RotmatRotVec(Rotmat, Vin) +% +% Rotmat ==> Rotation matrix to apply to Vin +% Vin ==> Input vector to rotate +% Vout ==> Rotated output vector + +% Main function +function [Vout] = RotmatRotVec(Rotmat, Vin) + + % Rotate the vector as required + Vout = zeros(size(Vin)); + Vout(:) = Rotmat * Vin(:); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/BeginBoolean.m b/tro2022/transform/RotationsLib/Test/BeginBoolean.m new file mode 100644 index 0000000..61b0a16 --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/BeginBoolean.m @@ -0,0 +1,15 @@ +% BeginBoolean.m - Philipp Allgeuer - 05/11/14 +% Convenience function for testing boolean conditions. +% +% function [B] = BeginBoolean() +% +% B ==> Boolean flag initialised to true + +% Main function +function [B] = BeginBoolean() + + % Set the output boolean flag to true + B = true; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/BeginTest.m b/tro2022/transform/RotationsLib/Test/BeginTest.m new file mode 100644 index 0000000..703d98b --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/BeginTest.m @@ -0,0 +1,50 @@ +% BeginTest.m - Philipp Allgeuer - 05/11/14 +% Function to be called at the beginning of an individual test. +% +% function [N, varargout] = BeginTest(Title, N, ErrSize) +% +% Individual tests are intended to be combined into test scripts. Each individual +% test should return PASS/true on completion if the test was successful. +% +% Title ==> The name to use for this test +% N (in) ==> The number of test cases to use in the test +% ErrSize ==> A vector of values specifying the required number of columns in each error array +% N (out) ==> The number of test cases to use in the test (unmodified from the input) +% varargout ==> The zero-initialised error arrays (NxM where the M values come from the ErrSize input). +% If more outputs are present than numel(ErrSize), the additional arrays default to +% being of width 1 (column vectors). + +% Main function +function [N, varargout] = BeginTest(Title, N, ErrSize) + + % Start timing + tic; + + % Print a test header + fprintf('----------------------------------------------------------------------\n'); + fprintf('TEST: %s\n\n', Title); + + % Initialise the required error arrays with zeros + if nargin >= 2 + N = round(N); + end + if nargin < 3 + ErrSize = []; + end + for k = 1:(nargout-1) + if k <= numel(ErrSize) + varargout{k} = zeros(N,ErrSize(k)); + else + varargout{k} = zeros(N,1); + end + end + + % Flush the printed output to screen + if isOctave + fflush(stdout); + else + drawnow('update'); + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/BeginTestScript.m b/tro2022/transform/RotationsLib/Test/BeginTestScript.m new file mode 100644 index 0000000..09734f5 --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/BeginTestScript.m @@ -0,0 +1,45 @@ +% BeginTestScript.m - Philipp Allgeuer - 05/11/14 +% Function to be called at the beginning of a test script. +% +% function [Pass, Nnormal, Nlarge, Nsmall, Ntiny] = BeginTestScript(Title, N, Tol) +% +% Each test script may contain a multitude of individual tests. If each individual +% test returns PASS/true, then the entire test script returns PASS/true. +% +% Title ==> The name to use for this test script +% N ==> The nominal number of test cases to use in each individual test +% Tol ==> The nominal numerical bound below which errors are acceptable +% Pass ==> A pass flag to use that has been already initialised to true (use as Pass = Pass & EndTest(...)) +% Nnormal ==> The number of test cases to use for normal tests +% Nlarge ==> The number of test cases to use for fast tests +% Nsmall ==> The number of test cases to use for slow tests +% Ntiny ==> The number of test cases to use for ultraslow tests + +% Main function +function [Pass, Nnormal, Nlarge, Nsmall, Ntiny] = BeginTestScript(Title, N, Tol) + + % Print header + fprintf('======================================================================\n'); + fprintf('TEST SCRIPT: %s\n', Title); + fprintf('N = %d\n',N); + fprintf('Tol = %.3e\n',Tol); + fprintf('\n'); + + % Work out the various options for N + Nlarge = max(round(10*N),5); + Nnormal = max(round(N),5); + Nsmall = max(round(N/10),5); + Ntiny = max(round(N/300),5); + + % Initialise the pass flag + Pass = true; + + % Flush the printed output to screen + if isOctave + fflush(stdout); + else + drawnow('update'); + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/EndBoolean.m b/tro2022/transform/RotationsLib/Test/EndBoolean.m new file mode 100644 index 0000000..2ba7e94 --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/EndBoolean.m @@ -0,0 +1,25 @@ +% EndBoolean.m - Philipp Allgeuer - 05/11/14 +% Convenience function for testing boolean conditions. +% +% function [B] = EndBoolean(B) +% +% The value of the B input (passed unmodified out of the function) should +% be a boolean specifying whether all boolean conditions were true in a test. +% +% B (in) ==> Boolean flag whether all boolean tests were passed +% B (out) ==> Boolean flag whether all boolean tests were passed + +% Main function +function [B] = EndBoolean(B) + + % Print a message whether the boolean conditions were satisfied + if B + fprintf('All boolean conditions passed! * PASS *\n'); + else + fprintf('Failed due to boolean conditions! * FAIL *\n'); + warning('Boolean test(s) failed!'); + end + fprintf('\n'); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/EndTest.m b/tro2022/transform/RotationsLib/Test/EndTest.m new file mode 100644 index 0000000..01b1c4f --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/EndTest.m @@ -0,0 +1,47 @@ +% EndTest.m - Philipp Allgeuer - 05/11/14 +% Function to be called at the end of an individual test. +% +% function [Pass] = EndTest(Tol, varargin) +% +% Individual tests are intended to be combined into test scripts. Each individual +% test should return PASS/true on completion if the test was successful. +% +% Tol ==> The numerical bound below which errors in this test are acceptable +% varargin ==> Variable number of error arrays (column vectors or matrices) to display and check +% The order is 'name1', Err1, 'name2', Err2, ... +% Pass ==> Boolean flag whether this test was passed + +% Main function +function [Pass] = EndTest(Tol, varargin) + + % Default tolerance value + if nargin < 1 || Tol <= 0 + Tol = 128*eps; + end + + % Print the error statistics and see whether the test was passed + Pass = true; + for k = 1:2:(nargin-2) + Flag = PrintErrStats(varargin{k+1}, varargin{k}, Tol); + Pass = Pass & Flag; + if ~Flag + warning('Error test(s) failed!'); + disp(' '); + end + end + + % Stop timing + toc; + + % Trailing display + fprintf('\n'); + + % Flush the printed output to screen + if isOctave + fflush(stdout); + else + drawnow('update'); + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/EndTestScript.m b/tro2022/transform/RotationsLib/Test/EndTestScript.m new file mode 100644 index 0000000..ab96940 --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/EndTestScript.m @@ -0,0 +1,35 @@ +% EndTestScript.m - Philipp Allgeuer - 05/11/14 +% Function to be called at the end of a test script. +% +% function EndTestScript(Title, Pass) +% +% Each test script may contain a multitude of individual tests. If each individual +% test returns PASS/true, then the entire test script returns PASS/true. +% +% Title ==> The name to use for this test script +% Pass ==> Boolean flag specifying whether all the individual tests were passed + +% Main function +function EndTestScript(Title, Pass) + + % Print a summary of the test script results + fprintf('----------------------------------------------------------------------\n'); + fprintf('TEST SCRIPT: %s\n', Title); + if Pass + fprintf('Total result: * PASS * * PASS *\n'); + else + fprintf('Total result: * FAIL * * FAIL *\n'); + warning('Test script failed!'); + end + fprintf('Completed!\n'); + fprintf('\n'); + + % Flush the printed output to screen + if isOctave + fflush(stdout); + else + drawnow('update'); + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/PrintErrStats.m b/tro2022/transform/RotationsLib/Test/PrintErrStats.m new file mode 100644 index 0000000..4a3f0a4 --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/PrintErrStats.m @@ -0,0 +1,95 @@ +% PrintErrStats.m - Philipp Allgeuer - 05/11/14 +% Prints statistics about an error array that was generated as part of a test item. +% +% function [Pass] = PrintErrStats(Err, Title, Tol) +% +% Test items are the constituents of an individual test. If all of the errors are +% below the specified tolerance in absolute terms the test item is declared passed. +% +% Err ==> The error data to print the statistics of +% Title ==> The name to use for this test item +% Tol ==> The numerical bound below which errors are acceptable +% Pass ==> Boolean flag whether this test item was passed + +% Main function +function [Pass] = PrintErrStats(Err, Title, Tol) + + % Default tolerance value + if nargin < 3 + Tol = 128*eps; + end + + % Initialise the pass flag + Pass = false; + + % Print header for the results + fprintf('TEST ITEM: %s\n', Title); + + % Ensure that error data is actually available + if size(Err,1) < 2 + fprintf('WARNING: No error data was available (need >1 rows)! * FAIL *\n'); + warning('A warning occurred while trying to print out the error statistics.'); + return; + end + + % Calculate the error maxima and minima + abserr = abs(Err); + absmin = min(abserr); + absmax = max(abserr); + maxabsmax = max(absmax); + + % Evaluate the pass criterion + Pass = (maxabsmax <= Tol); + if Pass + str = 'PASS'; + else + str = 'FAIL'; + end + + % Print the error maxima and minima, and whether the test was passed + fprintf('Err minimum: %s\n', sprintf('%11.3e ',min(Err))); + fprintf('Err maximum: %s\n', sprintf('%11.3e ',max(Err))); + fprintf('Err abs minimum: %s\n', sprintf('%11.3e ',absmin)); + fprintf('Err abs maximum: %s <--- ABS MAX: %11.3e * %s *\n', sprintf('%11.3e ',absmax),maxabsmax,str); + + % Partition the error values into logarithmically spaced bins + bins = [0 10.^(-15:0) Inf]; + count = histc(abserr,bins,1); + while all(count(end,:) == 0) + count(end,:) = []; + end + bincumul = cumsum(count,1); + + % Issue warnings if we detect infinite and/or NaN values + warnhappened = false; + total = size(Err,1); + if ~all(isfinite(Err)) + fprintf('WARNING: There are non-finite values in the array!\n'); + warnhappened = true; + end + if any(bincumul(end,:) ~= total) + fprintf('WARNING: Sums don''t match => There be NaNs!\n'); + warnhappened = true; + end + if warnhappened + warning('A warning occurred while trying to print out the error statistics.'); + end + + % Print out a list of the error bins and the corresponding exceedance frequencies + fprintf('Less than %5.0e: %s\n', bins(2), sprintf('%d ',bincumul(1,:))); + for k = 1:size(bincumul,1) + if k+1 <= numel(bins) + fprintf('More than %5.0e: %s\n', bins(k+1), sprintf('%d ',total-bincumul(k,:))); + end + end + fprintf('\n'); + + % Flush the printed output to screen + if isOctave + fflush(stdout); + else + drawnow('update'); + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/RunAllTests.m b/tro2022/transform/RotationsLib/Test/RunAllTests.m new file mode 100644 index 0000000..10915dd --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/RunAllTests.m @@ -0,0 +1,186 @@ +% RunAllTests.m - Philipp Allgeuer - 05/11/14 +% Run all available/known test scripts of the Rotations Library. +% +% function [Pass] = RunAllTests(Speed, Inter) +% +% This function executes a collection of test scripts (e.g. TestRand, TestCompose, etc), +% each containing some number of individual tests. The number of test cases N used for +% each individual test script should be kept to a number such that, as a rule of thumb, +% each of the tests that constitute a test script take approximately 10s to execute in +% normal speed mode, or quicker if time is not a constraint in a particular test. If a +% test script fails then all remaining test scripts are still executed. +% +% Speed ==> One of 'Fast', 'Normal', 'Slow' or 'Ultraslow' (string) +% Inter ==> Boolean flag whether to also run interactive tests +% Pass ==> Boolean flag whether all tests were passed + +% Main function +function [Pass] = RunAllTests(Speed, Inter) + + % Process input parameters + if nargin < 1 + Speed = 'Normal'; + end + if nargin < 2 || ~isscalar(Inter) || ~islogical(Inter) + Inter = false; + end + + % Process speed parameter + switch lower(Speed) + case 'fast' + S = 1; + case 'normal' + S = 5; + case 'slow' + S = 20; + case 'ultraslow' + S = 50; + case 'timewarpslow' % Intended for developer only... + S = 500; + otherwise + error('Unrecognised speed parameter!'); + end + + % Print a header + fprintf('######################################################################\n'); + fprintf('################## Rotations Library => Run Tests ##################\n'); + fprintf('######################################################################\n'); + fprintf('Speed: %s\n',Speed); + if Inter + fprintf('Interactive tests: True\n'); + else + fprintf('Interactive tests: False\n'); + end + fprintf('\n'); + + % Initialise the pass flag + P = true; + + % Error tolerances to choose from for each test script + LTol = 2048*eps; + MTol = 256*eps; + HTol = 32*eps; + + % Retrieve the start time + StartTime = tic; + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Start of testing %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + % Tests: *Identity + % Assumes: None + P = P & TestIdentity(1*S, HTol, Inter); + + % Tests: Rand*, RandAng, RandVec, RandUnitVec + % Assumes: None + P = P & TestRand(10000*S, MTol, Inter); + + % Tests: PlotCsys + % Assumes: Rand* + P = P & TestPlotCsys(1*S, MTol, Inter); + + % Tests: *Equal, Ensure* + % Assumes: Rand* + P = P & TestEqual(600*S, MTol, Inter); + + % Tests: *From* + % Assumes: Rand*, *Equal + P = P & TestConversions(250*S, HTol, Inter); + + % Tests: Compose* + % Assumes: Rand*, *Equal, RotmatFrom*, *FromRotmat + P = P & TestCompose(500*S, HTol, Inter); + + % Tests: *FromAxis + % Assumes: RandVec, RandAng, *Equal, *FromQuat + P = P & TestFromAxis(600*S, MTol, Inter); + + % Tests: *Inv + % Assumes: Rand*, QuatFrom*, Compose*, *Equal + P = P & TestInv(1000*S, LTol, Inter); + + % Tests: QuatSlerp + % Assumes: RandQuat, ComposeQuat, QuatInv + P = P & TestSlerp(200*S, HTol, Inter); + + % Tests: *NoEYaw + % Assumes: Rand*, *Equal, EulerFrom*, RotmatFromQuat + P = P & TestNoEYaw(1600*S, MTol, Inter); + + % Tests: *NoFYaw + % Assumes: Rand*, FusedEqual, FusedFrom* + P = P & TestNoFYaw(1500*S, MTol, Inter); + + % Tests: *RotVec + % Assumes: Rand*, RandVec, RotmatFrom* + P = P & TestRotVec(700*S, MTol, Inter); + + % Tests: EYawOf* + % Assumes: Rand*, EulerFrom* + P = P & TestEYawOf(3000*S, HTol, Inter); + + % Tests: FYawOf* + % Assumes: Rand*, FusedFrom* + P = P & TestFYawOf(3000*S, HTol, Inter); + + % Tests: ZVecFrom* + % Assumes: Rand*, RotmatFrom* + P = P & TestZVecFrom(3000*S, HTol, Inter); + + % Tests: FromZVec* + % Assumes: RandUnitVec, ZVecFrom*, FYawOf* + P = P & TestFromZVec(3000*S, HTol, Inter); + + % Tests: *FromFYawBzG + % Assumes: RandUnitVec, FYawOf*, ZVecFrom* + P = P & TestFromFYawBzG(3000*S, LTol, Inter); + + % Tests: *FromFYawGzB + % Assumes: RandUnitVec, FYawOf*, RotmatFrom* + P = P & TestFromFYawGzB(3000*S, LTol, Inter); + + % Tests: AngVelFrom* + % Assumes: Rand*, RandUnitVec, QuatFrom*, QuatInv, ComposeQuat + P = P & TestAngVelFrom(650*S, MTol, Inter); + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% End of testing %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + % Calculate the total testing time + if isOctave + TimeTested = double(tic - StartTime) * 1e-6; + else + TimeTested = toc(StartTime); + end + Hours = floor(TimeTested/3600); + TimeTested = TimeTested - Hours*3600; + Mins = floor(TimeTested/60); + Secs = TimeTested - Mins*60; + + % Display the total testing time + fprintf('##############################\n'); + fprintf('Total time taken: %02d:%02d:%06.3f\n', Hours, Mins, Secs); + fprintf('\n'); + + % Convert the pass flag into a string + if P + PStr = 'PASS'; + else + PStr = 'FAIL'; + end + + % Summarise the test results + fprintf('######################################################################\n'); + fprintf('########### End of Rotations Library testing => * %s * ###########\n', PStr); + fprintf('######################################################################\n'); + fprintf('\n'); + + % Set the output pass flag + if nargout >= 1 + Pass = P; + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/TestAngVelFrom.m b/tro2022/transform/RotationsLib/Test/TestAngVelFrom.m new file mode 100644 index 0000000..3640067 --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/TestAngVelFrom.m @@ -0,0 +1,71 @@ +% TestAngVelFrom.m - Philipp Allgeuer - 05/11/14 +% Tests: AngVelFrom* +% Assumes: Rand*, RandUnitVec, QuatFrom*, QuatInv, ComposeQuat +% +% function [Pass] = TestAngVelFrom(N, Tol, Inter) +% +% N ==> Number of test cases to use in each test +% Tol ==> Numeric tolerance to use for testing +% Inter ==> Boolean flag whether to also run interactive tests +% Pass ==> Boolean flag whether all tests were passed + +% Main function +function [Pass] = TestAngVelFrom(N, Tol, Inter) + + % Process function inputs + if nargin < 1 || ~isscalar(N) || N < 1 + N = 650; + end + N = min(round(N),1000000); + if nargin < 2 || Tol <= 0 + Tol = 128*eps; + end + + % Begin test script + [P, Nnormal] = BeginTestScript('TestAngVelFrom', N, Tol); + + % + % Test AngVelFromEulerVel + % + + % Begin test + [N, ErrA] = BeginTest('AngVelFromEulerVel', Nnormal); + B = BeginBoolean(); + + % Perform the required testing + for k = 1:N + dt = 1e-6; + Er = RandEuler; + B = B && all(AngVelFromEulerVel(Er,[0 0 0]) == [0;0;0]); + dE = 3*(2*rand-1)*RandUnitVec(1,3)'; + AngVel = AngVelFromEulerVel(Er,dE); + dQ = ComposeQuat(QuatFromEuler(Er+dE*dt), QuatInv(QuatFromEuler(Er-dE*dt))); + ErrA(k) = norm(AngVel - (dQ(2:4)'/dt)); + end + + % End test + fprintf('Using special tolerance %g for error testing.\n\n', 1.5e-8); + P = P & EndBoolean(B); + P = P & EndTest(1.5e-8, 'AngVel error', ErrA); + + % + % End of test script + % + + % End test script + EndTestScript('TestAngVelFrom', P); + + % Set the output pass flag + if nargout >= 1 + Pass = P; + end + + % Clear the function variable workspace + if isOctave + clear -x Pass + else + clearvars -except Pass + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/TestCompose.m b/tro2022/transform/RotationsLib/Test/TestCompose.m new file mode 100644 index 0000000..905abcd --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/TestCompose.m @@ -0,0 +1,184 @@ +% TestCompose.m - Philipp Allgeuer - 05/11/14 +% Tests: Compose* +% Assumes: Rand*, *Equal, RotmatFrom*, *FromRotmat +% +% function [Pass] = TestCompose(N, Tol, Inter) +% +% N ==> Number of test cases to use in each test +% Tol ==> Numeric tolerance to use for testing +% Inter ==> Boolean flag whether to also run interactive tests +% Pass ==> Boolean flag whether all tests were passed + +% Main function +function [Pass] = TestCompose(N, Tol, Inter) + + % Process function inputs + if nargin < 1 || ~isscalar(N) || N < 1 + N = 500; + end + N = min(round(N),1000000); + if nargin < 2 || Tol <= 0 + Tol = 128*eps; + end + + % Begin test script + [P, Nnormal] = BeginTestScript('TestCompose', N, Tol); + + % + % Test ComposeEuler + % + + % Begin test + [N, ErrA, ErrB] = BeginTest('ComposeEuler', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + Er = RandEuler(2); + B = B && all(ComposeEuler() == [0 0 0]); + B = B && all(ComposeEuler(Er(1,:)) == Er(1,:)); + B = B && all(ComposeEuler(Er(2,:)) == Er(2,:)); + + % Calculate composition errors + for k = 1:N + Er = RandEuler(3); + REa = RotmatFromEuler(Er(1,:)); + REb = RotmatFromEuler(Er(2,:)); + REc = RotmatFromEuler(Er(3,:)); + [~, ErrA(k)] = EulerEqual(ComposeEuler(Er(1,:),Er(2,:)), EulerFromRotmat(REa*REb)); + [~, ErrB(k)] = EulerEqual(ComposeEuler(Er(1,:),Er(2,:),Er(3,:)), EulerFromRotmat(REa*REb*REc)); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, '2 arguments', ErrA, '3 arguments', ErrB); + + % + % Test ComposeFused + % + + % Begin test + [N, ErrA, ErrB] = BeginTest('ComposeFused', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + Fr = RandFused(2); + B = B && all(ComposeFused() == [0 0 0 1]); + B = B && all(ComposeFused(Fr(1,:)) == Fr(1,:)); + B = B && all(ComposeFused(Fr(2,:)) == Fr(2,:)); + + % Calculate composition errors + for k = 1:N + Fr = RandFused(3); + RFa = RotmatFromFused(Fr(1,:)); + RFb = RotmatFromFused(Fr(2,:)); + RFc = RotmatFromFused(Fr(3,:)); + [~, ErrA(k)] = FusedEqual(ComposeFused(Fr(1,:),Fr(2,:)), FusedFromRotmat(RFa*RFb)); + [~, ErrB(k)] = FusedEqual(ComposeFused(Fr(1,:),Fr(2,:),Fr(3,:)), FusedFromRotmat(RFa*RFb*RFc)); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, '2 arguments', ErrA, '3 arguments', ErrB); + + % + % Test ComposeQuat + % + + % Begin test + [N, ErrA, ErrB] = BeginTest('ComposeQuat', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + Qr = RandQuat(2); + B = B && all(ComposeQuat() == [1 0 0 0]); + B = B && all(ComposeQuat(Qr(1,:)) == Qr(1,:)); + B = B && all(ComposeQuat(Qr(2,:)) == Qr(2,:)); + + % Calculate composition errors + for k = 1:N + Qr = RandQuat(3); + RQa = RotmatFromQuat(Qr(1,:)); + RQb = RotmatFromQuat(Qr(2,:)); + RQc = RotmatFromQuat(Qr(3,:)); + [~, ErrA(k)] = QuatEqual(ComposeQuat(Qr(1,:),Qr(2,:)), QuatFromRotmat(RQa*RQb)); + [~, ErrB(k)] = QuatEqual(ComposeQuat(Qr(1,:),Qr(2,:),Qr(3,:)), QuatFromRotmat(RQa*RQb*RQc)); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, '2 arguments', ErrA, '3 arguments', ErrB); + + % + % Test ComposeRotmat + % + + % Begin test + [N, ErrA, ErrB] = BeginTest('ComposeRotmat', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + Rr = RandRotmat(2); + B = B && all(all(ComposeRotmat() == eye(3))); + B = B && all(all(ComposeRotmat(Rr(:,:,1)) == Rr(:,:,1))); + B = B && all(all(ComposeRotmat(Rr(:,:,2)) == Rr(:,:,2))); + + % Calculate composition errors + for k = 1:N + Rr = RandRotmat(3); + [~, ErrA(k)] = RotmatEqual(ComposeRotmat(Rr(:,:,1),Rr(:,:,2)), Rr(:,:,1)*Rr(:,:,2)); + [~, ErrB(k)] = RotmatEqual(ComposeRotmat(Rr(:,:,1),Rr(:,:,2),Rr(:,:,3)), Rr(:,:,1)*Rr(:,:,2)*Rr(:,:,3)); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, '2 arguments', ErrA, '3 arguments', ErrB); + + % + % Test ComposeTilt + % + + % Begin test + [N, ErrA, ErrB] = BeginTest('ComposeTilt', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + Tr = RandTilt(2); + B = B && all(ComposeTilt() == [0 0 0]); + B = B && all(ComposeTilt(Tr(1,:)) == Tr(1,:)); + B = B && all(ComposeTilt(Tr(2,:)) == Tr(2,:)); + + % Calculate composition errors + for k = 1:N + Tr = RandTilt(3); + RTa = RotmatFromTilt(Tr(1,:)); + RTb = RotmatFromTilt(Tr(2,:)); + RTc = RotmatFromTilt(Tr(3,:)); + [~, ErrA(k)] = TiltEqual(ComposeTilt(Tr(1,:),Tr(2,:)), TiltFromRotmat(RTa*RTb)); + [~, ErrB(k)] = TiltEqual(ComposeTilt(Tr(1,:),Tr(2,:),Tr(3,:)), TiltFromRotmat(RTa*RTb*RTc)); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, '2 arguments', ErrA, '3 arguments', ErrB); + + % + % End of test script + % + + % End test script + EndTestScript('TestCompose', P); + + % Set the output pass flag + if nargout >= 1 + Pass = P; + end + + % Clear the function variable workspace + if isOctave + clear -x Pass + else + clearvars -except Pass + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/TestConversions.m b/tro2022/transform/RotationsLib/Test/TestConversions.m new file mode 100644 index 0000000..c36cb1a --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/TestConversions.m @@ -0,0 +1,199 @@ +% TestConversions.m - Philipp Allgeuer - 05/11/14 +% Tests: *From* +% Assumes: Rand*, *Equal +% +% function [Pass] = TestConversions(N, Tol, Inter) +% +% N ==> Number of test cases to use in each test +% Tol ==> Numeric tolerance to use for testing +% Inter ==> Boolean flag whether to also run interactive tests +% Pass ==> Boolean flag whether all tests were passed + +% Main function +function [Pass] = TestConversions(N, Tol, Inter) + + % Process function inputs + if nargin < 1 || ~isscalar(N) || N < 1 + N = 250; + end + N = min(round(N),1000000); + if nargin < 2 || Tol <= 0 + Tol = 128*eps; + end + + % Begin test script + [P, Nnormal] = BeginTestScript('TestConversions', N, Tol); + + % + % Test 2-conversions + % + + % Begin test + N = BeginTest('2-conversions', Nnormal); + B = BeginBoolean(); + + % Initialise the error vectors + ErrA = zeros(N,4); + ErrB = zeros(N,4); + ErrC = zeros(N,4); + ErrD = zeros(N,4); + ErrE = zeros(N,4); + + % Perform the required testing + for k = 1:N + Er = RandEuler; + %[~, ErrA(k,1)] = EulerEqual(Er, EulerFromFused(FusedFromEuler(Er))); % Note: Extremely near Euler gimbal lock EulerFromFused becomes quite sensitive to noise, unavoidably producing errors up to 1e-6 in this 2-conversion. + [~, ErrA(k,2)] = EulerEqual(Er, EulerFromQuat(QuatFromEuler(Er))); + [~, ErrA(k,3)] = EulerEqual(Er, EulerFromRotmat(RotmatFromEuler(Er))); + [~, ErrA(k,4)] = EulerEqual(Er, EulerFromTilt(TiltFromEuler(Er))); + Fr = RandFused; + [~, ErrB(k,1)] = FusedEqual(Fr, FusedFromEuler(EulerFromFused(Fr))); + [~, ErrB(k,2)] = FusedEqual(Fr, FusedFromQuat(QuatFromFused(Fr))); + [~, ErrB(k,3)] = FusedEqual(Fr, FusedFromRotmat(RotmatFromFused(Fr))); + [~, ErrB(k,4)] = FusedEqual(Fr, FusedFromTilt(TiltFromFused(Fr))); + Qr = RandQuat; + [~, ErrC(k,1)] = QuatEqual(Qr, QuatFromEuler(EulerFromQuat(Qr))); + [~, ErrC(k,2)] = QuatEqual(Qr, QuatFromFused(FusedFromQuat(Qr))); + [~, ErrC(k,3)] = QuatEqual(Qr, QuatFromRotmat(RotmatFromQuat(Qr))); + [~, ErrC(k,4)] = QuatEqual(Qr, QuatFromTilt(TiltFromQuat(Qr))); + Rr = RandRotmat; + [~, ErrD(k,1)] = RotmatEqual(Rr, RotmatFromEuler(EulerFromRotmat(Rr))); + [~, ErrD(k,2)] = RotmatEqual(Rr, RotmatFromFused(FusedFromRotmat(Rr))); + [~, ErrD(k,3)] = RotmatEqual(Rr, RotmatFromQuat(QuatFromRotmat(Rr))); + [~, ErrD(k,4)] = RotmatEqual(Rr, RotmatFromTilt(TiltFromRotmat(Rr))); + Tr = RandTilt; + [~, ErrE(k,1)] = TiltEqual(Tr, TiltFromEuler(EulerFromTilt(Tr))); + [~, ErrE(k,2)] = TiltEqual(Tr, TiltFromFused(FusedFromTilt(Tr))); + [~, ErrE(k,3)] = TiltEqual(Tr, TiltFromQuat(QuatFromTilt(Tr))); + [~, ErrE(k,4)] = TiltEqual(Tr, TiltFromRotmat(RotmatFromTilt(Tr))); + end + + % End test + fprintf('Using special tolerance %g for error testing.\n\n', 1.5e-8); + P = P & EndBoolean(B); + P = P & EndTest(1.5e-8, 'Euler 2-conversions', ErrA, 'Fused 2-conversions', ErrB, 'Quat 2-conversions', ErrC, 'Rotmat 2-conversions', ErrD, 'Tilt 2-conversions', ErrE); + + % + % Test 3-conversions + % + + % Begin test + N = BeginTest('3-conversions', Nnormal); + B = BeginBoolean(); + + % Initialise the error vectors + ErrA = zeros(N,3); + ErrB = zeros(N,3); + ErrC = zeros(N,3); + ErrD = zeros(N,3); + ErrE = zeros(N,3); + + % Perform the required testing + for k = 1:N + Er = RandEuler; + QEr = QuatFromEuler(Er); + [~, ErrA(k,1)] = QuatEqual(QuatFromFused(FusedFromEuler(Er)), QEr); + [~, ErrA(k,2)] = QuatEqual(QuatFromRotmat(RotmatFromEuler(Er)), QEr); + [~, ErrA(k,3)] = QuatEqual(QuatFromTilt(TiltFromEuler(Er)), QEr); + + Fr = RandFused; + RFr = RotmatFromFused(Fr); + [~, ErrB(k,1)] = RotmatEqual(RotmatFromEuler(EulerFromFused(Fr)), RFr); + [~, ErrB(k,2)] = RotmatEqual(RotmatFromQuat(QuatFromFused(Fr)), RFr); + [~, ErrB(k,3)] = RotmatEqual(RotmatFromTilt(TiltFromFused(Fr)), RFr); + + Qr = RandQuat; + TQr = TiltFromQuat(Qr); + [~, ErrC(k,1)] = TiltEqual(TiltFromEuler(EulerFromQuat(Qr)), TQr); + [~, ErrC(k,2)] = TiltEqual(TiltFromFused(FusedFromQuat(Qr)), TQr); + [~, ErrC(k,3)] = TiltEqual(TiltFromRotmat(RotmatFromQuat(Qr)), TQr); + + Rr = RandRotmat; + ERr = EulerFromRotmat(Rr); +% [~, ErrD(k,1)] = EulerEqual(EulerFromFused(FusedFromRotmat(Rr)), ERr); % Note: The EulerFromFused function is sensitive to noise very near gimbal lock and infrequently gives errors in excess of 1e-10. + [~, ErrD(k,2)] = EulerEqual(EulerFromQuat(QuatFromRotmat(Rr)), ERr); + [~, ErrD(k,3)] = EulerEqual(EulerFromTilt(TiltFromRotmat(Rr)), ERr); + + Tr = RandTilt; + FTr = FusedFromTilt(Tr); + [~, ErrE(k,1)] = FusedEqual(FusedFromEuler(EulerFromTilt(Tr)), FTr); + [~, ErrE(k,2)] = FusedEqual(FusedFromQuat(QuatFromTilt(Tr)), FTr); + [~, ErrE(k,3)] = FusedEqual(FusedFromRotmat(RotmatFromTilt(Tr)), FTr); + end + + % End test + fprintf('Using special tolerance %g for error testing.\n\n', 1.5e-8); + P = P & EndBoolean(B); + P = P & EndTest(1.5e-8, 'Euler-Quat 3-conversions', ErrA, 'Fused-Rotmat 3-conversions', ErrB, 'Quat-Tilt 3-conversions', ErrC, 'Rotmat-Euler 3-conversions', ErrD, 'Tilt-Fused 3-conversions', ErrE); + + % + % Test additional outputs + % + + % Begin test + N = BeginTest('Additional outputs', Nnormal); + B = BeginBoolean(); + + % Initialise the error vectors + ErrA = zeros(N,4); + ErrB = zeros(N,4); + + % Perform the required testing + for k = 1:N + Er = RandEuler; + Fr = RandFused; + Qr = RandQuat; + Rr = RandRotmat; + + TFr = TiltFromFused(Fr); + [~, Tilt] = EulerFromFused(Fr); + [~, ErrA(k,1)] = TiltEqual(Tilt, TFr); + [~, Tilt] = QuatFromFused(Fr); + [~, ErrA(k,2)] = TiltEqual(Tilt, TFr); + [~, Tilt] = RotmatFromFused(Fr); + [~, ErrA(k,3)] = TiltEqual(Tilt, TFr); + + REr = RotmatFromEuler(Er); + [~, Rotmat] = TiltFromEuler(Er); + [~, ErrA(k,4)] = RotmatEqual(Rotmat, REr); + + REr = RotmatFromEuler(Er); + TEr = TiltFromEuler(Er); + [~, Rotmat, Tilt] = FusedFromEuler(Er); + [~, ErrB(k,1)] = RotmatEqual(Rotmat, REr); + [~, ErrB(k,2)] = TiltEqual(Tilt, TEr); + + TQr = TiltFromQuat(Qr); + [~, Tilt] = FusedFromQuat(Qr); + [~, ErrB(k,3)] = TiltEqual(Tilt, TQr); + + TRr = TiltFromRotmat(Rr); + [~, Tilt] = FusedFromRotmat(Rr); + [~, ErrB(k,4)] = TiltEqual(Tilt, TRr); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Group 1', ErrA, 'Group 2', ErrB); + + % + % End of test script + % + + % End test script + EndTestScript('TestConversions', P); + + % Set the output pass flag + if nargout >= 1 + Pass = P; + end + + % Clear the function variable workspace + if isOctave + clear -x Pass + else + clearvars -except Pass + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/TestEYawOf.m b/tro2022/transform/RotationsLib/Test/TestEYawOf.m new file mode 100644 index 0000000..aed9387 --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/TestEYawOf.m @@ -0,0 +1,190 @@ +% TestEYawOf.m - Philipp Allgeuer - 05/11/14 +% Tests: EYawOf* +% Assumes: Rand*, EulerFrom* +% +% function [Pass] = TestEYawOf(N, Tol, Inter) +% +% N ==> Number of test cases to use in each test +% Tol ==> Numeric tolerance to use for testing +% Inter ==> Boolean flag whether to also run interactive tests +% Pass ==> Boolean flag whether all tests were passed + +% Main function +function [Pass] = TestEYawOf(N, Tol, Inter) + + % Process function inputs + if nargin < 1 || ~isscalar(N) || N < 1 + N = 3000; + end + N = min(round(N),1000000); + if nargin < 2 || Tol <= 0 + Tol = 128*eps; + end + + % Begin test script + [P, Nnormal] = BeginTestScript('TestEYawOf', N, Tol); + + % + % Test EYawOfEuler + % + + % Begin test + N = BeginTest('EYawOfEuler', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + B = B && (EYawOfEuler([0 0 0]) == 0); + for k = 1:N + Er = RandEuler; + B = B && (EYawOfEuler(Er) == Er(1)); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol); + + % + % Test EYawOfFused + % + + % Begin test + [N, ErrA] = BeginTest('EYawOfFused', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + B = B && (EYawOfFused([0 0 0 1]) == 0); + + % Perform the required testing + for k = 1:N + Fr = RandFused; + Er = EulerFromFused(Fr); + FEYaw = EYawOfFused(Fr); + EEYaw = Er(1); + if abs(FEYaw-EEYaw) > pi + if FEYaw > EEYaw + EEYaw = EEYaw + 2*pi; + else + FEYaw = FEYaw + 2*pi; + end + end + ErrA(k) = abs(FEYaw - EEYaw); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'EYaw error', ErrA); + + % + % Test EYawOfQuat + % + + % Begin test + [N, ErrA] = BeginTest('EYawOfQuat', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + B = B && (EYawOfQuat([1 0 0 0]) == 0); + + % Perform the required testing + for k = 1:N + Qr = RandQuat; + Er = EulerFromQuat(Qr); + QEYaw = EYawOfQuat(Qr); + EEYaw = Er(1); + if abs(QEYaw-EEYaw) > pi + if QEYaw > EEYaw + EEYaw = EEYaw + 2*pi; + else + QEYaw = QEYaw + 2*pi; + end + end + ErrA(k) = abs(QEYaw - EEYaw); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'EYaw error', ErrA); + + % + % Test EYawOfRotmat + % + + % Begin test + [N, ErrA] = BeginTest('EYawOfRotmat', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + B = B && (EYawOfRotmat(eye(3)) == 0); + + % Perform the required testing + for k = 1:N + Rr = RandRotmat; + Er = EulerFromRotmat(Rr); + REYaw = EYawOfRotmat(Rr); + EEYaw = Er(1); + if abs(REYaw-EEYaw) > pi + if REYaw > EEYaw + EEYaw = EEYaw + 2*pi; + else + REYaw = REYaw + 2*pi; + end + end + ErrA(k) = abs(REYaw - EEYaw); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'EYaw error', ErrA); + + % + % Test EYawOfTilt + % + + % Begin test + [N, ErrA] = BeginTest('EYawOfTilt', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + B = B && (EYawOfTilt([0 0 0]) == 0); + + % Perform the required testing + for k = 1:N + Tr = RandTilt; + Er = EulerFromTilt(Tr); + TEYaw = EYawOfTilt(Tr); + EEYaw = Er(1); + if abs(TEYaw-EEYaw) > pi + if TEYaw > EEYaw + EEYaw = EEYaw + 2*pi; + else + TEYaw = TEYaw + 2*pi; + end + end + ErrA(k) = abs(TEYaw - EEYaw); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'EYaw error', ErrA); + + % + % End of test script + % + + % End test script + EndTestScript('TestEYawOf', P); + + % Set the output pass flag + if nargout >= 1 + Pass = P; + end + + % Clear the function variable workspace + if isOctave + clear -x Pass + else + clearvars -except Pass + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/TestEqual.m b/tro2022/transform/RotationsLib/Test/TestEqual.m new file mode 100644 index 0000000..5556263 --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/TestEqual.m @@ -0,0 +1,238 @@ +% TestEqual.m - Philipp Allgeuer - 05/11/14 +% Tests: *Equal, Ensure* +% Assumes: Rand* +% +% function [Pass] = TestEqual(N, Tol, Inter) +% +% The fact that *Equal (EFT) is correct means that Ensure* (EFT) is also correct, +% as the former is based on the latter (assumed). Thus to assert that Ensure* +% is correct we only need to additionally check Ensure* (QR). +% +% N ==> Number of test cases to use in each test +% Tol ==> Numeric tolerance to use for testing +% Inter ==> Boolean flag whether to also run interactive tests +% Pass ==> Boolean flag whether all tests were passed + +% Main function +function [Pass] = TestEqual(N, Tol, Inter) + + % Process function inputs + if nargin < 1 || ~isscalar(N) || N < 1 + N = 600; + end + N = min(round(N),1000000); + if nargin < 2 || Tol <= 0 + Tol = 128*eps; + end + + % Begin test script + [P, Nnormal] = BeginTestScript('TestEqual', N, Tol); + + % + % Test EulerEqual + % + + % Begin test + N = BeginTest('EulerEqual', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + Er = RandEuler; + [Equal, Err] = EulerEqual(Er, Er, 0); + B = B && Equal && (Err == 0); + B = B && ~EulerEqual([1 0 0], [0 0 1], Tol); + for k = 1:N + Er = RandEuler; + B = B && EulerEqual(Er, Er+2*pi*(randi(9,1,3)-5), Tol); + B = B && EulerEqual(Er, [pi+Er(1) pi-Er(2) pi+Er(3)], Tol); + B = B && EulerEqual([Er(1) pi/2 Er(3)], [0 pi/2 Er(3)-Er(1)+2*pi*(randi(9)-5)], Tol); + B = B && EulerEqual([Er(1) -pi/2 Er(3)], [0 -pi/2 Er(3)+Er(1)+2*pi*(randi(9)-5)], Tol); + B = B && EulerEqual([Er(1) asin(1) Er(3)], [Er(1) asin(1-0.49*Tol*rand) Er(3)], Tol); + B = B && EulerEqual([Er(1) asin(-1) Er(3)], [Er(1) asin(-1+0.49*Tol*rand) Er(3)], Tol); + B = B && ~EulerEqual(Er, Er+[1 0 2], Tol); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol); + + % + % Test FusedEqual + % + + % Begin test + N = BeginTest('FusedEqual', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + Fr = RandFused; + [Equal, Err] = FusedEqual(Fr, Fr, 0); + B = B && Equal && (Err == 0); + B = B && ~FusedEqual([1 0 0 1], [0 0 1 1], Tol); + for k = 1:N + Fr = RandFused; + B = B && FusedEqual(Fr, Fr+[2*pi*(randi(9,1,3)-5) 0], Tol); + Scale = (pi/2)/(abs(Fr(2))+abs(Fr(3))); + B = B && FusedEqual([Fr(1) Fr(2:3)*Scale 1], [Fr(1) Fr(2:3)*Scale*(1+rand) -1], Tol); + B = B && FusedEqual([Fr(1) 0.5*Tol*(2*rand(1,2)-1) -1],[Fr(1)+2*pi*rand 0.5*Tol*(2*rand(1,2)-1) -1], Tol); + B = B && FusedEqual([Fr(1) asin(1) 0 Fr(4)], [Fr(1) asin(1-0.49*Tol*rand) 0 Fr(4)], Tol); + B = B && FusedEqual([Fr(1) 0 asin(1) Fr(4)], [Fr(1) 0 asin(1-0.49*Tol*rand) Fr(4)], Tol); + B = B && FusedEqual([Fr(1) asin(-1) 0 Fr(4)], [Fr(1) asin(-1+0.49*Tol*rand) 0 Fr(4)], Tol); + B = B && FusedEqual([Fr(1) 0 asin(-1) Fr(4)], [Fr(1) 0 asin(-1+0.49*Tol*rand) Fr(4)], Tol); + B = B && ~FusedEqual(Fr, Fr+[2 1 0 0], Tol); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol); + + % + % Test QuatEqual + % + + % Begin test + N = BeginTest('QuatEqual', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + Qr = RandQuat; + [Equal, Err] = QuatEqual(Qr, Qr, 0); + B = B && Equal && (Err == 0); + B = B && ~QuatEqual([0 1 0 0], [0 0 1 0], Tol); + for k = 1:N + Qr = RandQuat; + [Equal, Err] = QuatEqual(Qr, -Qr, 0); + B = B && Equal && (Err == 0); + if Qr(1) > 0.01 && Qr(1) < 0.99 + B = B && ~QuatEqual(Qr, [Qr(1) -Qr(2:4)], Tol); + end + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol); + + % + % Test RotmatEqual + % + + % Begin test + N = BeginTest('RotmatEqual', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + Rr = RandRotmat; + [Equal, Err] = RotmatEqual(Rr, Rr, 0); + B = B && Equal && (Err == 0); + B = B && ~RotmatEqual([0 1 0;1 0 0;0 0 -1], eye(3), Tol); + for k = 1:N + Rr = RandRotmat; + B = B && RotmatEqual(Rr, Rr+Tol*(2*rand(3,3)-1), Tol); + B = B && ~RotmatEqual(Rr, Rr+1.1*(Tol./(2*rand(3,3)-1)), Tol); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol); + + % + % Test TiltEqual + % + + % Begin test + N = BeginTest('TiltEqual', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + Tr = RandTilt; + [Equal, Err] = TiltEqual(Tr, Tr, 0); + B = B && Equal && (Err == 0); + B = B && ~TiltEqual([1 0 0.5], [0 0 1], Tol); + for k = 1:N + Tr = RandTilt; + B = B && TiltEqual(Tr, Tr+2*pi*(randi(9,1,3)-5), Tol); + B = B && TiltEqual(Tr, [Tr(1) Tr(2)+pi -Tr(3)], Tol); + B = B && TiltEqual([Tr(1:2) acos(1)], [Tr(1:2) acos(1-0.49*Tol*rand)], Tol); + B = B && TiltEqual([Tr(1:2) acos(-1)], [Tr(1:2) acos(-1+0.49*Tol*rand)], Tol); + B = B && ~TiltEqual(Tr, Tr+[1 0 2], Tol); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol); + + % + % Test EnsureQuat + % + + % Begin test + N = BeginTest('EnsureQuat', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + [Qout, WasBad, Norm] = EnsureQuat([1 0 0 0], 0, true); + B = B && all(Qout == [1 0 0 0]) && ~WasBad && (Norm == 1); + B = B && all(EnsureQuat([0 0 0 0], Tol, true) == [1 0 0 0]); + for k = 1:N + Qr = RandQuat; + [Qout, WasBad, Norm] = EnsureQuat(Qr, Tol, true); + B = B && ((WasBad && all(abs(Qr + Qout) <= Tol)) || (~WasBad && all(abs(Qr - Qout) <= Tol))); + B = B && (abs(Norm - 1) <= Tol); + Mag = 2*rand+1.1; + [Qout, WasBad, Norm] = EnsureQuat(Mag*Qr, Tol, false); + B = B && all(abs(Qout - Qr) <= Tol) && WasBad && (abs(Norm - Mag) <= Tol); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol); + + % + % Test EnsureRotmat + % + + % Begin test + N = BeginTest('EnsureRotmat', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + [~, WasBad] = EnsureRotmat([1 0 0.1;0 1 0;0 0 1], Tol); + B = B && WasBad; + [Rout, WasBad] = EnsureRotmat(eye(3), 0); + B = B && all(all(Rout == eye(3))) && ~WasBad; + for k = 1:N + Rr = RandRotmat; + [Rout, WasBad] = EnsureRotmat(Rr, Tol); + B = B && all(all(abs(Rout - Rr) <= Tol)) && ~WasBad; + Rnew = EnsureRotmat(Rr+0.1*(2*rand(3)-1), Tol); + B = B && (abs(det(Rr)-1) <= Tol); + B = B && all(all(abs(Rnew*Rnew'-eye(3)) <= Tol)); + B = B && all(all(abs(Rnew'*Rnew-eye(3)) <= Tol)); + B = B && all(abs(cross(Rnew(:,1),Rnew(:,2)) - Rnew(:,3)) <= Tol); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol); + + % + % End of test script + % + + % End test script + EndTestScript('TestEqual', P); + + % Set the output pass flag + if nargout >= 1 + Pass = P; + end + + % Clear the function variable workspace + if isOctave + clear -x Pass + else + clearvars -except Pass + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/TestFYawOf.m b/tro2022/transform/RotationsLib/Test/TestFYawOf.m new file mode 100644 index 0000000..ec4a6f9 --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/TestFYawOf.m @@ -0,0 +1,190 @@ +% TestFYawOf.m - Philipp Allgeuer - 05/11/14 +% Tests: FYawOf* +% Assumes: Rand*, FusedFrom* +% +% function [Pass] = TestFYawOf(N, Tol, Inter) +% +% N ==> Number of test cases to use in each test +% Tol ==> Numeric tolerance to use for testing +% Inter ==> Boolean flag whether to also run interactive tests +% Pass ==> Boolean flag whether all tests were passed + +% Main function +function [Pass] = TestFYawOf(N, Tol, Inter) + + % Process function inputs + if nargin < 1 || ~isscalar(N) || N < 1 + N = 3000; + end + N = min(round(N),1000000); + if nargin < 2 || Tol <= 0 + Tol = 128*eps; + end + + % Begin test script + [P, Nnormal] = BeginTestScript('TestFYawOf', N, Tol); + + % + % Test FYawOfEuler + % + + % Begin test + [N, ErrA] = BeginTest('FYawOfEuler', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + B = B && (FYawOfEuler([0 0 0]) == 0); + + % Perform the required testing + for k = 1:N + Er = RandEuler; + Fr = FusedFromEuler(Er); + EFYaw = FYawOfEuler(Er); + FFYaw = Fr(1); + if abs(EFYaw-FFYaw) > pi + if EFYaw > FFYaw + FFYaw = FFYaw + 2*pi; + else + EFYaw = EFYaw + 2*pi; + end + end + ErrA(k) = abs(EFYaw - FFYaw); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'FYaw error', ErrA); + + % + % Test FYawOfFused + % + + % Begin test + N = BeginTest('FYawOfFused', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + B = B && (FYawOfFused([0 0 0 1]) == 0); + for k = 1:N + Fr = RandFused; + B = B && (FYawOfFused(Fr) == Fr(1)); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol); + + % + % Test FYawOfQuat + % + + % Begin test + [N, ErrA] = BeginTest('FYawOfQuat', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + B = B && (FYawOfQuat([1 0 0 0]) == 0); + + % Perform the required testing + for k = 1:N + Qr = RandQuat; + Fr = FusedFromQuat(Qr); + QFYaw = FYawOfQuat(Qr); + FFYaw = Fr(1); + if abs(QFYaw-FFYaw) > pi + if QFYaw > FFYaw + FFYaw = FFYaw + 2*pi; + else + QFYaw = QFYaw + 2*pi; + end + end + ErrA(k) = abs(QFYaw - FFYaw); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'FYaw error', ErrA); + + % + % Test FYawOfRotmat + % + + % Begin test + [N, ErrA] = BeginTest('FYawOfRotmat', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + B = B && (FYawOfRotmat(eye(3)) == 0); + + % Perform the required testing + for k = 1:N + Rr = RandRotmat; + Fr = FusedFromRotmat(Rr); + RFYaw = FYawOfRotmat(Rr); + FFYaw = Fr(1); + if abs(RFYaw-FFYaw) > pi + if RFYaw > FFYaw + FFYaw = FFYaw + 2*pi; + else + RFYaw = RFYaw + 2*pi; + end + end + ErrA(k) = abs(RFYaw - FFYaw); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'FYaw error', ErrA); + + % + % Test FYawOfTilt + % + + % Begin test + [N, ErrA] = BeginTest('FYawOfTilt', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + B = B && (FYawOfTilt([0 0 0]) == 0); + + % Perform the required testing + for k = 1:N + Tr = RandTilt; + Fr = FusedFromTilt(Tr); + TFYaw = FYawOfTilt(Tr); + FFYaw = Fr(1); + if abs(TFYaw-FFYaw) > pi + if TFYaw > FFYaw + FFYaw = FFYaw + 2*pi; + else + TFYaw = TFYaw + 2*pi; + end + end + ErrA(k) = abs(TFYaw - FFYaw); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'FYaw error', ErrA); + + % + % End of test script + % + + % End test script + EndTestScript('TestFYawOf', P); + + % Set the output pass flag + if nargout >= 1 + Pass = P; + end + + % Clear the function variable workspace + if isOctave + clear -x Pass + else + clearvars -except Pass + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/TestFromAxis.m b/tro2022/transform/RotationsLib/Test/TestFromAxis.m new file mode 100644 index 0000000..a341039 --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/TestFromAxis.m @@ -0,0 +1,228 @@ +% TestFromAxis.m - Philipp Allgeuer - 05/11/14 +% Tests: *FromAxis +% Assumes: RandVec, RandAng, *Equal, *FromQuat +% +% function [Pass] = TestFromAxis(N, Tol, Inter) +% +% N ==> Number of test cases to use in each test +% Tol ==> Numeric tolerance to use for testing +% Inter ==> Boolean flag whether to also run interactive tests +% Pass ==> Boolean flag whether all tests were passed + +% Main function +function [Pass] = TestFromAxis(N, Tol, Inter) + + % Process function inputs + if nargin < 1 || ~isscalar(N) || N < 1 + N = 600; + end + N = min(round(N),1000000); + if nargin < 2 || Tol <= 0 + Tol = 128*eps; + end + + % Begin test script + [P, Nnormal] = BeginTestScript('TestFromAxis', N, Tol); + + % + % Test QuatFromAxis + % + + % Begin test + N = BeginTest('QuatFromAxis', Nnormal); + B = BeginBoolean(); + + % Perform the required testing + for k = 1:N + Vec = RandVec(1,5.0)'; + Ang = 3*RandAng; + C = cos(0.5*Ang); + S = sin(0.5*Ang); + if C < 0 + C = -C; + S = -S; + end + B = B && all(QuatFromAxis(Vec,0) == [1 0 0 0]); + B = B && all(QuatFromAxis('x',Ang) - [C S 0 0] <= Tol); + B = B && all(QuatFromAxis('y',Ang) - [C 0 S 0] <= Tol); + B = B && all(QuatFromAxis('z',Ang) - [C 0 0 S] <= Tol); + B = B && all(QuatFromAxis(Vec,Ang) - [C S*Vec/norm(Vec)] <= Tol); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(); + + % + % Test RotmatFromAxis + % + + % Begin test + N = BeginTest('RotmatFromAxis', Nnormal); + B = BeginBoolean(); + + % Perform the required testing + for k = 1:N + Vec = RandVec(1,5.0)'; + Ang = 3*RandAng; + B = B && RotmatEqual(RotmatFromAxis(Vec,0), eye(3), 2*eps); + B = B && RotmatEqual(RotmatFromAxis('x',Ang), RotmatFromQuat(QuatFromAxis('x',Ang)), Tol); + B = B && RotmatEqual(RotmatFromAxis('y',Ang), RotmatFromQuat(QuatFromAxis('y',Ang)), Tol); + B = B && RotmatEqual(RotmatFromAxis('z',Ang), RotmatFromQuat(QuatFromAxis('z',Ang)), Tol); + B = B && RotmatEqual(RotmatFromAxis(Vec,Ang), RotmatFromQuat(QuatFromAxis(Vec,Ang)), Tol); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(); + + % + % Test EulerFromAxis + % + + % Begin test + [N, ErrA] = BeginTest('EulerFromAxis', Nnormal); + B = BeginBoolean(); + + % Perform the required testing + for k = 1:N + Vec = RandVec(1,5.0)'; + Ang = 3*RandAng; + B = B && EulerEqual(EulerFromAxis(Vec,0), [0 0 0], 2*eps); + + RefQuat = QuatFromAxis('x',Ang); + [Euler, Quat] = EulerFromAxis('x', Ang); + B = B && QuatEqual(Quat, RefQuat, Tol); + [~, Error] = EulerEqual(Euler, EulerFromQuat(RefQuat), Tol); + ErrA(k) = max(ErrA(k), Error); + + RefQuat = QuatFromAxis('y',Ang); + [Euler, Quat] = EulerFromAxis('y', Ang); + B = B && QuatEqual(Quat, RefQuat, Tol); + [~, Error] = EulerEqual(Euler, EulerFromQuat(RefQuat), Tol); + ErrA(k) = max(ErrA(k), Error); + + RefQuat = QuatFromAxis('z',Ang); + [Euler, Quat] = EulerFromAxis('z', Ang); + B = B && QuatEqual(Quat, RefQuat, Tol); + [~, Error] = EulerEqual(Euler, EulerFromQuat(RefQuat), Tol); + ErrA(k) = max(ErrA(k), Error); + + RefQuat = QuatFromAxis(Vec,Ang); + [Euler, Quat] = EulerFromAxis(Vec, Ang); + B = B && QuatEqual(Quat, RefQuat, Tol); + [~, Error] = EulerEqual(Euler, EulerFromQuat(RefQuat), Tol); + ErrA(k) = max(ErrA(k), Error); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Euler representation error', ErrA); + + % + % Test FusedFromAxis + % + + % Begin test + [N, ErrA] = BeginTest('FusedFromAxis', Nnormal); + B = BeginBoolean(); + + % Perform the required testing + for k = 1:N + Vec = RandVec(1,5.0)'; + Ang = 3*RandAng; + B = B && FusedEqual(FusedFromAxis(Vec,0), [0 0 0 1], 2*eps); + + RefQuat = QuatFromAxis('x',Ang); + [Fused, Quat] = FusedFromAxis('x', Ang); + B = B && QuatEqual(Quat, RefQuat, Tol); + [~, Error] = FusedEqual(Fused, FusedFromQuat(RefQuat), Tol); + ErrA(k) = max(ErrA(k), Error); + + RefQuat = QuatFromAxis('y',Ang); + [Fused, Quat] = FusedFromAxis('y', Ang); + B = B && QuatEqual(Quat, RefQuat, Tol); + [~, Error] = FusedEqual(Fused, FusedFromQuat(RefQuat), Tol); + ErrA(k) = max(ErrA(k), Error); + + RefQuat = QuatFromAxis('z',Ang); + [Fused, Quat] = FusedFromAxis('z', Ang); + B = B && QuatEqual(Quat, RefQuat, Tol); + [~, Error] = FusedEqual(Fused, FusedFromQuat(RefQuat), Tol); + ErrA(k) = max(ErrA(k), Error); + + RefQuat = QuatFromAxis(Vec,Ang); + [Fused, Quat] = FusedFromAxis(Vec, Ang); + B = B && QuatEqual(Quat, RefQuat, Tol); + [~, Error] = FusedEqual(Fused, FusedFromQuat(RefQuat), Tol); + ErrA(k) = max(ErrA(k), Error); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Fused representation error', ErrA); + + % + % Test TiltFromAxis + % + + % Begin test + [N, ErrA] = BeginTest('TiltFromAxis', Nnormal); + B = BeginBoolean(); + + % Perform the required testing + for k = 1:N + Vec = RandVec(1,5.0)'; + Ang = 3*RandAng; + B = B && TiltEqual(TiltFromAxis(Vec,0), [0 0 0], 2*eps); + + RefQuat = QuatFromAxis('x',Ang); + [Tilt, Quat] = TiltFromAxis('x', Ang); + B = B && QuatEqual(Quat, RefQuat, Tol); + [~, Error] = TiltEqual(Tilt, TiltFromQuat(RefQuat), Tol); + ErrA(k) = max(ErrA(k), Error); + + RefQuat = QuatFromAxis('y',Ang); + [Tilt, Quat] = TiltFromAxis('y', Ang); + B = B && QuatEqual(Quat, RefQuat, Tol); + [~, Error] = TiltEqual(Tilt, TiltFromQuat(RefQuat), Tol); + ErrA(k) = max(ErrA(k), Error); + + RefQuat = QuatFromAxis('z',Ang); + [Tilt, Quat] = TiltFromAxis('z', Ang); + B = B && QuatEqual(Quat, RefQuat, Tol); + [~, Error] = TiltEqual(Tilt, TiltFromQuat(RefQuat), Tol); + ErrA(k) = max(ErrA(k), Error); + + RefQuat = QuatFromAxis(Vec,Ang); + [Tilt, Quat] = TiltFromAxis(Vec, Ang); + B = B && QuatEqual(Quat, RefQuat, Tol); + [~, Error] = TiltEqual(Tilt, TiltFromQuat(RefQuat), Tol); + ErrA(k) = max(ErrA(k), Error); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Tilt representation error', ErrA); + + % + % End of test script + % + + % End test script + EndTestScript('TestFromAxis', P); + + % Set the output pass flag + if nargout >= 1 + Pass = P; + end + + % Clear the function variable workspace + if isOctave + clear -x Pass + else + clearvars -except Pass + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/TestFromFYawBzG.m b/tro2022/transform/RotationsLib/Test/TestFromFYawBzG.m new file mode 100644 index 0000000..9845a22 --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/TestFromFYawBzG.m @@ -0,0 +1,223 @@ +% TestFromFYawBzG.m - Philipp Allgeuer - 14/10/16 +% Tests: *FromFYawBzG +% Assumes: RandUnitVec, FYawOf*, ZVecFrom* +% +% function [Pass] = TestFromFYawBzG(N, Tol, Inter) +% +% N ==> Number of test cases to use in each test +% Tol ==> Numeric tolerance to use for testing +% Inter ==> Boolean flag whether to also run interactive tests +% Pass ==> Boolean flag whether all tests were passed + +% Main function +function [Pass] = TestFromFYawBzG(N, Tol, Inter) + + % Process function inputs + if nargin < 1 || ~isscalar(N) || N < 1 + N = 3000; + end + N = min(round(N),1000000); + if nargin < 2 || Tol <= 0 + Tol = 128*eps; + end + + % Begin test script + [P, Nnormal] = BeginTestScript('TestFromFYawBzG', N, Tol); + + % + % Test EulerFromFYawBzG + % + + % Begin test + [N, ErrA] = BeginTest('EulerFromFYawBzG', Nnormal, 2); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(EulerFromFYawBzG(0, [0 0 1]) == [0 0 0]); + B = B && all(EulerFromFYawBzG(2, [0 0 1]) == [2 0 0]); + B = B && all(EulerFromFYawBzG(0, [0 0 -1]) == [0 0 pi]); + B = B && all(EulerFromFYawBzG(2, [0 0 -1]) == [2 0 pi]); + + % Perform the required testing + for k = 1:N + FYaw = 4*pi*(2*rand - 1); + BzG = RandUnitVec(1,3)'; + + EGB = EulerFromFYawBzG(FYaw, BzG); + + ErrFYaw = FYawOfEuler(EGB) - FYaw; + ErrFYaw = pi - mod(pi - ErrFYaw, 2*pi); + ErrA(k,1) = abs(ErrFYaw); + ErrA(k,2) = norm(ZVecFromEuler(EGB) - BzG); + end + + % End test + fprintf('Using special tolerance %g for error testing.\n\n', 1.5e-8); + P = P & EndBoolean(B); + P = P & EndTest(1.5e-8, 'EGB output correct', ErrA); + + % + % Test FusedFromFYawBzG + % + + % Begin test + [N, ErrA] = BeginTest('FusedFromFYawBzG', Nnormal, 2); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(FusedFromFYawBzG(0, [0 0 1]) == [0 0 0 1]); + B = B && all(FusedFromFYawBzG(2, [0 0 1]) == [2 0 0 1]); + B = B && all(FusedFromFYawBzG(0, [0 0 -1]) == [0 0 0 -1]); + B = B && all(FusedFromFYawBzG(2, [0 0 -1]) == [2 0 0 -1]); + + % Perform the required testing + for k = 1:N + FYaw = 4*pi*(2*rand - 1); + BzG = RandUnitVec(1,3)'; + + FGB = FusedFromFYawBzG(FYaw, BzG); + + ErrFYaw = FGB(1) - FYaw; + ErrFYaw = pi - mod(pi - ErrFYaw, 2*pi); + ErrA(k,1) = abs(ErrFYaw); + ErrA(k,2) = norm(ZVecFromFused(FGB) - BzG); + end + + % End test + fprintf('Using special tolerance %g for error testing.\n\n', 1.5e-8); + P = P & EndBoolean(B); + P = P & EndTest(1.5e-8, 'FGB output correct', ErrA); + + % + % Test QuatFromFYawBzG + % + + % Begin test + [N, ErrA, ErrB] = BeginTest('QuatFromFYawBzG', Nnormal, [1 2]); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(QuatFromFYawBzG(0, [0 0 1]) == [1 0 0 0]); + B = B && all(QuatFromFYawBzG(0, [0 0 -1]) == [0 1 0 0]); + B = B && all(QuatFromFYawBzG(2, [0 0 -1]) == [0 1 0 0]); + + % Perform the required testing + for k = 1:N + FYaw = 4*pi*(2*rand - 1); + BzG = RandUnitVec(1,3)'; + if BzG(3) <= -1 + continue; + end + + qGB = QuatFromFYawBzG(FYaw, BzG); + + ErrA(k,1) = abs(norm(qGB) - 1); + + ErrFYaw = FYawOfQuat(qGB) - FYaw; + ErrFYaw = pi - mod(pi - ErrFYaw, 2*pi); + ErrB(k,1) = abs(ErrFYaw); + ErrB(k,2) = norm(ZVecFromQuat(qGB) - BzG); + end + + % End test + fprintf('Using special tolerance %g for error testing.\n\n', 1.5e-8); + P = P & EndBoolean(B); + P = P & EndTest(1.5e-8, 'qGB valid', ErrA, 'qGB output correct', ErrB); + + % + % Test RotmatFromFYawBzG + % + + % Begin test + [N, ErrA, ErrB, ErrC, ErrD] = BeginTest('RotmatFromFYawBzG', Nnormal, [2 1 2 2]); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(all(RotmatFromFYawBzG(0, [0 0 1]) == eye(3))); + B = B && all(all(RotmatFromFYawBzG(0, [0 0 -1]) == diag([1 -1 -1]))); + B = B && all(all(RotmatFromFYawBzG(2, [0 0 -1]) == diag([1 -1 -1]))); + + % Perform the required testing + for k = 1:N + FYaw = 4*pi*(2*rand - 1); + BzG = RandUnitVec(1,3)'; + if BzG(3) <= -1 + continue; + end + + [RGB, qGB] = RotmatFromFYawBzG(FYaw, BzG); + + ErrA(k,1) = norm(RGB'*RGB - eye(3)); + ErrA(k,2) = abs(det(RGB) - 1); + + ErrB(k,1) = abs(norm(qGB) - 1); + + ErrFYaw = FYawOfRotmat(RGB) - FYaw; + ErrFYaw = pi - mod(pi - ErrFYaw, 2*pi); + ErrC(k,1) = abs(ErrFYaw); + ErrC(k,2) = norm(RGB(3,:) - BzG); + + ErrFYaw = FYawOfQuat(qGB) - FYaw; + ErrFYaw = pi - mod(pi - ErrFYaw, 2*pi); + ErrD(k,1) = abs(ErrFYaw); + ErrD(k,2) = norm(ZVecFromQuat(qGB) - BzG); + end + + % End test + fprintf('Using special tolerance %g for error testing.\n\n', 1.5e-8); + P = P & EndBoolean(B); + P = P & EndTest(1.5e-8, 'RGB valid', ErrA, 'qGB valid', ErrB, 'RGB output correct', ErrC, 'qGB output correct', ErrD); + + % + % Test TiltFromFYawBzG + % + + % Begin test + [N, ErrA] = BeginTest('TiltFromFYawBzG', Nnormal, 2); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(TiltFromFYawBzG(0, [0 0 1]) == [0 0 0]); + B = B && all(TiltFromFYawBzG(2, [0 0 1]) == [2 0 0]); + B = B && all(TiltFromFYawBzG(0, [0 0 -1]) == [0 0 pi]); + B = B && all(TiltFromFYawBzG(2, [0 0 -1]) == [2 0 pi]); + + % Perform the required testing + for k = 1:N + FYaw = 4*pi*(2*rand - 1); + BzG = RandUnitVec(1,3)'; + + TGB = TiltFromFYawBzG(FYaw, BzG); + + ErrFYaw = TGB(1) - FYaw; + ErrFYaw = pi - mod(pi - ErrFYaw, 2*pi); + ErrA(k,1) = abs(ErrFYaw); + ErrA(k,2) = norm(ZVecFromTilt(TGB) - BzG); + end + + % End test + fprintf('Using special tolerance %g for error testing.\n\n', 1.5e-8); + P = P & EndBoolean(B); + P = P & EndTest(1.5e-8, 'TGB output correct', ErrA); + + % + % End of test script + % + + % End test script + EndTestScript('TestFromFYawBzG', P); + + % Set the output pass flag + if nargout >= 1 + Pass = P; + end + + % Clear the function variable workspace + if isOctave + clear -x Pass + else + clearvars -except Pass + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/TestFromFYawGzB.m b/tro2022/transform/RotationsLib/Test/TestFromFYawGzB.m new file mode 100644 index 0000000..6c5606a --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/TestFromFYawGzB.m @@ -0,0 +1,228 @@ +% TestFromFYawGzB.m - Philipp Allgeuer - 27/10/16 +% Tests: *FromFYawGzB +% Assumes: RandUnitVec, FYawOf*, RotmatFrom* +% +% function [Pass] = TestFromFYawGzB(N, Tol, Inter) +% +% N ==> Number of test cases to use in each test +% Tol ==> Numeric tolerance to use for testing +% Inter ==> Boolean flag whether to also run interactive tests +% Pass ==> Boolean flag whether all tests were passed + +% Main function +function [Pass] = TestFromFYawGzB(N, Tol, Inter) + + % Process function inputs + if nargin < 1 || ~isscalar(N) || N < 1 + N = 3000; + end + N = min(round(N),1000000); + if nargin < 2 || Tol <= 0 + Tol = 128*eps; + end + + % Begin test script + [P, Nnormal] = BeginTestScript('TestFromFYawGzB', N, Tol); + + % + % Test EulerFromFYawGzB + % + + % Begin test + [N, ErrA] = BeginTest('EulerFromFYawGzB', Nnormal, 2); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(EulerFromFYawGzB(0, [0 0 1]) == [0 0 0]); + B = B && all(EulerFromFYawGzB(2, [0 0 1]) == [2 0 0]); + B = B && all(EulerFromFYawGzB(0, [0 0 -1]) == [0 0 pi]); + B = B && all(EulerFromFYawGzB(2, [0 0 -1]) == [2 0 pi]); + + % Perform the required testing + for k = 1:N + FYaw = 4*pi*(2*rand - 1); + GzB = RandUnitVec(1,3); + + EGB = EulerFromFYawGzB(FYaw, GzB); + RGB = RotmatFromEuler(EGB); + + ErrFYaw = FYawOfEuler(EGB) - FYaw; + ErrFYaw = pi - mod(pi - ErrFYaw, 2*pi); + ErrA(k,1) = abs(ErrFYaw); + ErrA(k,2) = norm(RGB(:,3) - GzB); + end + + % End test + fprintf('Using special tolerance %g for error testing.\n\n', 1.5e-8); + P = P & EndBoolean(B); + P = P & EndTest(1.5e-8, 'EGB output correct', ErrA); + + % + % Test FusedFromFYawGzB + % + + % Begin test + [N, ErrA] = BeginTest('FusedFromFYawGzB', Nnormal, 2); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(FusedFromFYawGzB(0, [0 0 1]) == [0 0 0 1]); + B = B && all(FusedFromFYawGzB(2, [0 0 1]) == [2 0 0 1]); + B = B && all(FusedFromFYawGzB(0, [0 0 -1]) == [0 0 0 -1]); + B = B && all(FusedFromFYawGzB(2, [0 0 -1]) == [2 0 0 -1]); + + % Perform the required testing + for k = 1:N + FYaw = 4*pi*(2*rand - 1); + GzB = RandUnitVec(1,3); + + FGB = FusedFromFYawGzB(FYaw, GzB); + RGB = RotmatFromFused(FGB); + + ErrFYaw = FGB(1) - FYaw; + ErrFYaw = pi - mod(pi - ErrFYaw, 2*pi); + ErrA(k,1) = abs(ErrFYaw); + ErrA(k,2) = norm(RGB(:,3) - GzB); + end + + % End test + fprintf('Using special tolerance %g for error testing.\n\n', 1.5e-8); + P = P & EndBoolean(B); + P = P & EndTest(1.5e-8, 'FGB output correct', ErrA); + + % + % Test QuatFromFYawGzB + % + + % Begin test + [N, ErrA, ErrB] = BeginTest('QuatFromFYawGzB', Nnormal, [1 2]); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(QuatFromFYawGzB(0, [0 0 1]) == [1 0 0 0]); + B = B && all(QuatFromFYawGzB(0, [0 0 -1]) == [0 1 0 0]); + B = B && all(QuatFromFYawGzB(2, [0 0 -1]) == [0 1 0 0]); + + % Perform the required testing + for k = 1:N + FYaw = 4*pi*(2*rand - 1); + GzB = RandUnitVec(1,3); + if GzB(3) <= -1 + continue; + end + + qGB = QuatFromFYawGzB(FYaw, GzB); + RGB = RotmatFromQuat(qGB); + + ErrA(k,1) = abs(norm(qGB) - 1); + + ErrFYaw = FYawOfQuat(qGB) - FYaw; + ErrFYaw = pi - mod(pi - ErrFYaw, 2*pi); + ErrB(k,1) = abs(ErrFYaw); + ErrB(k,2) = norm(RGB(:,3) - GzB); + end + + % End test + fprintf('Using special tolerance %g for error testing.\n\n', 1.5e-8); + P = P & EndBoolean(B); + P = P & EndTest(1.5e-8, 'qGB valid', ErrA, 'qGB output correct', ErrB); + + % + % Test RotmatFromFYawGzB + % + + % Begin test + [N, ErrA, ErrB, ErrC, ErrD] = BeginTest('RotmatFromFYawGzB', Nnormal, [2 1 2 2]); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(all(RotmatFromFYawGzB(0, [0 0 1]) == eye(3))); + B = B && all(all(RotmatFromFYawGzB(0, [0 0 -1]) == diag([1 -1 -1]))); + B = B && all(all(RotmatFromFYawGzB(2, [0 0 -1]) == diag([1 -1 -1]))); + + % Perform the required testing + for k = 1:N + FYaw = 4*pi*(2*rand - 1); + GzB = RandUnitVec(1,3); + if GzB(3) <= -1 + continue; + end + + [RGB, qGB] = RotmatFromFYawGzB(FYaw, GzB); + RGBq = RotmatFromQuat(qGB); + + ErrA(k,1) = norm(RGB'*RGB - eye(3)); + ErrA(k,2) = abs(det(RGB) - 1); + + ErrB(k,1) = abs(norm(qGB) - 1); + + ErrFYaw = FYawOfRotmat(RGB) - FYaw; + ErrFYaw = pi - mod(pi - ErrFYaw, 2*pi); + ErrC(k,1) = abs(ErrFYaw); + ErrC(k,2) = norm(RGB(:,3) - GzB); + + ErrFYaw = FYawOfQuat(qGB) - FYaw; + ErrFYaw = pi - mod(pi - ErrFYaw, 2*pi); + ErrD(k,1) = abs(ErrFYaw); + ErrD(k,2) = norm(RGBq(:,3) - GzB); + end + + % End test + fprintf('Using special tolerance %g for error testing.\n\n', 1.5e-8); + P = P & EndBoolean(B); + P = P & EndTest(1.5e-8, 'RGB valid', ErrA, 'qGB valid', ErrB, 'RGB output correct', ErrC, 'qGB output correct', ErrD); + + % + % Test TiltFromFYawGzB + % + + % Begin test + [N, ErrA] = BeginTest('TiltFromFYawGzB', Nnormal, 2); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(TiltFromFYawGzB(0, [0 0 1]) == [0 0 0]); + B = B && all(TiltFromFYawGzB(2, [0 0 1]) == [2 0 0]); + B = B && all(TiltFromFYawGzB(0, [0 0 -1]) == [0 0 pi]); + B = B && all(TiltFromFYawGzB(2, [0 0 -1]) == [2 0 pi]); + + % Perform the required testing + for k = 1:N + FYaw = 4*pi*(2*rand - 1); + GzB = RandUnitVec(1,3); + + TGB = TiltFromFYawGzB(FYaw, GzB); + RGB = RotmatFromTilt(TGB); + + ErrFYaw = TGB(1) - FYaw; + ErrFYaw = pi - mod(pi - ErrFYaw, 2*pi); + ErrA(k,1) = abs(ErrFYaw); + ErrA(k,2) = norm(RGB(:,3) - GzB); + end + + % End test + fprintf('Using special tolerance %g for error testing.\n\n', 1.5e-8); + P = P & EndBoolean(B); + P = P & EndTest(1.5e-8, 'TGB output correct', ErrA); + + % + % End of test script + % + + % End test script + EndTestScript('TestFromFYawGzB', P); + + % Set the output pass flag + if nargout >= 1 + Pass = P; + end + + % Clear the function variable workspace + if isOctave + clear -x Pass + else + clearvars -except Pass + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/TestFromFYawTilt.m b/tro2022/transform/RotationsLib/Test/TestFromFYawTilt.m new file mode 100644 index 0000000..8d33e4c --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/TestFromFYawTilt.m @@ -0,0 +1,84 @@ +% TestFromFYawTilt.m - Philipp Allgeuer - 05/05/17 +% Tests: *FromFYawTilt +% Assumes: RandTilt, RandQuat, QuatFromTilt, QuatEqual, QuatMult, FYawOfQuat, QuatNoFYaw +% +% function [Pass] = TestFromFYawTilt(N, Tol, Inter) +% +% N ==> Number of test cases to use in each test +% Tol ==> Numeric tolerance to use for testing +% Inter ==> Boolean flag whether to also run interactive tests +% Pass ==> Boolean flag whether all tests were passed + +% Main function +function [Pass] = TestFromFYawTilt(N, Tol, Inter) + + % Process function inputs + if nargin < 1 || ~isscalar(N) || N < 1 + N = 3000; + end + N = min(round(N),1000000); + if nargin < 2 || Tol <= 0 + Tol = 128*eps; + end + + % Begin test script + [P, Nnormal] = BeginTestScript('TestFromFYawTilt', N, Tol); + + % + % Test QuatFromFYawTilt + % + + % Begin test + [N, ErrA, ErrB, ErrC] = BeginTest('QuatFromFYawTilt', Nnormal, [3 3 3]); + + % Perform the required testing + for k = 1:N + FYawG = 4*pi*(2*rand - 1); + TiltH1 = RandTilt; + TiltH2 = TiltH1(2:3); + TiltH3 = QuatFromTilt(TiltH1); + qGH = RandQuat; + + [qHB1, qGB1] = QuatFromFYawTilt(FYawG, TiltH1, qGH); + [qHB2, qGB2] = QuatFromFYawTilt(FYawG, TiltH2, qGH); + [qHB3, qGB3] = QuatFromFYawTilt(FYawG, TiltH3, qGH); + + [~, ErrA(k,1)] = QuatEqual(QuatMult(qGH, qHB1), qGB1); + [~, ErrA(k,2)] = QuatEqual(QuatMult(qGH, qHB2), qGB2); + [~, ErrA(k,3)] = QuatEqual(QuatMult(qGH, qHB3), qGB3); + + ErrB(k,1) = norm(pi - mod(pi - (FYawOfQuat(qGB1) - FYawG), 2*pi)); + ErrB(k,2) = norm(pi - mod(pi - (FYawOfQuat(qGB2) - FYawG), 2*pi)); + ErrB(k,3) = norm(pi - mod(pi - (FYawOfQuat(qGB3) - FYawG), 2*pi)); + + qRef = QuatFromTilt([0 TiltH2]); + [~, ErrC(k,1)] = QuatEqual(QuatNoFYaw(qHB1), qRef); + [~, ErrC(k,2)] = QuatEqual(QuatNoFYaw(qHB2), qRef); + [~, ErrC(k,3)] = QuatEqual(QuatNoFYaw(qHB3), qRef); + end + + % End test + fprintf('Using special tolerance %g for error testing.\n\n', 1.5e-8); + P = P & EndTest(1.5e-8, 'Self-consistency', ErrA, 'FYaw correct', ErrB, 'Tilt correct', ErrC); + + % + % End of test script + % + + % End test script + EndTestScript('TestFromFYawTilt', P); + + % Set the output pass flag + if nargout >= 1 + Pass = P; + end + + % Clear the function variable workspace + if isOctave + clear -x Pass + else + clearvars -except Pass + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/TestFromZVec.m b/tro2022/transform/RotationsLib/Test/TestFromZVec.m new file mode 100644 index 0000000..893958e --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/TestFromZVec.m @@ -0,0 +1,183 @@ +% TestFromZVec.m - Philipp Allgeuer - 21/10/16 +% Tests: FromZVec* +% Assumes: RandUnitVec, ZVecFrom*, FYawOf* +% +% function [Pass] = TestFromZVec(N, Tol, Inter) +% +% N ==> Number of test cases to use in each test +% Tol ==> Numeric tolerance to use for testing +% Inter ==> Boolean flag whether to also run interactive tests +% Pass ==> Boolean flag whether all tests were passed + +% Main function +function [Pass] = TestFromZVec(N, Tol, Inter) + + % Process function inputs + if nargin < 1 || ~isscalar(N) || N < 1 + N = 3000; + end + N = min(round(N),1000000); + if nargin < 2 || Tol <= 0 + Tol = 128*eps; + end + + % Begin test script + [P, Nnormal] = BeginTestScript('TestFromZVec', N, Tol); + + % + % Test EulerFromZVec + % + + % Begin test + [N, ErrA] = BeginTest('EulerFromZVec', Nnormal, 1); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(EulerFromZVec([0 0 1]) == [0 0 0]); + B = B && all(EulerFromZVec([0 0 -1]) == [0 0 pi]); + + % Perform the required testing + for k = 1:N + ZVec = RandUnitVec(1,3)'; + Euler = EulerFromZVec(ZVec); + + ErrA(k,1) = norm(ZVecFromEuler(Euler) - ZVec); + B = B && (Euler(1) == 0); + end + + % End test + fprintf('Using special tolerance %g for error testing.\n\n', 1.5e-8); + P = P & EndBoolean(B); + P = P & EndTest(1.5e-8, 'Output correct', ErrA); + + % + % Test FusedFromZVec + % + + % Begin test + [N, ErrA] = BeginTest('FusedFromZVec', Nnormal, 1); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(FusedFromZVec([0 0 1]) == [0 0 0 1]); + B = B && all(FusedFromZVec([0 0 -1]) == [0 0 0 -1]); + + % Perform the required testing + for k = 1:N + ZVec = RandUnitVec(1,3)'; + Fused = FusedFromZVec(ZVec); + + ErrA(k,1) = norm(ZVecFromFused(Fused) - ZVec); + B = B && (Fused(1) == 0); + end + + % End test + fprintf('Using special tolerance %g for error testing.\n\n', 1.5e-8); + P = P & EndBoolean(B); + P = P & EndTest(1.5e-8, 'Output correct', ErrA); + + % + % Test QuatFromZVec + % + + % Begin test + [N, ErrA, ErrB] = BeginTest('QuatFromZVec', Nnormal, [1 1]); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(QuatFromZVec([0 0 1]) == [1 0 0 0]); + B = B && all(QuatFromZVec([0 0 -1]) == [0 1 0 0]); + + % Perform the required testing + for k = 1:N + ZVec = RandUnitVec(1,3)'; + Quat = QuatFromZVec(ZVec); + + ErrA(k,1) = norm(ZVecFromQuat(Quat) - ZVec); + ErrB(k,1) = abs(norm(Quat) - 1); + B = B && (Quat(4) == 0); + end + + % End test + fprintf('Using special tolerance %g for error testing.\n\n', 1.5e-8); + P = P & EndBoolean(B); + P = P & EndTest(1.5e-8, 'Output correct', ErrA, 'Output valid', ErrB); + + % + % Test RotmatFromZVec + % + + % Begin test + [N, ErrA, ErrB] = BeginTest('RotmatFromZVec', Nnormal, [3 3]); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(all(RotmatFromZVec([0 0 1]) == eye(3))); + B = B && all(all(RotmatFromZVec([0 0 -1]) == diag([1 -1 -1]))); + + % Perform the required testing + for k = 1:N + ZVec = RandUnitVec(1,3)'; + [Rotmat, Quat] = RotmatFromZVec(ZVec); + + ErrA(k,1) = abs(FYawOfRotmat(Rotmat)); + ErrA(k,2) = norm(Rotmat(3,:) - ZVec); + ErrA(k,3) = norm(ZVecFromQuat(Quat) - ZVec); + ErrB(k,1) = norm(Rotmat'*Rotmat - eye(3)); + ErrB(k,2) = abs(det(Rotmat) - 1); + ErrB(k,3) = abs(norm(Quat) - 1); + B = B && (Quat(4) == 0); + end + + % End test + fprintf('Using special tolerance %g for error testing.\n\n', 1.5e-8); + P = P & EndBoolean(B); + P = P & EndTest(1.5e-8, 'Output correct', ErrA, 'Output valid', ErrB); + + % + % Test TiltFromZVec + % + + % Begin test + [N, ErrA] = BeginTest('TiltFromZVec', Nnormal, 1); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(TiltFromZVec([0 0 1]) == [0 0 0]); + B = B && all(TiltFromZVec([0 0 -1]) == [0 0 pi]); + + % Perform the required testing + for k = 1:N + ZVec = RandUnitVec(1,3)'; + Tilt = TiltFromZVec(ZVec); + + ErrA(k,1) = norm(ZVecFromTilt(Tilt) - ZVec); + B = B && (Tilt(1) == 0); + end + + % End test + fprintf('Using special tolerance %g for error testing.\n\n', 1.5e-8); + P = P & EndBoolean(B); + P = P & EndTest(1.5e-8, 'Output correct', ErrA); + + % + % End of test script + % + + % End test script + EndTestScript('TestFromZVec', P); + + % Set the output pass flag + if nargout >= 1 + Pass = P; + end + + % Clear the function variable workspace + if isOctave + clear -x Pass + else + clearvars -except Pass + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/TestIdentity.m b/tro2022/transform/RotationsLib/Test/TestIdentity.m new file mode 100644 index 0000000..d861018 --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/TestIdentity.m @@ -0,0 +1,66 @@ +% TestIdentity.m - Philipp Allgeuer - 05/11/14 +% Tests: *Identity +% Assumes: None +% +% function [Pass] = TestIdentity(N, Tol, Inter) +% +% N ==> Number of test cases to use in each test +% Tol ==> Numeric tolerance to use for testing +% Inter ==> Boolean flag whether to also run interactive tests +% Pass ==> Boolean flag whether all tests were passed + +% Main function +function [Pass] = TestIdentity(N, Tol, Inter) + + % Process function inputs + if nargin < 1 || ~isscalar(N) || N < 1 + N = 1; + end + N = min(round(N),1000000); + if nargin < 2 || Tol <= 0 + Tol = 128*eps; + end + + % Begin test script + P = BeginTestScript('TestIdentity', N, Tol); + + % + % Test *Identity + % + + % Begin test + BeginTest('*Identity'); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(EulerIdentity == [0 0 0]); + B = B && all(FusedIdentity == [0 0 0 1]); + B = B && all(QuatIdentity == [1 0 0 0]); + B = B && all(all(RotmatIdentity == eye(3))); + B = B && all(TiltIdentity == [0 0 0]); + + % End test + P = P & EndBoolean(B); + P = P & EndTest(); + + % + % End of test script + % + + % End test script + EndTestScript('TestIdentity', P); + + % Set the output pass flag + if nargout >= 1 + Pass = P; + end + + % Clear the function variable workspace + if isOctave + clear -x Pass + else + clearvars -except Pass + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/TestInv.m b/tro2022/transform/RotationsLib/Test/TestInv.m new file mode 100644 index 0000000..0d7c00f --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/TestInv.m @@ -0,0 +1,183 @@ +% TestInv.m - Philipp Allgeuer - 05/11/14 +% Tests: *Inv +% Assumes: Rand*, QuatFrom*, Compose*, *Equal +% +% function [Pass] = TestInv(N, Tol, Inter) +% +% N ==> Number of test cases to use in each test +% Tol ==> Numeric tolerance to use for testing +% Inter ==> Boolean flag whether to also run interactive tests +% Pass ==> Boolean flag whether all tests were passed + +% Main function +function [Pass] = TestInv(N, Tol, Inter) + + % Process function inputs + if nargin < 1 || ~isscalar(N) || N < 1 + N = 1000; + end + N = min(round(N),1000000); + if nargin < 2 || Tol <= 0 + Tol = 128*eps; + end + + % Begin test script + [P, Nnormal] = BeginTestScript('TestInv', N, Tol); + + % + % Test EulerInv + % + + % Begin test + [N, ErrA, ErrB] = BeginTest('EulerInv', Nnormal); + B = BeginBoolean(); + + % Set the identity + Ident = [0 0 0]; + + % Boolean conditions + B = B && EulerEqual(EulerInv(Ident), Ident, Tol); + + % Calculate inversion errors + for k = 1:N + Er = RandEuler; + Erinv = EulerInv(Er); + QEr = QuatFromEuler(Er); + QErinv = QuatFromEuler(Erinv); + [~, ErrA(k)] = QuatEqual(ComposeQuat(QEr,QErinv), [1 0 0 0], Tol); + [~, ErrB(k)] = QuatEqual(ComposeQuat(QErinv,QEr), [1 0 0 0], Tol); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(4*Tol, 'Composition E*inv(E)', ErrA, 'Composition inv(E)*E', ErrB); + + % + % Test FusedInv + % + + % Begin test + [N, ErrA, ErrB] = BeginTest('FusedInv', Nnormal); + B = BeginBoolean(); + + % Set the identity + Ident = [0 0 0 1]; + + % Boolean conditions + B = B && FusedEqual(FusedInv(Ident), Ident, Tol); + + % Calculate inversion errors + for k = 1:N + Fr = RandFused; + Frinv = FusedInv(Fr); + QFr = QuatFromFused(Fr); + QFrinv = QuatFromFused(Frinv); + [~, ErrA(k)] = QuatEqual(ComposeQuat(QFr,QFrinv), [1 0 0 0], Tol); + [~, ErrB(k)] = QuatEqual(ComposeQuat(QFrinv,QFr), [1 0 0 0], Tol); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(4*Tol, 'Composition F*inv(F)', ErrA, 'Composition inv(F)*F', ErrB); + + % + % Test QuatInv + % + + % Begin test + [N, ErrA, ErrB] = BeginTest('QuatInv', Nnormal); + B = BeginBoolean(); + + % Set the identity + Ident = [1 0 0 0]; + + % Boolean conditions + B = B && QuatEqual(QuatInv(Ident), Ident, Tol); + + % Calculate inversion errors + for k = 1:N + Qr = RandQuat; + Qrinv = QuatInv(Qr); + [~, ErrA(k)] = QuatEqual(ComposeQuat(Qr,Qrinv), Ident, Tol); + [~, ErrB(k)] = QuatEqual(ComposeQuat(Qrinv,Qr), Ident, Tol); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Composition Q*inv(Q)', ErrA, 'Composition inv(Q)*Q', ErrB); + + % + % Test RotmatInv + % + + % Begin test + [N, ErrA, ErrB] = BeginTest('RotmatInv', Nnormal); + B = BeginBoolean(); + + % Set the identity + Ident = eye(3); + + % Boolean conditions + B = B && RotmatEqual(RotmatInv(Ident), Ident, Tol); + + % Calculate inversion errors + for k = 1:N + Rr = RandRotmat; + Rrinv = RotmatInv(Rr); + [~, ErrA(k)] = RotmatEqual(ComposeRotmat(Rr,Rrinv), Ident, Tol); + [~, ErrB(k)] = RotmatEqual(ComposeRotmat(Rrinv,Rr), Ident, Tol); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Composition R*inv(R)', ErrA, 'Composition inv(R)*R', ErrB); + + % + % Test TiltInv + % + + % Begin test + [N, ErrA, ErrB] = BeginTest('TiltInv', Nnormal); + B = BeginBoolean(); + + % Set the identity + Ident = [0 0 0]; + + % Boolean conditions + B = B && TiltEqual(TiltInv(Ident), Ident, Tol); + + % Calculate inversion errors + for k = 1:N + Tr = RandTilt; + Trinv = TiltInv(Tr); + QTr = QuatFromTilt(Tr); + QTrinv = QuatFromTilt(Trinv); + [~, ErrA(k)] = QuatEqual(ComposeQuat(QTr,QTrinv), [1 0 0 0], Tol); + [~, ErrB(k)] = QuatEqual(ComposeQuat(QTrinv,QTr), [1 0 0 0], Tol); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(4*Tol, 'Composition T*inv(T)', ErrA, 'Composition inv(T)*T', ErrB); + + % + % End of test script + % + + % End test script + EndTestScript('TestInv', P); + + % Set the output pass flag + if nargout >= 1 + Pass = P; + end + + % Clear the function variable workspace + if isOctave + clear -x Pass + else + clearvars -except Pass + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/TestNoEYaw.m b/tro2022/transform/RotationsLib/Test/TestNoEYaw.m new file mode 100644 index 0000000..874d4c1 --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/TestNoEYaw.m @@ -0,0 +1,158 @@ +% TestNoEYaw.m - Philipp Allgeuer - 05/11/14 +% Tests: *NoEYaw +% Assumes: Rand*, *Equal, EulerFrom*, RotmatFromQuat +% +% function [Pass] = TestNoEYaw(N, Tol, Inter) +% +% N ==> Number of test cases to use in each test +% Tol ==> Numeric tolerance to use for testing +% Inter ==> Boolean flag whether to also run interactive tests +% Pass ==> Boolean flag whether all tests were passed + +% Main function +function [Pass] = TestNoEYaw(N, Tol, Inter) + + % Process function inputs + if nargin < 1 || ~isscalar(N) || N < 1 + N = 1600; + end + N = min(round(N),1000000); + if nargin < 2 || Tol <= 0 + Tol = 128*eps; + end + + % Begin test script + [P, Nnormal] = BeginTestScript('TestNoEYaw', N, Tol); + + % + % Test EulerNoEYaw + % + + % Begin test + N = BeginTest('EulerNoEYaw', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(EulerNoEYaw([0 0 0]) == [0 0 0]); + for k = 1:N + Er = RandEuler; + B = B && all(EulerNoEYaw(Er) == [0 Er(2:3)]); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(); + + % + % Test FusedNoEYaw + % + + % Begin test + [N, ErrA] = BeginTest('FusedNoEYaw', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(FusedNoEYaw([0 0 0 1]) == [0 0 0 1]); + + % Perform the required testing + for k = 1:N + Fr = RandFused; + Er = EulerFromFused(Fr); + Enoyaw = EulerFromFused(FusedNoEYaw(Fr)); + [~, ErrA(k)] = EulerEqual([0 Er(2:3)], Enoyaw, Tol); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Euler representation error', ErrA); + + % + % Test QuatNoEYaw + % + + % Begin test + [N, ErrA] = BeginTest('QuatNoEYaw', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(QuatNoEYaw([1 0 0 0]) == [1 0 0 0]); + + % Perform the required testing + for k = 1:N + Qr = RandQuat; + Rr = RotmatFromQuat(Qr); + Rnoyaw = RotmatFromQuat(QuatNoEYaw(Qr)); + [~, ErrA(k)] = RotmatEqual(RotmatNoEYaw(Rr), Rnoyaw, Tol); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Rotmat representation error', ErrA); + + % + % Test RotmatNoEYaw + % + + % Begin test + [N, ErrA] = BeginTest('RotmatNoEYaw', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(all(RotmatNoEYaw(eye(3)) == eye(3))); + + % Perform the required testing + for k = 1:N + Rr = RandRotmat; + Er = EulerFromRotmat(Rr); + Enoyaw = EulerFromRotmat(RotmatNoEYaw(Rr)); + [~, ErrA(k)] = EulerEqual([0 Er(2:3)], Enoyaw, Tol); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Euler representation error', ErrA); + + % + % Test TiltNoEYaw + % + + % Begin test + [N, ErrA] = BeginTest('TiltNoEYaw', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(TiltNoEYaw([0 0 0]) == [0 0 0]); + + % Perform the required testing + for k = 1:N + Tr = RandTilt; + Er = EulerFromTilt(Tr); + Enoyaw = EulerFromTilt(TiltNoEYaw(Tr)); + [~, ErrA(k)] = EulerEqual([0 Er(2:3)], Enoyaw, Tol); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Euler representation error', ErrA); + + % + % End of test script + % + + % End test script + EndTestScript('TestNoEYaw', P); + + % Set the output pass flag + if nargout >= 1 + Pass = P; + end + + % Clear the function variable workspace + if isOctave + clear -x Pass + else + clearvars -except Pass + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/TestNoFYaw.m b/tro2022/transform/RotationsLib/Test/TestNoFYaw.m new file mode 100644 index 0000000..d7c39a4 --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/TestNoFYaw.m @@ -0,0 +1,154 @@ +% TestNoFYaw.m - Philipp Allgeuer - 05/11/14 +% Tests: *NoFYaw +% Assumes: Rand*, FusedEqual, FusedFrom* +% +% function [Pass] = TestNoFYaw(N, Tol, Inter) +% +% N ==> Number of test cases to use in each test +% Tol ==> Numeric tolerance to use for testing +% Inter ==> Boolean flag whether to also run interactive tests +% Pass ==> Boolean flag whether all tests were passed + +% Main function +function [Pass] = TestNoFYaw(N, Tol, Inter) + + % Process function inputs + if nargin < 1 || ~isscalar(N) || N < 1 + N = 1500; + end + N = min(round(N),1000000); + if nargin < 2 || Tol <= 0 + Tol = 128*eps; + end + + % Begin test script + [P, Nnormal] = BeginTestScript('TestNoFYaw', N, Tol); + + % + % Test EulerNoFYaw + % + + % Begin test + [N, ErrA] = BeginTest('EulerNoFYaw', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(EulerNoFYaw([0 0 0]) == [0 0 0]); + + % Perform the required testing + for k = 1:N + Er = RandEuler; + Fr = FusedFromEuler(Er); + Fnoyaw = FusedFromEuler(EulerNoFYaw(Er)); + [~, ErrA(k)] = FusedEqual([0 Fr(2:4)], Fnoyaw, Tol); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Fused representation error', ErrA); + + % + % Test FusedNoFYaw + % + + % Begin test + N = BeginTest('FusedNoFYaw', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(FusedNoFYaw([0 0 0 1]) == [0 0 0 1]); + for k = 1:N + Fr = RandFused; + B = B && all(FusedNoFYaw(Fr) == [0 Fr(2:4)]); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(); + + % + % Test QuatNoFYaw + % + + % Begin test + [N, ErrA] = BeginTest('QuatNoFYaw', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(QuatNoFYaw([1 0 0 0]) == [1 0 0 0]); + + % Perform the required testing + for k = 1:N + Qr = RandQuat; + Fr = FusedFromQuat(Qr); + Fnoyaw = FusedFromQuat(QuatNoFYaw(Qr)); + [~, ErrA(k)] = FusedEqual([0 Fr(2:4)], Fnoyaw, Tol); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Fused representation error', ErrA); + + % + % Test RotmatNoFYaw + % + + % Begin test + [N, ErrA] = BeginTest('RotmatNoFYaw', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(all(RotmatNoFYaw(eye(3)) == eye(3))); + + % Perform the required testing + for k = 1:N + Rr = RandRotmat; + Fr = FusedFromRotmat(Rr); + Fnoyaw = FusedFromRotmat(RotmatNoFYaw(Rr)); + [~, ErrA(k)] = FusedEqual([0 Fr(2:4)], Fnoyaw, Tol); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Fused representation error', ErrA); + + % + % Test TiltNoFYaw + % + + % Begin test + N = BeginTest('TiltNoFYaw', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(TiltNoFYaw([0 0 0]) == [0 0 0]); + for k = 1:N + Tr = RandTilt; + B = B && all(TiltNoFYaw(Tr) == [0 Tr(2:3)]); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(); + + % + % End of test script + % + + % End test script + EndTestScript('TestNoFYaw', P); + + % Set the output pass flag + if nargout >= 1 + Pass = P; + end + + % Clear the function variable workspace + if isOctave + clear -x Pass + else + clearvars -except Pass + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/TestPlotCsys.m b/tro2022/transform/RotationsLib/Test/TestPlotCsys.m new file mode 100644 index 0000000..a6d4af8 --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/TestPlotCsys.m @@ -0,0 +1,134 @@ +% TestPlotCsys.m - Philipp Allgeuer - 05/11/14 +% Tests: PlotCsys +% Assumes: Rand* +% +% function [Pass] = TestPlotCsys(N, Tol, Inter) +% +% N ==> Number of test cases to use in each test +% Tol ==> Numeric tolerance to use for testing +% Inter ==> Boolean flag whether to also run interactive tests +% Pass ==> Boolean flag whether all tests were passed + +% Main function +function [Pass] = TestPlotCsys(N, Tol, Inter) + + % Process function inputs + if nargin < 1 || ~isscalar(N) || N < 1 + N = 1; + end + N = min(round(N),1000000); + if nargin < 2 || Tol <= 0 + Tol = 128*eps; + end + + % Begin test script + P = BeginTestScript('TestPlotCsys', N, Tol); + + % + % Interactive testing + % + + % Run the interactive testing component + if Inter + BeginTest('Interactive'); + B = BeginBoolean(); + B = B && TestPlotCsysInter(); + P = P & EndBoolean(B); + P = P & EndTest(); + end + + % + % End of test script + % + + % End test script + EndTestScript('TestPlotCsys', P); + + % Set the output pass flag + if nargout >= 1 + Pass = P; + end + + % Clear the function variable workspace + if isOctave + clear -x Pass + else + clearvars -except Pass + end + +end + +% Interactive testing component +function [B] = TestPlotCsysInter() + + % Initialise variables + B = true; + h = []; + + % Plot 1 + h = [h figure()]; + PlotCsys; + hold on; + PlotCsys(RandRotmat,'R'); + PlotCsys(RandQuat,'Q'); + hold off; + title('PlotCsys: Plot 1'); + xlabel('x \rightarrow'); + ylabel('y \rightarrow'); + zlabel('z \rightarrow'); + grid on; + disp('Plot 1 should contain:'); + disp(' - Identity csys with labels {x,y,z}'); + disp(' - Random csys with labels {x_R,y_R,z_R}'); + disp(' - Random csys with labels {x_Q,y_Q,z_Q}'); + disp(' '); + + % Plot 2 + h = [h figure()]; + PlotCsys(eye(3),'G',[0 0 0],3); + hold on; + PlotCsys(RandRotmat,'R',[-2 2 1]); + PlotCsys(RandQuat,'Qs',[1 -1 0],[2 1 1.5]); + hold off; + title('PlotCsys: Plot 2'); + xlabel('x \rightarrow'); + ylabel('y \rightarrow'); + zlabel('z \rightarrow'); + grid on; + disp('Plot 2 should contain:'); + disp(' - Identity csys of size 3 with labels {x_G,y_G,z_G}'); + disp(' - Random csys of size 1 centered at [-2 2 1] with labels {x_R,y_R,z_R}'); + disp(' - Random csys of size [2 1 1.5] centered at [1 -1 0] with labels {x_Qs,y_Qs,z_Qs}'); + disp(' '); + + % Plot 3 + h = [h figure()]; + PlotCsys(eye(3),'G'); + hold on; + PlotCsys(RandRotmat,'rand',[0 0 0]',1,'LineWidth',2.0); + PlotCsys(RandRotmat,'D',[0 0 0],1,'LineWidth',1.5,'LineStyle','--'); + hold off; + title('PlotCsys: Plot 3'); + xlabel('x \rightarrow'); + ylabel('y \rightarrow'); + zlabel('z \rightarrow'); + grid on; + disp('Plot 3 should contain:'); + disp(' - Identity csys with labels {x_G,y_G,z_G}'); + disp(' - Random csys with linewidth 2.0 and labels {x_rand,y_rand,z_rand}'); + disp(' - Random csys with linewidth 1.5, dashed linestyle and labels {x_D,y_D,z_D}'); + disp(' '); + + % Wait for the user to appreciate the interactive testing + fprintf('Press enter to close any opened figures and continue...'); + pause; + fprintf('\n'); + for f = h + if ishandle(f) + close(f); + end + end + pause(0.3); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/TestRand.m b/tro2022/transform/RotationsLib/Test/TestRand.m new file mode 100644 index 0000000..c3b917f --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/TestRand.m @@ -0,0 +1,317 @@ +% TestRand.m - Philipp Allgeuer - 05/11/14 +% Tests: Rand*, RandAng, RandVec, RandUnitVec +% Assumes: None +% +% function [Pass] = TestRand(N, Tol, Inter) +% +% This script outputs many manual comparisons that can be further manually +% checked by the testing human if desired. In each case the expected value +% is displayed next to an actual value, and they should be 'close' to each +% other. How close the values can be expected to be is in proportion to the +% number of test cases used, and thus difficult to computationally quantify. +% +% N ==> Number of test cases to use in each test +% Tol ==> Numeric tolerance to use for testing +% Inter ==> Boolean flag whether to also run interactive tests +% Pass ==> Boolean flag whether all tests were passed + +% Main function +function [Pass] = TestRand(N, Tol, Inter) + + % Process function inputs + if nargin < 1 || ~isscalar(N) || N < 1 + N = 10000; + end + N = min(round(N),1000000); + if nargin < 2 || Tol <= 0 + Tol = 128*eps; + end + if nargin < 3 + Inter = false; + end + + % Begin test script + [P, Nnormal, ~, ~, Ntiny] = BeginTestScript('TestRand', N, Tol); + + % + % Test RandEuler + % + + % Begin test + N = BeginTest('RandEuler', Nnormal); + B = BeginBoolean(); + + % Generate a set of random rotations + D = RandEuler(N); + + % Range checking + B = TestRanges(B, D, N, 3, [-pi -pi/2 -pi], [pi pi/2 pi]); + + % End test + P = P & EndBoolean(B); + P = P & EndTest(); + + % + % Test RandFused + % + + % Begin test + N = BeginTest('RandFused', Nnormal); + B = BeginBoolean(); + + % Generate a set of random rotations + D = RandFused(N); + + % Range checking + B = TestRanges(B, D, N, 4, [-pi -pi/2 -pi/2 -1], [pi pi/2 pi/2 1]); + B = B && all((D(:,4) == 1) | (D(:,4) == -1)); + + % Test the validity criterion + crit = sin(D(:,2)).^2 + sin(D(:,3)).^2; + B = B && all(crit >= 0.0) && all(crit <= 1.0); + + % End test + P = P & EndBoolean(B); + P = P & EndTest(); + + % + % Test RandQuat + % + + % Begin test + [N, ErrA] = BeginTest('RandQuat', Nnormal); + B = BeginBoolean(); + + % Generate a set of random rotations + D = RandQuat(N); + + % Range checking + B = TestRanges(B, D, N, 4, -ones(1,4), ones(1,4)); + + % Check the quaternion norm + ErrA = sqrt(D(:,1).^2 + D(:,2).^2 + D(:,3).^2 + D(:,4).^2) - 1.0; + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Unit norm', ErrA); + + % + % Test RandRotmat + % + + % Begin test + [N, ErrA] = BeginTest('RandRotmat', Ntiny); + B = BeginBoolean(); + + % Generate a set of random rotations + R = RandRotmat(N); + D = zeros(N,9); + for k = 1:N + Tmp = R(:,:,k); + D(k,:) = Tmp(:)'; + end + + % Range checking + B = TestRanges(B, D, N, 9, -ones(1,9), ones(1,9)); + + % Check that the rotation matrices are valid by re-orthogonalising them and seeing if they stay the same + for k = 1:N + T = R(:,:,k); + U = T/sqrtm(T'*T); + ErrA(k) = norm(U - T); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Orthogonality', ErrA); + + % + % Test RandTilt + % + + % Begin test + N = BeginTest('RandTilt', Nnormal); + B = BeginBoolean(); + + % Generate a set of random rotations + D = RandTilt(N); + + % Range checking + B = TestRanges(B, D, N, 3, [-pi -pi 0], [pi pi pi]); + + % End test + P = P & EndBoolean(B); + P = P & EndTest(); + + % + % Test RandAng + % + + % Begin test + N = BeginTest('RandAng', Nnormal); + B = BeginBoolean(); + + % Generate random vectors + A = [RandAng(N) RandAng(N,pi/2) RandAng(N,[-1 0.5])]; + + % Range checking + B = TestRanges(B, A, N, 3, [-pi 0 -1], [pi pi/2 0.5]); + + % End test + P = P & EndBoolean(B); + P = P & EndTest(); + + % + % Test RandVec + % + + % Begin test + N = BeginTest('RandVec', Nnormal); + B = BeginBoolean(); + + % Generate random vectors + V = RandVec(N, 3.0); + + % Range checking + B = TestRanges(B, V', N, 3, [-3 -3 -3], [3 3 3]); + + % Check the vector 2-norm is in range + Mag = sqrt(V(1,:).^2 + V(2,:).^2 + V(3,:).^2); + B = B && all(Mag >= 0) && all(Mag <= 3); + + % Human checking + fprintf('Min mag: 0.0 => %.3g\n' , min(Mag)); + fprintf('Mean mag: 1.5 => %.3g\n' , mean(Mag)); + fprintf('Max mag: 3.0 => %.3g\n\n', max(Mag)); + + % End test + P = P & EndBoolean(B); + P = P & EndTest(); + + % + % Test RandUnitVec + % + + % Begin test + [N, MagErr] = BeginTest('RandUnitVec', Nnormal); + B = BeginBoolean(); + + % Generate random vectors + V = RandUnitVec(N, 5); + + % Range checking + B = TestRanges(B, V', N, 5, -ones(1,5), ones(1,5)); + + % Check the vector 2-norm + Mag = sqrt(sum(V.^2,1)); + MagErr = abs(Mag - 1)'; + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Magnitude error', MagErr); + + % + % Interactive testing + % + + % Run the interactive testing component + if Inter + BeginTest('Interactive'); + B = BeginBoolean(); + B = B && TestRandInter(); + P = P & EndBoolean(B); + P = P & EndTest(); + end + + % + % End of test script + % + + % End test script + EndTestScript('TestRand', P); + + % Set the output pass flag + if nargout >= 1 + Pass = P; + end + + % Clear the function variable workspace + if isOctave + clear -x Pass + else + clearvars -except Pass + end + +end + +% Interactive testing component +function [B] = TestRandInter() + + % Initialise variables + B = true; + h = []; + + % Rand fused angle plots + D = RandFused(10000); + + % Fused yaw plot + h = [h figure()]; + hist(D(:,1),30); + title('RandFused: Fused yaw'); + xlabel('Fused yaw'); + ylabel('Count'); + grid on; + + % Fused roll/pitch plot + h = [h figure()]; + plot(D(:,2),D(:,3),'b.'); + title('RandFused: Fused roll vs fused pitch'); + xlabel('Fused pitch'); + ylabel('Fused roll'); + grid on; + + % Wait for the user to appreciate the interactive testing + fprintf('Press enter to close any opened figures and continue...'); + pause; + fprintf('\n'); + for f = h + if ishandle(f) + close(f); + end + end + pause(0.3); + +end + +% Function to test ranges +function [B] = TestRanges(B, D, N, ElemSize, LBnd, UBnd) + + % Decide on an acceptable actual/expected proximity factor given the number of samples N + Fact = min(max(1500/N,0.01),1); + + % Calculate the actual and expected bounds + actmin = min(D); + expmin = LBnd; + actmax = max(D); + expmax = UBnd; + actmean = mean(D); + expmean = 0.5*(LBnd+UBnd); + expdiff = expmax - expmin; + + % Boolean conditions + B = B && all(size(D) == [N ElemSize]); + B = B && all(all(D >= repmat(expmin,N,1))) && all(all(D <= repmat(expmax,N,1))); + B = B && all(actmin <= expmin + Fact*expdiff); + B = B && all(actmax >= expmax - Fact*expdiff); + B = B && all(actmean >= expmean - 2*Fact*expdiff) && all(actmean <= expmean + 2*Fact*expdiff); + + % Human checking + fprintf('Minimums: %s\n' ,sprintf('%10.4g ',actmin)); + fprintf('Should be: %s\n\n',sprintf('%10.4g ',expmin)); + fprintf('Means: %s\n' ,sprintf('%10.4g ',actmean)); + fprintf('Should be: %s\n\n',sprintf('%10.4g ',expmean)); + fprintf('Maximums: %s\n' ,sprintf('%10.4g ',actmax)); + fprintf('Should be: %s\n\n',sprintf('%10.4g ',expmax)); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/TestRotVec.m b/tro2022/transform/RotationsLib/Test/TestRotVec.m new file mode 100644 index 0000000..c82fd29 --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/TestRotVec.m @@ -0,0 +1,156 @@ +% TestRotVec.m - Philipp Allgeuer - 05/11/14 +% Tests: *RotVec +% Assumes: Rand*, RandVec, RotmatFrom* +% +% function [Pass] = TestRotVec(N, Tol, Inter) +% +% N ==> Number of test cases to use in each test +% Tol ==> Numeric tolerance to use for testing +% Inter ==> Boolean flag whether to also run interactive tests +% Pass ==> Boolean flag whether all tests were passed + +% Main function +function [Pass] = TestRotVec(N, Tol, Inter) + + % Process function inputs + if nargin < 1 || ~isscalar(N) || N < 1 + N = 700; + end + N = min(round(N),1000000); + if nargin < 2 || Tol <= 0 + Tol = 128*eps; + end + + % Begin test script + [P, Nnormal] = BeginTestScript('TestRotVec', N, Tol); + + % + % Test EulerRotVec + % + + % Begin test + [N, ErrA] = BeginTest('EulerRotVec', Nnormal); + B = BeginBoolean(); + + % Perform the required testing + for k = 1:N + Vin = RandVec(1,3); + Er = RandEuler; + Rr = RotmatFromEuler(Er); + B = B && all(EulerRotVec(Er,[0 0 0]) == [0 0 0]); + B = B && all(EulerRotVec(Er,[0;0;0]) == [0;0;0]); + ErrA(k) = norm(EulerRotVec(Er,Vin) - Rr*Vin); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Rotated vector norm error', ErrA); + + % + % Test FusedRotVec + % + + % Begin test + [N, ErrA] = BeginTest('FusedRotVec', Nnormal); + B = BeginBoolean(); + + % Perform the required testing + for k = 1:N + Vin = RandVec(1,3); + Fr = RandFused; + Rr = RotmatFromFused(Fr); + B = B && all(FusedRotVec(Fr,[0 0 0]) == [0 0 0]); + B = B && all(FusedRotVec(Fr,[0;0;0]) == [0;0;0]); + ErrA(k) = norm(FusedRotVec(Fr,Vin) - Rr*Vin); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Rotated vector norm error', ErrA); + + % + % Test QuatRotVec + % + + % Begin test + [N, ErrA] = BeginTest('QuatRotVec', Nnormal); + B = BeginBoolean(); + + % Perform the required testing + for k = 1:N + Vin = RandVec(1,3); + Qr = RandQuat; + Rr = RotmatFromQuat(Qr); + B = B && all(QuatRotVec(Qr,[0 0 0]) == [0 0 0]); + B = B && all(QuatRotVec(Qr,[0;0;0]) == [0;0;0]); + ErrA(k) = norm(QuatRotVec(Qr,Vin) - Rr*Vin); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Rotated vector norm error', ErrA); + + % + % Test RotmatRotVec + % + + % Begin test + [N, ErrA] = BeginTest('RotmatRotVec', Nnormal); + B = BeginBoolean(); + + % Perform the required testing + for k = 1:N + Vin = RandVec(1,3); + Rr = RandRotmat; + B = B && all(RotmatRotVec(Rr,[0 0 0]) == [0 0 0]); + B = B && all(RotmatRotVec(Rr,[0;0;0]) == [0;0;0]); + ErrA(k) = norm(RotmatRotVec(Rr,Vin) - Rr*Vin); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Rotated vector norm error', ErrA); + + % + % Test TiltRotVec + % + + % Begin test + [N, ErrA] = BeginTest('TiltRotVec', Nnormal); + B = BeginBoolean(); + + % Perform the required testing + for k = 1:N + Vin = RandVec(1,3); + Tr = RandTilt; + Rr = RotmatFromTilt(Tr); + B = B && all(TiltRotVec(Tr,[0 0 0]) == [0 0 0]); + B = B && all(TiltRotVec(Tr,[0;0;0]) == [0;0;0]); + ErrA(k) = norm(TiltRotVec(Tr,Vin) - Rr*Vin); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Rotated vector norm error', ErrA); + + % + % End of test script + % + + % End test script + EndTestScript('TestRotVec', P); + + % Set the output pass flag + if nargout >= 1 + Pass = P; + end + + % Clear the function variable workspace + if isOctave + clear -x Pass + else + clearvars -except Pass + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/TestSlerp.m b/tro2022/transform/RotationsLib/Test/TestSlerp.m new file mode 100644 index 0000000..bb429c0 --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/TestSlerp.m @@ -0,0 +1,99 @@ +% TestSlerp.m - Philipp Allgeuer - 31/10/16 +% Tests: QuatSlerp +% Assumes: RandQuat, ComposeQuat, QuatInv +% +% function [Pass] = TestSlerp(N, Tol, Inter) +% +% N ==> Number of test cases to use in each test +% Tol ==> Numeric tolerance to use for testing +% Inter ==> Boolean flag whether to also run interactive tests +% Pass ==> Boolean flag whether all tests were passed + +% Main function +function [Pass] = TestSlerp(N, Tol, Inter) + + % Process function inputs + if nargin < 1 || ~isscalar(N) || N < 1 + N = 200; + end + N = min(round(N),1000000); + if nargin < 2 || Tol <= 0 + Tol = 128*eps; + end + + % Begin test script + [P, Nnormal] = BeginTestScript('TestSlerp', N, Tol); + + % + % Test QuatSlerp + % + + % Begin test + [N, ErrA, ErrB, ErrC, ErrD] = BeginTest('QuatSlerp', Nnormal, [5 2 3 1]); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(QuatSlerp([cos(0.1) sin(0.1) 0 0], [cos(0.1) -sin(0.1) 0 0], 0.5) == [1 0 0 0]); + B = B && all(QuatSlerp([cos(0.2) 0 sin(0.2) 0], [-cos(0.2) 0 sin(0.2) 0], 0.5) == [1 0 0 0]); + B = B && all(QuatSlerp([cos(0.3) 0 0 sin(0.3)], [cos(0.3) 0 0 -sin(0.3)], 0.5) == [1 0 0 0]); + + % Perform the required testing + for k = 1:N + qa = RandQuat; + qb = RandQuat; + + qout = [QuatSlerp(qa, qb, 0.0); QuatSlerp(qa, qb, 0.25); QuatSlerp(qa, qb, 0.5); QuatSlerp(qa, qb, 0.75); QuatSlerp(qa, qb, 1.0)]; + dqout = [ComposeQuat(qout(2,:), QuatInv(qout(1,:))); ComposeQuat(qout(3,:), QuatInv(qout(2,:))); ComposeQuat(qout(4,:), QuatInv(qout(3,:))); ComposeQuat(qout(5,:), QuatInv(qout(4,:)))]; + + ErrA(k,1) = norm(QuatSlerp(qa, qa, 0.0) - qa); + ErrA(k,2) = norm(QuatSlerp(qa, qa, 0.5) - qa); + ErrA(k,3) = norm(QuatSlerp(qa, qa, 1.0) - qa); + + if qout(1,1)*qa(1) >= 0 + ErrB(k,1) = norm(qout(1,:) - qa); + else + ErrB(k,1) = norm(qout(1,:) + qa); + end + if qout(5,1)*qb(1) >= 0 + ErrB(k,2) = norm(qout(5,:) - qb); + else + ErrB(k,2) = norm(qout(5,:) + qb); + end + + ErrC(k,1) = norm(dqout(1,:) - dqout(2,:)); + ErrC(k,2) = norm(dqout(2,:) - dqout(3,:)); + ErrC(k,3) = norm(dqout(3,:) - dqout(4,:)); + + total = ComposeQuat(dqout(1,:), dqout(2,:), dqout(3,:), dqout(4,:), qa); + if total(1)*qb(1) >= 0 + ErrD(k,4) = norm(total - qb); + else + ErrD(k,4) = norm(total + qb); + end + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Trivial slerp', ErrA, 'Slerp endpoints', ErrB, 'Slerp linearity', ErrC, 'Slerp total', ErrD); + + % + % End of test script + % + + % End test script + EndTestScript('TestSlerp', P); + + % Set the output pass flag + if nargout >= 1 + Pass = P; + end + + % Clear the function variable workspace + if isOctave + clear -x Pass + else + clearvars -except Pass + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/TestTemplate.m b/tro2022/transform/RotationsLib/Test/TestTemplate.m new file mode 100644 index 0000000..5f43ca6 --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/TestTemplate.m @@ -0,0 +1,65 @@ +% TESTTEMPLATE.m - Philipp Allgeuer - DATE +% Tests: FUNCTION_LIST1 +% Assumes: FUNCTION_LIST2 +% +% function [Pass] = TESTTEMPLATE(N, Tol, Inter) +% +% N ==> Number of test cases to use in each test +% Tol ==> Numeric tolerance to use for testing +% Inter ==> Boolean flag whether to also run interactive tests +% Pass ==> Boolean flag whether all tests were passed + +% Main function +function [Pass] = TESTTEMPLATE(N, Tol, Inter) + + % Process function inputs + if nargin < 1 || ~isscalar(N) || N < 1 + N = 100; % TODO: Set a meaningful default N + end + N = min(round(N),1000000); + if nargin < 2 || Tol <= 0 + Tol = 128*eps; + end + + % Begin test script + [P, Nnormal] = BeginTestScript('TESTTEMPLATE', N, Tol); + + % + % Test FUNCTION + % + + % Begin test + [N, ErrA, ErrB] = BeginTest('FUNCTION', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + B = B && BOOLEAN_TESTS; + + % Perform the required testing + TESTS_THAT_CALCULATE_ERRORS_ERRA_AND_ERRB; + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'TESTITEMNAME1', ErrA, 'TESTITEMNAME2', ErrB); + + % + % End of test script + % + + % End test script + EndTestScript('TESTTEMPLATE', P); + + % Set the output pass flag + if nargout >= 1 + Pass = P; + end + + % Clear the function variable workspace + if isOctave + clear -x Pass + else + clearvars -except Pass + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/TestZVecFrom.m b/tro2022/transform/RotationsLib/Test/TestZVecFrom.m new file mode 100644 index 0000000..279560e --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/TestZVecFrom.m @@ -0,0 +1,176 @@ +% TestZVecFrom.m - Philipp Allgeuer - 20/10/16 +% Tests: ZVecFrom* +% Assumes: Rand*, RotmatFrom* +% +% function [Pass] = TestZVecFrom(N, Tol, Inter) +% +% N ==> Number of test cases to use in each test +% Tol ==> Numeric tolerance to use for testing +% Inter ==> Boolean flag whether to also run interactive tests +% Pass ==> Boolean flag whether all tests were passed + +% Main function +function [Pass] = TestZVecFrom(N, Tol, Inter) + + % Process function inputs + if nargin < 1 || ~isscalar(N) || N < 1 + N = 3000; + end + N = min(round(N),1000000); + if nargin < 2 || Tol <= 0 + Tol = 128*eps; + end + + % Begin test script + [P, Nnormal] = BeginTestScript('TestZVecFrom', N, Tol); + + % + % Test ZVecFromEuler + % + + % Begin test + [N, ErrA] = BeginTest('ZVecFromEuler', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(ZVecFromEuler([0 0 0]) == [0 0 1]); + + % Perform the required testing + for k = 1:N + Er = RandEuler; + Zr = ZVecFromEuler(Er); + + ErrA(k) = abs(norm(Zr) - 1); + + Rr = RotmatFromEuler(Er); + B = B && all(Zr == Rr(3,:)); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Norm error', ErrA); + + % + % Test ZVecFromFused + % + + % Begin test + [N, ErrA] = BeginTest('ZVecFromFused', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(ZVecFromFused([0 0 0 1]) == [0 0 1]); + + % Perform the required testing + for k = 1:N + Fr = RandFused; + Zr = ZVecFromFused(Fr); + + ErrA(k) = abs(norm(Zr) - 1); + + Rr = RotmatFromFused(Fr); + B = B && all(Zr == Rr(3,:)); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Norm error', ErrA); + + % + % Test ZVecFromQuat + % + + % Begin test + [N, ErrA] = BeginTest('ZVecFromQuat', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(ZVecFromQuat([1 0 0 0]) == [0 0 1]); + + % Perform the required testing + for k = 1:N + Qr = RandQuat; + Zr = ZVecFromQuat(Qr); + + ErrA(k) = abs(norm(Zr) - 1); + + Rr = RotmatFromQuat(Qr); + B = B && all(Zr == Rr(3,:)); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Norm error', ErrA); + + % + % Test ZVecFromRotmat + % + + % Begin test + [N, ErrA] = BeginTest('ZVecFromRotmat', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(ZVecFromRotmat(eye(3)) == [0 0 1]); + + % Perform the required testing + for k = 1:N + Rr = RandRotmat; + Zr = ZVecFromRotmat(Rr); + + ErrA(k) = abs(norm(Zr) - 1); + + B = B && all(Zr == Rr(3,:)); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Norm error', ErrA); + + % + % Test ZVecFromTilt + % + + % Begin test + [N, ErrA] = BeginTest('ZVecFromTilt', Nnormal); + B = BeginBoolean(); + + % Boolean conditions + B = B && all(ZVecFromTilt([0 0 0]) == [0 0 1]); + + % Perform the required testing + for k = 1:N + Tr = RandTilt; + Zr = ZVecFromTilt(Tr); + + ErrA(k) = abs(norm(Zr) - 1); + + Rr = RotmatFromTilt(Tr); + B = B && all(Zr == Rr(3,:)); + end + + % End test + P = P & EndBoolean(B); + P = P & EndTest(Tol, 'Norm error', ErrA); + + % + % End of test script + % + + % End test script + EndTestScript('TestZVecFrom', P); + + % Set the output pass flag + if nargout >= 1 + Pass = P; + end + + % Clear the function variable workspace + if isOctave + clear -x Pass + else + clearvars -except Pass + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/Test/isOctave.m b/tro2022/transform/RotationsLib/Test/isOctave.m new file mode 100644 index 0000000..61f15c8 --- /dev/null +++ b/tro2022/transform/RotationsLib/Test/isOctave.m @@ -0,0 +1,23 @@ +% isOctave.m - Philipp Allgeuer - 05/11/14 +% Check whether the execution environment is Octave or Matlab. +% +% function [IsOct] = isOctave() +% +% IsOct ==> Boolean flag whether this script is running in Octave (true) or Matlab (false) + +% Main function +function [IsOct] = isOctave() + + % Declare persistent boolean flag + persistent O; + + % Define the flag if required + if isempty(O) + O = (exist('OCTAVE_VERSION', 'builtin') ~= 0); + end + + % Copy the flag to the output + IsOct = O; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/TiltEqual.m b/tro2022/transform/RotationsLib/TiltEqual.m new file mode 100644 index 0000000..ccd7dfc --- /dev/null +++ b/tro2022/transform/RotationsLib/TiltEqual.m @@ -0,0 +1,54 @@ +% TiltEqual.m - Philipp Allgeuer - 05/11/14 +% Returns whether two tilt angles rotations represent the same rotation. +% +% function [Equal, Err] = TiltEqual(T, U, Tol) +% +% Refer to EnsureTilt for more information on the unique form of a tilt +% angles rotation. +% +% T ==> First tilt angles rotation to compare +% U ==> Second tilt angles rotation to compare +% Tol ==> Allowed tolerance between T and U for which equality is still asserted +% Equal ==> Boolean flag whether T equals U to the given L_inf norm tolerance +% Err ==> The quantified error between T and U (0 if exactly equal) + +% Main function +function [Equal, Err] = TiltEqual(T, U, Tol) + + % Default tolerance + if nargin < 3 + Tol = 1e-10; + else + Tol = abs(Tol); + end + + % Get the required value of pi + PI = GetPI(T); + + % Convert both tilt angles into their unique representations + T = EnsureTilt(T, Tol, true); + U = EnsureTilt(U, Tol, true); + + % Handle angle wrapping issues + if abs(T(1)-U(1)) > PI + if T(1) > U(1) + U(1) = U(1) + 2*PI; + else + T(1) = T(1) + 2*PI; + end + end + + % Construct the vectors to compare + % The tilt axis angle has a singularity when the tilt angle is zero, so a geometrically relevant term is checked instead of the tilt axis angle directly + % The tilt angle suffers from the numerical sensitivity of acos, so the cos thereof is checked + TComp = [T(1) (sin(T(3))^2)*[cos(T(2)) sin(T(2))] cos(T(3))]; + UComp = [U(1) (sin(U(3))^2)*[cos(U(2)) sin(U(2))] cos(U(3))]; + + % Calculate the deviation between the two rotations + Err = max(abs(TComp-UComp)); + + % Check whether the two rotations are within tolerance + Equal = (Err <= Tol); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/TiltFromAbsPhase.m b/tro2022/transform/RotationsLib/TiltFromAbsPhase.m new file mode 100644 index 0000000..df960d4 --- /dev/null +++ b/tro2022/transform/RotationsLib/TiltFromAbsPhase.m @@ -0,0 +1,20 @@ +% TiltFromAbsPhase.m - Philipp Allgeuer - 16/01/18 +% Converts an absolute tilt phase rotation to the corresponding tilt angles representation. +% +% function [Tilt] = TiltFromAbsPhase(AbsPhase) +% +% AbsPhase ==> Input absolute tilt phase rotation +% Tilt ==> Equivalent tilt angles rotation + +% Main function +function [Tilt] = TiltFromAbsPhase(AbsPhase) + + % Construct the output tilt angles + psi = AbsPhase(3); + gamma = atan2(AbsPhase(2), AbsPhase(1)) - psi; + gamma = wrap(gamma); + alpha = sqrt(AbsPhase(1)*AbsPhase(1) + AbsPhase(2)*AbsPhase(2)); + Tilt = [psi gamma alpha]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/TiltFromAxis.m b/tro2022/transform/RotationsLib/TiltFromAxis.m new file mode 100644 index 0000000..372952a --- /dev/null +++ b/tro2022/transform/RotationsLib/TiltFromAxis.m @@ -0,0 +1,75 @@ +% TiltFromAxis.m - Philipp Allgeuer - 05/11/14 +% Constructs a tilt angles rotation based on an axis-angle specification. +% +% function [Tilt, Quat] = TiltFromAxis(Axis, Angle) +% +% Axis ==> The vector corresponding to the axis of rotation (magnitude is unimportant) +% either given as the vector [ux uy uz] or one of {'x', 'y', 'z'} +% Angle ==> The angle of the rotation given by the right hand rule +% Tilt ==> Equivalent tilt angles rotation +% Quat ==> Equivalent quaternion rotation + +% Main function +function [Tilt, Quat] = TiltFromAxis(Axis, Angle) + + % Handle case of missing arguments + if nargin < 2 + Tilt = [0 0 0]; + Quat = [1 0 0 0]; + return; + end + + % Get the required value of pi + PI = GetPI(Angle); + + % Wrap the rotation angle to (-pi,pi] + Angle = wrap(Angle); + + % Precompute the required sin and cos terms + hcang = cos(Angle/2); + hsang = sin(Angle/2); + + % Handle case of standard axis rotations + if strcmpi(Axis, 'x') + if Angle >= 0 + Tilt = [0 0 Angle]; + else + Tilt = [0 PI -Angle]; + end + Quat = [hcang hsang 0 0]; + return; + elseif strcmpi(Axis, 'y') + if Angle >= 0 + Tilt = [0 PI/2 Angle]; + else + Tilt = [0 -PI/2 -Angle]; + end + Quat = [hcang 0 hsang 0]; + return; + elseif strcmpi(Axis, 'z') + Tilt = [Angle 0 0]; + Quat = [hcang 0 0 hsang]; + return; + end + + % Axis error checking + if ~(isnumeric(Axis) && isvector(Axis) && length(Axis) == 3) + error('Axis must be a 3-vector, or one of {''x'',''y'',''z''}.'); + end + + % Normalise the axis vector + AxisNorm = norm(Axis); + if AxisNorm <= 0 + Tilt = [0 0 0]; % Return the identity rotation if Axis is the zero vector + Quat = [1 0 0 0]; + return; + else + Axis = Axis/AxisNorm; + end + + % Construct the required rotation + Quat = [hcang hsang*Axis(:)']; + Tilt = TiltFromQuat(Quat); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/TiltFromEuler.m b/tro2022/transform/RotationsLib/TiltFromEuler.m new file mode 100644 index 0000000..ffdca25 --- /dev/null +++ b/tro2022/transform/RotationsLib/TiltFromEuler.m @@ -0,0 +1,35 @@ +% TiltFromEuler.m - Philipp Allgeuer - 05/11/14 +% Converts a ZYX Euler angles rotation to the corresponding tilt angles representation. +% +% function [Tilt, Rotmat] = TiltFromEuler(Euler) +% +% The output ranges are: +% Fused yaw: psi is in (-pi,pi] +% Tilt axis angle: gamma is in (-pi,pi] +% Tilt angle: alpha is in [0,pi] +% +% Euler ==> Input ZYX Euler angles rotation +% Tilt ==> Equivalent tilt angles rotation +% Rotmat ==> Equivalent rotation matrix + +% Main function +function [Tilt, Rotmat] = TiltFromEuler(Euler) + + % Calculation of the fused yaw in a numerically stable manner requires the complete rotation matrix representation + Rotmat = RotmatFromEuler(Euler); + + % Calculate the fused yaw + psi = FYawOfRotmat(Rotmat); + + % Calculate the tilt axis angle + gamma = atan2(-Rotmat(3,1),Rotmat(3,2)); + + % Calculate the tilt angle + calpha = max(min(Rotmat(3,3),1.0),-1.0); % Note: If Rotmat is valid then this should only trim at most a few eps... + alpha = acos(calpha); + + % Return the tilt angles representation + Tilt = [psi gamma alpha]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/TiltFromFYawBzG.m b/tro2022/transform/RotationsLib/TiltFromFYawBzG.m new file mode 100644 index 0000000..0edc052 --- /dev/null +++ b/tro2022/transform/RotationsLib/TiltFromFYawBzG.m @@ -0,0 +1,25 @@ +% TiltFromFYawBzG - Philipp Allgeuer - 20/10/16 +% Calculates the tilt angles (TGB) with a given fused yaw and global z-axis in body-fixed coordinates (BzG). +% +% function [TGB] = TiltFromFYawBzG(FYaw, BzG) +% +% It is assumed that BzG is a unit vector! + +% Main function +function [TGB] = TiltFromFYawBzG(FYaw, BzG) + + % Wrap the fused yaw to the desired range + FYaw = wrap(FYaw); + + % Calculate the tilt axis angle + gamma = atan2(-BzG(1), BzG(2)); + + % Calculate the tilt angle + calpha = max(min(BzG(3),1.0),-1.0); % Note: If BzG is a unit vector then this should only trim at most a few eps... + alpha = acos(calpha); + + % Construct the output tilt angles + TGB = [FYaw gamma alpha]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/TiltFromFYawGzB.m b/tro2022/transform/RotationsLib/TiltFromFYawGzB.m new file mode 100644 index 0000000..461219b --- /dev/null +++ b/tro2022/transform/RotationsLib/TiltFromFYawGzB.m @@ -0,0 +1,30 @@ +% TiltFromFYawGzB - Philipp Allgeuer - 26/10/16 +% Calculates the tilt angles (TGB) with a given fused yaw and body-fixed z-axis in global coordinates (GzB). +% +% function [TGB] = TiltFromFYawGzB(FYaw, GzB) +% +% It is assumed that GzB is a unit vector! + +% Main function +function [TGB] = TiltFromFYawGzB(FYaw, GzB) + + % Wrap the fused yaw to the desired range + FYaw = wrap(FYaw); + + % Calculate the tilt axis angle + if GzB(1) == 0 && GzB(2) == 0 + gamma = 0; + else + gamma = atan2(GzB(1), -GzB(2)) - FYaw; + gamma = wrap(gamma); + end + + % Calculate the tilt angle + calpha = max(min(GzB(3),1.0),-1.0); % Note: If GzB is a unit vector then this should only trim at most a few eps... + alpha = acos(calpha); + + % Construct the tilt angles representation + TGB = [FYaw gamma alpha]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/TiltFromFused.m b/tro2022/transform/RotationsLib/TiltFromFused.m new file mode 100644 index 0000000..8d9c438 --- /dev/null +++ b/tro2022/transform/RotationsLib/TiltFromFused.m @@ -0,0 +1,41 @@ +% TiltFromFused.m - Philipp Allgeuer - 05/11/14 +% Converts a fused angles rotation to the corresponding tilt angles representation. +% +% function [Tilt] = TiltFromFused(Fused) +% +% The output ranges are: +% Fused yaw: psi is in (-pi,pi] +% Tilt axis angle: gamma is in (-pi,pi] +% Tilt angle: alpha is in [0,pi] +% +% Fused ==> Input fused angles rotation +% Tilt ==> Equivalent tilt angles rotation + +% Main function +function [Tilt] = TiltFromFused(Fused) + + % Precalculate the sin values + sth = sin(Fused(2)); + sphi = sin(Fused(3)); + + % Calculate the cos of the tilt angle alpha + crit = sth*sth + sphi*sphi; + if crit >= 1.0 + calpha = 0.0; + else + if Fused(4) >= 0 + calpha = sqrt(1-crit); + else + calpha = -sqrt(1-crit); + end + end + + % Calculate the tilt axis angle and the tilt angle + gamma = atan2(sth,sphi); + alpha = acos(calpha); + + % Return the tilt angles representation + Tilt = [Fused(1) gamma alpha]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/TiltFromPhase.m b/tro2022/transform/RotationsLib/TiltFromPhase.m new file mode 100644 index 0000000..5ca9cf4 --- /dev/null +++ b/tro2022/transform/RotationsLib/TiltFromPhase.m @@ -0,0 +1,19 @@ +% TiltFromPhase.m - Philipp Allgeuer - 16/01/18 +% Converts a tilt phase rotation to the corresponding tilt angles representation. +% +% function [Tilt] = TiltFromPhase(Phase) +% +% Phase ==> Input tilt phase rotation +% Tilt ==> Equivalent tilt angles rotation + +% Main function +function [Tilt] = TiltFromPhase(Phase) + + % Construct the output tilt angles + psi = Phase(3); + gamma = atan2(Phase(2), Phase(1)); + alpha = sqrt(Phase(1)*Phase(1) + Phase(2)*Phase(2)); + Tilt = [psi gamma alpha]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/TiltFromQuat.m b/tro2022/transform/RotationsLib/TiltFromQuat.m new file mode 100644 index 0000000..c64aafd --- /dev/null +++ b/tro2022/transform/RotationsLib/TiltFromQuat.m @@ -0,0 +1,32 @@ +% TiltFromQuat.m - Philipp Allgeuer - 05/11/14 +% Converts a quaternion rotation to the corresponding tilt angles representation. +% +% function [Tilt] = TiltFromQuat(Quat) +% +% The output ranges are: +% Fused yaw: psi is in (-pi,pi] +% Tilt axis angle: gamma is in (-pi,pi] +% Tilt angle: alpha is in [0,pi] +% +% Quat ==> Input quaternion rotation (assumed to have unit norm) +% Tilt ==> Equivalent tilt angles rotation + +% Main function +function [Tilt] = TiltFromQuat(Quat) + + % Calculate the fused yaw + psi = 2.0*atan2(Quat(4),Quat(1)); + psi = wrap(psi); + + % Calculate the tilt axis angle + gamma = atan2(Quat(1)*Quat(3)-Quat(2)*Quat(4),Quat(1)*Quat(2)+Quat(3)*Quat(4)); + + % Calculate the tilt angle + calpha = max(min(2*(Quat(1)*Quat(1)+Quat(4)*Quat(4))-1,1.0),-1.0); % Note: If Quat is valid then this should only trim at most a few eps... + alpha = acos(calpha); + + % Return the tilt angles representation + Tilt = [psi gamma alpha]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/TiltFromRotmat.m b/tro2022/transform/RotationsLib/TiltFromRotmat.m new file mode 100644 index 0000000..cc2ba7e --- /dev/null +++ b/tro2022/transform/RotationsLib/TiltFromRotmat.m @@ -0,0 +1,31 @@ +% TiltFromRotmat.m - Philipp Allgeuer - 05/11/14 +% Converts a rotation matrix to the corresponding tilt angles representation. +% +% function [Tilt] = TiltFromRotmat(Rotmat) +% +% The output ranges are: +% Fused yaw: psi is in (-pi,pi] +% Tilt axis angle: gamma is in (-pi,pi] +% Tilt angle: alpha is in [0,pi] +% +% Rotmat ==> Input rotation matrix +% Tilt ==> Equivalent tilt angles rotation + +% Main function +function [Tilt] = TiltFromRotmat(Rotmat) + + % Calculate the tilt angle alpha + calpha = max(min(Rotmat(3,3),1.0),-1.0); % Note: If Rotmat is valid then this should only trim at most a few eps... + alpha = acos(calpha); + + % Calculate the tilt axis angle + gamma = atan2(-Rotmat(3,1),Rotmat(3,2)); + + % Calculate the fused yaw + psi = FYawOfRotmat(Rotmat); + + % Return the tilt angles representation + Tilt = [psi gamma alpha]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/TiltFromZVec.m b/tro2022/transform/RotationsLib/TiltFromZVec.m new file mode 100644 index 0000000..016b7d3 --- /dev/null +++ b/tro2022/transform/RotationsLib/TiltFromZVec.m @@ -0,0 +1,25 @@ +% TiltFromZVec.m - Philipp Allgeuer - 21/10/16 +% Converts a z-vector to the corresponding zero fused yaw tilt angles representation. +% +% function [Tilt] = TiltFromZVec(ZVec) +% +% It is assumed that ZVec is a unit vector! +% +% ZVec ==> Input z-vector +% Tilt ==> Corresponding zero fused yaw tilt angles rotation + +% Main function +function [Tilt] = TiltFromZVec(ZVec) + + % Calculate the tilt axis angle + gamma = atan2(-ZVec(1),ZVec(2)); + + % Calculate the tilt angle alpha + calpha = max(min(ZVec(3),1.0),-1.0); % Note: If ZVec is a unit vector then this should only trim at most a few eps... + alpha = acos(calpha); + + % Return the tilt angles representation + Tilt = [0 gamma alpha]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/TiltIdentity.m b/tro2022/transform/RotationsLib/TiltIdentity.m new file mode 100644 index 0000000..514a32b --- /dev/null +++ b/tro2022/transform/RotationsLib/TiltIdentity.m @@ -0,0 +1,15 @@ +% TiltIdentity.m - Philipp Allgeuer - 05/11/14 +% Returns the identity tilt angles rotation. +% +% function [TI] = TiltIdentity() +% +% TI ==> Identity rotation in the tilt angles representation + +% Main function +function [TI] = TiltIdentity() + + % Return the required identity rotation + TI = [0 0 0]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/TiltInv.m b/tro2022/transform/RotationsLib/TiltInv.m new file mode 100644 index 0000000..41ce8f4 --- /dev/null +++ b/tro2022/transform/RotationsLib/TiltInv.m @@ -0,0 +1,23 @@ +% TiltInv.m - Philipp Allgeuer - 05/11/14 +% Calculates the inverse of a tilt angles rotation. +% +% function [Tinv] = TiltInv(T) +% +% T ==> Input tilt angles rotation +% Tinv ==> Inverse tilt angles rotation + +% Main function +function [Tinv] = TiltInv(T) + + % Get the required value of pi + PI = GetPI(T); + + % Calculate the inverse tilt axis angle + gammainv = T(1) + T(2) - PI; + gammainv = wrap(gammainv); + + % Construct the inverse rotation + Tinv = [-T(1) gammainv T(3)]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/TiltNoEYaw.m b/tro2022/transform/RotationsLib/TiltNoEYaw.m new file mode 100644 index 0000000..59c254b --- /dev/null +++ b/tro2022/transform/RotationsLib/TiltNoEYaw.m @@ -0,0 +1,25 @@ +% TiltNoEYaw.m - Philipp Allgeuer - 05/11/14 +% Removes the ZYX yaw component of a tilt angles rotation. +% +% function [Tout, EYaw, TEYaw] = TiltNoEYaw(Tin) +% +% Tin ==> Input tilt angles rotation +% Tout ==> Output tilt angles rotation with no ZYX yaw +% EYaw ==> ZYX yaw of the input rotation +% TEYaw ==> Tilt angles representation of the ZYX yaw component +% of the input rotation + +% Main function +function [Tout, EYaw, TEYaw] = TiltNoEYaw(Tin) + + % Calculate the ZYX yaw of the input + EYaw = EYawOfTilt(Tin); + + % Construct the ZYX yaw component of the rotation + TEYaw = [EYaw 0 0]; + + % Remove the ZYX yaw component of the rotation + Tout = [Tin(1)-EYaw Tin(2:3)]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/TiltNoFYaw.m b/tro2022/transform/RotationsLib/TiltNoFYaw.m new file mode 100644 index 0000000..a44775f --- /dev/null +++ b/tro2022/transform/RotationsLib/TiltNoFYaw.m @@ -0,0 +1,25 @@ +% TiltNoFYaw.m - Philipp Allgeuer - 05/11/14 +% Removes the fused yaw component of a tilt angles rotation. +% +% function [Tout, FYaw, TFYaw] = TiltNoFYaw(Tin) +% +% Tin ==> Input tilt angles rotation +% Tout ==> Output tilt angles rotation with zero fused yaw +% FYaw ==> Fused yaw of the input rotation +% TFYaw ==> Tilt angles representation of the fused yaw component +% of the input rotation + +% Main function +function [Tout, FYaw, TFYaw] = TiltNoFYaw(Tin) + + % Calculate the fused yaw of the input + FYaw = Tin(1); + + % Construct the fused yaw component of the rotation + TFYaw = [FYaw 0 0]; + + % Remove the fused yaw component of the rotation + Tout = [0 Tin(2:3)]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/TiltRotVec.m b/tro2022/transform/RotationsLib/TiltRotVec.m new file mode 100644 index 0000000..03e1d4f --- /dev/null +++ b/tro2022/transform/RotationsLib/TiltRotVec.m @@ -0,0 +1,18 @@ +% TiltRotVec.m - Philipp Allgeuer - 05/11/14 +% Rotate a vector by a given tilt angles rotation. +% +% function [Vout] = TiltRotVec(Tilt, Vin) +% +% Tilt ==> Tilt angles rotation to apply to Vin +% Vin ==> Input vector to rotate +% Vout ==> Rotated output vector + +% Main function +function [Vout] = TiltRotVec(Tilt, Vin) + + % Rotate the vector as required + Vout = zeros(size(Vin)); + Vout(:) = RotmatFromTilt(Tilt) * Vin(:); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/TiltVectorSum.m b/tro2022/transform/RotationsLib/TiltVectorSum.m new file mode 100644 index 0000000..88f7039 --- /dev/null +++ b/tro2022/transform/RotationsLib/TiltVectorSum.m @@ -0,0 +1,37 @@ +% TiltVectorSum.m - Philipp Allgeuer - 20/02/17 +% Adds multiple tilt rotations together using a vectorial method. +% +% function [tilt] = TiltVectorSum(varargin) +% +% varargin ==> Input tilt rotations in the format [gamma alpha] +% tilt ==> Output tilt rotation in the format [gamma alpha] +% +% The order of inputs is unimportant as vectorial addition is commutative and associative. +% Do not confuse 'tilt rotations' with 'tilt angles rotations'. To join the latter into a +% single rotation, refer to the ComposeTilt() function. +% +function [tilt] = TiltVectorSum(varargin) + + % Sum up the input tilt rotations using a vectorial method + Num = length(varargin); + if Num <= 0 + tilt = [0 0]; + elseif Num == 1 + tilt = varargin{1}; + if tilt(2) < 0 + tilt(1) = tilt(1); + tilt(2) = -tilt(2); + end + tilt(1) = wrap(tilt(1)); + else + polar = varargin{1}; + sum = polar(2)*[cos(polar(1)) sin(polar(1))]; + for k = 2:Num + polar = varargin{k}; + sum = sum + polar(2)*[cos(polar(1)) sin(polar(1))]; + end + tilt = [atan2(sum(2),sum(1)) sqrt(sum(1)*sum(1) + sum(2)*sum(2))]; + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/VecSlerp.m b/tro2022/transform/RotationsLib/VecSlerp.m new file mode 100644 index 0000000..ca5ebc1 --- /dev/null +++ b/tro2022/transform/RotationsLib/VecSlerp.m @@ -0,0 +1,54 @@ +% VecSlerp.m - Philipp Allgeuer - 17/02/17 +% Calculates spherical linear interpolation (slerp) for arbitrary vectors. +% +% function [Vu] = VecSlerp(V1, V2, u) +% +% V1 ==> Vector to interpolate from (u = 0) +% V2 ==> Vector to interpolate to (u = 1) +% u ==> Dimensionless parameter at which to evaluate the interpolation +% Vu ==> Interpolated output vector +% +% The input vectors do not have to be unit vectors, as long as they are not zero, but the +% output vector is a unit vector. + +% Main function +function [Vu] = VecSlerp(V1, V2, u) + + % See whether the output will have to be flipped at the end + flip = (size(V1,1) > size(V1,2)); + + % Normalise the input vectors + normV1 = norm(V1); + normV2 = norm(V2); + if normV1 <= 0 || normV2 <= 0 + warning('The vectors to interpolate should not be zero!'); + Vu = zeros(size(V1)); + return + end + V1 = V1(:)' / normV1; + V2 = V2(:)' / normV2; + + % Ensure u is the right shape for vectorisation + u = u(:); + + % Calculate the dot product of the two vectors + dprod = dot(V1, V2); + + % If V1 and V2 are close enough together then just use linear interpolation, otherwise perform spherical linear interpolation + if dprod >= 1 - 5e-9 % A dot product within this tolerance of unity produces a negligible amount of error if using linear interpolation instead + Vu = (1-u)*V1 + u*V2; + else + htheta = acos(dprod); + Vu = sin((1-u)*htheta)*V1 + sin(u*htheta)*V2; + end + + % Normalise the output vectors + Vu = Vu./sqrt(sum(Vu.^2,2)); + + % Flip the output if appropriate + if flip + Vu = Vu'; + end + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/ZVecFromEuler.m b/tro2022/transform/RotationsLib/ZVecFromEuler.m new file mode 100644 index 0000000..34e3708 --- /dev/null +++ b/tro2022/transform/RotationsLib/ZVecFromEuler.m @@ -0,0 +1,22 @@ +% ZVecFromEuler.m - Philipp Allgeuer - 20/10/16 +% Converts a ZYX Euler angles rotation to the corresponding z-vector. +% +% function [ZVec] = ZVecFromEuler(Euler) +% +% Euler ==> Input ZYX Euler angles rotation +% ZVec ==> Corresponding z-vector + +% Main function +function [ZVec] = ZVecFromEuler(Euler) + + % Precalculate the sin and cos values + cth = cos(Euler(2)); + sth = sin(Euler(2)); + cphi = cos(Euler(3)); + sphi = sin(Euler(3)); + + % Calculate the required z-vector + ZVec = [-sth cth*sphi cth*cphi]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/ZVecFromFused.m b/tro2022/transform/RotationsLib/ZVecFromFused.m new file mode 100644 index 0000000..1c6156d --- /dev/null +++ b/tro2022/transform/RotationsLib/ZVecFromFused.m @@ -0,0 +1,32 @@ +% ZVecFromFused.m - Philipp Allgeuer - 20/10/16 +% Converts a fused angles rotation to the corresponding z-vector. +% +% function [ZVec] = ZVecFromFused(Fused) +% +% Fused ==> Input fused angles rotation +% ZVec ==> Corresponding z-vector + +% Main function +function [ZVec] = ZVecFromFused(Fused) + + % Precalculate the sin values + sth = sin(Fused(2)); + sphi = sin(Fused(3)); + + % Calculate the cos of the tilt angle + crit = sth*sth + sphi*sphi; + if crit >= 1.0 + calpha = 0.0; + else + if Fused(4) >= 0 + calpha = sqrt(1-crit); + else + calpha = -sqrt(1-crit); + end + end + + % Calculate the required z-vector + ZVec = [-sth sphi calpha]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/ZVecFromQuat.m b/tro2022/transform/RotationsLib/ZVecFromQuat.m new file mode 100644 index 0000000..1db7245 --- /dev/null +++ b/tro2022/transform/RotationsLib/ZVecFromQuat.m @@ -0,0 +1,26 @@ +% ZVecFromQuat.m - Philipp Allgeuer - 20/10/16 +% Converts a quaternion rotation to the corresponding z-vector. +% +% function [ZVec] = ZVecFromQuat(Quat) +% +% Note that the output of this function to machine precision is orthogonal, as long as +% the input quaternion is. It is possible however that one of the values comes out as +% +-(1+k*eps), so be careful with inverse trigonometric functions. +% +% Quat ==> Input quaternion rotation (assumed to have unit norm) +% ZVec ==> Corresponding z-vector + +% Main function +function [ZVec] = ZVecFromQuat(Quat) + + % Data aliases + w = Quat(1); + x = Quat(2); + y = Quat(3); + z = Quat(4); + + % Construct the required z-vector (assuming the quaternion is normalised) + ZVec = [2*(x*z-y*w) 2*(y*z+x*w) 1-2*(x*x+y*y)]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/ZVecFromRotmat.m b/tro2022/transform/RotationsLib/ZVecFromRotmat.m new file mode 100644 index 0000000..7a33dc4 --- /dev/null +++ b/tro2022/transform/RotationsLib/ZVecFromRotmat.m @@ -0,0 +1,16 @@ +% ZVecFromRotmat.m - Philipp Allgeuer - 20/10/16 +% Converts a rotation matrix to the corresponding z-vector. +% +% function [ZVec] = ZVecFromRotmat(Rotmat) +% +% Rotmat ==> Input rotation matrix +% ZVec ==> Corresponding z-vector + +% Main function +function [ZVec] = ZVecFromRotmat(Rotmat) + + % Retrieve the required z-vector + ZVec = Rotmat(3,:); + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/ZVecFromTilt.m b/tro2022/transform/RotationsLib/ZVecFromTilt.m new file mode 100644 index 0000000..be2224f --- /dev/null +++ b/tro2022/transform/RotationsLib/ZVecFromTilt.m @@ -0,0 +1,22 @@ +% ZVecFromTilt.m - Philipp Allgeuer - 20/10/16 +% Converts a tilt angles rotation to the corresponding z-vector. +% +% function [ZVec] = ZVecFromTilt(Tilt) +% +% Tilt ==> Input tilt angles rotation +% ZVec ==> Corresponding z-vector + +% Main function +function [ZVec] = ZVecFromTilt(Tilt) + + % Precalculate the required trigonometric terms + cgam = cos(Tilt(2)); + sgam = sin(Tilt(2)); + calpha = cos(Tilt(3)); + salpha = sin(Tilt(3)); + + % Calculate the required z-vector + ZVec = [-salpha*sgam salpha*cgam calpha]; + +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/RotationsLib/wrap.m b/tro2022/transform/RotationsLib/wrap.m new file mode 100644 index 0000000..72fa925 --- /dev/null +++ b/tro2022/transform/RotationsLib/wrap.m @@ -0,0 +1,6 @@ +% Wrap angle in radians to (-pi,pi] +function [out] = wrap(in) + PI = GetPI(in); + out = in + 2*PI*floor((PI - in)/(2*PI)); +end +% EOF \ No newline at end of file diff --git a/tro2022/transform/global2local.m b/tro2022/transform/global2local.m new file mode 100644 index 0000000..81eca0a --- /dev/null +++ b/tro2022/transform/global2local.m @@ -0,0 +1,8 @@ +function [scan_local] = global2local_fast(scan, lidar_to_base_init_se3, scan_pose_se3) + +scan_global_hmg = [scan, ones(size(scan, 1), 1)]'; +scan_hmg = inv(lidar_to_base_init_se3) * inv(scan_pose_se3) * scan_global_hmg; +scan_local = scan_hmg(1:3, :)'; + +end + diff --git a/tro2022/transform/local2global.m b/tro2022/transform/local2global.m new file mode 100644 index 0000000..16c61b5 --- /dev/null +++ b/tro2022/transform/local2global.m @@ -0,0 +1,8 @@ +function [scan_global] = local2global_fast(scan, lidar_to_base_init_se3, scan_pose_se3) + +scan_hmg = [scan, ones(length(scan), 1)]'; +scan_global_hmg = scan_pose_se3 * lidar_to_base_init_se3 * scan_hmg; % remember this order (left multiplication!) +scan_global = scan_global_hmg(1:3, :)'; + +end + diff --git a/tro2022/transform/makeLiDARGTposes.m b/tro2022/transform/makeLiDARGTposes.m new file mode 100644 index 0000000..e412c6b --- /dev/null +++ b/tro2022/transform/makeLiDARGTposes.m @@ -0,0 +1,62 @@ + +poses_cam = readmatrix("08_gt_cam.txt"); + +cam2lidar_se3 = [ + -1.857739385241e-03 -9.999659513510e-01 -8.039975204516e-03 -4.784029760483e-03 + -6.481465826011e-03 8.051860151134e-03 -9.999466081774e-01 -7.337429464231e-02 + 9.999773098287e-01 -1.805528627661e-03 -6.496203536139e-03 -3.339968064433e-01 + 0 0 0 1; +]; + +lidar2cam_se3 = inv(cam2lidar_se3); + +%% +lidar_init_pose = eye(4); + +poses_lidar = reshape(lidar_init_pose, 1, 16); + +lidar_prev = lidar_init_pose; + +for ii = 2:length(poses_cam) + + cam_curr = [reshape(poses_cam(ii, :), 4, 3)'; 0,0,0,1]; + cam_prev = [reshape(poses_cam(ii-1, :), 4, 3)'; 0,0,0,1]; + + rel_cam2cam = inv(cam_prev) * cam_curr; + + rel_lidar2lidar = lidar2cam_se3 * rel_cam2cam * cam2lidar_se3; + lidar_curr = lidar_prev * rel_lidar2lidar; + + poses_lidar = [poses_lidar; reshape(lidar_curr', 1, 16)]; + + lidar_prev = lidar_curr; +end + +poses_lidar = poses_lidar(:, 1:12); + +%% + +times = readmatrix("times.txt"); +times = times*10e8; +times = uint64(times); + +fid = fopen( 'poses.csv', 'w' ); + +for jj = 1 : length( times ) + fprintf( fid, '%d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d \n', ... + times(jj), ... + poses_lidar(jj, 1), poses_lidar(jj, 2), poses_lidar(jj, 3), poses_lidar(jj, 4), ... + poses_lidar(jj, 5), poses_lidar(jj, 6), poses_lidar(jj, 7), poses_lidar(jj, 8), ... + poses_lidar(jj, 9), poses_lidar(jj, 10), poses_lidar(jj, 11), poses_lidar(jj, 12) ... + ); +end +fclose( fid ); + + + +%% +poses_lidar_xyz = poses_lidar(:, [4, 8, 12]); + +figure(1); clf; +pcshow(poses_lidar_xyz); +