pi/openvins_linux
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

gps fusion implemented

Browse Source
This commit is contained in:
admin1
2022-08-28 22:25:47 +03:00
parent b8ee6672d1
commit 979c7a2250
27 changed files with 132151 additions and 110 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
global_fusion/CMakeLists.txt Normal file
View File

@@ -0,0 +1,39 @@
cmake_minimum_required(VERSION 2.8.3)
project(global_fusion)
set(CMAKE_BUILD_TYPE "Debug")
set(CMAKE_CXX_FLAGS "-std=c++11")
set(CMAKE_CXX_FLAGS_DEBUG "-O3 -Wall -g")
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
rosbag
tf
std_msgs
geometry_msgs
sensor_msgs
nav_msgs
)
find_package(Ceres REQUIRED)
add_subdirectory(./Thirdparty/GeographicLib/)
include_directories(
${catkin_INCLUDE_DIRS}
${CERES_INCLUDE_DIRS}
./Thirdparty/GeographicLib/include/
)
catkin_package()
add_executable(global_fusion_node
src/globalOptNode.cpp
src/globalOpt.cpp
# src/KITTIGPSTest.cpp
)
target_link_libraries(global_fusion_node ${catkin_LIBRARIES} ${CERES_LIBRARIES} libGeographiccc)

36
global_fusion/Thirdparty/GeographicLib/CMakeLists.txt vendored Normal file
View File

@@ -0,0 +1,36 @@
project (GeographicLib)
# Version information
set (PROJECT_VERSION_MAJOR 1)
set (PROJECT_VERSION_MINOR 49)
set (PROJECT_VERSION_PATCH 0)
set (PROJECT_VERSION "${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}")
if (PROJECT_VERSION_PATCH GREATER 0)
set (PROJECT_VERSION "${PROJECT_VERSION}.${PROJECT_VERSION_PATCH}")
endif ()
# The library version tracks the numbering given by libtool in the
# autoconf set up.
set (LIBVERSION_API 17)
set (LIBVERSION_BUILD 17.1.2)
string (TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER)
string (TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER)
cmake_minimum_required (VERSION 2.8.4) # This version was released 2011-02-16
# (7) Set the default "real" precision. This should probably be left
# at 2 (double).
set (GEOGRAPHICLIB_PRECISION 2 CACHE STRING
"Precision: 1 = float, 2 = double, 3 = extended, 4 = quadruple, 5 = variable")
set_property (CACHE GEOGRAPHICLIB_PRECISION PROPERTY STRINGS 1 2 3 4 5)
set (LIBNAME Geographic)
include_directories(
./include/
)
add_library(libGeographiccc src/LocalCartesian.cpp
src/Geocentric.cpp
src/Math.cpp)

12
global_fusion/Thirdparty/GeographicLib/include/Config.h vendored Normal file
View File

@@ -0,0 +1,12 @@
// This will be overwritten by ./configure
#define GEOGRAPHICLIB_VERSION_STRING "1.49"
#define GEOGRAPHICLIB_VERSION_MAJOR 1
#define GEOGRAPHICLIB_VERSION_MINOR 49
#define GEOGRAPHICLIB_VERSION_PATCH 0
// Undefine HAVE_LONG_DOUBLE if this type is unknown to the compiler
#define GEOGRAPHICLIB_HAVE_LONG_DOUBLE 1
// Define WORDS_BIGENDIAN to be 1 if your machine is big endian
/* #undef WORDS_BIGENDIAN */

403
global_fusion/Thirdparty/GeographicLib/include/Constants.hpp vendored Normal file
View File

@@ -0,0 +1,403 @@
/**
* \file Constants.hpp
* \brief Header for GeographicLib::Constants class
*
* Copyright (c) Charles Karney (2008-2016) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#if !defined(GEOGRAPHICLIB_CONSTANTS_HPP)
#define GEOGRAPHICLIB_CONSTANTS_HPP 1
#include "Config.h"
/**
* @relates GeographicLib::Constants
* Pack the version components into a single integer. Users should not rely on
* this particular packing of the components of the version number; see the
* documentation for GEOGRAPHICLIB_VERSION, below.
**********************************************************************/
#define GEOGRAPHICLIB_VERSION_NUM(a,b,c) ((((a) * 10000 + (b)) * 100) + (c))
/**
* @relates GeographicLib::Constants
* The version of GeographicLib as a single integer, packed as MMmmmmpp where
* MM is the major version, mmmm is the minor version, and pp is the patch
* level. Users should not rely on this particular packing of the components
* of the version number. Instead they should use a test such as \code
#if GEOGRAPHICLIB_VERSION >= GEOGRAPHICLIB_VERSION_NUM(1,37,0)
...
#endif
* \endcode
**********************************************************************/
#define GEOGRAPHICLIB_VERSION \
GEOGRAPHICLIB_VERSION_NUM(GEOGRAPHICLIB_VERSION_MAJOR, \
GEOGRAPHICLIB_VERSION_MINOR, \
GEOGRAPHICLIB_VERSION_PATCH)
/**
* @relates GeographicLib::Constants
* Is the C++11 static_assert available?
**********************************************************************/
#if !defined(GEOGRAPHICLIB_HAS_STATIC_ASSERT)
# if __cplusplus >= 201103 || defined(__GXX_EXPERIMENTAL_CXX0X__)
# define GEOGRAPHICLIB_HAS_STATIC_ASSERT 1
# elif defined(_MSC_VER) && _MSC_VER >= 1600
// For reference, here is a table of Visual Studio and _MSC_VER
// correspondences:
//
// _MSC_VER Visual Studio
// 1100 vc5
// 1200 vc6
// 1300 vc7
// 1310 vc7.1 (2003)
// 1400 vc8 (2005)
// 1500 vc9 (2008)
// 1600 vc10 (2010)
// 1700 vc11 (2012)
// 1800 vc12 (2013)
// 1900 vc14 (2015)
// 1910+ vc15 (2017)
# define GEOGRAPHICLIB_HAS_STATIC_ASSERT 1
# else
# define GEOGRAPHICLIB_HAS_STATIC_ASSERT 0
# endif
#endif
/**
* @relates GeographicLib::Constants
* A compile-time assert. Use C++11 static_assert, if available.
**********************************************************************/
#if !defined(GEOGRAPHICLIB_STATIC_ASSERT)
# if GEOGRAPHICLIB_HAS_STATIC_ASSERT
# define GEOGRAPHICLIB_STATIC_ASSERT static_assert
# else
# define GEOGRAPHICLIB_STATIC_ASSERT(cond,reason) \
{ enum{ GEOGRAPHICLIB_STATIC_ASSERT_ENUM = 1/int(cond) }; }
# endif
#endif
#if defined(_MSC_VER) && defined(GEOGRAPHICLIB_SHARED_LIB) && \
GEOGRAPHICLIB_SHARED_LIB
# if GEOGRAPHICLIB_SHARED_LIB > 1
# error GEOGRAPHICLIB_SHARED_LIB must be 0 or 1
# elif defined(GeographicLib_EXPORTS)
# define GEOGRAPHICLIB_EXPORT __declspec(dllexport)
# else
# define GEOGRAPHICLIB_EXPORT __declspec(dllimport)
# endif
#else
# define GEOGRAPHICLIB_EXPORT
#endif
// Use GEOGRAPHICLIB_DEPRECATED to mark functions, types or variables as
// deprecated. Code inspired by Apache Subversion's svn_types.h file (via
// MPFR).
#if defined(__GNUC__)
# if __GNUC__ > 4
# define GEOGRAPHICLIB_DEPRECATED(msg) __attribute__((deprecated(msg)))
# else
# define GEOGRAPHICLIB_DEPRECATED(msg) __attribute__((deprecated))
# endif
#elif defined(_MSC_VER) && _MSC_VER >= 1300
# define GEOGRAPHICLIB_DEPRECATED(msg) __declspec(deprecated(msg))
#else
# define GEOGRAPHICLIB_DEPRECATED(msg)
#endif
#include <stdexcept>
#include <string>
#include "Math.hpp"
/**
* \brief Namespace for %GeographicLib
*
* All of %GeographicLib is defined within the GeographicLib namespace. In
* addition all the header files are included via %GeographicLib/Class.hpp.
* This minimizes the likelihood of conflicts with other packages.
**********************************************************************/
namespace GeographicLib {
/**
* \brief %Constants needed by %GeographicLib
*
* Define constants specifying the WGS84 ellipsoid, the UTM and UPS
* projections, and various unit conversions.
*
* Example of use:
* \include example-Constants.cpp
**********************************************************************/
class GEOGRAPHICLIB_EXPORT Constants {
private:
typedef Math::real real;
Constants(); // Disable constructor
public:
/**
* A synonym for Math::degree<real>().
**********************************************************************/
static Math::real degree() { return Math::degree(); }
/**
* @return the number of radians in an arcminute.
**********************************************************************/
static Math::real arcminute()
{ return Math::degree() / 60; }
/**
* @return the number of radians in an arcsecond.
**********************************************************************/
static Math::real arcsecond()
{ return Math::degree() / 3600; }
/** \name Ellipsoid parameters
**********************************************************************/
///@{
/**
* @tparam T the type of the returned value.
* @return the equatorial radius of WGS84 ellipsoid (6378137 m).
**********************************************************************/
template<typename T> static T WGS84_a()
{ return 6378137 * meter<T>(); }
/**
* A synonym for WGS84_a<real>().
**********************************************************************/
static Math::real WGS84_a() { return WGS84_a<real>(); }
/**
* @tparam T the type of the returned value.
* @return the flattening of WGS84 ellipsoid (1/298.257223563).
**********************************************************************/
template<typename T> static T WGS84_f() {
// Evaluating this as 1000000000 / T(298257223563LL) reduces the
// round-off error by about 10%. However, expressing the flattening as
// 1/298.257223563 is well ingrained.
return 1 / ( T(298257223563LL) / 1000000000 );
}
/**
* A synonym for WGS84_f<real>().
**********************************************************************/
static Math::real WGS84_f() { return WGS84_f<real>(); }
/**
* @tparam T the type of the returned value.
* @return the gravitational constant of the WGS84 ellipsoid, \e GM, in
* m<sup>3</sup> s<sup>&minus;2</sup>.
**********************************************************************/
template<typename T> static T WGS84_GM()
{ return T(3986004) * 100000000 + 41800000; }
/**
* A synonym for WGS84_GM<real>().
**********************************************************************/
static Math::real WGS84_GM() { return WGS84_GM<real>(); }
/**
* @tparam T the type of the returned value.
* @return the angular velocity of the WGS84 ellipsoid, &omega;, in rad
* s<sup>&minus;1</sup>.
**********************************************************************/
template<typename T> static T WGS84_omega()
{ return 7292115 / (T(1000000) * 100000); }
/**
* A synonym for WGS84_omega<real>().
**********************************************************************/
static Math::real WGS84_omega() { return WGS84_omega<real>(); }
/**
* @tparam T the type of the returned value.
* @return the equatorial radius of GRS80 ellipsoid, \e a, in m.
**********************************************************************/
template<typename T> static T GRS80_a()
{ return 6378137 * meter<T>(); }
/**
* A synonym for GRS80_a<real>().
**********************************************************************/
static Math::real GRS80_a() { return GRS80_a<real>(); }
/**
* @tparam T the type of the returned value.
* @return the gravitational constant of the GRS80 ellipsoid, \e GM, in
* m<sup>3</sup> s<sup>&minus;2</sup>.
**********************************************************************/
template<typename T> static T GRS80_GM()
{ return T(3986005) * 100000000; }
/**
* A synonym for GRS80_GM<real>().
**********************************************************************/
static Math::real GRS80_GM() { return GRS80_GM<real>(); }
/**
* @tparam T the type of the returned value.
* @return the angular velocity of the GRS80 ellipsoid, &omega;, in rad
* s<sup>&minus;1</sup>.
*
* This is about 2 &pi; 366.25 / (365.25 &times; 24 &times; 3600) rad
* s<sup>&minus;1</sup>. 365.25 is the number of days in a Julian year and
* 365.35/366.25 converts from solar days to sidereal days. Using the
* number of days in a Gregorian year (365.2425) results in a worse
* approximation (because the Gregorian year includes the precession of the
* earth's axis).
**********************************************************************/
template<typename T> static T GRS80_omega()
{ return 7292115 / (T(1000000) * 100000); }
/**
* A synonym for GRS80_omega<real>().
**********************************************************************/
static Math::real GRS80_omega() { return GRS80_omega<real>(); }
/**
* @tparam T the type of the returned value.
* @return the dynamical form factor of the GRS80 ellipsoid,
* <i>J</i><sub>2</sub>.
**********************************************************************/
template<typename T> static T GRS80_J2()
{ return T(108263) / 100000000; }
/**
* A synonym for GRS80_J2<real>().
**********************************************************************/
static Math::real GRS80_J2() { return GRS80_J2<real>(); }
/**
* @tparam T the type of the returned value.
* @return the central scale factor for UTM (0.9996).
**********************************************************************/
template<typename T> static T UTM_k0()
{return T(9996) / 10000; }
/**
* A synonym for UTM_k0<real>().
**********************************************************************/
static Math::real UTM_k0() { return UTM_k0<real>(); }
/**
* @tparam T the type of the returned value.
* @return the central scale factor for UPS (0.994).
**********************************************************************/
template<typename T> static T UPS_k0()
{ return T(994) / 1000; }
/**
* A synonym for UPS_k0<real>().
**********************************************************************/
static Math::real UPS_k0() { return UPS_k0<real>(); }
///@}
/** \name SI units
**********************************************************************/
///@{
/**
* @tparam T the type of the returned value.
* @return the number of meters in a meter.
*
* This is unity, but this lets the internal system of units be changed if
* necessary.
**********************************************************************/
template<typename T> static T meter() { return T(1); }
/**
* A synonym for meter<real>().
**********************************************************************/
static Math::real meter() { return meter<real>(); }
/**
* @return the number of meters in a kilometer.
**********************************************************************/
static Math::real kilometer()
{ return 1000 * meter<real>(); }
/**
* @return the number of meters in a nautical mile (approximately 1 arc
* minute)
**********************************************************************/
static Math::real nauticalmile()
{ return 1852 * meter<real>(); }
/**
* @tparam T the type of the returned value.
* @return the number of square meters in a square meter.
*
* This is unity, but this lets the internal system of units be changed if
* necessary.
**********************************************************************/
template<typename T> static T square_meter()
{ return meter<real>() * meter<real>(); }
/**
* A synonym for square_meter<real>().
**********************************************************************/
static Math::real square_meter()
{ return square_meter<real>(); }
/**
* @return the number of square meters in a hectare.
**********************************************************************/
static Math::real hectare()
{ return 10000 * square_meter<real>(); }
/**
* @return the number of square meters in a square kilometer.
**********************************************************************/
static Math::real square_kilometer()
{ return kilometer() * kilometer(); }
/**
* @return the number of square meters in a square nautical mile.
**********************************************************************/
static Math::real square_nauticalmile()
{ return nauticalmile() * nauticalmile(); }
///@}
/** \name Anachronistic British units
**********************************************************************/
///@{
/**
* @return the number of meters in an international foot.
**********************************************************************/
static Math::real foot()
{ return real(254 * 12) / 10000 * meter<real>(); }
/**
* @return the number of meters in a yard.
**********************************************************************/
static Math::real yard() { return 3 * foot(); }
/**
* @return the number of meters in a fathom.
**********************************************************************/
static Math::real fathom() { return 2 * yard(); }
/**
* @return the number of meters in a chain.
**********************************************************************/
static Math::real chain() { return 22 * yard(); }
/**
* @return the number of meters in a furlong.
**********************************************************************/
static Math::real furlong() { return 10 * chain(); }
/**
* @return the number of meters in a statute mile.
**********************************************************************/
static Math::real mile() { return 8 * furlong(); }
/**
* @return the number of square meters in an acre.
**********************************************************************/
static Math::real acre() { return chain() * furlong(); }
/**
* @return the number of square meters in a square statute mile.
**********************************************************************/
static Math::real square_mile() { return mile() * mile(); }
///@}
/** \name Anachronistic US units
**********************************************************************/
///@{
/**
* @return the number of meters in a US survey foot.
**********************************************************************/
static Math::real surveyfoot()
{ return real(1200) / 3937 * meter<real>(); }
///@}
};
/**
* \brief Exception handling for %GeographicLib
*
* A class to handle exceptions. It's derived from std::runtime_error so it
* can be caught by the usual catch clauses.
*
* Example of use:
* \include example-GeographicErr.cpp
**********************************************************************/
class GeographicErr : public std::runtime_error {
public:
/**
* Constructor
*
* @param[in] msg a string message, which is accessible in the catch
* clause via what().
**********************************************************************/
GeographicErr(const std::string& msg) : std::runtime_error(msg) {}
};
} // namespace GeographicLib
#endif // GEOGRAPHICLIB_CONSTANTS_HPP

