coordinate_system.py

#  ===============================================================================================================
#  Copyright (c) 2019, Cornell University. All rights reserved.
#
#  Redistribution and use in source and binary forms, with or without modification, are permitted provided that
#  the following conditions are met:
#
#      * Redistributions of source code must retain the above copyright otice, this list of conditions and
#        the following disclaimer.
#
#      * 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.
#
#      * Neither the name of Cornell University nor the names of its contributors may be used to endorse or
#        promote products derived from this software without specific prior written permission.
#
#  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT HOLDERS OR CONTRIBUTORS 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.
#
#  Author: Kai Zhang (kz298@cornell.edu)
#
#  The research is based upon work supported by the Office of the Director of National Intelligence (ODNI),
#  Intelligence Advanced Research Projects Activity (IARPA), via DOI/IBC Contract Number D17PC00287.
#  The U.S. Government is authorized to reproduce and distribute copies of this work for Governmental purposes.
#  ===============================================================================================================


import os
import json
from lib.latlonalt_enu_converter import latlonalt_to_enu, enu_to_latlonalt


# global: (xx, yy, zz) = (lat, lon, alt)
# local: enu

def local_to_global(work_dir, xx, yy, zz):
    with open(os.path.join(work_dir, 'aoi.json')) as fp:
        bbx = json.load(fp)
    
    lat0 = (bbx['lat_min'] + bbx['lat_max']) / 2.0
    lon0 = (bbx['lon_min'] + bbx['lon_max']) / 2.0
    alt0 = bbx['alt_min']

    xx, yy, zz = enu_to_latlonalt(xx, yy, zz, lat0, lon0, alt0)

    return xx, yy, zz


def global_to_local(work_dir, xx, yy, zz):
    with open(os.path.join(work_dir, 'aoi.json')) as fp:
        bbx = json.load(fp)

    lat0 = (bbx['lat_min'] + bbx['lat_max']) / 2.0
    lon0 = (bbx['lon_min'] + bbx['lon_max']) / 2.0
    alt0 = bbx['alt_min']

    xx, yy, zz = latlonalt_to_enu(xx, yy, zz, lat0, lon0, alt0)

    return xx, yy, zz