├── ros_wrapper
    ├── pictures
    │   └── rviz_screenshot_2020_08_29-19_44_14.png
    ├── src
    │   ├── eskf_node.cpp
    │   └── eskf_localization_wrapper.cpp
    ├── launch
    │   └── eskf_localization.launch
    ├── include
    │   └── eskf_localization_wrapper.h
    └── rviz
    │   └── rviz_config.rviz
├── eskf_localizer
    ├── include
    │   └── eskf_localizer
    │   │   ├── mag_processor.h
    │   │   ├── eskf.h
    │   │   ├── gps_processor.h
    │   │   ├── imu_processor.h
    │   │   ├── eskf_localizer.h
    │   │   ├── initializer.h
    │   │   ├── base_type.h
    │   │   └── utils.h
    ├── third_party
    │   └── GeographicLib
    │   │   ├── include
    │   │       ├── Config.h
    │   │       ├── LocalCartesian.hpp
    │   │       ├── Geocentric.hpp
    │   │       ├── Constants.hpp
    │   │       └── Math.hpp
    │   │   ├── CMakeLists.txt
    │   │   └── src
    │   │       ├── LocalCartesian.cpp
    │   │       ├── Math.cpp
    │   │       └── Geocentric.cpp
    ├── CMakeLists.txt
    └── src
    │   ├── gps_processor.cpp
    │   ├── mag_processor.cpp
    │   ├── imu_processor.cpp
    │   ├── eskf.cpp
    │   ├── eskf_localizer.cpp
    │   └── initializer.cpp
├── CMakeLists.txt
├── README.md
└── package.xml


/ros_wrapper/pictures/rviz_screenshot_2020_08_29-19_44_14.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/infinity1096/eskf_localization/HEAD/ros_wrapper/pictures/rviz_screenshot_2020_08_29-19_44_14.png


--------------------------------------------------------------------------------
/eskf_localizer/include/eskf_localizer/mag_processor.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include "eskf_localizer/base_type.h"
 4 | #include <Eigen/Core>
 5 | 
 6 | namespace ESKF_Localization{
 7 | 
 8 | class MagProcessor{
 9 | 
10 | public:
11 | 	MagProcessor(Eigen::Matrix3d V);
12 | 	void Mag_correct(const MagDataPtr MagData, State* state);
13 | private:
14 | 	Eigen::Matrix3d V_;				//covariance
15 | };
16 | 
17 | }
18 | 


--------------------------------------------------------------------------------
/ros_wrapper/src/eskf_node.cpp:
--------------------------------------------------------------------------------
 1 | #include <memory>
 2 | 
 3 | #include <ros/ros.h>
 4 | 
 5 | #include "eskf_localization_wrapper.h"
 6 | 
 7 | int main (int argc, char** argv) {
 8 | 
 9 |     // Initialize ros.
10 |     ros::init(argc, argv, "eskf_localization");
11 |     ros::NodeHandle nh;
12 | 
13 |     // Initialize localizer.
14 |     ESKFLocalizationWrapper localizer(nh);
15 | 
16 |     ros::spin();
17 |     return 1;
18 | }
19 | 


--------------------------------------------------------------------------------
/eskf_localizer/include/eskf_localizer/eskf.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include "eskf_localizer/base_type.h"
 4 | 
 5 | namespace ESKF_Localization{
 6 | 
 7 | void ESKF_update_nominal(ImuDataPtr imu_data,double dt, State* state, Eigen::Vector3d g);
 8 | void ESKF_predict(Eigen::MatrixXd Fx, Eigen::MatrixXd Q, State* state);
 9 | void ESKF_correct(Eigen::VectorXd z,Eigen::VectorXd h_x, Eigen::MatrixXd H, Eigen::MatrixXd V, State* state);
10 | 
11 | }
12 | 


--------------------------------------------------------------------------------
/eskf_localizer/include/eskf_localizer/gps_processor.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include "eskf_localizer/base_type.h"
 4 | #include <Eigen/Core>
 5 | 
 6 | namespace ESKF_Localization{
 7 | 
 8 | class GpsProcessor{
 9 | 
10 | public:
11 | 	GpsProcessor(Eigen::Vector3d I_p_Gps);
12 | 	void Gps_correct(const GpsPositionDataPtr GpsData, State* state);
13 | private:
14 | 	Eigen::Vector3d I_p_Gps_;		//position of GPS in IMU frame
15 | };
16 | 
17 | }
18 | 


--------------------------------------------------------------------------------
/eskf_localizer/third_party/GeographicLib/include/Config.h:
--------------------------------------------------------------------------------
 1 | // This will be overwritten by ./configure
 2 | 
 3 | #define GEOGRAPHICLIB_VERSION_STRING "1.49"
 4 | #define GEOGRAPHICLIB_VERSION_MAJOR 1
 5 | #define GEOGRAPHICLIB_VERSION_MINOR 49
 6 | #define GEOGRAPHICLIB_VERSION_PATCH 0
 7 | 
 8 | // Undefine HAVE_LONG_DOUBLE if this type is unknown to the compiler
 9 | #define GEOGRAPHICLIB_HAVE_LONG_DOUBLE 1
10 | 
11 | // Define WORDS_BIGENDIAN to be 1 if your machine is big endian
12 | /* #undef WORDS_BIGENDIAN */
13 | 


--------------------------------------------------------------------------------
/eskf_localizer/include/eskf_localizer/imu_processor.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include "eskf_localizer/base_type.h"
 4 | 
 5 | namespace ESKF_Localization{
 6 | 
 7 | class ImuProcessor{
 8 | 
 9 | public:
10 | 	ImuProcessor(const double am_noise, const double wm_noise,
11 | 				const double ab_noise, const double wb_noise, const Eigen::Vector3d gravity);
12 | 
13 | 	void Imu_predict(const ImuDataPtr imu_data,double dt, State* state);
14 | 
15 | private:
16 | 	double am_noise_;	//accelerometer measurement noise
17 | 	double wm_noise_;	//gyroscope measurement noise
18 | 	double ab_noise_;	//accelerometer bias noise
19 | 	double wb_noise_;	//gyroscope bias noise
20 | 	Eigen::Vector3d gravity_;
21 | };
22 | 
23 | }
24 | 


--------------------------------------------------------------------------------
/eskf_localizer/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(eskf_localizer)
 3 | 
 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
 5 | 
 6 | find_package(Eigen3  REQUIRED)
 7 | 
 8 | # Add GeographicLib.
 9 | add_subdirectory(third_party/GeographicLib)
10 | 
11 | include_directories(
12 |   include
13 |   ${EIGEN3_INCLUDE_DIR}
14 |   third_party/GeographicLib/include
15 |   ##only for testing
16 |   ${catkin_INCLUDE_DIRS}
17 | )
18 | 
19 | add_library(eskf_localizer_lib
20 |   src/eskf_localizer.cpp
21 |   src/eskf.cpp
22 |   src/imu_processor.cpp
23 |   src/gps_processor.cpp
24 |   src/mag_processor.cpp
25 |   src/initializer.cpp
26 | )
27 | 
28 | target_link_libraries(
29 | 	eskf_localizer_lib
30 | 	${catkin_LIBRARIES}
31 | )
32 | 
33 | 
34 | target_link_libraries(eskf_localizer_lib
35 | 	${EIGEN3_LIBS}
36 | 	libGeographiccc
37 | )
38 | 
39 | 


--------------------------------------------------------------------------------
/ros_wrapper/launch/eskf_localization.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |     <param name="am_noise"      type="double" value="1e-2" />
 3 |     <param name="wm_noise"      type="double" value="1e-4" />
 4 |     <param name="ab_noise"  	type="double" value="1e-6" />
 5 |     <param name="wb_noise" 		type="double" value="1e-6" />
 6 | 
 7 |     <param name="I_p_Gps_x"       type="double" value="0.0" />
 8 |     <param name="I_p_Gps_y"       type="double" value="0.0" />
 9 |     <param name="I_p_Gps_z"       type="double" value="0.0" />