267
global_fusion/Thirdparty/GeographicLib/include/Geocentric.hpp vendored Normal file
View File

@@ -0,0 +1,267 @@
/**
* \file Geocentric.hpp
* \brief Header for GeographicLib::Geocentric class
*
* Copyright (c) Charles Karney (2008-2016) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#if !defined(GEOGRAPHICLIB_GEOCENTRIC_HPP)
#define GEOGRAPHICLIB_GEOCENTRIC_HPP 1
#include <vector>
#include "Constants.hpp"
namespace GeographicLib {
/**
* \brief %Geocentric coordinates
*
* Convert between geodetic coordinates latitude = \e lat, longitude = \e
* lon, height = \e h (measured vertically from the surface of the ellipsoid)
* to geocentric coordinates (\e X, \e Y, \e Z). The origin of geocentric
* coordinates is at the center of the earth. The \e Z axis goes thru the
* north pole, \e lat = 90&deg;. The \e X axis goes thru \e lat = 0,
* \e lon = 0. %Geocentric coordinates are also known as earth centered,
* earth fixed (ECEF) coordinates.
*
* The conversion from geographic to geocentric coordinates is
* straightforward. For the reverse transformation we use
* - H. Vermeille,
* <a href="https://doi.org/10.1007/s00190-002-0273-6"> Direct
* transformation from geocentric coordinates to geodetic coordinates</a>,
* J. Geodesy 76, 451--454 (2002).
* .
* Several changes have been made to ensure that the method returns accurate
* results for all finite inputs (even if \e h is infinite). The changes are
* described in Appendix B of
* - C. F. F. Karney,
* <a href="https://arxiv.org/abs/1102.1215v1">Geodesics
* on an ellipsoid of revolution</a>,
* Feb. 2011;
* preprint
* <a href="https://arxiv.org/abs/1102.1215v1">arxiv:1102.1215v1</a>.
* .
* Vermeille similarly updated his method in
* - H. Vermeille,
* <a href="https://doi.org/10.1007/s00190-010-0419-x">
* An analytical method to transform geocentric into
* geodetic coordinates</a>, J. Geodesy 85, 105--117 (2011).
* .
* See \ref geocentric for more information.
*
* The errors in these routines are close to round-off. Specifically, for
* points within 5000 km of the surface of the ellipsoid (either inside or
* outside the ellipsoid), the error is bounded by 7 nm (7 nanometers) for
* the WGS84 ellipsoid. See \ref geocentric for further information on the
* errors.
*
* Example of use:
* \include example-Geocentric.cpp
*
* <a href="CartConvert.1.html">CartConvert</a> is a command-line utility
* providing access to the functionality of Geocentric and LocalCartesian.
**********************************************************************/
class GEOGRAPHICLIB_EXPORT Geocentric {
private:
typedef Math::real real;
friend class LocalCartesian;
friend class MagneticCircle; // MagneticCircle uses Rotation
friend class MagneticModel; // MagneticModel uses IntForward
friend class GravityCircle; // GravityCircle uses Rotation
friend class GravityModel; // GravityModel uses IntForward
friend class NormalGravity; // NormalGravity uses IntForward
static const size_t dim_ = 3;
static const size_t dim2_ = dim_ * dim_;
real _a, _f, _e2, _e2m, _e2a, _e4a, _maxrad;
static void Rotation(real sphi, real cphi, real slam, real clam,
real M[dim2_]);
static void Rotate(real M[dim2_], real x, real y, real z,
real& X, real& Y, real& Z) {
// Perform [X,Y,Z]^t = M.[x,y,z]^t
// (typically local cartesian to geocentric)
X = M[0] * x + M[1] * y + M[2] * z;
Y = M[3] * x + M[4] * y + M[5] * z;
Z = M[6] * x + M[7] * y + M[8] * z;
}
static void Unrotate(real M[dim2_], real X, real Y, real Z,
real& x, real& y, real& z) {
// Perform [x,y,z]^t = M^t.[X,Y,Z]^t
// (typically geocentric to local cartesian)
x = M[0] * X + M[3] * Y + M[6] * Z;
y = M[1] * X + M[4] * Y + M[7] * Z;
z = M[2] * X + M[5] * Y + M[8] * Z;
}
void IntForward(real lat, real lon, real h, real& X, real& Y, real& Z,
real M[dim2_]) const;
void IntReverse(real X, real Y, real Z, real& lat, real& lon, real& h,
real M[dim2_]) const;
public:
/**
* Constructor for a ellipsoid with
*
* @param[in] a equatorial radius (meters).
* @param[in] f flattening of ellipsoid. Setting \e f = 0 gives a sphere.
* Negative \e f gives a prolate ellipsoid.
* @exception GeographicErr if \e a or (1 &minus; \e f) \e a is not
* positive.
**********************************************************************/
Geocentric(real a, real f);
/**
* A default constructor (for use by NormalGravity).
**********************************************************************/
Geocentric() : _a(-1) {}
/**
* Convert from geodetic to geocentric coordinates.
*
* @param[in] lat latitude of point (degrees).
* @param[in] lon longitude of point (degrees).
* @param[in] h height of point above the ellipsoid (meters).
* @param[out] X geocentric coordinate (meters).
* @param[out] Y geocentric coordinate (meters).
* @param[out] Z geocentric coordinate (meters).
*
* \e lat should be in the range [&minus;90&deg;, 90&deg;].
**********************************************************************/
void Forward(real lat, real lon, real h, real& X, real& Y, real& Z)
const {
if (Init())
IntForward(lat, lon, h, X, Y, Z, NULL);
}
/**
* Convert from geodetic to geocentric coordinates and return rotation
* matrix.
*
* @param[in] lat latitude of point (degrees).
* @param[in] lon longitude of point (degrees).
* @param[in] h height of point above the ellipsoid (meters).
* @param[out] X geocentric coordinate (meters).
* @param[out] Y geocentric coordinate (meters).
* @param[out] Z geocentric coordinate (meters).
* @param[out] M if the length of the vector is 9, fill with the rotation
* matrix in row-major order.
*
* Let \e v be a unit vector located at (\e lat, \e lon, \e h). We can
* express \e v as \e column vectors in one of two ways
* - in east, north, up coordinates (where the components are relative to a
* local coordinate system at (\e lat, \e lon, \e h)); call this
* representation \e v1.
* - in geocentric \e X, \e Y, \e Z coordinates; call this representation
* \e v0.
* .
* Then we have \e v0 = \e M &sdot; \e v1.
**********************************************************************/
void Forward(real lat, real lon, real h, real& X, real& Y, real& Z,
std::vector<real>& M)
const {
if (!Init())
return;
if (M.end() == M.begin() + dim2_) {
real t[dim2_];
IntForward(lat, lon, h, X, Y, Z, t);
std::copy(t, t + dim2_, M.begin());
} else
IntForward(lat, lon, h, X, Y, Z, NULL);
}
/**
* Convert from geocentric to geodetic to coordinates.
*
* @param[in] X geocentric coordinate (meters).
* @param[in] Y geocentric coordinate (meters).
* @param[in] Z geocentric coordinate (meters).
* @param[out] lat latitude of point (degrees).
* @param[out] lon longitude of point (degrees).
* @param[out] h height of point above the ellipsoid (meters).
*
* In general there are multiple solutions and the result which maximizes
* \e h is returned. If there are still multiple solutions with different
* latitudes (applies only if \e Z = 0), then the solution with \e lat > 0
* is returned. If there are still multiple solutions with different
* longitudes (applies only if \e X = \e Y = 0) then \e lon = 0 is
* returned. The value of \e h returned satisfies \e h &ge; &minus; \e a
* (1 &minus; <i>e</i><sup>2</sup>) / sqrt(1 &minus; <i>e</i><sup>2</sup>
* sin<sup>2</sup>\e lat). The value of \e lon returned is in the range
* [&minus;180&deg;, 180&deg;].
**********************************************************************/
void Reverse(real X, real Y, real Z, real& lat, real& lon, real& h)
const {
if (Init())
IntReverse(X, Y, Z, lat, lon, h, NULL);
}
/**
* Convert from geocentric to geodetic to coordinates.
*
* @param[in] X geocentric coordinate (meters).
* @param[in] Y geocentric coordinate (meters).
* @param[in] Z geocentric coordinate (meters).
* @param[out] lat latitude of point (degrees).
* @param[out] lon longitude of point (degrees).
* @param[out] h height of point above the ellipsoid (meters).
* @param[out] M if the length of the vector is 9, fill with the rotation
* matrix in row-major order.
*
* Let \e v be a unit vector located at (\e lat, \e lon, \e h). We can
* express \e v as \e column vectors in one of two ways
* - in east, north, up coordinates (where the components are relative to a
* local coordinate system at (\e lat, \e lon, \e h)); call this
* representation \e v1.
* - in geocentric \e X, \e Y, \e Z coordinates; call this representation
* \e v0.
* .
* Then we have \e v1 = <i>M</i><sup>T</sup> &sdot; \e v0, where
* <i>M</i><sup>T</sup> is the transpose of \e M.
**********************************************************************/
void Reverse(real X, real Y, real Z, real& lat, real& lon, real& h,
std::vector<real>& M)
const {
if (!Init())
return;
if (M.end() == M.begin() + dim2_) {
real t[dim2_];
IntReverse(X, Y, Z, lat, lon, h, t);
std::copy(t, t + dim2_, M.begin());
} else
IntReverse(X, Y, Z, lat, lon, h, NULL);
}
/** \name Inspector functions
**********************************************************************/
///@{
/**
* @return true if the object has been initialized.
**********************************************************************/
bool Init() const { return _a > 0; }
/**
* @return \e a the equatorial radius of the ellipsoid (meters). This is
* the value used in the constructor.
**********************************************************************/
Math::real MajorRadius() const
{ return Init() ? _a : Math::NaN(); }
/**
* @return \e f the flattening of the ellipsoid. This is the
* value used in the constructor.
**********************************************************************/
Math::real Flattening() const
{ return Init() ? _f : Math::NaN(); }
///@}
/**
* A global instantiation of Geocentric with the parameters for the WGS84
* ellipsoid.
**********************************************************************/
static const Geocentric& WGS84();
};
} // namespace GeographicLib
#endif // GEOGRAPHICLIB_GEOCENTRIC_HPP

236
global_fusion/Thirdparty/GeographicLib/include/LocalCartesian.hpp vendored Normal file
View File

