Skip to content

E
eskf-gps-imu-fusion

饺子大魔王的男人 / eskf-gps-imu-fusion
与 Fork 源项目一致

从无法访问的项目Fork

通知 1
Star 0
Fork 0
E
eskf-gps-imu-fusion

前往新版Gitcode，体验更适合开发者的 AI 搜索 >>

提交 cda303da 编写于 作者: 一点儿也不萌的萌萌's avatar 一点儿也不萌的萌萌
浏览文件
操作

eskf-gps-imu-fusion

上级
展开全部 隐藏空白更改
内联 并排
Showing with 2692 addition and 0 deletion
.gitignore 0 → 100644
浏览文件 @ cda303da
# Created by .ignore support plugin (hsz.mobi)
### C++ template
# Prerequisites
*.d
# Compiled Object files
*.slo
*.lo
*.o
*.obj
# Precompiled Headers
*.gch
*.pch
# Compiled Dynamic libraries
*.so
*.dylib
*.dll
# Fortran module files
*.mod
*.smod
# Compiled Static libraries
*.lai
*.la
*.a
*.lib
# Executables
*.exe
*.out
*.app
### C template
# Prerequisites
*.d
# Object files
*.o
*.ko
*.obj
*.elf
# Linker output
*.ilk
*.map
*.exp
# Precompiled Headers
*.gch
*.pch
# Libraries
*.lib
*.a
*.la
*.lo
# Shared objects (inc. Windows DLLs)
*.dll
*.so
*.so.*
*.dylib
# Executables
*.exe
*.out
*.app
*.i*86
*.x86_64
*.hex
# Debug files
*.dSYM/
*.su
*.idb
*.pdb
# Kernel Module Compile Results
*.mod*
*.cmd
.tmp_versions/
modules.order
Module.symvers
Mkfile.old
dkms.conf
# folder
/build
/cmake-build-debug
\ No newline at end of file
3rd/GeographicLib/CMakeLists.txt 0 → 100755
浏览文件 @ cda303da
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)
\ No newline at end of file
3rd/GeographicLib/include/Geocentric/Config.h 0 → 100755
浏览文件 @ cda303da
// 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 */
3rd/GeographicLib/include/Geocentric/Constants.hpp 0 → 100755
浏览文件 @ cda303da
/**
* \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
3rd/GeographicLib/include/Geocentric/Geocentric.hpp 0 → 100755
浏览文件 @ cda303da
/**
* \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
3rd/GeographicLib/include/Geocentric/LocalCartesian.hpp 0 → 100755
浏览文件 @ cda303da
/**
* \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
3rd/GeographicLib/include/Geocentric/Math.hpp 0 → 100755
浏览文件 @ cda303da
此差异已折叠。
3rd/GeographicLib/src/Geocentric.cpp 0 → 100755
浏览文件 @ cda303da
/**
* \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/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
3rd/GeographicLib/src/LocalCartesian.cpp 0 → 100755
浏览文件 @ cda303da
/**
* \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 "Geocentric/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
3rd/GeographicLib/src/Math.cpp 0 → 100755
浏览文件 @ cda303da
/**
* \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 "Geocentric/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
3rd/sophus/common.hpp 0 → 100644
浏览文件 @ cda303da
#ifndef SOPHUS_COMMON_HPP
#define SOPHUS_COMMON_HPP
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <iostream>
#include <Eigen/Core>
// following boost's assert.hpp
#undef SOPHUS_ENSURE
// ENSURES are similar to ASSERTS, but they are always checked for (including in
// release builds). At the moment there are no ASSERTS in Sophus which should
// only be used for checks which are performance critical.
#ifdef __GNUC__
#define SOPHUS_FUNCTION __PRETTY_FUNCTION__
#elif (_MSC_VER >= 1310)
#define SOPHUS_FUNCTION __FUNCTION__
#else
#define SOPHUS_FUNCTION "unknown"
#endif
// Make sure this compiles with older versions of Eigen which do not have
// EIGEN_DEVICE_FUNC defined.
#ifndef EIGEN_DEVICE_FUNC
#define EIGEN_DEVICE_FUNC
#endif
#define SOPHUS_FUNC EIGEN_DEVICE_FUNC
namespace Sophus {
namespace details {
// Following: http://stackoverflow.com/a/22759544
template <class T>
class IsStreamable {
private:
template <class TT>
static auto test(int)
-> decltype(std::declval<std::stringstream&>() << std::declval<TT>(),
std::true_type());
template <class>
static auto test(...) -> std::false_type;
public:
static bool const value = decltype(test<T>(0))::value;
};
template <class T>
class ArgToStream {
public:
static void impl(std::stringstream& stream, T&& arg) {
stream << std::forward<T>(arg);
}
};
inline void FormatStream(std::stringstream& stream, char const* text) {
stream << text;
return;
}
// Following: http://en.cppreference.com/w/cpp/language/parameter_pack
template <class T, typename... Args>
void FormatStream(std::stringstream& stream, char const* text, T&& arg,
Args&&... args) {
static_assert(IsStreamable<T>::value,
"One of the args has no ostream overload!");
for (; *text != '\0'; ++text) {
if (*text == '%') {
ArgToStream<T&&>::impl(stream, std::forward<T>(arg));
FormatStream(stream, text + 1, std::forward<Args>(args)...);
return;
}
stream << *text;
}
stream << "\nFormat-Warning: There are " << sizeof...(Args) + 1
<< " args unused.";
return;
}
template <class... Args>
std::string FormatString(char const* text, Args&&... args) {
std::stringstream stream;
FormatStream(stream, text, std::forward<Args>(args)...);
return stream.str();
}
inline std::string FormatString() { return std::string(); }
} // namespace details
} // namespace Sophus
#if defined(SOPHUS_DISABLE_ENSURES)
#define SOPHUS_ENSURE(expr, description, ...) ((void)0)
#elif defined(SOPHUS_ENABLE_ENSURE_HANDLER)
namespace Sophus {
void ensureFailed(char const* function, char const* file, int line,
char const* description);
}
#define SOPHUS_ENSURE(expr, description, ...) \
((expr) ? ((void)0) \
: ::Sophus::ensureFailed( \
SOPHUS_FUNCTION, __FILE__, __LINE__, \
Sophus::details::FormatString((description), ##__VA_ARGS__) \
.c_str()))
#else
namespace Sophus {
template <class... Args>
SOPHUS_FUNC void defaultEnsure(char const* function, char const* file, int line,
char const* description, Args&&... args) {
std::printf("Sophus ensure failed in function '%s', file '%s', line %d.\n",
function, file, line);
#ifdef __CUDACC__
std::printf("%s", description);
#else
std::cout << details::FormatString(description, std::forward<Args>(args)...)
<< std::endl;
std::abort();
#endif
}
} // namespace Sophus
#define SOPHUS_ENSURE(expr, description, ...) \
((expr) ? ((void)0) \
: Sophus::defaultEnsure(SOPHUS_FUNCTION, __FILE__, __LINE__, \
(description), ##__VA_ARGS__))
#endif
namespace Sophus {
template <class Scalar>
struct Constants {
SOPHUS_FUNC static Scalar epsilon() { return Scalar(1e-10); }
SOPHUS_FUNC static Scalar pi() { return Scalar(M_PI); }
};
template <>
struct Constants<float> {
SOPHUS_FUNC static float epsilon() { return static_cast<float>(1e-5); }
SOPHUS_FUNC static float pi() { return static_cast<float>(M_PI); }
};
}
#endif // SOPHUS_COMMON_HPP
3rd/sophus/example_ensure_handler.cpp 0 → 100644
浏览文件 @ cda303da
// This file is part of Sophus.
//
// Copyright 2013 Hauke Strasdat
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to
// deal in the Software without restriction, including without limitation the
// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
// sell copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
#include "common.hpp"
#include <cstdio>
#include <cstdlib>
namespace Sophus {
void ensureFailed(char const* function, char const* file, int line,
char const* description) {
std::printf("Sophus ensure failed in function '%s', file '%s', line %d.\n",
file, function, line);
std::printf("Description: %s\n", description);
std::abort();
}
}
3rd/sophus/rxso3.hpp 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
3rd/sophus/se2.hpp 0 → 100644
浏览文件 @ cda303da
#ifndef SOPHUS_SE2_HPP
#define SOPHUS_SE2_HPP
#include "so2.hpp"
namespace Sophus {
template <class Scalar_, int Options = 0>
class SE2;
using SE2d = SE2<double>;
using SE2f = SE2<float>;
} // namespace Sophus
namespace Eigen {
namespace internal {
template <class Scalar_, int Options>
struct traits<Sophus::SE2<Scalar_, Options>> {
using Scalar = Scalar_;
using TranslationType = Sophus::Vector2<Scalar>;
using SO2Type = Sophus::SO2<Scalar>;
};
template <class Scalar_, int Options>
struct traits<Map<Sophus::SE2<Scalar_>, Options>>
: traits<Sophus::SE2<Scalar_, Options>> {
using Scalar = Scalar_;
using TranslationType = Map<Sophus::Vector2<Scalar>, Options>;
using SO2Type = Map<Sophus::SO2<Scalar>, Options>;
};
template <class Scalar_, int Options>
struct traits<Map<Sophus::SE2<Scalar_> const, Options>>
: traits<Sophus::SE2<Scalar_, Options> const> {
using Scalar = Scalar_;
using TranslationType = Map<Sophus::Vector2<Scalar> const, Options>;
using SO2Type = Map<Sophus::SO2<Scalar> const, Options>;
};
} // namespace internal
} // namespace Eigen
namespace Sophus {
// SE2 base type - implements SE2 class but is storage agnostic.
//
// SE(2) is the group of rotations and translation in 2d. It is the semi-direct
// product of SO(2) and the 2d Euclidean vector space. The class is represented
// using a composition of SO2Group for rotation and a 2-vector for translation.
//
// SE(2) is neither compact, nor a commutative group.
//
// See SO2Group for more details of the rotation representation in 2d.
//
template <class Derived>
class SE2Base {
public:
using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
using TranslationType =
typename Eigen::internal::traits<Derived>::TranslationType;
using SO2Type = typename Eigen::internal::traits<Derived>::SO2Type;
// Degrees of freedom of manifold, number of dimensions in tangent space
// (two for translation, three for rotation).
static int constexpr DoF = 3;
// Number of internal parameters used (tuple for complex, two for
// translation).
static int constexpr num_parameters = 4;
// Group transformations are 3x3 matrices.
static int constexpr N = 3;
using Transformation = Matrix<Scalar, N, N>;
using Point = Vector2<Scalar>;
using Tangent = Vector<Scalar, DoF>;
using Adjoint = Matrix<Scalar, DoF, DoF>;
// Adjoint transformation
//
// This function return the adjoint transformation ``Ad`` of the group
// element ``A`` such that for all ``x`` it holds that
// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below.
//
SOPHUS_FUNC Adjoint Adj() const {
Matrix<Scalar, 2, 2> const& R = so2().matrix();
Transformation res;
res.setIdentity();
res.template topLeftCorner<2, 2>() = R;
res(0, 2) = translation()[1];
res(1, 2) = -translation()[0];
return res;
}
// Returns copy of instance casted to NewScalarType.
//
template <class NewScalarType>
SOPHUS_FUNC SE2<NewScalarType> cast() const {
return SE2<NewScalarType>(so2().template cast<NewScalarType>(),
translation().template cast<NewScalarType>());
}
// Returns group inverse.
//
SOPHUS_FUNC SE2<Scalar> inverse() const {
SO2<Scalar> const invR = so2().inverse();
return SE2<Scalar>(invR, invR * (translation() * Scalar(-1)));
}
// Logarithmic map
//
// Returns tangent space representation (= twist) of the instance.
//
SOPHUS_FUNC Tangent log() const { return log(*this); }
/**
* \brief Normalize SO2 element
*
* It re-normalizes the SO2 element.
*/
SOPHUS_FUNC void normalize() { so2().normalize(); }
// Returns 3x3 matrix representation of the instance.
//
// It has the following form:
//
// | R t |
// | o 1 |
//
// where ``R`` is a 2x2 rotation matrix, ``t`` a translation 2-vector and
// ``o`` a 2-column vector of zeros.
//
SOPHUS_FUNC Transformation matrix() const {
Transformation homogenious_matrix;
homogenious_matrix.template topLeftCorner<2, 3>() = matrix2x3();
homogenious_matrix.row(2) =
Matrix<Scalar, 1, 3>(Scalar(0), Scalar(0), Scalar(1));
return homogenious_matrix;
}
// Returns the significant first two rows of the matrix above.
//
SOPHUS_FUNC Matrix<Scalar, 2, 3> matrix2x3() const {
Matrix<Scalar, 2, 3> matrix;
matrix.template topLeftCorner<2, 2>() = rotationMatrix();
matrix.col(2) = translation();
return matrix;
}
// Assignment operator.
//
template <class OtherDerived>
SOPHUS_FUNC SE2Base<Derived>& operator=(SE2Base<OtherDerived> const& other) {
so2() = other.so2();
translation() = other.translation();
return *this;
}
// Group multiplication, which is rotation concatenation.
//
SOPHUS_FUNC SE2<Scalar> operator*(SE2<Scalar> const& other) const {
SE2<Scalar> result(*this);
result *= other;
return result;
}
// Group action on 2-points.
//
// This function rotates and translates a two dimensional point ``p`` by the
// SE(2) element ``bar_T_foo = (bar_R_foo, t_bar)`` (= rigid body
// transformation):
//
// ``p_bar = bar_R_foo * p_foo + t_bar``.
//
SOPHUS_FUNC Point operator*(Point const& p) const {
return so2() * p + translation();
}
// In-place group multiplication.
//
SOPHUS_FUNC SE2Base<Derived>& operator*=(SE2<Scalar> const& other) {
translation() += so2() * (other.translation());
so2() *= other.so2();
return *this;
}
// Returns rotation matrix.
//
SOPHUS_FUNC Matrix<Scalar, 2, 2> rotationMatrix() const {
return so2().matrix();
}
// Takes in complex number, and normalizes it.
//
// Precondition: The complex number must not be close to zero.
//
SOPHUS_FUNC void setComplex(Sophus::Vector2<Scalar> const& complex) {
return so2().setComplex(complex);
}
// Sets ``so3`` using ``rotation_matrix``.
//
// Precondition: ``R`` must be orthogonal and ``det(R)=1``.
//
SOPHUS_FUNC void setRotationMatrix(Matrix<Scalar, 2, 2> const& R) {
so2().setComplex(Scalar(0.5) * (R(0, 0) + R(1, 1)),
Scalar(0.5) * (R(1, 0) - R(0, 1)));
}
// Mutator of SO3 group.
//
SOPHUS_FUNC
SO2Type& so2() { return static_cast<Derived*>(this)->so2(); }
// Accessor of SO3 group.
//
SOPHUS_FUNC
SO2Type const& so2() const {
return static_cast<Derived const*>(this)->so2();
}
// Mutator of translation vector.
//
SOPHUS_FUNC
TranslationType& translation() {
return static_cast<Derived*>(this)->translation();
}
// Accessor of translation vector
//
SOPHUS_FUNC
TranslationType const& translation() const {
return static_cast<Derived const*>(this)->translation();
}
// Accessor of unit complex number.
//
SOPHUS_FUNC
typename Eigen::internal::traits<Derived>::SO2Type::Complex const&
unit_complex() const {
return so2().unit_complex();
}
////////////////////////////////////////////////////////////////////////////
// public static functions
////////////////////////////////////////////////////////////////////////////
// Derivative of Lie bracket with respect to first element.
//
// This function returns ``D_a [a, b]`` with ``D_a`` being the
// differential operator with respect to ``a``, ``[a, b]`` being the lie
// bracket of the Lie algebra se3.
// See ``lieBracket()`` below.
//
SOPHUS_FUNC static Transformation d_lieBracketab_by_d_a(Tangent const& b) {
static Scalar const zero = Scalar(0);
Vector2<Scalar> upsilon2 = b.template head<2>();
Scalar theta2 = b[2];
Transformation res;
res << zero, theta2, -upsilon2[1], -theta2, zero, upsilon2[0], zero, zero,
zero;
return res;
}
// Group exponential
//
// This functions takes in an element of tangent space (= twist ``a``) and
// returns the corresponding element of the group SE(2).
//
// The first two components of ``a`` represent the translational part
// ``upsilon`` in the tangent space of SE(2), while the last three components
// of ``a`` represents the rotation vector ``omega``.
// To be more specific, this function computes ``expmat(hat(a))`` with
// ``expmat(.)`` being the matrix exponential and ``hat(.)`` the hat-operator
// of SE(2), see below.
//
SOPHUS_FUNC static SE2<Scalar> exp(Tangent const& a) {
Scalar theta = a[2];
SO2<Scalar> so2 = SO2<Scalar>::exp(theta);
Scalar sin_theta_by_theta;
Scalar one_minus_cos_theta_by_theta;
if (std::abs(theta) < Constants<Scalar>::epsilon()) {
Scalar theta_sq = theta * theta;
sin_theta_by_theta = Scalar(1.) - Scalar(1. / 6.) * theta_sq;
one_minus_cos_theta_by_theta =
Scalar(0.5) * theta - Scalar(1. / 24.) * theta * theta_sq;
} else {
sin_theta_by_theta = so2.unit_complex().y() / theta;
one_minus_cos_theta_by_theta =
(Scalar(1.) - so2.unit_complex().x()) / theta;
}
Vector2<Scalar> trans(
sin_theta_by_theta * a[0] - one_minus_cos_theta_by_theta * a[1],
one_minus_cos_theta_by_theta * a[0] + sin_theta_by_theta * a[1]);
return SE2<Scalar>(so2, trans);
}
// Returns the ith infinitesimal generators of SE(2).
//
// The infinitesimal generators of SE(2) are:
//
// | 0 0 1 |
// G_0 = | 0 0 0 |
// | 0 0 0 |
//
// | 0 0 0 |
// G_1 = | 0 0 1 |
// | 0 0 0 |
//
// | 0 -1 0 |
// G_2 = | 1 0 0 |
// | 0 0 0 |
// Precondition: ``i`` must be in 0, 1 or 2.
//
SOPHUS_FUNC static Transformation generator(int i) {
SOPHUS_ENSURE(i >= 0 || i <= 2, "i should be in range [0,2].");
Tangent e;
e.setZero();
e[i] = Scalar(1);
return hat(e);
}
// hat-operator
//
// It takes in the 3-vector representation (= twist) and returns the
// corresponding matrix representation of Lie algebra element.
//
// Formally, the ``hat()`` operator of SE(3) is defined as
//
// ``hat(.): R^3 -> R^{3x33}, hat(a) = sum_i a_i * G_i`` (for i=0,1,2)
//
// with ``G_i`` being the ith infinitesimal generator of SE(2).
//
SOPHUS_FUNC static Transformation hat(Tangent const& a) {
Transformation Omega;
Omega.setZero();
Omega.template topLeftCorner<2, 2>() = SO2<Scalar>::hat(a[2]);
Omega.col(2).template head<2>() = a.template head<2>();
return Omega;
}
// Lie bracket
//
// It computes the Lie bracket of SE(2). To be more specific, it computes
//
// ``[omega_1, omega_2]_se2 := vee([hat(omega_1), hat(omega_2)])``
//
// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.) the
// hat-operator and ``vee(.)`` the vee-operator of SE(2).
//
SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {
Vector2<Scalar> upsilon1 = a.template head<2>();
Vector2<Scalar> upsilon2 = b.template head<2>();
Scalar theta1 = a[2];
Scalar theta2 = b[2];
return Tangent(-theta1 * upsilon2[1] + theta2 * upsilon1[1],
theta1 * upsilon2[0] - theta2 * upsilon1[0], Scalar(0));
}
// Logarithmic map
//
// Computes the logarithm, the inverse of the group exponential which maps
// element of the group (rigid body transformations) to elements of the
// tangent space (twist).
//
// To be specific, this function computes ``vee(logmat(.))`` with
// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator
// of SE(2).
//
SOPHUS_FUNC static Tangent log(SE2<Scalar> const& other) {
Tangent upsilon_theta;
SO2<Scalar> const& so2 = other.so2();
Scalar theta = SO2<Scalar>::log(so2);
upsilon_theta[2] = theta;
Scalar halftheta = Scalar(0.5) * theta;
Scalar halftheta_by_tan_of_halftheta;
Vector2<Scalar> const& z = so2.unit_complex();
Scalar real_minus_one = z.x() - Scalar(1.);
if (std::abs(real_minus_one) < Constants<Scalar>::epsilon()) {
halftheta_by_tan_of_halftheta =
Scalar(1.) - Scalar(1. / 12) * theta * theta;
} else {
halftheta_by_tan_of_halftheta = -(halftheta * z.y()) / (real_minus_one);
}
Matrix<Scalar, 2, 2> V_inv;
V_inv << halftheta_by_tan_of_halftheta, halftheta, -halftheta,
halftheta_by_tan_of_halftheta;
upsilon_theta.template head<2>() = V_inv * other.translation();
return upsilon_theta;
}
// vee-operator
//
// It takes the 3x3-matrix representation ``Omega`` and maps it to the
// corresponding 3-vector representation of Lie algebra.
//
// This is the inverse of the hat-operator, see above.
//
// Precondition: ``Omega`` must have the following structure:
//
// | 0 -d a |
// | d 0 b |
// | 0 0 0 | .
//
SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
SOPHUS_ENSURE(
Omega.row(2).template lpNorm<1>() < Constants<Scalar>::epsilon(),
"Omega: \n%", Omega);
Tangent upsilon_omega;
upsilon_omega.template head<2>() = Omega.col(2).template head<2>();
upsilon_omega[2] = SO2<Scalar>::vee(Omega.template topLeftCorner<2, 2>());
return upsilon_omega;
}
};
// SE2 default type - Constructors and default storage for SE3 Type.
template <class Scalar_, int Options>
class SE2 : public SE2Base<SE2<Scalar_, Options>> {
using Base = SE2Base<SE2<Scalar_, Options>>;
public:
using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
using Point = typename Base::Point;
using Tangent = typename Base::Tangent;
using Adjoint = typename Base::Adjoint;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// Default constructor initialize rigid body motion to the identity.
//
SOPHUS_FUNC SE2() : translation_(Vector2<Scalar>::Zero()) {}
// Copy constructor
//
template <class OtherDerived>
SOPHUS_FUNC SE2(SE2Base<OtherDerived> const& other)
: so2_(other.so2()), translation_(other.translation()) {}
// Constructor from SO3 and translation vector
//
template <class OtherDerived>
SOPHUS_FUNC SE2(SO2Base<OtherDerived> const& so2, Point const& translation)
: so2_(so2), translation_(translation) {}
// Constructor from rotation matrix and translation vector
//
// Precondition: Rotation matrix needs to be orthogonal with determinant of 1.
//
SOPHUS_FUNC
SE2(typename SO2<Scalar>::Transformation const& rotation_matrix,
Point const& translation)
: so2_(rotation_matrix), translation_(translation) {}
// Constructor from rotation angle and translation vector.
//
SOPHUS_FUNC SE2(Scalar const& theta, Point const& translation)
: so2_(theta), translation_(translation) {}
// Constructor from complex number and translation vector
//
// Precondition: ``complex` must not be close to zero.
SOPHUS_FUNC SE2(Vector2<Scalar> const& complex, Point const& translation)
: so2_(complex), translation_(translation) {}
// Constructor from 3x3 matrix
//
// Precondition: Rotation matrix needs to be orthogonal with determinant of 1.
// The last row must be (0, 0, 1).
//
SOPHUS_FUNC explicit SE2(Transformation const& T)
: so2_(T.template topLeftCorner<2, 2>().eval()),
translation_(T.template block<2, 1>(0, 2)) {}
// This provides unsafe read/write access to internal data. SO(2) is
// represented by a complex number (two parameters). When using direct write
// access, the user needs to take care of that the complex number stays
// normalized.
//
SOPHUS_FUNC Scalar* data() {
// so2_ and translation_ are layed out sequentially with no padding
return so2_.data();
}
// Const version of data() above.
//
SOPHUS_FUNC Scalar const* data() const {
// so2_ and translation_ are layed out sequentially with no padding
return so2_.data();
}
// Accessor of SO3
//
SOPHUS_FUNC SO2<Scalar>& so2() { return so2_; }
// Mutator of SO3
//
SOPHUS_FUNC SO2<Scalar> const& so2() const { return so2_; }
// Mutator of translation vector
//
SOPHUS_FUNC Vector2<Scalar>& translation() { return translation_; }
// Accessor of translation vector
//
SOPHUS_FUNC Vector2<Scalar> const& translation() const {
return translation_;
}
protected:
Sophus::SO2<Scalar> so2_;
Vector2<Scalar> translation_;
};
template <class Scalar, int Options = 0>
using SE2Group[[deprecated]] = SE2<Scalar, Options>;
} // end namespace
namespace Eigen {
// Specialization of Eigen::Map for ``SE2``.
//
// Allows us to wrap SE2 objects around POD array.
template <class Scalar_, int Options>
class Map<Sophus::SE2<Scalar_>, Options>
: public Sophus::SE2Base<Map<Sophus::SE2<Scalar_>, Options>> {
using Base = Sophus::SE2Base<Map<Sophus::SE2<Scalar_>, Options>>;
public:
using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
using Point = typename Base::Point;
using Tangent = typename Base::Tangent;
using Adjoint = typename Base::Adjoint;
EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
using Base::operator*=;
using Base::operator*;
SOPHUS_FUNC
Map(Scalar* coeffs)
: so2_(coeffs),
translation_(coeffs + Sophus::SO2<Scalar>::num_parameters) {}
// Mutator of SO3
//
SOPHUS_FUNC Map<Sophus::SO2<Scalar>, Options>& so2() { return so2_; }
// Accessor of SO3
//
SOPHUS_FUNC Map<Sophus::SO2<Scalar>, Options> const& so2() const {
return so2_;
}
// Mutator of translation vector
//
SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options>& translation() {
return translation_;
}
// Accessor of translation vector
//
SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options> const& translation() const {
return translation_;
}
protected:
Map<Sophus::SO2<Scalar>, Options> so2_;
Map<Sophus::Vector2<Scalar>, Options> translation_;
};
// Specialization of Eigen::Map for ``SE2 const``.
//
// Allows us to wrap SE2 objects around POD array.
template <class Scalar_, int Options>
class Map<Sophus::SE2<Scalar_> const, Options>
: public Sophus::SE2Base<Map<Sophus::SE2<Scalar_> const, Options>> {
using Base = Sophus::SE2Base<Map<Sophus::SE2<Scalar_> const, Options>>;
public:
using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
using Point = typename Base::Point;
using Tangent = typename Base::Tangent;
using Adjoint = typename Base::Adjoint;
EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
using Base::operator*=;
using Base::operator*;
SOPHUS_FUNC Map(Scalar const* coeffs)
: so2_(coeffs),
translation_(coeffs + Sophus::SO2<Scalar>::num_parameters) {}
// Accessor of SO3
//
SOPHUS_FUNC Map<Sophus::SO2<Scalar> const, Options> const& so2() const {
return so2_;
}
// Accessor of translation vector
//
SOPHUS_FUNC Map<Sophus::Vector2<Scalar> const, Options> const& translation()
const {
return translation_;
}
protected:
Map<Sophus::SO2<Scalar> const, Options> const so2_;
Map<Sophus::Vector2<Scalar> const, Options> const translation_;
};
}
#endif
3rd/sophus/se3.hpp 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
3rd/sophus/sim3.hpp 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
3rd/sophus/so2.hpp 0 → 100644
浏览文件 @ cda303da
#ifndef SOPHUS_SO2_HPP
#define SOPHUS_SO2_HPP
#include <complex>
// Include only the selective set of Eigen headers that we need.
// This helps when using Sophus with unusual compilers, like nvcc.
#include <Eigen/LU>
#include "types.hpp"
namespace Sophus {
template <class Scalar_, int Options = 0>
class SO2;
using SO2d = SO2<double>;
using SO2f = SO2<float>;
} // namespace Sophus
namespace Eigen {
namespace internal {
template <class Scalar_, int Options>
struct traits<Sophus::SO2<Scalar_, Options>> {
using Scalar = Scalar_;
using ComplexType = Sophus::Vector2<Scalar>;
};
template <class Scalar_, int Options>
struct traits<Map<Sophus::SO2<Scalar_>, Options>>
: traits<Sophus::SO2<Scalar_, Options>> {
using Scalar = Scalar_;
using ComplexType = Map<Sophus::Vector2<Scalar>, Options>;
};
template <class Scalar_, int Options>
struct traits<Map<Sophus::SO2<Scalar_> const, Options>>
: traits<Sophus::SO2<Scalar_, Options> const> {
using Scalar = Scalar_;
using ComplexType = Map<Sophus::Vector2<Scalar> const, Options>;
};
} // namespace internal
} // namespace Eigen
namespace Sophus {
// SO2 base type - implements SO2 class but is storage agnostic.
//
// SO(2) is the group of rotations in 2d. As a matrix group, it is the set of
// matrices which are orthogonal such that ``R * R' = I`` (with ``R'`` being the
// transpose of ``R``) and have a positive determinant. In particular, the
// determinant is 1. Let ``theta`` be the rotation angle, the rotation matrix
// can be written in close form:
//
// | cos(theta) -sin(theta) |
// | sin(theta) cos(theta) |
//
// As a matter of fact, the first column of those matrices is isomorph to the
// set of unit complex numbers U(1). Thus, internally, SO2 is represented as
// complex number with length 1.
//
// SO(2) is a compact and commutative group. First it is compact since the set
// of rotation matrices is a closed and bounded set. Second it is commutative
// since ``R(alpha) * R(beta) = R(beta) * R(alpha``, simply because ``alpha +
// beta = beta + alpha`` with ``alpha`` and ``beta`` being rotation angles
// (about the same axis).
//
// Class invairant: The 2-norm of ``unit_complex`` must be close to 1.
// Technically speaking, it must hold that:
//
// ``|unit_complex().squaredNorm() - 1| <= Constants<Scalar>::epsilon()``.
template <class Derived>
class SO2Base {
public:
using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
using Complex = typename Eigen::internal::traits<Derived>::ComplexType;
// Degrees of freedom of manifold, number of dimensions in tangent space (one
// since we only have in-plane rotations).
static int constexpr DoF = 1;
// Number of internal parameters used (complex numbers are a tuples).
static int constexpr num_parameters = 2;
// Group transformations are 2x2 matrices.
static int constexpr N = 2;
using Transformation = Matrix<Scalar, N, N>;
using Point = Vector2<Scalar>;
using Tangent = Scalar;
using Adjoint = Scalar;
// Adjoint transformation
//
// This function return the adjoint transformation ``Ad`` of the group
// element ``A`` such that for all ``x`` it holds that
// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below.
//
// It simply ``1``, since ``SO(2)`` is a commutative group.
//
SOPHUS_FUNC Adjoint Adj() const { return 1; }
// Returns copy of instance casted to NewScalarType.
//
template <class NewScalarType>
SOPHUS_FUNC SO2<NewScalarType> cast() const {
return SO2<NewScalarType>(unit_complex().template cast<NewScalarType>());
}
// This provides unsafe read/write access to internal data. SO(2) is
// represented by a unit complex number (two parameters). When using direct
// write access, the user needs to take care of that the complex number stays
// normalized.
//
SOPHUS_FUNC Scalar* data() { return unit_complex_nonconst().data(); }
// Const version of data() above.
//
SOPHUS_FUNC Scalar const* data() const { return unit_complex().data(); }
// Returns ``*this`` times the ith generator of internal U(1) representation.
//
SOPHUS_FUNC SO2<Scalar> inverse() const {
return SO2<Scalar>(unit_complex().x(), -unit_complex().y());
}
// Logarithmic map
//
// Returns tangent space representation (= rotation angle) of the instance.
//
SOPHUS_FUNC Scalar log() const { return SO2<Scalar>::log(*this); }
// It re-normalizes ``unit_complex`` to unit length.
//
// Note: Because of the class invariant, there is typically no need to call
// this function directly.
//
SOPHUS_FUNC void normalize() {
Scalar length = std::sqrt(unit_complex().x() * unit_complex().x() +
unit_complex().y() * unit_complex().y());
SOPHUS_ENSURE(length >= Constants<Scalar>::epsilon(),
"Complex number should not be close to zero!");
unit_complex_nonconst().x() /= length;
unit_complex_nonconst().y() /= length;
}
// Returns 2x2 matrix representation of the instance.
//
// For SO(2), the matrix representation is an orthogonal matrix ``R`` with
// ``det(R)=1``, thus the so-called "rotation matrix".
//
SOPHUS_FUNC Transformation matrix() const {
Scalar const& real = unit_complex().x();
Scalar const& imag = unit_complex().y();
Transformation R;
// clang-format off
R <<
real, -imag,
imag, real;
// clang-format on
return R;
}
// Assignment operator
//
template <class OtherDerived>
SOPHUS_FUNC SO2Base<Derived>& operator=(SO2Base<OtherDerived> const& other) {
unit_complex_nonconst() = other.unit_complex();
return *this;
}
// Group multiplication, which is rotation concatenation.
//
SOPHUS_FUNC SO2<Scalar> operator*(SO2<Scalar> const& other) const {
SO2<Scalar> result(*this);
result *= other;
return result;
}
// Group action on 3-points.
//
// This function rotates a 3 dimensional point ``p`` by the SO3 element
// ``bar_R_foo`` (= rotation matrix): ``p_bar = bar_R_foo * p_foo``.
//
SOPHUS_FUNC Point operator*(Point const& p) const {
Scalar const& real = unit_complex().x();
Scalar const& imag = unit_complex().y();
return Point(real * p[0] - imag * p[1], imag * p[0] + real * p[1]);
}
// In-place group multiplication.
//
SOPHUS_FUNC SO2Base<Derived> operator*=(SO2<Scalar> const& other) {
Scalar lhs_real = unit_complex().x();
Scalar lhs_imag = unit_complex().y();
Scalar const& rhs_real = other.unit_complex().x();
Scalar const& rhs_imag = other.unit_complex().y();
// complex multiplication
unit_complex_nonconst().x() = lhs_real * rhs_real - lhs_imag * rhs_imag;
unit_complex_nonconst().y() = lhs_real * rhs_imag + lhs_imag * rhs_real;
Scalar squared_norm = unit_complex_nonconst().squaredNorm();
// We can assume that the squared-norm is close to 1 since we deal with a
// unit complex number. Due to numerical precision issues, there might
// be a small drift after pose concatenation. Hence, we need to renormalizes
// the complex number here.
// Since squared-norm is close to 1, we do not need to calculate the costly
// square-root, but can use an approximation around 1 (see
// http://stackoverflow.com/a/12934750 for details).
if (squared_norm != Scalar(1.0)) {
unit_complex_nonconst() *= Scalar(2.0) / (Scalar(1.0) + squared_norm);
}
return *this;
}
// Takes in complex number / tuple and normalizes it.
//
// Precondition: The complex number must not be close to zero.
//
SOPHUS_FUNC void setComplex(Point const& complex) {
unit_complex_nonconst() = complex;
normalize();
}
// Accessor of unit quaternion.
//
SOPHUS_FUNC
Complex const& unit_complex() const {
return static_cast<Derived const*>(this)->unit_complex();
}
////////////////////////////////////////////////////////////////////////////
// public static functions
////////////////////////////////////////////////////////////////////////////
// Group exponential
//
// This functions takes in an element of tangent space (= rotation angle
// ``theta``) and returns the corresponding element of the group SO(2).
//
// To be more specific, this function computes ``expmat(hat(omega))``
// with ``expmat(.)`` being the matrix exponential and ``hat(.)`` being the
// hat()-operator of SO(2).
//
SOPHUS_FUNC static SO2<Scalar> exp(Tangent const& theta) {
return SO2<Scalar>(std::cos(theta), std::sin(theta));
}
// Returns the infinitesimal generators of SO3.
//
// The infinitesimal generators of SO(2) is:
//
// | 0 1 |
// | -1 0 |
//
SOPHUS_FUNC static Transformation generator() { return hat(1); }
// hat-operator
//
// It takes in the scalar representation ``theta`` (= rotation angle) and
// returns the corresponding matrix representation of Lie algebra element.
//
// Formally, the ``hat()`` operator of SO(2) is defined as
//
// ``hat(.): R^2 -> R^{2x2}, hat(theta) = theta * G``
//
// with ``G`` being the infinitesimal generator of SO(2).
//
// The corresponding inverse is the ``vee``-operator, see below.
//
SOPHUS_FUNC static Transformation hat(Tangent const& theta) {
Transformation Omega;
// clang-format off
Omega <<
Scalar(0), -theta,
theta, Scalar(0);
// clang-format on
return Omega;
}
// Lie bracket
//
// It returns the Lie bracket of SO(2). Since SO(2) is a commutative group,
// the Lie bracket is simple ``0``.
//
SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) {
return Scalar(0);
}
// Logarithmic map
//
// Computes the logarithm, the inverse of the group exponential which maps
// element of the group (rotation matrices) to elements of the tangent space
// (rotation angles).
//
// To be specific, this function computes ``vee(logmat(.))`` with
// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator
// of SO(2).
//
SOPHUS_FUNC static Tangent log(SO2<Scalar> const& other) {
using std::atan2;
return atan2(other.unit_complex_.y(), other.unit_complex().x());
}
// vee-operator
//
// It takes the 2x2-matrix representation ``Omega`` and maps it to the
// corresponding scalar representation of Lie algebra.
//
// This is the inverse of the hat-operator, see above.
//
// Precondition: ``Omega`` must have the following structure:
//
// | 0 -a |
// | a 0 |
//
SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
using std::abs;
SOPHUS_ENSURE(
Omega.diagonal().template lpNorm<1>() < Constants<Scalar>::epsilon(),
"Omega: \n%", Omega);
SOPHUS_ENSURE(abs(Omega(1, 0) + Omega(0, 1)) < Constants<Scalar>::epsilon(),
"Omega: %s", Omega);
return Omega(1, 0);
}
private:
// Mutator of unit_complex is private to ensure class invariant. That is
// the complex number must stay close to unit length.
//
SOPHUS_FUNC
Complex& unit_complex_nonconst() {
return static_cast<Derived*>(this)->unit_complex_nonconst();
}
};
// SO2 default type - Constructors and default storage for SO2 Type
template <class Scalar_, int Options>
class SO2 : public SO2Base<SO2<Scalar_, Options>> {
using Base = SO2Base<SO2<Scalar_, Options>>;
public:
using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
using Point = typename Base::Point;
using Tangent = typename Base::Tangent;
using Adjoint = typename Base::Adjoint;
// ``Base`` is friend so unit_complex_nonconst can be accessed from ``Base``.
friend class SO2Base<SO2<Scalar, Options>>;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// Default constructor initialize unit complex number to identity rotation.
//
SOPHUS_FUNC SO2() : unit_complex_(Scalar(1), Scalar(0)) {}
// Copy constructor
//
template <class OtherDerived>
SOPHUS_FUNC SO2(SO2Base<OtherDerived> const& other)
: unit_complex_(other.unit_complex()) {}
// Constructor from rotation matrix
//
// Precondition: rotation matrix need to be orthogonal with determinant of 1.
//
SOPHUS_FUNC explicit SO2(Transformation const& R)
: unit_complex_(Scalar(0.5) * (R(0, 0) + R(1, 1)),
Scalar(0.5) * (R(1, 0) - R(0, 1))) {
SOPHUS_ENSURE(
std::abs(R.determinant() - Scalar(1)) <= Constants<Scalar>::epsilon(),
"det(R) should be (close to) 1.");
}
// Constructor from pair of real and imaginary number.
//
// Precondition: The pair must not be close to zero.
//
SOPHUS_FUNC SO2(Scalar const& real, Scalar const& imag)
: unit_complex_(real, imag) {
Base::normalize();
}
// Constructor from 2-vector.
//
// Precondition: The vector must not be close to zero.
//
SOPHUS_FUNC explicit SO2(Vector2<Scalar> const& complex)
: unit_complex_(complex) {
Base::normalize();
}
// Constructor from an rotation angle.
//
SOPHUS_FUNC explicit SO2(Scalar theta) {
unit_complex_nonconst() = SO2<Scalar>::exp(theta).unit_complex();
}
// Accessor of unit complex number
//
SOPHUS_FUNC Vector2<Scalar> const& unit_complex() const {
return unit_complex_;
}
protected:
// Mutator of complex number is protected to ensure class invariant.
//
SOPHUS_FUNC Vector2<Scalar>& unit_complex_nonconst() { return unit_complex_; }
Sophus::Vector2<Scalar> unit_complex_;
};
template <class Scalar, int Options = 0>
using SO2Group[[deprecated]] = SO2<Scalar, Options>;
} // namespace Sophus
namespace Eigen {
// Specialization of Eigen::Map for ``SO2``.
//
// Allows us to wrap SO2 objects around POD array (e.g. external c style
// complex number / tuple).
template <class Scalar_, int Options>
class Map<Sophus::SO2<Scalar_>, Options>
: public Sophus::SO2Base<Map<Sophus::SO2<Scalar_>, Options>> {
using Base = Sophus::SO2Base<Map<Sophus::SO2<Scalar_>, Options>>;
public:
using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
using Point = typename Base::Point;
using Tangent = typename Base::Tangent;
using Adjoint = typename Base::Adjoint;
// ``Base`` is friend so unit_complex_nonconst can be accessed from ``Base``.
friend class Sophus::SO2Base<Map<Sophus::SO2<Scalar_>, Options>>;
EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
using Base::operator*=;
using Base::operator*;
SOPHUS_FUNC
Map(Scalar* coeffs) : unit_complex_(coeffs) {}
// Accessor of unit complex number.
//
SOPHUS_FUNC
Map<Sophus::Vector2<Scalar>, Options> const& unit_complex() const {
return unit_complex_;
}
protected:
// Mutator of unit_complex is protected to ensure class invariant.
//
SOPHUS_FUNC
Map<Sophus::Vector2<Scalar>, Options>& unit_complex_nonconst() {
return unit_complex_;
}
Map<Matrix<Scalar, 2, 1>, Options> unit_complex_;
};
// Specialization of Eigen::Map for ``SO2 const``.
//
// Allows us to wrap SO2 objects around POD array (e.g. external c style
// complex number / tuple).
template <class Scalar_, int Options>
class Map<Sophus::SO2<Scalar_> const, Options>
: public Sophus::SO2Base<Map<Sophus::SO2<Scalar_> const, Options>> {
using Base = Sophus::SO2Base<Map<Sophus::SO2<Scalar_> const, Options>>;
public:
using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
using Point = typename Base::Point;
using Tangent = typename Base::Tangent;
using Adjoint = typename Base::Adjoint;
EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
using Base::operator*=;
using Base::operator*;
SOPHUS_FUNC Map(Scalar const* coeffs) : unit_complex_(coeffs) {}
// Accessor of unit complex number.
//
SOPHUS_FUNC Map<Sophus::Vector2<Scalar> const, Options> const& unit_complex()
const {
return unit_complex_;
}
protected:
// Mutator of unit_complex is protected to ensure class invariant.
//
Map<Matrix<Scalar, 2, 1> const, Options> const unit_complex_;
};
}
#endif // SOPHUS_SO2_HPP
3rd/sophus/so3.hpp 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
3rd/sophus/test_macros.hpp 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
3rd/sophus/types.hpp 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
CMakeLists.txt 0 → 100644
浏览文件 @ cda303da
cmake_minimum_required(VERSION 3.10)
project(gps_imu_fusion)
set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CXX_STANDARD 11)
set(ALL_TARGET_LIBRARIES "")
include(cmake/eigen.cmake)
include(cmake/YAML.cmake)
include(cmake/global_defination.cmake)
include_directories(${PROJECT_SOURCE_DIR}/3rd/GeographicLib/include/)
include_directories(${PROJECT_SOURCE_DIR}/include)
add_library(DEPS SHARED
src/eskf.cpp
src/gps_flow.cpp
src/imu_flow.cpp
src/eskf_flow.cpp
src/observability_analysis.cpp
3rd/GeographicLib/src/LocalCartesian.cpp
3rd/GeographicLib/src/Geocentric.cpp
3rd/GeographicLib/src/Math.cpp
)
target_link_libraries(DEPS
${ALL_TARGET_LIBRARIES}
)
add_executable(gps_imu_fusion app/gps_imu_fusion.cpp)
target_link_libraries(gps_imu_fusion DEPS)
################# TEST #################
add_executable(test_gps test/test_gps.cpp)
target_link_libraries(test_gps DEPS)
add_executable(test_imu test/test_imu.cpp)
target_link_libraries(test_imu DEPS)
app/gps_imu_fusion.cpp 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
cmake/YAML.cmake 0 → 100755
浏览文件 @ cda303da
find_package(PkgConfig REQUIRED)
pkg_check_modules(YAML_CPP REQUIRED yaml-cpp)
include_directories(${YAML_CPP_INCLUDEDIR})
list(APPEND ALL_TARGET_LIBRARIES ${YAML_CPP_LIBRARIES})
\ No newline at end of file
cmake/eigen.cmake 0 → 100644
浏览文件 @ cda303da
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIRS})
\ No newline at end of file
cmake/global_defination.cmake 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
cmake/sophus.cmake 0 → 100755
浏览文件 @ cda303da
此差异已折叠。
config/config.yaml 0 → 100755
浏览文件 @ cda303da
此差异已折叠。
data/raw_data/accel-0.csv 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
data/raw_data/gps-0.csv 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
data/raw_data/gps_time.csv 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
data/raw_data/gyro-0.csv 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
data/raw_data/ref_accel.csv 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
data/raw_data/ref_gps.csv 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
data/raw_data/ref_gyro.csv 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
data/raw_data/time.csv 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
gnss-inss-sim/my_test.csv 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
gnss-inss-sim/my_test.py 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
gnss-inss-sim/readme.md 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
include/eskf.h 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
include/eskf_flow.h 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
include/global_defination.h.in 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
include/gps_data.h 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
include/gps_flow.h 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
include/imu_data.h 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
include/imu_flow.h 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
include/observability_analysis.h 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
include/tool.h 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
readme.md 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
src/eskf.cpp 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
src/eskf_flow.cpp 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
src/gps_flow.cpp 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
src/imu_flow.cpp 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
src/observability_analysis.cpp 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
test/test_gps.cpp 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
test/test_imu.cpp 0 → 100644
浏览文件 @ cda303da
此差异已折叠。
Markdown is supported
0% 或 .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑！
想要评论请 注册 或