10 | 
11 |     <node name="nmea_topic_driver" pkg="nmea_navsat_driver" type="nmea_topic_driver" output="screen" />
12 |     <node name="eskf_localization_node" pkg="eskf_localization" type="eskf_localization_node" output="screen" />
13 | 
14 |     <node pkg="rviz" type="rviz" name="rviz" args="-d $(find eskf_localization)/ros_wrapper/rviz/rviz_config.rviz" output="screen"/>
15 | 
16 | </launch>


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(eskf_localization)
 3 | 
 4 | add_compile_options(-std=c++14)
 5 | 
 6 | ## Find catkin macros and libraries
 7 | find_package(catkin REQUIRED COMPONENTS
 8 |   roscpp
 9 |   std_msgs
10 |   geometry_msgs
11 |   nav_msgs
12 |   tf
13 | )
14 | 
15 | catkin_package()
16 | 
17 | add_subdirectory(eskf_localizer)
18 | 
19 | include_directories(
20 |   ${catkin_INCLUDE_DIRS}
21 |   ${EIGEN3_INCLUDE_DIR}
22 |   eskf_localizer/include
23 |   ros_wrapper/include
24 | )
25 | 
26 | add_library(ros_wrapper_lib
27 | 	ros_wrapper/src/eskf_localization_wrapper.cpp
28 | )
29 | 
30 | target_link_libraries(ros_wrapper_lib
31 |   ${catkin_LIBRARIES}
32 |   eskf_localizer_lib
33 | )
34 | 
35 | ## Localization node.
36 | add_executable(${PROJECT_NAME}_node ros_wrapper/src/eskf_node.cpp)
37 | target_link_libraries(${PROJECT_NAME}_node
38 |   ros_wrapper_lib
39 | )
40 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # eskf_localization
 2 | 
 3 | ![Fused Path](https://github.com/infinity1096/eskf_localization/blob/master/ros_wrapper/pictures/rviz_screenshot_2020_08_29-19_44_14.png)
 4 | 
 5 | ## Description
 6 | ROS implementation of the Error State Kalman Filter according to *Quaternion kinematics for the error-state Kalman filter* by Joan Solà. 
 7 | Full text can be obtained [here](https://arxiv.org/abs/1711.02508).
 8 | 
 9 | Node fuses information from IMU, GPS, and magnetometer, publishes fused pose.
10 | 
11 | For more details, consult: https://zhuanlan.zhihu.com/c_1266722926339645440
12 | 
13 | ## Usage
14 | ```
15 | roslaunch eskf_localization eskf_localization.launch
16 | ```
17 | On another terminal,
18 | ```
19 | rosbag play utbm_robocar_dataset_20180719_noimage.bag
20 | ```
21 | 
22 | ## Reference
23 | - https://zhuanlan.zhihu.com/p/152662055
24 | - dataset: https://epan-utbm.github.io/utbm_robocar_dataset/
25 |   - recommended: https://drive.utbm.fr/s/MR7DdRcWYysk9Do/download
26 | 


--------------------------------------------------------------------------------
/eskf_localizer/src/gps_processor.cpp:
--------------------------------------------------------------------------------
 1 | #include "eskf_localizer/gps_processor.h"
 2 | #include "eskf_localizer/base_type.h"
 3 | #include "eskf_localizer/utils.h"
 4 | #include "eskf_localizer/eskf.h"
 5 | 
 6 | #include <Eigen/Core>
 7 | 
 8 | 
 9 | namespace ESKF_Localization{
10 | 
11 | GpsProcessor::GpsProcessor(Eigen::Vector3d I_p_Gps){
12 | 	GpsProcessor::I_p_Gps_ = I_p_Gps;
13 | }
14 | 
15 | void GpsProcessor::Gps_correct(const GpsPositionDataPtr GpsData, State* state){
16 | 
17 | 	Eigen::Vector3d z;
18 | 	ConvertLLAToENU(state->lla_origin, GpsData->lla, &z);
19 | 
20 | 	Eigen::Vector3d h_x = state->G_p_I + state->G_R_I * I_p_Gps_;
21 | 
22 | 	Eigen::Matrix<double,3,15> H = Eigen::Matrix<double,3,15>::Zero();
23 | 	H.block<3,3>(0,0) = Eigen::Matrix3d::Identity();
24 | 	H.block<3,3>(0,6) = -state->G_R_I * hat(I_p_Gps_);
25 | 
26 | 	//Eigen::Matrix3d V = GpsData->cov;
27 | 	Eigen::Matrix3d V;
28 | 	V << 1.5,0,0,0,1.5,0,0,0,4.0;
29 | 
30 | 	ESKF_correct(z,h_x,H,V,state);
31 | }
32 | }
33 | 


--------------------------------------------------------------------------------
/eskf_localizer/src/mag_processor.cpp:
--------------------------------------------------------------------------------
 1 | #include "eskf_localizer/mag_processor.h"
 2 | #include "eskf_localizer/base_type.h"
 3 | #include "eskf_localizer/utils.h"
 4 | #include "eskf_localizer/eskf.h"
 5 | 
 6 | #include <Eigen/Core>
 7 | #include <Eigen/Geometry>
 8 | 
 9 | //FIXME Only for testing
10 | #include <ros/console.h>
11 | 
12 | 
13 | namespace ESKF_Localization{
14 | 
15 | MagProcessor::MagProcessor(Eigen::Matrix3d V){
16 | 	V_ = V;
17 | }
18 | 
19 | void MagProcessor::Mag_correct(const MagDataPtr MagData, State* state){
20 | 	Eigen::Vector3d z = MagData->mag;
21 | 	Eigen::Vector3d h_x = state->G_R_I.transpose() * state->m_ref;
22 | 	Eigen::Quaterniond q(state->G_R_I);
23 | 
24 | 	Eigen::Matrix<double,3,15> H = Eigen::Matrix<double,3,15>::Zero();
25 | 	Eigen::Matrix<double,4,3> temp;
26 | 	temp << Eigen::Matrix<double,1,3>::Zero(), 0.5*Eigen::Matrix3d::Identity();
27 | 	H.block<3,3>(0,6) = diff_qT_a_q_diff_q(q,state->m_ref) * quat_l(q) * temp;
28 | 
29 | 	ESKF_correct(z,h_x,H,V_,state);
30 | }
31 | }
32 | 


--------------------------------------------------------------------------------
/ros_wrapper/include/eskf_localization_wrapper.h:
--------------------------------------------------------------------------------
 1 | #include <ros/ros.h>
 2 | #include "eskf_localizer/eskf_localizer.h"
 3 | 
 4 | #include <sensor_msgs/Imu.h>
 5 | #include <sensor_msgs/NavSatFix.h>
 6 | #include <sensor_msgs/MagneticField.h>
 7 | #include <nav_msgs/Path.h>
 8 | 
 9 | #include <tf/transform_broadcaster.h>
10 | 
11 | class ESKFLocalizationWrapper{
12 | 
13 | public:
14 | 	ESKFLocalizationWrapper(ros::NodeHandle nh);
15 | 	~ESKFLocalizationWrapper();
16 | 
17 |     void ImuCallback(const sensor_msgs::ImuConstPtr& imu_msg_ptr);
18 |     void GpsPositionCallback(const sensor_msgs::NavSatFixConstPtr& gps_msg_ptr);
19 |     void MagCallback(const sensor_msgs::MagneticFieldConstPtr& mag_msg_ptr);
20 | 
21 | private:
22 | 
23 | 	ros::Subscriber imu_sub_;
24 | 	ros::Subscriber gps_sub_;
25 | 	ros::Subscriber mag_sub_;
26 | 	ros::Publisher fused_pose_pub_;
27 | 	ros::Publisher fused_path_pub_;
28 | 	ros::Publisher fused_fix_pub_;
29 | 	ros::Publisher fused_odom_pub_;
30 | 
31 |     nav_msgs::Path ros_path_;
32 | 
33 | 	tf::TransformBroadcaster br_;
34 | 
35 | 	std::unique_ptr<ESKF_Localization::ESKF_Localizer> eskf_localizer_;
36 | 
37 | 	void addStateToPath(ESKF_Localization::State* state);
38 | 	void publishState();
39 | };
40 | 


--------------------------------------------------------------------------------
/eskf_localizer/include/eskf_localizer/eskf_localizer.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include "eskf_localizer/base_type.h"
 4 | #include "eskf_localizer/initializer.h"
 5 | #include "eskf_localizer/imu_processor.h"
 6 | #include "eskf_localizer/gps_processor.h"
 7 | #include "eskf_localizer/mag_processor.h"
 8 | 
 9 | #include <Eigen/Core>
10 | 
11 | #include <geometry_msgs/Pose.h>
12 | #include <sensor_msgs/NavSatFix.h>
13 | #include <nav_msgs/Odometry.h>
14 | 
15 | namespace ESKF_Localization{
16 | 
17 | class ESKF_Localizer{
18 | 
19 | public:
20 | 	ESKF_Localizer(const double am_noise, const double wm_noise,
21 | 			const double ab_noise, const double wb_noise, Eigen::Vector3d I_p_Gps);
22 | 	void processImuData(ImuDataPtr imu_data);
23 | 	void processGpsData(GpsPositionDataPtr gps_data);
24 | 	void processMagData(MagDataPtr mag_data);
25 | 
26 | 	State* getState();
27 | 	geometry_msgs::Pose getFusedPose();
28 | 	sensor_msgs::NavSatFix getFusedFix();
29 | 	nav_msgs::Odometry getFusedOdometry();
30 | 
31 | private:
32 | 
33 | 	std::unique_ptr<Initializer> initializer_;
34 | 	std::unique_ptr<ImuProcessor> imu_processor_;
35 | 	std::unique_ptr<GpsProcessor> gps_processor_;
36 | 	std::unique_ptr<MagProcessor> mag_processor_;
37 | 
38 | 	State state_;
39 | 
40 | 	double last_t_;
41 | };
42 | }
43 | 


--------------------------------------------------------------------------------
/eskf_localizer/include/eskf_localizer/initializer.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include "eskf_localizer/base_type.h"
 4 | #include <deque>
 5 | 
 6 | namespace ESKF_Localization{
 7 | 
 8 | class Initializer{
 9 | 
10 | public:
11 | 	Initializer(int imu_initialize_goal, int gps_initialize_goal, int mag_initialize_goal, State* state);
12 | 
13 | 	void Imu_initialize(ESKF_Localization::ImuDataPtr imu_data);
14 | 	void Gps_initialize(ESKF_Localization::GpsPositionDataPtr gps_data);
15 | 	void Mag_initialize(ESKF_Localization::MagDataPtr mag_data);
16 | 
17 | 	bool is_initialized();
18 | private:
19 | 
20 | 	int imu_initialize_goal_ = 10;
21 | 	int gps_initialize_goal_ = 10;
22 | 	int mag_initialize_goal_ = 10;
23 | 
24 | 	bool got_first_gps_message_ = false;//usually GPS message comes after imu/mag messages.
25 | 							   //start collecting IMU/MAG data samples after the first message is received,
26 | 							   //which indicates consecutive GPS data begins.
27 | 
28 | 	bool imu_initialized_ = false;
29 | 	bool gps_initialized_ = false;
30 | 	bool mag_initialized_ = false;
31 | 	bool eskf_initialized_ = false;
32 | 
33 | 	std::deque<ImuDataPtr> imu_buffer_;
34 | 	std::deque<GpsPositionDataPtr> gps_buffer_;
35 | 	std::deque<MagDataPtr> mag_buffer_;
36 | 
37 | 	State* state_;
38 | };
39 | 
40 | }
41 | 


--------------------------------------------------------------------------------
/eskf_localizer/third_party/GeographicLib/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | project (GeographicLib)
 2 | 
 3 | # Version information
 4 | set (PROJECT_VERSION_MAJOR 1)
 5 | set (PROJECT_VERSION_MINOR 49)
 6 | set (PROJECT_VERSION_PATCH 0)
 7 | set (PROJECT_VERSION "${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}")
 8 | if (PROJECT_VERSION_PATCH GREATER 0)
 9 |   set (PROJECT_VERSION "${PROJECT_VERSION}.${PROJECT_VERSION_PATCH}")
10 | endif ()
11 | 
12 | # The library version tracks the numbering given by libtool in the
13 | # autoconf set up.
14 | set (LIBVERSION_API 17)
15 | set (LIBVERSION_BUILD 17.1.2)
16 | string (TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER)
17 | string (TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER)
18 | 
19 | cmake_minimum_required (VERSION 2.8.4) # This version was released 2011-02-16
20 | 
21 | # (7) Set the default "real" precision.  This should probably be left
22 | # at 2 (double).
23 | set (GEOGRAPHICLIB_PRECISION 2 CACHE STRING
24 |   "Precision: 1 = float, 2 = double, 3 = extended, 4 = quadruple, 5 = variable")
25 | set_property (CACHE GEOGRAPHICLIB_PRECISION PROPERTY STRINGS 1 2 3 4 5)
26 | 
27 | 
28 | set (LIBNAME Geographic)
29 | 
30 | include_directories(
31 |   ./include/
32 | )
33 | 
34 | add_library(libGeographiccc src/LocalCartesian.cpp
35 |                           src/Geocentric.cpp
36 |                           src/Math.cpp)


--------------------------------------------------------------------------------
/eskf_localizer/include/eskf_localizer/base_type.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <memory>
 4 | #include <Eigen/Core>
 5 | #include <Eigen/Geometry>
 6 | 
 7 | namespace ESKF_Localization{
 8 | 
 9 | struct ImuData{
10 | 	double timestamp;			//Unit: Seconds
11 | 
12 | 	Eigen::Vector3d accel;		//Unit: m/s^2
13 | 	Eigen::Vector3d gyro;		//Unit: rad/s
14 | 
15 | 	Eigen::Quaterniond quat;	//Orientation estimation from IMU. For example, DMP.
16 | 	Eigen::Quaterniond last_quat;
17 | };
18 | using ImuDataPtr = std::shared_ptr<ImuData>;
19 | 
20 | struct GpsPositionData{
21 | 	double timestamp;			//Unit: Seconds
22 | 
23 | 	Eigen::Vector3d lla;		//Unit: Latitude,Longitude(degrees); Altitude(meters)
24 | 	Eigen::Matrix3d cov;		//Unit: Position covariance(m^2)
25 | };
26 | using GpsPositionDataPtr = std::shared_ptr<GpsPositionData>;
27 | 
28 | struct MagData{
29 | 	double timestamp;			//Unit: Seconds
30 | 
31 | 	Eigen::Vector3d mag;		//Unit: uT
32 | };
33 | using MagDataPtr = std::shared_ptr<MagData>;
34 | 
35 | struct State{
36 | 	double timestamp;
37 | 
38 | 	//Nominal states
39 | 	Eigen::Vector3d G_p_I;			//Position of IMU frame(I) in global frame(G)
40 | 	Eigen::Vector3d G_v_I;			//Velocity of IMU frame(I) in global frame(G)
41 | 	Eigen::Matrix3d G_R_I;			//Rotation from IMU frame(I) to global frame(G)
42 | 	Eigen::Vector3d ab;				//Accelerometer bias
43 | 	Eigen::Vector3d wb;				//Gyroscope bias
44 | 
45 | 
46 | 	Eigen::Vector3d lla_origin;
47 | 	//Global frame is a ENU frame centered at lla_origin
48 | 	//lla = latitude, longitude, altitude
49 | 
50 | 	Eigen::Vector3d m_ref;
51 | 	//megnetic field reference
52 | 
53 | 	Eigen::Matrix<double,15,15> P;	//Covariance of the error state
54 | };
55 | 
56 | }
57 | 


--------------------------------------------------------------------------------
/eskf_localizer/src/imu_processor.cpp:
--------------------------------------------------------------------------------
 1 | #include "eskf_localizer/imu_processor.h"
 2 | #include "eskf_localizer/base_type.h"
 3 | #include "eskf_localizer/utils.h"
 4 | #include "eskf_localizer/eskf.h"
 5 | 
 6 | #include <Eigen/Core>
 7 | #include <Eigen/Geometry>
 8 | 
 9 | #include <ros/console.h>
10 | 
11 | namespace ESKF_Localization{
12 | 
13 | ImuProcessor::ImuProcessor(const double am_noise, const double wm_noise,
14 | 							const double ab_noise, const double wb_noise, const Eigen::Vector3d gravity){
15 | 	ImuProcessor::am_noise_ = am_noise;
16 | 	ImuProcessor::wm_noise_ = wm_noise;
17 | 	ImuProcessor::ab_noise_ = ab_noise;
18 | 	ImuProcessor::wb_noise_ = wb_noise;
19 | 	ImuProcessor::gravity_ = gravity;
20 | }
21 | 
22 | void ImuProcessor::Imu_predict(const ImuDataPtr imu_data, double dt, State* state){
23 | 
24 | 	ESKF_update_nominal(imu_data,dt,state,gravity_);
25 | 
26 | 	Eigen::Matrix<double,15,15> Fx = Eigen::Matrix<double,15,15>::Identity();
27 | 	Fx.block<3,3>(0,3) = Eigen::Matrix3d::Identity() * dt;
28 | 	Fx.block<3,3>(3,6) = - state->G_R_I*hat(imu_data->accel - state->ab)*dt;
29 | 	Fx.block<3,3>(3,9) = - state->G_R_I*dt;
30 | 
31 | 	Eigen::Vector3d delta_angle_axis = (imu_data->gyro - state->wb) * dt;
32 | 	if (delta_angle_axis.norm() >= 1e-12){
33 | 		Fx.block<3,3>(6,6) = Eigen::AngleAxisd(delta_angle_axis.norm(),delta_angle_axis.normalized()).toRotationMatrix().transpose();
34 | 	}else{
35 | 		Fx.block<3,3>(6,6) = Eigen::Matrix3d::Identity();
36 | 
37 | 	}
38 | 
39 | 	Fx.block<3,3>(6,12) = -Eigen::Matrix3d::Identity()*dt;
40 | 
41 | 	Eigen::Matrix<double,15,12> Fi = Eigen::Matrix<double,15,12>::Zero();
42 | 	Fi.block<12,12>(3,0) = Eigen::Matrix<double,12,12>::Identity();
43 | 
44 | 	Eigen::Matrix<double,12,12> Q = Eigen::Matrix<double,12,12>::Zero();
45 | 	Q.block<3,3>(0,0) = Eigen::Matrix3d::Identity() * am_noise_ * dt * dt;
46 | 	Q.block<3,3>(3,3) = Eigen::Matrix3d::Identity() * wm_noise_ * dt * dt;
47 | 	Q.block<3,3>(6,6) = Eigen::Matrix3d::Identity() * ab_noise_ * dt;
48 | 	Q.block<3,3>(9,9) = Eigen::Matrix3d::Identity() * wb_noise_ * dt;
49 | 
50 | 	ESKF_predict(Fx, Fi * Q * Fi.transpose(), state);
51 | }
52 | 
53 | }
54 | 


--------------------------------------------------------------------------------
/eskf_localizer/third_party/GeographicLib/src/LocalCartesian.cpp:
--------------------------------------------------------------------------------
 1 | /**
 2 |  * \file LocalCartesian.cpp
 3 |  * \brief Implementation for GeographicLib::LocalCartesian class
 4 |  *
 5 |  * Copyright (c) Charles Karney (2008-2015) <charles@karney.com> and licensed
 6 |  * under the MIT/X11 License.  For more information, see
 7 |  * https://geographiclib.sourceforge.io/
 8 |  **********************************************************************/
 9 | 
10 | #include "LocalCartesian.hpp"
11 | 
12 | namespace GeographicLib {
13 | 
14 |   using namespace std;
15 | 
16 |   void LocalCartesian::Reset(real lat0, real lon0, real h0) {
17 |     _lat0 = Math::LatFix(lat0);
18 |     _lon0 = Math::AngNormalize(lon0);
19 |     _h0 = h0;
20 |     _earth.Forward(_lat0, _lon0, _h0, _x0, _y0, _z0);
21 |     real sphi, cphi, slam, clam;
22 |     Math::sincosd(_lat0, sphi, cphi);
23 |     Math::sincosd(_lon0, slam, clam);
24 |     Geocentric::Rotation(sphi, cphi, slam, clam, _r);
25 |   }
26 | 
27 |   void LocalCartesian::MatrixMultiply(real M[dim2_]) const {
28 |     // M = r' . M
29 |     real t[dim2_];
30 |     copy(M, M + dim2_, t);
31 |     for (size_t i = 0; i < dim2_; ++i) {
32 |       size_t row = i / dim_, col = i % dim_;
33 |       M[i] = _r[row] * t[col] + _r[row+3] * t[col+3] + _r[row+6] * t[col+6];
34 |     }
35 |   }
36 | 
37 |   void LocalCartesian::IntForward(real lat, real lon, real h,
38 |                                   real& x, real& y, real& z,
39 |                                   real M[dim2_]) const {
40 |     real xc, yc, zc;
41 |     _earth.IntForward(lat, lon, h, xc, yc, zc, M);
42 |     xc -= _x0; yc -= _y0; zc -= _z0;
43 |     x = _r[0] * xc + _r[3] * yc + _r[6] * zc;
44 |     y = _r[1] * xc + _r[4] * yc + _r[7] * zc;
45 |     z = _r[2] * xc + _r[5] * yc + _r[8] * zc;
46 |     if (M)
47 |       MatrixMultiply(M);
48 |   }
49 | 
50 |   void LocalCartesian::IntReverse(real x, real y, real z,
51 |                                   real& lat, real& lon, real& h,
52 |                                   real M[dim2_]) const {
53 |     real
54 |       xc = _x0 + _r[0] * x + _r[1] * y + _r[2] * z,
55 |       yc = _y0 + _r[3] * x + _r[4] * y + _r[5] * z,
56 |       zc = _z0 + _r[6] * x + _r[7] * y + _r[8] * z;
57 |     _earth.IntReverse(xc, yc, zc, lat, lon, h, M);
58 |     if (M)
59 |       MatrixMultiply(M);
60 |   }
61 | 
62 | } // namespace GeographicLib
63 | 


--------------------------------------------------------------------------------
/eskf_localizer/third_party/GeographicLib/src/Math.cpp:
--------------------------------------------------------------------------------
 1 | /**
 2 |  * \file Math.cpp
 3 |  * \brief Implementation for GeographicLib::Math class
 4 |  *
 5 |  * Copyright (c) Charles Karney (2015) <charles@karney.com> and licensed
 6 |  * under the MIT/X11 License.  For more information, see
 7 |  * https://geographiclib.sourceforge.io/
 8 |  **********************************************************************/
 9 | 
10 | #include "Math.hpp"
11 | 
12 | #if defined(_MSC_VER)
13 | // Squelch warnings about constant conditional expressions
14 | #  pragma warning (disable: 4127)
15 | #endif
16 | 
17 | namespace GeographicLib {
18 | 
19 |   using namespace std;
20 | 
21 |   template<typename T> T Math::eatanhe(T x, T es)  {
22 |     return es > T(0) ? es * atanh(es * x) : -es * atan(es * x);
23 |   }
24 | 
25 |   template<typename T> T Math::taupf(T tau, T es) {
26 |     T tau1 = hypot(T(1), tau),
27 |       sig = sinh( eatanhe(tau / tau1, es ) );
28 |     return hypot(T(1), sig) * tau - sig * tau1;
29 |   }
30 | 
31 |   template<typename T> T Math::tauf(T taup, T es) {
32 |     static const int numit = 5;
33 |     static const T tol = sqrt(numeric_limits<T>::epsilon()) / T(10);
34 |     T e2m = T(1) - sq(es),
35 |       // To lowest order in e^2, taup = (1 - e^2) * tau = _e2m * tau; so use
36 |       // tau = taup/_e2m as a starting guess.  (This starting guess is the
37 |       // geocentric latitude which, to first order in the flattening, is equal
38 |       // to the conformal latitude.)  Only 1 iteration is needed for |lat| <
39 |       // 3.35 deg, otherwise 2 iterations are needed.  If, instead, tau = taup
40 |       // is used the mean number of iterations increases to 1.99 (2 iterations
41 |       // are needed except near tau = 0).
42 |       tau = taup/e2m,
43 |       stol = tol * max(T(1), abs(taup));
44 |     // min iterations = 1, max iterations = 2; mean = 1.94
45 |     for (int i = 0; i < numit || GEOGRAPHICLIB_PANIC; ++i) {
46 |       T taupa = taupf(tau, es),
47 |         dtau = (taup - taupa) * (1 + e2m * sq(tau)) /
48 |         ( e2m * hypot(T(1), tau) * hypot(T(1), taupa) );
49 |       tau += dtau;
50 |       if (!(abs(dtau) >= stol))
51 |         break;
52 |     }
53 |     return tau;
54 |   }
55 | 
56 |   /// \cond SKIP
57 |   // Instantiate
58 |   template Math::real Math::eatanhe<Math::real>(Math::real, Math::real);
59 |   template Math::real Math::taupf<Math::real>(Math::real, Math::real);
60 |   template Math::real Math::tauf<Math::real>(Math::real, Math::real);
61 |   /// \endcond
62 | 
63 | } // namespace GeographicLib
64 | 


--------------------------------------------------------------------------------
/eskf_localizer/include/eskf_localizer/utils.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <math.h>
 4 | #include <Eigen/Core>
 5 | #include <Eigen/Geometry>
 6 | #include <LocalCartesian.hpp>
 7 | 
 8 | namespace ESKF_Localization {
 9 | 
10 | constexpr double kDegreeToRadian = M_PI / 180.;
11 | constexpr double kRadianToDegree = 180. / M_PI;
12 | 
13 | inline void ConvertLLAToENU(const Eigen::Vector3d& init_lla, 
14 |                             const Eigen::Vector3d& point_lla, 
15 |                             Eigen::Vector3d* point_enu) {
16 |     static GeographicLib::LocalCartesian local_cartesian;
17 |     local_cartesian.Reset(init_lla(0), init_lla(1), init_lla(2));
18 |     local_cartesian.Forward(point_lla(0), point_lla(1), point_lla(2), 
19 |                             point_enu->data()[0], point_enu->data()[1], point_enu->data()[2]);
20 | }
21 | 
22 | inline void ConvertENUToLLA(const Eigen::Vector3d& init_lla, 
23 |                             const Eigen::Vector3d& point_enu,
24 |                             Eigen::Vector3d* point_lla) {
25 |     static GeographicLib::LocalCartesian local_cartesian;
26 |     local_cartesian.Reset(init_lla(0), init_lla(1), init_lla(2));
27 |     local_cartesian.Reverse(point_enu(0), point_enu(1), point_enu(2), 
28 |                             point_lla->data()[0], point_lla->data()[1], point_lla->data()[2]);                            
29 | }
30 | 
31 | inline Eigen::Matrix3d hat(const Eigen::Vector3d& v) {
32 |     Eigen::Matrix3d w;
33 |     w <<  0.,   -v(2),  v(1),
34 |           v(2),  0.,   -v(0),
35 |          -v(1),  v(0),  0.;
36 | 
37 |     return w;
38 | }
39 | 
40 | inline Eigen::Matrix<double,3,4> diff_q_a_qT_diff_q(const Eigen::Quaterniond& q, const Eigen::Vector3d& a){
41 | 	Eigen::Matrix<double,3,4> jacob = Eigen::Matrix<double,3,4>::Zero();
42 | 	Eigen::Vector3d v(q.x(),q.y(),q.z());
43 | 
44 | 	jacob.block<3,1>(0,0) = q.w() * a + v.cross(a);
45 | 	jacob.block<3,3>(0,1) = v.transpose()*a*Eigen::Matrix3d::Identity() + v*a.transpose() - a *v.transpose() - q.w()*hat(a);
46 | 	return jacob;
47 | }
48 | 
49 | inline Eigen::Matrix<double,3,4> diff_qT_a_q_diff_q(const Eigen::Quaterniond& q, const Eigen::Vector3d& a){
50 | 	Eigen::Matrix<double,3,4> jacob = diff_q_a_qT_diff_q(q,a);
51 | 	Eigen::Matrix4d diff_qT_diff_q = Eigen::Matrix4d::Identity();
52 | 	diff_qT_diff_q.block<3,3>(1,1) = -Eigen::Matrix3d::Identity();
53 | 	return jacob * diff_qT_diff_q;
54 | }
55 | 
56 | inline Eigen::Matrix4d quat_l(const Eigen::Quaterniond& q){
57 | 	Eigen::Matrix4d mat;
58 | 	Eigen::Vector3d qv(q.x(),q.y(),q.z());
59 | 	mat << 0, -qv.transpose(), qv, hat(qv);
60 | 	mat += q.w() * Eigen::Matrix4d::Identity();
61 | 	return mat;
62 | }
63 | 
64 | }  // namespace ImuGpsLocalization
65 | 


--------------------------------------------------------------------------------
/eskf_localizer/src/eskf.cpp:
--------------------------------------------------------------------------------
 1 | #include "eskf_localizer/base_type.h"
 2 | #include "eskf_localizer/eskf.h"
 3 | #include <Eigen/Core>
 4 | #include <Eigen/Geometry>
 5 | 
 6 | #include <ros/console.h>
 7 | 
 8 | namespace ESKF_Localization{
 9 | 
10 | 	void ESKF_update_nominal(ImuDataPtr imu_data,double dt, State* state, Eigen::Vector3d g){
11 | 		double dt2_2 = 0.5*dt*dt;
12 | 		State old_state = *state;
13 | 
14 | 		state->G_p_I = old_state.G_p_I + dt * old_state.G_v_I + dt2_2 * (old_state.G_R_I * (imu_data->accel - old_state.ab) + g);
15 | 		state->G_v_I = old_state.G_v_I + dt * (old_state.G_R_I * (imu_data->accel - old_state.ab) + g);
16 | 
17 | 		if (imu_data->quat.norm() != 0 && imu_data->last_quat.norm() != 0){
18 | 			ROS_INFO("Updating orientation by quaternion");
19 | 			//quaternion valid, update orientation according to delta quaternion
20 | 			Eigen::Matrix3d d_rot(imu_data->last_quat.inverse() * imu_data->quat);
21 | 
22 | 			state->G_R_I = old_state.G_R_I * d_rot;
23 | 			const Eigen::Vector3d d_theta = (- old_state.wb) * dt;
24 | 			if (d_theta.norm() >= 1e-12){
25 | 				Eigen::AngleAxisd d_rot2(d_theta.norm(),d_theta.normalized());
26 | 				state->G_R_I *= d_rot2.toRotationMatrix();
27 | 			}
28 | 		}else{
29 | 			const Eigen::Vector3d d_theta = (imu_data->gyro - old_state.wb) * dt;
30 | 			if (d_theta.norm() >= 1e-12){
31 | 				Eigen::AngleAxisd d_rot(d_theta.norm(),d_theta.normalized());
32 | 				state->G_R_I = old_state.G_R_I * d_rot.toRotationMatrix();
33 | 			}
34 | 		}
35 | 
36 | 		if(imu_data->quat.norm() != 0){
37 | 			//current quaternion valid, haven't got last quaternion.
38 | 			imu_data->last_quat = imu_data->quat;
39 | 		}
40 | 
41 | 		//state->ab = old_state.ab;
42 | 		//state->wb = old_state.wb;
43 | 	}
44 | 
45 | 	void ESKF_predict(Eigen::MatrixXd Fx, Eigen::MatrixXd Q, State* state){
46 | 		State old_state = *state;
47 | 		state->P = (Fx * old_state.P * Fx.transpose() + Q).eval();
48 | 	}
49 | 
50 | 	void ESKF_correct(Eigen::VectorXd z,Eigen::VectorXd h_x, Eigen::MatrixXd H, Eigen::MatrixXd V, State* state){
51 | 		State old_state = *state;
52 | 
53 | 		Eigen::MatrixXd K = old_state.P * H.transpose() * (H * old_state.P * H.transpose() + V).inverse();
54 | 		Eigen::VectorXd del_x = K * (z - h_x);
55 | 
56 | 		Eigen::MatrixXd I_KH = Eigen::Matrix<double, 15, 15>::Identity() - K * H;
57 | 		state->P = (I_KH * old_state.P * I_KH.transpose() + K * V * K.transpose()).eval();
58 | 
59 | 		//inject error state
60 | 		state->G_p_I = old_state.G_p_I + del_x.segment<3>(0);
61 | 		state->G_v_I = old_state.G_v_I + del_x.segment<3>(3);
62 | 		const Eigen::Vector3d del_theta = del_x.segment<3>(6);
63 | 		if (del_theta.norm() >= 1e-12){
64 | 			state->G_R_I = old_state.G_R_I * Eigen::AngleAxisd(del_theta.norm(),del_theta.normalized()).toRotationMatrix();
65 | 		}
66 | 		state->ab = old_state.ab + del_x.segment<3>(9);
67 | 		state->wb = old_state.wb + del_x.segment<3>(12);
68 | 	}
69 | }
70 | 
71 | 
72 | 
73 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>eskf_localization</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The eskf_localization package</description>
 6 | 
 7 |   <!-- One maintainer tag required, multiple allowed, one person per tag -->
 8 |   <!-- Example:  -->
 9 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10 |   <maintainer email="inf@todo.todo">inf</maintainer>
11 | 
12 | 
13 |   <!-- One license tag required, multiple allowed, one license per tag -->
14 |   <!-- Commonly used license strings: -->
15 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16 |   <license>TODO</license>
17 | 
18 | 
19 |   <!-- Url tags are optional, but multiple are allowed, one per tag -->
20 |   <!-- Optional attribute type can be: website, bugtracker, or repository -->
21 |   <!-- Example: -->
22 |   <!-- <url type="website">http://wiki.ros.org/eskf_localization</url> -->
23 | 
24 | 
25 |   <!-- Author tags are optional, multiple are allowed, one per tag -->
26 |   <!-- Authors do not have to be maintainers, but could be -->
27 |   <!-- Example: -->
28 |   <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
29 | 
30 | 
31 |   <!-- The *depend tags are used to specify dependencies -->
32 |   <!-- Dependencies can be catkin packages or system dependencies -->
33 |   <!-- Examples: -->
34 |   <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
35 |   <!--   <depend>roscpp</depend> -->
36 |   <!--   Note that this is equivalent to the following: -->
37 |   <!--   <build_depend>roscpp</build_depend> -->
38 |   <!--   <exec_depend>roscpp</exec_depend> -->
39 |   <!-- Use build_depend for packages you need at compile time: -->
40 |   <!--   <build_depend>message_generation</build_depend> -->
41 |   <!-- Use build_export_depend for packages you need in order to build against this package: -->
42 |   <!--   <build_export_depend>message_generation</build_export_depend> -->
43 |   <!-- Use buildtool_depend for build tool packages: -->
44 |   <!--   <buildtool_depend>catkin</buildtool_depend> -->
45 |   <!-- Use exec_depend for packages you need at runtime: -->
46 |   <!--   <exec_depend>message_runtime</exec_depend> -->
47 |   <!-- Use test_depend for packages you need only for testing: -->
48 |   <!--   <test_depend>gtest</test_depend> -->
49 |   <!-- Use doc_depend for packages you need only for building documentation: -->
50 |   <!--   <doc_depend>doxygen</doc_depend> -->
51 |   <buildtool_depend>catkin</buildtool_depend>
52 |   <build_depend>roscpp</build_depend>
53 |   <build_depend>rospy</build_depend>
54 |   <build_depend>std_msgs</build_depend>
55 |   <build_depend>sensor_msgs</build_depend>
56 |   <build_depend>nav_msgs</build_depend>
57 |   <build_depend>geometry_msgs</build_depend>
58 |   <build_depend>tf</build_depend>
59 |   <build_export_depend>roscpp</build_export_depend>
60 |   <build_export_depend>rospy</build_export_depend>
61 |   <build_export_depend>std_msgs</build_export_depend>
62 |   <build_export_depend>sensor_msgs</build_export_depend>
63 |   <build_export_depend>nav_msgs</build_export_depend>
64 |   <build_export_depend>geometry_msgs</build_export_depend>
65 |   <build_export_depend>tf</build_export_depend>
66 |   <exec_depend>roscpp</exec_depend>
67 |   <exec_depend>rospy</exec_depend>
68 |   <exec_depend>std_msgs</exec_depend>
69 |   <exec_depend>sensor_msgs</exec_depend>
70 |   <exec_depend>nav_msgs</exec_depend>
71 |   <exec_depend>geometry_msgs</exec_depend>
72 |   <exec_depend>tf</exec_depend>
73 | 
74 | 
75 |   <!-- The export tag contains other, unspecified, tags -->
76 |   <export>
77 |     <!-- Other tools can request additional information be placed here -->
78 | 
79 |   </export>
80 | </package>
81 | 


--------------------------------------------------------------------------------
/eskf_localizer/src/eskf_localizer.cpp:
--------------------------------------------------------------------------------
  1 | #include "eskf_localizer/eskf_localizer.h"
  2 | #include "eskf_localizer/base_type.h"
  3 | #include "eskf_localizer/utils.h"
  4 | 
  5 | #include <Eigen/Core>
  6 | #include <Eigen/Geometry>
  7 | 
  8 | #include <geometry_msgs/Pose.h>
  9 | #include <sensor_msgs/NavSatFix.h>
 10 | #include <nav_msgs/Odometry.h>
 11 | 
 12 | namespace ESKF_Localization{
 13 | 	ESKF_Localizer::ESKF_Localizer(const double am_noise, const double wm_noise,
 14 | 			const double ab_noise, const double wb_noise, Eigen::Vector3d I_p_Gps){
 15 | 		state_.G_p_I = Eigen::Vector3d::Zero();
 16 | 		state_.G_v_I = Eigen::Vector3d::Zero();
 17 | 		state_.G_R_I = Eigen::Matrix3d::Identity();
 18 | 		state_.ab = Eigen::Vector3d::Zero();
 19 | 		state_.wb = Eigen::Vector3d::Zero();
 20 | 
 21 | 		initializer_ = std::make_unique<Initializer>(10,10,10,&state_);
 22 | 		Eigen::Vector3d g(0,0,-9.81);
 23 | 		imu_processor_ = std::make_unique<ImuProcessor>(am_noise,wm_noise,ab_noise,wb_noise,g);
 24 | 		gps_processor_ = std::make_unique<GpsProcessor>(I_p_Gps);
 25 | 		mag_processor_ = std::make_unique<MagProcessor>(0.7*Eigen::Matrix3d::Identity());
 26 | 		last_t_ = ros::Time::now().toSec();
 27 | 	}
 28 | 
 29 | 	void ESKF_Localizer::processImuData(ImuDataPtr imu_data){
 30 | 		if(!initializer_->is_initialized()){
 31 | 			initializer_->Imu_initialize(imu_data);
 32 | 			return;
 33 | 		}
 34 | 
 35 | 		double t = imu_data->timestamp;
 36 | 		double dt = t - last_t_;
 37 | 		last_t_ = t;
 38 | 		if (dt >= 0.5 || dt <= 0){dt = 0.01;}
 39 | 
 40 | 		imu_processor_->Imu_predict(imu_data,dt,&state_);
 41 | 	}
 42 | 
 43 | 	void ESKF_Localizer::processGpsData(GpsPositionDataPtr gps_data){
 44 | 		if(!initializer_->is_initialized()){
 45 | 			initializer_->Gps_initialize(gps_data);
 46 | 			return;
 47 | 		}
 48 | 
 49 | 		gps_processor_->Gps_correct(gps_data,&state_);
 50 | 	}
 51 | 
 52 | 	void ESKF_Localizer::processMagData(MagDataPtr mag_data){
 53 | 		if(!initializer_->is_initialized()){
 54 | 			initializer_->Mag_initialize(mag_data);
 55 | 			return;
 56 | 		}
 57 | 
 58 | 		//mag_processor_->Mag_correct(mag_data,&state_);
 59 | 	}
 60 | 
 61 | 	State* ESKF_Localizer::getState(){
 62 | 		return &state_;
 63 | 	}
 64 | 
 65 |     geometry_msgs::Pose ESKF_Localizer::getFusedPose(){
 66 |     	geometry_msgs::Pose fusedPose;
 67 |     	fusedPose.position.x = state_.G_p_I.x();
 68 |     	fusedPose.position.y = state_.G_p_I.y();
 69 |     	fusedPose.position.z = state_.G_p_I.z();
 70 | 
 71 |     	Eigen::Quaterniond q(state_.G_R_I);
 72 | 
 73 |     	fusedPose.orientation.w = q.w();
 74 |     	fusedPose.orientation.x = q.x();
 75 |     	fusedPose.orientation.y = q.y();
 76 |     	fusedPose.orientation.z = q.z();
 77 | 
 78 |     	return fusedPose;
 79 |     }
 80 | 
 81 |     sensor_msgs::NavSatFix ESKF_Localizer::getFusedFix(){
 82 |     	sensor_msgs::NavSatFix fusedFix;
 83 |     	Eigen::Vector3d fused_lla;
 84 |     	ConvertENUToLLA(state_.lla_origin, state_.G_p_I,&fused_lla);
 85 | 
 86 |     	fusedFix.header.frame_id = "world";
 87 |     	fusedFix.header.stamp = ros::Time::now();
 88 |     	fusedFix.latitude = fused_lla[0];
 89 |     	fusedFix.longitude = fused_lla[1];
 90 |     	fusedFix.altitude = fused_lla[2];
 91 |     	return fusedFix;
 92 |     }
 93 | 
 94 |     nav_msgs::Odometry ESKF_Localizer::getFusedOdometry(){
 95 |     	nav_msgs::Odometry odom;
 96 | 
 97 |     	odom.header.frame_id = "world";
 98 |     	odom.header.stamp = ros::Time::now();
 99 |     	odom.child_frame_id = "XIMU";
100 | 
101 |     	odom.pose.pose = getFusedPose();
102 | 
103 |     	Eigen::Vector3d I_v = state_.G_R_I.transpose() * state_.G_v_I;//global velocity to local velocity
104 |     	//Eigen::Vector3d I_w = ; NO angular velocity estimation
105 | 
106 |     	//Note: twist message is specified in frame given by "child_frame_id", i.e. XIMU.
107 |     	odom.twist.twist.linear.x = I_v[0];
108 |     	odom.twist.twist.linear.y = I_v[1];
109 |     	odom.twist.twist.linear.z = I_v[2];
110 | 
111 |     	return odom;
112 |     }
113 | 
114 | }
115 | 
116 | 
117 | 
118 | 


--------------------------------------------------------------------------------
/eskf_localizer/src/initializer.cpp:
--------------------------------------------------------------------------------
  1 | #include "eskf_localizer/initializer.h"
  2 | #include "eskf_localizer/base_type.h"
  3 | #include <Eigen/Core>
  4 | #include <Eigen/Geometry>
  5 | 
  6 | //FIXME
  7 | #include <ros/console.h>
  8 | 
  9 | namespace ESKF_Localization{
 10 | 
 11 | Initializer::Initializer(int imu_initialize_goal, int gps_initialize_goal,int mag_initialize_goal, State* state){
 12 | 	imu_initialize_goal_ = imu_initialize_goal;
 13 | 	gps_initialize_goal_ = gps_initialize_goal;
 14 | 	mag_initialize_goal_ = mag_initialize_goal;
 15 | 	state_ = state;
 16 | 
 17 | 	//initial value of P
 18 | 	state->P.block<3,3>(0,0) = 10.0 * Eigen::Matrix3d::Identity();		//position cov
 19 | 	state->P.block<3,3>(3,3) = 5.0 * Eigen::Matrix3d::Identity();		//velocity cov
 20 | 	state->P.block<3,3>(6,6) = 0.04 * Eigen::Matrix3d::Identity();		//roll-pitch-yaw cov
 21 | 	state->P.block<3,3>(9,9) = 0.0004 * Eigen::Matrix3d::Identity();	//acceleration bias cov
 22 | 	state->P.block<3,3>(12,12) = 0.0004 * Eigen::Matrix3d::Identity();	//gyroscope bias cov
 23 | }
 24 | 
 25 | void Initializer::Imu_initialize(ESKF_Localization::ImuDataPtr imu_data){
 26 | 	if(!got_first_gps_message_){
 27 | 		return;
 28 | 	}
 29 | 
 30 | 	//if (gps_initialized_){return;}magnetic is slower than imu, constantly update buffer
 31 | 
 32 | 	imu_buffer_.push_back(imu_data);
 33 | 	if (imu_buffer_.size() > imu_initialize_goal_){
 34 | 		imu_buffer_.pop_front();
 35 | 		imu_initialized_ = true;
 36 | 	}
 37 | 
 38 | }
 39 | 
 40 | void Initializer::Gps_initialize(ESKF_Localization::GpsPositionDataPtr gps_data){
 41 | 	got_first_gps_message_ = true;
 42 | 
 43 | 	if (gps_initialized_){return;}
 44 | 
 45 | 	if (gps_buffer_.size() < gps_initialize_goal_){
 46 | 		gps_buffer_.push_back(gps_data);
 47 | 	}else{
 48 | 		//compute lla_origin
 49 | 		Eigen::Vector3d lla_average(0.,0.,0.);
 50 | 		for (ESKF_Localization::GpsPositionDataPtr gps_d : gps_buffer_){
 51 | 			lla_average += gps_d->lla;
 52 | 		}
 53 | 		lla_average /= gps_initialize_goal_;
 54 | 		state_->lla_origin = lla_average;
 55 | 		gps_initialized_ = true;
 56 | 		ROS_INFO("[Initializer] Gps initialized with lla = %f,%f,%f",state_->lla_origin.x(),state_->lla_origin.y(),state_->lla_origin.z());
 57 | 	}
 58 | 
 59 | }
 60 | 
 61 | void Initializer::Mag_initialize(ESKF_Localization::MagDataPtr mag_data){
 62 | 	if (!got_first_gps_message_ || mag_initialized_){
 63 | 		return;
 64 | 	}
 65 | 	if (mag_buffer_.size() < mag_initialize_goal_){
 66 | 		mag_buffer_.push_back(mag_data);
 67 | 	}
 68 | 	if (imu_initialized_ && mag_buffer_.size() == mag_initialize_goal_){
 69 | 		//average imu and mag samples
 70 | 		Eigen::Vector3d am_init = Eigen::Vector3d::Zero();
 71 | 		Eigen::Vector3d mm_init = Eigen::Vector3d::Zero();
 72 | 
 73 | 		for (ESKF_Localization::ImuDataPtr imu_d : imu_buffer_){
 74 | 			am_init += imu_d->accel;
 75 | 		}
 76 | 		am_init /= imu_buffer_.size();
 77 | 
 78 | 		for (ESKF_Localization::MagDataPtr mag_d : mag_buffer_){
 79 | 			mm_init += mag_d->mag;
 80 | 		}
 81 | 		mm_init /= mag_buffer_.size();
 82 | 
 83 | 		//calculate initial orientation
 84 | 		Eigen::Matrix3d I_R_G;
 85 | 		Eigen::Vector3d x_ref, y_ref, z_ref;//global x,y,z axis reference in Imu frame
 86 | 
 87 | 		z_ref = am_init.normalized();//z reference - negative gravity
 88 | 		x_ref = mm_init.cross(z_ref).normalized();//mag(North + UP component) cross with z_ref(Up) to get East
 89 | 		y_ref = z_ref.cross(x_ref).normalized();//z_ref(Up) cross with x_ref(East) to get North
 90 | 
 91 | 		//fill in global-to-local rotation matrix, which is the inverse of state->G_R_I that we store orientation
 92 | 		I_R_G.block<3,1>(0,0) = x_ref;
 93 | 		I_R_G.block<3,1>(0,1) = y_ref;
 94 | 		I_R_G.block<3,1>(0,2) = z_ref;
 95 | 
 96 | 		//update state
 97 | 		state_->G_R_I = I_R_G.transpose();
 98 | 
 99 | 		//FIXME
100 | 		Eigen::Quaterniond q(state_->G_R_I);
101 | 		ROS_INFO("[Initializer] Orientation initialized with q: w:%f, x:%f, y:%f, z:%f",q.w(),q.x(),q.y(),q.z());
102 | 
103 | 		//calculate m_ref
104 | 		state_->m_ref = state_->G_R_I * mm_init;
105 | 
106 | 		//FIXME
107 | 		ROS_INFO("[Initializer] magnetic reference initialized with m_ref = %f,%f,%f",state_->m_ref[0],state_->m_ref[1],state_->m_ref[2]);
108 | 
109 | 		mag_initialized_ = true;
110 | 	}
111 | 
112 | }
113 | 
114 | bool Initializer::is_initialized(){
115 | 	eskf_initialized_ = imu_initialized_ && gps_initialized_ && mag_initialized_;
116 | 
117 | 	return eskf_initialized_;
118 | }
119 | 
120 | 
121 | }
122 | 


--------------------------------------------------------------------------------
/ros_wrapper/src/eskf_localization_wrapper.cpp:
--------------------------------------------------------------------------------
  1 | #include "eskf_localization_wrapper.h"
  2 | #include "eskf_localizer/base_type.h"
  3 | 
  4 | #include <Eigen/Geometry>
  5 | 
  6 | #include <ros/ros.h>
  7 | #include <geometry_msgs/Pose.h>
  8 | #include <sensor_msgs/NavSatFix.h>
  9 | #include <nav_msgs/Odometry.h>
 10 | #include <tf/tf.h>
 11 | 
 12 | ESKFLocalizationWrapper::ESKFLocalizationWrapper(ros::NodeHandle nh){
 13 | 
 14 | 	//obtain constants from ros param
 15 | 	double am_noise, wm_noise, ab_noise, wb_noise;
 16 | 	double x,y,z;
 17 | 
 18 | 	nh.param("am_noise", am_noise, 1e-2);
 19 | 	nh.param("wm_noise", wm_noise, 1e-4);
 20 | 	nh.param("ab_noise", ab_noise, 1e-6);
 21 | 	nh.param("wb_noise", wb_noise, 1e-6);
 22 | 
 23 | 	nh.param("I_p_Gps_x", x, 0.0);
 24 | 	nh.param("I_p_Gps_y", y, 0.0);
 25 | 	nh.param("I_p_Gps_z", z, 0.0);
 26 | 	const Eigen::Vector3d I_p_Gps(x,y,z);
 27 | 
 28 | 	//initialize eskf_localizer with these parameters
 29 | 	eskf_localizer_ = std::make_unique<ESKF_Localization::ESKF_Localizer>(
 30 | 			am_noise,wm_noise, ab_noise, wb_noise, I_p_Gps);
 31 | 
 32 | 	imu_sub_ = nh.subscribe("/imu/data",10,&ESKFLocalizationWrapper::ImuCallback,this);
 33 | 	gps_sub_ = nh.subscribe("/fix",10,&ESKFLocalizationWrapper::GpsPositionCallback,this);
 34 | 	mag_sub_ = nh.subscribe("/imu/mag",10,&ESKFLocalizationWrapper::MagCallback,this);
 35 | 
 36 | 	fused_pose_pub_ = nh.advertise<geometry_msgs::Pose>("/fused_pose", 10);
 37 | 	fused_path_pub_ = nh.advertise<nav_msgs::Path>("/fused_path", 10);
 38 | 	fused_fix_pub_ = nh.advertise<sensor_msgs::NavSatFix>("/fused_fix",10);
 39 | 	fused_odom_pub_ = nh.advertise<nav_msgs::Odometry>("/fused_odom",10);
 40 | }
 41 | 
 42 | ESKFLocalizationWrapper::~ESKFLocalizationWrapper(){
 43 | 
 44 | }
 45 | 
 46 | void ESKFLocalizationWrapper::ImuCallback(const sensor_msgs::ImuConstPtr& imu_msg_ptr){
 47 | 	ESKF_Localization::ImuDataPtr imu_data_ptr = std::make_shared<ESKF_Localization::ImuData>();
 48 | 	imu_data_ptr->timestamp = ros::Time::now().toSec();
 49 | 
 50 | 	imu_data_ptr->accel << 	imu_msg_ptr->linear_acceleration.x,
 51 | 							imu_msg_ptr->linear_acceleration.y,
 52 | 							imu_msg_ptr->linear_acceleration.z;
 53 | 
 54 | 	imu_data_ptr->gyro << 	imu_msg_ptr->angular_velocity.x,
 55 | 							imu_msg_ptr->angular_velocity.y,
 56 | 							imu_msg_ptr->angular_velocity.z;
 57 | 
 58 | 	imu_data_ptr->quat = Eigen::Quaterniond(imu_msg_ptr->orientation.w,
 59 | 							imu_msg_ptr->orientation.x,
 60 | 							imu_msg_ptr->orientation.y,
 61 | 							imu_msg_ptr->orientation.z);
 62 | 
 63 | 	eskf_localizer_->processImuData(imu_data_ptr);
 64 | 
 65 | 	publishState();
 66 | }
 67 | 
 68 | void ESKFLocalizationWrapper::GpsPositionCallback(const sensor_msgs::NavSatFixConstPtr& gps_msg_ptr){
 69 | 	ESKF_Localization::GpsPositionDataPtr gps_data_ptr = std::make_shared<ESKF_Localization::GpsPositionData>();
 70 | 	gps_data_ptr->timestamp = ros::Time::now().toSec();
 71 | 
 72 | 	gps_data_ptr->lla << 	gps_msg_ptr->latitude,
 73 | 							gps_msg_ptr->longitude,
 74 | 							gps_msg_ptr->altitude;
 75 | 
 76 | 	gps_data_ptr->cov = Eigen::Map<const Eigen::Matrix3d>(gps_msg_ptr->position_covariance.data());
 77 | 
 78 | 	eskf_localizer_->processGpsData(gps_data_ptr);
 79 | 	publishState();
 80 | }
 81 | 
 82 | void ESKFLocalizationWrapper::MagCallback(const sensor_msgs::MagneticFieldConstPtr& mag_msg_ptr){
 83 | 	ESKF_Localization::MagDataPtr mag_data_ptr = std::make_shared<ESKF_Localization::MagData>();
 84 | 	mag_data_ptr->timestamp = ros::Time::now().toSec();
 85 | 
 86 | 	mag_data_ptr->mag << 	mag_msg_ptr->magnetic_field.x,
 87 | 							mag_msg_ptr->magnetic_field.y,
 88 | 							mag_msg_ptr->magnetic_field.z;
 89 | 
 90 | 	eskf_localizer_->processMagData(mag_data_ptr);
 91 | 	publishState();
 92 | }
 93 | 
 94 | void ESKFLocalizationWrapper::addStateToPath(ESKF_Localization::State* state){
 95 |     ros_path_.header.frame_id = "world";
 96 |     ros_path_.header.stamp = ros::Time::now();
 97 | 
 98 |     geometry_msgs::PoseStamped pose;
 99 |     pose.header = ros_path_.header;
100 | 
101 |     pose.pose.position.x = state->G_p_I[0];
102 |     pose.pose.position.y = state->G_p_I[1];
103 |     pose.pose.position.z = state->G_p_I[2];
104 | 
105 |     const Eigen::Quaterniond G_q_I(state->G_R_I);
106 |     pose.pose.orientation.x = G_q_I.x();
107 |     pose.pose.orientation.y = G_q_I.y();
108 |     pose.pose.orientation.z = G_q_I.z();
109 |     pose.pose.orientation.w = G_q_I.w();
110 | 
111 |     ros_path_.poses.push_back(pose);
112 | }
113 | 
114 | void ESKFLocalizationWrapper::publishState(){
115 | 	//publish fused states
116 | 	fused_pose_pub_.publish(eskf_localizer_->getFusedPose());
117 | 	fused_odom_pub_.publish(eskf_localizer_->getFusedOdometry());
118 | 	fused_fix_pub_.publish(eskf_localizer_->getFusedFix());
119 | 	addStateToPath(eskf_localizer_->getState());
120 | 	fused_path_pub_.publish(ros_path_);
121 | 
122 | 	//publish transform
123 | 	ESKF_Localization::State* state = eskf_localizer_->getState();
124 | 	tf::Transform tf;
125 | 	tf.setOrigin(tf::Vector3(state->G_p_I[0],state->G_p_I[1],state->G_p_I[2]));
126 | 	Eigen::Quaterniond quat(state->G_R_I);
127 | 	tf::Quaternion q(quat.x(),quat.y(),quat.z(),quat.w());//Caution! tf::Quaternion is in (x,y,z,w)
128 | 	tf.setRotation(q);
129 | 	br_.sendTransform(tf::StampedTransform(tf,ros::Time::now(),"world","XIMU"));
130 | }
131 | 
132 | 


--------------------------------------------------------------------------------
/ros_wrapper/rviz/rviz_config.rviz:
--------------------------------------------------------------------------------
  1 | Panels:
  2 |   - Class: rviz/Displays
  3 |     Help Height: 139
  4 |     Name: Displays
  5 |     Property Tree Widget:
  6 |       Expanded:
  7 |         - /Grid1
  8 |       Splitter Ratio: 0.5
  9 |     Tree Height: 1542
 10 |   - Class: rviz/Selection
 11 |     Name: Selection
 12 |   - Class: rviz/Tool Properties
 13 |     Expanded:
 14 |       - /2D Pose Estimate1
 15 |       - /2D Nav Goal1
 16 |       - /Publish Point1
 17 |     Name: Tool Properties
 18 |     Splitter Ratio: 0.5886790156364441
 19 |   - Class: rviz/Views
 20 |     Expanded:
 21 |       - /Current View1
 22 |     Name: Views
 23 |     Splitter Ratio: 0.5
 24 |   - Class: rviz/Time
 25 |     Experimental: false
 26 |     Name: Time
 27 |     SyncMode: 0
 28 |     SyncSource: ""
 29 | Preferences:
 30 |   PromptSaveOnExit: true
 31 | Toolbars:
 32 |   toolButtonStyle: 2
 33 | Visualization Manager:
 34 |   Class: ""
 35 |   Displays:
 36 |     - Alpha: 0.5
 37 |       Cell Size: 5
 38 |       Class: rviz/Grid
 39 |       Color: 160; 160; 164
 40 |       Enabled: true
 41 |       Line Style:
 42 |         Line Width: 0.029999999329447746
 43 |         Value: Lines
 44 |       Name: Grid
 45 |       Normal Cell Count: 0
 46 |       Offset:
 47 |         X: 0
 48 |         Y: 0
 49 |         Z: 0
 50 |       Plane: XY
 51 |       Plane Cell Count: 275
 52 |       Reference Frame: <Fixed Frame>
 53 |       Value: true
 54 |     - Class: rviz/TF
 55 |       Enabled: true
 56 |       Frame Timeout: 15
 57 |       Frames:
 58 |         All Enabled: false
 59 |         XIMU:
 60 |           Value: true
 61 |         world:
 62 |           Value: false
 63 |       Marker Scale: 30
 64 |       Name: TF
 65 |       Show Arrows: true
 66 |       Show Axes: true
 67 |       Show Names: true
 68 |       Tree:
 69 |         world:
 70 |           XIMU:
 71 |             {}
 72 |       Update Interval: 0
 73 |       Value: true
 74 |     - Alpha: 1
 75 |       Buffer Length: 1
 76 |       Class: rviz/Path
 77 |       Color: 25; 255; 0
 78 |       Enabled: true
 79 |       Head Diameter: 0.30000001192092896
 80 |       Head Length: 0.20000000298023224
 81 |       Length: 0.30000001192092896
 82 |       Line Style: Lines
 83 |       Line Width: 0.029999999329447746
 84 |       Name: Path
 85 |       Offset:
 86 |         X: 0
 87 |         Y: 0
 88 |         Z: 0
 89 |       Pose Color: 255; 85; 255
 90 |       Pose Style: None
 91 |       Radius: 0.029999999329447746
 92 |       Shaft Diameter: 0.10000000149011612
 93 |       Shaft Length: 0.10000000149011612
 94 |       Topic: /fused_path
 95 |       Unreliable: false
 96 |       Value: true
 97 |     - Class: rviz/Axes
 98 |       Enabled: true
 99 |       Length: 10
100 |       Name: Axes
101 |       Radius: 0.30000001192092896
102 |       Reference Frame: <Fixed Frame>
103 |       Value: true
104 |   Enabled: true
105 |   Global Options:
106 |     Background Color: 48; 48; 48
107 |     Default Light: true
108 |     Fixed Frame: world
109 |     Frame Rate: 60
110 |   Name: root
111 |   Tools:
112 |     - Class: rviz/Interact
113 |       Hide Inactive Objects: true
114 |     - Class: rviz/MoveCamera
115 |     - Class: rviz/Select
116 |     - Class: rviz/FocusCamera
117 |     - Class: rviz/Measure
118 |     - Class: rviz/SetInitialPose
119 |       Theta std deviation: 0.2617993950843811
120 |       Topic: /initialpose
121 |       X std deviation: 0.5
122 |       Y std deviation: 0.5
123 |     - Class: rviz/SetGoal
124 |       Topic: /move_base_simple/goal
125 |     - Class: rviz/PublishPoint
126 |       Single click: true
127 |       Topic: /clicked_point
128 |   Value: true
129 |   Views:
130 |     Current:
131 |       Class: rviz/Orbit
132 |       Distance: 1200
133 |       Enable Stereo Rendering:
134 |         Stereo Eye Separation: 0.05999999865889549
135 |         Stereo Focal Distance: 1
136 |         Swap Stereo Eyes: false
137 |         Value: false
138 |       Focal Point:
139 |         X: 0
140 |         Y: -175
141 |         Z: 0
142 |       Focal Shape Fixed Size: false
143 |       Focal Shape Size: 0.05000000074505806
144 |       Invert Z Axis: false
145 |       Name: Current View
146 |       Near Clip Distance: 0.009999999776482582
147 |       Pitch: 1.5697963237762451
148 |       Target Frame: <Fixed Frame>
149 |       Value: Orbit (rviz)
150 |       Yaw: 4.712485313415527
151 |     Saved: ~
152 | Window Geometry:
153 |   Displays:
154 |     collapsed: false
155 |   Height: 2049
156 |   Hide Left Dock: false
157 |   Hide Right Dock: false
158 |   QMainWindow State: 000000ff00000000fd00000004000000000000030b000006fffc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000006ff0000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000005320000023b0000000000000000fb0000000a0049006d00610067006500000004500000031d0000000000000000fb0000000c00430061006d00650072006101000006110000015c0000000000000000000000010000020b000006fffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006e000006ff0000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e7a0000005afc0100000002fb0000000800540069006d0065010000000000000e7a0000057100fffffffb0000000800540069006d006501000000000000045000000000000000000000094c000006ff00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
159 |   Selection:
160 |     collapsed: false
161 |   Time:
162 |     collapsed: false
163 |   Tool Properties:
164 |     collapsed: false
165 |   Views:
166 |     collapsed: false
167 |   Width: 3706
168 |   X: 134
169 |   Y: 55
170 | 


--------------------------------------------------------------------------------
/eskf_localizer/third_party/GeographicLib/src/Geocentric.cpp:
--------------------------------------------------------------------------------
  1 | /**
  2 |  * \file Geocentric.cpp
  3 |  * \brief Implementation for GeographicLib::Geocentric class
  4 |  *
  5 |  * Copyright (c) Charles Karney (2008-2017) <charles@karney.com> and licensed
  6 |  * under the MIT/X11 License.  For more information, see
  7 |  * https://geographiclib.sourceforge.io/
  8 |  **********************************************************************/
  9 | 
 10 | #include "Geocentric.hpp"
 11 | 
 12 | namespace GeographicLib {
 13 | 
 14 |   using namespace std;
 15 | 
 16 |   Geocentric::Geocentric(real a, real f)
 17 |     : _a(a)
 18 |     , _f(f)
 19 |     , _e2(_f * (2 - _f))
 20 |     , _e2m(Math::sq(1 - _f))    // 1 - _e2
 21 |     , _e2a(abs(_e2))
 22 |     , _e4a(Math::sq(_e2))
 23 |     , _maxrad(2 * _a / numeric_limits<real>::epsilon())
 24 |   {
 25 |     if (!(Math::isfinite(_a) && _a > 0))
 26 |       throw GeographicErr("Equatorial radius is not positive");
 27 |     if (!(Math::isfinite(_f) && _f < 1))
 28 |       throw GeographicErr("Polar semi-axis is not positive");
 29 |   }
 30 | 
 31 |   const Geocentric& Geocentric::WGS84() {
 32 |     static const Geocentric wgs84(Constants::WGS84_a(), Constants::WGS84_f());
 33 |     return wgs84;
 34 |   }
 35 | 
 36 |   void Geocentric::IntForward(real lat, real lon, real h,
 37 |                               real& X, real& Y, real& Z,
 38 |                               real M[dim2_]) const {
 39 |     real sphi, cphi, slam, clam;
 40 |     Math::sincosd(Math::LatFix(lat), sphi, cphi);
 41 |     Math::sincosd(lon, slam, clam);
 42 |     real n = _a/sqrt(1 - _e2 * Math::sq(sphi));
 43 |     Z = (_e2m * n + h) * sphi;
 44 |     X = (n + h) * cphi;
 45 |     Y = X * slam;
 46 |     X *= clam;
 47 |     if (M)
 48 |       Rotation(sphi, cphi, slam, clam, M);
 49 |   }
 50 | 
 51 |   void Geocentric::IntReverse(real X, real Y, real Z,
 52 |                               real& lat, real& lon, real& h,
 53 |                               real M[dim2_]) const {
 54 |     real
 55 |       R = Math::hypot(X, Y),
 56 |       slam = R != 0 ? Y / R : 0,
 57 |       clam = R != 0 ? X / R : 1;
 58 |     h = Math::hypot(R, Z);      // Distance to center of earth
 59 |     real sphi, cphi;
 60 |     if (h > _maxrad) {
 61 |       // We really far away (> 12 million light years); treat the earth as a
 62 |       // point and h, above, is an acceptable approximation to the height.
 63 |       // This avoids overflow, e.g., in the computation of disc below.  It's
 64 |       // possible that h has overflowed to inf; but that's OK.
 65 |       //
 66 |       // Treat the case X, Y finite, but R overflows to +inf by scaling by 2.
 67 |       R = Math::hypot(X/2, Y/2);
 68 |       slam = R != 0 ? (Y/2) / R : 0;
 69 |       clam = R != 0 ? (X/2) / R : 1;
 70 |       real H = Math::hypot(Z/2, R);
 71 |       sphi = (Z/2) / H;
 72 |       cphi = R / H;
 73 |     } else if (_e4a == 0) {
 74 |       // Treat the spherical case.  Dealing with underflow in the general case
 75 |       // with _e2 = 0 is difficult.  Origin maps to N pole same as with
 76 |       // ellipsoid.
 77 |       real H = Math::hypot(h == 0 ? 1 : Z, R);
 78 |       sphi = (h == 0 ? 1 : Z) / H;
 79 |       cphi = R / H;
 80 |       h -= _a;
 81 |     } else {
 82 |       // Treat prolate spheroids by swapping R and Z here and by switching
 83 |       // the arguments to phi = atan2(...) at the end.
 84 |       real
 85 |         p = Math::sq(R / _a),
 86 |         q = _e2m * Math::sq(Z / _a),
 87 |         r = (p + q - _e4a) / 6;
 88 |       if (_f < 0) swap(p, q);
 89 |       if ( !(_e4a * q == 0 && r <= 0) ) {
 90 |         real
 91 |           // Avoid possible division by zero when r = 0 by multiplying
 92 |           // equations for s and t by r^3 and r, resp.
 93 |           S = _e4a * p * q / 4, // S = r^3 * s
 94 |           r2 = Math::sq(r),
 95 |           r3 = r * r2,
 96 |           disc = S * (2 * r3 + S);
 97 |         real u = r;
 98 |         if (disc >= 0) {
 99 |           real T3 = S + r3;
100 |           // Pick the sign on the sqrt to maximize abs(T3).  This minimizes
101 |           // loss of precision due to cancellation.  The result is unchanged
102 |           // because of the way the T is used in definition of u.
103 |           T3 += T3 < 0 ? -sqrt(disc) : sqrt(disc); // T3 = (r * t)^3
104 |           // N.B. cbrt always returns the real root.  cbrt(-8) = -2.
105 |           real T = Math::cbrt(T3); // T = r * t
106 |           // T can be zero; but then r2 / T -> 0.
107 |           u += T + (T != 0 ? r2 / T : 0);
108 |         } else {
109 |           // T is complex, but the way u is defined the result is real.
110 |           real ang = atan2(sqrt(-disc), -(S + r3));
111 |           // There are three possible cube roots.  We choose the root which
112 |           // avoids cancellation.  Note that disc < 0 implies that r < 0.
113 |           u += 2 * r * cos(ang / 3);
114 |         }
115 |         real
116 |           v = sqrt(Math::sq(u) + _e4a * q), // guaranteed positive
117 |           // Avoid loss of accuracy when u < 0.  Underflow doesn't occur in
118 |           // e4 * q / (v - u) because u ~ e^4 when q is small and u < 0.
119 |           uv = u < 0 ? _e4a * q / (v - u) : u + v, // u+v, guaranteed positive
120 |           // Need to guard against w going negative due to roundoff in uv - q.
121 |           w = max(real(0), _e2a * (uv - q) / (2 * v)),
122 |           // Rearrange expression for k to avoid loss of accuracy due to
123 |           // subtraction.  Division by 0 not possible because uv > 0, w >= 0.
124 |           k = uv / (sqrt(uv + Math::sq(w)) + w),
125 |           k1 = _f >= 0 ? k : k - _e2,
126 |           k2 = _f >= 0 ? k + _e2 : k,
127 |           d = k1 * R / k2,
128 |           H = Math::hypot(Z/k1, R/k2);
129 |         sphi = (Z/k1) / H;
130 |         cphi = (R/k2) / H;
131 |         h = (1 - _e2m/k1) * Math::hypot(d, Z);
132 |       } else {                  // e4 * q == 0 && r <= 0
133 |         // This leads to k = 0 (oblate, equatorial plane) and k + e^2 = 0
134 |         // (prolate, rotation axis) and the generation of 0/0 in the general
135 |         // formulas for phi and h.  using the general formula and division by 0
136 |         // in formula for h.  So handle this case by taking the limits:
137 |         // f > 0: z -> 0, k      ->   e2 * sqrt(q)/sqrt(e4 - p)
138 |         // f < 0: R -> 0, k + e2 -> - e2 * sqrt(q)/sqrt(e4 - p)
139 |         real
140 |           zz = sqrt((_f >= 0 ? _e4a - p : p) / _e2m),
141 |           xx = sqrt( _f <  0 ? _e4a - p : p        ),
142 |           H = Math::hypot(zz, xx);
143 |         sphi = zz / H;
144 |         cphi = xx / H;
145 |         if (Z < 0) sphi = -sphi; // for tiny negative Z (not for prolate)
146 |         h = - _a * (_f >= 0 ? _e2m : 1) * H / _e2a;
147 |       }
148 |     }
149 |     lat = Math::atan2d(sphi, cphi);
150 |     lon = Math::atan2d(slam, clam);
151 |     if (M)
152 |       Rotation(sphi, cphi, slam, clam, M);
153 |   }
154 | 
155 |   void Geocentric::Rotation(real sphi, real cphi, real slam, real clam,
156 |                             real M[dim2_]) {
157 |     // This rotation matrix is given by the following quaternion operations
158 |     // qrot(lam, [0,0,1]) * qrot(phi, [0,-1,0]) * [1,1,1,1]/2
159 |     // or
160 |     // qrot(pi/2 + lam, [0,0,1]) * qrot(-pi/2 + phi , [-1,0,0])
161 |     // where
162 |     // qrot(t,v) = [cos(t/2), sin(t/2)*v[1], sin(t/2)*v[2], sin(t/2)*v[3]]
163 | 
164 |     // Local X axis (east) in geocentric coords
165 |     M[0] = -slam;        M[3] =  clam;        M[6] = 0;
166 |     // Local Y axis (north) in geocentric coords
167 |     M[1] = -clam * sphi; M[4] = -slam * sphi; M[7] = cphi;
168 |     // Local Z axis (up) in geocentric coords
169 |     M[2] =  clam * cphi; M[5] =  slam * cphi; M[8] = sphi;
170 |   }
171 | 
172 | } // namespace GeographicLib
173 | 


--------------------------------------------------------------------------------
/eskf_localizer/third_party/GeographicLib/include/LocalCartesian.hpp:
--------------------------------------------------------------------------------
  1 | /**
  2 |  * \file LocalCartesian.hpp
  3 |  * \brief Header for GeographicLib::LocalCartesian class
  4 |  *
  5 |  * Copyright (c) Charles Karney (2008-2016) <charles@karney.com> and licensed
  6 |  * under the MIT/X11 License.  For more information, see
  7 |  * https://geographiclib.sourceforge.io/
  8 |  **********************************************************************/
  9 | 
 10 | #if !defined(GEOGRAPHICLIB_LOCALCARTESIAN_HPP)
 11 | #define GEOGRAPHICLIB_LOCALCARTESIAN_HPP 1
 12 | 
 13 | #include "Geocentric.hpp"
 14 | #include "Constants.hpp"
 15 | 
 16 | namespace GeographicLib {
 17 | 
 18 |   /**
 19 |    * \brief Local cartesian coordinates
 20 |    *
 21 |    * Convert between geodetic coordinates latitude = \e lat, longitude = \e
 22 |    * lon, height = \e h (measured vertically from the surface of the ellipsoid)
 23 |    * to local cartesian coordinates (\e x, \e y, \e z).  The origin of local
 24 |    * cartesian coordinate system is at \e lat = \e lat0, \e lon = \e lon0, \e h
 25 |    * = \e h0. The \e z axis is normal to the ellipsoid; the \e y axis points
 26 |    * due north.  The plane \e z = - \e h0 is tangent to the ellipsoid.
 27 |    *
 28 |    * The conversions all take place via geocentric coordinates using a
 29 |    * Geocentric object (by default Geocentric::WGS84()).
 30 |    *
 31 |    * Example of use:
 32 |    * \include example-LocalCartesian.cpp
 33 |    *
 34 |    * <a href="CartConvert.1.html">CartConvert</a> is a command-line utility
 35 |    * providing access to the functionality of Geocentric and LocalCartesian.
 36 |    **********************************************************************/
 37 | 
 38 |   class GEOGRAPHICLIB_EXPORT LocalCartesian {
 39 |   private:
 40 |     typedef Math::real real;
 41 |     static const size_t dim_ = 3;
 42 |     static const size_t dim2_ = dim_ * dim_;
 43 |     Geocentric _earth;
 44 |     real _lat0, _lon0, _h0;
 45 |     real _x0, _y0, _z0, _r[dim2_];
 46 |     void IntForward(real lat, real lon, real h, real& x, real& y, real& z,
 47 |                     real M[dim2_]) const;
 48 |     void IntReverse(real x, real y, real z, real& lat, real& lon, real& h,
 49 |                     real M[dim2_]) const;
 50 |     void MatrixMultiply(real M[dim2_]) const;
 51 |   public:
 52 | 
 53 |     /**
 54 |      * Constructor setting the origin.
 55 |      *
 56 |      * @param[in] lat0 latitude at origin (degrees).
 57 |      * @param[in] lon0 longitude at origin (degrees).
 58 |      * @param[in] h0 height above ellipsoid at origin (meters); default 0.
 59 |      * @param[in] earth Geocentric object for the transformation; default
 60 |      *   Geocentric::WGS84().
 61 |      *
 62 |      * \e lat0 should be in the range [&minus;90&deg;, 90&deg;].
 63 |      **********************************************************************/
 64 |     LocalCartesian(real lat0, real lon0, real h0 = 0,
 65 |                    const Geocentric& earth = Geocentric::WGS84())
 66 |       : _earth(earth)
 67 |     { Reset(lat0, lon0, h0); }
 68 | 
 69 |     /**
 70 |      * Default constructor.
 71 |      *
 72 |      * @param[in] earth Geocentric object for the transformation; default
 73 |      *   Geocentric::WGS84().
 74 |      *
 75 |      * Sets \e lat0 = 0, \e lon0 = 0, \e h0 = 0.
 76 |      **********************************************************************/
 77 |     explicit LocalCartesian(const Geocentric& earth = Geocentric::WGS84())
 78 |       : _earth(earth)
 79 |     { Reset(real(0), real(0), real(0)); }
 80 | 
 81 |     /**
 82 |      * Reset the origin.
 83 |      *
 84 |      * @param[in] lat0 latitude at origin (degrees).
 85 |      * @param[in] lon0 longitude at origin (degrees).
 86 |      * @param[in] h0 height above ellipsoid at origin (meters); default 0.
 87 |      *
 88 |      * \e lat0 should be in the range [&minus;90&deg;, 90&deg;].
 89 |      **********************************************************************/
 90 |     void Reset(real lat0, real lon0, real h0 = 0);
 91 | 
 92 |     /**
 93 |      * Convert from geodetic to local cartesian coordinates.
 94 |      *
 95 |      * @param[in] lat latitude of point (degrees).
 96 |      * @param[in] lon longitude of point (degrees).
 97 |      * @param[in] h height of point above the ellipsoid (meters).
 98 |      * @param[out] x local cartesian coordinate (meters).
 99 |      * @param[out] y local cartesian coordinate (meters).
100 |      * @param[out] z local cartesian coordinate (meters).
101 |      *
102 |      * \e lat should be in the range [&minus;90&deg;, 90&deg;].
103 |      **********************************************************************/
104 |     void Forward(real lat, real lon, real h, real& x, real& y, real& z)
105 |       const {
106 |       IntForward(lat, lon, h, x, y, z, NULL);
107 |     }
108 | 
109 |     /**
110 |      * Convert from geodetic to local cartesian coordinates and return rotation
111 |      * matrix.
112 |      *
113 |      * @param[in] lat latitude of point (degrees).
114 |      * @param[in] lon longitude of point (degrees).
115 |      * @param[in] h height of point above the ellipsoid (meters).
116 |      * @param[out] x local cartesian coordinate (meters).
117 |      * @param[out] y local cartesian coordinate (meters).
118 |      * @param[out] z local cartesian coordinate (meters).
119 |      * @param[out] M if the length of the vector is 9, fill with the rotation
120 |      *   matrix in row-major order.
121 |      *
122 |      * \e lat should be in the range [&minus;90&deg;, 90&deg;].
123 |      *
124 |      * Let \e v be a unit vector located at (\e lat, \e lon, \e h).  We can
125 |      * express \e v as \e column vectors in one of two ways
126 |      * - in east, north, up coordinates (where the components are relative to a
127 |      *   local coordinate system at (\e lat, \e lon, \e h)); call this
128 |      *   representation \e v1.
129 |      * - in \e x, \e y, \e z coordinates (where the components are relative to
130 |      *   the local coordinate system at (\e lat0, \e lon0, \e h0)); call this
131 |      *   representation \e v0.
132 |      * .
133 |      * Then we have \e v0 = \e M &sdot; \e v1.
134 |      **********************************************************************/
135 |     void Forward(real lat, real lon, real h, real& x, real& y, real& z,
136 |                  std::vector<real>& M)
137 |       const  {
138 |       if (M.end() == M.begin() + dim2_) {
139 |         real t[dim2_];
140 |         IntForward(lat, lon, h, x, y, z, t);
141 |         std::copy(t, t + dim2_, M.begin());
142 |       } else
143 |         IntForward(lat, lon, h, x, y, z, NULL);
144 |     }
145 | 
146 |     /**
147 |      * Convert from local cartesian to geodetic coordinates.
148 |      *
149 |      * @param[in] x local cartesian coordinate (meters).
150 |      * @param[in] y local cartesian coordinate (meters).
151 |      * @param[in] z local cartesian coordinate (meters).
152 |      * @param[out] lat latitude of point (degrees).
153 |      * @param[out] lon longitude of point (degrees).
154 |      * @param[out] h height of point above the ellipsoid (meters).
155 |      *
156 |      * The value of \e lon returned is in the range [&minus;180&deg;,
157 |      * 180&deg;].
158 |      **********************************************************************/
159 |     void Reverse(real x, real y, real z, real& lat, real& lon, real& h)
160 |       const {
161 |       IntReverse(x, y, z, lat, lon, h, NULL);
162 |     }
163 | 
164 |     /**
165 |      * Convert from local cartesian to geodetic coordinates and return rotation
166 |      * matrix.
167 |      *
168 |      * @param[in] x local cartesian coordinate (meters).
169 |      * @param[in] y local cartesian coordinate (meters).
170 |      * @param[in] z local cartesian coordinate (meters).
171 |      * @param[out] lat latitude of point (degrees).
172 |      * @param[out] lon longitude of point (degrees).
173 |      * @param[out] h height of point above the ellipsoid (meters).
174 |      * @param[out] M if the length of the vector is 9, fill with the rotation
175 |      *   matrix in row-major order.
176 |      *
177 |      * Let \e v be a unit vector located at (\e lat, \e lon, \e h).  We can
178 |      * express \e v as \e column vectors in one of two ways
179 |      * - in east, north, up coordinates (where the components are relative to a
180 |      *   local coordinate system at (\e lat, \e lon, \e h)); call this
181 |      *   representation \e v1.
182 |      * - in \e x, \e y, \e z coordinates (where the components are relative to
183 |      *   the local coordinate system at (\e lat0, \e lon0, \e h0)); call this
184 |      *   representation \e v0.
185 |      * .
186 |      * Then we have \e v1 = <i>M</i><sup>T</sup> &sdot; \e v0, where
187 |      * <i>M</i><sup>T</sup> is the transpose of \e M.
188 |      **********************************************************************/
189 |     void Reverse(real x, real y, real z, real& lat, real& lon, real& h,
190 |                  std::vector<real>& M)
191 |       const {
192 |       if (M.end() == M.begin() + dim2_) {
193 |         real t[dim2_];
194 |         IntReverse(x, y, z, lat, lon, h, t);
195 |         std::copy(t, t + dim2_, M.begin());
196 |       } else
197 |         IntReverse(x, y, z, lat, lon, h, NULL);
198 |     }
199 | 
200 |     /** \name Inspector functions
201 |      **********************************************************************/
202 |     ///@{
203 |     /**
204 |      * @return latitude of the origin (degrees).
205 |      **********************************************************************/
206 |     Math::real LatitudeOrigin() const { return _lat0; }
207 | 
208 |     /**
209 |      * @return longitude of the origin (degrees).
210 |      **********************************************************************/
211 |     Math::real LongitudeOrigin() const { return _lon0; }
212 | 
213 |     /**
214 |      * @return height of the origin (meters).
215 |      **********************************************************************/
216 |     Math::real HeightOrigin() const { return _h0; }
217 | 
218 |     /**
219 |      * @return \e a the equatorial radius of the ellipsoid (meters).  This is
220 |      *   the value of \e a inherited from the Geocentric object used in the
221 |      *   constructor.
222 |      **********************************************************************/
223 |     Math::real MajorRadius() const { return _earth.MajorRadius(); }
224 | 
225 |     /**
226 |      * @return \e f the flattening of the ellipsoid.  This is the value
227 |      *   inherited from the Geocentric object used in the constructor.
228 |      **********************************************************************/
229 |     Math::real Flattening() const { return _earth.Flattening(); }
230 |     ///@}
231 | 
232 |   };
233 | 
234 | } // namespace GeographicLib
235 | 
236 | #endif  // GEOGRAPHICLIB_LOCALCARTESIAN_HPP
237 | 


--------------------------------------------------------------------------------
/eskf_localizer/third_party/GeographicLib/include/Geocentric.hpp:
--------------------------------------------------------------------------------
  1 | /**
  2 |  * \file Geocentric.hpp
  3 |  * \brief Header for GeographicLib::Geocentric class
  4 |  *
  5 |  * Copyright (c) Charles Karney (2008-2016) <charles@karney.com> and licensed
  6 |  * under the MIT/X11 License.  For more information, see
  7 |  * https://geographiclib.sourceforge.io/
  8 |  **********************************************************************/
  9 | 
 10 | #if !defined(GEOGRAPHICLIB_GEOCENTRIC_HPP)
 11 | #define GEOGRAPHICLIB_GEOCENTRIC_HPP 1
 12 | 
 13 | #include <vector>
 14 | #include "Constants.hpp"
 15 | 
 16 | namespace GeographicLib {
 17 | 
 18 |   /**
 19 |    * \brief %Geocentric coordinates
 20 |    *
 21 |    * Convert between geodetic coordinates latitude = \e lat, longitude = \e
 22 |    * lon, height = \e h (measured vertically from the surface of the ellipsoid)
 23 |    * to geocentric coordinates (\e X, \e Y, \e Z).  The origin of geocentric
 24 |    * coordinates is at the center of the earth.  The \e Z axis goes thru the
 25 |    * north pole, \e lat = 90&deg;.  The \e X axis goes thru \e lat = 0,
 26 |    * \e lon = 0.  %Geocentric coordinates are also known as earth centered,
 27 |    * earth fixed (ECEF) coordinates.
 28 |    *
 29 |    * The conversion from geographic to geocentric coordinates is
 30 |    * straightforward.  For the reverse transformation we use
 31 |    * - H. Vermeille,
 32 |    *   <a href="https://doi.org/10.1007/s00190-002-0273-6"> Direct
 33 |    *   transformation from geocentric coordinates to geodetic coordinates</a>,
 34 |    *   J. Geodesy 76, 451--454 (2002).
 35 |    * .
 36 |    * Several changes have been made to ensure that the method returns accurate
 37 |    * results for all finite inputs (even if \e h is infinite).  The changes are
 38 |    * described in Appendix B of
 39 |    * - C. F. F. Karney,
 40 |    *   <a href="https://arxiv.org/abs/1102.1215v1">Geodesics
 41 |    *   on an ellipsoid of revolution</a>,
 42 |    *   Feb. 2011;
 43 |    *   preprint
 44 |    *   <a href="https://arxiv.org/abs/1102.1215v1">arxiv:1102.1215v1</a>.
 45 |    * .
 46 |    * Vermeille similarly updated his method in
 47 |    * - H. Vermeille,
 48 |    *   <a href="https://doi.org/10.1007/s00190-010-0419-x">
 49 |    *   An analytical method to transform geocentric into
 50 |    *   geodetic coordinates</a>, J. Geodesy 85, 105--117 (2011).
 51 |    * .
 52 |    * See \ref geocentric for more information.
 53 |    *
 54 |    * The errors in these routines are close to round-off.  Specifically, for
 55 |    * points within 5000 km of the surface of the ellipsoid (either inside or
 56 |    * outside the ellipsoid), the error is bounded by 7 nm (7 nanometers) for
 57 |    * the WGS84 ellipsoid.  See \ref geocentric for further information on the
 58 |    * errors.
 59 |    *
 60 |    * Example of use:
 61 |    * \include example-Geocentric.cpp
 62 |    *
 63 |    * <a href="CartConvert.1.html">CartConvert</a> is a command-line utility
 64 |    * providing access to the functionality of Geocentric and LocalCartesian.
 65 |    **********************************************************************/
 66 | 
 67 |   class GEOGRAPHICLIB_EXPORT Geocentric {
 68 |   private:
 69 |     typedef Math::real real;
 70 |     friend class LocalCartesian;
 71 |     friend class MagneticCircle; // MagneticCircle uses Rotation
 72 |     friend class MagneticModel;  // MagneticModel uses IntForward
 73 |     friend class GravityCircle;  // GravityCircle uses Rotation
 74 |     friend class GravityModel;   // GravityModel uses IntForward
 75 |     friend class NormalGravity;  // NormalGravity uses IntForward
 76 |     static const size_t dim_ = 3;
 77 |     static const size_t dim2_ = dim_ * dim_;
 78 |     real _a, _f, _e2, _e2m, _e2a, _e4a, _maxrad;
 79 |     static void Rotation(real sphi, real cphi, real slam, real clam,
 80 |                          real M[dim2_]);
 81 |     static void Rotate(real M[dim2_], real x, real y, real z,
 82 |                        real& X, real& Y, real& Z) {
 83 |       // Perform [X,Y,Z]^t = M.[x,y,z]^t
 84 |       // (typically local cartesian to geocentric)
 85 |       X = M[0] * x + M[1] * y + M[2] * z;
 86 |       Y = M[3] * x + M[4] * y + M[5] * z;
 87 |       Z = M[6] * x + M[7] * y + M[8] * z;
 88 |     }
 89 |     static void Unrotate(real M[dim2_], real X, real Y, real Z,
 90 |                          real& x, real& y, real& z)  {
 91 |       // Perform [x,y,z]^t = M^t.[X,Y,Z]^t
 92 |       // (typically geocentric to local cartesian)
 93 |       x = M[0] * X + M[3] * Y + M[6] * Z;
 94 |       y = M[1] * X + M[4] * Y + M[7] * Z;
 95 |       z = M[2] * X + M[5] * Y + M[8] * Z;
 96 |     }
 97 |     void IntForward(real lat, real lon, real h, real& X, real& Y, real& Z,
 98 |                     real M[dim2_]) const;
 99 |     void IntReverse(real X, real Y, real Z, real& lat, real& lon, real& h,
100 |                     real M[dim2_]) const;
101 | 
102 |   public:
103 | 
104 |     /**
105 |      * Constructor for a ellipsoid with
106 |      *
107 |      * @param[in] a equatorial radius (meters).
108 |      * @param[in] f flattening of ellipsoid.  Setting \e f = 0 gives a sphere.
109 |      *   Negative \e f gives a prolate ellipsoid.
110 |      * @exception GeographicErr if \e a or (1 &minus; \e f) \e a is not
111 |      *   positive.
112 |      **********************************************************************/
113 |     Geocentric(real a, real f);
114 | 
115 |     /**
116 |      * A default constructor (for use by NormalGravity).
117 |      **********************************************************************/
118 |     Geocentric() : _a(-1) {}
119 | 
120 |     /**
121 |      * Convert from geodetic to geocentric coordinates.
122 |      *
123 |      * @param[in] lat latitude of point (degrees).
124 |      * @param[in] lon longitude of point (degrees).
125 |      * @param[in] h height of point above the ellipsoid (meters).
126 |      * @param[out] X geocentric coordinate (meters).
127 |      * @param[out] Y geocentric coordinate (meters).
128 |      * @param[out] Z geocentric coordinate (meters).
129 |      *
130 |      * \e lat should be in the range [&minus;90&deg;, 90&deg;].
131 |      **********************************************************************/
132 |     void Forward(real lat, real lon, real h, real& X, real& Y, real& Z)
133 |       const {
134 |       if (Init())
135 |         IntForward(lat, lon, h, X, Y, Z, NULL);
136 |     }
137 | 
138 |     /**
139 |      * Convert from geodetic to geocentric coordinates and return rotation
140 |      * matrix.
141 |      *
142 |      * @param[in] lat latitude of point (degrees).
143 |      * @param[in] lon longitude of point (degrees).
144 |      * @param[in] h height of point above the ellipsoid (meters).
145 |      * @param[out] X geocentric coordinate (meters).
146 |      * @param[out] Y geocentric coordinate (meters).
147 |      * @param[out] Z geocentric coordinate (meters).
148 |      * @param[out] M if the length of the vector is 9, fill with the rotation
149 |      *   matrix in row-major order.
150 |      *
151 |      * Let \e v be a unit vector located at (\e lat, \e lon, \e h).  We can
152 |      * express \e v as \e column vectors in one of two ways
153 |      * - in east, north, up coordinates (where the components are relative to a
154 |      *   local coordinate system at (\e lat, \e lon, \e h)); call this
155 |      *   representation \e v1.
156 |      * - in geocentric \e X, \e Y, \e Z coordinates; call this representation
157 |      *   \e v0.
158 |      * .
159 |      * Then we have \e v0 = \e M &sdot; \e v1.
160 |      **********************************************************************/
161 |     void Forward(real lat, real lon, real h, real& X, real& Y, real& Z,
162 |                  std::vector<real>& M)
163 |       const {
164 |       if (!Init())
165 |         return;
166 |       if (M.end() == M.begin() + dim2_) {
167 |         real t[dim2_];
168 |         IntForward(lat, lon, h, X, Y, Z, t);
169 |         std::copy(t, t + dim2_, M.begin());
170 |       } else
171 |         IntForward(lat, lon, h, X, Y, Z, NULL);
172 |     }
173 | 
174 |     /**
175 |      * Convert from geocentric to geodetic to coordinates.
176 |      *
177 |      * @param[in] X geocentric coordinate (meters).
178 |      * @param[in] Y geocentric coordinate (meters).
179 |      * @param[in] Z geocentric coordinate (meters).
180 |      * @param[out] lat latitude of point (degrees).
181 |      * @param[out] lon longitude of point (degrees).
182 |      * @param[out] h height of point above the ellipsoid (meters).
183 |      *
184 |      * In general there are multiple solutions and the result which maximizes
185 |      * \e h is returned.  If there are still multiple solutions with different
186 |      * latitudes (applies only if \e Z = 0), then the solution with \e lat > 0
187 |      * is returned.  If there are still multiple solutions with different
188 |      * longitudes (applies only if \e X = \e Y = 0) then \e lon = 0 is
189 |      * returned.  The value of \e h returned satisfies \e h &ge; &minus; \e a
190 |      * (1 &minus; <i>e</i><sup>2</sup>) / sqrt(1 &minus; <i>e</i><sup>2</sup>
191 |      * sin<sup>2</sup>\e lat).  The value of \e lon returned is in the range
192 |      * [&minus;180&deg;, 180&deg;].
193 |      **********************************************************************/
194 |     void Reverse(real X, real Y, real Z, real& lat, real& lon, real& h)
195 |       const {
196 |       if (Init())
197 |         IntReverse(X, Y, Z, lat, lon, h, NULL);
198 |     }
199 | 
200 |     /**
201 |      * Convert from geocentric to geodetic to coordinates.
202 |      *
203 |      * @param[in] X geocentric coordinate (meters).
204 |      * @param[in] Y geocentric coordinate (meters).
205 |      * @param[in] Z geocentric coordinate (meters).
206 |      * @param[out] lat latitude of point (degrees).
207 |      * @param[out] lon longitude of point (degrees).
208 |      * @param[out] h height of point above the ellipsoid (meters).
209 |      * @param[out] M if the length of the vector is 9, fill with the rotation
210 |      *   matrix in row-major order.
211 |      *
212 |      * Let \e v be a unit vector located at (\e lat, \e lon, \e h).  We can
213 |      * express \e v as \e column vectors in one of two ways
214 |      * - in east, north, up coordinates (where the components are relative to a
215 |      *   local coordinate system at (\e lat, \e lon, \e h)); call this
216 |      *   representation \e v1.
217 |      * - in geocentric \e X, \e Y, \e Z coordinates; call this representation
218 |      *   \e v0.
219 |      * .
220 |      * Then we have \e v1 = <i>M</i><sup>T</sup> &sdot; \e v0, where
221 |      * <i>M</i><sup>T</sup> is the transpose of \e M.
222 |      **********************************************************************/
223 |     void Reverse(real X, real Y, real Z, real& lat, real& lon, real& h,
224 |                  std::vector<real>& M)
225 |       const {
226 |       if (!Init())
227 |         return;
228 |       if (M.end() == M.begin() + dim2_) {
229 |         real t[dim2_];
230 |         IntReverse(X, Y, Z, lat, lon, h, t);
231 |         std::copy(t, t + dim2_, M.begin());
232 |       } else
233 |         IntReverse(X, Y, Z, lat, lon, h, NULL);
234 |     }
235 | 
236 |     /** \name Inspector functions
237 |      **********************************************************************/
238 |     ///@{
239 |     /**
240 |      * @return true if the object has been initialized.
241 |      **********************************************************************/
242 |     bool Init() const { return _a > 0; }
243 |     /**
244 |      * @return \e a the equatorial radius of the ellipsoid (meters).  This is
245 |      *   the value used in the constructor.
246 |      **********************************************************************/
247 |     Math::real MajorRadius() const
248 |     { return Init() ? _a : Math::NaN(); }
249 | 
250 |     /**
251 |      * @return \e f the  flattening of the ellipsoid.  This is the
252 |      *   value used in the constructor.
253 |      **********************************************************************/
254 |     Math::real Flattening() const
255 |     { return Init() ? _f : Math::NaN(); }
256 |     ///@}
257 | 
258 |     /**
259 |      * A global instantiation of Geocentric with the parameters for the WGS84
260 |      * ellipsoid.
261 |      **********************************************************************/
262 |     static const Geocentric& WGS84();
263 |   };
264 | 
265 | } // namespace GeographicLib
266 | 
267 | #endif  // GEOGRAPHICLIB_GEOCENTRIC_HPP
268 | 


--------------------------------------------------------------------------------
/eskf_localizer/third_party/GeographicLib/include/Constants.hpp:
--------------------------------------------------------------------------------
  1 | /**
  2 |  * \file Constants.hpp
  3 |  * \brief Header for GeographicLib::Constants class
  4 |  *
  5 |  * Copyright (c) Charles Karney (2008-2016) <charles@karney.com> and licensed
  6 |  * under the MIT/X11 License.  For more information, see
  7 |  * https://geographiclib.sourceforge.io/
  8 |  **********************************************************************/
  9 | 
 10 | #if !defined(GEOGRAPHICLIB_CONSTANTS_HPP)
 11 | #define GEOGRAPHICLIB_CONSTANTS_HPP 1
 12 | 
 13 | #include "Config.h"
 14 | 
 15 | /**
 16 |  * @relates GeographicLib::Constants
 17 |  * Pack the version components into a single integer.  Users should not rely on
 18 |  * this particular packing of the components of the version number; see the
 19 |  * documentation for GEOGRAPHICLIB_VERSION, below.
 20 |  **********************************************************************/
 21 | #define GEOGRAPHICLIB_VERSION_NUM(a,b,c) ((((a) * 10000 + (b)) * 100) + (c))
 22 | 
 23 | /**
 24 |  * @relates GeographicLib::Constants
 25 |  * The version of GeographicLib as a single integer, packed as MMmmmmpp where
 26 |  * MM is the major version, mmmm is the minor version, and pp is the patch
 27 |  * level.  Users should not rely on this particular packing of the components
 28 |  * of the version number.  Instead they should use a test such as \code
 29 |    #if GEOGRAPHICLIB_VERSION >= GEOGRAPHICLIB_VERSION_NUM(1,37,0)
 30 |    ...
 31 |    #endif
 32 |  * \endcode
 33 |  **********************************************************************/
 34 | #define GEOGRAPHICLIB_VERSION \
 35 |  GEOGRAPHICLIB_VERSION_NUM(GEOGRAPHICLIB_VERSION_MAJOR, \
 36 |                            GEOGRAPHICLIB_VERSION_MINOR, \
 37 |                            GEOGRAPHICLIB_VERSION_PATCH)
 38 | 
 39 | /**
 40 |  * @relates GeographicLib::Constants
 41 |  * Is the C++11 static_assert available?
 42 |  **********************************************************************/
 43 | #if !defined(GEOGRAPHICLIB_HAS_STATIC_ASSERT)
 44 | #  if __cplusplus >= 201103 || defined(__GXX_EXPERIMENTAL_CXX0X__)
 45 | #    define GEOGRAPHICLIB_HAS_STATIC_ASSERT 1
 46 | #  elif defined(_MSC_VER) && _MSC_VER >= 1600
 47 | // For reference, here is a table of Visual Studio and _MSC_VER
 48 | // correspondences:
 49 | //
 50 | // _MSC_VER  Visual Studio
 51 | //   1100     vc5
 52 | //   1200     vc6
 53 | //   1300     vc7
 54 | //   1310     vc7.1 (2003)
 55 | //   1400     vc8   (2005)
 56 | //   1500     vc9   (2008)
 57 | //   1600     vc10  (2010)
 58 | //   1700     vc11  (2012)
 59 | //   1800     vc12  (2013)
 60 | //   1900     vc14  (2015)
 61 | //   1910+    vc15  (2017)
 62 | #    define GEOGRAPHICLIB_HAS_STATIC_ASSERT 1
 63 | #  else
 64 | #    define GEOGRAPHICLIB_HAS_STATIC_ASSERT 0
 65 | #  endif
 66 | #endif
 67 | 
 68 | /**
 69 |  * @relates GeographicLib::Constants
 70 |  * A compile-time assert.  Use C++11 static_assert, if available.
 71 |  **********************************************************************/
 72 | #if !defined(GEOGRAPHICLIB_STATIC_ASSERT)
 73 | #  if GEOGRAPHICLIB_HAS_STATIC_ASSERT
 74 | #    define GEOGRAPHICLIB_STATIC_ASSERT static_assert
 75 | #  else
 76 | #    define GEOGRAPHICLIB_STATIC_ASSERT(cond,reason) \
 77 |             { enum{ GEOGRAPHICLIB_STATIC_ASSERT_ENUM = 1/int(cond) }; }
 78 | #  endif
 79 | #endif
 80 | 
 81 | #if defined(_MSC_VER) && defined(GEOGRAPHICLIB_SHARED_LIB) && \
 82 |   GEOGRAPHICLIB_SHARED_LIB
 83 | #  if GEOGRAPHICLIB_SHARED_LIB > 1
 84 | #    error GEOGRAPHICLIB_SHARED_LIB must be 0 or 1
 85 | #  elif defined(GeographicLib_EXPORTS)
 86 | #    define GEOGRAPHICLIB_EXPORT __declspec(dllexport)
 87 | #  else
 88 | #    define GEOGRAPHICLIB_EXPORT __declspec(dllimport)
 89 | #  endif
 90 | #else
 91 | #  define GEOGRAPHICLIB_EXPORT
 92 | #endif
 93 | 
 94 | // Use GEOGRAPHICLIB_DEPRECATED to mark functions, types or variables as
 95 | // deprecated.  Code inspired by Apache Subversion's svn_types.h file (via
 96 | // MPFR).
 97 | #if defined(__GNUC__)
 98 | #  if __GNUC__ > 4
 99 | #    define GEOGRAPHICLIB_DEPRECATED(msg) __attribute__((deprecated(msg)))
100 | #  else
101 | #    define GEOGRAPHICLIB_DEPRECATED(msg) __attribute__((deprecated))
102 | #  endif
103 | #elif defined(_MSC_VER) && _MSC_VER >= 1300
104 | #  define GEOGRAPHICLIB_DEPRECATED(msg) __declspec(deprecated(msg))
105 | #else
106 | #  define GEOGRAPHICLIB_DEPRECATED(msg)
107 | #endif
108 | 
109 | #include <stdexcept>
110 | #include <string>
111 | #include "Math.hpp"
112 | 
113 | /**
114 |  * \brief Namespace for %GeographicLib
115 |  *
116 |  * All of %GeographicLib is defined within the GeographicLib namespace.  In
117 |  * addition all the header files are included via %GeographicLib/Class.hpp.
118 |  * This minimizes the likelihood of conflicts with other packages.
119 |  **********************************************************************/
120 | namespace GeographicLib {
121 | 
122 |   /**
123 |    * \brief %Constants needed by %GeographicLib
124 |    *
125 |    * Define constants specifying the WGS84 ellipsoid, the UTM and UPS
126 |    * projections, and various unit conversions.
127 |    *
128 |    * Example of use:
129 |    * \include example-Constants.cpp
130 |    **********************************************************************/
131 |   class GEOGRAPHICLIB_EXPORT Constants {
132 |   private:
133 |     typedef Math::real real;
134 |     Constants();                // Disable constructor
135 | 
136 |   public:
137 |     /**
138 |      * A synonym for Math::degree<real>().
139 |      **********************************************************************/
140 |     static Math::real degree() { return Math::degree(); }
141 |     /**
142 |      * @return the number of radians in an arcminute.
143 |      **********************************************************************/
144 |     static Math::real arcminute()
145 |     { return Math::degree() / 60; }
146 |     /**
147 |      * @return the number of radians in an arcsecond.
148 |      **********************************************************************/
149 |     static Math::real arcsecond()
150 |     { return Math::degree() / 3600; }
151 | 
152 |     /** \name Ellipsoid parameters
153 |      **********************************************************************/
154 |     ///@{
155 |     /**
156 |      * @tparam T the type of the returned value.
157 |      * @return the equatorial radius of WGS84 ellipsoid (6378137 m).
158 |      **********************************************************************/
159 |     template<typename T> static T WGS84_a()
160 |     { return 6378137 * meter<T>(); }
161 |     /**
162 |      * A synonym for WGS84_a<real>().
163 |      **********************************************************************/
164 |     static Math::real WGS84_a() { return WGS84_a<real>(); }
165 |     /**
166 |      * @tparam T the type of the returned value.
167 |      * @return the flattening of WGS84 ellipsoid (1/298.257223563).
168 |      **********************************************************************/
169 |     template<typename T> static T WGS84_f() {
170 |       // Evaluating this as 1000000000 / T(298257223563LL) reduces the
171 |       // round-off error by about 10%.  However, expressing the flattening as
172 |       // 1/298.257223563 is well ingrained.
173 |       return 1 / ( T(298257223563LL) / 1000000000 );
174 |     }
175 |     /**
176 |      * A synonym for WGS84_f<real>().
177 |      **********************************************************************/
178 |     static Math::real WGS84_f() { return WGS84_f<real>(); }
179 |     /**
180 |      * @tparam T the type of the returned value.
181 |      * @return the gravitational constant of the WGS84 ellipsoid, \e GM, in
182 |      *   m<sup>3</sup> s<sup>&minus;2</sup>.
183 |      **********************************************************************/
184 |     template<typename T> static T WGS84_GM()
185 |     { return T(3986004) * 100000000 + 41800000; }
186 |     /**
187 |      * A synonym for WGS84_GM<real>().
188 |      **********************************************************************/
189 |     static Math::real WGS84_GM() { return WGS84_GM<real>(); }
190 |     /**
191 |      * @tparam T the type of the returned value.
192 |      * @return the angular velocity of the WGS84 ellipsoid, &omega;, in rad
193 |      *   s<sup>&minus;1</sup>.
194 |      **********************************************************************/
195 |     template<typename T> static T WGS84_omega()
196 |     { return 7292115 / (T(1000000) * 100000); }
197 |     /**
198 |      * A synonym for WGS84_omega<real>().
199 |      **********************************************************************/
200 |     static Math::real WGS84_omega() { return WGS84_omega<real>(); }
201 |     /**
202 |      * @tparam T the type of the returned value.
203 |      * @return the equatorial radius of GRS80 ellipsoid, \e a, in m.
204 |      **********************************************************************/
205 |     template<typename T> static T GRS80_a()
206 |     { return 6378137 * meter<T>(); }
207 |     /**
208 |      * A synonym for GRS80_a<real>().
209 |      **********************************************************************/
210 |     static Math::real GRS80_a() { return GRS80_a<real>(); }
211 |     /**
212 |      * @tparam T the type of the returned value.
213 |      * @return the gravitational constant of the GRS80 ellipsoid, \e GM, in
214 |      *   m<sup>3</sup> s<sup>&minus;2</sup>.
215 |      **********************************************************************/
216 |     template<typename T> static T GRS80_GM()
217 |     { return T(3986005) * 100000000; }
218 |     /**
219 |      * A synonym for GRS80_GM<real>().
220 |      **********************************************************************/
221 |     static Math::real GRS80_GM() { return GRS80_GM<real>(); }
222 |     /**
223 |      * @tparam T the type of the returned value.
224 |      * @return the angular velocity of the GRS80 ellipsoid, &omega;, in rad
225 |      *   s<sup>&minus;1</sup>.
226 |      *
227 |      * This is about 2 &pi; 366.25 / (365.25 &times; 24 &times; 3600) rad
228 |      * s<sup>&minus;1</sup>.  365.25 is the number of days in a Julian year and
229 |      * 365.35/366.25 converts from solar days to sidereal days.  Using the
230 |      * number of days in a Gregorian year (365.2425) results in a worse
231 |      * approximation (because the Gregorian year includes the precession of the
232 |      * earth's axis).
233 |      **********************************************************************/
234 |     template<typename T> static T GRS80_omega()
235 |     { return 7292115 / (T(1000000) * 100000); }
236 |     /**
237 |      * A synonym for GRS80_omega<real>().
238 |      **********************************************************************/
239 |     static Math::real GRS80_omega() { return GRS80_omega<real>(); }
240 |     /**
241 |      * @tparam T the type of the returned value.
242 |      * @return the dynamical form factor of the GRS80 ellipsoid,
243 |      *   <i>J</i><sub>2</sub>.
244 |      **********************************************************************/
245 |     template<typename T> static T GRS80_J2()
246 |     { return T(108263) / 100000000; }
247 |     /**
248 |      * A synonym for GRS80_J2<real>().
249 |      **********************************************************************/
250 |     static Math::real GRS80_J2() { return GRS80_J2<real>(); }
251 |     /**
252 |      * @tparam T the type of the returned value.
253 |      * @return the central scale factor for UTM (0.9996).
254 |      **********************************************************************/
255 |     template<typename T> static T UTM_k0()
256 |     {return T(9996) / 10000; }
257 |     /**
258 |      * A synonym for UTM_k0<real>().
259 |      **********************************************************************/
260 |     static Math::real UTM_k0() { return UTM_k0<real>(); }
261 |     /**
262 |      * @tparam T the type of the returned value.
263 |      * @return the central scale factor for UPS (0.994).
264 |      **********************************************************************/
265 |     template<typename T> static T UPS_k0()
266 |     { return T(994) / 1000; }
267 |     /**
268 |      * A synonym for UPS_k0<real>().
269 |      **********************************************************************/
270 |     static Math::real UPS_k0() { return UPS_k0<real>(); }
271 |     ///@}
272 | 
273 |     /** \name SI units
274 |      **********************************************************************/
275 |     ///@{
276 |     /**
277 |      * @tparam T the type of the returned value.
278 |      * @return the number of meters in a meter.
279 |      *
280 |      * This is unity, but this lets the internal system of units be changed if
281 |      * necessary.
282 |      **********************************************************************/
283 |     template<typename T> static T meter() { return T(1); }
284 |     /**
285 |      * A synonym for meter<real>().
286 |      **********************************************************************/
287 |     static Math::real meter() { return meter<real>(); }
288 |     /**
289 |      * @return the number of meters in a kilometer.
290 |      **********************************************************************/
291 |     static Math::real kilometer()
292 |     { return 1000 * meter<real>(); }
293 |     /**
294 |      * @return the number of meters in a nautical mile (approximately 1 arc
295 |      *   minute)
296 |      **********************************************************************/
297 |     static Math::real nauticalmile()
298 |     { return 1852 * meter<real>(); }
299 | 
300 |     /**
301 |      * @tparam T the type of the returned value.
302 |      * @return the number of square meters in a square meter.
303 |      *
304 |      * This is unity, but this lets the internal system of units be changed if
305 |      * necessary.
306 |      **********************************************************************/
307 |     template<typename T> static T square_meter()
308 |     { return meter<real>() * meter<real>(); }
309 |     /**
310 |      * A synonym for square_meter<real>().
311 |      **********************************************************************/
312 |     static Math::real square_meter()
313 |     { return square_meter<real>(); }
314 |     /**
315 |      * @return the number of square meters in a hectare.
316 |      **********************************************************************/
317 |     static Math::real hectare()
318 |     { return 10000 * square_meter<real>(); }
319 |     /**
320 |      * @return the number of square meters in a square kilometer.
321 |      **********************************************************************/
322 |     static Math::real square_kilometer()
323 |     { return kilometer() * kilometer(); }
324 |     /**
325 |      * @return the number of square meters in a square nautical mile.
326 |      **********************************************************************/
327 |     static Math::real square_nauticalmile()
328 |     { return nauticalmile() * nauticalmile(); }
329 |     ///@}
330 | 
331 |     /** \name Anachronistic British units
332 |      **********************************************************************/
333 |     ///@{
334 |     /**
335 |      * @return the number of meters in an international foot.
336 |      **********************************************************************/
337 |     static Math::real foot()
338 |     { return real(254 * 12) / 10000 * meter<real>(); }
339 |     /**
340 |      * @return the number of meters in a yard.
341 |      **********************************************************************/
342 |     static Math::real yard() { return 3 * foot(); }
343 |     /**
344 |      * @return the number of meters in a fathom.
345 |      **********************************************************************/
346 |     static Math::real fathom() { return 2 * yard(); }
347 |     /**
348 |      * @return the number of meters in a chain.
349 |      **********************************************************************/
350 |     static Math::real chain() { return 22 * yard(); }
351 |     /**
352 |      * @return the number of meters in a furlong.
353 |      **********************************************************************/
354 |     static Math::real furlong() { return 10 * chain(); }
355 |     /**
356 |      * @return the number of meters in a statute mile.
357 |      **********************************************************************/
358 |     static Math::real mile() { return 8 * furlong(); }
359 |     /**
360 |      * @return the number of square meters in an acre.
361 |      **********************************************************************/
362 |     static Math::real acre() { return chain() * furlong(); }
363 |     /**
364 |      * @return the number of square meters in a square statute mile.
365 |      **********************************************************************/
366 |     static Math::real square_mile() { return mile() * mile(); }
367 |     ///@}
368 | 
369 |     /** \name Anachronistic US units
370 |      **********************************************************************/
371 |     ///@{
372 |     /**
373 |      * @return the number of meters in a US survey foot.
374 |      **********************************************************************/
375 |     static Math::real surveyfoot()
376 |     { return real(1200) / 3937 * meter<real>(); }
377 |     ///@}
378 |   };
379 | 
380 |   /**
381 |    * \brief Exception handling for %GeographicLib
382 |    *
383 |    * A class to handle exceptions.  It's derived from std::runtime_error so it
384 |    * can be caught by the usual catch clauses.
385 |    *
386 |    * Example of use:
387 |    * \include example-GeographicErr.cpp
388 |    **********************************************************************/
389 |   class GeographicErr : public std::runtime_error {
390 |   public:
391 | 
392 |     /**
393 |      * Constructor
394 |      *
395 |      * @param[in] msg a string message, which is accessible in the catch
396 |      *   clause via what().
397 |      **********************************************************************/
398 |     GeographicErr(const std::string& msg) : std::runtime_error(msg) {}
399 |   };
400 | 
401 | } // namespace GeographicLib
402 | 
403 | #endif  // GEOGRAPHICLIB_CONSTANTS_HPP
404 | 


--------------------------------------------------------------------------------
/eskf_localizer/third_party/GeographicLib/include/Math.hpp:
--------------------------------------------------------------------------------
  1 | /**
  2 |  * \file Math.hpp
  3 |  * \brief Header for GeographicLib::Math class
  4 |  *
  5 |  * Copyright (c) Charles Karney (2008-2017) <charles@karney.com> and licensed
  6 |  * under the MIT/X11 License.  For more information, see
  7 |  * https://geographiclib.sourceforge.io/
  8 |  **********************************************************************/
  9 | 
 10 | // Constants.hpp includes Math.hpp.  Place this include outside Math.hpp's
 11 | // include guard to enforce this ordering.
 12 | #include "Constants.hpp"
 13 | 
 14 | #if !defined(GEOGRAPHICLIB_MATH_HPP)
 15 | #define GEOGRAPHICLIB_MATH_HPP 1
 16 | 
 17 | /**
 18 |  * Are C++11 math functions available?
 19 |  **********************************************************************/
 20 | #if !defined(GEOGRAPHICLIB_CXX11_MATH)
 21 | // Recent versions of g++ -std=c++11 (4.7 and later?) set __cplusplus to 201103
 22 | // and support the new C++11 mathematical functions, std::atanh, etc.  However
 23 | // the Android toolchain, which uses g++ -std=c++11 (4.8 as of 2014-03-11,
 24 | // according to Pullan Lu), does not support std::atanh.  Android toolchains
 25 | // might define __ANDROID__ or ANDROID; so need to check both.  With OSX the
 26 | // version is GNUC version 4.2 and __cplusplus is set to 201103, so remove the
 27 | // version check on GNUC.
 28 | #  if defined(__GNUC__) && __cplusplus >= 201103 && \
 29 |   !(defined(__ANDROID__) || defined(ANDROID) || defined(__CYGWIN__))
 30 | #    define GEOGRAPHICLIB_CXX11_MATH 1
 31 | // Visual C++ 12 supports these functions
 32 | #  elif defined(_MSC_VER) && _MSC_VER >= 1800
 33 | #    define GEOGRAPHICLIB_CXX11_MATH 1
 34 | #  else
 35 | #    define GEOGRAPHICLIB_CXX11_MATH 0
 36 | #  endif
 37 | #endif
 38 | 
 39 | #if !defined(GEOGRAPHICLIB_WORDS_BIGENDIAN)
 40 | #  define GEOGRAPHICLIB_WORDS_BIGENDIAN 0
 41 | #endif
 42 | 
 43 | #if !defined(GEOGRAPHICLIB_HAVE_LONG_DOUBLE)
 44 | #  define GEOGRAPHICLIB_HAVE_LONG_DOUBLE 0
 45 | #endif
 46 | 
 47 | #if !defined(GEOGRAPHICLIB_PRECISION)
 48 | /**
 49 |  * The precision of floating point numbers used in %GeographicLib.  1 means
 50 |  * float (single precision); 2 (the default) means double; 3 means long double;
 51 |  * 4 is reserved for quadruple precision.  Nearly all the testing has been
 52 |  * carried out with doubles and that's the recommended configuration.  In order
 53 |  * for long double to be used, GEOGRAPHICLIB_HAVE_LONG_DOUBLE needs to be
 54 |  * defined.  Note that with Microsoft Visual Studio, long double is the same as
 55 |  * double.
 56 |  **********************************************************************/
 57 | #  define GEOGRAPHICLIB_PRECISION 2
 58 | #endif
 59 | 
 60 | #include <cmath>
 61 | #include <algorithm>
 62 | #include <limits>
 63 | 
 64 | #if GEOGRAPHICLIB_PRECISION == 4
 65 | #include <boost/version.hpp>
 66 | #if BOOST_VERSION >= 105600
 67 | #include <boost/cstdfloat.hpp>
 68 | #endif
 69 | #include <boost/multiprecision/float128.hpp>
 70 | #include <boost/math/special_functions.hpp>
 71 | __float128 fmaq(__float128, __float128, __float128);
 72 | #elif GEOGRAPHICLIB_PRECISION == 5
 73 | #include <mpreal.h>
 74 | #endif
 75 | 
 76 | #if GEOGRAPHICLIB_PRECISION > 3
 77 | // volatile keyword makes no sense for multiprec types
 78 | #define GEOGRAPHICLIB_VOLATILE
 79 | // Signal a convergence failure with multiprec types by throwing an exception
 80 | // at loop exit.
 81 | #define GEOGRAPHICLIB_PANIC \
 82 |   (throw GeographicLib::GeographicErr("Convergence failure"), false)
 83 | #else
 84 | #define GEOGRAPHICLIB_VOLATILE volatile
 85 | // Ignore convergence failures with standard floating points types by allowing
 86 | // loop to exit cleanly.
 87 | #define GEOGRAPHICLIB_PANIC false
 88 | #endif
 89 | 
 90 | namespace GeographicLib {
 91 | 
 92 |   /**
 93 |    * \brief Mathematical functions needed by %GeographicLib
 94 |    *
 95 |    * Define mathematical functions in order to localize system dependencies and
 96 |    * to provide generic versions of the functions.  In addition define a real
 97 |    * type to be used by %GeographicLib.
 98 |    *
 99 |    * Example of use:
100 |    * \include example-Math.cpp
101 |    **********************************************************************/
102 |   class GEOGRAPHICLIB_EXPORT Math {
103 |   private:
104 |     void dummy() {
105 |       GEOGRAPHICLIB_STATIC_ASSERT(GEOGRAPHICLIB_PRECISION >= 1 &&
106 |                                   GEOGRAPHICLIB_PRECISION <= 5,
107 |                                   "Bad value of precision");
108 |     }
109 |     Math();                     // Disable constructor
110 |   public:
111 | 
112 | #if GEOGRAPHICLIB_HAVE_LONG_DOUBLE
113 |     /**
114 |      * The extended precision type for real numbers, used for some testing.
115 |      * This is long double on computers with this type; otherwise it is double.
116 |      **********************************************************************/
117 |     typedef long double extended;
118 | #else
119 |     typedef double extended;
120 | #endif
121 | 
122 | #if GEOGRAPHICLIB_PRECISION == 2
123 |     /**
124 |      * The real type for %GeographicLib. Nearly all the testing has been done
125 |      * with \e real = double.  However, the algorithms should also work with
126 |      * float and long double (where available).  (<b>CAUTION</b>: reasonable
127 |      * accuracy typically cannot be obtained using floats.)
128 |      **********************************************************************/
129 |     typedef double real;
130 | #elif GEOGRAPHICLIB_PRECISION == 1
131 |     typedef float real;
132 | #elif GEOGRAPHICLIB_PRECISION == 3
133 |     typedef extended real;
134 | #elif GEOGRAPHICLIB_PRECISION == 4
135 |     typedef boost::multiprecision::float128 real;
136 | #elif GEOGRAPHICLIB_PRECISION == 5
137 |     typedef mpfr::mpreal real;
138 | #else
139 |     typedef double real;
140 | #endif
141 | 
142 |     /**
143 |      * @return the number of bits of precision in a real number.
144 |      **********************************************************************/
145 |     static int digits() {
146 | #if GEOGRAPHICLIB_PRECISION != 5
147 |       return std::numeric_limits<real>::digits;
148 | #else
149 |       return std::numeric_limits<real>::digits();
150 | #endif
151 |     }
152 | 
153 |     /**
154 |      * Set the binary precision of a real number.
155 |      *
156 |      * @param[in] ndigits the number of bits of precision.
157 |      * @return the resulting number of bits of precision.
158 |      *
159 |      * This only has an effect when GEOGRAPHICLIB_PRECISION = 5.  See also
160 |      * Utility::set_digits for caveats about when this routine should be
161 |      * called.
162 |      **********************************************************************/
163 |     static int set_digits(int ndigits) {
164 | #if GEOGRAPHICLIB_PRECISION != 5
165 |       (void)ndigits;
166 | #else
167 |       mpfr::mpreal::set_default_prec(ndigits >= 2 ? ndigits : 2);
168 | #endif
169 |       return digits();
170 |     }
171 | 
172 |     /**
173 |      * @return the number of decimal digits of precision in a real number.
174 |      **********************************************************************/
175 |     static int digits10() {
176 | #if GEOGRAPHICLIB_PRECISION != 5
177 |       return std::numeric_limits<real>::digits10;
178 | #else
179 |       return std::numeric_limits<real>::digits10();
180 | #endif
181 |     }
182 | 
183 |     /**
184 |      * Number of additional decimal digits of precision for real relative to
185 |      * double (0 for float).
186 |      **********************************************************************/
187 |     static int extra_digits() {
188 |       return
189 |         digits10() > std::numeric_limits<double>::digits10 ?
190 |         digits10() - std::numeric_limits<double>::digits10 : 0;
191 |     }
192 | 
193 |     /**
194 |      * true if the machine is big-endian.
195 |      **********************************************************************/
196 |     static const bool bigendian = GEOGRAPHICLIB_WORDS_BIGENDIAN;
197 | 
198 |     /**
199 |      * @tparam T the type of the returned value.
200 |      * @return &pi;.
201 |      **********************************************************************/
202 |     template<typename T> static T pi() {
203 |       using std::atan2;
204 |       static const T pi = atan2(T(0), T(-1));
205 |       return pi;
206 |     }
207 |     /**
208 |      * A synonym for pi<real>().
209 |      **********************************************************************/
210 |     static real pi() { return pi<real>(); }
211 | 
212 |     /**
213 |      * @tparam T the type of the returned value.
214 |      * @return the number of radians in a degree.
215 |      **********************************************************************/
216 |     template<typename T> static T degree() {
217 |       static const T degree = pi<T>() / 180;
218 |       return degree;
219 |     }
220 |     /**
221 |      * A synonym for degree<real>().
222 |      **********************************************************************/
223 |     static real degree() { return degree<real>(); }
224 | 
225 |     /**
226 |      * Square a number.
227 |      *
228 |      * @tparam T the type of the argument and the returned value.
229 |      * @param[in] x
230 |      * @return <i>x</i><sup>2</sup>.
231 |      **********************************************************************/
232 |     template<typename T> static T sq(T x)
233 |     { return x * x; }
234 | 
235 |     /**
236 |      * The hypotenuse function avoiding underflow and overflow.
237 |      *
238 |      * @tparam T the type of the arguments and the returned value.
239 |      * @param[in] x
240 |      * @param[in] y
241 |      * @return sqrt(<i>x</i><sup>2</sup> + <i>y</i><sup>2</sup>).
242 |      **********************************************************************/
243 |     template<typename T> static T hypot(T x, T y) {
244 | #if GEOGRAPHICLIB_CXX11_MATH
245 |       using std::hypot; return hypot(x, y);
246 | #else
247 |       using std::abs; using std::sqrt;
248 |       x = abs(x); y = abs(y);
249 |       if (x < y) std::swap(x, y); // Now x >= y >= 0
250 |       y /= (x ? x : 1);
251 |       return x * sqrt(1 + y * y);
252 |       // For an alternative (square-root free) method see
253 |       // C. Moler and D. Morrision (1983) https://doi.org/10.1147/rd.276.0577
254 |       // and A. A. Dubrulle (1983) https://doi.org/10.1147/rd.276.0582
255 | #endif
256 |     }
257 | 
258 |     /**
259 |      * exp(\e x) &minus; 1 accurate near \e x = 0.
260 |      *
261 |      * @tparam T the type of the argument and the returned value.
262 |      * @param[in] x
263 |      * @return exp(\e x) &minus; 1.
264 |      **********************************************************************/
265 |     template<typename T> static T expm1(T x) {
266 | #if GEOGRAPHICLIB_CXX11_MATH
267 |       using std::expm1; return expm1(x);
268 | #else
269 |       using std::exp; using std::abs; using std::log;
270 |       GEOGRAPHICLIB_VOLATILE T
271 |         y = exp(x),
272 |         z = y - 1;
273 |       // The reasoning here is similar to that for log1p.  The expression
274 |       // mathematically reduces to exp(x) - 1, and the factor z/log(y) = (y -
275 |       // 1)/log(y) is a slowly varying quantity near y = 1 and is accurately
276 |       // computed.
277 |       return abs(x) > 1 ? z : (z == 0 ? x : x * z / log(y));
278 | #endif
279 |     }
280 | 
281 |     /**
282 |      * log(1 + \e x) accurate near \e x = 0.
283 |      *
284 |      * @tparam T the type of the argument and the returned value.
285 |      * @param[in] x
286 |      * @return log(1 + \e x).
287 |      **********************************************************************/
288 |     template<typename T> static T log1p(T x) {
289 | #if GEOGRAPHICLIB_CXX11_MATH
290 |       using std::log1p; return log1p(x);
291 | #else
292 |       using std::log;
293 |       GEOGRAPHICLIB_VOLATILE T
294 |         y = 1 + x,
295 |         z = y - 1;
296 |       // Here's the explanation for this magic: y = 1 + z, exactly, and z
297 |       // approx x, thus log(y)/z (which is nearly constant near z = 0) returns
298 |       // a good approximation to the true log(1 + x)/x.  The multiplication x *
299 |       // (log(y)/z) introduces little additional error.
300 |       return z == 0 ? x : x * log(y) / z;
301 | #endif
302 |     }
303 | 
304 |     /**
305 |      * The inverse hyperbolic sine function.
306 |      *
307 |      * @tparam T the type of the argument and the returned value.
308 |      * @param[in] x
309 |      * @return asinh(\e x).
310 |      **********************************************************************/
311 |     template<typename T> static T asinh(T x) {
312 | #if GEOGRAPHICLIB_CXX11_MATH
313 |       using std::asinh; return asinh(x);
314 | #else
315 |       using std::abs; T y = abs(x); // Enforce odd parity
316 |       y = log1p(y * (1 + y/(hypot(T(1), y) + 1)));
317 |       return x < 0 ? -y : y;
318 | #endif
319 |     }
320 | 
321 |     /**
322 |      * The inverse hyperbolic tangent function.
323 |      *
324 |      * @tparam T the type of the argument and the returned value.
325 |      * @param[in] x
326 |      * @return atanh(\e x).
327 |      **********************************************************************/
328 |     template<typename T> static T atanh(T x) {
329 | #if GEOGRAPHICLIB_CXX11_MATH
330 |       using std::atanh; return atanh(x);
331 | #else
332 |       using std::abs; T y = abs(x); // Enforce odd parity
333 |       y = log1p(2 * y/(1 - y))/2;
334 |       return x < 0 ? -y : y;
335 | #endif
336 |     }
337 | 
338 |     /**
339 |      * The cube root function.
340 |      *
341 |      * @tparam T the type of the argument and the returned value.
342 |      * @param[in] x
343 |      * @return the real cube root of \e x.
344 |      **********************************************************************/
345 |     template<typename T> static T cbrt(T x) {
346 | #if GEOGRAPHICLIB_CXX11_MATH
347 |       using std::cbrt; return cbrt(x);
348 | #else
349 |       using std::abs; using std::pow;
350 |       T y = pow(abs(x), 1/T(3)); // Return the real cube root
351 |       return x < 0 ? -y : y;
352 | #endif
353 |     }
354 | 
355 |     /**
356 |      * Fused multiply and add.
357 |      *
358 |      * @tparam T the type of the arguments and the returned value.
359 |      * @param[in] x
360 |      * @param[in] y
361 |      * @param[in] z
362 |      * @return <i>xy</i> + <i>z</i>, correctly rounded (on those platforms with
363 |      *   support for the <code>fma</code> instruction).
364 |      *
365 |      * On platforms without the <code>fma</code> instruction, no attempt is
366 |      * made to improve on the result of a rounded multiplication followed by a
367 |      * rounded addition.
368 |      **********************************************************************/
369 |     template<typename T> static T fma(T x, T y, T z) {
370 | #if GEOGRAPHICLIB_CXX11_MATH
371 |       using std::fma; return fma(x, y, z);
372 | #else
373 |       return x * y + z;
374 | #endif
375 |     }
376 | 
377 |     /**
378 |      * Normalize a two-vector.
379 |      *
380 |      * @tparam T the type of the argument and the returned value.
381 |      * @param[in,out] x on output set to <i>x</i>/hypot(<i>x</i>, <i>y</i>).
382 |      * @param[in,out] y on output set to <i>y</i>/hypot(<i>x</i>, <i>y</i>).
383 |      **********************************************************************/
384 |     template<typename T> static void norm(T& x, T& y)
385 |     { T h = hypot(x, y); x /= h; y /= h; }
386 | 
387 |     /**
388 |      * The error-free sum of two numbers.
389 |      *
390 |      * @tparam T the type of the argument and the returned value.
391 |      * @param[in] u
392 |      * @param[in] v
393 |      * @param[out] t the exact error given by (\e u + \e v) - \e s.
394 |      * @return \e s = round(\e u + \e v).
395 |      *
396 |      * See D. E. Knuth, TAOCP, Vol 2, 4.2.2, Theorem B.  (Note that \e t can be
397 |      * the same as one of the first two arguments.)
398 |      **********************************************************************/
399 |     template<typename T> static T sum(T u, T v, T& t) {
400 |       GEOGRAPHICLIB_VOLATILE T s = u + v;
401 |       GEOGRAPHICLIB_VOLATILE T up = s - v;
402 |       GEOGRAPHICLIB_VOLATILE T vpp = s - up;
403 |       up -= u;
404 |       vpp -= v;
405 |       t = -(up + vpp);
406 |       // u + v =       s      + t
407 |       //       = round(u + v) + t
408 |       return s;
409 |     }
410 | 
411 |     /**
412 |      * Evaluate a polynomial.
413 |      *
414 |      * @tparam T the type of the arguments and returned value.
415 |      * @param[in] N the order of the polynomial.
416 |      * @param[in] p the coefficient array (of size \e N + 1).
417 |      * @param[in] x the variable.
418 |      * @return the value of the polynomial.
419 |      *
420 |      * Evaluate <i>y</i> = &sum;<sub><i>n</i>=0..<i>N</i></sub>
421 |      * <i>p</i><sub><i>n</i></sub> <i>x</i><sup><i>N</i>&minus;<i>n</i></sup>.
422 |      * Return 0 if \e N &lt; 0.  Return <i>p</i><sub>0</sub>, if \e N = 0 (even
423 |      * if \e x is infinite or a nan).  The evaluation uses Horner's method.
424 |      **********************************************************************/
425 |     template<typename T> static T polyval(int N, const T p[], T x)
426 |     // This used to employ Math::fma; but that's too slow and it seemed not to
427 |     // improve the accuracy noticeably.  This might change when there's direct
428 |     // hardware support for fma.
429 |     { T y = N < 0 ? 0 : *p++; while (--N >= 0) y = y * x + *p++; return y; }
430 | 
431 |     /**
432 |      * Normalize an angle.
433 |      *
434 |      * @tparam T the type of the argument and returned value.
435 |      * @param[in] x the angle in degrees.
436 |      * @return the angle reduced to the range([&minus;180&deg;, 180&deg;].
437 |      *
438 |      * The range of \e x is unrestricted.
439 |      **********************************************************************/
440 |     template<typename T> static T AngNormalize(T x) {
441 | #if GEOGRAPHICLIB_CXX11_MATH && GEOGRAPHICLIB_PRECISION != 4
442 |       using std::remainder;
443 |       x = remainder(x, T(360)); return x != -180 ? x : 180;
444 | #else
445 |       using std::fmod;
446 |       T y = fmod(x, T(360));
447 | #if defined(_MSC_VER) && _MSC_VER < 1900
448 |       // Before version 14 (2015), Visual Studio had problems dealing
449 |       // with -0.0.  Specifically
450 |       //   VC 10,11,12 and 32-bit compile: fmod(-0.0, 360.0) -> +0.0
451 |       // sincosd has a similar fix.
452 |       // python 2.7 on Windows 32-bit machines has the same problem.
453 |       if (x == 0) y = x;
454 | #endif
455 |       return y <= -180 ? y + 360 : (y <= 180 ? y : y - 360);
456 | #endif
457 |     }
458 | 
459 |     /**
460 |      * Normalize a latitude.
461 |      *
462 |      * @tparam T the type of the argument and returned value.
463 |      * @param[in] x the angle in degrees.
464 |      * @return x if it is in the range [&minus;90&deg;, 90&deg;], otherwise
465 |      *   return NaN.
466 |      **********************************************************************/
467 |     template<typename T> static T LatFix(T x)
468 |     { using std::abs; return abs(x) > 90 ? NaN<T>() : x; }
469 | 
470 |     /**
471 |      * The exact difference of two angles reduced to
472 |      * (&minus;180&deg;, 180&deg;].
473 |      *
474 |      * @tparam T the type of the arguments and returned value.
475 |      * @param[in] x the first angle in degrees.
476 |      * @param[in] y the second angle in degrees.
477 |      * @param[out] e the error term in degrees.
478 |      * @return \e d, the truncated value of \e y &minus; \e x.
479 |      *
480 |      * This computes \e z = \e y &minus; \e x exactly, reduced to
481 |      * (&minus;180&deg;, 180&deg;]; and then sets \e z = \e d + \e e where \e d
482 |      * is the nearest representable number to \e z and \e e is the truncation
483 |      * error.  If \e d = &minus;180, then \e e &gt; 0; If \e d = 180, then \e e
484 |      * &le; 0.
485 |      **********************************************************************/
486 |     template<typename T> static T AngDiff(T x, T y, T& e) {
487 | #if GEOGRAPHICLIB_CXX11_MATH && GEOGRAPHICLIB_PRECISION != 4
488 |       using std::remainder;
489 |       T t, d = AngNormalize(sum(remainder(-x, T(360)),
490 |                                 remainder( y, T(360)), t));
491 | #else
492 |       T t, d = AngNormalize(sum(AngNormalize(-x), AngNormalize(y), t));
493 | #endif
494 |       // Here y - x = d + t (mod 360), exactly, where d is in (-180,180] and
495 |       // abs(t) <= eps (eps = 2^-45 for doubles).  The only case where the
496 |       // addition of t takes the result outside the range (-180,180] is d = 180
497 |       // and t > 0.  The case, d = -180 + eps, t = -eps, can't happen, since
498 |       // sum would have returned the exact result in such a case (i.e., given t
499 |       // = 0).
500 |       return sum(d == 180 && t > 0 ? -180 : d, t, e);
501 |     }
502 | 
503 |     /**
504 |      * Difference of two angles reduced to [&minus;180&deg;, 180&deg;]
505 |      *
506 |      * @tparam T the type of the arguments and returned value.
507 |      * @param[in] x the first angle in degrees.
508 |      * @param[in] y the second angle in degrees.
509 |      * @return \e y &minus; \e x, reduced to the range [&minus;180&deg;,
510 |      *   180&deg;].
511 |      *
512 |      * The result is equivalent to computing the difference exactly, reducing
513 |      * it to (&minus;180&deg;, 180&deg;] and rounding the result.  Note that
514 |      * this prescription allows &minus;180&deg; to be returned (e.g., if \e x
515 |      * is tiny and negative and \e y = 180&deg;).
516 |      **********************************************************************/
517 |     template<typename T> static T AngDiff(T x, T y)
518 |     { T e; return AngDiff(x, y, e); }
519 | 
520 |     /**
521 |      * Coarsen a value close to zero.
522 |      *
523 |      * @tparam T the type of the argument and returned value.
524 |      * @param[in] x
525 |      * @return the coarsened value.
526 |      *
527 |      * The makes the smallest gap in \e x = 1/16 - nextafter(1/16, 0) =
528 |      * 1/2<sup>57</sup> for reals = 0.7 pm on the earth if \e x is an angle in
529 |      * degrees.  (This is about 1000 times more resolution than we get with
530 |      * angles around 90&deg;.)  We use this to avoid having to deal with near
531 |      * singular cases when \e x is non-zero but tiny (e.g.,
532 |      * 10<sup>&minus;200</sup>).  This converts -0 to +0; however tiny negative
533 |      * numbers get converted to -0.
534 |      **********************************************************************/
535 |     template<typename T> static T AngRound(T x) {
536 |       using std::abs;
537 |       static const T z = 1/T(16);
538 |       if (x == 0) return 0;
539 |       GEOGRAPHICLIB_VOLATILE T y = abs(x);
540 |       // The compiler mustn't "simplify" z - (z - y) to y
541 |       y = y < z ? z - (z - y) : y;
542 |       return x < 0 ? -y : y;
543 |     }
544 | 
545 |     /**
546 |      * Evaluate the sine and cosine function with the argument in degrees
547 |      *
548 |      * @tparam T the type of the arguments.
549 |      * @param[in] x in degrees.
550 |      * @param[out] sinx sin(<i>x</i>).
551 |      * @param[out] cosx cos(<i>x</i>).
552 |      *
553 |      * The results obey exactly the elementary properties of the trigonometric
554 |      * functions, e.g., sin 9&deg; = cos 81&deg; = &minus; sin 123456789&deg;.
555 |      * If x = &minus;0, then \e sinx = &minus;0; this is the only case where
556 |      * &minus;0 is returned.
557 |      **********************************************************************/
558 |     template<typename T> static void sincosd(T x, T& sinx, T& cosx) {
559 |       // In order to minimize round-off errors, this function exactly reduces
560 |       // the argument to the range [-45, 45] before converting it to radians.
561 |       using std::sin; using std::cos;
562 |       T r; int q;
563 | #if GEOGRAPHICLIB_CXX11_MATH && GEOGRAPHICLIB_PRECISION <= 3 && \
564 |   !defined(__GNUC__)
565 |       // Disable for gcc because of bug in glibc version < 2.22, see
566 |       //   https://sourceware.org/bugzilla/show_bug.cgi?id=17569
567 |       // Once this fix is widely deployed, should insert a runtime test for the
568 |       // glibc version number.  For example
569 |       //   #include <gnu/libc-version.h>
570 |       //   std::string version(gnu_get_libc_version()); => "2.22"
571 |       using std::remquo;
572 |       r = remquo(x, T(90), &q);
573 | #else
574 |       using std::fmod; using std::floor;
575 |       r = fmod(x, T(360));
576 |       q = int(floor(r / 90 + T(0.5)));
577 |       r -= 90 * q;
578 | #endif
579 |       // now abs(r) <= 45
580 |       r *= degree();
581 |       // Possibly could call the gnu extension sincos
582 |       T s = sin(r), c = cos(r);
583 | #if defined(_MSC_VER) && _MSC_VER < 1900
584 |       // Before version 14 (2015), Visual Studio had problems dealing
585 |       // with -0.0.  Specifically
586 |       //   VC 10,11,12 and 32-bit compile: fmod(-0.0, 360.0) -> +0.0
587 |       //   VC 12       and 64-bit compile:  sin(-0.0)        -> +0.0
588 |       // AngNormalize has a similar fix.
589 |       // python 2.7 on Windows 32-bit machines has the same problem.
590 |       if (x == 0) s = x;
591 | #endif
592 |       switch (unsigned(q) & 3U) {
593 |       case 0U: sinx =  s; cosx =  c; break;
594 |       case 1U: sinx =  c; cosx = -s; break;
595 |       case 2U: sinx = -s; cosx = -c; break;
596 |       default: sinx = -c; cosx =  s; break; // case 3U
597 |       }
598 |       // Set sign of 0 results.  -0 only produced for sin(-0)
599 |       if (x != 0) { sinx += T(0); cosx += T(0); }
600 |     }
601 | 
602 |     /**
603 |      * Evaluate the sine function with the argument in degrees
604 |      *
605 |      * @tparam T the type of the argument and the returned value.
606 |      * @param[in] x in degrees.
607 |      * @return sin(<i>x</i>).
608 |      **********************************************************************/
609 |     template<typename T> static T sind(T x) {
610 |       // See sincosd
611 |       using std::sin; using std::cos;
612 |       T r; int q;
613 | #if GEOGRAPHICLIB_CXX11_MATH && GEOGRAPHICLIB_PRECISION <= 3 && \
614 |   !defined(__GNUC__)
615 |       using std::remquo;
616 |       r = remquo(x, T(90), &q);
617 | #else
618 |       using std::fmod; using std::floor;
619 |       r = fmod(x, T(360));
620 |       q = int(floor(r / 90 + T(0.5)));
621 |       r -= 90 * q;
622 | #endif
623 |       // now abs(r) <= 45
624 |       r *= degree();
625 |       unsigned p = unsigned(q);
626 |       r = p & 1U ? cos(r) : sin(r);
627 |       if (p & 2U) r = -r;
628 |       if (x != 0) r += T(0);
629 |       return r;
630 |     }
631 | 
632 |     /**
633 |      * Evaluate the cosine function with the argument in degrees
634 |      *
635 |      * @tparam T the type of the argument and the returned value.
636 |      * @param[in] x in degrees.
637 |      * @return cos(<i>x</i>).
638 |      **********************************************************************/
639 |     template<typename T> static T cosd(T x) {
640 |       // See sincosd
641 |       using std::sin; using std::cos;
642 |       T r; int q;
643 | #if GEOGRAPHICLIB_CXX11_MATH && GEOGRAPHICLIB_PRECISION <= 3 && \
644 |   !defined(__GNUC__)
645 |       using std::remquo;
646 |       r = remquo(x, T(90), &q);
647 | #else
648 |       using std::fmod; using std::floor;
649 |       r = fmod(x, T(360));
650 |       q = int(floor(r / 90 + T(0.5)));
651 |       r -= 90 * q;
652 | #endif
653 |       // now abs(r) <= 45
654 |       r *= degree();
655 |       unsigned p = unsigned(q + 1);
656 |       r = p & 1U ? cos(r) : sin(r);
657 |       if (p & 2U) r = -r;
658 |       return T(0) + r;
659 |     }
660 | 
661 |     /**
662 |      * Evaluate the tangent function with the argument in degrees
663 |      *
664 |      * @tparam T the type of the argument and the returned value.
665 |      * @param[in] x in degrees.
666 |      * @return tan(<i>x</i>).
667 |      *
668 |      * If \e x = &plusmn;90&deg;, then a suitably large (but finite) value is
669 |      * returned.
670 |      **********************************************************************/
671 |     template<typename T> static T tand(T x) {
672 |       static const T overflow = 1 / sq(std::numeric_limits<T>::epsilon());
673 |       T s, c;
674 |       sincosd(x, s, c);
675 |       return c != 0 ? s / c : (s < 0 ? -overflow : overflow);
676 |     }
677 | 
678 |     /**
679 |      * Evaluate the atan2 function with the result in degrees
680 |      *
681 |      * @tparam T the type of the arguments and the returned value.
682 |      * @param[in] y
683 |      * @param[in] x
684 |      * @return atan2(<i>y</i>, <i>x</i>) in degrees.
685 |      *
686 |      * The result is in the range (&minus;180&deg; 180&deg;].  N.B.,
687 |      * atan2d(&plusmn;0, &minus;1) = +180&deg;; atan2d(&minus;&epsilon;,
688 |      * &minus;1) = &minus;180&deg;, for &epsilon; positive and tiny;
689 |      * atan2d(&plusmn;0, +1) = &plusmn;0&deg;.
690 |      **********************************************************************/
691 |     template<typename T> static T atan2d(T y, T x) {
692 |       // In order to minimize round-off errors, this function rearranges the
693 |       // arguments so that result of atan2 is in the range [-pi/4, pi/4] before
694 |       // converting it to degrees and mapping the result to the correct
695 |       // quadrant.
696 |       using std::atan2; using std::abs;
697 |       int q = 0;
698 |       if (abs(y) > abs(x)) { std::swap(x, y); q = 2; }
699 |       if (x < 0) { x = -x; ++q; }
700 |       // here x >= 0 and x >= abs(y), so angle is in [-pi/4, pi/4]
701 |       T ang = atan2(y, x) / degree();
702 |       switch (q) {
703 |         // Note that atan2d(-0.0, 1.0) will return -0.  However, we expect that
704 |         // atan2d will not be called with y = -0.  If need be, include
705 |         //
706 |         //   case 0: ang = 0 + ang; break;
707 |         //
708 |         // and handle mpfr as in AngRound.
709 |       case 1: ang = (y >= 0 ? 180 : -180) - ang; break;
710 |       case 2: ang =  90 - ang; break;
711 |       case 3: ang = -90 + ang; break;
712 |       }
713 |       return ang;
714 |     }
715 | 
716 |     /**
717 |      * Evaluate the atan function with the result in degrees
718 |      *
719 |      * @tparam T the type of the argument and the returned value.
720 |      * @param[in] x
721 |      * @return atan(<i>x</i>) in degrees.
722 |      **********************************************************************/
723 |     template<typename T> static T atand(T x)
724 |     { return atan2d(x, T(1)); }
725 | 
726 |     /**
727 |      * Evaluate <i>e</i> atanh(<i>e x</i>)
728 |      *
729 |      * @tparam T the type of the argument and the returned value.
730 |      * @param[in] x
731 |      * @param[in] es the signed eccentricity =  sign(<i>e</i><sup>2</sup>)
732 |      *    sqrt(|<i>e</i><sup>2</sup>|)
733 |      * @return <i>e</i> atanh(<i>e x</i>)
734 |      *
735 |      * If <i>e</i><sup>2</sup> is negative (<i>e</i> is imaginary), the
736 |      * expression is evaluated in terms of atan.
737 |      **********************************************************************/
738 |     template<typename T> static T eatanhe(T x, T es);
739 | 
740 |     /**
741 |      * Copy the sign.
742 |      *
743 |      * @tparam T the type of the argument.
744 |      * @param[in] x gives the magitude of the result.
745 |      * @param[in] y gives the sign of the result.
746 |      * @return value with the magnitude of \e x and with the sign of \e y.
747 |      *
748 |      * This routine correctly handles the case \e y = &minus;0, returning
749 |      * &minus|<i>x</i>|.
750 |      **********************************************************************/
751 |     template<typename T> static T copysign(T x, T y) {
752 | #if GEOGRAPHICLIB_CXX11_MATH
753 |       using std::copysign; return copysign(x, y);
754 | #else
755 |       using std::abs;
756 |       // NaN counts as positive
757 |       return abs(x) * (y < 0 || (y == 0 && 1/y < 0) ? -1 : 1);
758 | #endif
759 |     }
760 | 
761 |     /**
762 |      * tan&chi; in terms of tan&phi;
763 |      *
764 |      * @tparam T the type of the argument and the returned value.
765 |      * @param[in] tau &tau; = tan&phi;
766 |      * @param[in] es the signed eccentricity = sign(<i>e</i><sup>2</sup>)
767 |      *   sqrt(|<i>e</i><sup>2</sup>|)
768 |      * @return &tau;&prime; = tan&chi;
769 |      *
770 |      * See Eqs. (7--9) of
771 |      * C. F. F. Karney,
772 |      * <a href="https://doi.org/10.1007/s00190-011-0445-3">
773 |      * Transverse Mercator with an accuracy of a few nanometers,</a>
774 |      * J. Geodesy 85(8), 475--485 (Aug. 2011)
775 |      * (preprint
776 |      * <a href="https://arxiv.org/abs/1002.1417">arXiv:1002.1417</a>).
777 |      **********************************************************************/
778 |     template<typename T> static T taupf(T tau, T es);
779 | 
780 |     /**
781 |      * tan&phi; in terms of tan&chi;
782 |      *
783 |      * @tparam T the type of the argument and the returned value.
784 |      * @param[in] taup &tau;&prime; = tan&chi;
785 |      * @param[in] es the signed eccentricity = sign(<i>e</i><sup>2</sup>)
786 |      *   sqrt(|<i>e</i><sup>2</sup>|)
787 |      * @return &tau; = tan&phi;
788 |      *
789 |      * See Eqs. (19--21) of
790 |      * C. F. F. Karney,
791 |      * <a href="https://doi.org/10.1007/s00190-011-0445-3">
792 |      * Transverse Mercator with an accuracy of a few nanometers,</a>
793 |      * J. Geodesy 85(8), 475--485 (Aug. 2011)
794 |      * (preprint
795 |      * <a href="https://arxiv.org/abs/1002.1417">arXiv:1002.1417</a>).
796 |      **********************************************************************/
797 |     template<typename T> static T tauf(T taup, T es);
798 | 
799 |     /**
800 |      * Test for finiteness.
801 |      *
802 |      * @tparam T the type of the argument.
803 |      * @param[in] x
804 |      * @return true if number is finite, false if NaN or infinite.
805 |      **********************************************************************/
806 |     template<typename T> static bool isfinite(T x) {
807 | #if GEOGRAPHICLIB_CXX11_MATH
808 |       using std::isfinite; return isfinite(x);
809 | #else
810 |       using std::abs;
811 | #if defined(_MSC_VER)
812 |       return abs(x) <= (std::numeric_limits<T>::max)();
813 | #else
814 |       // There's a problem using MPFR C++ 3.6.3 and g++ -std=c++14 (reported on
815 |       // 2015-05-04) with the parens around std::numeric_limits<T>::max.  Of
816 |       // course, these parens are only needed to deal with Windows stupidly
817 |       // defining max as a macro.  So don't insert the parens on non-Windows
818 |       // platforms.
819 |       return abs(x) <= std::numeric_limits<T>::max();
820 | #endif
821 | #endif
822 |     }
823 | 
824 |     /**
825 |      * The NaN (not a number)
826 |      *
827 |      * @tparam T the type of the returned value.
828 |      * @return NaN if available, otherwise return the max real of type T.
829 |      **********************************************************************/
830 |     template<typename T> static T NaN() {
831 | #if defined(_MSC_VER)
832 |       return std::numeric_limits<T>::has_quiet_NaN ?
833 |         std::numeric_limits<T>::quiet_NaN() :
834 |         (std::numeric_limits<T>::max)();
835 | #else
836 |       return std::numeric_limits<T>::has_quiet_NaN ?
837 |         std::numeric_limits<T>::quiet_NaN() :
838 |         std::numeric_limits<T>::max();
839 | #endif
840 |     }
841 |     /**
842 |      * A synonym for NaN<real>().
843 |      **********************************************************************/
844 |     static real NaN() { return NaN<real>(); }
845 | 
846 |     /**
847 |      * Test for NaN.
848 |      *
849 |      * @tparam T the type of the argument.
850 |      * @param[in] x
851 |      * @return true if argument is a NaN.
852 |      **********************************************************************/
853 |     template<typename T> static bool isnan(T x) {
854 | #if GEOGRAPHICLIB_CXX11_MATH
855 |       using std::isnan; return isnan(x);
856 | #else
857 |       return x != x;
858 | #endif
859 |     }
860 | 
861 |     /**
862 |      * Infinity
863 |      *
864 |      * @tparam T the type of the returned value.
865 |      * @return infinity if available, otherwise return the max real.
866 |      **********************************************************************/
867 |     template<typename T> static T infinity() {
868 | #if defined(_MSC_VER)
869 |       return std::numeric_limits<T>::has_infinity ?
870 |         std::numeric_limits<T>::infinity() :
871 |         (std::numeric_limits<T>::max)();
872 | #else
873 |       return std::numeric_limits<T>::has_infinity ?
874 |         std::numeric_limits<T>::infinity() :
875 |         std::numeric_limits<T>::max();
876 | #endif
877 |     }
878 |     /**
879 |      * A synonym for infinity<real>().
880 |      **********************************************************************/
881 |     static real infinity() { return infinity<real>(); }
882 | 
883 |     /**
884 |      * Swap the bytes of a quantity
885 |      *
886 |      * @tparam T the type of the argument and the returned value.
887 |      * @param[in] x
888 |      * @return x with its bytes swapped.
889 |      **********************************************************************/
890 |     template<typename T> static T swab(T x) {
891 |       union {
892 |         T r;
893 |         unsigned char c[sizeof(T)];
894 |       } b;
895 |       b.r = x;
896 |       for (int i = sizeof(T)/2; i--; )
897 |         std::swap(b.c[i], b.c[sizeof(T) - 1 - i]);
898 |       return b.r;
899 |     }
900 | 
901 | #if GEOGRAPHICLIB_PRECISION == 4
902 |     typedef boost::math::policies::policy
903 |       < boost::math::policies::domain_error
904 |         <boost::math::policies::errno_on_error>,
905 |         boost::math::policies::pole_error
906 |         <boost::math::policies::errno_on_error>,
907 |         boost::math::policies::overflow_error
908 |         <boost::math::policies::errno_on_error>,
909 |         boost::math::policies::evaluation_error
910 |         <boost::math::policies::errno_on_error> >
911 |       boost_special_functions_policy;
912 | 
913 |     static real hypot(real x, real y)
914 |     { return boost::math::hypot(x, y, boost_special_functions_policy()); }
915 | 
916 |     static real expm1(real x)
917 |     { return boost::math::expm1(x, boost_special_functions_policy()); }
918 | 
919 |     static real log1p(real x)
920 |     { return boost::math::log1p(x, boost_special_functions_policy()); }
921 | 
922 |     static real asinh(real x)
923 |     { return boost::math::asinh(x, boost_special_functions_policy()); }
924 | 
925 |     static real atanh(real x)
926 |     { return boost::math::atanh(x, boost_special_functions_policy()); }
927 | 
928 |     static real cbrt(real x)
929 |     { return boost::math::cbrt(x, boost_special_functions_policy()); }
930 | 
931 |     static real fma(real x, real y, real z)
932 |     { return fmaq(__float128(x), __float128(y), __float128(z)); }
933 | 
934 |     static real copysign(real x, real y)
935 |     { return boost::math::copysign(x, y); }
936 | 
937 |     static bool isnan(real x) { return boost::math::isnan(x); }
938 | 
939 |     static bool isfinite(real x) { return boost::math::isfinite(x); }
940 | #endif
941 |   };
942 | 
943 | } // namespace GeographicLib
944 | 
945 | #endif  // GEOGRAPHICLIB_MATH_HPP
946 | 


--------------------------------------------------------------------------------