@@ -0,0 +1,236 @@
/**
* \file LocalCartesian.hpp
* \brief Header for GeographicLib::LocalCartesian class
*
* Copyright (c) Charles Karney (2008-2016) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#if !defined(GEOGRAPHICLIB_LOCALCARTESIAN_HPP)
#define GEOGRAPHICLIB_LOCALCARTESIAN_HPP 1
#include "Geocentric.hpp"
#include "Constants.hpp"
namespace GeographicLib {
/**
* \brief Local cartesian coordinates
*
* Convert between geodetic coordinates latitude = \e lat, longitude = \e
* lon, height = \e h (measured vertically from the surface of the ellipsoid)
* to local cartesian coordinates (\e x, \e y, \e z). The origin of local
* cartesian coordinate system is at \e lat = \e lat0, \e lon = \e lon0, \e h
* = \e h0. The \e z axis is normal to the ellipsoid; the \e y axis points
* due north. The plane \e z = - \e h0 is tangent to the ellipsoid.
*
* The conversions all take place via geocentric coordinates using a
* Geocentric object (by default Geocentric::WGS84()).
*
* Example of use:
* \include example-LocalCartesian.cpp
*
* <a href="CartConvert.1.html">CartConvert</a> is a command-line utility
* providing access to the functionality of Geocentric and LocalCartesian.
**********************************************************************/
class GEOGRAPHICLIB_EXPORT LocalCartesian {
private:
typedef Math::real real;
static const size_t dim_ = 3;
static const size_t dim2_ = dim_ * dim_;
Geocentric _earth;
real _lat0, _lon0, _h0;
real _x0, _y0, _z0, _r[dim2_];
void IntForward(real lat, real lon, real h, real& x, real& y, real& z,
real M[dim2_]) const;
void IntReverse(real x, real y, real z, real& lat, real& lon, real& h,
real M[dim2_]) const;
void MatrixMultiply(real M[dim2_]) const;
public:
/**
* Constructor setting the origin.
*
* @param[in] lat0 latitude at origin (degrees).
* @param[in] lon0 longitude at origin (degrees).
* @param[in] h0 height above ellipsoid at origin (meters); default 0.
* @param[in] earth Geocentric object for the transformation; default
* Geocentric::WGS84().
*
* \e lat0 should be in the range [&minus;90&deg;, 90&deg;].
**********************************************************************/
LocalCartesian(real lat0, real lon0, real h0 = 0,
const Geocentric& earth = Geocentric::WGS84())
: _earth(earth)
{ Reset(lat0, lon0, h0); }
/**
* Default constructor.
*
* @param[in] earth Geocentric object for the transformation; default
* Geocentric::WGS84().
*
* Sets \e lat0 = 0, \e lon0 = 0, \e h0 = 0.
**********************************************************************/
explicit LocalCartesian(const Geocentric& earth = Geocentric::WGS84())
: _earth(earth)
{ Reset(real(0), real(0), real(0)); }
/**
* Reset the origin.
*
* @param[in] lat0 latitude at origin (degrees).
* @param[in] lon0 longitude at origin (degrees).
* @param[in] h0 height above ellipsoid at origin (meters); default 0.
*
* \e lat0 should be in the range [&minus;90&deg;, 90&deg;].
**********************************************************************/
void Reset(real lat0, real lon0, real h0 = 0);
/**
* Convert from geodetic to local cartesian coordinates.
*
* @param[in] lat latitude of point (degrees).
* @param[in] lon longitude of point (degrees).
* @param[in] h height of point above the ellipsoid (meters).
* @param[out] x local cartesian coordinate (meters).
* @param[out] y local cartesian coordinate (meters).
* @param[out] z local cartesian coordinate (meters).
*
* \e lat should be in the range [&minus;90&deg;, 90&deg;].
**********************************************************************/
void Forward(real lat, real lon, real h, real& x, real& y, real& z)
const {
IntForward(lat, lon, h, x, y, z, NULL);
}
/**
* Convert from geodetic to local cartesian coordinates and return rotation
* matrix.
*
* @param[in] lat latitude of point (degrees).
* @param[in] lon longitude of point (degrees).
* @param[in] h height of point above the ellipsoid (meters).
* @param[out] x local cartesian coordinate (meters).
* @param[out] y local cartesian coordinate (meters).
* @param[out] z local cartesian coordinate (meters).
* @param[out] M if the length of the vector is 9, fill with the rotation
* matrix in row-major order.
*
* \e lat should be in the range [&minus;90&deg;, 90&deg;].
*
* Let \e v be a unit vector located at (\e lat, \e lon, \e h). We can
* express \e v as \e column vectors in one of two ways
* - in east, north, up coordinates (where the components are relative to a
* local coordinate system at (\e lat, \e lon, \e h)); call this
* representation \e v1.
* - in \e x, \e y, \e z coordinates (where the components are relative to
* the local coordinate system at (\e lat0, \e lon0, \e h0)); call this
* representation \e v0.
* .
* Then we have \e v0 = \e M &sdot; \e v1.
**********************************************************************/
void Forward(real lat, real lon, real h, real& x, real& y, real& z,
std::vector<real>& M)
const {
if (M.end() == M.begin() + dim2_) {
real t[dim2_];
IntForward(lat, lon, h, x, y, z, t);
std::copy(t, t + dim2_, M.begin());
} else
IntForward(lat, lon, h, x, y, z, NULL);
}
/**
* Convert from local cartesian to geodetic coordinates.
*
* @param[in] x local cartesian coordinate (meters).
* @param[in] y local cartesian coordinate (meters).
* @param[in] z local cartesian coordinate (meters).
* @param[out] lat latitude of point (degrees).
* @param[out] lon longitude of point (degrees).
* @param[out] h height of point above the ellipsoid (meters).
*
* The value of \e lon returned is in the range [&minus;180&deg;,
* 180&deg;].
**********************************************************************/
void Reverse(real x, real y, real z, real& lat, real& lon, real& h)
const {
IntReverse(x, y, z, lat, lon, h, NULL);
}
/**
* Convert from local cartesian to geodetic coordinates and return rotation
* matrix.
*
* @param[in] x local cartesian coordinate (meters).
* @param[in] y local cartesian coordinate (meters).
* @param[in] z local cartesian coordinate (meters).
* @param[out] lat latitude of point (degrees).
* @param[out] lon longitude of point (degrees).
* @param[out] h height of point above the ellipsoid (meters).
* @param[out] M if the length of the vector is 9, fill with the rotation
* matrix in row-major order.
*
* Let \e v be a unit vector located at (\e lat, \e lon, \e h). We can
* express \e v as \e column vectors in one of two ways
* - in east, north, up coordinates (where the components are relative to a
* local coordinate system at (\e lat, \e lon, \e h)); call this
* representation \e v1.
* - in \e x, \e y, \e z coordinates (where the components are relative to
* the local coordinate system at (\e lat0, \e lon0, \e h0)); call this
* representation \e v0.
* .
* Then we have \e v1 = <i>M</i><sup>T</sup> &sdot; \e v0, where
* <i>M</i><sup>T</sup> is the transpose of \e M.
**********************************************************************/
void Reverse(real x, real y, real z, real& lat, real& lon, real& h,
std::vector<real>& M)
const {
if (M.end() == M.begin() + dim2_) {
real t[dim2_];
IntReverse(x, y, z, lat, lon, h, t);
std::copy(t, t + dim2_, M.begin());
} else
IntReverse(x, y, z, lat, lon, h, NULL);
}
/** \name Inspector functions
**********************************************************************/
///@{
/**
* @return latitude of the origin (degrees).
**********************************************************************/
Math::real LatitudeOrigin() const { return _lat0; }
/**
* @return longitude of the origin (degrees).
**********************************************************************/
Math::real LongitudeOrigin() const { return _lon0; }
/**
* @return height of the origin (meters).
**********************************************************************/
Math::real HeightOrigin() const { return _h0; }
/**
* @return \e a the equatorial radius of the ellipsoid (meters). This is
* the value of \e a inherited from the Geocentric object used in the
* constructor.
**********************************************************************/
Math::real MajorRadius() const { return _earth.MajorRadius(); }
/**
* @return \e f the flattening of the ellipsoid. This is the value
* inherited from the Geocentric object used in the constructor.
**********************************************************************/
Math::real Flattening() const { return _earth.Flattening(); }
///@}
};
} // namespace GeographicLib
#endif // GEOGRAPHICLIB_LOCALCARTESIAN_HPP

945
global_fusion/Thirdparty/GeographicLib/include/Math.hpp vendored Normal file
View File

