CalcFocalPlaneAngles.hpp

/******************************************************************************
* Copyright (c) ATTX LLC 2024. All Rights Reserved.
*
* This software and associated documentation (the "Software") are the 
* proprietary and confidential information of ATTX, LLC. The Software is 
* furnished under a license agreement between ATTX and the user organization 
* and may be used or copied only in accordance with the terms of the agreement.
* Refer to 'license/attx_license.adoc' for standard license terms.
*
* EXPORT CONTROL NOTICE: THIS SOFTWARE MAY INCLUDE CONTENT CONTROLLED UNDER THE
* INTERNATIONAL TRAFFIC IN ARMS REGULATIONS (ITAR) OR THE EXPORT ADMINISTRATION 
* REGULATIONS (EAR99). No part of the Software may be used, reproduced, or 
* transmitted in any form or by any means, for any purpose, without the express 
* written permission of ATTX, LLC.
******************************************************************************/
#ifndef GNC_NAVIGATION_MEASUREMENTS_CALC_FOCAL_PLANE_ANGLES_HPP
#define GNC_NAVIGATION_MEASUREMENTS_CALC_FOCAL_PLANE_ANGLES_HPP
 
#include <cmath>
 
#include "utils/Measurements.hpp"
#include "core/safemath.hpp"
 
#define FOCAL_PLANE_MEAS_TARGET_STATES 6
#define FOCAL_PLANE_MEAS_OBSERVER_STATES 3
#define FOCAL_PLANE_MEASUREMENTS 2
 
namespace clockwerk {
 
    /**
     * @brief Measurement class to calculate focal plane angles from tgt and observer state
     * 
     * This function calculates focal plane angles as (alpha, beta) for a given target in
     * an image frame using the known position of the imager and target. The focal plane
     * angles are defined as follows:
     * alpha = atan([xtgt - xobs]/[ztgt - zobs])
     * beta = atan([ytgt - yobs]/[ztgt - zobs])
     * 
     * Where target and observer states are differenced to generate the target state in the
     * focal frame. The focal frame is defined as:
     * Centered in the center of the reference image
     * X is "up" in the image
     * Y is "right" in the image
     * Z is "into" the image completing the right hand frame
     * 
     * |----------------------|
     * |         x^           |
     * |          |-->y       |
     * |                      |
     * |----------------------|
     */
    template <typename T>
    class CalcFocalPlaneAngles : public clockwerk::Measurements<T, FOCAL_PLANE_MEAS_TARGET_STATES, FOCAL_PLANE_MEASUREMENTS, FOCAL_PLANE_MEAS_OBSERVER_STATES> {
        public:
            /// @brief   Function to focal plane angles
            /// @param   time    The reference time (unused)
            /// @param   tar_state   The target reference state [x, y, z, vx, vy, vz]
            /// @param   obs_state   The observer reference state [x, y, z] representing camera position
            /// @param   out_measurements   Implicit return of [alpha, beta]
            int calculateMeasurements(T time, const std::array<T, FOCAL_PLANE_MEAS_TARGET_STATES> &tar_state, const std::array<T, FOCAL_PLANE_MEAS_OBSERVER_STATES> &obs_state,
                            std::array<T, FOCAL_PLANE_MEASUREMENTS> &out_measurements);
        private:
 
    };
 
    template <typename T>
    int CalcFocalPlaneAngles<T>::calculateMeasurements(T time, const std::array<T, FOCAL_PLANE_MEAS_TARGET_STATES> &tar_state, const std::array<T, FOCAL_PLANE_MEAS_OBSERVER_STATES> &obs_state,
                            std::array<T, FOCAL_PLANE_MEASUREMENTS> &out_measurements) {
        // First, calculate our Z value and confirm is is not zero
        T z = tar_state[2] - obs_state[2];
        if(std::abs(z) < SAFE_MATH_ZERO_TOLERANCE) {
            return ERROR_DIVIDE_BY_ZERO;
        }
 
        // Calculate alpha and beta as atan2              
        out_measurements[0] = atan2(tar_state[0] - obs_state[0], z);
        out_measurements[1] = atan2(tar_state[1] - obs_state[1], z);
 
        return NO_ERROR;
    };
 
}
 
#endif