@@ -0,0 +1,945 @@
/**
* \file Math.hpp
* \brief Header for GeographicLib::Math class
*
* Copyright (c) Charles Karney (2008-2017) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
// Constants.hpp includes Math.hpp. Place this include outside Math.hpp's
// include guard to enforce this ordering.
#include "Constants.hpp"
#if !defined(GEOGRAPHICLIB_MATH_HPP)
#define GEOGRAPHICLIB_MATH_HPP 1
/**
* Are C++11 math functions available?
**********************************************************************/
#if !defined(GEOGRAPHICLIB_CXX11_MATH)
// Recent versions of g++ -std=c++11 (4.7 and later?) set __cplusplus to 201103
// and support the new C++11 mathematical functions, std::atanh, etc. However
// the Android toolchain, which uses g++ -std=c++11 (4.8 as of 2014-03-11,
// according to Pullan Lu), does not support std::atanh. Android toolchains
// might define __ANDROID__ or ANDROID; so need to check both. With OSX the
// version is GNUC version 4.2 and __cplusplus is set to 201103, so remove the
// version check on GNUC.
# if defined(__GNUC__) && __cplusplus >= 201103 && \
!(defined(__ANDROID__) || defined(ANDROID) || defined(__CYGWIN__))
# define GEOGRAPHICLIB_CXX11_MATH 1
// Visual C++ 12 supports these functions
# elif defined(_MSC_VER) && _MSC_VER >= 1800
# define GEOGRAPHICLIB_CXX11_MATH 1
# else
# define GEOGRAPHICLIB_CXX11_MATH 0
# endif
#endif
#if !defined(GEOGRAPHICLIB_WORDS_BIGENDIAN)
# define GEOGRAPHICLIB_WORDS_BIGENDIAN 0
#endif
#if !defined(GEOGRAPHICLIB_HAVE_LONG_DOUBLE)
# define GEOGRAPHICLIB_HAVE_LONG_DOUBLE 0
#endif
#if !defined(GEOGRAPHICLIB_PRECISION)
/**
* The precision of floating point numbers used in %GeographicLib. 1 means
* float (single precision); 2 (the default) means double; 3 means long double;
* 4 is reserved for quadruple precision. Nearly all the testing has been
* carried out with doubles and that's the recommended configuration. In order
* for long double to be used, GEOGRAPHICLIB_HAVE_LONG_DOUBLE needs to be
* defined. Note that with Microsoft Visual Studio, long double is the same as
* double.
**********************************************************************/
# define GEOGRAPHICLIB_PRECISION 2
#endif
#include <cmath>
#include <algorithm>
#include <limits>
#if GEOGRAPHICLIB_PRECISION == 4
#include <boost/version.hpp>
#if BOOST_VERSION >= 105600
#include <boost/cstdfloat.hpp>
#endif
#include <boost/multiprecision/float128.hpp>
#include <boost/math/special_functions.hpp>
__float128 fmaq(__float128, __float128, __float128);
#elif GEOGRAPHICLIB_PRECISION == 5
#include <mpreal.h>
#endif
#if GEOGRAPHICLIB_PRECISION > 3
// volatile keyword makes no sense for multiprec types
#define GEOGRAPHICLIB_VOLATILE
// Signal a convergence failure with multiprec types by throwing an exception
// at loop exit.
#define GEOGRAPHICLIB_PANIC \
(throw GeographicLib::GeographicErr("Convergence failure"), false)
#else
#define GEOGRAPHICLIB_VOLATILE volatile
// Ignore convergence failures with standard floating points types by allowing
// loop to exit cleanly.
#define GEOGRAPHICLIB_PANIC false
#endif
namespace GeographicLib {
/**
* \brief Mathematical functions needed by %GeographicLib
*
* Define mathematical functions in order to localize system dependencies and
* to provide generic versions of the functions. In addition define a real
* type to be used by %GeographicLib.
*
* Example of use:
* \include example-Math.cpp
**********************************************************************/
class GEOGRAPHICLIB_EXPORT Math {
private:
void dummy() {
GEOGRAPHICLIB_STATIC_ASSERT(GEOGRAPHICLIB_PRECISION >= 1 &&
GEOGRAPHICLIB_PRECISION <= 5,
"Bad value of precision");
}
Math(); // Disable constructor
public:
#if GEOGRAPHICLIB_HAVE_LONG_DOUBLE
/**
* The extended precision type for real numbers, used for some testing.
* This is long double on computers with this type; otherwise it is double.
**********************************************************************/
typedef long double extended;
#else
typedef double extended;
#endif
#if GEOGRAPHICLIB_PRECISION == 2
/**
* The real type for %GeographicLib. Nearly all the testing has been done
* with \e real = double. However, the algorithms should also work with
* float and long double (where available). (<b>CAUTION</b>: reasonable
* accuracy typically cannot be obtained using floats.)
**********************************************************************/
typedef double real;
#elif GEOGRAPHICLIB_PRECISION == 1
typedef float real;
#elif GEOGRAPHICLIB_PRECISION == 3
typedef extended real;
#elif GEOGRAPHICLIB_PRECISION == 4
typedef boost::multiprecision::float128 real;
#elif GEOGRAPHICLIB_PRECISION == 5
typedef mpfr::mpreal real;
#else
typedef double real;
#endif
/**
* @return the number of bits of precision in a real number.
**********************************************************************/
static int digits() {
#if GEOGRAPHICLIB_PRECISION != 5
return std::numeric_limits<real>::digits;
#else
return std::numeric_limits<real>::digits();
#endif
}
/**
* Set the binary precision of a real number.
*
* @param[in] ndigits the number of bits of precision.
* @return the resulting number of bits of precision.
*
* This only has an effect when GEOGRAPHICLIB_PRECISION = 5. See also
* Utility::set_digits for caveats about when this routine should be
* called.
**********************************************************************/
static int set_digits(int ndigits) {
#if GEOGRAPHICLIB_PRECISION != 5
(void)ndigits;
#else
mpfr::mpreal::set_default_prec(ndigits >= 2 ? ndigits : 2);
#endif
return digits();
}
/**
* @return the number of decimal digits of precision in a real number.
**********************************************************************/
static int digits10() {
#if GEOGRAPHICLIB_PRECISION != 5
return std::numeric_limits<real>::digits10;
#else
return std::numeric_limits<real>::digits10();
#endif
}
/**
* Number of additional decimal digits of precision for real relative to
* double (0 for float).
**********************************************************************/
static int extra_digits() {
return
digits10() > std::numeric_limits<double>::digits10 ?
digits10() - std::numeric_limits<double>::digits10 : 0;
}
/**
* true if the machine is big-endian.
**********************************************************************/
static const bool bigendian = GEOGRAPHICLIB_WORDS_BIGENDIAN;
/**
* @tparam T the type of the returned value.
* @return &pi;.
**********************************************************************/
template<typename T> static T pi() {
using std::atan2;
static const T pi = atan2(T(0), T(-1));
return pi;
}
/**
* A synonym for pi<real>().
**********************************************************************/
static real pi() { return pi<real>(); }
/**
* @tparam T the type of the returned value.
* @return the number of radians in a degree.
**********************************************************************/
template<typename T> static T degree() {
static const T degree = pi<T>() / 180;
return degree;
}
/**
* A synonym for degree<real>().
**********************************************************************/
static real degree() { return degree<real>(); }
/**
* Square a number.
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @return <i>x</i><sup>2</sup>.
**********************************************************************/
template<typename T> static T sq(T x)
{ return x * x; }
/**
* The hypotenuse function avoiding underflow and overflow.
*
* @tparam T the type of the arguments and the returned value.
* @param[in] x
* @param[in] y
* @return sqrt(<i>x</i><sup>2</sup> + <i>y</i><sup>2</sup>).
**********************************************************************/
template<typename T> static T hypot(T x, T y) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::hypot; return hypot(x, y);
#else
using std::abs; using std::sqrt;
x = abs(x); y = abs(y);
if (x < y) std::swap(x, y); // Now x >= y >= 0
y /= (x ? x : 1);
return x * sqrt(1 + y * y);
// For an alternative (square-root free) method see
// C. Moler and D. Morrision (1983) https://doi.org/10.1147/rd.276.0577
// and A. A. Dubrulle (1983) https://doi.org/10.1147/rd.276.0582
#endif
}
/**
* exp(\e x) &minus; 1 accurate near \e x = 0.
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @return exp(\e x) &minus; 1.
**********************************************************************/
template<typename T> static T expm1(T x) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::expm1; return expm1(x);
#else
using std::exp; using std::abs; using std::log;
GEOGRAPHICLIB_VOLATILE T
y = exp(x),
z = y - 1;
// The reasoning here is similar to that for log1p. The expression
// mathematically reduces to exp(x) - 1, and the factor z/log(y) = (y -
// 1)/log(y) is a slowly varying quantity near y = 1 and is accurately
// computed.
return abs(x) > 1 ? z : (z == 0 ? x : x * z / log(y));
#endif
}
/**
* log(1 + \e x) accurate near \e x = 0.
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @return log(1 + \e x).
**********************************************************************/
template<typename T> static T log1p(T x) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::log1p; return log1p(x);
#else
using std::log;
GEOGRAPHICLIB_VOLATILE T
y = 1 + x,
z = y - 1;
// Here's the explanation for this magic: y = 1 + z, exactly, and z
// approx x, thus log(y)/z (which is nearly constant near z = 0) returns
// a good approximation to the true log(1 + x)/x. The multiplication x *
// (log(y)/z) introduces little additional error.
return z == 0 ? x : x * log(y) / z;
#endif
}
/**
* The inverse hyperbolic sine function.
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @return asinh(\e x).
**********************************************************************/
template<typename T> static T asinh(T x) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::asinh; return asinh(x);
#else
using std::abs; T y = abs(x); // Enforce odd parity
y = log1p(y * (1 + y/(hypot(T(1), y) + 1)));
return x < 0 ? -y : y;
#endif
}
/**
* The inverse hyperbolic tangent function.
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @return atanh(\e x).
**********************************************************************/
template<typename T> static T atanh(T x) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::atanh; return atanh(x);
#else
using std::abs; T y = abs(x); // Enforce odd parity
y = log1p(2 * y/(1 - y))/2;
return x < 0 ? -y : y;
#endif
}
/**
* The cube root function.
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @return the real cube root of \e x.
**********************************************************************/
template<typename T> static T cbrt(T x) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::cbrt; return cbrt(x);
#else
using std::abs; using std::pow;
T y = pow(abs(x), 1/T(3)); // Return the real cube root
return x < 0 ? -y : y;
#endif
}
/**
* Fused multiply and add.
*
* @tparam T the type of the arguments and the returned value.
* @param[in] x
* @param[in] y
* @param[in] z
* @return <i>xy</i> + <i>z</i>, correctly rounded (on those platforms with
* support for the <code>fma</code> instruction).
*
* On platforms without the <code>fma</code> instruction, no attempt is
* made to improve on the result of a rounded multiplication followed by a
* rounded addition.
**********************************************************************/
template<typename T> static T fma(T x, T y, T z) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::fma; return fma(x, y, z);
#else
return x * y + z;
#endif
}
/**
* Normalize a two-vector.
*
* @tparam T the type of the argument and the returned value.
* @param[in,out] x on output set to <i>x</i>/hypot(<i>x</i>, <i>y</i>).
* @param[in,out] y on output set to <i>y</i>/hypot(<i>x</i>, <i>y</i>).
**********************************************************************/
template<typename T> static void norm(T& x, T& y)
{ T h = hypot(x, y); x /= h; y /= h; }
/**
* The error-free sum of two numbers.
*
* @tparam T the type of the argument and the returned value.
* @param[in] u
* @param[in] v
* @param[out] t the exact error given by (\e u + \e v) - \e s.
* @return \e s = round(\e u + \e v).
*
* See D. E. Knuth, TAOCP, Vol 2, 4.2.2, Theorem B. (Note that \e t can be
* the same as one of the first two arguments.)
**********************************************************************/
template<typename T> static T sum(T u, T v, T& t) {
GEOGRAPHICLIB_VOLATILE T s = u + v;
GEOGRAPHICLIB_VOLATILE T up = s - v;
GEOGRAPHICLIB_VOLATILE T vpp = s - up;
up -= u;
vpp -= v;
t = -(up + vpp);
// u + v = s + t
// = round(u + v) + t
return s;
}
/**
* Evaluate a polynomial.
*
* @tparam T the type of the arguments and returned value.
* @param[in] N the order of the polynomial.
* @param[in] p the coefficient array (of size \e N + 1).
* @param[in] x the variable.
* @return the value of the polynomial.
*
* Evaluate <i>y</i> = &sum;<sub><i>n</i>=0..<i>N</i></sub>
* <i>p</i><sub><i>n</i></sub> <i>x</i><sup><i>N</i>&minus;<i>n</i></sup>.
* Return 0 if \e N &lt; 0. Return <i>p</i><sub>0</sub>, if \e N = 0 (even
* if \e x is infinite or a nan). The evaluation uses Horner's method.
**********************************************************************/
template<typename T> static T polyval(int N, const T p[], T x)
// This used to employ Math::fma; but that's too slow and it seemed not to
// improve the accuracy noticeably. This might change when there's direct
// hardware support for fma.
{ T y = N < 0 ? 0 : *p++; while (--N >= 0) y = y * x + *p++; return y; }
/**
* Normalize an angle.
*
* @tparam T the type of the argument and returned value.
* @param[in] x the angle in degrees.
* @return the angle reduced to the range([&minus;180&deg;, 180&deg;].
*
* The range of \e x is unrestricted.
**********************************************************************/
template<typename T> static T AngNormalize(T x) {
#if GEOGRAPHICLIB_CXX11_MATH && GEOGRAPHICLIB_PRECISION != 4
using std::remainder;
x = remainder(x, T(360)); return x != -180 ? x : 180;
#else
using std::fmod;
T y = fmod(x, T(360));
#if defined(_MSC_VER) && _MSC_VER < 1900
// Before version 14 (2015), Visual Studio had problems dealing
// with -0.0. Specifically
// VC 10,11,12 and 32-bit compile: fmod(-0.0, 360.0) -> +0.0
// sincosd has a similar fix.
// python 2.7 on Windows 32-bit machines has the same problem.
if (x == 0) y = x;
#endif
return y <= -180 ? y + 360 : (y <= 180 ? y : y - 360);
#endif
}
/**
* Normalize a latitude.
*
* @tparam T the type of the argument and returned value.
* @param[in] x the angle in degrees.
* @return x if it is in the range [&minus;90&deg;, 90&deg;], otherwise
* return NaN.
**********************************************************************/
template<typename T> static T LatFix(T x)
{ using std::abs; return abs(x) > 90 ? NaN<T>() : x; }
/**
* The exact difference of two angles reduced to
* (&minus;180&deg;, 180&deg;].
*
* @tparam T the type of the arguments and returned value.
* @param[in] x the first angle in degrees.
* @param[in] y the second angle in degrees.
* @param[out] e the error term in degrees.
* @return \e d, the truncated value of \e y &minus; \e x.
*
* This computes \e z = \e y &minus; \e x exactly, reduced to
* (&minus;180&deg;, 180&deg;]; and then sets \e z = \e d + \e e where \e d
* is the nearest representable number to \e z and \e e is the truncation
* error. If \e d = &minus;180, then \e e &gt; 0; If \e d = 180, then \e e
* &le; 0.
**********************************************************************/
template<typename T> static T AngDiff(T x, T y, T& e) {
#if GEOGRAPHICLIB_CXX11_MATH && GEOGRAPHICLIB_PRECISION != 4
using std::remainder;
T t, d = AngNormalize(sum(remainder(-x, T(360)),
remainder( y, T(360)), t));
#else
T t, d = AngNormalize(sum(AngNormalize(-x), AngNormalize(y), t));
#endif
// Here y - x = d + t (mod 360), exactly, where d is in (-180,180] and
// abs(t) <= eps (eps = 2^-45 for doubles). The only case where the
// addition of t takes the result outside the range (-180,180] is d = 180
// and t > 0. The case, d = -180 + eps, t = -eps, can't happen, since
// sum would have returned the exact result in such a case (i.e., given t
// = 0).
return sum(d == 180 && t > 0 ? -180 : d, t, e);
}
/**
* Difference of two angles reduced to [&minus;180&deg;, 180&deg;]
*
* @tparam T the type of the arguments and returned value.
* @param[in] x the first angle in degrees.
* @param[in] y the second angle in degrees.
* @return \e y &minus; \e x, reduced to the range [&minus;180&deg;,
* 180&deg;].
*
* The result is equivalent to computing the difference exactly, reducing
* it to (&minus;180&deg;, 180&deg;] and rounding the result. Note that
* this prescription allows &minus;180&deg; to be returned (e.g., if \e x
* is tiny and negative and \e y = 180&deg;).
**********************************************************************/
template<typename T> static T AngDiff(T x, T y)
{ T e; return AngDiff(x, y, e); }
/**
* Coarsen a value close to zero.
*
* @tparam T the type of the argument and returned value.
* @param[in] x
* @return the coarsened value.
*
* The makes the smallest gap in \e x = 1/16 - nextafter(1/16, 0) =
* 1/2<sup>57</sup> for reals = 0.7 pm on the earth if \e x is an angle in
* degrees. (This is about 1000 times more resolution than we get with
* angles around 90&deg;.) We use this to avoid having to deal with near
* singular cases when \e x is non-zero but tiny (e.g.,
* 10<sup>&minus;200</sup>). This converts -0 to +0; however tiny negative
* numbers get converted to -0.
**********************************************************************/
template<typename T> static T AngRound(T x) {
using std::abs;
static const T z = 1/T(16);
if (x == 0) return 0;
GEOGRAPHICLIB_VOLATILE T y = abs(x);
// The compiler mustn't "simplify" z - (z - y) to y
y = y < z ? z - (z - y) : y;
return x < 0 ? -y : y;
}
/**
* Evaluate the sine and cosine function with the argument in degrees
*
* @tparam T the type of the arguments.
* @param[in] x in degrees.
* @param[out] sinx sin(<i>x</i>).
* @param[out] cosx cos(<i>x</i>).
*
* The results obey exactly the elementary properties of the trigonometric
* functions, e.g., sin 9&deg; = cos 81&deg; = &minus; sin 123456789&deg;.
* If x = &minus;0, then \e sinx = &minus;0; this is the only case where
* &minus;0 is returned.
**********************************************************************/
template<typename T> static void sincosd(T x, T& sinx, T& cosx) {
// In order to minimize round-off errors, this function exactly reduces
// the argument to the range [-45, 45] before converting it to radians.
using std::sin; using std::cos;
T r; int q;
#if GEOGRAPHICLIB_CXX11_MATH && GEOGRAPHICLIB_PRECISION <= 3 && \
!defined(__GNUC__)
// Disable for gcc because of bug in glibc version < 2.22, see
// https://sourceware.org/bugzilla/show_bug.cgi?id=17569
// Once this fix is widely deployed, should insert a runtime test for the
// glibc version number. For example
// #include <gnu/libc-version.h>
// std::string version(gnu_get_libc_version()); => "2.22"
using std::remquo;
r = remquo(x, T(90), &q);
#else
using std::fmod; using std::floor;
r = fmod(x, T(360));
q = int(floor(r / 90 + T(0.5)));
r -= 90 * q;
#endif
// now abs(r) <= 45
r *= degree();
// Possibly could call the gnu extension sincos
T s = sin(r), c = cos(r);
#if defined(_MSC_VER) && _MSC_VER < 1900
// Before version 14 (2015), Visual Studio had problems dealing
// with -0.0. Specifically
// VC 10,11,12 and 32-bit compile: fmod(-0.0, 360.0) -> +0.0
// VC 12 and 64-bit compile: sin(-0.0) -> +0.0
// AngNormalize has a similar fix.
// python 2.7 on Windows 32-bit machines has the same problem.
if (x == 0) s = x;
#endif
switch (unsigned(q) & 3U) {
case 0U: sinx = s; cosx = c; break;
case 1U: sinx = c; cosx = -s; break;
case 2U: sinx = -s; cosx = -c; break;
default: sinx = -c; cosx = s; break; // case 3U
}
// Set sign of 0 results. -0 only produced for sin(-0)
if (x != 0) { sinx += T(0); cosx += T(0); }
}
/**
* Evaluate the sine function with the argument in degrees
*
* @tparam T the type of the argument and the returned value.
* @param[in] x in degrees.
* @return sin(<i>x</i>).
**********************************************************************/
template<typename T> static T sind(T x) {
// See sincosd
using std::sin; using std::cos;
T r; int q;
#if GEOGRAPHICLIB_CXX11_MATH && GEOGRAPHICLIB_PRECISION <= 3 && \
!defined(__GNUC__)
using std::remquo;
r = remquo(x, T(90), &q);
#else
using std::fmod; using std::floor;
r = fmod(x, T(360));
q = int(floor(r / 90 + T(0.5)));
r -= 90 * q;
#endif
// now abs(r) <= 45
r *= degree();
unsigned p = unsigned(q);
r = p & 1U ? cos(r) : sin(r);
if (p & 2U) r = -r;
if (x != 0) r += T(0);
return r;
}
/**
* Evaluate the cosine function with the argument in degrees
*
* @tparam T the type of the argument and the returned value.
* @param[in] x in degrees.
* @return cos(<i>x</i>).
**********************************************************************/
template<typename T> static T cosd(T x) {
// See sincosd
using std::sin; using std::cos;
T r; int q;
#if GEOGRAPHICLIB_CXX11_MATH && GEOGRAPHICLIB_PRECISION <= 3 && \
!defined(__GNUC__)
using std::remquo;
r = remquo(x, T(90), &q);
#else
using std::fmod; using std::floor;
r = fmod(x, T(360));
q = int(floor(r / 90 + T(0.5)));
r -= 90 * q;
#endif
// now abs(r) <= 45
r *= degree();
unsigned p = unsigned(q + 1);
r = p & 1U ? cos(r) : sin(r);
if (p & 2U) r = -r;
return T(0) + r;
}
/**
* Evaluate the tangent function with the argument in degrees
*
* @tparam T the type of the argument and the returned value.
* @param[in] x in degrees.
* @return tan(<i>x</i>).
*
* If \e x = &plusmn;90&deg;, then a suitably large (but finite) value is
* returned.
**********************************************************************/
template<typename T> static T tand(T x) {
static const T overflow = 1 / sq(std::numeric_limits<T>::epsilon());
T s, c;
sincosd(x, s, c);
return c != 0 ? s / c : (s < 0 ? -overflow : overflow);
}
/**
* Evaluate the atan2 function with the result in degrees
*
* @tparam T the type of the arguments and the returned value.
* @param[in] y
* @param[in] x
* @return atan2(<i>y</i>, <i>x</i>) in degrees.
*
* The result is in the range (&minus;180&deg; 180&deg;]. N.B.,
* atan2d(&plusmn;0, &minus;1) = +180&deg;; atan2d(&minus;&epsilon;,
* &minus;1) = &minus;180&deg;, for &epsilon; positive and tiny;
* atan2d(&plusmn;0, +1) = &plusmn;0&deg;.
**********************************************************************/
template<typename T> static T atan2d(T y, T x) {
// In order to minimize round-off errors, this function rearranges the
// arguments so that result of atan2 is in the range [-pi/4, pi/4] before
// converting it to degrees and mapping the result to the correct
// quadrant.
using std::atan2; using std::abs;
int q = 0;
if (abs(y) > abs(x)) { std::swap(x, y); q = 2; }
if (x < 0) { x = -x; ++q; }
// here x >= 0 and x >= abs(y), so angle is in [-pi/4, pi/4]
T ang = atan2(y, x) / degree();
switch (q) {
// Note that atan2d(-0.0, 1.0) will return -0. However, we expect that
// atan2d will not be called with y = -0. If need be, include
//
// case 0: ang = 0 + ang; break;
//
// and handle mpfr as in AngRound.
case 1: ang = (y >= 0 ? 180 : -180) - ang; break;
case 2: ang = 90 - ang; break;
case 3: ang = -90 + ang; break;
}
return ang;
}
/**
* Evaluate the atan function with the result in degrees
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @return atan(<i>x</i>) in degrees.
**********************************************************************/
template<typename T> static T atand(T x)
{ return atan2d(x, T(1)); }
/**
* Evaluate <i>e</i> atanh(<i>e x</i>)
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @param[in] es the signed eccentricity = sign(<i>e</i><sup>2</sup>)
* sqrt(|<i>e</i><sup>2</sup>|)
* @return <i>e</i> atanh(<i>e x</i>)
*
* If <i>e</i><sup>2</sup> is negative (<i>e</i> is imaginary), the
* expression is evaluated in terms of atan.
**********************************************************************/
template<typename T> static T eatanhe(T x, T es);
/**
* Copy the sign.
*
* @tparam T the type of the argument.
* @param[in] x gives the magitude of the result.
* @param[in] y gives the sign of the result.
* @return value with the magnitude of \e x and with the sign of \e y.
*
* This routine correctly handles the case \e y = &minus;0, returning
* &minus|<i>x</i>|.
**********************************************************************/
template<typename T> static T copysign(T x, T y) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::copysign; return copysign(x, y);
#else
using std::abs;
// NaN counts as positive
return abs(x) * (y < 0 || (y == 0 && 1/y < 0) ? -1 : 1);
#endif
}
/**
* tan&chi; in terms of tan&phi;
*
* @tparam T the type of the argument and the returned value.
* @param[in] tau &tau; = tan&phi;
* @param[in] es the signed eccentricity = sign(<i>e</i><sup>2</sup>)
* sqrt(|<i>e</i><sup>2</sup>|)
* @return &tau;&prime; = tan&chi;
*
* See Eqs. (7--9) of
* C. F. F. Karney,
* <a href="https://doi.org/10.1007/s00190-011-0445-3">
* Transverse Mercator with an accuracy of a few nanometers,</a>
* J. Geodesy 85(8), 475--485 (Aug. 2011)
* (preprint
* <a href="https://arxiv.org/abs/1002.1417">arXiv:1002.1417</a>).
**********************************************************************/
template<typename T> static T taupf(T tau, T es);
/**
* tan&phi; in terms of tan&chi;
*
* @tparam T the type of the argument and the returned value.
* @param[in] taup &tau;&prime; = tan&chi;
* @param[in] es the signed eccentricity = sign(<i>e</i><sup>2</sup>)
* sqrt(|<i>e</i><sup>2</sup>|)
* @return &tau; = tan&phi;
*
* See Eqs. (19--21) of
* C. F. F. Karney,
* <a href="https://doi.org/10.1007/s00190-011-0445-3">
* Transverse Mercator with an accuracy of a few nanometers,</a>
* J. Geodesy 85(8), 475--485 (Aug. 2011)
* (preprint
* <a href="https://arxiv.org/abs/1002.1417">arXiv:1002.1417</a>).
**********************************************************************/
template<typename T> static T tauf(T taup, T es);
/**
* Test for finiteness.
*
* @tparam T the type of the argument.
* @param[in] x
* @return true if number is finite, false if NaN or infinite.
**********************************************************************/
template<typename T> static bool isfinite(T x) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::isfinite; return isfinite(x);
#else
using std::abs;
#if defined(_MSC_VER)
return abs(x) <= (std::numeric_limits<T>::max)();
#else
// There's a problem using MPFR C++ 3.6.3 and g++ -std=c++14 (reported on
// 2015-05-04) with the parens around std::numeric_limits<T>::max. Of
// course, these parens are only needed to deal with Windows stupidly
// defining max as a macro. So don't insert the parens on non-Windows
// platforms.
return abs(x) <= std::numeric_limits<T>::max();
#endif
#endif
}
/**
* The NaN (not a number)
*
* @tparam T the type of the returned value.
* @return NaN if available, otherwise return the max real of type T.
**********************************************************************/
template<typename T> static T NaN() {
#if defined(_MSC_VER)
return std::numeric_limits<T>::has_quiet_NaN ?
std::numeric_limits<T>::quiet_NaN() :
(std::numeric_limits<T>::max)();
#else
return std::numeric_limits<T>::has_quiet_NaN ?
std::numeric_limits<T>::quiet_NaN() :
std::numeric_limits<T>::max();
#endif
}
/**
* A synonym for NaN<real>().
**********************************************************************/
static real NaN() { return NaN<real>(); }
/**
* Test for NaN.
*
* @tparam T the type of the argument.
* @param[in] x
* @return true if argument is a NaN.
**********************************************************************/
template<typename T> static bool isnan(T x) {
#if GEOGRAPHICLIB_CXX11_MATH
using std::isnan; return isnan(x);
#else
return x != x;
#endif
}
/**
* Infinity
*
* @tparam T the type of the returned value.
* @return infinity if available, otherwise return the max real.
**********************************************************************/
template<typename T> static T infinity() {
#if defined(_MSC_VER)
return std::numeric_limits<T>::has_infinity ?
std::numeric_limits<T>::infinity() :
(std::numeric_limits<T>::max)();
#else
return std::numeric_limits<T>::has_infinity ?
std::numeric_limits<T>::infinity() :
std::numeric_limits<T>::max();
#endif
}
/**
* A synonym for infinity<real>().
**********************************************************************/
static real infinity() { return infinity<real>(); }
/**
* Swap the bytes of a quantity
*
* @tparam T the type of the argument and the returned value.
* @param[in] x
* @return x with its bytes swapped.
**********************************************************************/
template<typename T> static T swab(T x) {
union {
T r;
unsigned char c[sizeof(T)];
} b;
b.r = x;
for (int i = sizeof(T)/2; i--; )
std::swap(b.c[i], b.c[sizeof(T) - 1 - i]);
return b.r;
}
#if GEOGRAPHICLIB_PRECISION == 4
typedef boost::math::policies::policy
< boost::math::policies::domain_error
<boost::math::policies::errno_on_error>,
boost::math::policies::pole_error
<boost::math::policies::errno_on_error>,
boost::math::policies::overflow_error
<boost::math::policies::errno_on_error>,
boost::math::policies::evaluation_error
<boost::math::policies::errno_on_error> >
boost_special_functions_policy;
static real hypot(real x, real y)
{ return boost::math::hypot(x, y, boost_special_functions_policy()); }
static real expm1(real x)
{ return boost::math::expm1(x, boost_special_functions_policy()); }
static real log1p(real x)
{ return boost::math::log1p(x, boost_special_functions_policy()); }
static real asinh(real x)
{ return boost::math::asinh(x, boost_special_functions_policy()); }
static real atanh(real x)
{ return boost::math::atanh(x, boost_special_functions_policy()); }
static real cbrt(real x)
{ return boost::math::cbrt(x, boost_special_functions_policy()); }
static real fma(real x, real y, real z)
{ return fmaq(__float128(x), __float128(y), __float128(z)); }
static real copysign(real x, real y)
{ return boost::math::copysign(x, y); }
static bool isnan(real x) { return boost::math::isnan(x); }
static bool isfinite(real x) { return boost::math::isfinite(x); }
#endif
};
} // namespace GeographicLib
#endif // GEOGRAPHICLIB_MATH_HPP

172
global_fusion/Thirdparty/GeographicLib/src/Geocentric.cpp vendored Normal file
View File

@@ -0,0 +1,172 @@
/**
* \file Geocentric.cpp
* \brief Implementation for GeographicLib::Geocentric class
*
* Copyright (c) Charles Karney (2008-2017) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include "Geocentric.hpp"
namespace GeographicLib {
using namespace std;
Geocentric::Geocentric(real a, real f)
: _a(a)
, _f(f)
, _e2(_f * (2 - _f))
, _e2m(Math::sq(1 - _f)) // 1 - _e2
, _e2a(abs(_e2))
, _e4a(Math::sq(_e2))
, _maxrad(2 * _a / numeric_limits<real>::epsilon())
{
if (!(Math::isfinite(_a) && _a > 0))
throw GeographicErr("Equatorial radius is not positive");
if (!(Math::isfinite(_f) && _f < 1))
throw GeographicErr("Polar semi-axis is not positive");
}
const Geocentric& Geocentric::WGS84() {
static const Geocentric wgs84(Constants::WGS84_a(), Constants::WGS84_f());
return wgs84;
}
void Geocentric::IntForward(real lat, real lon, real h,
real& X, real& Y, real& Z,
real M[dim2_]) const {
real sphi, cphi, slam, clam;
Math::sincosd(Math::LatFix(lat), sphi, cphi);
Math::sincosd(lon, slam, clam);
real n = _a/sqrt(1 - _e2 * Math::sq(sphi));
Z = (_e2m * n + h) * sphi;
X = (n + h) * cphi;
Y = X * slam;
X *= clam;
if (M)
Rotation(sphi, cphi, slam, clam, M);
}
void Geocentric::IntReverse(real X, real Y, real Z,
real& lat, real& lon, real& h,
real M[dim2_]) const {
real
R = Math::hypot(X, Y),
slam = R != 0 ? Y / R : 0,
clam = R != 0 ? X / R : 1;
h = Math::hypot(R, Z); // Distance to center of earth
real sphi, cphi;
if (h > _maxrad) {
// We really far away (> 12 million light years); treat the earth as a
// point and h, above, is an acceptable approximation to the height.
// This avoids overflow, e.g., in the computation of disc below. It's
// possible that h has overflowed to inf; but that's OK.
//
// Treat the case X, Y finite, but R overflows to +inf by scaling by 2.
R = Math::hypot(X/2, Y/2);
slam = R != 0 ? (Y/2) / R : 0;
clam = R != 0 ? (X/2) / R : 1;
real H = Math::hypot(Z/2, R);
sphi = (Z/2) / H;
cphi = R / H;
} else if (_e4a == 0) {
// Treat the spherical case. Dealing with underflow in the general case
// with _e2 = 0 is difficult. Origin maps to N pole same as with
// ellipsoid.
real H = Math::hypot(h == 0 ? 1 : Z, R);
sphi = (h == 0 ? 1 : Z) / H;
cphi = R / H;
h -= _a;
} else {
// Treat prolate spheroids by swapping R and Z here and by switching
// the arguments to phi = atan2(...) at the end.
real
p = Math::sq(R / _a),
q = _e2m * Math::sq(Z / _a),
r = (p + q - _e4a) / 6;
if (_f < 0) swap(p, q);
if ( !(_e4a * q == 0 && r <= 0) ) {
real
// Avoid possible division by zero when r = 0 by multiplying
// equations for s and t by r^3 and r, resp.
S = _e4a * p * q / 4, // S = r^3 * s
r2 = Math::sq(r),
r3 = r * r2,
disc = S * (2 * r3 + S);
real u = r;
if (disc >= 0) {
real T3 = S + r3;
// Pick the sign on the sqrt to maximize abs(T3). This minimizes
// loss of precision due to cancellation. The result is unchanged
// because of the way the T is used in definition of u.
T3 += T3 < 0 ? -sqrt(disc) : sqrt(disc); // T3 = (r * t)^3
// N.B. cbrt always returns the real root. cbrt(-8) = -2.
real T = Math::cbrt(T3); // T = r * t
// T can be zero; but then r2 / T -> 0.
u += T + (T != 0 ? r2 / T : 0);
} else {
// T is complex, but the way u is defined the result is real.
real ang = atan2(sqrt(-disc), -(S + r3));
// There are three possible cube roots. We choose the root which
// avoids cancellation. Note that disc < 0 implies that r < 0.
u += 2 * r * cos(ang / 3);
}
real
v = sqrt(Math::sq(u) + _e4a * q), // guaranteed positive
// Avoid loss of accuracy when u < 0. Underflow doesn't occur in
// e4 * q / (v - u) because u ~ e^4 when q is small and u < 0.
uv = u < 0 ? _e4a * q / (v - u) : u + v, // u+v, guaranteed positive
// Need to guard against w going negative due to roundoff in uv - q.
w = max(real(0), _e2a * (uv - q) / (2 * v)),
// Rearrange expression for k to avoid loss of accuracy due to
// subtraction. Division by 0 not possible because uv > 0, w >= 0.
k = uv / (sqrt(uv + Math::sq(w)) + w),
k1 = _f >= 0 ? k : k - _e2,
k2 = _f >= 0 ? k + _e2 : k,
d = k1 * R / k2,
H = Math::hypot(Z/k1, R/k2);
sphi = (Z/k1) / H;
cphi = (R/k2) / H;
h = (1 - _e2m/k1) * Math::hypot(d, Z);
} else { // e4 * q == 0 && r <= 0
// This leads to k = 0 (oblate, equatorial plane) and k + e^2 = 0
// (prolate, rotation axis) and the generation of 0/0 in the general
// formulas for phi and h. using the general formula and division by 0
// in formula for h. So handle this case by taking the limits:
// f > 0: z -> 0, k -> e2 * sqrt(q)/sqrt(e4 - p)
// f < 0: R -> 0, k + e2 -> - e2 * sqrt(q)/sqrt(e4 - p)
real
zz = sqrt((_f >= 0 ? _e4a - p : p) / _e2m),
xx = sqrt( _f < 0 ? _e4a - p : p ),
H = Math::hypot(zz, xx);
sphi = zz / H;
cphi = xx / H;
if (Z < 0) sphi = -sphi; // for tiny negative Z (not for prolate)
h = - _a * (_f >= 0 ? _e2m : 1) * H / _e2a;
}
}
lat = Math::atan2d(sphi, cphi);
lon = Math::atan2d(slam, clam);
if (M)
Rotation(sphi, cphi, slam, clam, M);
}
void Geocentric::Rotation(real sphi, real cphi, real slam, real clam,
real M[dim2_]) {
// This rotation matrix is given by the following quaternion operations
// qrot(lam, [0,0,1]) * qrot(phi, [0,-1,0]) * [1,1,1,1]/2
// or
// qrot(pi/2 + lam, [0,0,1]) * qrot(-pi/2 + phi , [-1,0,0])
// where
// qrot(t,v) = [cos(t/2), sin(t/2)*v[1], sin(t/2)*v[2], sin(t/2)*v[3]]
// Local X axis (east) in geocentric coords
M[0] = -slam; M[3] = clam; M[6] = 0;
// Local Y axis (north) in geocentric coords
M[1] = -clam * sphi; M[4] = -slam * sphi; M[7] = cphi;
// Local Z axis (up) in geocentric coords
M[2] = clam * cphi; M[5] = slam * cphi; M[8] = sphi;
}
} // namespace GeographicLib

62
global_fusion/Thirdparty/GeographicLib/src/LocalCartesian.cpp vendored Normal file
View File

@@ -0,0 +1,62 @@
/**
* \file LocalCartesian.cpp
* \brief Implementation for GeographicLib::LocalCartesian class
*
* Copyright (c) Charles Karney (2008-2015) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include "LocalCartesian.hpp"
namespace GeographicLib {
using namespace std;
void LocalCartesian::Reset(real lat0, real lon0, real h0) {
_lat0 = Math::LatFix(lat0);
_lon0 = Math::AngNormalize(lon0);
_h0 = h0;
_earth.Forward(_lat0, _lon0, _h0, _x0, _y0, _z0);
real sphi, cphi, slam, clam;
Math::sincosd(_lat0, sphi, cphi);
Math::sincosd(_lon0, slam, clam);
Geocentric::Rotation(sphi, cphi, slam, clam, _r);
}
void LocalCartesian::MatrixMultiply(real M[dim2_]) const {
// M = r' . M
real t[dim2_];
copy(M, M + dim2_, t);
for (size_t i = 0; i < dim2_; ++i) {
size_t row = i / dim_, col = i % dim_;
M[i] = _r[row] * t[col] + _r[row+3] * t[col+3] + _r[row+6] * t[col+6];
}
}
void LocalCartesian::IntForward(real lat, real lon, real h,
real& x, real& y, real& z,
real M[dim2_]) const {
real xc, yc, zc;
_earth.IntForward(lat, lon, h, xc, yc, zc, M);
xc -= _x0; yc -= _y0; zc -= _z0;
x = _r[0] * xc + _r[3] * yc + _r[6] * zc;
y = _r[1] * xc + _r[4] * yc + _r[7] * zc;
z = _r[2] * xc + _r[5] * yc + _r[8] * zc;
if (M)
MatrixMultiply(M);
}
void LocalCartesian::IntReverse(real x, real y, real z,
real& lat, real& lon, real& h,
real M[dim2_]) const {
real
xc = _x0 + _r[0] * x + _r[1] * y + _r[2] * z,
yc = _y0 + _r[3] * x + _r[4] * y + _r[5] * z,
zc = _z0 + _r[6] * x + _r[7] * y + _r[8] * z;
_earth.IntReverse(xc, yc, zc, lat, lon, h, M);
if (M)
MatrixMultiply(M);
}
} // namespace GeographicLib

63
global_fusion/Thirdparty/GeographicLib/src/Math.cpp vendored Normal file
View File

@@ -0,0 +1,63 @@
/**
* \file Math.cpp
* \brief Implementation for GeographicLib::Math class
*
* Copyright (c) Charles Karney (2015) <charles@karney.com> and licensed
* under the MIT/X11 License. For more information, see
* https://geographiclib.sourceforge.io/
**********************************************************************/
#include "Math.hpp"
#if defined(_MSC_VER)
// Squelch warnings about constant conditional expressions
# pragma warning (disable: 4127)
#endif
namespace GeographicLib {
using namespace std;
template<typename T> T Math::eatanhe(T x, T es) {
return es > T(0) ? es * atanh(es * x) : -es * atan(es * x);
}
template<typename T> T Math::taupf(T tau, T es) {
T tau1 = hypot(T(1), tau),
sig = sinh( eatanhe(tau / tau1, es ) );
return hypot(T(1), sig) * tau - sig * tau1;
}
template<typename T> T Math::tauf(T taup, T es) {
static const int numit = 5;
static const T tol = sqrt(numeric_limits<T>::epsilon()) / T(10);
T e2m = T(1) - sq(es),
// To lowest order in e^2, taup = (1 - e^2) * tau = _e2m * tau; so use
// tau = taup/_e2m as a starting guess. (This starting guess is the
// geocentric latitude which, to first order in the flattening, is equal
// to the conformal latitude.) Only 1 iteration is needed for |lat| <
// 3.35 deg, otherwise 2 iterations are needed. If, instead, tau = taup
// is used the mean number of iterations increases to 1.99 (2 iterations
// are needed except near tau = 0).
tau = taup/e2m,
stol = tol * max(T(1), abs(taup));
// min iterations = 1, max iterations = 2; mean = 1.94
for (int i = 0; i < numit || GEOGRAPHICLIB_PANIC; ++i) {
T taupa = taupf(tau, es),
dtau = (taup - taupa) * (1 + e2m * sq(tau)) /
( e2m * hypot(T(1), tau) * hypot(T(1), taupa) );
tau += dtau;
if (!(abs(dtau) >= stol))
break;
}
return tau;
}
/// \cond SKIP
// Instantiate
template Math::real Math::eatanhe<Math::real>(Math::real, Math::real);
template Math::real Math::taupf<Math::real>(Math::real, Math::real);
template Math::real Math::tauf<Math::real>(Math::real, Math::real);
/// \endcond
} // namespace GeographicLib

128026
global_fusion/data/vio_global.csv Normal file
View File

File diff suppressed because it is too large Load Diff

6
global_fusion/launch/global_fusion.launch Normal file
View File

@@ -0,0 +1,6 @@
<launch>
<node name="global_fusion_node" pkg="global_fusion" type="global_fusion_node" output="screen" required="true">
<remap from="/gps" to="/gps/fix"/>
<remap from="/vins_estimator/odometry" to="/ov_msckf/loop_pose"/>
</node>
</launch>

6
global_fusion/launch/global_fusion_kitti.launch Normal file
View File

@@ -0,0 +1,6 @@
<launch>
<node name="global_fusion_node" pkg="global_fusion" type="global_fusion_node" output="screen" required="true">
<remap from="/gps" to="/kitti/oxts/gps/fix"/>
<remap from="/vins_estimator/odometry" to="/ov_msckf/loop_pose"/>
</node>
</launch>

38
global_fusion/package.xml Normal file
View File

@@ -0,0 +1,38 @@
<?xml version="1.0"?>
<package format="3">
<!-- Package Information -->
<name>global_fusion</name>
<version>1.0.0</version>
<description>
Package to perform global fusion.
</description>
<url type="website">https://docs.openvins.com/</url>
<url type="bugtracker">https://github.com/rpng/open_vins/issues</url>
<url type="repository">https://github.com/rpng/open_vins</url>
<!-- Code Authors -->
<author email="pgeneva@udel.edu">Patrick Geneva</author>
<maintainer email="pgeneva@udel.edu">Patrick Geneva</maintainer>
<!-- Licensing -->
<license>GNU General Public License v3.0</license>
<!-- ROS1: Dependencies needed to compile this package. -->
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
<depend condition="$ROS_VERSION == 1">cmake_modules</depend>
<depend condition="$ROS_VERSION == 1">roscpp</depend>
<depend condition="$ROS_VERSION == 1">rosbag</depend>
<depend condition="$ROS_VERSION == 1">tf</depend>
<depend condition="$ROS_VERSION == 1">std_msgs</depend>
<!-- ROS2: Dependencies needed to compile this package. -->
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
<!-- Note the export is required to expose the executables -->
<export>
<build_type condition="$ROS_VERSION == 1">catkin</build_type>
<build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
</export>
</package>

122
global_fusion/src/Factors.h Normal file
View File

@@ -0,0 +1,122 @@
//
// Created by admin1 on 25.08.2022.
//
/*******************************************************
* Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
*
* This file is part of VINS.
*
* Licensed under the GNU General Public License v3.0;
* you may not use this file except in compliance with the License.
*
* Author: Qin Tong (qintonguav@gmail.com)
*******************************************************/
#ifndef GLOBAL_FUSION_FACTORS_H
#define GLOBAL_FUSION_FACTORS_H
#endif //GLOBAL_FUSION_FACTORS_H
#pragma once
#include <ceres/ceres.h>
#include <ceres/rotation.h>
template <typename T> inline
void QuaternionInverse(const T q[4], T q_inverse[4])
{
q_inverse[0] = q[0];
q_inverse[1] = -q[1];
q_inverse[2] = -q[2];
q_inverse[3] = -q[3];
};
struct TError
{
TError(double t_x, double t_y, double t_z, double var)
:t_x(t_x), t_y(t_y), t_z(t_z), var(var){}
template <typename T>
bool operator()(const T* tj, T* residuals) const
{
residuals[0] = (tj[0] - T(t_x)) / T(var);
residuals[1] = (tj[1] - T(t_y)) / T(var);
residuals[2] = (tj[2] - T(t_z)) / T(var);
return true;
}
static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z, const double var)
{
return (new ceres::AutoDiffCostFunction<
TError, 3, 3>(
new TError(t_x, t_y, t_z, var)));
}
double t_x, t_y, t_z, var;
};
struct RelativeRTError
{
RelativeRTError(double t_x, double t_y, double t_z,
double q_w, double q_x, double q_y, double q_z,
double t_var, double q_var)
:t_x(t_x), t_y(t_y), t_z(t_z),
q_w(q_w), q_x(q_x), q_y(q_y), q_z(q_z),
t_var(t_var), q_var(q_var){}
template <typename T>
bool operator()(const T* const w_q_i, const T* ti, const T* w_q_j, const T* tj, T* residuals) const
{
T t_w_ij[3];
t_w_ij[0] = tj[0] - ti[0];
t_w_ij[1] = tj[1] - ti[1];
t_w_ij[2] = tj[2] - ti[2];
T i_q_w[4];
QuaternionInverse(w_q_i, i_q_w);
T t_i_ij[3];
ceres::QuaternionRotatePoint(i_q_w, t_w_ij, t_i_ij);
residuals[0] = (t_i_ij[0] - T(t_x)) / T(t_var);
residuals[1] = (t_i_ij[1] - T(t_y)) / T(t_var);
residuals[2] = (t_i_ij[2] - T(t_z)) / T(t_var);
T relative_q[4];
relative_q[0] = T(q_w);
relative_q[1] = T(q_x);
relative_q[2] = T(q_y);
relative_q[3] = T(q_z);
T q_i_j[4];
ceres::QuaternionProduct(i_q_w, w_q_j, q_i_j);
T relative_q_inv[4];
QuaternionInverse(relative_q, relative_q_inv);
T error_q[4];
ceres::QuaternionProduct(relative_q_inv, q_i_j, error_q);
residuals[3] = T(2) * error_q[1] / T(q_var);
residuals[4] = T(2) * error_q[2] / T(q_var);
residuals[5] = T(2) * error_q[3] / T(q_var);
return true;
}
static ceres::CostFunction* Create(const double t_x, const double t_y, const double t_z,
const double q_w, const double q_x, const double q_y, const double q_z,
const double t_var, const double q_var)
{
return (new ceres::AutoDiffCostFunction<
RelativeRTError, 6, 4, 3, 4, 3>(
new RelativeRTError(t_x, t_y, t_z, q_w, q_x, q_y, q_z, t_var, q_var)));
}
double t_x, t_y, t_z, t_norm;
double q_w, q_x, q_y, q_z;
double t_var, q_var;
};

188
global_fusion/src/KITTIGPSTest.cpp Normal file
View File

@@ -0,0 +1,188 @@
//
// Created by admin1 on 27.08.2022.
//
/*******************************************************
* Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
*
* This file is part of VINS.
*
* Licensed under the GNU General Public License v3.0;
* you may not use this file except in compliance with the License.
*
* Author: Qin Tong (qintonguav@gmail.com)
*******************************************************/
#include <iostream>
#include <stdio.h>
#include <cmath>
#include <string>
#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
using namespace std;
ros::Publisher pubGPS;
int main(int argc, char** argv)
{
ros::init(argc, argv, "kitti_glob");
ros::NodeHandle n("~");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
pubGPS = n.advertise<sensor_msgs::NavSatFix>("/gps", 1000);
if(argc != 3)
{
printf("please intput: rosrun vins kitti_gps_test [config file] [data folder] \n"
"for example: rosrun vins kitti_gps_test "
"~/catkin_ws/src/VINS-Fusion/config/kitti_raw/kitti_10_03_config.yaml "
"/media/tony-ws1/disk_D/kitti/2011_10_03/2011_10_03_drive_0027_sync/ \n");
return 1;
}
string config_file = argv[1];
printf("config_file: %s\n", argv[1]);
string sequence = argv[2];
printf("read sequence: %s\n", argv[2]);
string dataPath = sequence + "/";
// load image list
FILE* file;
file = std::fopen((dataPath + "image_00/timestamps.txt").c_str() , "r");
if(file == NULL){
printf("cannot find file: %simage_00/timestamps.txt \n", dataPath.c_str());
ROS_BREAK();
return 0;
}
vector<double> imageTimeList;
int year, month, day;
int hour, minute;
double second;
while (fscanf(file, "%d-%d-%d %d:%d:%lf", &year, &month, &day, &hour, &minute, &second) != EOF)
{
//printf("%lf\n", second);
imageTimeList.push_back(hour * 60 * 60 + minute * 60 + second);
}
std::fclose(file);
// load gps list
vector<double> GPSTimeList;
{
FILE* file;
file = std::fopen((dataPath + "oxts/timestamps.txt").c_str() , "r");
if(file == NULL){
printf("cannot find file: %soxts/timestamps.txt \n", dataPath.c_str());
ROS_BREAK();
return 0;
}
int year, month, day;
int hour, minute;
double second;
while (fscanf(file, "%d-%d-%d %d:%d:%lf", &year, &month, &day, &hour, &minute, &second) != EOF)
{
//printf("%lf\n", second);
GPSTimeList.push_back(hour * 60 * 60 + minute * 60 + second);
}
std::fclose(file);
}
readParameters(config_file);
estimator.setParameter();
registerPub(n);
FILE* outFile;
outFile = fopen((OUTPUT_FOLDER + "/vio.txt").c_str(),"w");
if(outFile == NULL)
printf("Output path dosen't exist: %s\n", OUTPUT_FOLDER.c_str());
string leftImagePath, rightImagePath;
cv::Mat imLeft, imRight;
double baseTime;
for (size_t i = 0; i < imageTimeList.size(); i++)
{
if(ros::ok())
{
if(imageTimeList[0] < GPSTimeList[0])
baseTime = imageTimeList[0];
else
baseTime = GPSTimeList[0];
//printf("base time is %f\n", baseTime);
printf("process image %d\n", (int)i);
stringstream ss;
ss << setfill('0') << setw(10) << i;
leftImagePath = dataPath + "image_00/data/" + ss.str() + ".png";
rightImagePath = dataPath + "image_01/data/" + ss.str() + ".png";
printf("%s\n", leftImagePath.c_str() );
printf("%s\n", rightImagePath.c_str() );
imLeft = cv::imread(leftImagePath, cv::IMREAD_GRAYSCALE );
imRight = cv::imread(rightImagePath, cv::IMREAD_GRAYSCALE );
double imgTime = imageTimeList[i] - baseTime;
// load gps
FILE* GPSFile;
string GPSFilePath = dataPath + "oxts/data/" + ss.str() + ".txt";
GPSFile = std::fopen(GPSFilePath.c_str() , "r");
if(GPSFile == NULL){
printf("cannot find file: %s\n", GPSFilePath.c_str());
ROS_BREAK();
return 0;
}
double lat, lon, alt, roll, pitch, yaw;
double vn, ve, vf, vl, vu;
double ax, ay, az, af, al, au;
double wx, wy, wz, wf, wl, wu;
double pos_accuracy, vel_accuracy;
double navstat, numsats;
double velmode, orimode;
fscanf(GPSFile, "%lf %lf %lf %lf %lf %lf ", &lat, &lon, &alt, &roll, &pitch, &yaw);
//printf("lat:%lf lon:%lf alt:%lf roll:%lf pitch:%lf yaw:%lf \n", lat, lon, alt, roll, pitch, yaw);
fscanf(GPSFile, "%lf %lf %lf %lf %lf ", &vn, &ve, &vf, &vl, &vu);
//printf("vn:%lf ve:%lf vf:%lf vl:%lf vu:%lf \n", vn, ve, vf, vl, vu);
fscanf(GPSFile, "%lf %lf %lf %lf %lf %lf ", &ax, &ay, &az, &af, &al, &au);
//printf("ax:%lf ay:%lf az:%lf af:%lf al:%lf au:%lf\n", ax, ay, az, af, al, au);
fscanf(GPSFile, "%lf %lf %lf %lf %lf %lf ", &wx, &wy, &wz, &wf, &wl, &wu);
//printf("wx:%lf wy:%lf wz:%lf wf:%lf wl:%lf wu:%lf\n", wx, wy, wz, wf, wl, wu);
fscanf(GPSFile, "%lf %lf %lf %lf %lf %lf ", &pos_accuracy, &vel_accuracy, &navstat, &numsats, &velmode, &orimode);
//printf("pos_accuracy:%lf vel_accuracy:%lf navstat:%lf numsats:%lf velmode:%lf orimode:%lf\n",
// pos_accuracy, vel_accuracy, navstat, numsats, velmode, orimode);
std::fclose(GPSFile);
sensor_msgs::NavSatFix gps_position;
gps_position.header.frame_id = "NED";
gps_position.header.stamp = ros::Time(imgTime);
gps_position.status.status = navstat;
gps_position.status.service = numsats;
gps_position.latitude = lat;
gps_position.longitude = lon;
gps_position.altitude = alt;
gps_position.position_covariance[0] = pos_accuracy;
//printf("pos_accuracy %f \n", pos_accuracy);
pubGPS.publish(gps_position);
estimator.inputImage(imgTime, imLeft, imRight);
Eigen::Matrix<double, 4, 4> pose;
estimator.getPoseInWorldFrame(pose);
if(outFile != NULL)
fprintf (outFile, "%f %f %f %f %f %f %f %f %f %f %f %f \n",pose(0,0), pose(0,1), pose(0,2),pose(0,3),
pose(1,0), pose(1,1), pose(1,2),pose(1,3),
pose(2,0), pose(2,1), pose(2,2),pose(2,3));
// cv::imshow("leftImage", imLeft);
// cv::imshow("rightImage", imRight);
// cv::waitKey(2);
}
else
break;
}
if(outFile != NULL)
fclose (outFile);
return 0;
}

255
global_fusion/src/globalOpt.cpp Normal file
View File

@@ -0,0 +1,255 @@
//
// Created by admin1 on 25.08.2022.
//
/*******************************************************
* Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
*
* This file is part of VINS.
*
* Licensed under the GNU General Public License v3.0;
* you may not use this file except in compliance with the License.
*
* Author: Qin Tong (qintonguav@gmail.com)
*******************************************************/
#include "globalOpt.h"
#include "Factors.h"
GlobalOptimization::GlobalOptimization() {
initGPS = false;
newGPS = false;
// I suppose the matrix which transforms the gps coordinates to vio (or reverse)
WGPS_T_WVIO = Eigen::Matrix4d::Identity();
threadOpt = std::thread(&GlobalOptimization::optimize, this);
}
GlobalOptimization::~GlobalOptimization() {
threadOpt.detach();
}
void GlobalOptimization::GPS2XYZ(double latitude, double longitude, double altitude, double* xyz){
if (!initGPS)
{
geoConverter.Reset(latitude, longitude, altitude);
initGPS = true;
}
geoConverter.Forward(latitude, longitude, altitude, xyz[0], xyz[1], xyz[2]);
}
void GlobalOptimization::inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ){
mPoseMap.lock();
vector<double> localPose{OdomP.x(), OdomP.y(), OdomP.z(),
OdomQ.w(), OdomQ.x(), OdomQ.y(), OdomQ.z()};
localPoseMap[t] = localPose;
Eigen::Quaterniond globalQ;
globalQ = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomQ;
Eigen::Vector3d globalP = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomP + WGPS_T_WVIO.block<3, 1>(0, 3);
vector<double> globalPose{globalP.x(), globalP.y(), globalP.z(),
globalQ.w(), globalQ.x(), globalQ.y(), globalQ.z()};
globalPoseMap[t] = globalPose;
lastP = globalP;
lastQ = globalQ;
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = ros::Time(t);
pose_stamped.header.frame_id = "gps_global";
pose_stamped.pose.position.x = lastP.x();
pose_stamped.pose.position.y = lastP.y();
pose_stamped.pose.position.z = lastP.z();
pose_stamped.pose.orientation.x = lastQ.x();
pose_stamped.pose.orientation.y = lastQ.y();
pose_stamped.pose.orientation.z = lastQ.z();
pose_stamped.pose.orientation.w = lastQ.w();
global_path.header = pose_stamped.header;
global_path.poses.push_back(pose_stamped);
mPoseMap.unlock();
};
void GlobalOptimization::getGlobalOdom(Eigen::Vector3d &odomP, Eigen::Quaterniond &odomQ){
odomP = lastP;
odomQ = lastQ;
}
void GlobalOptimization::inputGPS(double t, double latitude, double longitude, double altitude, double posAccuracy){
double xyz[3];
GPS2XYZ(latitude, longitude, altitude, xyz);
vector<double> tmp{xyz[0], xyz[1], xyz[2], posAccuracy};
//printf("new gps: t: %f x: %f y: %f z:%f \n", t, tmp[0], tmp[1], tmp[2]);
GPSPositionMap[t] = tmp;
newGPS = true;
}
void GlobalOptimization::optimize(){
while(true){
if (newGPS){
newGPS = false;
printf("global optimization\n");
TicToc globalOptimizationTime;
ceres::Problem problem;
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.max_num_iterations = 5;
ceres::Solver::Summary summary;
ceres::LossFunction *loss_function;
loss_function = new ceres::HuberLoss(1.0);
ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();
//add param
mPoseMap.lock();
int length = localPoseMap.size();
double t_array[length][3];
double q_array[length][4];
map<double, vector<double>>::iterator iter;
iter = globalPoseMap.begin();
for (int i = 0; i < length; i++, iter++){
t_array[i][0] = iter->second[0];
t_array[i][1] = iter->second[1];
t_array[i][2] = iter->second[2];
q_array[i][0] = iter->second[3];
q_array[i][1] = iter->second[4];
q_array[i][2] = iter->second[5];
q_array[i][3] = iter->second[6];
problem.AddParameterBlock(q_array[i], 4, local_parameterization);
problem.AddParameterBlock(t_array[i], 3);
}
map<double, vector<double>>::iterator iterVIO, iterVIONext, iterGPS;
int i = 0;
for (iterVIO = localPoseMap.begin(); iterVIO != localPoseMap.end(); iterVIO++, i++)
{
//vio factor
iterVIONext = iterVIO;
iterVIONext++;
if(iterVIONext != localPoseMap.end())
{
Eigen::Matrix4d wTi = Eigen::Matrix4d::Identity();
Eigen::Matrix4d wTj = Eigen::Matrix4d::Identity();
wTi.block<3, 3>(0, 0) = Eigen::Quaterniond(iterVIO->second[3], iterVIO->second[4],
iterVIO->second[5], iterVIO->second[6]).toRotationMatrix();
wTi.block<3, 1>(0, 3) = Eigen::Vector3d(iterVIO->second[0], iterVIO->second[1], iterVIO->second[2]);
wTj.block<3, 3>(0, 0) = Eigen::Quaterniond(iterVIONext->second[3], iterVIONext->second[4],
iterVIONext->second[5], iterVIONext->second[6]).toRotationMatrix();
wTj.block<3, 1>(0, 3) = Eigen::Vector3d(iterVIONext->second[0], iterVIONext->second[1], iterVIONext->second[2]);
Eigen::Matrix4d iTj = wTi.inverse() * wTj;
Eigen::Quaterniond iQj;
iQj = iTj.block<3, 3>(0, 0);
Eigen::Vector3d iPj = iTj.block<3, 1>(0, 3);
ceres::CostFunction* vio_function = RelativeRTError::Create(iPj.x(), iPj.y(), iPj.z(),
iQj.w(), iQj.x(), iQj.y(), iQj.z(),
0.1, 0.01);
problem.AddResidualBlock(vio_function, NULL, q_array[i], t_array[i], q_array[i+1], t_array[i+1]);
/*
double **para = new double *[4];
para[0] = q_array[i];
para[1] = t_array[i];
para[3] = q_array[i+1];
para[4] = t_array[i+1];
double *tmp_r = new double[6];
double **jaco = new double *[4];
jaco[0] = new double[6 * 4];
jaco[1] = new double[6 * 3];
jaco[2] = new double[6 * 4];
jaco[3] = new double[6 * 3];
vio_function->Evaluate(para, tmp_r, jaco);
std::cout << Eigen::Map<Eigen::Matrix<double, 6, 1>>(tmp_r).transpose() << std::endl
<< std::endl;
std::cout << Eigen::Map<Eigen::Matrix<double, 6, 4, Eigen::RowMajor>>(jaco[0]) << std::endl
<< std::endl;
std::cout << Eigen::Map<Eigen::Matrix<double, 6, 3, Eigen::RowMajor>>(jaco[1]) << std::endl
<< std::endl;
std::cout << Eigen::Map<Eigen::Matrix<double, 6, 4, Eigen::RowMajor>>(jaco[2]) << std::endl
<< std::endl;
std::cout << Eigen::Map<Eigen::Matrix<double, 6, 3, Eigen::RowMajor>>(jaco[3]) << std::endl
<< std::endl;
*/
}
//gps factor
double t = iterVIO->first;
iterGPS = GPSPositionMap.find(t);
if (iterGPS != GPSPositionMap.end())
{
ceres::CostFunction* gps_function = TError::Create(iterGPS->second[0], iterGPS->second[1],
iterGPS->second[2], iterGPS->second[3]);
//printf("inverse weight %f \n", iterGPS->second[3]);
problem.AddResidualBlock(gps_function, loss_function, t_array[i]);
/*
double **para = new double *[1];
para[0] = t_array[i];
double *tmp_r = new double[3];
double **jaco = new double *[1];
jaco[0] = new double[3 * 3];
gps_function->Evaluate(para, tmp_r, jaco);
std::cout << Eigen::Map<Eigen::Matrix<double, 3, 1>>(tmp_r).transpose() << std::endl
<< std::endl;
std::cout << Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(jaco[0]) << std::endl
<< std::endl;
*/
}
}
//mPoseMap.unlock();
ceres::Solve(options, &problem, &summary);
//std::cout << summary.BriefReport() << "\n";
// update global pose
//mPoseMap.lock();
iter = globalPoseMap.begin();
for (int i = 0; i < length; i++, iter++)
{
vector<double> globalPose{t_array[i][0], t_array[i][1], t_array[i][2],
q_array[i][0], q_array[i][1], q_array[i][2], q_array[i][3]};
iter->second = globalPose;
if(i == length - 1)
{
Eigen::Matrix4d WVIO_T_body = Eigen::Matrix4d::Identity();
Eigen::Matrix4d WGPS_T_body = Eigen::Matrix4d::Identity();
double t = iter->first;
WVIO_T_body.block<3, 3>(0, 0) = Eigen::Quaterniond(localPoseMap[t][3], localPoseMap[t][4],
localPoseMap[t][5], localPoseMap[t][6]).toRotationMatrix();
WVIO_T_body.block<3, 1>(0, 3) = Eigen::Vector3d(localPoseMap[t][0], localPoseMap[t][1], localPoseMap[t][2]);
WGPS_T_body.block<3, 3>(0, 0) = Eigen::Quaterniond(globalPose[3], globalPose[4],
globalPose[5], globalPose[6]).toRotationMatrix();
WGPS_T_body.block<3, 1>(0, 3) = Eigen::Vector3d(globalPose[0], globalPose[1], globalPose[2]);
WGPS_T_WVIO = WGPS_T_body * WVIO_T_body.inverse();
}
}
updateGlobalPath();
//printf("global time %f \n", globalOptimizationTime.toc());
mPoseMap.unlock();
}
std::chrono::milliseconds dura(2000);
std::this_thread::sleep_for(dura);
}
return;
}
void GlobalOptimization::updateGlobalPath()
{
global_path.poses.clear();
map<double, vector<double>>::iterator iter;
for (iter = globalPoseMap.begin(); iter != globalPoseMap.end(); iter++)
{
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = ros::Time(iter->first);
pose_stamped.header.frame_id = "gps_global";
pose_stamped.pose.position.x = iter->second[0];
pose_stamped.pose.position.y = iter->second[1];
pose_stamped.pose.position.z = iter->second[2];
pose_stamped.pose.orientation.w = iter->second[3];
pose_stamped.pose.orientation.x = iter->second[4];
pose_stamped.pose.orientation.y = iter->second[5];
pose_stamped.pose.orientation.z = iter->second[6];
global_path.poses.push_back(pose_stamped);
}
}

66
global_fusion/src/globalOpt.h Normal file
View File

@@ -0,0 +1,66 @@
//
// Created by admin1 on 25.08.2022.
//
/*******************************************************
* Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
*
* This file is part of VINS.
*
* Licensed under the GNU General Public License v3.0;
* you may not use this file except in compliance with the License.
*
* Author: Qin Tong (qintonguav@gmail.com)
*******************************************************/
#ifndef GLOBAL_FUSION_GLOBALOPT_H
#define GLOBAL_FUSION_GLOBALOPT_H
#endif //GLOBAL_FUSION_GLOBALOPT_H
#pragma once
#include <vector>
#include <map>
#include <iostream>
#include <mutex>
#include <thread>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
#include <ceres/ceres.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include "LocalCartesian.hpp"
#include "tic_toc.h"
using namespace std;
class GlobalOptimization
{
public:
GlobalOptimization();
~GlobalOptimization();
void inputGPS(double t, double latitude, double longitude, double altitude, double posAccuracy);
void inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ);
void getGlobalOdom(Eigen::Vector3d &odomP, Eigen::Quaterniond &domQ);
nav_msgs::Path global_path;
private:
// this function is interesting. I suppose it transforms from the gps coordinate system to the
// local (vins) coordinate system. Which has the center at the start of the sequence by the way.
void GPS2XYZ(double latitude, double longitude, double altitude, double* xyz);
void optimize();
void updateGlobalPath();
// format t, tx, ty, tz, qw, qx, qy, qz
map<double, vector<double>> localPoseMap;
map<double, vector<double>> globalPoseMap;
map<double, vector<double>> GPSPositionMap;
bool initGPS;
bool newGPS;
GeographicLib::LocalCartesian geoConverter;
std::mutex mPoseMap;
Eigen::Matrix4d WGPS_T_WVIO;
Eigen::Vector3d lastP;
Eigen::Quaterniond lastQ;
std::thread threadOpt;
};

203
global_fusion/src/globalOptNode.cpp Normal file
View File

@@ -0,0 +1,203 @@
//
// Created by admin1 on 25.08.2022.
//
/*******************************************************
* Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
*
* This file is part of VINS.
*
* Licensed under the GNU General Public License v3.0;
* you may not use this file except in compliance with the License.
*
* Author: Qin Tong (qintonguav@gmail.com)
*******************************************************/
#include "ros/ros.h"
#include "globalOpt.h"
#include <sensor_msgs/NavSatFix.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
#include <iostream>
#include <stdio.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <fstream>
#include <queue>
#include <mutex>
#include <math.h>
GlobalOptimization globalEstimator;
ros::Publisher pub_global_odometry, pub_global_path, pub_car, pub_imu_tf_global;
nav_msgs::Path *global_path;
// we need to use pointer becuase otherwise TransformBroadcaster will be initializing here, before
// ros::init() called
std::shared_ptr<tf::TransformBroadcaster> TfBr;
double last_vio_t = -1;
std::queue<sensor_msgs::NavSatFixConstPtr> gpsQueue;
std::mutex m_buf;
void publish_car_model(double t, Eigen::Vector3d t_w_car, Eigen::Quaterniond q_w_car)
{
visualization_msgs::MarkerArray markerArray_msg;
visualization_msgs::Marker car_mesh;
car_mesh.header.stamp = ros::Time(t);
car_mesh.header.frame_id = "gps_global";
car_mesh.type = visualization_msgs::Marker::MESH_RESOURCE;
car_mesh.action = visualization_msgs::Marker::ADD;
car_mesh.id = 0;
car_mesh.mesh_resource = "package://global_fusion/models/car.dae";
Eigen::Matrix3d rot;
rot << 0, 0, -1, 0, -1, 0, -1, 0, 0;
Eigen::Quaterniond Q;
Q = q_w_car * rot;
car_mesh.pose.position.x = t_w_car.x();
car_mesh.pose.position.y = t_w_car.y();
car_mesh.pose.position.z = t_w_car.z();
car_mesh.pose.orientation.w = Q.w();
car_mesh.pose.orientation.x = Q.x();
car_mesh.pose.orientation.y = Q.y();
car_mesh.pose.orientation.z = Q.z();
car_mesh.color.a = 1.0;
car_mesh.color.r = 1.0;
car_mesh.color.g = 0.0;
car_mesh.color.b = 0.0;
float major_scale = 2.0;
car_mesh.scale.x = major_scale;
car_mesh.scale.y = major_scale;
car_mesh.scale.z = major_scale;
markerArray_msg.markers.push_back(car_mesh);
pub_car.publish(markerArray_msg);
}
void GPS_callback(const sensor_msgs::NavSatFixConstPtr &GPS_msg)
{
//printf("gps_callback! \n");
m_buf.lock();
gpsQueue.push(GPS_msg);
m_buf.unlock();
}
void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
{
//printf("vio_callback! \n");
double t = pose_msg->header.stamp.toSec();
last_vio_t = t;
Eigen::Vector3d vio_t(pose_msg->pose.pose.position.x, pose_msg->pose.pose.position.y, pose_msg->pose.pose.position.z);
Eigen::Quaterniond vio_q;
vio_q.w() = pose_msg->pose.pose.orientation.w;
vio_q.x() = pose_msg->pose.pose.orientation.x;
vio_q.y() = pose_msg->pose.pose.orientation.y;
vio_q.z() = pose_msg->pose.pose.orientation.z;
globalEstimator.inputOdom(t, vio_t, vio_q);
m_buf.lock();
while(!gpsQueue.empty())
{
sensor_msgs::NavSatFixConstPtr GPS_msg = gpsQueue.front();
double gps_t = GPS_msg->header.stamp.toSec();
printf("vio t: %f, gps t: %f \n", t, gps_t);
// 10ms sync tolerance
// PI: Let's change the sync tolerance from 10 ms to 100 ms
// if(gps_t >= t - 0.01 && gps_t <= t + 0.01)
if(gps_t >= t - 0.1 && gps_t <= t + 0.1)
{
//printf("receive GPS with timestamp %f\n", GPS_msg->header.stamp.toSec());
double latitude = GPS_msg->latitude;
double longitude = GPS_msg->longitude;
double altitude = GPS_msg->altitude;
//int numSats = GPS_msg->status.service;
// it was made for KITTI dataset before, which only gives the one parameter. But here we have 3 variances
// (the other values in the covariance matrix are 0).
double pos_accuracy;
pos_accuracy = std::sqrt( pow(GPS_msg->position_covariance[0], 2.0) + pow(GPS_msg->position_covariance[4], 2) + pow(GPS_msg->position_covariance[8], 2) );
if(pos_accuracy <= 0)
pos_accuracy = 1;
//printf("receive covariance %lf \n", pos_accuracy);
//if(GPS_msg->status.status > 8)
globalEstimator.inputGPS(t, latitude, longitude, altitude, pos_accuracy);
gpsQueue.pop();
break;
}
else if(gps_t < t - 0.01)
gpsQueue.pop();
else if(gps_t > t + 0.01)
break;
}
m_buf.unlock();
Eigen::Vector3d global_t;
Eigen:: Quaterniond global_q;
globalEstimator.getGlobalOdom(global_t, global_q);
nav_msgs::Odometry odometry;
odometry.header = pose_msg->header;
odometry.header.frame_id = "gps_global";
odometry.child_frame_id = "cam0_gps";
odometry.pose.pose.position.x = global_t.x();
odometry.pose.pose.position.y = global_t.y();
odometry.pose.pose.position.z = global_t.z();
odometry.pose.pose.orientation.x = global_q.x();
odometry.pose.pose.orientation.y = global_q.y();
odometry.pose.pose.orientation.z = global_q.z();
odometry.pose.pose.orientation.w = global_q.w();
pub_global_odometry.publish(odometry);
pub_global_path.publish(*global_path);
publish_car_model(t, global_t, global_q);
// Publish the tf so that Camera can bind to it.
tf::StampedTransform trans;
trans.frame_id_ = "gps_global";
trans.child_frame_id_ = "cam0_gps";
trans.stamp_ = pose_msg->header.stamp;
tf::Quaternion quat(global_q.x(), global_q.y(), global_q.z(), global_q.w());
trans.setRotation(quat);
tf::Vector3 orig(global_t.x(), global_t.y(), global_t.z());
trans.setOrigin(orig);
TfBr->sendTransform(trans);
// pub_imu_tf_global.publish(trans);
// write result to file
std::ofstream foutC("/home/admin1/workspace/catkin_ws_ov/src/open_vins/global_fusion/data/vio_global.csv", ios::app);
foutC.setf(ios::fixed, ios::floatfield);
foutC.precision(0);
foutC << pose_msg->header.stamp.toSec() * 1e9 << ",";
foutC.precision(5);
foutC << global_t.x() << ","
<< global_t.y() << ","
<< global_t.z() << ","
<< global_q.w() << ","
<< global_q.x() << ","
<< global_q.y() << ","
<< global_q.z() << endl;
foutC.close();
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "globalEstimator");
ros::NodeHandle n("~");
global_path = &globalEstimator.global_path;
TfBr = std::make_shared<tf::TransformBroadcaster>();
ros::Subscriber sub_GPS = n.subscribe("/gps", 100, GPS_callback);
ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 100, vio_callback);
pub_global_path = n.advertise<nav_msgs::Path>("global_path", 100);
pub_global_odometry = n.advertise<nav_msgs::Odometry>("global_odometry", 100);
// pub_imu_tf_global = n.advertise<tf::StampedTransform>("tf_imu_global", 100);
pub_car = n.advertise<visualization_msgs::MarkerArray>("car_model", 1000);
ros::spin();
return 0;
}

47
global_fusion/src/tic_toc.h Normal file
View File

@@ -0,0 +1,47 @@
//
// Created by admin1 on 25.08.2022.
//
/*******************************************************
* Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology
*
* This file is part of VINS.
*
* Licensed under the GNU General Public License v3.0;
* you may not use this file except in compliance with the License.
*******************************************************/
#ifndef GLOBAL_FUSION_TIC_TOC_H
#define GLOBAL_FUSION_TIC_TOC_H
#endif //GLOBAL_FUSION_TIC_TOC_H
#pragma once
#include <ctime>
#include <cstdlib>
#include <chrono>
class TicToc
{
public:
TicToc()
{
tic();
}
void tic()
{
start = std::chrono::system_clock::now();
}
double toc()
{
end = std::chrono::system_clock::now();
std::chrono::duration<double> elapsed_seconds = end - start;
return elapsed_seconds.count() * 1000;
}
private:
std::chrono::time_point<std::chrono::system_clock> start, end;
};